76 virtual bool init(
bool ignore_checks) = 0;
77 virtual void run() = 0;
86 virtual const char *
name()
const = 0;
89 virtual const char *
name4()
const = 0;
138 #if FRAME_CONFIG == HELI_FRAME 166 #if MODE_ACRO_ENABLED == ENABLED 173 virtual bool init(
bool ignore_checks)
override;
174 virtual void run()
override;
183 const char *
name()
const override {
return "ACRO"; }
184 const char *
name4()
const override {
return "ACRO"; }
186 void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in,
float &roll_out,
float &pitch_out,
float &yaw_out);
193 #if FRAME_CONFIG == HELI_FRAME 200 bool init(
bool ignore_checks)
override;
215 bool init(
bool ignore_checks)
override;
225 const char *
name()
const override {
return "ALT_HOLD"; }
226 const char *
name4()
const override {
return "ALTH"; }
239 bool init(
bool ignore_checks)
override;
253 void takeoff_start(
const Location& dest_loc);
254 void wp_start(
const Vector3f& destination);
257 void land_start(
const Vector3f& destination);
258 void circle_movetoedge_start(
const Location_Class &circle_center,
float radius_m);
262 void nav_guided_start();
266 void payload_place_start();
278 const char *
name()
const override {
return "AUTO"; }
279 const char *
name4()
const override {
return "AUTO"; }
296 void nav_guided_run();
299 void payload_place_start(
const Vector3f& destination);
300 void payload_place_run();
301 bool payload_place_run_should_run();
302 void payload_place_run_loiter();
303 void payload_place_run_descend();
304 void payload_place_run_release();
317 #if NAV_GUIDED == ENABLED 329 #if CAMERA == ENABLED 333 #if PARACHUTE == ENABLED 336 #if GRIPPER_ENABLED == ENABLED 339 #if WINCH_ENABLED == ENABLED 345 bool verify_takeoff();
347 bool verify_payload_place();
348 bool verify_loiter_unlimited();
349 bool verify_loiter_time();
351 bool verify_wait_delay();
352 bool verify_within_distance();
357 #if NAV_GUIDED == ENABLED 391 #if AUTOTUNE_ENABLED == ENABLED 398 bool init(
bool ignore_checks)
override;
406 void save_tuning_gains();
412 const char *
name()
const override {
return "AUTOTUNE"; }
413 const char *
name4()
const override {
return "ATUN"; }
417 bool start(
bool ignore_checks);
419 void autotune_attitude_control();
420 void backup_gains_and_initialise();
421 void load_orig_gains();
422 void load_tuned_gains();
423 void load_intra_test_gains();
424 void load_twitch_gains();
425 void update_gcs(uint8_t message_id);
427 bool pitch_enabled();
429 void twitching_test_rate(
float rate,
float rate_target,
float &meas_rate_min,
float &meas_rate_max);
430 void twitching_test_angle(
float angle,
float rate,
float angle_target,
float &meas_angle_min,
float &meas_angle_max,
float &meas_rate_min,
float &meas_rate_max);
431 void twitching_measure_acceleration(
float &rate_of_change,
float rate_measurement,
float &rate_measurement_max);
432 void updating_rate_d_up(
float &tune_d,
float tune_d_min,
float tune_d_max,
float tune_d_step_ratio,
float &tune_p,
float tune_p_min,
float tune_p_max,
float tune_p_step_ratio,
float rate_target,
float meas_rate_min,
float meas_rate_max);
433 void updating_rate_d_down(
float &tune_d,
float tune_d_min,
float tune_d_step_ratio,
float &tune_p,
float tune_p_min,
float tune_p_max,
float tune_p_step_ratio,
float rate_target,
float meas_rate_min,
float meas_rate_max);
434 void updating_rate_p_up_d_down(
float &tune_d,
float tune_d_min,
float tune_d_step_ratio,
float &tune_p,
float tune_p_min,
float tune_p_max,
float tune_p_step_ratio,
float rate_target,
float meas_rate_min,
float meas_rate_max);
435 void updating_angle_p_down(
float &tune_p,
float tune_p_min,
float tune_p_step_ratio,
float angle_target,
float meas_angle_max,
float meas_rate_min,
float meas_rate_max);
436 void updating_angle_p_up(
float &tune_p,
float tune_p_max,
float tune_p_step_ratio,
float angle_target,
float meas_angle_max,
float meas_rate_min,
float meas_rate_max);
439 #if LOGGING_ENABLED == ENABLED 440 void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step,
float meas_target,
float meas_min,
float meas_max,
float new_gain_rp,
float new_gain_rd,
float new_gain_sp,
float new_ddt);
441 void Log_Write_AutoTuneDetails(
float angle_cd,
float rate_cds);
444 void send_step_string();
445 const char *level_issue_string()
const;
446 const char * type_string()
const;
447 void announce_state_to_gcs();
448 void do_gcs_announcements();
459 bool check_level(
const enum LEVEL_ISSUE issue,
const float current,
const float maximum);
460 bool currently_level();
472 WAITING_FOR_LEVEL = 0,
522 float orig_roll_rp = 0, orig_roll_ri, orig_roll_rd,
orig_roll_sp, orig_roll_accel;
523 float orig_pitch_rp = 0, orig_pitch_ri, orig_pitch_rd,
orig_pitch_sp, orig_pitch_accel;
524 float orig_yaw_rp = 0, orig_yaw_ri, orig_yaw_rd, orig_yaw_rLPF,
orig_yaw_sp, orig_yaw_accel;
553 bool init(
bool ignore_checks)
override;
561 void timeout_to_loiter_ms(uint32_t timeout_ms);
565 const char *
name()
const override {
return "BRAKE"; }
566 const char *
name4()
const override {
return "BRAK"; }
582 bool init(
bool ignore_checks)
override;
592 const char *
name()
const override {
return "CIRCLE"; }
593 const char *
name4()
const override {
return "CIRC"; }
601 bool pilot_yaw_override =
false;
611 bool init(
bool ignore_checks)
override;
621 const char *
name()
const override {
return "DRIFT"; }
622 const char *
name4()
const override {
return "DRIF"; }
626 float get_throttle_assist(
float velz,
float pilot_throttle_scaled);
637 bool init(
bool ignore_checks)
override;
647 const char *
name()
const override {
return "FLIP"; }
648 const char *
name4()
const override {
return "FLIP"; }
658 #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED 669 bool init(
bool ignore_checks)
override;
670 void run(
void)
override;
680 const char *
name()
const override {
return "FLOWHOLD"; }
681 const char *
name4()
const override {
return "FHLD"; }
694 void flow_to_angle(
Vector2f &bf_angle);
698 bool flowhold_init(
bool ignore_checks);
700 void flowhold_flow_to_angle(
Vector2f &angle,
bool stick_input);
701 void update_height_estimate(
void);
704 const float height_min = 0.1;
707 const float height_max = 3.0;
748 bool init(
bool ignore_checks)
override;
758 bool set_destination(
const Vector3f& destination,
bool use_yaw =
false,
float yaw_cd = 0.0,
bool use_yaw_rate =
false,
float yaw_rate_cds = 0.0,
bool yaw_relative =
false);
759 bool set_destination(
const Location_Class& dest_loc,
bool use_yaw =
false,
float yaw_cd = 0.0,
bool use_yaw_rate =
false,
float yaw_rate_cds = 0.0,
bool yaw_relative =
false);
761 void set_velocity(
const Vector3f& velocity,
bool use_yaw =
false,
float yaw_cd = 0.0,
bool use_yaw_rate =
false,
float yaw_rate_cds = 0.0,
bool yaw_relative =
false,
bool log_request =
true);
762 bool set_destination_posvel(
const Vector3f& destination,
const Vector3f& velocity,
bool use_yaw =
false,
float yaw_cd = 0.0,
bool use_yaw_rate =
false,
float yaw_rate_cds = 0.0,
bool yaw_relative =
false);
765 void limit_init_time_and_pos();
766 void limit_set(uint32_t timeout_ms,
float alt_min_cm,
float alt_max_cm,
float horiz_max_cm);
769 bool takeoff_start(
float final_alt_above_home);
773 void angle_control_start();
774 void angle_control_run();
778 const char *
name()
const override {
return "GUIDED"; }
779 const char *
name4()
const override {
return "GUID"; }
786 void pos_control_start();
787 void vel_control_start();
788 void posvel_control_start();
790 void pos_control_run();
791 void vel_control_run();
792 void posvel_control_run();
793 void set_desired_velocity_with_accel_and_fence_limits(
const Vector3f& vel_des);
794 void set_yaw_state(
bool use_yaw,
float yaw_cd,
bool use_yaw_rate,
float yaw_rate_cds,
bool relative_angle);
808 bool init(
bool ignore_checks)
override;
818 const char *
name()
const override {
return "GUIDED_NOGPS"; }
819 const char *
name4()
const override {
return "GNGP"; }
832 bool init(
bool ignore_checks)
override;
841 void do_not_use_GPS();
845 const char *
name()
const override {
return "LAND"; }
846 const char *
name4()
const override {
return "LAND"; }
861 bool init(
bool ignore_checks)
override;
869 #if PRECISION_LANDING == ENABLED 875 const char *
name()
const override {
return "LOITER"; }
876 const char *
name4()
const override {
return "LOIT"; }
881 #if PRECISION_LANDING == ENABLED 882 bool do_precision_loiter();
883 void precision_loiter_xy();
888 #if PRECISION_LANDING == ENABLED 901 bool init(
bool ignore_checks)
override;
911 const char *
name()
const override {
return "POSHOLD"; }
912 const char *
name4()
const override {
return "PHLD"; }
916 void poshold_update_pilot_lean_angle(
float &lean_angle_filtered,
float &lean_angle_raw);
917 int16_t poshold_mix_controls(
float mix_ratio, int16_t first_control, int16_t second_control);
918 void poshold_update_brake_angle_from_velocity(int16_t &brake_angle,
float velocity);
919 void poshold_update_wind_comp_estimate();
920 void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle);
921 void poshold_roll_controller_to_pilot_override();
922 void poshold_pitch_controller_to_pilot_override();
933 bool init(
bool ignore_checks)
override;
937 void run(
bool disarm_on_land);
951 void restart_without_terrain();
955 const char *
name()
const override {
return "RTL"; }
956 const char *
name4()
const override {
return "RTL "; }
961 void descent_start();
964 void land_run(
bool disarm_on_land);
972 void climb_return_run();
973 void loiterathome_start();
974 void loiterathome_run();
975 void build_path(
bool terrain_following_allowed);
976 void compute_return_target(
bool terrain_following_allowed);
980 bool _state_complete =
false;
993 uint32_t _loiter_start_time = 0;
1003 bool init(
bool ignore_checks)
override;
1004 void run()
override;
1011 void save_position();
1016 const char *
name()
const override {
return "SMARTRTL"; }
1017 const char *
name4()
const override {
return "SRTL"; }
1024 void wait_cleanup_run();
1025 void path_follow_run();
1026 void pre_land_position_run();
1039 bool init(
bool ignore_checks)
override;
1040 void run()
override;
1049 const char *
name()
const override {
return "SPORT"; }
1050 const char *
name4()
const override {
return "SPRT"; }
1063 virtual bool init(
bool ignore_checks)
override;
1064 virtual void run()
override;
1073 const char *
name()
const override {
return "STABILIZE"; }
1074 const char *
name4()
const override {
return "STAB"; }
1080 #if FRAME_CONFIG == HELI_FRAME 1087 bool init(
bool ignore_checks)
override;
1088 void run()
override;
1104 bool init(
bool ignore_checks)
override;
1105 void run()
override;
1114 ThrowType_Upward = 0,
1120 const char *
name()
const override {
return "THROW"; }
1121 const char *
name4()
const override {
return "THRW"; }
1125 bool throw_detected();
1126 bool throw_position_good();
1127 bool throw_height_good();
1128 bool throw_attitude_good();
1155 bool init(
bool ignore_checks)
override;
1156 void run()
override;
1163 bool set_velocity(
const Vector3f& velocity_neu);
1167 const char *
name()
const override {
return "AVOID_ADSB"; }
1168 const char *
name4()
const override {
return "AVOI"; }
1181 bool init(
bool ignore_checks)
override;
1182 void run()
override;
1191 const char *
name()
const override {
return "FOLLOW"; }
1192 const char *
name4()
const override {
return "FOLL"; }
const char * name4() const override
bool is_autopilot() const override
void land_run_vertical_control(bool pause_descent=false)
bool requires_GPS() const override
const char * name() const override
AC_PosControl *& pos_control
bool has_manual_throttle() const override
Vector2f delta_velocity_ne
bool has_manual_throttle() const override
RC_Channel *& channel_throttle
Location_Class origin_point
bool requires_GPS() const override
bool has_manual_throttle() const override
bool allows_arming(bool from_gcs) const override
bool is_autopilot() const override
const char * name() const override
bool allows_arming(bool from_gcs) const override
bool has_manual_throttle() const override
const char * name() const override
LowPassFilterFloat rotation_rate_filt
const char * name() const override
const char * name4() const override
bool allows_arming(bool from_gcs) const override
const char * name() const override
bool is_autopilot() const override
bool is_autopilot() const override
void takeoff_get_climb_rates(float &pilot_climb_rate, float &takeoff_climb_rate)
const char * name() const override
virtual bool in_guided_mode() const
autopilot_yaw_mode mode() const
bool is_autopilot() const override
const char * name() const override
bool allows_arming(bool from_gcs) const override
const char * name4() const override
float & ekfNavVelGainScaler
void zero_throttle_and_relax_ac()
bool takeoff_triggered(float target_climb_rate) const
uint16_t get_pilot_speed_dn(void)
struct _USB_OTG_GOTGCTL_TypeDef::@51 b
const char * name() const override
Vector2f last_flow_rate_rps
float descend_throttle_level
void set_throttle_takeoff(void)
const AP_Param::Info var_info[]
int16_t _fixed_yaw_slewrate
bool is_autopilot() const override
bool in_guided_mode() const
bool allows_arming(bool from_gcs) const override
const char * name() const override
RC_Channel *& channel_pitch
virtual bool is_autopilot() const
const char * name() const override
bool requires_GPS() const override
bool allows_arming(bool from_gcs) const override
const char * name() const override
uint32_t descend_start_timestamp
const char * name() const override
Vector3f flip_orig_attitude
const char * name4() const override
bool is_autopilot() const override
const char * name4() const override
Location_Class return_target
bool has_manual_throttle() const override
void takeoff_timer_start(float alt_cm)
bool requires_GPS() const override
bool allows_arming(bool from_gcs) const override
const char * name4() const override
virtual int32_t wp_bearing() const
Location_Class descent_target
bool is_autopilot() const override
float descend_start_altitude
bool has_manual_throttle() const override
int32_t nav_delay_time_max
void set_precision_loiter_enabled(bool value)
uint32_t hover_start_timestamp
bool allows_arming(bool from_gcs) const override
const char * name4() const override
bool allows_arming(bool from_gcs) const override
const char * name4() const override
bool requires_GPS() const override
const char * name() const override
bool is_autopilot() const override
bool allows_arming(bool from_gcs) const override
uint32_t free_fall_start_ms
bool has_manual_throttle() const override
void set_mode_to_default(bool rtl)
const char * name() const override
bool allows_arming(bool from_gcs) const override
const char * name4() const override
bool has_manual_throttle() const override
AP_InertialNav & inertial_nav
const char * name4() const override
bool allows_arming(bool from_gcs) const override
bool has_manual_throttle() const override
bool allows_arming(bool from_gcs) const override
bool allows_arming(bool from_gcs) const override
virtual const char * name4() const =0
bool has_manual_throttle() const override
bool is_autopilot() const override
virtual bool landing_gear_should_be_deployed() const
uint32_t last_stick_input_ms
bool requires_GPS() const override
bool has_manual_throttle() const override
bool is_autopilot() const override
float hover_throttle_level
#define AC_AttitudeControl_t
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
bool requires_GPS() const override
bool has_manual_throttle() const override
bool has_manual_throttle() const override
const char * name() const override
bool is_autopilot() const override
bool has_manual_throttle() const override
bool requires_GPS() const override
bool has_manual_throttle() const override
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
float get_non_takeoff_throttle(void)
const char * name() const override
bool requires_GPS() const override
virtual bool init(bool ignore_checks)=0
bool has_manual_throttle() const override
bool requires_GPS() const override
virtual const char * name() const =0
int32_t get_alt_above_ground(void)
const char * name4() const override
void update_simple_mode(void)
bool allows_arming(bool from_gcs) const override
bool requires_GPS() const override
bool in_guided_mode() const
virtual bool requires_GPS() const =0
const char * name4() const override
bool landing_gear_should_be_deployed() const override
float get_avoidance_adjusted_climbrate(float target_rate)
void set_descent_target_alt(uint32_t alt)
bool allows_arming(bool from_gcs) const override
const char * name() const override
bool requires_GPS() const override
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid=0.0f)
const char * name4() const override
bool has_manual_throttle() const override
Location_Class climb_target
uint32_t nav_delay_time_start
void land_run_horizontal_control()
bool allows_arming(bool from_gcs) const override
RC_Channel *& channel_roll
heli_flags_t & heli_flags
bool allows_arming(bool from_gcs) const override
bool has_manual_throttle() const override
float free_fall_start_velz
autopilot_yaw_mode default_mode(bool rtl) const
bool is_autopilot() const override
float get_pilot_desired_yaw_rate(int16_t stick_angle)
virtual bool allows_arming(bool from_gcs) const =0
RC_Channel *& channel_yaw
void set_roi(const Location &roi_location)
bool is_autopilot() const override
const char * name4() const override
takeoff_state_t & takeoff_state
bool _precision_loiter_enabled
bool requires_GPS() const override
virtual bool get_wp(Location_Class &loc)
bool requires_GPS() const override
AC_AttitudeControl_t *& attitude_control
const char * name4() const override
void set_land_complete(bool b)
bool allows_arming(bool from_gcs) const override
bool requires_GPS() const override
bool requires_GPS() const override
bool requires_GPS() const override
bool is_autopilot() const override
bool has_manual_throttle() const override
bool has_manual_throttle() const override
bool allows_arming(bool from_gcs) const override
const char * name4() const override
virtual bool has_manual_throttle() const =0
bool requires_GPS() const override
bool has_manual_throttle() const override
bool requires_GPS() const override
LowPassFilterVector2f flow_filter
bool is_autopilot() const override
bool is_autopilot() const override
bool is_autopilot() const override
const char * name() const override
virtual void run_autopilot()
bool is_autopilot() const override
void set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
virtual uint32_t wp_distance() const
uint32_t place_start_timestamp
const char * name4() const override
const char * name() const override
void set_rate(float new_rate_cds)
const char * name4() const override
const char * name4() const override
bool is_autopilot() const override
bool requires_GPS() const override
bool requires_GPS() const override
const char * name() const override
void set_mode(autopilot_yaw_mode new_mode)
bool allows_arming(bool from_gcs) const override
void Log_Write_Event(uint8_t id)
const char * name4() const override
bool is_autopilot() const override
const char * name4() const override
const char * name() const override
float get_pilot_desired_climb_rate(float throttle_control)