Horizontal guidance for rotorcrafts. More...

#include "math/pprz_algebra_int.h"
#include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
#include "generated/airframe.h"
#include "std.h"

Include dependency graph for guidance_h.h:

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

Go to the source code of this file.

Data Structures
struct	HorizontalGuidanceSetpoint

struct	HorizontalGuidanceReference

struct	HorizontalGuidanceGains

struct	HorizontalGuidance

Macros
#define	GUIDANCE_H_USE_REF TRUE
	Use horizontal guidance reference trajectory. More...

#define	GUIDANCE_H_USE_SPEED_REF TRUE
	Use horizontal guidance speed reference. More...

#define	GUIDANCE_H_MODE_KILL 0

#define	GUIDANCE_H_MODE_RATE 1

#define	GUIDANCE_H_MODE_ATTITUDE 2

#define	GUIDANCE_H_MODE_HOVER 3

#define	GUIDANCE_H_MODE_NAV 4

#define	GUIDANCE_H_MODE_RC_DIRECT 5

#define	GUIDANCE_H_MODE_CARE_FREE 6

#define	GUIDANCE_H_MODE_FORWARD 7

#define	GUIDANCE_H_MODE_MODULE 8

#define	GUIDANCE_H_MODE_FLIP 9

#define	GUIDANCE_H_MODE_GUIDED 10

#define	guidance_h_SetUseRef(_val)

Functions
void	guidance_h_init (void)

void	guidance_h_mode_changed (uint8_t new_mode)

void	guidance_h_read_rc (bool in_flight)

void	guidance_h_run (bool in_flight)

void	guidance_h_set_igain (uint32_t igain)

bool	guidance_h_set_guided_pos (float x, float y)
	Set horizontal position setpoint in GUIDED mode. More...

bool	guidance_h_set_guided_heading (float heading)
	Set heading setpoint in GUIDED mode. More...

bool	guidance_h_set_guided_body_vel (float vx, float vy)
	Set body relative horizontal velocity setpoint in GUIDED mode. More...

bool	guidance_h_set_guided_vel (float vx, float vy)
	Set horizontal velocity setpoint in GUIDED mode. More...

bool	guidance_h_set_guided_heading_rate (float rate)
	Set heading rate setpoint in GUIDED mode. More...

const struct Int32Vect2 *	guidance_h_get_pos_err (void)
	Gets the position error. More...

static void	guidance_h_SetMaxSpeed (float speed)

static void	guidance_h_SetOmega (float omega)

static void	guidance_h_SetZeta (float zeta)

static void	guidance_h_SetTau (float tau)

Variables
struct HorizontalGuidance	guidance_h

int32_t	transition_percentage

int32_t	transition_theta_offset

Detailed Description

Horizontal guidance for rotorcrafts.

Definition in file guidance_h.h.

Data Structure Documentation

struct HorizontalGuidanceSetpoint

Definition at line 65 of file guidance_h.h.

Collaboration diagram for HorizontalGuidanceSetpoint:

Data Fields
int32_t	heading	with INT32_ANGLE_FRAC
int32_t	heading_rate	with INT32_RATE_FRAC
uint8_t	mask	bit 5: vx & vy, bit 6: vz, bit 7: vyaw
struct Int32Vect2	pos	horizontal position setpoint in NED. fixed point representation: Q23.8 accuracy 0.0039, range 8388km
struct Int32Vect2	speed	only used in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode

struct HorizontalGuidanceReference

Definition at line 77 of file guidance_h.h.

Collaboration diagram for HorizontalGuidanceReference:

Data Fields
struct Int32Vect2	accel	with INT32_ACCEL_FRAC
struct Int32Vect2	pos	with INT32_POS_FRAC
struct Int32Vect2	speed	with INT32_SPEED_FRAC

struct HorizontalGuidanceGains

Definition at line 83 of file guidance_h.h.

Data Fields
int32_t	a
int32_t	d
int32_t	i
int32_t	p
int32_t	v

struct HorizontalGuidance

Definition at line 91 of file guidance_h.h.

Collaboration diagram for HorizontalGuidance:

Data Fields
bool	approx_force_by_thrust
struct HorizontalGuidanceGains	gains
uint8_t	mode
struct Int32Eulers	rc_sp	with INT32_ANGLE_FRAC
struct HorizontalGuidanceReference	ref	reference calculated from setpoints
struct HorizontalGuidanceSetpoint	sp	setpoints
bool	use_ref

Macro Definition Documentation

#define GUIDANCE_H_MODE_ATTITUDE 2

Definition at line 54 of file guidance_h.h.

Referenced by autopilot_set_mode(), booz_fms_impl_init(), guidance_h_mode_changed(), guidance_h_read_rc(), guidance_h_run(), vi_init(), and vi_periodic().

#define GUIDANCE_H_MODE_CARE_FREE 6

Definition at line 58 of file guidance_h.h.

Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().

#define GUIDANCE_H_MODE_FLIP 9

Definition at line 61 of file guidance_h.h.

Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().

#define GUIDANCE_H_MODE_FORWARD 7

Definition at line 59 of file guidance_h.h.

Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().

#define GUIDANCE_H_MODE_GUIDED 10

Definition at line 62 of file guidance_h.h.

Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_run(), guidance_h_set_guided_heading(), guidance_h_set_guided_heading_rate(), guidance_h_set_guided_pos(), and guidance_h_set_guided_vel().

#define GUIDANCE_H_MODE_HOVER 3

Definition at line 55 of file guidance_h.h.

Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), guidance_h_run(), and guidance_h_update_reference().

#define GUIDANCE_H_MODE_KILL 0

Definition at line 52 of file guidance_h.h.

Referenced by autopilot_set_mode(), guidance_h_init(), and guidance_h_mode_changed().

#define GUIDANCE_H_MODE_MODULE 8

Definition at line 60 of file guidance_h.h.

Referenced by guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().

#define GUIDANCE_H_MODE_NAV 4

Definition at line 56 of file guidance_h.h.

Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().

#define GUIDANCE_H_MODE_RATE 1

Definition at line 53 of file guidance_h.h.

Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().

#define GUIDANCE_H_MODE_RC_DIRECT 5

Definition at line 57 of file guidance_h.h.

Referenced by autopilot_set_mode(), guidance_h_mode_changed(), guidance_h_read_rc(), and guidance_h_run().

#define guidance_h_SetUseRef ( _val )

Value:

{                    \
    guidance_h.use_ref = _val && GUIDANCE_H_USE_REF;    \
  }

Definition at line 159 of file guidance_h.h.

#define GUIDANCE_H_USE_REF TRUE

Use horizontal guidance reference trajectory.

Default is TRUE, define to FALSE to always disable it.

Definition at line 41 of file guidance_h.h.

Referenced by guidance_h_init().

#define GUIDANCE_H_USE_SPEED_REF TRUE

Use horizontal guidance speed reference.

This also allows to give velocity commands via RC in GUIDANCE_H_MODE_HOVER. Default is TRUE, define to FALSE to always disable it.

Definition at line 49 of file guidance_h.h.

Function Documentation

const struct Int32Vect2* guidance_h_get_pos_err ( void )

Gets the position error.

Parameters

none.

Returns: Pointer to a structure containing x and y position errors

Definition at line 732 of file guidance_h.c.

References guidance_h_pos_err.

void guidance_h_init ( void )

Definition at line 169 of file guidance_h.c.

Referenced by autopilot_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_h_mode_changed ( uint8_t new_mode )

Definition at line 222 of file guidance_h.c.

References guidance_flip_enter(), guidance_h, guidance_h_hover_enter(), GUIDANCE_H_MODE_ATTITUDE, GUIDANCE_H_MODE_CARE_FREE, GUIDANCE_H_MODE_FLIP, GUIDANCE_H_MODE_FORWARD, GUIDANCE_H_MODE_GUIDED, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_KILL, GUIDANCE_H_MODE_MODULE, GUIDANCE_H_MODE_NAV, GUIDANCE_H_MODE_RATE, GUIDANCE_H_MODE_RC_DIRECT, guidance_h_module_enter(), guidance_h_nav_enter(), guidance_hybrid_norm_ref_airspeed, guidance_indi_enter(), HorizontalGuidance::mode, stabilization_attitude_enter(), stabilization_attitude_reset_care_free_heading(), stabilization_none_enter(), and stabilization_rate_enter().

Referenced by autopilot_set_mode().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_h_read_rc ( bool in_flight )

Definition at line 306 of file guidance_h.c.

References FALSE, guidance_h, GUIDANCE_H_MODE_ATTITUDE, GUIDANCE_H_MODE_CARE_FREE, GUIDANCE_H_MODE_FLIP, GUIDANCE_H_MODE_FORWARD, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_MODULE, GUIDANCE_H_MODE_NAV, GUIDANCE_H_MODE_RATE, GUIDANCE_H_MODE_RC_DIRECT, guidance_h_module_read_rc(), INT_EULERS_ZERO, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, radio_control, RC_OK, HorizontalGuidance::rc_sp, read_rc_setpoint_speed_i(), HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, stabilization_attitude_read_rc(), stabilization_attitude_read_rc_setpoint_eulers(), stabilization_none_read_rc(), stabilization_rate_read_rc(), stabilization_rate_read_rc_switched_sticks(), RadioControl::status, and TRUE.

Referenced by autopilot_on_rc_frame().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_h_run ( bool in_flight )

Definition at line 365 of file guidance_h.c.

References guidance_flip_run(), guidance_h, guidance_h_cmd_earth, guidance_h_hover_enter(), GUIDANCE_H_MODE_ATTITUDE, GUIDANCE_H_MODE_CARE_FREE, GUIDANCE_H_MODE_FLIP, GUIDANCE_H_MODE_FORWARD, GUIDANCE_H_MODE_GUIDED, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_MODULE, GUIDANCE_H_MODE_NAV, GUIDANCE_H_MODE_RATE, GUIDANCE_H_MODE_RC_DIRECT, guidance_h_module_run(), guidance_h_nav_enter(), guidance_h_traj_run(), guidance_h_update_reference(), guidance_hybrid_reset_heading(), guidance_hybrid_run(), guidance_indi_run(), HorizontalGuidanceSetpoint::heading, horizontal_mode, HORIZONTAL_MODE_ATTITUDE, HORIZONTAL_MODE_MANUAL, INT32_ANGLE_NORMALIZE, INT32_PERCENTAGE_FRAC, INT32_VECT2_NED_OF_ENU, HorizontalGuidance::mode, nav_cmd_pitch, nav_cmd_roll, nav_cmd_yaw, nav_heading, nav_pitch, nav_roll, navigation_carrot, navigation_target, Int32Eulers::phi, HorizontalGuidanceSetpoint::pos, Int32Eulers::psi, HorizontalGuidance::rc_sp, HorizontalGuidance::sp, stabilization_attitude_run(), stabilization_attitude_set_earth_cmd_i(), stabilization_attitude_set_rpy_setpoint_i(), stabilization_cmd, stabilization_none_run(), stabilization_rate_run(), Int32Eulers::theta, and transition_run().

Referenced by autopilot_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

bool guidance_h_set_guided_body_vel	(	float	vx,
		float	vy
	)

Set body relative horizontal velocity setpoint in GUIDED mode.

Parameters

vx	forward velocity (body frame) in meters/sec.
vy	right velocity (body frame) in meters/sec.

Returns: TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)

Definition at line 703 of file guidance_h.c.

References guidance_h_set_guided_vel(), FloatEulers::psi, Int32Eulers::psi, and stateGetNedToBodyEulers_f().

Referenced by autopilot_guided_update().

Here is the call graph for this function:

Here is the caller graph for this function:

bool guidance_h_set_guided_heading ( float heading )

Set heading setpoint in GUIDED mode.

Parameters

heading Setpoint in radians.

Returns: TRUE if setpoint was set (currently in GUIDANCE_H_MODE_GUIDED)

Definition at line 692 of file guidance_h.c.

References ANGLE_BFP_OF_REAL, guidance_h, GUIDANCE_H_MODE_GUIDED, HorizontalGuidanceSetpoint::heading, INT32_ANGLE_NORMALIZE, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, and HorizontalGuidance::sp.

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

Here is the caller graph for this function:

bool guidance_h_set_guided_heading_rate ( float rate )

Set heading rate setpoint in GUIDED mode.

Parameters

rate	Heading rate in radians.

Returns: TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)

Definition at line 722 of file guidance_h.c.

References guidance_h, GUIDANCE_H_MODE_GUIDED, HorizontalGuidanceSetpoint::heading_rate, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, RATE_BFP_OF_REAL, and HorizontalGuidance::sp.

Referenced by autopilot_guided_update().

Here is the caller graph for this function:

bool guidance_h_set_guided_pos	(	float	x,
		float	y
	)

Set horizontal position setpoint in GUIDED mode.

Parameters

x	North position (local NED frame) in meters.
y	East position (local NED frame) in meters.

Returns: TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)

Definition at line 681 of file guidance_h.c.

References guidance_h, GUIDANCE_H_MODE_GUIDED, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, HorizontalGuidanceSetpoint::pos, POS_BFP_OF_REAL, HorizontalGuidance::sp, Int32Vect2::x, and Int32Vect2::y.

Referenced by autopilot_guided_goto_ned(), and autopilot_guided_update().

Here is the caller graph for this function:

bool guidance_h_set_guided_vel	(	float	vx,
		float	vy
	)

Set horizontal velocity setpoint in GUIDED mode.

Parameters

vx	North velocity (local NED frame) in meters/sec.
vy	East velocity (local NED frame) in meters/sec.

Returns: TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)

Definition at line 711 of file guidance_h.c.

References guidance_h, GUIDANCE_H_MODE_GUIDED, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, SPEED_BFP_OF_REAL, Int32Vect2::x, and Int32Vect2::y.

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

Here is the caller graph for this function:

void guidance_h_set_igain ( uint32_t igain )

Definition at line 675 of file guidance_h.c.

References HorizontalGuidance::gains, guidance_h, guidance_h_trim_att_integrator, HorizontalGuidanceGains::i, and INT_VECT2_ZERO.

static void guidance_h_SetMaxSpeed ( float speed )

inlinestatic

Definition at line 163 of file guidance_h.h.

References gh_set_max_speed().

Here is the call graph for this function:

static void guidance_h_SetOmega ( float omega )

inlinestatic

Definition at line 168 of file guidance_h.h.

References gh_set_omega().

Here is the call graph for this function:

static void guidance_h_SetTau ( float tau )

inlinestatic

Definition at line 178 of file guidance_h.h.

References gh_set_tau().

Here is the call graph for this function:

static void guidance_h_SetZeta ( float zeta )

inlinestatic

Definition at line 173 of file guidance_h.h.

References gh_set_zeta().

Here is the call graph for this function:

Variable Documentation

int32_t transition_percentage

Definition at line 82 of file guidance_h.c.

int32_t transition_theta_offset

Definition at line 83 of file guidance_h.c.

Referenced by stabilization_attitude_read_rc_roll_pitch_earth_quat_f().

Data Structures

Macros

Functions

Variables

Detailed Description

Data Structure Documentation

Macro Definition Documentation

Function Documentation

Variable Documentation