Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Core autopilot interface common to all firmwares. More...
#include "std.h"
#include "paparazzi.h"
#include "generated/airframe.h"
#include "state.h"
#include "autopilot_utils.h"
#include "autopilot_firmware.h"
#include "autopilot_static.h"
Go to the source code of this file.
Data Structures | |
struct | pprz_autopilot |
PPRZ Autopilot structure definition. More... | |
Macros | |
#define | USE_GENERATED_AUTOPILOT FALSE |
#define | autopilot_ResetFlightTimeAndLaunch(_) autopilot_reset_flight_time() |
#define | autopilot_KillThrottle(_kill) autopilot_set_kill_throttle(_kill) |
#define | autopilot_StoreSettings(_) autopilot_store_settings() |
#define | autopilot_ClearSettings(_) autopilot_clear_settings() |
Functions | |
void | autopilot_init (void) |
Autopilot initialization function. More... | |
void | autopilot_periodic (void) |
Autopilot periodic call at PERIODIC_FREQUENCY. More... | |
void | autopilot_event (void) |
Autopilot event check function. More... | |
void | autopilot_on_rc_frame (void) |
Autopilot RC input event hadler. More... | |
void | autopilot_failsafe_checks (void) |
Autopilot periodic failsafe checks. More... | |
bool | autopilot_set_mode (uint8_t new_autopilot_mode) |
Set new autopilot mode. More... | |
void | autopilot_SetModeHandler (float new_autopilot_mode) |
Handler for setter function with dl_setting. More... | |
uint8_t | autopilot_get_mode (void) |
Get autopilot mode. More... | |
void | autopilot_reset_flight_time (void) |
Reset flight time and launch status Also provide macro for dl_setting backward compatibility. More... | |
void | autopilot_force_motors_on (bool motors_on) |
Force start/stop the motors WARNING This will skip he preflight checks. More... | |
bool | autopilot_set_motors_on (bool motors_on) |
Start or stop motors May have no effect if motors has auto-start based on throttle setpoint or when the preflight checks are failing. More... | |
bool | autopilot_arming_motors_on (bool motors_on) |
Start or stop the motors during arming May not happen when preflight checks are failing. More... | |
bool | autopilot_get_motors_on (void) |
Get motor status. More... | |
void | autopilot_set_kill_throttle (bool kill) |
Enable or disable motor control from autopilot Also provide macro for dl_setting backward compatibility. More... | |
bool | autopilot_throttle_killed (void) |
Get kill status. More... | |
void | autopilot_check_in_flight (bool motors_on) |
Check if airframe is in flight. More... | |
void | autopilot_set_in_flight (bool in_flight) |
Set in flight status. More... | |
bool | autopilot_in_flight (void) |
Get in flight status. More... | |
void | autopilot_reset_in_flight_counter (void) |
reset in_flight counter actual implementation is firmware dependent More... | |
void | autopilot_store_settings (void) |
Store marked settings in flash Try to make sure that we don't write to flash while flying Also provide macro for dl_setting backward compatibility. More... | |
void | autopilot_clear_settings (void) |
Clear marked settings in flash try to make sure that we don't write to flash while flying Also provide macro for dl_setting backward compatibility. More... | |
void | autopilot_send_version (void) |
Report autopilot version on default downlink channel. More... | |
void | autopilot_send_mode (void) |
Report autopilot mode on default downlink channel. More... | |
Variables | |
struct pprz_autopilot | autopilot |
Global autopilot structure. More... | |
Core autopilot interface common to all firmwares.
Using either static or generated autopilot logic, which depends on the firmware.
Definition in file autopilot.h.
struct pprz_autopilot |
PPRZ Autopilot structure definition.
Definition at line 62 of file autopilot.h.
Data Fields | ||
---|---|---|
uint8_t | arming_status | arming status |
bool | detect_ground_once | enable automatic detection of ground (one shot) |
uint16_t | flight_time | flight time in seconds |
bool | ground_detected | automatic detection of landing |
bool | in_flight | in flight status |
bool | kill_throttle | allow autopilot to use throttle |
bool | launch | request launch |
uint8_t | mode | current autopilot mode |
uint8_t | mode_auto2 | FIXME hide this in a private part ? |
bool | motors_on | motor status |
pprz_t | throttle | throttle level as will be displayed in GCS |
bool | use_rc | enable/disable RC input |
#define autopilot_ClearSettings | ( | _ | ) | autopilot_clear_settings() |
Definition at line 204 of file autopilot.h.
#define autopilot_KillThrottle | ( | _kill | ) | autopilot_set_kill_throttle(_kill) |
Definition at line 161 of file autopilot.h.
#define autopilot_ResetFlightTimeAndLaunch | ( | _ | ) | autopilot_reset_flight_time() |
Definition at line 123 of file autopilot.h.
#define autopilot_StoreSettings | ( | _ | ) | autopilot_store_settings() |
Definition at line 197 of file autopilot.h.
#define USE_GENERATED_AUTOPILOT FALSE |
Definition at line 51 of file autopilot.h.
bool autopilot_arming_motors_on | ( | bool | motors_on | ) |
Start or stop the motors during arming May not happen when preflight checks are failing.
Start or stop the motors during arming May not happen when preflight checks are failing.
Definition at line 273 of file autopilot.c.
References autopilot, pprz_autopilot::motors_on, preflight_bypass, and preflight_check().
Referenced by autopilot_arming_check_motors_on().
void autopilot_check_in_flight | ( | bool | motors_on | ) |
Check if airframe is in flight.
[in] | motors_on | motors status |
Check if airframe is in flight.
Definition at line 321 of file autopilot.c.
References autopilot, autopilot_in_flight_counter, autopilot_in_flight_end_detection(), AUTOPILOT_IN_FLIGHT_MIN_THRUST, AUTOPILOT_IN_FLIGHT_TIME, Stabilization::cmd, pprz_autopilot::in_flight, and stabilization.
Referenced by autopilot_periodic().
void autopilot_clear_settings | ( | void | ) |
Clear marked settings in flash try to make sure that we don't write to flash while flying Also provide macro for dl_setting backward compatibility.
Clear marked settings in flash try to make sure that we don't write to flash while flying Also provide macro for dl_setting backward compatibility.
Definition at line 357 of file autopilot.c.
References autopilot, pprz_autopilot::kill_throttle, settings_clear(), and settings_clear_flag.
void autopilot_event | ( | void | ) |
Autopilot event check function.
Autopilot event check function.
Autopilot event check function.
used for automatic ground detection
Definition at line 174 of file autopilot.c.
References AP_MODE_FAILSAFE, autopilot, autopilot_ground_detection(), pprz_autopilot::detect_ground_once, pprz_autopilot::ground_detected, and pprz_autopilot::mode.
void autopilot_failsafe_checks | ( | void | ) |
Autopilot periodic failsafe checks.
Autopilot periodic failsafe checks.
Autopilot periodic failsafe checks.
Checks order:
send mode if changed at the end
Definition at line 189 of file autopilot.c.
References AP_MODE_AUTO1, AP_MODE_AUTO2, AP_MODE_FAILSAFE, AP_MODE_GPS_OUT_OF_ORDER, AP_MODE_GUIDED, AP_MODE_HOME, AP_MODE_KILL, AP_MODE_MANUAL, AP_MODE_NAV, autopilot, autopilot_get_mode(), autopilot_get_motors_on(), autopilot_set_kill_throttle(), autopilot_set_mode(), Electrical::bat_critical, datalink_lost(), datalink_time, electrical, pprz_autopilot::flight_time, gps, GpsIsLost, higher_than_max_altitude(), GpsState::last_3dfix_time, pprz_autopilot::launch, MIN_PPRZ, sys_time::nb_sec, pprz_mode_update(), radio_control, radio_control_get(), RADIO_KILL_SWITCH, RADIO_THROTTLE, RadioControlIsLost, RC_LOST_MODE, RC_OK, RC_REALLY_LOST, rc_settings(), RadioControl::status, THROTTLE_THRESHOLD_TAKEOFF, too_far_from_home, and RadioControl::values.
Referenced by autopilot_periodic(), and main_ap_init().
void autopilot_force_motors_on | ( | bool | motors_on | ) |
Force start/stop the motors WARNING This will skip he preflight checks.
motors_on | Wheter the motors should be forced on/off |
Force start/stop the motors WARNING This will skip he preflight checks.
Definition at line 237 of file autopilot.c.
References autopilot, autopilot_generated_set_motors_on(), autopilot_static_set_motors_on(), pprz_autopilot::kill_throttle, and pprz_autopilot::motors_on.
Referenced by autopilot_set_motors_on().
uint8_t autopilot_get_mode | ( | void | ) |
Get autopilot mode.
Get autopilot mode.
Definition at line 222 of file autopilot.c.
References autopilot, and pprz_autopilot::mode.
Referenced by attitude_loop(), autopilot_arming_check_motors_on(), autopilot_arming_check_valid(), autopilot_arming_set(), autopilot_failsafe_checks(), autopilot_static_on_rc_frame(), cam_periodic(), collective_tracking_control(), distributed_circular(), generic_com_periodic(), guidance_flip_enter(), gvf_low_level_control_2D(), gvf_parametric_low_level_control_2D(), gvf_parametric_low_level_control_3D(), h_ctl_pitch_loop(), h_ctl_roll_loop(), mavlink_common_message_handler(), mavlink_send_heartbeat(), navigation_task(), NavKillThrottle(), NavResurrect(), periodic_auto1_commands(), pprz_mode_update(), send_fbw_status(), send_mode(), v_ctl_set_pitch(), v_ctl_set_throttle(), and vPoint().
bool autopilot_get_motors_on | ( | void | ) |
Get motor status.
Get motor status.
Definition at line 295 of file autopilot.c.
References autopilot, and pprz_autopilot::motors_on.
Referenced by actuators_ardrone_motor_status(), actuators_bebop_commit(), actuators_esc32_commit(), autopilot_arming_check_motors_on(), autopilot_failsafe_checks(), autopilot_periodic(), get_act_state_oneloop(), hott_check_serial_data(), hott_update_eam_msg(), hott_update_gam_msg(), intermcu_send_commands(), mavlink_common_message_handler(), nav_takeoff_run(), nps_autopilot_run_step(), sdlogger_spi_direct_periodic(), stabilization_attitude_run(), and throttle_curve_run().
bool autopilot_in_flight | ( | void | ) |
Get in flight status.
Get in flight status.
Definition at line 340 of file autopilot.c.
References autopilot, and pprz_autopilot::in_flight.
Referenced by autopilot_generated_periodic(), autopilot_periodic(), autopilot_static_periodic(), e_identification_fr_periodic(), ekf_aw_wrapper_periodic(), guidance_flip_run(), guidance_h_run(), guidance_module_enter(), ins_ekf2_update(), ins_flow_update(), mav_exercise_periodic(), mavlink_send_extended_sys_state(), nav_is_in_flight(), orange_avoider_periodic(), and rc_cb().
void autopilot_init | ( | void | ) |
Autopilot initialization function.
Definition at line 107 of file autopilot.c.
References autopilot, autopilot_firmware_init(), autopilot_generated_init(), AUTOPILOT_RC_ID, autopilot_static_init(), DefaultPeriodic, pprz_autopilot::detect_ground_once, pprz_autopilot::flight_time, pprz_autopilot::ground_detected, pprz_autopilot::in_flight, pprz_autopilot::kill_throttle, pprz_autopilot::mode_auto2, MODE_AUTO2, pprz_autopilot::motors_on, rc_cb(), rc_ev, register_periodic_telemetry(), send_alive(), send_attitude(), send_autopilot_version(), send_dl_value(), send_minimal_com(), pprz_autopilot::throttle, and pprz_autopilot::use_rc.
void autopilot_on_rc_frame | ( | void | ) |
Autopilot RC input event hadler.
Autopilot RC input event hadler.
Definition at line 178 of file autopilot.c.
References autopilot_generated_on_rc_frame(), and autopilot_static_on_rc_frame().
Referenced by rc_cb().
void autopilot_periodic | ( | void | ) |
Autopilot periodic call at PERIODIC_FREQUENCY.
Autopilot periodic call at PERIODIC_FREQUENCY.
Definition at line 146 of file autopilot.c.
References autopilot, autopilot_check_in_flight(), autopilot_failsafe_checks(), autopilot_generated_periodic(), autopilot_get_motors_on(), autopilot_in_flight(), autopilot_static_periodic(), and pprz_autopilot::flight_time.
void autopilot_reset_flight_time | ( | void | ) |
Reset flight time and launch status Also provide macro for dl_setting backward compatibility.
Reset flight time and launch status Also provide macro for dl_setting backward compatibility.
Definition at line 229 of file autopilot.c.
References autopilot, pprz_autopilot::flight_time, and pprz_autopilot::launch.
void autopilot_reset_in_flight_counter | ( | void | ) |
reset in_flight counter actual implementation is firmware dependent
reset in_flight counter actual implementation is firmware dependent
Definition at line 326 of file autopilot.c.
References autopilot_in_flight_counter.
Referenced by autopilot_set_in_flight().
void autopilot_send_mode | ( | void | ) |
Report autopilot mode on default downlink channel.
Report autopilot mode on default downlink channel.
Definition at line 375 of file autopilot.c.
References DefaultChannel, DefaultDevice, send_mode(), and send_status().
Referenced by autopilot_set_mode().
void autopilot_send_version | ( | void | ) |
Report autopilot version on default downlink channel.
Report autopilot version on default downlink channel.
Definition at line 367 of file autopilot.c.
References DefaultChannel, DefaultDevice, and send_autopilot_version().
Referenced by telemetry_reporting_task().
void autopilot_set_in_flight | ( | bool | in_flight | ) |
Set in flight status.
[in] | in_flight | in flight status |
Set in flight status.
Definition at line 330 of file autopilot.c.
References autopilot, autopilot_reset_in_flight_counter(), and pprz_autopilot::in_flight.
Referenced by autopilot_arming_check_motors_on(), autopilot_static_set_mode(), and nav_takeoff_run().
void autopilot_set_kill_throttle | ( | bool | kill | ) |
Enable or disable motor control from autopilot Also provide macro for dl_setting backward compatibility.
[in] | kill | true to disable (kill), false to enable (un-kill) |
Enable or disable motor control from autopilot Also provide macro for dl_setting backward compatibility.
Definition at line 302 of file autopilot.c.
References autopilot_set_motors_on().
Referenced by actuators_disco_commit(), autopilot_failsafe_checks(), nav_bungee_takeoff_run(), nav_bungee_takeoff_setup(), and nav_skid_landing_run().
bool autopilot_set_mode | ( | uint8_t | new_autopilot_mode | ) |
Set new autopilot mode.
[in] | new_autopilot_mode | new mode to set |
Set new autopilot mode.
Definition at line 193 of file autopilot.c.
References autopilot, autopilot_generated_set_mode(), autopilot_send_mode(), autopilot_static_set_mode(), pprz_autopilot::mode, and mode.
Referenced by autopilot_failsafe_checks(), guidance_flip_run(), guidance_module_run(), mavlink_common_message_handler(), nav_set_failsafe(), and pprz_mode_update().
bool autopilot_set_motors_on | ( | bool | motors_on | ) |
Start or stop motors May have no effect if motors has auto-start based on throttle setpoint or when the preflight checks are failing.
[in] | motors_on | true to start motors, false to stop |
Start or stop motors May have no effect if motors has auto-start based on throttle setpoint or when the preflight checks are failing.
Definition at line 250 of file autopilot.c.
References autopilot, autopilot_force_motors_on(), pprz_autopilot::motors_on, preflight_bypass, and preflight_check().
Referenced by actuators_ardrone_motor_status(), actuators_bebop_commit(), autopilot_set_kill_throttle(), autopilot_static_set_mode(), ground_detect_periodic(), mavlink_common_message_handler(), nav_takeoff_run(), NavKillThrottle(), and NavResurrect().
void autopilot_SetModeHandler | ( | float | mode | ) |
Handler for setter function with dl_setting.
Handler for setter function with dl_setting.
Definition at line 211 of file autopilot.c.
References autopilot_generated_SetModeHandler(), autopilot_static_SetModeHandler(), and mode.
void autopilot_store_settings | ( | void | ) |
Store marked settings in flash Try to make sure that we don't write to flash while flying Also provide macro for dl_setting backward compatibility.
Store marked settings in flash Try to make sure that we don't write to flash while flying Also provide macro for dl_setting backward compatibility.
Definition at line 347 of file autopilot.c.
References autopilot, pprz_autopilot::kill_throttle, settings_store(), and settings_store_flag.
bool autopilot_throttle_killed | ( | void | ) |
Get kill status.
Get kill status.
Definition at line 313 of file autopilot.c.
References autopilot, and pprz_autopilot::kill_throttle.
Referenced by electrical_periodic(), geo_mag_periodic(), mavlink_send_heartbeat(), nav_skid_landing_run(), v_ctl_climb_loop(), and v_ctl_landing_loop().
|
extern |
Global autopilot structure.
Definition at line 1 of file autopilot.c.
Referenced by attitude_loop(), autopilot_arming_check_motors_on(), autopilot_arming_check_valid(), autopilot_arming_motors_on(), autopilot_check_in_flight(), autopilot_clear_settings(), autopilot_event(), autopilot_failsafe_checks(), autopilot_force_motors_on(), autopilot_generated_init(), autopilot_generated_on_rc_frame(), autopilot_generated_periodic(), autopilot_generated_set_mode(), autopilot_generated_set_motors_on(), autopilot_get_mode(), autopilot_get_motors_on(), autopilot_in_flight(), autopilot_init(), autopilot_periodic(), autopilot_reset_flight_time(), autopilot_set_in_flight(), autopilot_set_mode(), autopilot_set_motors_on(), autopilot_static_init(), autopilot_static_on_rc_frame(), autopilot_static_periodic(), autopilot_static_set_mode(), autopilot_static_set_motors_on(), autopilot_store_settings(), autopilot_throttle_killed(), calc_flight_time_left(), check_parachute_arming(), check_parachute_trigger(), draw_osd(), Drift_correction(), e_identification_fr_periodic(), eff_scheduling_rotwing_update_hover_motor_effectiveness(), gazebo_write(), generic_com_periodic(), gsm_send_report_continue(), h_ctl_course_loop(), h_ctl_pitch_loop(), h_ctl_roll_loop(), main_recovery_init(), mf_daq_send_state(), monitor_task(), nav_bungee_takeoff_run(), nav_catapult_highrate_module(), nav_check_wp_time(), nav_detect_ground(), nav_home(), nav_takeoff_run(), nps_autopilot_run_step(), oneloop_andi_run(), parse_mf_daq_msg(), send_energy(), send_fp(), send_minimal_com(), send_mode(), send_status(), uav_recovery_periodic(), v_ctl_climb_loop(), v_ctl_guidance_loop(), v_ctl_set_pitch(), and v_ctl_set_throttle().