guidance_v.c File Reference

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

Include dependency graph for guidance_v.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   150
#define	V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN   0.
#define	V_CTL_AUTO_PITCH_MAX_SUM_ERR   100
#define	V_CTL_PITCH_TRIM   0.
#define	V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION   1.0f
#define	V_CTL_THROTTLE_SLEW   1.
	auto pitch inner loop More...

Functions
static void	v_ctl_climb_auto_throttle_loop (void)
	auto throttle inner loop More...
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...
void	v_ctl_climb_loop (void)
	Auto-throttle inner loop. More...
void	v_ctl_landing_loop (void)
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_altitude_pre_climb_correction
float	v_ctl_altitude_max_climb
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_igain
float	v_ctl_auto_pitch_sum_err
pprz_t	v_ctl_throttle_setpoint
pprz_t	v_ctl_throttle_slewed
float	v_ctl_pitch_setpoint
float	v_ctl_pitch_trim

Detailed Description

Vertical control for fixed wing vehicles.

Definition in file guidance_v.c.

Macro Definition Documentation

V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION

#define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION   1.0f

Definition at line 119 of file guidance_v.c.

V_CTL_AUTO_PITCH_MAX_SUM_ERR

#define V_CTL_AUTO_PITCH_MAX_SUM_ERR   100

Definition at line 87 of file guidance_v.c.

V_CTL_AUTO_THROTTLE_DGAIN

#define V_CTL_AUTO_THROTTLE_DGAIN   0.

Definition at line 52 of file guidance_v.c.

V_CTL_AUTO_THROTTLE_MAX_SUM_ERR

#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR   150

Definition at line 65 of file guidance_v.c.

V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN

#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN   0.

Definition at line 70 of file guidance_v.c.

V_CTL_PITCH_TRIM

#define V_CTL_PITCH_TRIM   0.

Definition at line 93 of file guidance_v.c.

V_CTL_THROTTLE_SLEW

#define V_CTL_THROTTLE_SLEW   1.

auto pitch inner loop

computes a v_ctl_pitch_setpoint from a climb_setpoint given a fixed throttle

Definition at line 548 of file guidance_v.c.

Function Documentation

v_ctl_altitude_loop()

void v_ctl_altitude_loop

(

void

)

outer loop

Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode

Definition at line 308 of file guidance_v.c.

References UtmCoor_f::alt, stateGetPositionUtm_f(), v_ctl_altitude_error, v_ctl_altitude_max_climb, v_ctl_altitude_pgain, v_ctl_altitude_pre_climb, v_ctl_altitude_pre_climb_correction, v_ctl_altitude_setpoint, V_CTL_AUTO_THROTTLE_AGRESSIVE, V_CTL_AUTO_THROTTLE_BLENDED, V_CTL_AUTO_THROTTLE_STANDARD, v_ctl_auto_throttle_submode, v_ctl_climb_mode, V_CTL_CLIMB_MODE_AUTO_THROTTLE, and v_ctl_climb_setpoint.

Referenced by v_ctl_guidance_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

v_ctl_climb_auto_throttle_loop()

static void v_ctl_climb_auto_throttle_loop ( void  )

inlinestatic

auto throttle inner loop

Definition at line 409 of file guidance_v.c.

References controlled_throttle, MAX_PPRZ, nav_pitch, stateGetSpeedEnu_f(), TRIM_UPPRZ, v_ctl_altitude_error, V_CTL_AUTO_THROTTLE_AGRESSIVE, V_CTL_AUTO_THROTTLE_BLENDED, 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_pitch_of_vz_dgain, v_ctl_auto_throttle_pitch_of_vz_pgain, V_CTL_AUTO_THROTTLE_STANDARD, v_ctl_auto_throttle_submode, v_ctl_auto_throttle_sum_err, v_ctl_climb_setpoint, v_ctl_pitch_setpoint, v_ctl_pitch_trim, v_ctl_throttle_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:

v_ctl_climb_loop()

void v_ctl_climb_loop

(

void

)

Auto-throttle inner loop.

Definition at line 341 of file guidance_v.c.

References v_ctl_climb_auto_throttle_loop(), v_ctl_climb_mode, V_CTL_CLIMB_MODE_AUTO_PITCH, and V_CTL_CLIMB_MODE_AUTO_THROTTLE.

Referenced by v_ctl_guidance_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

v_ctl_guidance_loop()

void v_ctl_guidance_loop

(

void

)

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

Definition at line 265 of file guidance_v.c.

References ap_electrical, 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 Electrical::vsupply.

Here is the call graph for this function:

v_ctl_init()

void v_ctl_init

(

void

)

Definition at line 172 of file guidance_v.c.

References v_ctl_altitude_error, v_ctl_altitude_max_climb, V_CTL_ALTITUDE_MAX_CLIMB, v_ctl_altitude_pgain, v_ctl_altitude_pre_climb, v_ctl_altitude_pre_climb_correction, V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION, v_ctl_altitude_setpoint, v_ctl_auto_airspeed_controlled, v_ctl_auto_airspeed_setpoint, v_ctl_auto_groundspeed_igain, v_ctl_auto_groundspeed_pgain, v_ctl_auto_groundspeed_setpoint, v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_PITCH_IGAIN, v_ctl_auto_pitch_igain, v_ctl_auto_pitch_pgain, v_ctl_auto_pitch_sum_err, v_ctl_auto_throttle_climb_throttle_increment, v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_DGAIN, v_ctl_auto_throttle_dgain, v_ctl_auto_throttle_igain, v_ctl_auto_throttle_max_cruise_throttle, v_ctl_auto_throttle_min_cruise_throttle, v_ctl_auto_throttle_nominal_cruise_throttle, v_ctl_auto_throttle_pgain, v_ctl_auto_throttle_pitch_of_vz_dgain, V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN, v_ctl_auto_throttle_pitch_of_vz_pgain, V_CTL_AUTO_THROTTLE_STANDARD, v_ctl_auto_throttle_submode, v_ctl_auto_throttle_sum_err, v_ctl_climb_mode, V_CTL_CLIMB_MODE_AUTO_THROTTLE, v_ctl_climb_setpoint, v_ctl_mode, V_CTL_MODE_MANUAL, v_ctl_pitch_setpoint, V_CTL_PITCH_TRIM, v_ctl_pitch_trim, and v_ctl_throttle_setpoint.

v_ctl_landing_loop()

void v_ctl_landing_loop

(

void

)

Definition at line 357 of file guidance_v.c.

References UtmCoor_f::alt, autopilot_throttle_killed(), CONTROL_FREQUENCY, h_ctl_roll_setpoint, lateral_mode, LATERAL_MODE_ROLL, MAX_PPRZ, nav_pitch, stateGetHorizontalSpeedNorm_f(), stateGetPositionUtm_f(), v_ctl_altitude_setpoint, and v_ctl_throttle_setpoint.

Referenced by attitude_loop(), and v_ctl_guidance_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

v_ctl_throttle_slew()

void v_ctl_throttle_slew

(

void

)

Computes slewed throttle from throttle setpoint called at 20Hz.

Computes throttle_slewed from throttle_setpoint.

Definition at line 553 of file guidance_v.c.

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

Variable Documentation

v_ctl_altitude_error

float v_ctl_altitude_error

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

Definition at line 42 of file guidance_v.c.

Referenced by v_ctl_altitude_loop(), v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_altitude_max_climb

float v_ctl_altitude_max_climb

Definition at line 44 of file guidance_v.c.

Referenced by v_ctl_altitude_loop(), and v_ctl_init().

v_ctl_altitude_pgain

float v_ctl_altitude_pgain

Definition at line 41 of file guidance_v.c.

Referenced by v_ctl_altitude_loop(), and v_ctl_init().

v_ctl_altitude_pre_climb

float v_ctl_altitude_pre_climb

Path Angle.

Definition at line 40 of file guidance_v.c.

Referenced by v_ctl_altitude_loop(), and v_ctl_init().

v_ctl_altitude_pre_climb_correction

float v_ctl_altitude_pre_climb_correction

Definition at line 43 of file guidance_v.c.

Referenced by v_ctl_altitude_loop(), and v_ctl_init().

v_ctl_altitude_setpoint

float v_ctl_altitude_setpoint

in meters above MSL

Definition at line 39 of file guidance_v.c.

Referenced by v_ctl_altitude_loop(), v_ctl_init(), and v_ctl_landing_loop().

v_ctl_auto_pitch_igain

float v_ctl_auto_pitch_igain

Definition at line 85 of file guidance_v.c.

Referenced by v_ctl_init().

v_ctl_auto_pitch_pgain

float v_ctl_auto_pitch_pgain

Definition at line 84 of file guidance_v.c.

Referenced by v_ctl_init().

v_ctl_auto_pitch_sum_err

float v_ctl_auto_pitch_sum_err

Definition at line 86 of file guidance_v.c.

Referenced by v_ctl_init().

v_ctl_auto_throttle_climb_throttle_increment

float v_ctl_auto_throttle_climb_throttle_increment

Definition at line 60 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_auto_throttle_cruise_throttle

float v_ctl_auto_throttle_cruise_throttle

Definition at line 56 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_auto_throttle_dgain

float v_ctl_auto_throttle_dgain

Definition at line 63 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_auto_throttle_igain

float v_ctl_auto_throttle_igain

Definition at line 62 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_auto_throttle_max_cruise_throttle

float v_ctl_auto_throttle_max_cruise_throttle

Definition at line 59 of file guidance_v.c.

Referenced by v_ctl_init().

v_ctl_auto_throttle_min_cruise_throttle

float v_ctl_auto_throttle_min_cruise_throttle

Definition at line 58 of file guidance_v.c.

Referenced by v_ctl_init().

v_ctl_auto_throttle_nominal_cruise_throttle

float v_ctl_auto_throttle_nominal_cruise_throttle

Definition at line 57 of file guidance_v.c.

Referenced by v_ctl_init().

v_ctl_auto_throttle_pgain

float v_ctl_auto_throttle_pgain

Definition at line 61 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_auto_throttle_pitch_of_vz_dgain

float v_ctl_auto_throttle_pitch_of_vz_dgain

Definition at line 67 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_auto_throttle_pitch_of_vz_pgain

float v_ctl_auto_throttle_pitch_of_vz_pgain

Definition at line 66 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_auto_throttle_submode

uint8_t v_ctl_auto_throttle_submode

Definition at line 49 of file guidance_v.c.

Referenced by v_ctl_altitude_loop(), v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_auto_throttle_sum_err

float v_ctl_auto_throttle_sum_err

Definition at line 64 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_climb_mode

uint8_t v_ctl_climb_mode

Definition at line 48 of file guidance_v.c.

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

v_ctl_climb_setpoint

float v_ctl_climb_setpoint

Definition at line 47 of file guidance_v.c.

Referenced by v_ctl_altitude_loop(), v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_mode

uint8_t v_ctl_mode

Definition at line 36 of file guidance_v.c.

Referenced by v_ctl_guidance_loop(), and v_ctl_init().

v_ctl_pitch_setpoint

float v_ctl_pitch_setpoint

Definition at line 91 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), v_ctl_guidance_loop(), and v_ctl_init().

v_ctl_pitch_trim

float v_ctl_pitch_trim

Definition at line 95 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), and v_ctl_init().

v_ctl_throttle_setpoint

pprz_t v_ctl_throttle_setpoint

Definition at line 89 of file guidance_v.c.

Referenced by v_ctl_climb_auto_throttle_loop(), v_ctl_guidance_loop(), v_ctl_init(), v_ctl_landing_loop(), and v_ctl_throttle_slew().

v_ctl_throttle_slewed

pprz_t v_ctl_throttle_slewed

Definition at line 90 of file guidance_v.c.

Referenced by v_ctl_throttle_slew().