guidance_h.h File Reference

Horizontal guidance for rotorcrafts. More...

#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
#include "firmwares/rotorcraft/stabilization.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	HorizontalGuidanceRCInput
struct	HorizontalGuidance

Macros
#define	GUIDANCE_H_USE_REF   TRUE
	Use horizontal guidance reference trajectory.
#define	GUIDANCE_H_USE_SPEED_REF   TRUE
	Use horizontal guidance speed reference.
#define	GUIDANCE_H_MODE_NONE   0
#define	GUIDANCE_H_MODE_HOVER   1
#define	GUIDANCE_H_MODE_NAV   2
#define	GUIDANCE_H_MODE_GUIDED   3
#define	GUIDANCE_H_MAX_BANK   RadOfDeg(20)
	Max bank controlled by guidance.
#define	guidance_h_SetUseRef(_val)

Functions
void	guidance_h_init (void)
void	guidance_h_mode_changed (uint8_t new_mode)
void	guidance_h_run_enter (void)
struct StabilizationSetpoint	guidance_h_run (bool in_flight)
struct StabilizationSetpoint	guidance_h_run_pos (bool in_flight, struct HorizontalGuidance *gh)
struct StabilizationSetpoint	guidance_h_run_speed (bool in_flight, struct HorizontalGuidance *gh)
struct StabilizationSetpoint	guidance_h_run_accel (bool in_flight, struct HorizontalGuidance *gh)
void	guidance_h_hover_enter (void)
void	guidance_h_nav_enter (void)
struct StabilizationSetpoint	guidance_h_from_nav (bool in_flight)
	Set horizontal guidance from NAV and run control loop.
struct StabilizationSetpoint	guidance_h_guided_run (bool in_flight)
	Run GUIDED mode control.
void	guidance_h_set_pos (float x, float y)
	Set horizontal position setpoint.
void	guidance_h_set_heading (float heading)
	Set heading setpoint.
void	guidance_h_set_body_vel (float vx, float vy)
	Set body relative horizontal velocity setpoint.
void	guidance_h_set_acc (float ax, float ay)
	Set horizontal velocity setpoint.
void	guidance_h_set_body_acc (float ax, float ay)
	Set body relative horizontal acceleration setpoint.
void	guidance_h_set_vel (float vx, float vy)
	Set horizontal acceleration setpoint.
void	guidance_h_set_heading_rate (float rate)
	Set heading rate setpoint.
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

Detailed Description

Horizontal guidance for rotorcrafts.

Definition in file guidance_h.h.

---

Data Structure Documentation

HorizontalGuidanceReference

struct HorizontalGuidanceReference

Definition at line 91 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

HorizontalGuidanceRCInput

struct HorizontalGuidanceRCInput

Definition at line 97 of file guidance_h.h.

Collaboration diagram for HorizontalGuidanceRCInput:

Data Fields
float	heading
float	last_ts
struct Int32Vect2	vect

HorizontalGuidance

struct HorizontalGuidance

Definition at line 103 of file guidance_h.h.

Collaboration diagram for HorizontalGuidance:

Data Fields
uint8_t	mode
struct HorizontalGuidanceRCInput	rc_sp	remote control setpoint
struct HorizontalGuidanceReference	ref	reference calculated from setpoints
struct HorizontalGuidanceSetpoint	sp	setpoints
bool	use_ref

Macro Definition Documentation

GUIDANCE_H_MAX_BANK

#define GUIDANCE_H_MAX_BANK   RadOfDeg(20)

Max bank controlled by guidance.

Definition at line 64 of file guidance_h.h.

GUIDANCE_H_MODE_GUIDED

#define GUIDANCE_H_MODE_GUIDED   3

Definition at line 59 of file guidance_h.h.

GUIDANCE_H_MODE_HOVER

#define GUIDANCE_H_MODE_HOVER   1

Definition at line 57 of file guidance_h.h.

GUIDANCE_H_MODE_NAV

#define GUIDANCE_H_MODE_NAV   2

Definition at line 58 of file guidance_h.h.

GUIDANCE_H_MODE_NONE

#define GUIDANCE_H_MODE_NONE   0

Definition at line 56 of file guidance_h.h.

guidance_h_SetUseRef

#define guidance_h_SetUseRef

(

_val

)

Value:

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

Definition at line 177 of file guidance_h.h.

GUIDANCE_H_USE_REF

#define GUIDANCE_H_USE_REF   TRUE

Use horizontal guidance reference trajectory.

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

Definition at line 45 of file guidance_h.h.

GUIDANCE_H_USE_SPEED_REF

#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 53 of file guidance_h.h.

Function Documentation

guidance_h_from_nav()

struct StabilizationSetpoint guidance_h_from_nav ( bool  in_flight )

extern

Set horizontal guidance from NAV and run control loop.

Definition at line 375 of file guidance_h.c.

References RotorcraftNavigation::accel, RotorcraftNavigation::carrot, foo, RotorcraftNavigation::fp_max_speed, guidance_h, guidance_h_cmd, guidance_h_guided_run(), guidance_h_nav_enter(), guidance_h_run_accel(), guidance_h_run_pos(), guidance_h_run_speed(), guidance_h_set_acc(), guidance_h_set_heading(), guidance_h_set_pos(), guidance_h_set_vel(), guidance_h_SetMaxSpeed(), guidance_h_update_reference(), RotorcraftNavigation::heading, RotorcraftNavigation::horizontal_mode, nav, NAV_HORIZONTAL_MODE_ATTITUDE, NAV_HORIZONTAL_MODE_GUIDED, NAV_HORIZONTAL_MODE_NONE, NAV_SETPOINT_MODE_ACCEL, NAV_SETPOINT_MODE_POS, NAV_SETPOINT_MODE_QUAT, NAV_SETPOINT_MODE_SPEED, FloatEulers::phi, RotorcraftNavigation::pitch, HorizontalGuidanceSetpoint::pos, POS_BFP_OF_REAL, RotorcraftNavigation::quat, RotorcraftNavigation::roll, RotorcraftNavigation::setpoint_mode, HorizontalGuidance::sp, StabilizationSetpoint::sp, RotorcraftNavigation::speed, stab_sp_from_eulers_f(), stab_sp_from_quat_f(), STAB_SP_SET_EULERS_ZERO, Int32Vect2::x, EnuCoor_f::x, Int32Vect2::y, and EnuCoor_f::y.

Referenced by guidance_h_run().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_guided_run()

struct StabilizationSetpoint guidance_h_guided_run ( bool  in_flight )

extern

Run GUIDED mode control.

Definition at line 441 of file guidance_h.c.

References guidance_h, guidance_h_cmd, guidance_h_hover_enter(), guidance_h_run_pos(), and guidance_h_update_reference().

Referenced by guidance_h_from_nav(), and guidance_h_run().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_hover_enter()

void guidance_h_hover_enter ( void  )

extern

Definition at line 343 of file guidance_h.c.

References guidance_h, guidance_h_run_enter(), guidance_h_set_heading(), guidance_h_set_pos(), HorizontalGuidanceRCInput::heading, FloatEulers::psi, HorizontalGuidance::rc_sp, reset_guidance_reference_from_current_position(), HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), FloatVect2::x, Int32Vect2::x, FloatVect2::y, and Int32Vect2::y.

Referenced by guidance_h_guided_run(), guidance_h_mode_changed(), and wedgebug_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_init()

void guidance_h_init ( void  )

extern

Definition at line 86 of file guidance_h.c.

References DefaultPeriodic, foo, gh_ref_init(), guidance_h, GUIDANCE_H_MODE_NONE, GUIDANCE_H_RC_ID, GUIDANCE_H_USE_REF, HorizontalGuidanceSetpoint::h_mask, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceRCInput::heading, HorizontalGuidanceSetpoint::heading_rate, INT_VECT2_ZERO, HorizontalGuidanceRCInput::last_ts, HorizontalGuidance::mode, HorizontalGuidanceSetpoint::pos, rc_cb(), rc_ev, HorizontalGuidance::rc_sp, register_periodic_telemetry(), send_gh(), send_href(), HorizontalGuidance::sp, HorizontalGuidance::use_ref, HorizontalGuidanceRCInput::vect, and HorizontalGuidanceSetpoint::yaw_mask.

Here is the call graph for this function:

guidance_h_mode_changed()

void guidance_h_mode_changed ( uint8_t  new_mode )

extern

Definition at line 128 of file guidance_h.c.

References foo, guidance_h, guidance_h_hover_enter(), GUIDANCE_H_MODE_GUIDED, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, guidance_h_nav_enter(), and HorizontalGuidance::mode.

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

void guidance_h_nav_enter ( void  )

extern

Definition at line 364 of file guidance_h.c.

References RotorcraftNavigation::carrot, guidance_h_run_enter(), guidance_h_set_heading(), guidance_h_set_pos(), RotorcraftNavigation::heading, nav, reset_guidance_reference_from_current_position(), EnuCoor_f::x, and EnuCoor_f::y.

Referenced by guidance_h_from_nav(), and guidance_h_mode_changed().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_run()

struct StabilizationSetpoint guidance_h_run ( bool  in_flight )

extern

Definition at line 244 of file guidance_h.c.

References guidance_h, guidance_h_from_nav(), guidance_h_guided_run(), GUIDANCE_H_MODE_GUIDED, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceRCInput::heading, HorizontalGuidance::mode, HorizontalGuidance::rc_sp, HorizontalGuidance::sp, StabilizationSetpoint::sp, and STAB_SP_SET_EULERS_ZERO.

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

struct StabilizationSetpoint guidance_h_run_accel ( bool  in_flight, struct HorizontalGuidance *  gh  )

extern

Definition at line 58 of file guidance_oneloop.c.

References _gv, _v_mode, foo, GUIDANCE_ONELOOP_H_ACCEL, and guidance_oneloop_run_mode().

Referenced by guidance_h_from_nav().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_run_enter()

void guidance_h_run_enter ( void  )

extern

Definition at line 35 of file guidance_oneloop.c.

References CTRL_ANDI, and oneloop_andi_enter().

Referenced by guidance_h_hover_enter(), and guidance_h_nav_enter().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_run_pos()

struct StabilizationSetpoint guidance_h_run_pos ( bool  in_flight, struct HorizontalGuidance *  gh  )

extern

Definition at line 48 of file guidance_oneloop.c.

References _gv, _v_mode, foo, GUIDANCE_ONELOOP_H_POS, and guidance_oneloop_run_mode().

Referenced by guidance_h_from_nav(), and guidance_h_guided_run().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_run_speed()

struct StabilizationSetpoint guidance_h_run_speed ( bool  in_flight, struct HorizontalGuidance *  gh  )

extern

Definition at line 53 of file guidance_oneloop.c.

References _gv, _v_mode, foo, GUIDANCE_ONELOOP_H_SPEED, and guidance_oneloop_run_mode().

Referenced by guidance_h_from_nav().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_set_acc()

void guidance_h_set_acc ( float  ax, float  ay  )

extern

Set horizontal velocity setpoint.

Parameters

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

Definition at line 498 of file guidance_h.c.

References HorizontalGuidanceSetpoint::accel, ACCEL_BFP_OF_REAL, guidance_h, HorizontalGuidanceSetpoint::h_mask, reset_guidance_reference_from_current_position(), HorizontalGuidance::sp, Int32Vect2::x, and Int32Vect2::y.

Referenced by guidance_h_from_nav(), and guidance_h_set_body_acc().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_set_body_acc()

void guidance_h_set_body_acc ( float  ax, float  ay  )

extern

Set body relative horizontal acceleration setpoint.

Parameters

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

Definition at line 490 of file guidance_h.c.

References foo, guidance_h_set_acc(), FloatEulers::psi, and stateGetNedToBodyEulers_f().

Here is the call graph for this function:

guidance_h_set_body_vel()

void guidance_h_set_body_vel ( float  vx, float  vy  )

extern

Set body relative horizontal velocity setpoint.

Parameters

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

Definition at line 472 of file guidance_h.c.

References foo, guidance_h_set_vel(), FloatEulers::psi, and stateGetNedToBodyEulers_f().

Referenced by orange_avoider_guided_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_set_heading()

void guidance_h_set_heading ( float  heading )

extern

Set heading setpoint.

Parameters

heading

Setpoint in radians.

Definition at line 465 of file guidance_h.c.

References FLOAT_ANGLE_NORMALIZE, guidance_h, HorizontalGuidanceSetpoint::heading, heading, HorizontalGuidance::sp, and HorizontalGuidanceSetpoint::yaw_mask.

Referenced by guidance_h_from_nav(), guidance_h_hover_enter(), guidance_h_nav_enter(), orange_avoider_guided_periodic(), and wedgebug_periodic().

Here is the caller graph for this function:

guidance_h_set_heading_rate()

void guidance_h_set_heading_rate ( float  rate )

extern

Set heading rate setpoint.

Parameters

rate	Heading rate in radians.

Definition at line 508 of file guidance_h.c.

References guidance_h, HorizontalGuidanceSetpoint::heading_rate, HorizontalGuidance::sp, and HorizontalGuidanceSetpoint::yaw_mask.

Referenced by orange_avoider_guided_periodic().

Here is the caller graph for this function:

guidance_h_set_pos()

void guidance_h_set_pos ( float  x, float  y  )

extern

Set horizontal position setpoint.

Parameters

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

Definition at line 455 of file guidance_h.c.

References guidance_h, HorizontalGuidanceSetpoint::h_mask, HorizontalGuidanceSetpoint::pos, POS_BFP_OF_REAL, reset_guidance_reference_from_current_position(), HorizontalGuidance::sp, Int32Vect2::x, and Int32Vect2::y.

Referenced by guidance_h_from_nav(), guidance_h_hover_enter(), guidance_h_nav_enter(), and wedgebug_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_set_vel()

void guidance_h_set_vel ( float  vx, float  vy  )

extern

Set horizontal acceleration setpoint.

Parameters

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

Definition at line 480 of file guidance_h.c.

References guidance_h, HorizontalGuidanceSetpoint::h_mask, reset_guidance_reference_from_current_position(), HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, SPEED_BFP_OF_REAL, Int32Vect2::x, and Int32Vect2::y.

Referenced by guidance_h_from_nav(), and guidance_h_set_body_vel().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_SetMaxSpeed()

static void guidance_h_SetMaxSpeed ( float  speed )

inlinestatic

Definition at line 181 of file guidance_h.h.

References gh_set_max_speed().

Referenced by guidance_h_from_nav().

Here is the call graph for this function:

Here is the caller graph for this function:

guidance_h_SetOmega()

static void guidance_h_SetOmega ( float  omega )

inlinestatic

Definition at line 186 of file guidance_h.h.

References gh_set_omega().

Here is the call graph for this function:

guidance_h_SetTau()

static void guidance_h_SetTau ( float  tau )

inlinestatic

Definition at line 196 of file guidance_h.h.

References gh_set_tau().

Here is the call graph for this function:

guidance_h_SetZeta()

static void guidance_h_SetZeta ( float  zeta )

inlinestatic

Definition at line 191 of file guidance_h.h.

References gh_set_zeta().

Here is the call graph for this function:

Variable Documentation

guidance_h

struct HorizontalGuidance guidance_h

extern

Definition at line 45 of file guidance_h.c.

Referenced by autopilot_static_periodic(), guidance_h_from_nav(), guidance_h_guided_run(), guidance_h_hover_enter(), guidance_h_init(), guidance_h_mode_changed(), guidance_h_run(), guidance_h_set_acc(), guidance_h_set_heading(), guidance_h_set_heading_rate(), guidance_h_set_pos(), guidance_h_set_vel(), guidance_h_update_reference(), guidance_pid_h_run(), ins_ekf2_publish_attitude(), orange_avoider_guided_periodic(), rc_cb(), reset_guidance_reference_from_current_position(), rotwing_state_periodic(), send_fp(), send_gh(), send_hover_loop(), send_href(), send_status(), and stabilization_indi_set_wls_settings().