Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
autopilot.h File Reference

Autopilot modes. More...

#include "std.h"
#include "generated/airframe.h"
#include "state.h"
#include "subsystems/settings.h"
#include "pprzlink/pprzlink_transport.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_SetModeHandler (float new_autopilot_mode)
 AP mode setting handler. More...
 
void autopilot_set_motors_on (bool motors_on)
 
void autopilot_check_in_flight (bool 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)
 
void send_autopilot_version (struct transport_tx *trans, struct link_device *dev)
 
bool autopilot_guided_goto_ned (float x, float y, float z, float heading)
 Set position and heading setpoints in GUIDED mode. More...
 
bool autopilot_guided_goto_ned_relative (float dx, float dy, float dz, float dyaw)
 Set position and heading setpoints wrt. More...
 
bool autopilot_guided_goto_body_relative (float dx, float dy, float dz, float dyaw)
 Set position and heading setpoints wrt. More...
 
bool autopilot_guided_move_ned (float vx, float vy, float vz, float heading)
 Set velocity and heading setpoints in GUIDED mode. More...
 
void autopilot_guided_update (uint8_t flags, float x, float y, float z, float yaw)
 Set guided setpoints using flag mask in GUIDED mode. More...
 

Variables

uint8_t autopilot_mode
 
uint8_t autopilot_mode_auto2
 
bool autopilot_motors_on
 
bool autopilot_in_flight
 
bool kill_throttle
 
bool autopilot_rc
 
bool autopilot_power_switch
 
bool autopilot_ground_detected
 
bool 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_FLIP   18

Definition at line 54 of file autopilot.h.

Referenced by autopilot_set_mode(), autopilot_SetModeHandler(), and failsafe_check().

#define AP_MODE_FORWARD   16

Definition at line 52 of file autopilot.h.

Referenced by autopilot_set_mode().

#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_MODULE   17
#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) \
}
#define FALSE
Definition: std.h:5
void autopilot_set_motors_on(bool motors_on)
Definition: autopilot.c:704
#define TRUE
Definition: std.h:4
if(PrimarySpektrumState.SpektrumTimer)

Definition at line 92 of file autopilot.h.

#define autopilot_SetPowerSwitch (   _v)
Value:
{ \
}
bool autopilot_power_switch
Definition: autopilot.c:80

Definition at line 107 of file autopilot.h.

#define MODE_AUTO1   AP_MODE_HOVER_Z_HOLD

Definition at line 85 of file autopilot.h.

Referenced by ap_mode_of_3way_switch().

#define MODE_AUTO2   AP_MODE_NAV

Definition at line 88 of file autopilot.h.

Referenced by autopilot_init().

#define MODE_MANUAL   AP_MODE_ATTITUDE_DIRECT
#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]; \
}
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
if(PrimarySpektrumState.SpektrumTimer)

Set Rotorcraft commands.

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

Definition at line 126 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 147 of file autopilot.h.

Referenced by DetectGroundEvent().

Function Documentation

void autopilot_check_in_flight ( bool  motors_on)

Definition at line 669 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, stabilization_cmd, stateGetAccelNed_f(), and stateGetSpeedNed_f().

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 174 of file autopilot.h.

References kill_throttle, settings_clear(), and settings_clear_flag.

+ Here is the call graph for this function:

bool 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
dxrelative position (body frame, forward) in meters.
dyrelative position (body frame, right) in meters.
dzrelative position (body frame, down) in meters.
dyawOffset relative to current heading setpoint in radians.
Returns
TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 573 of file autopilot.c.

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

Referenced by mavlink_common_message_handler().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Set position and heading setpoints in GUIDED mode.

Parameters
xNorth position (local NED frame) in meters.
yEast position (local NED frame) in meters.
zDown position (local NED frame) in meters.
headingSetpoint in radians.
Returns
TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 550 of file autopilot.c.

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

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

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool 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
dxOffset relative to current north position (local NED frame) in meters.
dyOffset relative to current east position (local NED frame) in meters.
dzOffset relative to current down position (local NED frame) in meters.
dyawOffset relative to current heading setpoint in radians.
Returns
TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 561 of file autopilot.c.

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

Referenced by mavlink_common_message_handler().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool autopilot_guided_move_ned ( float  vx,
float  vy,
float  vz,
float  heading 
)

Set velocity and heading setpoints in GUIDED mode.

Parameters
vxNorth velocity (local NED frame) in meters/sec.
vyEast velocity (local NED frame) in meters/sec.
vzDown velocity (local NED frame) in meters/sec.
headingSetpoint in radians.
Returns
TRUE if setpoint was set (currently in AP_MODE_GUIDED)

Definition at line 586 of file autopilot.c.

References AP_MODE_GUIDED, autopilot_mode, guidance_h_set_guided_heading(), guidance_h_set_guided_vel(), and guidance_v_set_guided_vz().

Referenced by mavlink_common_message_handler().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void autopilot_guided_update ( uint8_t  flags,
float  x,
float  y,
float  z,
float  yaw 
)

Set guided setpoints using flag mask in GUIDED mode.

Parameters
flagsBits 0-3 are used to determine the axis system to be used. If bits 0 and 1 are clear then the coordinates are set in absolute NE coordinates. If bit 1 is set bit 0 is ignored. Bits 5-7 define whether the setpoints should be used as position or velocity. Bit flags are defined as follows: bit 0: x,y as offset coordinates bit 1: x,y in body coordinates bit 2: z as offset coordinates bit 3: yaw as offset coordinates bit 4: free bit 5: x,y as vel bit 6: z as vel bit 7: yaw as rate
xNorth position/velocity in meters or meters/sec.
yEast position/velocity in meters or meters/sec.
zDown position/velocity in meters or meters/sec.
yawHeading or heading rate setpoint in radians or radians/sec.

Definition at line 610 of file autopilot.c.

References AP_MODE_GUIDED, autopilot_mode, guidance_h_set_guided_body_vel(), guidance_h_set_guided_heading(), guidance_h_set_guided_heading_rate(), guidance_h_set_guided_pos(), guidance_h_set_guided_vel(), guidance_v_set_guided_vz(), guidance_v_set_guided_z(), FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), stateIsLocalCoordinateValid(), FloatVect2::x, NedCoor_f::x, FloatVect2::y, NedCoor_f::y, and NedCoor_f::z.

Referenced by firmware_parse_msg().

+ 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 758 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_set_mode ( uint8_t  new_autopilot_mode)

Definition at line 424 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(), autopilot_SetModeHandler(), 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  motors_on)

Definition at line 704 of file autopilot.c.

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

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:

void autopilot_SetModeHandler ( float  mode)

AP mode setting handler.

Checks RC status before calling autopilot_set_mode function

Definition at line 408 of file autopilot.c.

References AP_MODE_FAILSAFE, AP_MODE_FLIP, AP_MODE_GUIDED, AP_MODE_HOME, AP_MODE_KILL, AP_MODE_MODULE, AP_MODE_NAV, autopilot_set_mode(), radio_control, RC_OK, and RadioControl::status.

+ Here is the call graph for this function:

static void autopilot_StoreSettings ( float  store)
inlinestatic

Definition at line 166 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 151 of file autopilot.h.

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

Referenced by main_event().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void send_autopilot_version ( struct transport_tx *  trans,
struct link_device *  dev 
)

Definition at line 64 of file autopilot.c.

Referenced by autopilot_init().

+ Here is the caller graph for this function:

Variable Documentation

bool autopilot_detect_ground_once

Definition at line 83 of file autopilot.c.

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

bool autopilot_ground_detected
uint8_t autopilot_mode_auto2

Definition at line 70 of file autopilot.c.

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

bool autopilot_motors_on

Definition at line 76 of file autopilot.c.

bool autopilot_power_switch

Definition at line 80 of file autopilot.c.

Referenced by autopilot_init().

bool autopilot_rc

Definition at line 79 of file autopilot.c.

Referenced by autopilot_init(), and main_event().