![]() |
Paparazzi UAS
v6.3_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_set_motors_on (bool motors_on) |
Start or stop motors May have no effect if motors has auto-start based on throttle setpoint. 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 187 of file autopilot.h.
#define autopilot_KillThrottle | ( | _kill | ) | autopilot_set_kill_throttle(_kill) |
Definition at line 144 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 180 of file autopilot.h.
#define USE_GENERATED_AUTOPILOT FALSE |
Definition at line 51 of file autopilot.h.
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 270 of file autopilot.c.
References autopilot, autopilot_in_flight_counter, AUTOPILOT_IN_FLIGHT_MIN_ACCEL, AUTOPILOT_IN_FLIGHT_MIN_SPEED, AUTOPILOT_IN_FLIGHT_MIN_THRUST, AUTOPILOT_IN_FLIGHT_TIME, pprz_autopilot::in_flight, stabilization_cmd, stateGetAccelNed_f(), stateGetSpeedNed_f(), and NedCoor_f::z.
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 306 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 173 of file autopilot.c.
References AP_MODE_FAILSAFE, autopilot, pprz_autopilot::detect_ground_once, pprz_autopilot::ground_detected, pprz_autopilot::mode, stateGetAccelNed_f(), THRESHOLD_GROUND_DETECT, and NedCoor_f::z.
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 188 of file autopilot.c.
References AP_MODE_AUTO1, AP_MODE_AUTO2, AP_MODE_FAILSAFE, AP_MODE_FLIP, AP_MODE_GPS_OUT_OF_ORDER, AP_MODE_GUIDED, AP_MODE_HOME, AP_MODE_KILL, AP_MODE_MANUAL, AP_MODE_MODULE, AP_MODE_NAV, autopilot, autopilot_get_mode(), autopilot_get_motors_on(), autopilot_send_mode(), 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().
uint8_t autopilot_get_mode | ( | void | ) |
Get autopilot mode.
Get autopilot mode.
Definition at line 216 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_guided_goto_body_relative(), autopilot_guided_goto_ned(), autopilot_guided_goto_ned_relative(), autopilot_guided_move_ned(), autopilot_guided_parse_GUIDED(), 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(), stabilization_opticflow_vel_cb(), 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 244 of file autopilot.c.
References autopilot, and pprz_autopilot::motors_on.
Referenced by actuators_ardrone_motor_status(), actuators_bebop_commit(), actuators_esc32_commit(), autopilot_failsafe_checks(), autopilot_periodic(), 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(), stabilization_indi_rate_run(), and throttle_curve_run().
bool autopilot_in_flight | ( | void | ) |
Get in flight status.
Get in flight status.
Definition at line 289 of file autopilot.c.
References autopilot, and pprz_autopilot::in_flight.
Referenced by autopilot_generated_periodic(), autopilot_periodic(), autopilot_static_on_rc_frame(), autopilot_static_periodic(), e_identification_fr_periodic(), gps_sim_hitl_event(), guidance_flip_run(), ins_ekf2_publish_attitude(), ins_ekf2_update(), ins_flow_update(), mav_exercise_periodic(), nav_is_in_flight(), and orange_avoider_periodic().
void autopilot_init | ( | void | ) |
Autopilot initialization function.
Definition at line 106 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 177 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 145 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 223 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 275 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 324 of file autopilot.c.
References DefaultChannel, DefaultDevice, and send_mode().
Referenced by autopilot_failsafe_checks().
void autopilot_send_version | ( | void | ) |
Report autopilot version on default downlink channel.
Report autopilot version on default downlink channel.
Definition at line 316 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 279 of file autopilot.c.
References autopilot, autopilot_reset_in_flight_counter(), and pprz_autopilot::in_flight.
Referenced by autopilot_arming_check_motors_on(), and autopilot_static_set_mode().
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 251 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 192 of file autopilot.c.
References autopilot, autopilot_generated_set_mode(), autopilot_static_set_mode(), pprz_autopilot::mode, and mode.
Referenced by autopilot_failsafe_checks(), guidance_flip_run(), mavlink_common_message_handler(), nav_set_failsafe(), and pprz_mode_update().
void autopilot_set_motors_on | ( | bool | motors_on | ) |
Start or stop motors May have no effect if motors has auto-start based on throttle setpoint.
[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.
Definition at line 232 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 actuators_ardrone_motor_status(), actuators_bebop_commit(), autopilot_set_kill_throttle(), autopilot_static_set_mode(), 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 205 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 296 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 262 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_check_in_flight(), autopilot_clear_settings(), autopilot_event(), autopilot_failsafe_checks(), 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(), draw_osd(), Drift_correction(), e_identification_fr_periodic(), gazebo_write(), generic_com_periodic(), gsm_send_report_continue(), guidance_h_module_enter(), guidance_h_module_read_rc(), 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(), 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().