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 "modules/core/abi.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.

Functions
static void	rc_cb (uint8_t sender_id UNUSED, struct RadioControl *rc)

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

void	guidance_v_init (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

void	guidance_v_thrust_adapt (bool in_flight)

struct ThrustSetpoint	guidance_v_run (bool in_flight)
	Guidance vertical run functions.

void	guidance_v_z_enter (void)

void	guidance_v_set_ref (int32_t pos, int32_t speed, int32_t accel)
	Set guidance ref parameters.

void	guidance_v_update_ref (void)

struct ThrustSetpoint	guidance_v_from_nav (bool in_flight)
	Set guidance setpoint from NAV and run hover loop.

void	guidance_v_guided_enter (void)
	Enter GUIDED mode control.

struct ThrustSetpoint	guidance_v_guided_run (bool in_flight)
	Run GUIDED mode control.

void	guidance_v_set_z (float z)
	Set z position setpoint.

void	guidance_v_set_vz (float vz)
	Set z velocity setpoint.

void	guidance_v_set_th (float th)
	Set throttle setpoint.

Variables
struct VerticalGuidance	guidance_v

static bool	desired_zd_updated

static int	guidance_v_guided_mode

static abi_event	rc_ev

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

◆ GUIDANCE_V_GUIDED_MODE_CLIMB

#define GUIDANCE_V_GUIDED_MODE_CLIMB 1

Definition at line 65 of file guidance_v.c.

◆ GUIDANCE_V_GUIDED_MODE_THROTTLE

#define GUIDANCE_V_GUIDED_MODE_THROTTLE 2

Definition at line 66 of file guidance_v.c.

◆ GUIDANCE_V_GUIDED_MODE_ZHOLD

#define GUIDANCE_V_GUIDED_MODE_ZHOLD 0

Definition at line 64 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 53 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 57 of file guidance_v.c.

◆ GUIDANCE_V_NOMINAL_HOVER_THROTTLE

#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4

Definition at line 43 of file guidance_v.c.

◆ GUIDANCE_V_RC_ID

#define GUIDANCE_V_RC_ID ABI_BROADCAST

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

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

struct ThrustSetpoint guidance_v_from_nav ( bool in_flight )

Set guidance setpoint from NAV and run hover loop.

Definition at line 300 of file guidance_v.c.

References RotorcraftNavigation::climb, 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(), nav, RotorcraftNavigation::nav_altitude, NAV_VERTICAL_MODE_ALT, NAV_VERTICAL_MODE_CLIMB, NAV_VERTICAL_MODE_GUIDED, NAV_VERTICAL_MODE_MANUAL, POS_BFP_OF_REAL, ThrustSetpoint::sp, SPEED_BFP_OF_REAL, stateGetPositionNed_i(), stateGetSpeedNed_i(), th_sp_from_thrust_i(), RotorcraftNavigation::throttle, THRUST_AXIS_Z, THRUST_SP_SET_ZERO, 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 328 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()

struct ThrustSetpoint guidance_v_guided_run ( bool in_flight )

Run GUIDED mode control.

Definition at line 338 of file guidance_v.c.

References 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(), ThrustSetpoint::sp, stateGetPositionNed_i(), VerticalGuidance::th_sp, th_sp_from_thrust_i(), THRUST_AXIS_Z, THRUST_SP_SET_ZERO, 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 91 of file guidance_v.c.

References BFP_OF_REAL, DefaultPeriodic, desired_zd_updated, foo, guidance_v, guidance_v_guided_mode, GUIDANCE_V_GUIDED_MODE_ZHOLD, GUIDANCE_V_MODE_KILL, GUIDANCE_V_NOMINAL_HOVER_THROTTLE, GUIDANCE_V_RC_ID, gv_adapt_init(), INT32_TRIG_FRAC, VerticalGuidance::mode, VerticalGuidance::nominal_throttle, rc_cb(), rc_ev, 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 132 of file guidance_v.c.

References foo, guidance_v, guidance_v_guided_enter(), GUIDANCE_V_MODE_CLIMB, GUIDANCE_V_MODE_GUIDED, GUIDANCE_V_MODE_HOVER, GUIDANCE_V_MODE_NAV, GUIDANCE_V_MODE_RC_CLIMB, guidance_v_run_enter(), GuidanceVSetRef, VerticalGuidance::mode, stateGetPositionNed_i(), stateGetSpeedNed_i(), and VerticalGuidance::zd_sp.

Referenced by autopilot_static_set_mode(), and guidance_module_enter().

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

References gv_adapt_init().

Here is the call graph for this function:

◆ guidance_v_run()

struct ThrustSetpoint guidance_v_run ( bool in_flight )

Guidance vertical run functions.

Returns: a thrust setpoint structure

Definition at line 221 of file guidance_v.c.

References 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_GUIDED, GUIDANCE_V_MODE_HOVER, GUIDANCE_V_MODE_NAV, GUIDANCE_V_MODE_RC_CLIMB, GUIDANCE_V_MODE_RC_DIRECT, guidance_v_run_speed(), guidance_v_thrust_adapt(), gv_update_ref_from_zd_sp(), VerticalGuidance::mode, VerticalGuidance::rc_delta_t, VerticalGuidance::rc_zd_sp, stateGetPositionNed_i(), th_sp_from_thrust_i(), VerticalGuidance::thrust, THRUST_AXIS_Z, THRUST_SP_SET_ZERO, NedCoor_i::z, VerticalGuidance::z_sp, and VerticalGuidance::zd_sp.

Referenced by autopilot_static_periodic(), and guidance_module_run().

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

References foo, 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 378 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_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 368 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 guidance_v_guided_enter(), and wedgebug_periodic().

Here is the caller graph for this function:

◆ guidance_v_thrust_adapt()

void guidance_v_thrust_adapt ( bool in_flight )

Definition at line 199 of file guidance_v.c.

References Stabilization::cmd, desired_zd_updated, foo, get_vertical_thrust_coeff(), guidance_v, gv_adapt_init(), gv_adapt_run(), INT32_TRIG_FRAC, stabilization, 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 289 of file guidance_v.c.

References desired_zd_updated, foo, 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 267 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:

◆ rc_cb()

static void rc_cb	(	uint8_t sender_id	UNUSED,
		struct RadioControl *	rc
	)

static

Definition at line 111 of file guidance_v.c.

References foo, guidance_v, GUIDANCE_V_CLIMB_RC_DEADBAND, GUIDANCE_V_MAX_RC_CLIMB_SPEED, GUIDANCE_V_MAX_RC_DESCENT_SPEED, MAX_PPRZ, RADIO_THROTTLE, VerticalGuidance::rc_delta_t, VerticalGuidance::rc_zd_sp, and SPEED_BFP_OF_REAL.

Referenced by guidance_v_init().

Here is the caller graph for this function:

◆ send_tune_vert()

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

static

Definition at line 81 of file guidance_v.c.

References dev, foo, 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 62 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 68 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().

◆ rc_ev

abi_event rc_ev

static

Definition at line 74 of file guidance_v.c.

Referenced by guidance_v_init().

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

#define	GUIDANCE_V_RC_ID ABI_BROADCAST

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

◆ GUIDANCE_V_RC_ID

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

◆ rc_cb()

◆ send_tune_vert()

Variable Documentation

◆ desired_zd_updated

◆ guidance_v

◆ guidance_v_guided_mode

◆ rc_ev