guidance_v.c File Reference

Vertical guidance for rotorcrafts. More...

#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/navigation.h"
#include "state.h"
#include "math/pprz_algebra_int.h"

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)
bool_t	guidance_v_set_guided_z (float z)
	Set z setpoint in GUIDED mode. More...

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

Referenced by run_hover_loop().

#define GUIDANCE_V_ADAPT_THROTTLE_ENABLED   TRUE

Definition at line 59 of file guidance_v.c.

Referenced by guidance_v_init().

#define GUIDANCE_V_CLIMB_RC_DEADBAND   MAX_PPRZ/10

Definition at line 67 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 83 of file guidance_v.c.

Referenced by run_hover_loop().

#define GUIDANCE_V_MAX_ERR_ZD   SPEED_BFP_OF_REAL(10.)

Definition at line 91 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 71 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 75 of file guidance_v.c.

Referenced by guidance_v_read_rc().

#define GUIDANCE_V_MAX_SUM_ERR   2000000

Definition at line 95 of file guidance_v.c.

Referenced by run_hover_loop().

#define GUIDANCE_V_MIN_ERR_Z   POS_BFP_OF_REAL(-10.)

Definition at line 79 of file guidance_v.c.

Referenced by run_hover_loop().

#define GUIDANCE_V_MIN_ERR_ZD   SPEED_BFP_OF_REAL(-10.)

Definition at line 87 of file guidance_v.c.

Referenced by run_hover_loop().

#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE   0.4

Definition at line 57 of file guidance_v.c.

Referenced by guidance_v_init().

#define GuidanceVSetRef	(		_pos,
			_speed,
			_accel
	)

Value:

{ \
    gv_set_ref(_pos, _speed, _accel);        \
    guidance_v_z_ref = _pos;             \
    guidance_v_zd_ref = _speed;          \
    guidance_v_zdd_ref = _accel;             \
  }

Definition at line 133 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 369 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 174 of file guidance_v.c.

References DefaultPeriodic, GUIDANCE_V_ADAPT_THROTTLE_ENABLED, GUIDANCE_V_MODE_KILL, guidance_v_module_init(), 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 223 of file guidance_v.c.

References GUIDANCE_V_MODE_CLIMB, GUIDANCE_V_MODE_FLIP, GUIDANCE_V_MODE_GUIDED, GUIDANCE_V_MODE_HOVER, GUIDANCE_V_MODE_MODULE, GUIDANCE_V_MODE_NAV, GUIDANCE_V_MODE_RC_CLIMB, guidance_v_module_enter(), GuidanceVSetRef, stateGetPositionNed_i(), stateGetSpeedNed_i(), FloatVect3::z, 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 264 of file guidance_v.c.

References gv_adapt_init().

Here is the call graph for this function:

void guidance_v_read_rc

(

void

)

Definition at line 201 of file guidance_v.c.

References GUIDANCE_V_CLIMB_RC_DEADBAND, GUIDANCE_V_MAX_RC_CLIMB_SPEED, GUIDANCE_V_MAX_RC_DESCENT_SPEED, MAX_PPRZ, radio_control, RADIO_THROTTLE, SPEED_BFP_OF_REAL, and RadioControl::values.

Referenced by autopilot_on_rc_frame().

Here is the caller graph for this function:

void guidance_v_run

(

bool_t

in_flight

)

Definition at line 272 of file guidance_v.c.

References get_vertical_thrust_coeff(), guidance_v_delta_t, GUIDANCE_V_MODE_CLIMB, GUIDANCE_V_MODE_FLIP, GUIDANCE_V_MODE_GUIDED, GUIDANCE_V_MODE_HOVER, GUIDANCE_V_MODE_MODULE, GUIDANCE_V_MODE_NAV, GUIDANCE_V_MODE_RC_CLIMB, GUIDANCE_V_MODE_RC_DIRECT, guidance_v_module_run(), guidance_v_rc_delta_t, guidance_v_rc_zd_sp, guidance_v_thrust_coeff, GuidanceVSetRef, gv_adapt_init(), gv_adapt_run(), gv_update_ref_from_z_sp(), gv_update_ref_from_zd_sp(), INT32_TRIG_FRAC, Min, nav_climb, nav_flight_altitude, nav_throttle, radio_control, RC_OK, run_hover_loop(), stabilization_cmd, stateGetAccelNed_i(), stateGetPositionNed_i(), stateGetSpeedNed_i(), RadioControl::status, vertical_mode, VERTICAL_MODE_ALT, VERTICAL_MODE_CLIMB, VERTICAL_MODE_MANUAL, FloatVect3::z, and NedCoor_i::z.

Referenced by autopilot_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t guidance_v_set_guided_z

(

float

z

)

Set z setpoint in GUIDED mode.

Parameters

z	Setpoint (down is positive) in meters.

Returns

TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED)

Definition at line 453 of file guidance_v.c.

References FALSE, GUIDANCE_V_MODE_GUIDED, POS_BFP_OF_REAL, and TRUE.

Referenced by autopilot_guided_goto_ned().

Here is the caller graph for this function:

static void run_hover_loop ( bool_t  in_flight )

static

Definition at line 399 of file guidance_v.c.

References BFP_OF_REAL, FF_CMD_FRAC, guidance_v_fb_cmd, GUIDANCE_V_MAX_ERR_Z, GUIDANCE_V_MAX_ERR_ZD, GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MIN_ERR_ZD, guidance_v_z_sum_err, gv_adapt_X, GV_ADAPT_X_FRAC, gv_z_ref, GV_Z_REF_FRAC, gv_zd_ref, GV_ZD_REF_FRAC, gv_zdd_ref, GV_ZDD_REF_FRAC, INT32_ACCEL_FRAC, INT32_POS_FRAC, INT32_SPEED_FRAC, INT32_TRIG_FRAC, MAX_PPRZ, stateGetPositionNed_i(), stateGetSpeedNed_i(), and NedCoor_i::z.

Referenced by guidance_v_run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

bool_t guidance_v_adapt_throttle_enabled

Use adaptive throttle command estimation.

Definition at line 104 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 101 of file guidance_v.c.

Referenced by guidance_v_run().

int32_t guidance_v_fb_cmd

feed-back command

Definition at line 100 of file guidance_v.c.

Referenced by run_hover_loop().

int32_t guidance_v_ff_cmd

feed-forward command

Definition at line 99 of file guidance_v.c.

int32_t guidance_v_kd

vertical control D-gain

Definition at line 125 of file guidance_v.c.

int32_t guidance_v_ki

vertical control I-gain

Definition at line 126 of file guidance_v.c.

int32_t guidance_v_kp

vertical control P-gain

Definition at line 124 of file guidance_v.c.

uint8_t guidance_v_mode

Definition at line 98 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 103 of file guidance_v.c.

int32_t guidance_v_rc_delta_t

Direct throttle from radio control.

range 0:MAX_PPRZ

Definition at line 110 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 116 of file guidance_v.c.

Referenced by guidance_v_run().

int32_t guidance_v_thrust_coeff

Definition at line 130 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 120 of file guidance_v.c.

Referenced by gps_sim_hitl_event().

int32_t guidance_v_z_sp

altitude setpoint in meters (input).

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

Definition at line 118 of file guidance_v.c.

Referenced by guidance_indi_run(), and send_fp().

int32_t guidance_v_z_sum_err

accumulator for I-gain

Definition at line 128 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 121 of file guidance_v.c.

Referenced by gps_sim_hitl_event().

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

Referenced by gps_sim_hitl_event().