Vertical guidance for rotorcrafts. More...

#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
#include "modules/radio_control/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/navigation.h"
#include "state.h"
#include "math/pprz_algebra_int.h"
#include "modules/datalink/telemetry.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_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_GUIDED_MODE_ZHOLD 0

#define	GUIDANCE_V_GUIDED_MODE_CLIMB 1

#define	GUIDANCE_V_GUIDED_MODE_THROTTLE 2

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

static int32_t	get_vertical_thrust_coeff (void)
	get the cosine of the angle between thrust vector and gravity vector More...

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	guidance_v_update_ref (void)

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

void	guidance_v_set_z (float z)
	Set z position setpoint. More...

void	guidance_v_set_vz (float vz)
	Set z velocity setpoint. More...

void	guidance_v_set_th (float th)
	Set throttle setpoint. More...

Variables
struct VerticalGuidance	guidance_v

static bool	desired_zd_updated

static int	guidance_v_guided_mode

Detailed Description

Vertical guidance for rotorcrafts.

Definition in file guidance_v.c.

Macro Definition Documentation

◆ GUIDANCE_V_CLIMB_RC_DEADBAND

#define GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10

Definition at line 48 of file guidance_v.c.

◆ GUIDANCE_V_GUIDED_MODE_CLIMB

#define GUIDANCE_V_GUIDED_MODE_CLIMB 1

Definition at line 64 of file guidance_v.c.

◆ GUIDANCE_V_GUIDED_MODE_THROTTLE

#define GUIDANCE_V_GUIDED_MODE_THROTTLE 2

Definition at line 65 of file guidance_v.c.

◆ GUIDANCE_V_GUIDED_MODE_ZHOLD

#define GUIDANCE_V_GUIDED_MODE_ZHOLD 0

Definition at line 63 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 52 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 56 of file guidance_v.c.

◆ GUIDANCE_V_NOMINAL_HOVER_THROTTLE

#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4

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

References BFP_OF_REAL, 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()

void guidance_v_from_nav ( bool in_flight )

Set guidance setpoint from NAV and run hover loop.

Definition at line 316 of file guidance_v.c.

References RotorcraftNavigation::climb, VerticalGuidance::delta_t, guidance_v, guidance_v_guided_run(), guidance_v_run_enter(), guidance_v_run_pos(), guidance_v_run_speed(), guidance_v_update_ref(), GuidanceVSetRef, gv_update_ref_from_z_sp(), gv_update_ref_from_zd_sp(), Min, nav, RotorcraftNavigation::nav_altitude, NAV_VERTICAL_MODE_ALT, NAV_VERTICAL_MODE_CLIMB, NAV_VERTICAL_MODE_GUIDED, NAV_VERTICAL_MODE_MANUAL, POS_BFP_OF_REAL, radio_control, VerticalGuidance::rc_delta_t, RC_OK, SPEED_BFP_OF_REAL, stabilization_cmd, stateGetPositionNed_i(), stateGetSpeedNed_i(), RadioControl::status, RotorcraftNavigation::throttle, RotorcraftNavigation::vertical_mode, NedCoor_i::z, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.

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

References guidance_v_run_enter(), guidance_v_set_z(), GuidanceVSetRef, stateGetPositionNed_f(), stateGetPositionNed_i(), and stateGetSpeedNed_i().

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

References VerticalGuidance::delta_t, guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_CLIMB, GUIDANCE_V_GUIDED_MODE_THROTTLE, GUIDANCE_V_GUIDED_MODE_ZHOLD, guidance_v_run_pos(), guidance_v_run_speed(), guidance_v_update_ref(), gv_update_ref_from_z_sp(), gv_update_ref_from_zd_sp(), Min, radio_control, VerticalGuidance::rc_delta_t, RC_OK, stabilization_cmd, stateGetPositionNed_i(), RadioControl::status, VerticalGuidance::th_sp, NedCoor_i::z, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.

Referenced by guidance_v_from_nav(), and 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 83 of file guidance_v.c.

References BFP_OF_REAL, DefaultPeriodic, desired_zd_updated, guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, GUIDANCE_V_MODE_KILL, guidance_v_module_init(), gv_adapt_init(), INT32_TRIG_FRAC, VerticalGuidance::mode, register_periodic_telemetry(), send_tune_vert(), and VerticalGuidance::thrust_coeff.

Here is the call graph for this function:

◆ guidance_v_mode_changed()

void guidance_v_mode_changed ( uint8_t new_mode )

Definition at line 126 of file guidance_v.c.

References guidance_v, guidance_v_guided_enter(), 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_run_enter(), GuidanceVSetRef, VerticalGuidance::mode, stateGetPositionNed_i(), stateGetSpeedNed_i(), and VerticalGuidance::zd_sp.

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

References guidance_v, GUIDANCE_V_CLIMB_RC_DEADBAND, GUIDANCE_V_MAX_RC_CLIMB_SPEED, GUIDANCE_V_MAX_RC_DESCENT_SPEED, MAX_PPRZ, radio_control, RADIO_THROTTLE, VerticalGuidance::rc_delta_t, VerticalGuidance::rc_zd_sp, 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 223 of file guidance_v.c.

References VerticalGuidance::delta_t, desired_zd_updated, guidance_v, guidance_v_from_nav(), guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, guidance_v_guided_run(), 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_run_speed(), guidance_v_thrust_adapt(), gv_update_ref_from_zd_sp(), Min, VerticalGuidance::mode, radio_control, VerticalGuidance::rc_delta_t, RC_OK, VerticalGuidance::rc_zd_sp, stabilization_cmd, stateGetPositionNed_i(), RadioControl::status, NedCoor_i::z, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.

Referenced by autopilot_static_periodic().

Here is the call graph for this function:

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

References guidance_v, gv_set_ref(), VerticalGuidance::z_ref, VerticalGuidance::zd_ref, and VerticalGuidance::zdd_ref.

Here is the call graph for this function:

◆ guidance_v_set_th()

void guidance_v_set_th ( float th )

Set throttle setpoint.

Parameters

th	Throttle setpoint between 0. and 1.

Definition at line 410 of file guidance_v.c.

References guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_THROTTLE, GuidanceVSetRef, MAX_PPRZ, stateGetPositionNed_i(), stateGetSpeedNed_i(), and VerticalGuidance::th_sp.

Here is the call graph for this function:

◆ guidance_v_set_vz()

void guidance_v_set_vz ( float vz )

Set z velocity setpoint.

Parameters

vz	Setpoint (down is positive) in meters/second.

Definition at line 402 of file guidance_v.c.

References guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_CLIMB, SPEED_BFP_OF_REAL, and VerticalGuidance::zd_sp.

Referenced by autopilot_guided_move_ned(), autopilot_guided_update(), and autopilot_static_set_mode().

Here is the caller graph for this function:

◆ guidance_v_set_z()

void guidance_v_set_z ( float z )

Set z position setpoint.

Parameters

z	Setpoint (down is positive) in meters.

Definition at line 392 of file guidance_v.c.

References guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, POS_BFP_OF_REAL, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.

Referenced by autopilot_guided_goto_ned(), autopilot_guided_update(), and guidance_v_guided_enter().

Here is the caller graph for this function:

◆ guidance_v_thrust_adapt()

void guidance_v_thrust_adapt ( bool in_flight )

Definition at line 202 of file guidance_v.c.

References desired_zd_updated, get_vertical_thrust_coeff(), guidance_v, gv_adapt_init(), gv_adapt_run(), INT32_TRIG_FRAC, stabilization_cmd, stateGetAccelNed_i(), VerticalGuidance::thrust_coeff, and VerticalGuidance::zd_ref.

Referenced by guidance_v_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ guidance_v_update_ref()

void guidance_v_update_ref ( void )

Definition at line 305 of file guidance_v.c.

References desired_zd_updated, guidance_v, 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, VerticalGuidance::z_ref, VerticalGuidance::zd_ref, and VerticalGuidance::zdd_ref.

Referenced by guidance_v_from_nav(), and guidance_v_guided_run().

Here is the caller graph for this function:

◆ guidance_v_z_enter()

void guidance_v_z_enter ( void )

Definition at line 283 of file guidance_v.c.

References guidance_v, guidance_v_run_enter(), GuidanceVSetRef, stateGetPositionNed_i(), NedCoor_i::z, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.

Here is the call graph for this function:

◆ send_tune_vert()

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

static

Definition at line 73 of file guidance_v.c.

References dev, guidance_v, stateGetPositionNed_i(), VerticalGuidance::z_ref, VerticalGuidance::z_sp, and VerticalGuidance::zd_ref.

Referenced by guidance_v_init().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ desired_zd_updated

bool desired_zd_updated

static

Definition at line 61 of file guidance_v.c.

Referenced by guidance_v_init(), guidance_v_run(), guidance_v_thrust_adapt(), and guidance_v_update_ref().

◆ guidance_v

struct VerticalGuidance guidance_v

◆ guidance_v_guided_mode

int guidance_v_guided_mode

static

Definition at line 67 of file guidance_v.c.

Referenced by guidance_v_guided_run(), guidance_v_init(), guidance_v_run(), guidance_v_set_th(), guidance_v_set_vz(), and guidance_v_set_z().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ GUIDANCE_V_CLIMB_RC_DEADBAND

◆ GUIDANCE_V_GUIDED_MODE_CLIMB

◆ GUIDANCE_V_GUIDED_MODE_THROTTLE

◆ GUIDANCE_V_GUIDED_MODE_ZHOLD

◆ GUIDANCE_V_MAX_RC_CLIMB_SPEED

◆ GUIDANCE_V_MAX_RC_DESCENT_SPEED

◆ GUIDANCE_V_NOMINAL_HOVER_THROTTLE

Function Documentation

◆ get_vertical_thrust_coeff()

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

◆ guidance_v_set_th()

◆ guidance_v_set_vz()

◆ guidance_v_set_z()

◆ guidance_v_thrust_adapt()

◆ guidance_v_update_ref()

◆ guidance_v_z_enter()

◆ send_tune_vert()

Variable Documentation

◆ desired_zd_updated

◆ guidance_v

◆ guidance_v_guided_mode