guidance_v_ref.c File Reference

Reference generation for vertical guidance. More...

#include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
#include "generated/airframe.h"

Include dependency graph for guidance_v_ref.c:

Go to the source code of this file.

Macros
#define	GUIDANCE_V_REF_MIN_ZDD   (-2.0*9.81)
#define	GV_MIN_ZDD   BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZDD, GV_ZDD_REF_FRAC)
#define	GUIDANCE_V_REF_MAX_ZDD   ( 0.8*9.81)
#define	GV_MAX_ZDD   BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC)
#define	GUIDANCE_V_REF_MAX_Z_DIFF   10.0
	maximum distance altitude setpoint is advanced in climb mode More...
#define	GV_MAX_Z_DIFF   BFP_OF_REAL(GUIDANCE_V_REF_MAX_Z_DIFF, GV_Z_REF_FRAC)
#define	GV_MIN_ZD   BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC)
#define	GV_MAX_ZD   BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC)
#define	GUIDANCE_V_REF_OMEGA   RadOfDeg(100.)
#define	GUIDANCE_V_REF_ZETA   0.85
#define	GV_ZETA_OMEGA_FRAC   10
#define	GV_ZETA_OMEGA   BFP_OF_REAL((GUIDANCE_V_REF_ZETA*GUIDANCE_V_REF_OMEGA), GV_ZETA_OMEGA_FRAC)
#define	GV_OMEGA_2_FRAC   7
#define	GV_OMEGA_2   BFP_OF_REAL((GUIDANCE_V_REF_OMEGA*GUIDANCE_V_REF_OMEGA), GV_OMEGA_2_FRAC)
#define	GV_REF_THAU_F   0.25
#define	GV_REF_INV_THAU_FRAC   16
#define	GV_REF_INV_THAU   BFP_OF_REAL((1./0.25), GV_REF_INV_THAU_FRAC)

Functions
void	gv_set_ref (int32_t alt, int32_t speed, int32_t accel)
void	gv_update_ref_from_z_sp (int32_t z_sp)
void	gv_update_ref_from_zd_sp (int32_t zd_sp, int32_t z_pos)
	update vertical reference from speed setpoint. More...

Variables
int32_t	gv_zdd_ref
	reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC Q23.8 : accuracy 0.0039 , range 8388km/s^2 More...
int32_t	gv_zd_ref
	reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC Q14.17 : accuracy 0.0000076 , range 16384m/s2 More...
int64_t	gv_z_ref
	reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37.26 : More...

Detailed Description

Reference generation for vertical guidance.

Definition in file guidance_v_ref.c.

Macro Definition Documentation

#define GUIDANCE_V_REF_MAX_Z_DIFF   10.0

maximum distance altitude setpoint is advanced in climb mode

Definition at line 63 of file guidance_v_ref.c.

#define GUIDANCE_V_REF_MAX_ZDD   ( 0.8*9.81)

Definition at line 57 of file guidance_v_ref.c.

#define GUIDANCE_V_REF_MIN_ZDD   (-2.0*9.81)

Definition at line 52 of file guidance_v_ref.c.

#define GUIDANCE_V_REF_OMEGA   RadOfDeg(100.)

Definition at line 72 of file guidance_v_ref.c.

#define GUIDANCE_V_REF_ZETA   0.85

Definition at line 75 of file guidance_v_ref.c.

#define GV_MAX_Z_DIFF   BFP_OF_REAL(GUIDANCE_V_REF_MAX_Z_DIFF, GV_Z_REF_FRAC)

Definition at line 65 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_zd_sp().

#define GV_MAX_ZD   BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC)

Definition at line 68 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_z_sp(), and gv_update_ref_from_zd_sp().

#define GV_MAX_ZDD   BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC)

Definition at line 59 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_z_sp(), and gv_update_ref_from_zd_sp().

#define GV_MIN_ZD   BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC)

Definition at line 67 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_z_sp(), and gv_update_ref_from_zd_sp().

#define GV_MIN_ZDD   BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZDD, GV_ZDD_REF_FRAC)

Definition at line 54 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_z_sp(), and gv_update_ref_from_zd_sp().

#define GV_OMEGA_2   BFP_OF_REAL((GUIDANCE_V_REF_OMEGA*GUIDANCE_V_REF_OMEGA), GV_OMEGA_2_FRAC)

Definition at line 80 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_z_sp().

#define GV_OMEGA_2_FRAC   7

Definition at line 79 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_z_sp().

#define GV_REF_INV_THAU   BFP_OF_REAL((1./0.25), GV_REF_INV_THAU_FRAC)

Definition at line 85 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_zd_sp().

#define GV_REF_INV_THAU_FRAC   16

Definition at line 84 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_zd_sp().

#define GV_REF_THAU_F   0.25

Definition at line 83 of file guidance_v_ref.c.

#define GV_ZETA_OMEGA   BFP_OF_REAL((GUIDANCE_V_REF_ZETA*GUIDANCE_V_REF_OMEGA), GV_ZETA_OMEGA_FRAC)

Definition at line 78 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_z_sp().

#define GV_ZETA_OMEGA_FRAC   10

Definition at line 77 of file guidance_v_ref.c.

Referenced by gv_update_ref_from_z_sp().

Function Documentation

void gv_set_ref	(	int32_t	alt,
		int32_t	speed,
		int32_t	accel
	)

Definition at line 87 of file guidance_v_ref.c.

References 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, and INT32_SPEED_FRAC.

Referenced by gps_sim_hitl_event(), and guidance_v_set_ref().

Here is the caller graph for this function:

void gv_update_ref_from_z_sp

(

int32_t

z_sp

)

Definition at line 95 of file guidance_v_ref.c.

References GV_MAX_ZD, GV_MAX_ZDD, GV_MIN_ZD, GV_MIN_ZDD, GV_OMEGA_2, GV_OMEGA_2_FRAC, gv_z_ref, GV_Z_REF_FRAC, gv_zd_ref, GV_ZD_REF_FRAC, gv_zdd_ref, GV_ZDD_REF_FRAC, GV_ZETA_OMEGA, GV_ZETA_OMEGA_FRAC, and INT32_POS_FRAC.

Referenced by guidance_v_from_nav(), and guidance_v_guided_run().

Here is the caller graph for this function:

void gv_update_ref_from_zd_sp	(	int32_t	zd_sp,
		int32_t	z_pos
	)

update vertical reference from speed setpoint.

Parameters

zd_sp	vertical speed setpoint with INT32_SPEED_FRAC
z_pos	current vertical position (z-down) with INT32_POS_FRAC

Definition at line 129 of file guidance_v_ref.c.

References GV_MAX_Z_DIFF, GV_MAX_ZD, GV_MAX_ZDD, GV_MIN_ZD, GV_MIN_ZDD, GV_REF_INV_THAU, GV_REF_INV_THAU_FRAC, gv_z_ref, GV_Z_REF_FRAC, gv_zd_ref, GV_ZD_REF_FRAC, gv_zdd_ref, GV_ZDD_REF_FRAC, INT32_POS_FRAC, and INT32_SPEED_FRAC.

Referenced by guidance_v_from_nav(), guidance_v_guided_run(), and guidance_v_run().

Here is the caller graph for this function:

Variable Documentation

int64_t gv_z_ref

reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37.26 :

Definition at line 47 of file guidance_v_ref.c.

Referenced by gv_set_ref(), gv_update_ref_from_z_sp(), gv_update_ref_from_zd_sp(), and run_hover_loop().

int32_t gv_zd_ref

reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC Q14.17 : accuracy 0.0000076 , range 16384m/s2

Definition at line 41 of file guidance_v_ref.c.

Referenced by gv_set_ref(), gv_update_ref_from_z_sp(), gv_update_ref_from_zd_sp(), and run_hover_loop().

int32_t gv_zdd_ref

reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC Q23.8 : accuracy 0.0039 , range 8388km/s^2

Definition at line 35 of file guidance_v_ref.c.

Referenced by gv_set_ref(), gv_update_ref_from_z_sp(), gv_update_ref_from_zd_sp(), and run_hover_loop().