Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
guidance_v.c File Reference
+ Include dependency graph for guidance_v.c:

Go to the source code of this file.

Macros

#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE   0.4
 
#define GUIDANCE_V_ADAPT_THROTTLE_ENABLED   TRUE
 
#define GUIDANCE_V_CLIMB_RC_DEADBAND   MAX_PPRZ/10
 
#define GUIDANCE_V_MAX_RC_CLIMB_SPEED   GUIDANCE_V_REF_MIN_ZD
 
#define GUIDANCE_V_MAX_RC_DESCENT_SPEED   GUIDANCE_V_REF_MAX_ZD
 
#define GUIDANCE_V_MIN_ERR_Z   POS_BFP_OF_REAL(-10.)
 
#define GUIDANCE_V_MAX_ERR_Z   POS_BFP_OF_REAL(10.)
 
#define GUIDANCE_V_MIN_ERR_ZD   SPEED_BFP_OF_REAL(-10.)
 
#define GUIDANCE_V_MAX_ERR_ZD   SPEED_BFP_OF_REAL(10.)
 
#define GUIDANCE_V_MAX_SUM_ERR   2000000
 
#define GUIDANCE_V_MAX_CMD   0.9*MAX_PPRZ
 
#define GUIDANCE_V_GUIDED_MODE_ZHOLD   0
 
#define GUIDANCE_V_GUIDED_MODE_CLIMB   1
 
#define GUIDANCE_V_GUIDED_MODE_THROTTLE   2
 
#define FF_CMD_FRAC   18
 

Functions

static int32_t get_vertical_thrust_coeff (void)
 get the cosine of the angle between thrust vector and gravity vector More...
 
static void send_vert_loop (struct transport_tx *trans, struct link_device *dev)
 
static void send_tune_vert (struct transport_tx *trans, struct link_device *dev)
 
void guidance_v_init (void)
 
void guidance_v_read_rc (void)
 
void guidance_v_mode_changed (uint8_t new_mode)
 
void guidance_v_notify_in_flight (bool in_flight)
 
void guidance_v_thrust_adapt (bool in_flight)
 
void guidance_v_run (bool in_flight)
 
void guidance_v_z_enter (void)
 
void guidance_v_set_ref (int32_t pos, int32_t speed, int32_t accel)
 Set guidance ref parameters. More...
 
void run_hover_loop (bool in_flight)
 
void guidance_v_from_nav (bool in_flight)
 Set guidance setpoint from NAV and run hover loop. More...
 
void guidance_v_guided_enter (void)
 Enter GUIDED mode control. More...
 
void guidance_v_guided_run (bool in_flight)
 Run GUIDED mode control. More...
 
bool guidance_v_set_guided_z (float z)
 Set z setpoint in GUIDED mode. More...
 
bool guidance_v_set_guided_vz (float vz)
 Set z velocity setpoint in GUIDED mode. More...
 
bool guidance_v_set_guided_th (float th)
 

Variables

uint8_t guidance_v_mode
 
int32_t guidance_v_ff_cmd
 feed-forward command More...
 
int32_t guidance_v_fb_cmd
 feed-back command More...
 
int32_t guidance_v_delta_t
 thrust command. More...
 
float guidance_v_nominal_throttle
 nominal throttle for hover. More...
 
bool guidance_v_adapt_throttle_enabled
 Use adaptive throttle command estimation. More...
 
static bool desired_zd_updated
 
int guidance_v_guided_mode
 
int32_t guidance_v_rc_delta_t
 Direct throttle from radio control. More...
 
int32_t guidance_v_rc_zd_sp
 Vertical speed setpoint from radio control. More...
 
int32_t guidance_v_z_sp
 altitude setpoint in meters (input). More...
 
int32_t guidance_v_zd_sp
 vertical speed setpoint in meter/s (input). More...
 
int32_t guidance_v_th_sp
 
int32_t guidance_v_z_ref
 altitude reference in meters. More...
 
int32_t guidance_v_zd_ref
 vertical speed reference in meter/s. More...
 
int32_t guidance_v_zdd_ref
 vertical acceleration reference in meter/s^2. More...
 
int32_t guidance_v_kp
 vertical control P-gain More...
 
int32_t guidance_v_kd
 vertical control D-gain More...
 
int32_t guidance_v_ki
 vertical control I-gain More...
 
int32_t guidance_v_z_sum_err
 accumulator for I-gain More...
 
int32_t guidance_v_thrust_coeff
 

Detailed Description

Vertical guidance for rotorcrafts.

Definition in file guidance_v.c.

Macro Definition Documentation

◆ FF_CMD_FRAC

#define FF_CMD_FRAC   18

Definition at line 410 of file guidance_v.c.

◆ GUIDANCE_V_ADAPT_THROTTLE_ENABLED

#define GUIDANCE_V_ADAPT_THROTTLE_ENABLED   TRUE

Definition at line 60 of file guidance_v.c.

◆ GUIDANCE_V_CLIMB_RC_DEADBAND

#define GUIDANCE_V_CLIMB_RC_DEADBAND   MAX_PPRZ/10

Definition at line 68 of file guidance_v.c.

◆ GUIDANCE_V_GUIDED_MODE_CLIMB

#define GUIDANCE_V_GUIDED_MODE_CLIMB   1

Definition at line 113 of file guidance_v.c.

◆ GUIDANCE_V_GUIDED_MODE_THROTTLE

#define GUIDANCE_V_GUIDED_MODE_THROTTLE   2

Definition at line 114 of file guidance_v.c.

◆ GUIDANCE_V_GUIDED_MODE_ZHOLD

#define GUIDANCE_V_GUIDED_MODE_ZHOLD   0

Definition at line 112 of file guidance_v.c.

◆ GUIDANCE_V_MAX_CMD

#define GUIDANCE_V_MAX_CMD   0.9*MAX_PPRZ

Definition at line 100 of file guidance_v.c.

◆ GUIDANCE_V_MAX_ERR_Z

#define GUIDANCE_V_MAX_ERR_Z   POS_BFP_OF_REAL(10.)

Definition at line 84 of file guidance_v.c.

◆ GUIDANCE_V_MAX_ERR_ZD

#define GUIDANCE_V_MAX_ERR_ZD   SPEED_BFP_OF_REAL(10.)

Definition at line 92 of file guidance_v.c.

◆ GUIDANCE_V_MAX_RC_CLIMB_SPEED

#define GUIDANCE_V_MAX_RC_CLIMB_SPEED   GUIDANCE_V_REF_MIN_ZD

Definition at line 72 of file guidance_v.c.

◆ GUIDANCE_V_MAX_RC_DESCENT_SPEED

#define GUIDANCE_V_MAX_RC_DESCENT_SPEED   GUIDANCE_V_REF_MAX_ZD

Definition at line 76 of file guidance_v.c.

◆ GUIDANCE_V_MAX_SUM_ERR

#define GUIDANCE_V_MAX_SUM_ERR   2000000

Definition at line 96 of file guidance_v.c.

◆ GUIDANCE_V_MIN_ERR_Z

#define GUIDANCE_V_MIN_ERR_Z   POS_BFP_OF_REAL(-10.)

Definition at line 80 of file guidance_v.c.

◆ GUIDANCE_V_MIN_ERR_ZD

#define GUIDANCE_V_MIN_ERR_ZD   SPEED_BFP_OF_REAL(-10.)

Definition at line 88 of file guidance_v.c.

◆ GUIDANCE_V_NOMINAL_HOVER_THROTTLE

#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE   0.4

Definition at line 58 of file guidance_v.c.

Function Documentation

◆ get_vertical_thrust_coeff()

static int32_t get_vertical_thrust_coeff ( void  )
static

get the cosine of the angle between thrust vector and gravity vector

Definition at line 382 of file guidance_v.c.

References BFP_OF_REAL, f, INT32_TRIG_FRAC, Int32RMat::m, and stateGetNedToBodyRMat_i().

Referenced by guidance_v_thrust_adapt().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ guidance_v_from_nav()

◆ guidance_v_guided_enter()

void guidance_v_guided_enter ( void  )

Enter GUIDED mode control.

Definition at line 505 of file guidance_v.c.

References guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, guidance_v_z_sp, guidance_v_z_sum_err, guidance_v_zd_sp, GuidanceVSetRef, stateGetPositionNed_i(), stateGetSpeedNed_i(), and NedCoor_i::z.

Referenced by guidance_v_mode_changed().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ guidance_v_guided_run()

void guidance_v_guided_run ( bool  in_flight)

◆ guidance_v_init()

◆ guidance_v_mode_changed()

◆ guidance_v_notify_in_flight()

void guidance_v_notify_in_flight ( bool  in_flight)

Definition at line 271 of file guidance_v.c.

References gv_adapt_init().

+ Here is the call graph for this function:

◆ guidance_v_read_rc()

◆ guidance_v_run()

◆ guidance_v_set_guided_th()

bool guidance_v_set_guided_th ( float  th)

◆ guidance_v_set_guided_vz()

bool guidance_v_set_guided_vz ( float  vz)

Set z velocity setpoint in GUIDED mode.

Parameters
vzSetpoint (down is positive) in meters/second.
Returns
TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED)

Definition at line 568 of file guidance_v.c.

References guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_CLIMB, guidance_v_mode, GUIDANCE_V_MODE_GUIDED, guidance_v_zd_sp, and SPEED_BFP_OF_REAL.

Referenced by autopilot_guided_move_ned(), and autopilot_guided_update().

+ Here is the caller graph for this function:

◆ guidance_v_set_guided_z()

bool guidance_v_set_guided_z ( float  z)

Set z setpoint in GUIDED mode.

Parameters
zSetpoint (down is positive) in meters.
Returns
TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED)

Definition at line 551 of file guidance_v.c.

References guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, guidance_v_mode, GUIDANCE_V_MODE_GUIDED, guidance_v_z_sp, guidance_v_zd_sp, and POS_BFP_OF_REAL.

Referenced by autopilot_guided_goto_ned(), and autopilot_guided_update().

+ Here is the caller graph for this function:

◆ guidance_v_set_ref()

void guidance_v_set_ref ( int32_t  pos,
int32_t  speed,
int32_t  accel 
)

Set guidance ref parameters.

Definition at line 372 of file guidance_v.c.

References guidance_v_z_ref, guidance_v_zd_ref, guidance_v_zdd_ref, and gv_set_ref().

+ Here is the call graph for this function:

◆ guidance_v_thrust_adapt()

void guidance_v_thrust_adapt ( bool  in_flight)

Definition at line 278 of file guidance_v.c.

References desired_zd_updated, get_vertical_thrust_coeff(), guidance_v_thrust_coeff, guidance_v_zd_ref, gv_adapt_init(), gv_adapt_run(), INT32_TRIG_FRAC, stabilization_cmd, and stateGetAccelNed_i().

Referenced by guidance_v_run().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ guidance_v_z_enter()

void guidance_v_z_enter ( void  )

Definition at line 359 of file guidance_v.c.

References guidance_v_z_sp, guidance_v_z_sum_err, guidance_v_zd_sp, GuidanceVSetRef, stateGetPositionNed_i(), and NedCoor_i::z.

+ Here is the call graph for this function:

◆ run_hover_loop()

◆ send_tune_vert()

static void send_tune_vert ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 168 of file guidance_v.c.

References dev, guidance_v_z_ref, guidance_v_z_sp, guidance_v_zd_ref, and stateGetPositionNed_i().

Referenced by guidance_v_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ send_vert_loop()

static void send_vert_loop ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Variable Documentation

◆ desired_zd_updated

bool desired_zd_updated
static

◆ guidance_v_adapt_throttle_enabled

bool guidance_v_adapt_throttle_enabled

Use adaptive throttle command estimation.

Definition at line 109 of file guidance_v.c.

Referenced by guidance_v_init(), and run_hover_loop().

◆ guidance_v_delta_t

int32_t guidance_v_delta_t

thrust command.

summation of feed-forward and feed-back commands, valid range 0 : MAX_PPRZ

Definition at line 106 of file guidance_v.c.

Referenced by guidance_hybrid_vertical(), guidance_v_from_nav(), guidance_v_guided_run(), guidance_v_run(), run_hover_loop(), and send_vert_loop().

◆ guidance_v_fb_cmd

int32_t guidance_v_fb_cmd

feed-back command

Definition at line 105 of file guidance_v.c.

Referenced by run_hover_loop(), and send_vert_loop().

◆ guidance_v_ff_cmd

int32_t guidance_v_ff_cmd

feed-forward command

Definition at line 104 of file guidance_v.c.

Referenced by run_hover_loop(), and send_vert_loop().

◆ guidance_v_guided_mode

◆ guidance_v_kd

int32_t guidance_v_kd

vertical control D-gain

Definition at line 137 of file guidance_v.c.

Referenced by guidance_hybrid_vertical(), guidance_v_init(), and run_hover_loop().

◆ guidance_v_ki

int32_t guidance_v_ki

vertical control I-gain

Definition at line 138 of file guidance_v.c.

Referenced by guidance_hybrid_vertical(), guidance_v_init(), and run_hover_loop().

◆ guidance_v_kp

int32_t guidance_v_kp

vertical control P-gain

Definition at line 136 of file guidance_v.c.

Referenced by guidance_hybrid_vertical(), guidance_v_init(), and run_hover_loop().

◆ guidance_v_mode

◆ guidance_v_nominal_throttle

float guidance_v_nominal_throttle

nominal throttle for hover.

This is only used if GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined! Unit: factor of MAX_PPRZ with range 0.1 : 0.9

Definition at line 108 of file guidance_v.c.

Referenced by guidance_hybrid_vertical(), guidance_v_init(), run_hover_loop(), and vertical_ctrl_module_init().

◆ guidance_v_rc_delta_t

int32_t guidance_v_rc_delta_t

Direct throttle from radio control.

range 0:MAX_PPRZ

Definition at line 121 of file guidance_v.c.

Referenced by guidance_v_from_nav(), guidance_v_guided_run(), guidance_v_read_rc(), and guidance_v_run().

◆ guidance_v_rc_zd_sp

int32_t guidance_v_rc_zd_sp

Vertical speed setpoint from radio control.

fixed point representation: Q12.19 accuracy 0.0000019, range +/-4096

Definition at line 127 of file guidance_v.c.

Referenced by guidance_v_read_rc(), and guidance_v_run().

◆ guidance_v_th_sp

int32_t guidance_v_th_sp

Definition at line 131 of file guidance_v.c.

Referenced by guidance_v_guided_run(), and guidance_v_set_guided_th().

◆ guidance_v_thrust_coeff

int32_t guidance_v_thrust_coeff

◆ guidance_v_z_ref

int32_t guidance_v_z_ref

altitude reference in meters.

fixed point representation: Q23.8 accuracy 0.0039, range 8388km

Definition at line 132 of file guidance_v.c.

Referenced by gps_sim_hitl_event(), guidance_indi_run(), guidance_v_set_ref(), run_hover_loop(), send_tune_vert(), and send_vert_loop().

◆ guidance_v_z_sp

int32_t guidance_v_z_sp

altitude setpoint in meters (input).

fixed point representation: Q23.8 accuracy 0.0039, range 8388km

Definition at line 129 of file guidance_v.c.

Referenced by guidance_v_from_nav(), guidance_v_guided_enter(), guidance_v_guided_run(), guidance_v_run(), guidance_v_set_guided_z(), guidance_v_z_enter(), send_fp(), send_tune_vert(), and send_vert_loop().

◆ guidance_v_z_sum_err

int32_t guidance_v_z_sum_err

◆ guidance_v_zd_ref

int32_t guidance_v_zd_ref

vertical speed reference in meter/s.

fixed point representation: Q12.19 accuracy 0.0000038, range 4096

Definition at line 133 of file guidance_v.c.

Referenced by gps_sim_hitl_event(), guidance_indi_run(), guidance_v_set_ref(), guidance_v_thrust_adapt(), run_hover_loop(), send_tune_vert(), and send_vert_loop().

◆ guidance_v_zd_sp

int32_t guidance_v_zd_sp

◆ guidance_v_zdd_ref

int32_t guidance_v_zdd_ref

vertical acceleration reference in meter/s^2.

fixed point representation: Q21.10 accuracy 0.0009766, range 2097152

Definition at line 134 of file guidance_v.c.

Referenced by gps_sim_hitl_event(), guidance_indi_run(), guidance_v_set_ref(), run_hover_loop(), and send_vert_loop().