autopilot.h File Reference

Autopilot modes. More...

#include "std.h"
#include "generated/airframe.h"
#include "state.h"
#include "subsystems/settings.h"

Include dependency graph for autopilot.h:

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

Go to the source code of this file.

Macros
#define	AP_MODE_KILL   0
#define	AP_MODE_FAILSAFE   1
#define	AP_MODE_HOME   2
#define	AP_MODE_RATE_DIRECT   3
#define	AP_MODE_ATTITUDE_DIRECT   4
#define	AP_MODE_RATE_RC_CLIMB   5
#define	AP_MODE_ATTITUDE_RC_CLIMB   6
#define	AP_MODE_ATTITUDE_CLIMB   7
#define	AP_MODE_RATE_Z_HOLD   8
#define	AP_MODE_ATTITUDE_Z_HOLD   9
#define	AP_MODE_HOVER_DIRECT   10
#define	AP_MODE_HOVER_CLIMB   11
#define	AP_MODE_HOVER_Z_HOLD   12
#define	AP_MODE_NAV   13
#define	AP_MODE_RC_DIRECT   14
#define	AP_MODE_CARE_FREE_DIRECT   15
#define	AP_MODE_FORWARD   16
#define	AP_MODE_MODULE   17
#define	AP_MODE_FLIP   18
#define	AP_MODE_GUIDED   19
#define	MODE_MANUAL   AP_MODE_ATTITUDE_DIRECT
	Default RC mode. More...
#define	MODE_AUTO1   AP_MODE_HOVER_Z_HOLD
#define	MODE_AUTO2   AP_MODE_NAV
#define	autopilot_KillThrottle(_kill)
#define	autopilot_SetPowerSwitch(_v)
#define	SetRotorcraftCommands(_cmd, _in_flight,_motor_on)
	Set Rotorcraft commands. More...
#define	THRESHOLD_GROUND_DETECT   25.0
	Z-acceleration threshold to detect ground in m/s^2. More...

Functions
void	autopilot_init (void)
	Autopilot inititalization. More...
void	autopilot_periodic (void)
void	autopilot_on_rc_frame (void)
	Get autopilot mode from two 2way switches. More...
void	autopilot_set_mode (uint8_t new_autopilot_mode)
void	autopilot_set_motors_on (bool_t motors_on)
void	autopilot_check_in_flight (bool_t motors_on)
static void	DetectGroundEvent (void)
	Ground detection based on vertical acceleration. More...
static void	autopilot_StoreSettings (float store)
static void	autopilot_ClearSettings (float clear)
bool_t	autopilot_guided_goto_ned (float x, float y, float z, float heading)
	Set position and heading setpoints in GUIDED mode. More...
bool_t	autopilot_guided_goto_ned_relative (float dx, float dy, float dz, float dyaw)
	Set position and heading setpoints wrt. More...
bool_t	autopilot_guided_goto_body_relative (float dx, float dy, float dz, float dyaw)
	Set position and heading setpoints wrt. More...

Variables
uint8_t	autopilot_mode
uint8_t	autopilot_mode_auto2
bool_t	autopilot_motors_on
bool_t	autopilot_in_flight
bool_t	kill_throttle
bool_t	autopilot_rc
bool_t	autopilot_power_switch
bool_t	autopilot_ground_detected
bool_t	autopilot_detect_ground_once
uint16_t	autopilot_flight_time
	flight time in seconds. More...

Detailed Description

Autopilot modes.

Definition in file autopilot.h.

Macro Definition Documentation

#define AP_MODE_ATTITUDE_CLIMB   7

Definition at line 43 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_ATTITUDE_DIRECT   4

Definition at line 40 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_ATTITUDE_RC_CLIMB   6

Definition at line 42 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_ATTITUDE_Z_HOLD   9

Definition at line 45 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_CARE_FREE_DIRECT   15

Definition at line 51 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_FAILSAFE   1

Definition at line 37 of file autopilot.h.

Referenced by autopilot_arming_check_motors_on(), autopilot_arming_set(), autopilot_on_rc_frame(), autopilot_periodic(), autopilot_set_mode(), DetectGroundEvent(), and failsafe_check().

#define AP_MODE_FLIP   18

Definition at line 54 of file autopilot.h.

Referenced by autopilot_set_mode().

#define AP_MODE_FORWARD   16

Definition at line 52 of file autopilot.h.

Referenced by autopilot_set_mode().

#define AP_MODE_GUIDED   19

Definition at line 55 of file autopilot.h.

Referenced by autopilot_guided_goto_body_relative(), autopilot_guided_goto_ned(), autopilot_guided_goto_ned_relative(), autopilot_set_mode(), mavlink_common_message_handler(), and mavlink_send_heartbeat().

#define AP_MODE_HOME   2

Definition at line 38 of file autopilot.h.

Referenced by autopilot_on_rc_frame(), autopilot_periodic(), autopilot_set_mode(), failsafe_check(), and mavlink_send_heartbeat().

#define AP_MODE_HOVER_CLIMB   11

Definition at line 47 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_HOVER_DIRECT   10

Definition at line 46 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_HOVER_Z_HOLD   12

Definition at line 48 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_KILL   0

Definition at line 36 of file autopilot.h.

Referenced by autopilot_arming_check_motors_on(), autopilot_arming_set(), autopilot_init(), autopilot_on_rc_frame(), autopilot_periodic(), autopilot_set_mode(), autopilot_set_motors_on(), and failsafe_check().

#define AP_MODE_MODULE   17

Definition at line 53 of file autopilot.h.

Referenced by autopilot_set_mode(), and stabilization_opticflow_vel_cb().

#define AP_MODE_NAV   13

Definition at line 49 of file autopilot.h.

Referenced by autopilot_on_rc_frame(), autopilot_periodic(), autopilot_set_mode(), failsafe_check(), mavlink_common_message_handler(), and mavlink_send_heartbeat().

#define AP_MODE_RATE_DIRECT   3

Definition at line 39 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_RATE_RC_CLIMB   5

Definition at line 41 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_RATE_Z_HOLD   8

Definition at line 44 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define AP_MODE_RC_DIRECT   14

Definition at line 50 of file autopilot.h.

Referenced by autopilot_set_mode(), and mavlink_send_heartbeat().

#define autopilot_KillThrottle

(

_kill

)

Value:

{ \
    if (_kill)                          \
      autopilot_set_motors_on(FALSE);   \
    else                                \
      autopilot_set_motors_on(TRUE);    \
  }

Definition at line 91 of file autopilot.h.

#define autopilot_SetPowerSwitch

(

_v

)

Value:

{  \
    autopilot_power_switch = _v;        \
  }

Definition at line 106 of file autopilot.h.

#define MODE_AUTO1   AP_MODE_HOVER_Z_HOLD

Definition at line 84 of file autopilot.h.

Referenced by ap_mode_of_3way_switch().

#define MODE_AUTO2   AP_MODE_NAV

Definition at line 87 of file autopilot.h.

Referenced by autopilot_init().

#define MODE_MANUAL   AP_MODE_ATTITUDE_DIRECT

Default RC mode.

Definition at line 81 of file autopilot.h.

Referenced by ap_mode_of_3way_switch(), autopilot_arming_check_motors_on(), autopilot_arming_set(), and autopilot_on_rc_frame().

#define SetRotorcraftCommands	(		_cmd,
			_in_flight,
			_motor_on
	)

Value:

{ \
    if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; }               \
    if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; }             \
    commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL];                \
    commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH];              \
    commands[COMMAND_YAW] = _cmd[COMMAND_YAW];                  \
    commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST];            \
  }

Set Rotorcraft commands.

Limit thrust and/or yaw depending of the in_flight and motors_on flag status

Definition at line 125 of file autopilot.h.

Referenced by autopilot_periodic().

#define THRESHOLD_GROUND_DETECT   25.0

Z-acceleration threshold to detect ground in m/s^2.

Definition at line 146 of file autopilot.h.

Referenced by DetectGroundEvent().

Function Documentation

void autopilot_check_in_flight

(

bool_t

motors_on

)

Definition at line 552 of file autopilot.c.

References autopilot_in_flight, autopilot_in_flight_counter, AUTOPILOT_IN_FLIGHT_MIN_ACCEL, AUTOPILOT_IN_FLIGHT_MIN_SPEED, AUTOPILOT_IN_FLIGHT_MIN_THRUST, AUTOPILOT_IN_FLIGHT_TIME, FALSE, stabilization_cmd, stateGetAccelNed_f(), stateGetSpeedNed_f(), and TRUE.

Referenced by failsafe_check().

Here is the call graph for this function:

Here is the caller graph for this function:

static void autopilot_ClearSettings ( float  clear )

inlinestatic

Definition at line 173 of file autopilot.h.

References kill_throttle, settings_clear(), and settings_clear_flag.

Here is the call graph for this function:

bool_t autopilot_guided_goto_body_relative	(	float	dx,
		float	dy,
		float	dz,
		float	dyaw
	)

Set position and heading setpoints wrt.

current position AND heading in GUIDED mode.

Parameters

dx	relative position (body frame, forward) in meters.
dy	relative position (body frame, right) in meters.
dz	relative position (body frame, down) in meters.
dyaw	Offset relative to current heading setpoint in radians.

Returns

TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 539 of file autopilot.c.

References AP_MODE_GUIDED, autopilot_guided_goto_ned(), autopilot_mode, FALSE, heading, FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), stateIsLocalCoordinateValid(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by dl_parse_msg(), and mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t autopilot_guided_goto_ned	(	float	x,
		float	y,
		float	z,
		float	heading
	)

Set position and heading setpoints in GUIDED mode.

Parameters

x	North position (local NED frame) in meters.
y	East position (local NED frame) in meters.
z	Down position (local NED frame) in meters.
heading	Setpoint in radians.

Returns

TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 516 of file autopilot.c.

References AP_MODE_GUIDED, autopilot_mode, FALSE, guidance_h_set_guided_heading(), guidance_h_set_guided_pos(), guidance_v_set_guided_z(), and TRUE.

Referenced by autopilot_guided_goto_body_relative(), autopilot_guided_goto_ned_relative(), dl_parse_msg(), and mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t autopilot_guided_goto_ned_relative	(	float	dx,
		float	dy,
		float	dz,
		float	dyaw
	)

Set position and heading setpoints wrt.

current position in GUIDED mode.

Parameters

dx	Offset relative to current north position (local NED frame) in meters.
dy	Offset relative to current east position (local NED frame) in meters.
dz	Offset relative to current down position (local NED frame) in meters.
dyaw	Offset relative to current heading setpoint in radians.

Returns

TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 527 of file autopilot.c.

References AP_MODE_GUIDED, autopilot_guided_goto_ned(), autopilot_mode, FALSE, heading, FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), stateIsLocalCoordinateValid(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by dl_parse_msg(), and mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

void autopilot_init

(

void

)

Autopilot inititalization.

Definition at line 175 of file autopilot.c.

References AP_MODE_KILL, autopilot_arming_init(), autopilot_detect_ground_once, autopilot_flight_time, autopilot_ground_detected, autopilot_in_flight, autopilot_in_flight_counter, autopilot_mode, autopilot_mode_auto2, autopilot_motors_on, autopilot_power_switch, autopilot_rc, autopilot_set_mode(), DefaultPeriodic, FALSE, gpio_clear(), gpio_setup_output(), gps_lost, guidance_h_init(), guidance_v_init(), kill_throttle, lateral_mode, LATERAL_MODE_MANUAL, launch, MODE_AUTO2, MODE_STARTUP, nav_init(), power_switch, POWER_SWITCH_GPIO, pprz_mode, PPRZ_MODE_AUTO2, register_periodic_telemetry(), send_actuators(), send_alive(), send_attitude(), send_autopilot_version(), send_dl_value(), send_energy(), send_fp(), send_rc(), send_rotorcraft_cmd(), send_rotorcraft_rc(), send_status(), stabilization_attitude_init(), stabilization_init(), stabilization_none_init(), stabilization_rate_init(), and TRUE.

Referenced by init_ap(), and main_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void autopilot_on_rc_frame

(

void

)

Get autopilot mode from two 2way switches.

RADIO_MODE switch just selectes between MANUAL and AUTO. If not MANUAL, the RADIO_AUTO_MODE switch selects between AUTO1 and AUTO2.

This is mainly a cludge for entry level radios with no three-way switch, but two available two-way switches which can be used.

Definition at line 644 of file autopilot.c.

References ahrs_is_aligned(), AP_MODE_FAILSAFE, AP_MODE_HOME, AP_MODE_KILL, AP_MODE_NAV, ap_mode_of_3way_switch(), autopilot_arming_check_motors_on(), autopilot_in_flight, autopilot_mode, autopilot_motors_on, autopilot_set_mode(), commands, GpsIsLost, guidance_h_read_rc(), guidance_v_read_rc(), kill_switch_is_on(), kill_throttle, MODE_MANUAL, radio_control, too_far_from_home, UNLOCKED_HOME_MODE, and RadioControl::values.

Referenced by main_event().

Here is the call graph for this function:

Here is the caller graph for this function:

void autopilot_periodic

(

void

)

Definition at line 334 of file autopilot.c.

References AP_MODE_FAILSAFE, AP_MODE_HOME, AP_MODE_KILL, AP_MODE_NAV, autopilot_detect_ground_once, autopilot_ground_detected, autopilot_in_flight, autopilot_mode, autopilot_motors_on, autopilot_set_mode(), commands_failsafe, compute_dist2_to_home(), dist2_to_home, failsafe_mode_dist2, FAILSAFE_MODE_TOO_FAR_FROM_HOME, FALSE, guidance_h_run(), guidance_v_run(), nav_home(), nav_periodic_task(), NAV_PRESCALER, SetCommands, SetRotorcraftCommands, stabilization_cmd, and too_far_from_home.

Referenced by main_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

void autopilot_set_mode

(

uint8_t

new_autopilot_mode

)

Definition at line 395 of file autopilot.c.

References ahrs_is_aligned(), AP_MODE_ATTITUDE_CLIMB, AP_MODE_ATTITUDE_DIRECT, AP_MODE_ATTITUDE_RC_CLIMB, AP_MODE_ATTITUDE_Z_HOLD, AP_MODE_CARE_FREE_DIRECT, AP_MODE_FAILSAFE, AP_MODE_FLIP, AP_MODE_FORWARD, AP_MODE_GUIDED, AP_MODE_HOME, AP_MODE_HOVER_CLIMB, AP_MODE_HOVER_DIRECT, AP_MODE_HOVER_Z_HOLD, AP_MODE_KILL, AP_MODE_MODULE, AP_MODE_NAV, AP_MODE_RATE_DIRECT, AP_MODE_RATE_RC_CLIMB, AP_MODE_RATE_Z_HOLD, AP_MODE_RC_DIRECT, autopilot_in_flight, autopilot_in_flight_counter, autopilot_mode, autopilot_set_motors_on(), FAILSAFE_DESCENT_SPEED, FALSE, GUIDANCE_H_MODE_ATTITUDE, GUIDANCE_H_MODE_CARE_FREE, guidance_h_mode_changed(), GUIDANCE_H_MODE_FLIP, GUIDANCE_H_MODE_FORWARD, GUIDANCE_H_MODE_GUIDED, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_KILL, GUIDANCE_H_MODE_MODULE_SETTING, GUIDANCE_H_MODE_NAV, GUIDANCE_H_MODE_RATE, GUIDANCE_H_MODE_RC_DIRECT, guidance_v_mode_changed(), GUIDANCE_V_MODE_CLIMB, GUIDANCE_V_MODE_FLIP, GUIDANCE_V_MODE_GUIDED, GUIDANCE_V_MODE_HOVER, GUIDANCE_V_MODE_KILL, GUIDANCE_V_MODE_MODULE_SETTING, GUIDANCE_V_MODE_NAV, GUIDANCE_V_MODE_RC_CLIMB, GUIDANCE_V_MODE_RC_DIRECT, guidance_v_zd_sp, MODE_STARTUP, SPEED_BFP_OF_REAL, stabilization_attitude_set_failsafe_setpoint(), and stabilization_cmd.

Referenced by autopilot_init(), autopilot_on_rc_frame(), autopilot_periodic(), failsafe_check(), guidance_flip_run(), and mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

void autopilot_set_motors_on

(

bool_t

motors_on

)

Definition at line 587 of file autopilot.c.

References ahrs_is_aligned(), AP_MODE_KILL, autopilot_arming_set(), autopilot_mode, autopilot_motors_on, FALSE, kill_throttle, and TRUE.

Referenced by actuators_ardrone_motor_status(), actuators_bebop_commit(), autopilot_set_mode(), and mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

static void autopilot_StoreSettings ( float  store )

inlinestatic

Definition at line 165 of file autopilot.h.

References kill_throttle, settings_store(), and settings_store_flag.

Here is the call graph for this function:

static void DetectGroundEvent ( void  )

inlinestatic

Ground detection based on vertical acceleration.

Definition at line 150 of file autopilot.h.

References AP_MODE_FAILSAFE, autopilot_detect_ground_once, autopilot_ground_detected, autopilot_mode, FALSE, stateGetAccelNed_f(), THRESHOLD_GROUND_DETECT, TRUE, and NedCoor_f::z.

Referenced by main_event().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

bool_t autopilot_detect_ground_once

Definition at line 79 of file autopilot.c.

Referenced by autopilot_init(), autopilot_periodic(), and DetectGroundEvent().

uint16_t autopilot_flight_time

flight time in seconds.

Definition at line 48 of file autopilot.c.

Referenced by attitude_loop(), autopilot_init(), generic_com_periodic(), gsm_send_report_continue(), main_periodic(), mf_daq_send_state(), monitor_task(), nav_check_wp_time(), parse_mf_daq_msg(), send_fp(), and telecommand_task().

bool_t autopilot_ground_detected

Definition at line 78 of file autopilot.c.

Referenced by autopilot_init(), autopilot_periodic(), DetectGroundEvent(), and nav_detect_ground().

bool_t autopilot_in_flight

Definition at line 68 of file autopilot.c.

Referenced by autopilot_check_in_flight(), autopilot_init(), autopilot_on_rc_frame(), autopilot_periodic(), autopilot_set_mode(), gps_sim_hitl_event(), guidance_flip_run(), main_periodic(), nav_is_in_flight(), and send_status().

uint8_t autopilot_mode

Definition at line 65 of file autopilot.c.

Referenced by autopilot_arming_check_motors_on(), autopilot_arming_set(), autopilot_guided_goto_body_relative(), autopilot_guided_goto_ned(), autopilot_guided_goto_ned_relative(), autopilot_init(), autopilot_on_rc_frame(), autopilot_periodic(), autopilot_set_mode(), autopilot_set_motors_on(), DetectGroundEvent(), event_task_fbw(), failsafe_check(), guidance_flip_enter(), main_periodic(), mavlink_common_message_handler(), mavlink_send_heartbeat(), send_status(), and stabilization_opticflow_vel_cb().

uint8_t autopilot_mode_auto2

Definition at line 66 of file autopilot.c.

Referenced by ap_mode_of_3way_switch(), autopilot_init(), and guidance_flip_run().

bool_t autopilot_motors_on

Definition at line 72 of file autopilot.c.

Referenced by actuators_ardrone_motor_status(), actuators_bebop_commit(), actuators_esc32_commit(), autopilot_arming_check_motors_on(), autopilot_init(), autopilot_on_rc_frame(), autopilot_periodic(), autopilot_set_motors_on(), failsafe_check(), hott_check_serial_data(), hott_update_eam_msg(), hott_update_gam_msg(), mavlink_common_message_handler(), and send_status().

bool_t autopilot_power_switch

Definition at line 76 of file autopilot.c.

Referenced by autopilot_init().

bool_t autopilot_rc

Definition at line 75 of file autopilot.c.

Referenced by autopilot_init(), and main_event().

bool_t kill_throttle

Definition at line 42 of file autopilot.c.

Referenced by attitude_loop(), autopilot_init(), autopilot_on_rc_frame(), autopilot_set_motors_on(), electrical_periodic(), geo_mag_periodic(), mavlink_send_heartbeat(), monitor_task(), nav_bungee_takeoff_run(), nav_bungee_takeoff_setup(), and v_ctl_climb_loop().