Horizontal guidance for rotorcrafts. More...

#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_flip.h"
#include "firmwares/rotorcraft/guidance/guidance_indi.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/navigation.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "state.h"
#include "subsystems/datalink/telemetry.h"

Include dependency graph for guidance_h.c:

Go to the source code of this file.

Macros
#define	GUIDANCE_H_AGAIN 0

#define	GUIDANCE_H_VGAIN 0

#define	GUIDANCE_H_MAX_BANK RadOfDeg(20)

#define	GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE

#define	GUIDANCE_INDI FALSE

#define	MAX_POS_ERR POS_BFP_OF_REAL(16.)

#define	MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)

#define	GUIDANCE_H_THRUST_CMD_FILTER 10

#define	GH_GAIN_SCALE 2

Functions
static void	guidance_h_update_reference (void)

static void	guidance_h_traj_run (bool in_flight)

static void	transition_run (bool to_forward)

static void	read_rc_setpoint_speed_i (struct Int32Vect2 *speed_sp, bool in_flight)
	read speed setpoint from RC More...

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

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

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

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

void	guidance_h_init (void)

static void	reset_guidance_reference_from_current_position (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_hover_enter (void)

void	guidance_h_nav_enter (void)

void	guidance_h_from_nav (bool in_flight)
	Set horizontal guidance from NAV and run control loop. More...

void	guidance_h_set_igain (uint32_t igain)

void	guidance_h_guided_run (bool in_flight)
	Run GUIDED mode control. More...

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

Variables
struct HorizontalGuidance	guidance_h

int32_t	transition_percentage

int32_t	transition_theta_offset

struct Int32Vect2	guidance_h_pos_err

struct Int32Vect2	guidance_h_speed_err

struct Int32Vect2	guidance_h_trim_att_integrator

struct Int32Vect2	guidance_h_cmd_earth
	horizontal guidance command. More...

Detailed Description

Horizontal guidance for rotorcrafts.

Definition in file guidance_h.c.

Macro Definition Documentation

#define GH_GAIN_SCALE 2

Definition at line 451 of file guidance_h.c.

Referenced by guidance_h_traj_run().

#define GUIDANCE_H_AGAIN 0

Definition at line 49 of file guidance_h.c.

Referenced by guidance_h_init().

#define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE

Definition at line 73 of file guidance_h.c.

Referenced by guidance_h_init().

#define GUIDANCE_H_MAX_BANK RadOfDeg(20)

Definition at line 66 of file guidance_h.c.

Referenced by guidance_h_traj_run(), and guidance_indi_run().

#define GUIDANCE_H_THRUST_CMD_FILTER 10

Definition at line 446 of file guidance_h.c.

Referenced by guidance_h_traj_run().

#define GUIDANCE_H_VGAIN 0

Definition at line 53 of file guidance_h.c.

Referenced by guidance_h_init().

#define GUIDANCE_INDI FALSE

Definition at line 77 of file guidance_h.c.

#define MAX_POS_ERR POS_BFP_OF_REAL(16.)

Definition at line 442 of file guidance_h.c.

Referenced by guidance_h_traj_run().

#define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)

Definition at line 443 of file guidance_h.c.

Referenced by guidance_h_traj_run().

Function Documentation

void guidance_h_from_nav ( bool in_flight )

Set horizontal guidance from NAV and run control loop.

Definition at line 561 of file guidance_h.c.

References ANGLE_BFP_OF_REAL, ANGLE_FLOAT_OF_BFP, FLOAT_ANGLE_NORMALIZE, guidance_h, guidance_h_cmd_earth, 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_VECT2_NED_OF_ENU, 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::sp, stabilization_attitude_run(), stabilization_attitude_set_earth_cmd_i(), stabilization_attitude_set_rpy_setpoint_i(), stabilization_cmd, and Int32Eulers::theta.

Referenced by guidance_h_run().

Here is the call graph for this function:

Here is the caller graph for this function:

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 740 of file guidance_h.c.

References guidance_h_pos_err.

void guidance_h_guided_run ( bool in_flight )

Run GUIDED mode control.

Definition at line 668 of file guidance_h.c.

References ANGLE_BFP_OF_REAL, guidance_h, guidance_h_cmd_earth, guidance_h_hover_enter(), guidance_h_traj_run(), guidance_h_update_reference(), guidance_indi_run(), HorizontalGuidanceSetpoint::heading, HorizontalGuidance::sp, stabilization_attitude_run(), and stabilization_attitude_set_earth_cmd_i().

Referenced by guidance_h_run().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_h_hover_enter ( void )

Definition at line 524 of file guidance_h.c.

References guidance_h, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceSetpoint::mask, HorizontalGuidanceSetpoint::pos, FloatEulers::psi, HorizontalGuidance::rc_sp, reset_guidance_reference_from_current_position(), HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, stateGetNedToBodyEulers_f(), stateGetPositionNed_i(), VECT2_COPY, Int32Vect2::x, and Int32Vect2::y.

Referenced by guidance_h_guided_run(), and guidance_h_mode_changed().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_h_init ( void )

Definition at line 167 of file guidance_h.c.

Here is the call graph for this function:

void guidance_h_mode_changed ( uint8_t new_mode )

Definition at line 213 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_static_set_mode().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_h_nav_enter ( void )

Definition at line 547 of file guidance_h.c.

References guidance_h, HorizontalGuidanceSetpoint::heading, INT32_VECT2_NED_OF_ENU, HorizontalGuidanceSetpoint::mask, nav_heading, navigation_carrot, HorizontalGuidanceSetpoint::pos, FloatEulers::psi, Int32Eulers::psi, reset_guidance_reference_from_current_position(), HorizontalGuidance::sp, stateGetNedToBodyEulers_f(), and stateGetNedToBodyEulers_i().

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:

void guidance_h_read_rc ( bool in_flight )

Definition at line 292 of file guidance_h.c.

References FALSE, FLOAT_EULERS_ZERO, 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(), 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_f(), stabilization_none_read_rc(), stabilization_rate_read_rc(), stabilization_rate_read_rc_switched_sticks(), RadioControl::status, and TRUE.

Referenced by autopilot_static_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 351 of file guidance_h.c.

References guidance_flip_run(), guidance_h, guidance_h_from_nav(), guidance_h_guided_run(), 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(), HorizontalGuidanceSetpoint::heading, INT32_PERCENTAGE_FRAC, HorizontalGuidance::mode, FloatEulers::psi, HorizontalGuidance::rc_sp, HorizontalGuidance::sp, stabilization_attitude_run(), stabilization_none_run(), stabilization_rate_run(), and transition_run().

Referenced by autopilot_static_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 711 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 700 of file guidance_h.c.

References FLOAT_ANGLE_NORMALIZE, guidance_h, GUIDANCE_H_MODE_GUIDED, heading, HorizontalGuidanceSetpoint::heading, 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 730 of file guidance_h.c.

References guidance_h, GUIDANCE_H_MODE_GUIDED, HorizontalGuidanceSetpoint::heading_rate, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, 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 689 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 719 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 661 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_traj_run ( bool in_flight )

static

Definition at line 454 of file guidance_h.c.

References HorizontalGuidanceGains::a, HorizontalGuidanceReference::accel, ANGLE_BFP_OF_REAL, HorizontalGuidance::approx_force_by_thrust, BFP_OF_REAL, HorizontalGuidanceGains::d, HorizontalGuidance::gains, GH_GAIN_SCALE, guidance_h, guidance_h_cmd_earth, GUIDANCE_H_MAX_BANK, guidance_h_pos_err, guidance_h_speed_err, GUIDANCE_H_THRUST_CMD_FILTER, guidance_h_trim_att_integrator, guidance_v_thrust_coeff, HorizontalGuidanceGains::i, INT32_ACCEL_FRAC, INT32_ANGLE_FRAC, INT32_ANGLE_PI_2, INT32_POS_FRAC, INT32_SPEED_FRAC, INT32_TRIG_FRAC, INT_VECT2_ZERO, MAX_POS_ERR, MAX_PPRZ, MAX_SPEED_ERR, Min, HorizontalGuidanceGains::p, HorizontalGuidanceReference::pos, HorizontalGuidance::ref, HorizontalGuidanceReference::speed, stabilization_cmd, stateGetPositionNed_i(), stateGetSpeedNed_i(), HorizontalGuidanceGains::v, VECT2_DIFF, VECT2_STRIM, Int32Vect2::x, and Int32Vect2::y.

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:

static void guidance_h_update_reference ( void )

static

Definition at line 406 of file guidance_h.c.

References HorizontalGuidanceReference::accel, GuidanceHRef::accel, FLOAT_ANGLE_NORMALIZE, GH_ACCEL_REF_FRAC, GH_POS_REF_FRAC, gh_ref, GH_SPEED_REF_FRAC, gh_update_ref_from_pos_sp(), gh_update_ref_from_speed_sp(), guidance_h, GUIDANCE_H_MODE_HOVER, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceSetpoint::heading_rate, INT32_ACCEL_FRAC, INT32_POS_FRAC, INT32_SPEED_FRAC, INT32_VECT2_LSHIFT, INT32_VECT2_RSHIFT, INT_VECT2_ZERO, HorizontalGuidanceSetpoint::mask, HorizontalGuidance::mode, HorizontalGuidanceReference::pos, GuidanceHRef::pos, HorizontalGuidanceSetpoint::pos, HorizontalGuidance::ref, HorizontalGuidance::sp, HorizontalGuidanceReference::speed, GuidanceHRef::speed, HorizontalGuidanceSetpoint::speed, HorizontalGuidance::use_ref, and VECT2_COPY.

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:

static void read_rc_setpoint_speed_i	(	struct Int32Vect2 *	speed_sp,
		bool	in_flight
	)

static

read speed setpoint from RC

Todo:: calc proper scale while making sure a division by zero can't occur

Definition at line 631 of file guidance_h.c.

References GUIDANCE_H_REF_MAX_SPEED, INT32_TRIG_FRAC, MAX_PPRZ, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, radio_control, RADIO_PITCH, RADIO_ROLL, SPEED_BFP_OF_REAL, stateGetNedToBodyEulers_i(), RadioControl::values, Int32Vect2::x, and Int32Vect2::y.

Referenced by guidance_h_read_rc().

Here is the call graph for this function:

Here is the caller graph for this function:

static void reset_guidance_reference_from_current_position ( void )

inlinestatic

Definition at line 203 of file guidance_h.c.

References HorizontalGuidanceReference::accel, gh_set_ref(), guidance_h, guidance_h_trim_att_integrator, INT_VECT2_ZERO, HorizontalGuidanceReference::pos, HorizontalGuidance::ref, HorizontalGuidanceReference::speed, stateGetPositionNed_i(), stateGetSpeedNed_i(), and VECT2_COPY.

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:

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

static

Definition at line 108 of file guidance_h.c.

References guidance_h, HorizontalGuidanceReference::pos, HorizontalGuidanceSetpoint::pos, HorizontalGuidance::ref, HorizontalGuidance::sp, stateGetPositionNed_i(), NedCoor_i::x, Int32Vect2::x, NedCoor_i::y, and Int32Vect2::y.

Referenced by guidance_h_init().

Here is the call graph for this function:

Here is the caller graph for this function:

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

static

Definition at line 117 of file guidance_h.c.

References guidance_h, guidance_h_cmd_earth, guidance_h_pos_err, guidance_h_speed_err, guidance_h_trim_att_integrator, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceSetpoint::pos, HorizontalGuidance::sp, stateGetAccelNed_i(), stateGetPositionNed_i(), stateGetSpeedNed_i(), NedCoor_i::x, Int32Vect2::x, NedCoor_i::y, and Int32Vect2::y.

Referenced by guidance_h_init().

Here is the call graph for this function:

Here is the caller graph for this function:

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

static

Definition at line 139 of file guidance_h.c.

References HorizontalGuidanceReference::accel, guidance_h, HorizontalGuidanceReference::pos, HorizontalGuidanceSetpoint::pos, HorizontalGuidance::ref, HorizontalGuidance::sp, HorizontalGuidanceReference::speed, HorizontalGuidanceSetpoint::speed, Int32Vect2::x, and Int32Vect2::y.

Referenced by guidance_h_init().

Here is the caller graph for this function:

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

static

Definition at line 150 of file guidance_h.c.

References radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_YAW, stabilization_cmd, stateGetNedToBodyEulers_i(), and RadioControl::values.

Referenced by guidance_h_init().

Here is the call graph for this function:

Here is the caller graph for this function:

static void transition_run ( bool to_forward )

inlinestatic

Definition at line 613 of file guidance_h.c.

References ANGLE_BFP_OF_REAL, INT32_ANGLE_FRAC, INT32_PERCENTAGE_FRAC, and INT_MULT_RSHIFT.

Referenced by guidance_h_run().

Here is the caller graph for this function:

Variable Documentation

struct Int32Vect2 guidance_h_cmd_earth

horizontal guidance command.

In north/east with INT32_ANGLE_FRAC

Todo:: convert to real force command

Definition at line 96 of file guidance_h.c.

Referenced by guidance_h_from_nav(), guidance_h_guided_run(), guidance_h_traj_run(), and send_hover_loop().

struct Int32Vect2 guidance_h_pos_err

Definition at line 88 of file guidance_h.c.

Referenced by guidance_h_get_pos_err(), guidance_h_traj_run(), and send_hover_loop().

struct Int32Vect2 guidance_h_speed_err

Definition at line 89 of file guidance_h.c.

Referenced by guidance_h_traj_run(), and send_hover_loop().

struct Int32Vect2 guidance_h_trim_att_integrator

Definition at line 90 of file guidance_h.c.

Referenced by guidance_h_init(), guidance_h_set_igain(), guidance_h_traj_run(), reset_guidance_reference_from_current_position(), and send_hover_loop().

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

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

Function Documentation

Variable Documentation