Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_v.c File Reference

Vertical guidance for rotorcrafts. More...

+ 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 GuidanceVSetRef(_pos, _speed, _accel)
 
#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 run_hover_loop (bool_t in_flight)
 
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_t in_flight)
 
void guidance_v_run (bool_t in_flight)
 

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_t guidance_v_adapt_throttle_enabled
 Use adaptive throttle command estimation. More...
 
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_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

#define FF_CMD_FRAC   18

Definition at line 363 of file guidance_v.c.

Referenced by run_hover_loop().

#define GUIDANCE_V_ADAPT_THROTTLE_ENABLED   TRUE

Definition at line 58 of file guidance_v.c.

Referenced by guidance_v_init().

#define GUIDANCE_V_CLIMB_RC_DEADBAND   MAX_PPRZ/10

Definition at line 66 of file guidance_v.c.

Referenced by guidance_v_read_rc().

#define GUIDANCE_V_MAX_ERR_Z   POS_BFP_OF_REAL(10.)

Definition at line 82 of file guidance_v.c.

Referenced by run_hover_loop().

#define GUIDANCE_V_MAX_ERR_ZD   SPEED_BFP_OF_REAL(10.)

Definition at line 90 of file guidance_v.c.

Referenced by run_hover_loop().

#define GUIDANCE_V_MAX_RC_CLIMB_SPEED   GUIDANCE_V_REF_MIN_ZD

Definition at line 70 of file guidance_v.c.

Referenced by guidance_v_read_rc().

#define GUIDANCE_V_MAX_RC_DESCENT_SPEED   GUIDANCE_V_REF_MAX_ZD

Definition at line 74 of file guidance_v.c.

Referenced by guidance_v_read_rc().

#define GUIDANCE_V_MAX_SUM_ERR   2000000

Definition at line 94 of file guidance_v.c.

Referenced by run_hover_loop().

#define GUIDANCE_V_MIN_ERR_Z   POS_BFP_OF_REAL(-10.)

Definition at line 78 of file guidance_v.c.

Referenced by run_hover_loop().

#define GUIDANCE_V_MIN_ERR_ZD   SPEED_BFP_OF_REAL(-10.)

Definition at line 86 of file guidance_v.c.

Referenced by run_hover_loop().

#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE   0.4

Definition at line 56 of file guidance_v.c.

Referenced by guidance_v_init().

#define GuidanceVSetRef (   _pos,
  _speed,
  _accel 
)
Value:
{ \
gv_set_ref(_pos, _speed, _accel); \
}
int32_t guidance_v_zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.c:121
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:119
int32_t guidance_v_zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.c:120
void gv_set_ref(int32_t alt, int32_t speed, int32_t accel)

Definition at line 132 of file guidance_v.c.

Referenced by guidance_v_mode_changed(), and guidance_v_run().

Function Documentation

static int32_t get_vertical_thrust_coeff ( void  )
static

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

Definition at line 338 of file guidance_v.c.

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

Referenced by guidance_v_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void guidance_v_init ( void  )

Definition at line 171 of file guidance_v.c.

References GUIDANCE_V_ADAPT_THROTTLE_ENABLED, GUIDANCE_V_MODE_KILL, GUIDANCE_V_NOMINAL_HOVER_THROTTLE, gv_adapt_init(), and register_periodic_telemetry().

Referenced by autopilot_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void guidance_v_mode_changed ( uint8_t  new_mode)

Definition at line 213 of file guidance_v.c.

References GUIDANCE_V_MODE_CLIMB, GUIDANCE_V_MODE_HOVER, GUIDANCE_V_MODE_NAV, GUIDANCE_V_MODE_RC_CLIMB, GuidanceVSetRef, stateGetPositionNed_i(), stateGetSpeedNed_i(), and NedCoor_i::z.

Referenced by autopilot_set_mode().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void guidance_v_notify_in_flight ( bool_t  in_flight)

Definition at line 242 of file guidance_v.c.

References gv_adapt_init().

+ Here is the call graph for this function:

void guidance_v_read_rc ( void  )

Variable Documentation

bool_t guidance_v_adapt_throttle_enabled

Use adaptive throttle command estimation.

Definition at line 103 of file guidance_v.c.

int32_t guidance_v_delta_t

thrust command.

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

Definition at line 100 of file guidance_v.c.

Referenced by guidance_v_run().

int32_t guidance_v_fb_cmd

feed-back command

Definition at line 99 of file guidance_v.c.

Referenced by run_hover_loop().

int32_t guidance_v_ff_cmd

feed-forward command

Definition at line 98 of file guidance_v.c.

int32_t guidance_v_kd

vertical control D-gain

Definition at line 124 of file guidance_v.c.

int32_t guidance_v_ki

vertical control I-gain

Definition at line 125 of file guidance_v.c.

int32_t guidance_v_kp

vertical control P-gain

Definition at line 123 of file guidance_v.c.

uint8_t guidance_v_mode

Definition at line 97 of file guidance_v.c.

Referenced by booz_fms_impl_periodic(), and send_status().

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 102 of file guidance_v.c.

int32_t guidance_v_rc_delta_t

Direct throttle from radio control.

range 0:MAX_PPRZ

Definition at line 109 of file guidance_v.c.

Referenced by guidance_v_run().

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 115 of file guidance_v.c.

Referenced by guidance_v_run().

int32_t guidance_v_thrust_coeff

Definition at line 129 of file guidance_v.c.

Referenced by guidance_h_traj_run(), and guidance_v_run().

int32_t guidance_v_z_ref

altitude reference in meters.

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

Definition at line 119 of file guidance_v.c.

int32_t guidance_v_z_sp

altitude setpoint in meters (input).

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

Definition at line 117 of file guidance_v.c.

Referenced by send_fp().

int32_t guidance_v_z_sum_err

accumulator for I-gain

Definition at line 127 of file guidance_v.c.

Referenced by run_hover_loop().

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 120 of file guidance_v.c.

int32_t guidance_v_zd_sp

vertical speed setpoint in meter/s (input).

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

Definition at line 118 of file guidance_v.c.

Referenced by autopilot_set_mode().

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 121 of file guidance_v.c.