"New" vertical control for fixed wing vehicles. More...

#include "firmwares/fixedwing/guidance/guidance_v.h"
#include "firmwares/fixedwing/guidance/guidance_v_n.h"
#include "state.h"
#include "firmwares/fixedwing/nav.h"
#include "generated/airframe.h"
#include "autopilot.h"

Include dependency graph for guidance_v_n.c:

Go to the source code of this file.

Macros
#define	V_CTL_AUTO_THROTTLE_DGAIN 0.

#define	V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 0.4

#define	V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0.

#define	V_CTL_AUTO_PITCH_MAX_SUM_ERR (RadOfDeg(10.))

#define	V_CTL_AUTO_PITCH_DGAIN 0.

#define	V_CTL_AUTO_PITCH_IGAIN 0.

#define	V_CTL_PITCH_TRIM 0.

#define	V_CTL_AUTO_CLIMB_LIMIT (0.5/4.0)

#define	LEAD_CTRL_TAU 0.8
	outer loop More...

#define	LEAD_CTRL_A 1.

#define	LEAD_CTRL_Te (1./60.)

#define	V_CTL_THROTTLE_SLEW (1.)

Functions
void	v_ctl_init (void)

void	v_ctl_guidance_loop (void)
	General guidance logic This will call the proper control loops according to the sub-modes. More...

void	v_ctl_altitude_loop (void)
	outer loop More...

static void	v_ctl_set_pitch (void)

static void	v_ctl_set_throttle (void)

void	v_ctl_climb_loop (void)
	Auto-throttle inner loop. More...

void	v_ctl_throttle_slew (void)
	Computes slewed throttle from throttle setpoint called at 20Hz. More...

Variables
uint8_t	v_ctl_mode

float	v_ctl_altitude_setpoint
	in meters above MSL More...

float	v_ctl_altitude_pre_climb
	Path Angle. More...

float	v_ctl_altitude_pgain

float	v_ctl_altitude_error
	in meters, (setpoint - alt) -> positive = too low More...

float	v_ctl_climb_setpoint

uint8_t	v_ctl_climb_mode

uint8_t	v_ctl_auto_throttle_submode

float	v_ctl_auto_throttle_cruise_throttle

float	v_ctl_auto_throttle_nominal_cruise_throttle

float	v_ctl_auto_throttle_min_cruise_throttle

float	v_ctl_auto_throttle_max_cruise_throttle

float	v_ctl_auto_throttle_climb_throttle_increment

float	v_ctl_auto_throttle_pgain

float	v_ctl_auto_throttle_igain

float	v_ctl_auto_throttle_dgain

float	v_ctl_auto_throttle_sum_err

float	v_ctl_auto_throttle_pitch_of_vz_pgain

float	v_ctl_auto_throttle_pitch_of_vz_dgain

float	v_ctl_auto_pitch_pgain

float	v_ctl_auto_pitch_dgain

float	v_ctl_auto_pitch_igain

float	v_ctl_auto_pitch_sum_err

float	controlled_throttle

pprz_t	v_ctl_throttle_setpoint

pprz_t	v_ctl_throttle_slewed

float	v_ctl_pitch_setpoint

float	v_ctl_pitch_trim

uint8_t	v_ctl_speed_mode

Detailed Description

"New" vertical control for fixed wing vehicles.

Definition in file guidance_v_n.c.

Macro Definition Documentation

#define LEAD_CTRL_A 1.

Definition at line 227 of file guidance_v_n.c.

#define LEAD_CTRL_TAU 0.8

outer loop

Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode

Definition at line 226 of file guidance_v_n.c.

#define LEAD_CTRL_Te (1./60.)

Definition at line 228 of file guidance_v_n.c.

#define V_CTL_AUTO_CLIMB_LIMIT (0.5/4.0)

Definition at line 96 of file guidance_v_n.c.

Referenced by v_ctl_altitude_loop().

#define V_CTL_AUTO_PITCH_DGAIN 0.

Definition at line 79 of file guidance_v_n.c.

Referenced by v_ctl_init().

#define V_CTL_AUTO_PITCH_IGAIN 0.

Definition at line 82 of file guidance_v_n.c.

Referenced by v_ctl_init().

#define V_CTL_AUTO_PITCH_MAX_SUM_ERR (RadOfDeg(10.))

Definition at line 76 of file guidance_v_n.c.

Referenced by v_ctl_set_pitch().

#define V_CTL_AUTO_THROTTLE_DGAIN 0.

Definition at line 50 of file guidance_v_n.c.

Referenced by v_ctl_init().

#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 0.4

Definition at line 63 of file guidance_v_n.c.

Referenced by v_ctl_set_throttle().

#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0.

Definition at line 68 of file guidance_v_n.c.

Referenced by v_ctl_init().

#define V_CTL_PITCH_TRIM 0.

Definition at line 90 of file guidance_v_n.c.

Referenced by v_ctl_init().

#define V_CTL_THROTTLE_SLEW (1.)

Definition at line 426 of file guidance_v_n.c.

Referenced by v_ctl_throttle_slew().

Function Documentation

void v_ctl_altitude_loop ( void )

outer loop

Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode

Definition at line 230 of file guidance_v_n.c.

References UtmCoor_f::alt, STALL_AIRSPEED, stateGetPositionUtm_f(), v_ctl_altitude_error, V_CTL_ALTITUDE_MAX_CLIMB, v_ctl_altitude_pgain, v_ctl_altitude_pre_climb, v_ctl_altitude_setpoint, v_ctl_auto_airspeed_setpoint, V_CTL_AUTO_CLIMB_LIMIT, v_ctl_climb_setpoint, and v_ctl_max_climb.

Referenced by v_ctl_guidance_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

void v_ctl_climb_loop ( void )

Auto-throttle inner loop.

Definition at line 390 of file guidance_v_n.c.

References ac_char_update(), accel_imu_meas, ACCELS_FLOAT_OF_BFP, AIRSPEED_SETPOINT_SLEW, autopilot, autopilot_throttle_killed(), controlled_throttle, dt_attidude, float_quat_vmult(), imu_to_body_quat, pprz_autopilot::launch, low_pass_vdot(), MAX_PPRZ, nav_pitch, stateGetAirspeed_f(), stateGetHorizontalSpeedNorm_f(), stateGetNedToBodyEulers_f(), stateGetSpeedEnu_f(), TRIM_UPPRZ, v_ctl_airspeed_pgain, v_ctl_auto_airspeed_controlled, v_ctl_auto_airspeed_setpoint, v_ctl_auto_airspeed_setpoint_slew, v_ctl_auto_groundspeed_igain, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR, v_ctl_auto_groundspeed_pgain, v_ctl_auto_groundspeed_setpoint, v_ctl_auto_groundspeed_sum_err, v_ctl_auto_pitch_of_airspeed_dgain, v_ctl_auto_pitch_of_airspeed_igain, v_ctl_auto_pitch_of_airspeed_pgain, v_ctl_auto_throttle_climb_throttle_increment, v_ctl_auto_throttle_cruise_throttle, v_ctl_auto_throttle_nominal_cruise_pitch, v_ctl_auto_throttle_nominal_cruise_throttle, v_ctl_auto_throttle_of_airspeed_igain, v_ctl_auto_throttle_of_airspeed_pgain, v_ctl_auto_throttle_pitch_of_vz_pgain, v_ctl_climb_setpoint, v_ctl_desired_acceleration, v_ctl_energy_diff_igain, v_ctl_energy_diff_pgain, v_ctl_energy_total_igain, v_ctl_energy_total_pgain, V_CTL_GLIDE_RATIO, v_ctl_max_acceleration, v_ctl_mode, V_CTL_MODE_AUTO_CLIMB, v_ctl_pitch_setpoint, v_ctl_set_pitch(), v_ctl_set_throttle(), V_CTL_SPEED_AIRSPEED, V_CTL_SPEED_GROUNDSPEED, v_ctl_speed_mode, V_CTL_SPEED_THROTTLE, v_ctl_throttle_setpoint, FloatVect3::x, and EnuCoor_f::z.

Referenced by v_ctl_guidance_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

void v_ctl_guidance_loop ( void )

General guidance logic This will call the proper control loops according to the sub-modes.

Definition at line 182 of file guidance_v_n.c.

References autopilot, pprz_autopilot::flight_time, pprz_autopilot::kill_throttle, pprz_autopilot::launch, MAX_PPRZ, nav_pitch, nav_throttle_setpoint, TRIM_PPRZ, TRIM_UPPRZ, v_ctl_altitude_loop(), v_ctl_climb_loop(), v_ctl_landing_loop(), v_ctl_mode, V_CTL_MODE_AUTO_ALT, V_CTL_MODE_AUTO_CLIMB, V_CTL_MODE_AUTO_THROTTLE, V_CTL_MODE_LANDING, v_ctl_pitch_setpoint, v_ctl_throttle_setpoint, and vsupply.

Here is the call graph for this function:

void v_ctl_init ( void )

Definition at line 121 of file guidance_v_n.c.

Here is the call graph for this function:

static void v_ctl_set_pitch ( void )

inlinestatic

Definition at line 253 of file guidance_v_n.c.

References AP_MODE_MANUAL, autopilot, autopilot_get_mode(), pprz_autopilot::launch, nav_pitch, stateGetSpeedEnu_f(), v_ctl_auto_pitch_dgain, v_ctl_auto_pitch_igain, V_CTL_AUTO_PITCH_MAX_SUM_ERR, v_ctl_auto_pitch_pgain, v_ctl_auto_pitch_sum_err, v_ctl_auto_throttle_pitch_of_vz_pgain, v_ctl_climb_setpoint, v_ctl_pitch_setpoint, v_ctl_pitch_trim, and EnuCoor_f::z.

Referenced by v_ctl_climb_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

static void v_ctl_set_throttle ( void )

inlinestatic

Definition at line 281 of file guidance_v_n.c.

References AP_MODE_MANUAL, autopilot, autopilot_get_mode(), controlled_throttle, pprz_autopilot::launch, stateGetSpeedEnu_f(), v_ctl_auto_throttle_climb_throttle_increment, v_ctl_auto_throttle_cruise_throttle, v_ctl_auto_throttle_dgain, v_ctl_auto_throttle_igain, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR, v_ctl_auto_throttle_pgain, v_ctl_auto_throttle_sum_err, v_ctl_climb_setpoint, and EnuCoor_f::z.

Referenced by v_ctl_climb_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

void v_ctl_throttle_slew ( void )

Computes slewed throttle from throttle setpoint called at 20Hz.

Computes throttle_slewed from throttle_setpoint.

Definition at line 431 of file guidance_v_n.c.

References MAX_PPRZ, TRIM_PPRZ, v_ctl_throttle_setpoint, V_CTL_THROTTLE_SLEW, and v_ctl_throttle_slewed.

Variable Documentation

float controlled_throttle

Definition at line 85 of file guidance_v_n.c.

Referenced by v_ctl_climb_auto_throttle_loop(), v_ctl_climb_loop(), v_ctl_init(), and v_ctl_set_throttle().

float v_ctl_altitude_error

in meters, (setpoint - alt) -> positive = too low

Definition at line 42 of file guidance_v_n.c.

Referenced by v_ctl_altitude_loop(), and v_ctl_init().

float v_ctl_altitude_pgain

Definition at line 41 of file guidance_v_n.c.

Referenced by v_ctl_altitude_loop(), and v_ctl_init().

float v_ctl_altitude_pre_climb

Path Angle.

Definition at line 40 of file guidance_v_n.c.

Referenced by v_ctl_altitude_loop(), and v_ctl_init().

float v_ctl_altitude_setpoint

in meters above MSL

Definition at line 39 of file guidance_v_n.c.

Referenced by v_ctl_altitude_loop(), and v_ctl_init().

float v_ctl_auto_pitch_dgain

Definition at line 73 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_pitch().

float v_ctl_auto_pitch_igain

Definition at line 74 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_pitch().

float v_ctl_auto_pitch_pgain

Definition at line 72 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_pitch().

float v_ctl_auto_pitch_sum_err

Definition at line 75 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_pitch().

float v_ctl_auto_throttle_climb_throttle_increment

Definition at line 58 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_throttle().

float v_ctl_auto_throttle_cruise_throttle

Definition at line 54 of file guidance_v_n.c.

Referenced by v_ctl_climb_loop(), v_ctl_init(), and v_ctl_set_throttle().

float v_ctl_auto_throttle_dgain

Definition at line 61 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_throttle().

float v_ctl_auto_throttle_igain

Definition at line 60 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_throttle().

float v_ctl_auto_throttle_max_cruise_throttle

Definition at line 57 of file guidance_v_n.c.

Referenced by v_ctl_init().

float v_ctl_auto_throttle_min_cruise_throttle

Definition at line 56 of file guidance_v_n.c.

Referenced by v_ctl_init().

float v_ctl_auto_throttle_nominal_cruise_throttle

Definition at line 55 of file guidance_v_n.c.

Referenced by v_ctl_init().

float v_ctl_auto_throttle_pgain

Definition at line 59 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_throttle().

float v_ctl_auto_throttle_pitch_of_vz_dgain

Definition at line 65 of file guidance_v_n.c.

Referenced by v_ctl_init().

float v_ctl_auto_throttle_pitch_of_vz_pgain

Definition at line 64 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_pitch().

uint8_t v_ctl_auto_throttle_submode

Definition at line 47 of file guidance_v_n.c.

Referenced by v_ctl_init().

float v_ctl_auto_throttle_sum_err

Definition at line 62 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_throttle().

uint8_t v_ctl_climb_mode

Definition at line 46 of file guidance_v_n.c.

Referenced by v_ctl_init().

float v_ctl_climb_setpoint

Definition at line 45 of file guidance_v_n.c.

Referenced by v_ctl_altitude_loop(), v_ctl_init(), v_ctl_set_pitch(), and v_ctl_set_throttle().

uint8_t v_ctl_mode

Definition at line 36 of file guidance_v_n.c.

Referenced by v_ctl_guidance_loop(), and v_ctl_init().

float v_ctl_pitch_setpoint

Definition at line 88 of file guidance_v_n.c.

Referenced by v_ctl_climb_loop(), v_ctl_guidance_loop(), v_ctl_init(), and v_ctl_set_pitch().

float v_ctl_pitch_trim

Definition at line 92 of file guidance_v_n.c.

Referenced by v_ctl_init(), and v_ctl_set_pitch().

uint8_t v_ctl_speed_mode

Definition at line 99 of file guidance_v_n.c.

Referenced by v_ctl_climb_loop(), and v_ctl_init().

pprz_t v_ctl_throttle_setpoint

Definition at line 86 of file guidance_v_n.c.

Referenced by v_ctl_climb_loop(), v_ctl_guidance_loop(), v_ctl_init(), and v_ctl_throttle_slew().

pprz_t v_ctl_throttle_slewed

Definition at line 87 of file guidance_v_n.c.

Referenced by v_ctl_throttle_slew().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

Function Documentation

Variable Documentation