guidance_h.c File Reference

Horizontal guidance for rotorcrafts. More...

#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/navigation.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "modules/radio_control/radio_control.h"
#include "modules/core/abi.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "state.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for guidance_h.c:

Go to the source code of this file.

Macros
#define	GUIDANCE_H_RC_ID   ABI_BROADCAST
#define	GUIDANCE_H_SP_MAX_R   60.f
#define	GUIDANCE_H_DEADBAND_R   200
#define	YAW_DEADBAND_EXCEEDED(_rc)

Functions
static void	guidance_h_update_reference (void)
static void	rc_cb (uint8_t sender_id UNUSED, struct RadioControl *rc)
static void	send_gh (struct transport_tx *trans, struct link_device *dev)
static void	send_href (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)
static void	read_rc_setpoint_heading (struct HorizontalGuidanceRCInput *rc_sp, bool in_flight, struct RadioControl *rc)
static void	read_rc_setpoint_speed_i (struct Int32Vect2 *speed_sp, bool in_flight, struct RadioControl *rc)
	read speed setpoint from RC
struct StabilizationSetpoint	guidance_h_run (bool in_flight)
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_vel (float vx, float vy)
	Set horizontal velocity setpoint.
void	guidance_h_set_body_acc (float ax, float ay)
	Set body relative horizontal acceleration setpoint.
void	guidance_h_set_acc (float ax, float ay)
	Set horizontal acceleration setpoint.
void	guidance_h_set_heading_rate (float rate)
	Set heading rate setpoint.
void	guidance_h_set_all (float x, float y, float vx, float vy, float ax, float ay)
	Set position speed and acceleration (NED frame).

Variables
struct HorizontalGuidance	guidance_h
struct StabilizationSetpoint	guidance_h_cmd
	horizontal guidance command.
static abi_event	rc_ev

Detailed Description

Horizontal guidance for rotorcrafts.

Definition in file guidance_h.c.

Macro Definition Documentation

GUIDANCE_H_DEADBAND_R

#define GUIDANCE_H_DEADBAND_R   200

Definition at line 160 of file guidance_h.c.

GUIDANCE_H_RC_ID

#define GUIDANCE_H_RC_ID   ABI_BROADCAST

Definition at line 55 of file guidance_h.c.

GUIDANCE_H_SP_MAX_R

#define GUIDANCE_H_SP_MAX_R   60.f

Definition at line 155 of file guidance_h.c.

YAW_DEADBAND_EXCEEDED

#define YAW_DEADBAND_EXCEEDED

(

_rc

)

Value:

  (rc->values[RADIO_YAW] >  GUIDANCE_H_DEADBAND_R || \
   rc->values[RADIO_YAW] < -GUIDANCE_H_DEADBAND_R)

Definition at line 163 of file guidance_h.c.

Function Documentation

guidance_h_from_nav()

struct StabilizationSetpoint guidance_h_from_nav

(

bool

in_flight

)

Set horizontal guidance from NAV and run control loop.

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

)

Run GUIDED mode control.

Definition at line 457 of file guidance_h.c.

References guidance_h, guidance_h_cmd, guidance_h_hover_enter(), guidance_h_run_pos(), guidance_h_run_speed(), guidance_h_update_reference(), HorizontalGuidanceSetpoint::h_mask, and HorizontalGuidance::sp.

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

)

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

)

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

)

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(), control_mixing_heewing_attitude_direct_enter(), control_mixing_heewing_attitude_plane_enter(), control_mixing_heewing_nav_enter(), 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

)

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

)

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(), control_mixing_heewing_nav_run(), and guidance_module_run().

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
	)

Set horizontal acceleration setpoint.

Parameters

ax	North acceleration (local NED frame) in meters/sec².
ay	East acceleration (local NED frame) in meters/sec².

Definition at line 520 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_all()

void guidance_h_set_all	(	float	x,
		float	y,
		float	vx,
		float	vy,
		float	ax,
		float	ay
	)

Set position speed and acceleration (NED frame).

Parameters

x	North position (local NED frame) in meters.
y	East position (local NED frame) in meters.
vx	North velocity (local NED frame) in meters/sec.
vy	East velocity (local NED frame) in meters/sec.
ax	North acceleration (local NED frame) in meters/sec².
ay	East acceleration (local NED frame) in meters/sec².

Definition at line 536 of file guidance_h.c.

References HorizontalGuidanceSetpoint::accel, ACCEL_BFP_OF_REAL, guidance_h, HorizontalGuidanceSetpoint::h_mask, HorizontalGuidanceSetpoint::pos, POS_BFP_OF_REAL, HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, SPEED_BFP_OF_REAL, Int32Vect2::x, and Int32Vect2::y.

guidance_h_set_body_acc()

void guidance_h_set_body_acc	(	float	ax,
		float	ay
	)

Set body relative horizontal acceleration setpoint.

Parameters

ax	forward acceleration (body frame) in meters/sec².
ay	right acceleration (body frame) in meters/sec².

Definition at line 512 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
	)

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

)

Set heading setpoint.

Parameters

heading

Setpoint in radians.

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

)

Set heading rate setpoint.

Parameters

rate	Heading rate in radians.

Definition at line 530 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
	)

Set horizontal position setpoint.

Parameters

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

Definition at line 477 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
	)

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 502 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_update_reference()

static void guidance_h_update_reference ( void  )

static

Definition at line 273 of file guidance_h.c.

References HorizontalGuidanceSetpoint::accel, HorizontalGuidanceReference::accel, GuidanceHRef::accel, ACCEL_BFP_OF_REAL, ACCEL_FLOAT_OF_BFP, FLOAT_ANGLE_NORMALIZE, foo, GH_POS_REF_FRAC, gh_ref, gh_set_ref(), gh_update_ref_from_accel_sp(), gh_update_ref_from_pos_sp(), gh_update_ref_from_speed_sp(), guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, HorizontalGuidanceSetpoint::h_mask, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceSetpoint::heading_rate, INT32_POS_FRAC, INT32_VECT2_RSHIFT, INT_VECT2_ZERO, HorizontalGuidance::mode, nav, NAV_SETPOINT_MODE_ACCEL, NAV_SETPOINT_MODE_POS, NAV_SETPOINT_MODE_SPEED, HorizontalGuidanceSetpoint::pos, HorizontalGuidanceReference::pos, GuidanceHRef::pos, HorizontalGuidance::ref, RotorcraftNavigation::setpoint_mode, HorizontalGuidance::sp, sp_accel, HorizontalGuidanceSetpoint::speed, HorizontalGuidanceReference::speed, GuidanceHRef::speed, SPEED_BFP_OF_REAL, SPEED_FLOAT_OF_BFP, stateGetPositionNed_i(), stateGetSpeedNed_i(), HorizontalGuidance::use_ref, VECT2_COPY, FloatVect2::x, FloatVect3::x, Int32Vect2::x, NedCoor_i::x, FloatVect2::y, FloatVect3::y, Int32Vect2::y, NedCoor_i::y, and HorizontalGuidanceSetpoint::yaw_mask.

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:

rc_cb()

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

static

Definition at line 219 of file guidance_h.c.

References autopilot_in_flight(), foo, guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, HorizontalGuidanceSetpoint::h_mask, INT_VECT2_ZERO, HorizontalGuidance::mode, radio_control, RC_OK, HorizontalGuidance::rc_sp, read_rc_setpoint_heading(), read_rc_setpoint_speed_i(), HorizontalGuidance::sp, HorizontalGuidanceSetpoint::speed, RadioControl::status, and HorizontalGuidanceRCInput::vect.

Referenced by guidance_h_init().

Here is the call graph for this function:

Here is the caller graph for this function:

read_rc_setpoint_heading()

static void read_rc_setpoint_heading ( struct HorizontalGuidanceRCInput *  rc_sp, bool  in_flight, struct RadioControl *  rc  )

static

Definition at line 167 of file guidance_h.c.

References FLOAT_ANGLE_NORMALIZE, foo, get_sys_time_float(), GUIDANCE_H_SP_MAX_R, HorizontalGuidanceRCInput::heading, HorizontalGuidanceRCInput::last_ts, MAX_PPRZ, FloatEulers::psi, RADIO_YAW, stateGetNedToBodyEulers_f(), THROTTLE_STICK_DOWN_FROM_RC, and YAW_DEADBAND_EXCEEDED.

Referenced by rc_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

read_rc_setpoint_speed_i()

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

static

read speed setpoint from RC

Todo:

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

Definition at line 189 of file guidance_h.c.

References foo, GUIDANCE_H_REF_MAX_SPEED, INT32_TRIG_FRAC, MAX_PPRZ, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, RADIO_PITCH, RADIO_ROLL, SPEED_BFP_OF_REAL, speed_sp, stateGetNedToBodyEulers_i(), FloatVect3::x, and FloatVect3::y.

Referenced by rc_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

reset_guidance_reference_from_current_position()

static void reset_guidance_reference_from_current_position ( void  )

inlinestatic

Definition at line 114 of file guidance_h.c.

References HorizontalGuidanceReference::accel, FLOAT_VECT2_ZERO, foo, gh_set_ref(), guidance_h, INT_VECT2_ZERO, HorizontalGuidanceReference::pos, HorizontalGuidance::ref, HorizontalGuidanceReference::speed, SPEED_FLOAT_OF_BFP, stateGetPositionNed_i(), stateGetSpeedNed_i(), VECT2_COPY, FloatVect2::x, Int32Vect2::x, and Int32Vect2::y.

Referenced by guidance_h_hover_enter(), guidance_h_nav_enter(), guidance_h_set_acc(), guidance_h_set_pos(), and guidance_h_set_vel().

Here is the call graph for this function:

Here is the caller graph for this function:

send_gh()

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

static

Definition at line 64 of file guidance_h.c.

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

Referenced by guidance_h_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_href()

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

static

Definition at line 73 of file guidance_h.c.

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

Referenced by guidance_h_init().

Here is the caller graph for this function:

Variable Documentation

guidance_h

struct HorizontalGuidance guidance_h

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_all(), 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(), guidance_plane_attitude_from_nav(), 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().

guidance_h_cmd

struct StabilizationSetpoint guidance_h_cmd

horizontal guidance command.

In north/east with INT32_ANGLE_FRAC

Definition at line 50 of file guidance_h.c.

Referenced by guidance_h_from_nav(), and guidance_h_guided_run().

rc_ev

abi_event rc_ev

static

Definition at line 58 of file guidance_h.c.

Referenced by guidance_h_init().