Paparazzi UAS
v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
|
Autopilot modes. More...
#include "std.h"
#include "generated/airframe.h"
#include "state.h"
#include "subsystems/settings.h"
#include "pprzlink/pprzlink_transport.h"
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... | |
Autopilot modes.
Definition in file autopilot.h.
#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(), autopilot_SetModeHandler(), DetectGroundEvent(), and failsafe_check().
#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_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_guided_move_ned(), autopilot_guided_update(), autopilot_set_mode(), autopilot_SetModeHandler(), failsafe_check(), 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(), autopilot_SetModeHandler(), 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(), autopilot_SetModeHandler(), and failsafe_check().
#define AP_MODE_MODULE 17 |
Definition at line 53 of file autopilot.h.
Referenced by autopilot_set_mode(), autopilot_SetModeHandler(), failsafe_check(), 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(), autopilot_SetModeHandler(), 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 | ) |
Definition at line 92 of file autopilot.h.
#define autopilot_SetPowerSwitch | ( | _v | ) |
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 |
Default RC mode.
Definition at line 82 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 | |||
) |
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().
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().
|
inlinestatic |
Definition at line 174 of file autopilot.h.
References kill_throttle, settings_clear(), and settings_clear_flag.
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.
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. |
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().
bool autopilot_guided_goto_ned | ( | float | x, |
float | y, | ||
float | z, | ||
float | heading | ||
) |
Set position and heading setpoints in GUIDED mode.
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. |
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().
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.
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. |
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().
bool autopilot_guided_move_ned | ( | float | vx, |
float | vy, | ||
float | vz, | ||
float | heading | ||
) |
Set velocity and heading setpoints in GUIDED mode.
vx | North velocity (local NED frame) in meters/sec. |
vy | East velocity (local NED frame) in meters/sec. |
vz | Down velocity (local NED frame) in meters/sec. |
heading | Setpoint in radians. |
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().
void autopilot_guided_update | ( | uint8_t | flags, |
float | x, | ||
float | y, | ||
float | z, | ||
float | yaw | ||
) |
Set guided setpoints using flag mask in GUIDED mode.
flags | Bits 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 |
x | North position/velocity in meters or meters/sec. |
y | East position/velocity in meters or meters/sec. |
z | Down position/velocity in meters or meters/sec. |
yaw | Heading 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().
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, 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_airspeed(), send_alive(), send_attitude(), send_autopilot_version(), send_bat(), send_desired(), send_dl_value(), send_energy(), send_estimator(), send_fp(), send_mode(), send_rc(), send_rotorcraft_cmd(), send_rotorcraft_rc(), send_status(), stabilization_attitude_init(), stabilization_init(), stabilization_none_init(), and stabilization_rate_init().
Referenced by init_ap(), and main_init().
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().
void autopilot_periodic | ( | void | ) |
Definition at line 344 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(), datalink_lost(), dist2_to_home, failsafe_mode_dist2, FAILSAFE_MODE_TOO_FAR_FROM_HOME, guidance_h_run(), guidance_v_run(), higher_than_max_altitude(), nav_home(), nav_periodic_task(), NAV_PRESCALER, SetCommands, SetRotorcraftCommands, stabilization_cmd, and too_far_from_home.
Referenced by main_periodic().
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().
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().
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.
|
inlinestatic |
Definition at line 166 of file autopilot.h.
References kill_throttle, settings_store(), and settings_store_flag.
|
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().
void send_autopilot_version | ( | struct transport_tx * | trans, |
struct link_device * | dev | ||
) |
Definition at line 64 of file autopilot.c.
Referenced by autopilot_init().
bool autopilot_detect_ground_once |
Definition at line 83 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_bat(), send_fp(), and telecommand_task().
bool autopilot_ground_detected |
Definition at line 82 of file autopilot.c.
Referenced by autopilot_init(), autopilot_periodic(), DetectGroundEvent(), and nav_detect_ground().
bool autopilot_in_flight |
Definition at line 72 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 69 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_guided_move_ned(), autopilot_guided_update(), autopilot_init(), autopilot_on_rc_frame(), autopilot_periodic(), autopilot_set_mode(), autopilot_set_motors_on(), DetectGroundEvent(), 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 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().
bool 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(), nav_launcher_run(), nav_launcher_setup(), nav_skid_landing_run(), nps_autopilot_run_step(), send_bat(), v_ctl_climb_loop(), and v_ctl_landing_loop().