#include "std.h"
#include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"

Include dependency graph for guidance_v.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Macros
#define	GUIDANCE_V_MODE_KILL 0

#define	GUIDANCE_V_MODE_RC_DIRECT 1

#define	GUIDANCE_V_MODE_RC_CLIMB 2

#define	GUIDANCE_V_MODE_CLIMB 3

#define	GUIDANCE_V_MODE_HOVER 4

#define	GUIDANCE_V_MODE_NAV 5

#define	GUIDANCE_V_MODE_MODULE 6

#define	GUIDANCE_V_MODE_FLIP 7

#define	GUIDANCE_V_MODE_GUIDED 8

#define	GuidanceVSetRef guidance_v_set_ref

#define	guidance_v_SetKi(_val)

Functions
void	guidance_v_init (void)

void	guidance_v_read_rc (void)

void	guidance_v_mode_changed (uint8_t new_mode)

void	guidance_v_thrust_adapt (bool in_flight)

void	guidance_v_notify_in_flight (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_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_z_sum_err
	accumulator for I-gain More...

int32_t	guidance_v_ff_cmd
	feed-forward command More...

int32_t	guidance_v_fb_cmd
	feed-back command More...

int32_t	guidance_v_rc_delta_t
	Direct throttle from radio control. 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...

int32_t	guidance_v_thrust_coeff

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...

Detailed Description

Vertical guidance for rotorcrafts.

Definition in file guidance_v.h.

Macro Definition Documentation

◆ GUIDANCE_V_MODE_CLIMB

#define GUIDANCE_V_MODE_CLIMB 3

Definition at line 38 of file guidance_v.h.

◆ GUIDANCE_V_MODE_FLIP

#define GUIDANCE_V_MODE_FLIP 7

Definition at line 42 of file guidance_v.h.

◆ GUIDANCE_V_MODE_GUIDED

#define GUIDANCE_V_MODE_GUIDED 8

Definition at line 43 of file guidance_v.h.

◆ GUIDANCE_V_MODE_HOVER

#define GUIDANCE_V_MODE_HOVER 4

Definition at line 39 of file guidance_v.h.

◆ GUIDANCE_V_MODE_KILL

#define GUIDANCE_V_MODE_KILL 0

Definition at line 35 of file guidance_v.h.

◆ GUIDANCE_V_MODE_MODULE

#define GUIDANCE_V_MODE_MODULE 6

Definition at line 41 of file guidance_v.h.

◆ GUIDANCE_V_MODE_NAV

#define GUIDANCE_V_MODE_NAV 5

Definition at line 40 of file guidance_v.h.

◆ GUIDANCE_V_MODE_RC_CLIMB

#define GUIDANCE_V_MODE_RC_CLIMB 2

Definition at line 37 of file guidance_v.h.

◆ GUIDANCE_V_MODE_RC_DIRECT

#define GUIDANCE_V_MODE_RC_DIRECT 1

Definition at line 36 of file guidance_v.h.

◆ guidance_v_SetKi

#define guidance_v_SetKi ( _val )

Value:

    {      \
    guidance_v_ki = _val;       \
    guidance_v_z_sum_err = 0;     \
  }

Definition at line 150 of file guidance_v.h.

◆ GuidanceVSetRef

#define GuidanceVSetRef guidance_v_set_ref

Definition at line 120 of file guidance_v.h.

Function Documentation

◆ guidance_v_from_nav()

void guidance_v_from_nav ( bool in_flight )

Set guidance setpoint from NAV and run hover loop.

Definition at line 473 of file guidance_v.c.

References guidance_hybrid_vertical(), guidance_v_delta_t, guidance_v_rc_delta_t, guidance_v_z_sp, guidance_v_z_sum_err, guidance_v_zd_sp, GuidanceVSetRef, gv_update_ref_from_z_sp(), gv_update_ref_from_zd_sp(), Min, nav_climb, nav_flight_altitude, nav_throttle, radio_control, RC_OK, run_hover_loop(), stabilization_cmd, stateGetPositionNed_i(), stateGetSpeedNed_i(), RadioControl::status, vertical_mode, VERTICAL_MODE_ALT, VERTICAL_MODE_CLIMB, VERTICAL_MODE_MANUAL, 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:

◆ 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 )

Run GUIDED mode control.

Definition at line 519 of file guidance_v.c.

References guidance_v_delta_t, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_CLIMB, GUIDANCE_V_GUIDED_MODE_THROTTLE, GUIDANCE_V_GUIDED_MODE_ZHOLD, guidance_v_rc_delta_t, guidance_v_th_sp, guidance_v_z_sp, guidance_v_zd_sp, gv_update_ref_from_z_sp(), gv_update_ref_from_zd_sp(), Min, radio_control, RC_OK, run_hover_loop(), stabilization_cmd, stateGetPositionNed_i(), RadioControl::status, 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:

◆ guidance_v_init()

void guidance_v_init ( void )

Definition at line 178 of file guidance_v.c.

References BFP_OF_REAL, DefaultPeriodic, desired_zd_updated, f, GUIDANCE_V_ADAPT_THROTTLE_ENABLED, guidance_v_adapt_throttle_enabled, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, guidance_v_kd, guidance_v_ki, guidance_v_kp, guidance_v_mode, GUIDANCE_V_MODE_KILL, guidance_v_module_init(), GUIDANCE_V_NOMINAL_HOVER_THROTTLE, guidance_v_nominal_throttle, guidance_v_thrust_coeff, guidance_v_z_sum_err, gv_adapt_init(), INT32_TRIG_FRAC, register_periodic_telemetry(), send_tune_vert(), and send_vert_loop().

Here is the call graph for this function:

◆ guidance_v_mode_changed()

void guidance_v_mode_changed ( uint8_t new_mode )

Definition at line 231 of file guidance_v.c.

References guidance_v_guided_enter(), guidance_v_mode, 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(), guidance_v_z_sum_err, guidance_v_zd_sp, GuidanceVSetRef, stateGetPositionNed_i(), and stateGetSpeedNed_i().

Referenced by autopilot_static_set_mode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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()

void guidance_v_read_rc ( void )

Definition at line 209 of file guidance_v.c.

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

Referenced by autopilot_static_on_rc_frame().

Here is the caller graph for this function:

◆ guidance_v_run()

void guidance_v_run ( bool in_flight )

Definition at line 299 of file guidance_v.c.

References desired_zd_updated, guidance_v_delta_t, guidance_v_from_nav(), guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, guidance_v_guided_run(), guidance_v_mode, 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_adapt(), guidance_v_z_sp, guidance_v_zd_sp, gv_update_ref_from_zd_sp(), Min, radio_control, RC_OK, run_hover_loop(), stabilization_cmd, stateGetPositionNed_i(), RadioControl::status, and NedCoor_i::z.

Referenced by autopilot_static_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ guidance_v_set_guided_th()

bool guidance_v_set_guided_th ( float th )

Definition at line 582 of file guidance_v.c.

References guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_THROTTLE, guidance_v_mode, GUIDANCE_V_MODE_GUIDED, guidance_v_th_sp, GuidanceVSetRef, MAX_PPRZ, stateGetPositionNed_i(), and stateGetSpeedNed_i().

Here is the call graph for this function:

◆ guidance_v_set_guided_vz()

bool guidance_v_set_guided_vz ( float vz )

Set z velocity setpoint in GUIDED mode.

Parameters

vz	Setpoint (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

z	Setpoint (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()

void run_hover_loop ( bool in_flight )

Definition at line 412 of file guidance_v.c.

References BFP_OF_REAL, desired_zd_updated, FF_CMD_FRAC, guidance_v_adapt_throttle_enabled, guidance_v_delta_t, guidance_v_fb_cmd, guidance_v_ff_cmd, guidance_v_kd, guidance_v_ki, guidance_v_kp, GUIDANCE_V_MAX_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_nominal_throttle, guidance_v_thrust_coeff, guidance_v_z_ref, guidance_v_z_sum_err, guidance_v_zd_ref, guidance_v_zdd_ref, 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_from_nav(), guidance_v_guided_run(), and guidance_v_run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ 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_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

uint8_t guidance_v_mode

Definition at line 103 of file guidance_v.c.

Referenced by booz_fms_impl_periodic(), guidance_v_init(), guidance_v_mode_changed(), guidance_v_run(), guidance_v_set_guided_th(), guidance_v_set_guided_vz(), guidance_v_set_guided_z(), nav_get_speed_sp_from_go(), nav_get_speed_sp_from_line(), and send_status().

◆ 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_thrust_coeff

int32_t guidance_v_thrust_coeff

Definition at line 142 of file guidance_v.c.

Referenced by guidance_h_traj_run(), guidance_v_init(), guidance_v_thrust_adapt(), and run_hover_loop().

◆ 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

accumulator for I-gain

Definition at line 140 of file guidance_v.c.

Referenced by guidance_v_from_nav(), guidance_v_guided_enter(), guidance_v_init(), guidance_v_mode_changed(), guidance_v_z_enter(), run_hover_loop(), and send_vert_loop().

◆ 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_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

vertical speed setpoint in meter/s (input).

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

Definition at line 130 of file guidance_v.c.

Referenced by autopilot_static_set_mode(), guidance_v_from_nav(), guidance_v_guided_enter(), guidance_v_guided_run(), guidance_v_mode_changed(), guidance_v_run(), guidance_v_set_guided_vz(), guidance_v_set_guided_z(), guidance_v_z_enter(), nav_get_speed_sp_from_go(), nav_get_speed_sp_from_line(), and send_vert_loop().

◆ 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_v_set_ref(), run_hover_loop(), and send_vert_loop().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ GUIDANCE_V_MODE_CLIMB

◆ GUIDANCE_V_MODE_FLIP

◆ GUIDANCE_V_MODE_GUIDED

◆ GUIDANCE_V_MODE_HOVER

◆ GUIDANCE_V_MODE_KILL

◆ GUIDANCE_V_MODE_MODULE

◆ GUIDANCE_V_MODE_NAV

◆ GUIDANCE_V_MODE_RC_CLIMB

◆ GUIDANCE_V_MODE_RC_DIRECT

◆ guidance_v_SetKi

◆ GuidanceVSetRef

Function Documentation

◆ guidance_v_from_nav()

◆ guidance_v_guided_enter()

◆ guidance_v_guided_run()

◆ guidance_v_init()

◆ guidance_v_mode_changed()

◆ guidance_v_notify_in_flight()

◆ guidance_v_read_rc()

◆ guidance_v_run()

◆ guidance_v_set_guided_th()

◆ guidance_v_set_guided_vz()

◆ guidance_v_set_guided_z()

◆ guidance_v_set_ref()

◆ guidance_v_thrust_adapt()

◆ guidance_v_z_enter()

◆ run_hover_loop()

Variable Documentation

◆ guidance_v_adapt_throttle_enabled

◆ guidance_v_delta_t

◆ guidance_v_fb_cmd

◆ guidance_v_ff_cmd

◆ guidance_v_kd

◆ guidance_v_ki

◆ guidance_v_kp

◆ guidance_v_mode

◆ guidance_v_nominal_throttle

◆ guidance_v_rc_delta_t

◆ guidance_v_thrust_coeff

◆ guidance_v_z_ref

◆ guidance_v_z_sp

◆ guidance_v_z_sum_err

◆ guidance_v_zd_ref

◆ guidance_v_zd_sp

◆ guidance_v_zdd_ref