Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
autopilot.c File Reference

Autopilot. More...

+ Include dependency graph for autopilot.c:

Go to the source code of this file.

Macros

#define AUTOPILOT_IN_FLIGHT_TIME   20
 time steps for in_flight detection (at 20Hz, so 20=1second) More...
 
#define AUTOPILOT_IN_FLIGHT_MIN_SPEED   0.2
 minimum vertical speed for in_flight condition in m/s More...
 
#define AUTOPILOT_IN_FLIGHT_MIN_ACCEL   2.0
 minimum vertical acceleration for in_flight condition in m/s^2 More...
 
#define AUTOPILOT_IN_FLIGHT_MIN_THRUST   500
 minimum thrust for in_flight condition in pprz_t units (max = 9600) More...
 
#define FAILSAFE_DESCENT_SPEED   1.5
 Set descent speed in failsafe mode. More...
 
#define FAILSAFE_MODE_TOO_FAR_FROM_HOME   AP_MODE_FAILSAFE
 Mode that is set when the plane is really too far from home. More...
 
#define MODE_STARTUP   AP_MODE_KILL
 
#define UNLOCKED_HOME_MODE   TRUE
 
#define NAV_PRESCALER   (PERIODIC_FREQUENCY / NAV_FREQ)
 
#define THRESHOLD_1_PPRZ   (MIN_PPRZ / 2)
 
#define THRESHOLD_2_PPRZ   (MAX_PPRZ / 2)
 

Functions

static int ahrs_is_aligned (void)
 
void send_autopilot_version (struct transport_tx *trans, struct link_device *dev)
 
static void send_alive (struct transport_tx *trans, struct link_device *dev)
 
static void send_attitude (struct transport_tx *trans, struct link_device *dev)
 
static void send_status (struct transport_tx *trans, struct link_device *dev)
 
static void send_energy (struct transport_tx *trans, struct link_device *dev)
 
static void send_fp (struct transport_tx *trans, struct link_device *dev)
 
static void send_rc (struct transport_tx *trans, struct link_device *dev)
 
static void send_rotorcraft_rc (struct transport_tx *trans, struct link_device *dev)
 
static void send_dl_value (struct transport_tx *trans, struct link_device *dev)
 
static void send_rotorcraft_cmd (struct transport_tx *trans, struct link_device *dev)
 
void autopilot_init (void)
 Autopilot inititalization. More...
 
void autopilot_periodic (void)
 
void autopilot_set_mode (uint8_t new_autopilot_mode)
 
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...
 
void autopilot_check_in_flight (bool_t motors_on)
 
void autopilot_set_motors_on (bool_t motors_on)
 
static uint8_t ap_mode_of_3way_switch (void)
 get autopilot mode as set by RADIO_MODE 3-way switch More...
 
void autopilot_on_rc_frame (void)
 Get autopilot mode from two 2way switches. More...
 

Variables

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

Detailed Description

Autopilot.

Definition in file autopilot.c.

Macro Definition Documentation

#define AUTOPILOT_IN_FLIGHT_MIN_ACCEL   2.0

minimum vertical acceleration for in_flight condition in m/s^2

Definition at line 93 of file autopilot.c.

Referenced by autopilot_check_in_flight().

#define AUTOPILOT_IN_FLIGHT_MIN_SPEED   0.2

minimum vertical speed for in_flight condition in m/s

Definition at line 88 of file autopilot.c.

Referenced by autopilot_check_in_flight().

#define AUTOPILOT_IN_FLIGHT_MIN_THRUST   500

minimum thrust for in_flight condition in pprz_t units (max = 9600)

Definition at line 98 of file autopilot.c.

Referenced by autopilot_check_in_flight().

#define AUTOPILOT_IN_FLIGHT_TIME   20

time steps for in_flight detection (at 20Hz, so 20=1second)

Definition at line 83 of file autopilot.c.

Referenced by autopilot_check_in_flight().

#define FAILSAFE_DESCENT_SPEED   1.5

Set descent speed in failsafe mode.

Definition at line 116 of file autopilot.c.

Referenced by autopilot_set_mode().

#define FAILSAFE_MODE_TOO_FAR_FROM_HOME   AP_MODE_FAILSAFE

Mode that is set when the plane is really too far from home.

Definition at line 122 of file autopilot.c.

Referenced by autopilot_periodic().

#define MODE_STARTUP   AP_MODE_KILL

Definition at line 138 of file autopilot.c.

Referenced by autopilot_init(), and autopilot_set_mode().

#define NAV_PRESCALER   (PERIODIC_FREQUENCY / NAV_FREQ)

Definition at line 333 of file autopilot.c.

Referenced by autopilot_periodic().

#define THRESHOLD_1_PPRZ   (MIN_PPRZ / 2)

Definition at line 599 of file autopilot.c.

Referenced by ap_mode_of_3way_switch().

#define THRESHOLD_2_PPRZ   (MAX_PPRZ / 2)

Definition at line 600 of file autopilot.c.

Referenced by ap_mode_of_3way_switch().

#define UNLOCKED_HOME_MODE   TRUE

Definition at line 144 of file autopilot.c.

Referenced by autopilot_on_rc_frame(), and pprz_mode_update().

Function Documentation

static int ahrs_is_aligned ( void  )
inlinestatic

Definition at line 102 of file autopilot.c.

References stateIsAttitudeValid().

Referenced by autopilot_on_rc_frame(), autopilot_set_mode(), and autopilot_set_motors_on().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static uint8_t ap_mode_of_3way_switch ( void  )
static

get autopilot mode as set by RADIO_MODE 3-way switch

Definition at line 603 of file autopilot.c.

References autopilot_mode_auto2, MODE_AUTO1, MODE_MANUAL, radio_control, RADIO_MODE, THRESHOLD_1_PPRZ, THRESHOLD_2_PPRZ, and RadioControl::values.

Referenced by autopilot_on_rc_frame().

+ Here is the caller graph for this function:

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:

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
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 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
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 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
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 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_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_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 send_alive ( struct transport_tx trans,
struct link_device dev 
)
static

Definition at line 165 of file autopilot.c.

Referenced by autopilot_init().

+ Here is the caller graph for this function:

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

Definition at line 170 of file autopilot.c.

References FloatEulers::phi, FloatEulers::psi, stateGetNedToBodyEulers_f(), and FloatEulers::theta.

Referenced by autopilot_init().

+ 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 158 of file autopilot.c.

Referenced by autopilot_init(), reporting_task(), and telemetry_periodic().

+ Here is the caller graph for this function:

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

Definition at line 267 of file autopilot.c.

Referenced by autopilot_init().

+ Here is the caller graph for this function:

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

Definition at line 203 of file autopilot.c.

References Electrical::current, electrical, Electrical::energy, and Electrical::vsupply.

Referenced by autopilot_init().

+ Here is the caller graph for this function:

static void send_fp ( struct transport_tx trans,
struct link_device dev 
)
static
static void send_rc ( struct transport_tx trans,
struct link_device dev 
)
static

Definition at line 237 of file autopilot.c.

References radio_control, RADIO_CONTROL_NB_CHANNEL, and RadioControl::values.

Referenced by autopilot_init(), and init_fbw().

+ Here is the caller graph for this function:

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

Definition at line 272 of file autopilot.c.

References stabilization_cmd.

Referenced by autopilot_init().

+ Here is the caller graph for this function:

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

Definition at line 242 of file autopilot.c.

References radio_control, RADIO_KILL_SWITCH, RADIO_MODE, RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, RADIO_YAW, RadioControl::status, and RadioControl::values.

Referenced by autopilot_init().

+ 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 70 of file autopilot.c.

bool_t autopilot_ground_detected
uint32_t autopilot_in_flight_counter

Definition at line 69 of file autopilot.c.

Referenced by autopilot_check_in_flight(), autopilot_init(), and autopilot_set_mode().

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_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 73 of file autopilot.c.

Referenced by autopilot_ClearSettings(), and autopilot_StoreSettings().