2016-03-21 01:33:42 -03:00
# pragma once
2019-05-09 23:18:49 -03:00
# include "Copter.h"
2022-03-14 23:53:57 -03:00
# include <AP_Math/chirp.h>
2023-08-08 21:28:58 -03:00
# include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
2024-05-13 13:16:57 -03:00
2019-09-07 20:33:56 -03:00
class Parameters ;
class ParametersG2 ;
class GCS_Copter ;
2023-09-28 02:05:51 -03:00
// object shared by both Guided and Auto for takeoff.
// position controller controls vehicle but the user can control the yaw.
class _AutoTakeoff {
public :
void run ( ) ;
void start ( float complete_alt_cm , bool terrain_alt ) ;
2024-09-10 09:12:31 -03:00
bool get_completion_pos ( Vector3p & pos_neu_cm ) ;
2023-09-28 02:05:51 -03:00
bool complete ; // true when takeoff is complete
private :
// altitude above-ekf-origin below which auto takeoff does not control horizontal position
bool no_nav_active ;
float no_nav_alt_cm ;
// auto takeoff variables
float complete_alt_cm ; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt)
bool terrain_alt ; // true if altitudes are above terrain
Vector3p complete_pos ; // target takeoff position as offset from ekf origin in cm
} ;
2023-10-18 23:34:30 -03:00
# if AC_PAYLOAD_PLACE_ENABLED
2023-09-27 22:24:36 -03:00
class PayloadPlace {
public :
void run ( ) ;
void start_descent ( ) ;
bool verify ( ) ;
enum class State : uint8_t {
FlyToLocation ,
Descent_Start ,
Descent ,
Release ,
Releasing ,
Delay ,
Ascent_Start ,
Ascent ,
Done ,
} ;
// these are set by the Mission code:
State state = State : : Descent_Start ; // records state of payload place
float descent_max_cm ;
private :
uint32_t descent_established_time_ms ; // milliseconds
uint32_t place_start_time_ms ; // milliseconds
float descent_thrust_level ;
float descent_start_altitude_cm ;
float descent_speed_cms ;
} ;
2023-10-18 23:34:30 -03:00
# endif
2023-09-27 22:24:36 -03:00
2017-12-10 23:51:13 -04:00
class Mode {
2023-09-27 22:24:36 -03:00
friend class PayloadPlace ;
2019-04-18 19:49:09 -03:00
public :
2016-03-21 01:33:42 -03:00
2019-09-07 20:33:56 -03:00
// Auto Pilot Modes enumeration
2019-10-17 00:49:22 -03:00
enum class Number : uint8_t {
2019-09-07 20:33:56 -03:00
STABILIZE = 0 , // manual airframe angle with manual throttle
ACRO = 1 , // manual body-frame angular rate with manual throttle
ALT_HOLD = 2 , // manual airframe angle with automatic throttle
AUTO = 3 , // fully automatic waypoint control using mission commands
GUIDED = 4 , // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
LOITER = 5 , // automatic horizontal acceleration with automatic throttle
RTL = 6 , // automatic return to launching point
CIRCLE = 7 , // automatic circular flight with automatic throttle
LAND = 9 , // automatic landing with horizontal position control
2021-01-06 11:19:23 -04:00
DRIFT = 11 , // semi-autonomous position, yaw and throttle control
2019-09-07 20:33:56 -03:00
SPORT = 13 , // manual earth-frame angular rate control with manual throttle
FLIP = 14 , // automatically flip the vehicle on the roll axis
AUTOTUNE = 15 , // automatically tune the vehicle's roll and pitch gains
POSHOLD = 16 , // automatic position hold with manual override, with automatic throttle
BRAKE = 17 , // full-brake using inertial/GPS system, no pilot input
THROW = 18 , // throw to launch mode using inertial/GPS system, no pilot input
AVOID_ADSB = 19 , // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
GUIDED_NOGPS = 20 , // guided mode but only accepts attitude and altitude
SMART_RTL = 21 , // SMART_RTL returns to home by retracing its steps
FLOWHOLD = 22 , // FLOWHOLD holds position with optical flow without rangefinder
FOLLOW = 23 , // follow attempts to follow another vehicle or ground station
ZIGZAG = 24 , // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
2019-07-29 04:55:40 -03:00
SYSTEMID = 25 , // System ID mode produces automated system identification signals in the controllers
2019-11-28 16:21:07 -04:00
AUTOROTATE = 26 , // Autonomous autorotation
2021-07-24 20:25:18 -03:00
AUTO_RTL = 27 , // Auto RTL, this is not a true mode, AUTO will report as this mode if entered to perform a DO_LAND_START Landing sequence
2021-07-27 16:51:19 -03:00
TURTLE = 28 , // Flip over after crash
2022-08-02 06:15:50 -03:00
// Mode number 127 reserved for the "drone show mode" in the Skybrush
// fork at https://github.com/skybrush-io/ardupilot
2019-09-07 20:33:56 -03:00
} ;
2018-02-07 22:21:09 -04:00
// constructor
Mode ( void ) ;
2017-12-25 23:55:42 -04:00
2019-01-02 23:07:59 -04:00
// do not allow copying
2022-09-30 06:50:43 -03:00
CLASS_NO_COPY ( Mode ) ;
2019-01-02 23:07:59 -04:00
2023-09-28 02:05:51 -03:00
friend class _AutoTakeoff ;
2020-01-30 02:10:15 -04:00
// returns a unique number specific to this mode
virtual Number mode_number ( ) const = 0 ;
2019-04-18 20:41:30 -03:00
// child classes should override these methods
2019-04-18 19:49:09 -03:00
virtual bool init ( bool ignore_checks ) {
return true ;
}
2021-04-20 09:36:36 -03:00
virtual void exit ( ) { } ;
2019-04-18 19:49:09 -03:00
virtual void run ( ) = 0 ;
2019-04-18 20:41:30 -03:00
virtual bool requires_GPS ( ) const = 0 ;
virtual bool has_manual_throttle ( ) const = 0 ;
2020-12-31 20:49:01 -04:00
virtual bool allows_arming ( AP_Arming : : Method method ) const = 0 ;
2019-04-18 20:41:30 -03:00
virtual bool is_autopilot ( ) const { return false ; }
virtual bool has_user_takeoff ( bool must_navigate ) const { return false ; }
virtual bool in_guided_mode ( ) const { return false ; }
2019-10-15 02:38:24 -03:00
virtual bool logs_attitude ( ) const { return false ; }
2021-02-07 00:58:50 -04:00
virtual bool allows_save_trim ( ) const { return false ; }
2020-02-10 22:06:57 -04:00
virtual bool allows_autotune ( ) const { return false ; }
2020-02-10 22:17:19 -04:00
virtual bool allows_flip ( ) const { return false ; }
2024-07-25 02:12:05 -03:00
virtual bool crash_check_enabled ( ) const { return true ; }
2019-04-18 19:49:09 -03:00
2024-02-29 19:31:38 -04:00
# if FRAME_CONFIG == HELI_FRAME
virtual bool allows_inverted ( ) const { return false ; } ;
# endif
2019-04-18 20:41:30 -03:00
// return a string for this flightmode
virtual const char * name ( ) const = 0 ;
virtual const char * name4 ( ) const = 0 ;
2017-12-25 23:55:42 -04:00
2017-12-12 05:45:40 -04:00
bool do_user_takeoff ( float takeoff_alt_cm , bool must_navigate ) ;
2019-04-18 19:49:09 -03:00
virtual bool is_taking_off ( ) const ;
static void takeoff_stop ( ) { takeoff . stop ( ) ; }
2017-12-12 05:45:40 -04:00
2019-04-18 19:49:09 -03:00
virtual bool is_landing ( ) const { return false ; }
2016-03-21 01:33:42 -03:00
2019-09-08 20:08:02 -03:00
// mode requires terrain to be present to be functional
virtual bool requires_terrain_failsafe ( ) const { return false ; }
2019-06-05 07:47:32 -03:00
// functions for reporting to GCS
2021-07-06 18:02:26 -03:00
virtual bool get_wp ( Location & loc ) const { return false ; } ;
2019-04-18 19:49:09 -03:00
virtual int32_t wp_bearing ( ) const { return 0 ; }
virtual uint32_t wp_distance ( ) const { return 0 ; }
virtual float crosstrack_error ( ) const { return 0.0f ; }
2019-06-05 07:47:32 -03:00
2022-07-27 06:20:29 -03:00
// functions to support MAV_CMD_DO_CHANGE_SPEED
virtual bool set_speed_xy ( float speed_xy_cms ) { return false ; }
virtual bool set_speed_up ( float speed_xy_cms ) { return false ; }
virtual bool set_speed_down ( float speed_xy_cms ) { return false ; }
2019-04-18 19:49:09 -03:00
int32_t get_alt_above_ground_cm ( void ) ;
2016-03-21 01:33:42 -03:00
2019-04-18 19:49:09 -03:00
// pilot input processing
2022-03-23 18:55:13 -03:00
void get_pilot_desired_lean_angles ( float & roll_out_cd , float & pitch_out_cd , float angle_max_cd , float angle_limit_cd ) const ;
2022-02-01 23:40:27 -04:00
Vector2f get_pilot_desired_velocity ( float vel_max ) const ;
2021-09-17 02:54:19 -03:00
float get_pilot_desired_yaw_rate ( float yaw_in ) ;
2019-04-18 19:49:09 -03:00
float get_pilot_desired_throttle ( ) const ;
2016-05-01 00:29:31 -03:00
2020-06-16 03:50:37 -03:00
// returns climb target_rate reduced to avoid obstacles and
// altitude fence
float get_avoidance_adjusted_climbrate ( float target_rate ) ;
2021-05-11 01:42:02 -03:00
const Vector3f & get_vel_desired_cms ( ) {
2019-04-18 19:49:09 -03:00
// note that position control isn't used in every mode, so
// this may return bogus data:
2021-05-11 01:42:02 -03:00
return pos_control - > get_vel_desired_cms ( ) ;
2019-04-18 19:49:09 -03:00
}
2021-08-14 06:12:18 -03:00
// send output to the motors, can be overridden by subclasses
virtual void output_to_motors ( ) ;
2021-09-03 03:22:25 -03:00
// returns true if pilot's yaw input should be used to adjust vehicle's heading
virtual bool use_pilot_yaw ( ) const { return true ; }
2022-01-27 14:36:21 -04:00
// pause and resume a mode
virtual bool pause ( ) { return false ; } ;
virtual bool resume ( ) { return false ; } ;
2024-07-02 00:07:58 -03:00
// handle situations where the vehicle is on the ground waiting for takeoff
2024-06-29 19:58:57 -03:00
void make_safe_ground_handling ( bool force_throttle_unlimited = false ) ;
2022-01-27 14:36:21 -04:00
2022-04-14 06:58:27 -03:00
// true if weathervaning is allowed in the current mode
2024-09-04 00:48:37 -03:00
# if WEATHERVANE_ENABLED
2022-04-14 06:58:27 -03:00
virtual bool allows_weathervaning ( ) const { return false ; }
# endif
2019-04-18 19:49:09 -03:00
protected :
2018-03-13 21:23:30 -03:00
// helper functions
2019-04-08 05:15:57 -03:00
bool is_disarmed_or_landed ( ) const ;
2019-01-08 06:43:51 -04:00
void zero_throttle_and_relax_ac ( bool spool_up = false ) ;
2019-02-28 05:03:23 -04:00
void zero_throttle_and_hold_attitude ( ) ;
2018-03-13 21:23:30 -03:00
2024-03-27 14:56:58 -03:00
// Return stopping point as a location with above origin alt frame
Location get_stopping_point ( ) const ;
2021-08-25 02:53:56 -03:00
// functions to control normal landing. pause_descent is true if vehicle should not descend
2018-03-19 14:26:35 -03:00
void land_run_horizontal_control ( ) ;
void land_run_vertical_control ( bool pause_descent = false ) ;
2021-08-25 02:53:56 -03:00
void land_run_horiz_and_vert_control ( bool pause_descent = false ) {
2021-07-21 16:33:37 -03:00
land_run_horizontal_control ( ) ;
land_run_vertical_control ( pause_descent ) ;
}
2021-08-25 02:53:56 -03:00
2023-10-18 23:34:30 -03:00
# if AC_PAYLOAD_PLACE_ENABLED
2023-09-27 22:24:36 -03:00
// payload place flight behaviour:
static PayloadPlace payload_place ;
2023-10-18 23:34:30 -03:00
# endif
2023-09-27 22:24:36 -03:00
2021-08-25 02:53:56 -03:00
// run normal or precision landing (if enabled)
// pause_descent is true if vehicle should not descend
void land_run_normal_or_precland ( bool pause_descent = false ) ;
2023-03-22 05:45:41 -03:00
# if AC_PRECLAND_ENABLED
2021-08-25 02:53:56 -03:00
// Go towards a position commanded by prec land state machine in order to retry landing
// The passed in location is expected to be NED and in meters
void precland_retry_position ( const Vector3f & retry_pos ) ;
// Run precland statemachine. This function should be called from any mode that wants to do precision landing.
// This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
void precland_run ( ) ;
# endif
2018-03-19 14:26:35 -03:00
2019-03-06 02:36:32 -04:00
// return expected input throttle setting to hover:
virtual float throttle_hover ( ) const ;
2019-02-28 05:03:23 -04:00
// Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
2024-04-16 18:43:28 -03:00
enum class AltHoldModeState {
MotorStopped ,
Takeoff ,
Landed_Ground_Idle ,
Landed_Pre_Takeoff ,
Flying
2019-02-28 05:03:23 -04:00
} ;
AltHoldModeState get_alt_hold_state ( float target_climb_rate_cms ) ;
2016-03-21 01:33:42 -03:00
// convenience references to avoid code churn in conversion:
Parameters & g ;
ParametersG2 & g2 ;
AC_WPNav * & wp_nav ;
2018-03-27 23:13:37 -03:00
AC_Loiter * & loiter_nav ;
2016-03-21 01:33:42 -03:00
AC_PosControl * & pos_control ;
AP_InertialNav & inertial_nav ;
AP_AHRS & ahrs ;
2023-07-19 10:07:31 -03:00
AC_AttitudeControl * & attitude_control ;
2016-03-21 01:33:42 -03:00
MOTOR_CLASS * & motors ;
RC_Channel * & channel_roll ;
RC_Channel * & channel_pitch ;
RC_Channel * & channel_throttle ;
RC_Channel * & channel_yaw ;
float & G_Dt ;
2017-12-12 06:34:49 -04:00
2018-06-26 00:31:40 -03:00
// note that we support two entirely different automatic takeoffs:
// "user-takeoff", which is available in modes such as ALT_HOLD
// (see has_user_takeoff method). "user-takeoff" is a simple
// reach-altitude-based-on-pilot-input-or-parameter routine.
// "auto-takeoff" is used by both Guided and Auto, and is
// basically waypoint navigation with pilot yaw permitted.
// user-takeoff support; takeoff state is shared across all mode instances
2018-03-16 03:22:14 -03:00
class _TakeOff {
public :
void start ( float alt_cm ) ;
void stop ( ) ;
2021-05-13 02:40:05 -03:00
void do_pilot_takeoff ( float & pilot_climb_rate ) ;
2018-03-16 03:22:14 -03:00
bool triggered ( float target_climb_rate ) const ;
bool running ( ) const { return _running ; }
private :
bool _running ;
2021-05-13 02:40:05 -03:00
float take_off_start_alt ;
2022-08-24 09:02:44 -03:00
float take_off_complete_alt ;
2018-03-16 03:22:14 -03:00
} ;
2017-12-12 06:34:49 -04:00
2018-03-16 03:22:14 -03:00
static _TakeOff takeoff ;
2017-12-12 06:34:49 -04:00
2017-12-12 06:09:48 -04:00
virtual bool do_user_takeoff_start ( float takeoff_alt_cm ) ;
2023-09-28 02:05:51 -03:00
static _AutoTakeoff auto_takeoff ;
2022-02-10 23:05:43 -04:00
2019-04-18 20:41:30 -03:00
public :
// Navigation Yaw control
class AutoYaw {
public :
2022-09-24 11:12:33 -03:00
// Autopilot Yaw Mode enumeration
enum class Mode {
HOLD = 0 , // hold zero yaw rate
LOOK_AT_NEXT_WP = 1 , // point towards next waypoint (no pilot input accepted)
ROI = 2 , // point towards a location held in roi (no pilot input accepted)
FIXED = 3 , // point towards a particular angle (no pilot input accepted)
LOOK_AHEAD = 4 , // point in the direction the copter is moving
RESETTOARMEDYAW = 5 , // point towards heading at time motors were armed
ANGLE_RATE = 6 , // turn at a specified rate from a starting angle
RATE = 7 , // turn at a specified rate (held in auto_yaw_rate)
CIRCLE = 8 , // use AC_Circle's provided yaw (used during Loiter-Turns commands)
PILOT_RATE = 9 , // target rate from pilot stick
2022-04-14 06:58:27 -03:00
WEATHERVANE = 10 , // yaw into wind
2022-09-24 11:12:33 -03:00
} ;
2019-04-18 20:41:30 -03:00
// mode(): current method of determining desired yaw:
2022-09-24 11:12:33 -03:00
Mode mode ( ) const { return _mode ; }
2019-04-18 20:41:30 -03:00
void set_mode_to_default ( bool rtl ) ;
2022-09-24 11:12:33 -03:00
void set_mode ( Mode new_mode ) ;
Mode default_mode ( bool rtl ) const ;
2019-04-18 20:41:30 -03:00
void set_rate ( float new_rate_cds ) ;
// set_roi(...): set a "look at" location:
void set_roi ( const Location & roi_location ) ;
void set_fixed_yaw ( float angle_deg ,
float turn_rate_dps ,
int8_t direction ,
bool relative_angle ) ;
2021-07-09 05:44:07 -03:00
void set_yaw_angle_rate ( float yaw_angle_d , float yaw_rate_ds ) ;
2022-09-24 11:12:33 -03:00
bool reached_fixed_yaw_target ( ) ;
2024-09-04 00:48:37 -03:00
# if WEATHERVANE_ENABLED
2022-04-14 06:58:27 -03:00
void update_weathervane ( const int16_t pilot_yaw_cds ) ;
# endif
2022-09-24 11:12:33 -03:00
AC_AttitudeControl : : HeadingCommand get_heading ( ) ;
2022-02-15 13:40:51 -04:00
2019-04-18 20:41:30 -03:00
private :
2023-03-04 07:02:41 -04:00
// yaw_cd(): main product of AutoYaw; the heading:
float yaw_cd ( ) ;
2022-09-24 11:12:33 -03:00
// rate_cds(): desired yaw rate in centidegrees/second:
2023-03-04 07:02:41 -04:00
float rate_cds ( ) ;
2022-09-24 11:12:33 -03:00
2024-04-09 03:48:42 -03:00
// returns a yaw in degrees, direction of vehicle travel:
2019-04-18 20:41:30 -03:00
float look_ahead_yaw ( ) ;
2024-04-09 03:48:42 -03:00
2021-02-05 05:55:45 -04:00
float roi_yaw ( ) const ;
2019-04-18 20:41:30 -03:00
// auto flight mode's yaw mode
2022-09-24 11:12:33 -03:00
Mode _mode = Mode : : LOOK_AT_NEXT_WP ;
2022-04-14 06:58:27 -03:00
Mode _last_mode ;
2019-04-18 20:41:30 -03:00
2022-09-24 11:12:33 -03:00
// Yaw will point at this location if mode is set to Mode::ROI
2019-04-18 20:41:30 -03:00
Vector3f roi ;
// yaw used for YAW_FIXED yaw_mode
2021-07-09 05:44:07 -03:00
float _fixed_yaw_offset_cd ;
2019-04-18 20:41:30 -03:00
// Deg/s we should turn
2021-07-09 05:44:07 -03:00
float _fixed_yaw_slewrate_cds ;
2021-07-09 22:25:52 -03:00
// time of the last yaw update
uint32_t _last_update_ms ;
2019-04-18 20:41:30 -03:00
// heading when in yaw_look_ahead_yaw
float _look_ahead_yaw ;
// turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
2021-07-09 05:44:07 -03:00
float _yaw_angle_cd ;
float _yaw_rate_cds ;
2022-09-24 11:12:33 -03:00
float _pilot_yaw_rate_cds ;
2019-04-18 20:41:30 -03:00
} ;
static AutoYaw auto_yaw ;
2016-03-21 01:33:42 -03:00
// pass-through functions to reduce code churn on conversion;
2017-12-10 23:51:13 -04:00
// these are candidates for moving into the Mode base
2016-03-21 01:33:42 -03:00
// class.
2018-02-07 22:21:09 -04:00
float get_pilot_desired_climb_rate ( float throttle_control ) ;
float get_non_takeoff_throttle ( void ) ;
void update_simple_mode ( void ) ;
2019-10-17 00:49:22 -03:00
bool set_mode ( Mode : : Number mode , ModeReason reason ) ;
2018-02-07 22:21:09 -04:00
void set_land_complete ( bool b ) ;
GCS_Copter & gcs ( ) ;
uint16_t get_pilot_speed_dn ( void ) ;
2016-03-21 01:33:42 -03:00
// end pass-through functions
} ;
2016-03-21 01:20:54 -03:00
2024-09-04 00:48:37 -03:00
# if MODE_ACRO_ENABLED
2017-12-10 23:51:13 -04:00
class ModeAcro : public Mode {
2016-03-21 01:20:54 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : ACRO ; }
2018-02-08 18:50:59 -04:00
2019-10-25 03:06:05 -03:00
enum class Trainer {
OFF = 0 ,
LEVELING = 1 ,
LIMITED = 2 ,
} ;
2020-06-16 17:01:19 -03:00
enum class AcroOptions {
AIR_MODE = 1 < < 0 ,
RATE_LOOP_ONLY = 1 < < 1 ,
} ;
2017-11-09 22:54:34 -04:00
virtual void run ( ) override ;
2016-03-21 01:20:54 -03:00
2018-02-08 18:50:59 -04:00
bool requires_GPS ( ) const override { return false ; }
bool has_manual_throttle ( ) const override { return true ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return true ; } ;
2019-04-18 20:41:30 -03:00
bool is_autopilot ( ) const override { return false ; }
2020-06-16 17:01:19 -03:00
bool init ( bool ignore_checks ) override ;
2021-04-20 09:36:36 -03:00
void exit ( ) override ;
2020-06-16 17:01:19 -03:00
// whether an air-mode aux switch has been toggled
void air_mode_aux_changed ( ) ;
2021-02-07 00:58:50 -04:00
bool allows_save_trim ( ) const override { return true ; }
2020-02-10 22:17:19 -04:00
bool allows_flip ( ) const override { return true ; }
2024-07-25 02:12:05 -03:00
bool crash_check_enabled ( ) const override { return false ; }
2016-03-21 01:20:54 -03:00
protected :
const char * name ( ) const override { return " ACRO " ; }
const char * name4 ( ) const override { return " ACRO " ; }
2021-09-17 02:54:19 -03:00
// get_pilot_desired_angle_rates - transform pilot's normalised roll pitch and yaw input into a desired lean angle rates
// inputs are -1 to 1 and the function returns desired angle rates in centi-degrees-per-second
void get_pilot_desired_angle_rates ( float roll_in , float pitch_in , float yaw_in , float & roll_out , float & pitch_out , float & yaw_out ) ;
2016-03-21 01:20:54 -03:00
2019-09-07 20:33:56 -03:00
float throttle_hover ( ) const override ;
2019-03-06 02:36:32 -04:00
2016-03-21 01:20:54 -03:00
private :
2020-06-16 17:01:19 -03:00
bool disable_air_mode_reset ;
2016-03-21 01:20:54 -03:00
} ;
2018-03-14 17:14:49 -03:00
# endif
2016-03-21 01:20:54 -03:00
# if FRAME_CONFIG == HELI_FRAME
2017-12-10 23:51:13 -04:00
class ModeAcro_Heli : public ModeAcro {
2016-03-21 01:20:54 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using ModeAcro : : Mode ;
2016-03-21 01:20:54 -03:00
bool init ( bool ignore_checks ) override ;
2017-11-09 22:54:34 -04:00
void run ( ) override ;
2019-03-18 13:08:47 -03:00
void virtual_flybar ( float & roll_out , float & pitch_out , float & yaw_out , float pitch_leak , float roll_leak ) ;
2016-03-21 01:20:54 -03:00
protected :
private :
} ;
# endif
2016-03-21 01:23:41 -03:00
2017-12-10 23:51:13 -04:00
class ModeAltHold : public Mode {
2016-03-21 01:23:41 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : ALT_HOLD ; }
2016-03-21 01:23:41 -03:00
bool init ( bool ignore_checks ) override ;
2017-11-09 22:54:34 -04:00
void run ( ) override ;
2016-03-21 01:23:41 -03:00
bool requires_GPS ( ) const override { return false ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return true ; } ;
2016-03-21 01:23:41 -03:00
bool is_autopilot ( ) const override { return false ; }
2016-05-01 00:29:31 -03:00
bool has_user_takeoff ( bool must_navigate ) const override {
return ! must_navigate ;
}
2020-02-10 22:06:57 -04:00
bool allows_autotune ( ) const override { return true ; }
2020-02-10 22:17:19 -04:00
bool allows_flip ( ) const override { return true ; }
2024-03-06 00:18:38 -04:00
# if FRAME_CONFIG == HELI_FRAME
bool allows_inverted ( ) const override { return true ; } ;
# endif
2016-03-21 01:23:41 -03:00
protected :
const char * name ( ) const override { return " ALT_HOLD " ; }
const char * name4 ( ) const override { return " ALTH " ; }
private :
} ;
2017-01-14 01:23:33 -04:00
2017-12-10 23:51:13 -04:00
class ModeAuto : public Mode {
2016-03-21 00:45:50 -03:00
public :
2023-09-27 22:24:36 -03:00
friend class PayloadPlace ; // in case wp_run is accidentally required
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2021-07-24 20:25:18 -03:00
Number mode_number ( ) const override { return auto_RTL ? Number : : AUTO_RTL : Number : : AUTO ; }
2016-03-21 00:45:50 -03:00
2018-02-08 18:50:59 -04:00
bool init ( bool ignore_checks ) override ;
2021-04-20 09:36:36 -03:00
void exit ( ) override ;
2018-02-08 18:50:59 -04:00
void run ( ) override ;
2016-03-21 00:45:50 -03:00
2022-05-25 09:26:00 -03:00
bool requires_GPS ( ) const override ;
2018-02-08 18:50:59 -04:00
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override ;
2019-04-18 20:41:30 -03:00
bool is_autopilot ( ) const override { return true ; }
2022-05-25 09:23:06 -03:00
bool in_guided_mode ( ) const override { return _mode = = SubMode : : NAVGUIDED | | _mode = = SubMode : : NAV_SCRIPT_TIME ; }
2024-03-06 00:18:38 -04:00
# if FRAME_CONFIG == HELI_FRAME
bool allows_inverted ( ) const override { return true ; } ;
# endif
2016-03-21 00:45:50 -03:00
2021-01-26 07:37:02 -04:00
// Auto modes
2021-04-13 12:06:43 -03:00
enum class SubMode : uint8_t {
2021-01-26 07:37:02 -04:00
TAKEOFF ,
WP ,
LAND ,
RTL ,
CIRCLE_MOVE_TO_EDGE ,
CIRCLE ,
NAVGUIDED ,
LOITER ,
LOITER_TO_ALT ,
2023-10-28 03:06:45 -03:00
# if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED
2021-01-26 07:37:02 -04:00
NAV_PAYLOAD_PLACE ,
2023-10-18 23:34:30 -03:00
# endif
2022-02-17 08:59:16 -04:00
NAV_SCRIPT_TIME ,
2022-05-25 02:20:27 -03:00
NAV_ATTITUDE_TIME ,
2021-01-26 07:37:02 -04:00
} ;
2022-05-25 09:26:00 -03:00
// set submode. returns true on success, false on failure
void set_submode ( SubMode new_submode ) ;
2016-03-21 00:45:50 -03:00
2022-01-27 14:36:21 -04:00
// pause continue in auto mode
bool pause ( ) override ;
bool resume ( ) override ;
2023-04-19 03:53:38 -03:00
bool paused ( ) const ;
2022-01-27 14:36:21 -04:00
2016-03-21 00:45:50 -03:00
bool loiter_start ( ) ;
void rtl_start ( ) ;
void takeoff_start ( const Location & dest_loc ) ;
2022-12-23 03:34:20 -04:00
bool wp_start ( const Location & dest_loc ) ;
2016-03-21 00:45:50 -03:00
void land_start ( ) ;
2023-02-14 18:17:31 -04:00
void circle_movetoedge_start ( const Location & circle_center , float radius_m , bool ccw_turn ) ;
2016-03-21 00:45:50 -03:00
void circle_start ( ) ;
void nav_guided_start ( ) ;
2018-04-30 06:50:04 -03:00
bool is_landing ( ) const override ;
2016-03-21 00:45:50 -03:00
2018-04-30 06:50:04 -03:00
bool is_taking_off ( ) const override ;
2021-09-03 03:22:25 -03:00
bool use_pilot_yaw ( ) const override ;
2018-04-30 06:50:04 -03:00
2022-07-27 06:20:29 -03:00
bool set_speed_xy ( float speed_xy_cms ) override ;
bool set_speed_up ( float speed_up_cms ) override ;
bool set_speed_down ( float speed_down_cms ) override ;
2019-09-08 20:08:02 -03:00
bool requires_terrain_failsafe ( ) const override { return true ; }
2016-03-21 00:45:50 -03:00
void payload_place_start ( ) ;
2017-12-10 22:50:49 -04:00
// for GCS_MAVLink to call:
bool do_guided ( const AP_Mission : : Mission_Command & cmd ) ;
2021-07-24 20:25:18 -03:00
// Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
bool jump_to_landing_sequence_auto_RTL ( ModeReason reason ) ;
2024-03-02 20:12:23 -04:00
// Join mission after DO_RETURN_PATH_START waypoint, if succeeds pretend to be Auto RTL mode
bool return_path_start_auto_RTL ( ModeReason reason ) ;
// Try join return path else do land start
bool return_path_or_jump_to_landing_sequence_auto_RTL ( ModeReason reason ) ;
2022-02-17 08:59:16 -04:00
// lua accessors for nav script time support
2022-10-13 21:37:52 -03:00
bool nav_script_time ( uint16_t & id , uint8_t & cmd , float & arg1 , float & arg2 , int16_t & arg3 , int16_t & arg4 ) ;
2022-02-17 08:59:16 -04:00
void nav_script_time_done ( uint16_t id ) ;
2018-04-24 20:31:01 -03:00
AP_Mission mission {
2019-05-09 23:18:49 -03:00
FUNCTOR_BIND_MEMBER ( & ModeAuto : : start_command , bool , const AP_Mission : : Mission_Command & ) ,
FUNCTOR_BIND_MEMBER ( & ModeAuto : : verify_command , bool , const AP_Mission : : Mission_Command & ) ,
FUNCTOR_BIND_MEMBER ( & ModeAuto : : exit_mission , void ) } ;
2018-04-24 20:31:01 -03:00
2022-01-07 21:25:10 -04:00
// Mission change detector
AP_Mission_ChangeDetector mis_change_detector ;
2022-04-14 06:58:27 -03:00
// true if weathervaning is allowed in auto
2024-09-04 00:48:37 -03:00
# if WEATHERVANE_ENABLED
2022-04-14 06:58:27 -03:00
bool allows_weathervaning ( void ) const override ;
# endif
2016-03-21 00:45:50 -03:00
protected :
2021-07-24 20:25:18 -03:00
const char * name ( ) const override { return auto_RTL ? " AUTO RTL " : " AUTO " ; }
const char * name4 ( ) const override { return auto_RTL ? " ARTL " : " AUTO " ; }
2016-03-21 00:45:50 -03:00
2018-02-07 22:21:09 -04:00
uint32_t wp_distance ( ) const override ;
int32_t wp_bearing ( ) const override ;
2018-05-23 12:34:24 -03:00
float crosstrack_error ( ) const override { return wp_nav - > crosstrack_error ( ) ; }
2021-07-06 18:02:26 -03:00
bool get_wp ( Location & loc ) const override ;
2017-12-05 21:28:32 -04:00
2016-03-21 00:45:50 -03:00
private :
2024-07-16 21:33:25 -03:00
enum class Option : int32_t {
2020-09-30 04:16:16 -03:00
AllowArming = ( 1 < < 0U ) ,
AllowTakeOffWithoutRaisingThrottle = ( 1 < < 1U ) ,
2020-11-12 20:15:53 -04:00
IgnorePilotYaw = ( 1 < < 2U ) ,
2022-04-14 06:58:27 -03:00
AllowWeatherVaning = ( 1 < < 7U ) ,
2020-09-24 20:42:34 -03:00
} ;
2024-07-16 21:33:25 -03:00
bool option_is_enabled ( Option option ) const ;
2020-09-24 20:42:34 -03:00
2024-03-02 20:12:23 -04:00
// Enter auto rtl pseudo mode
bool enter_auto_rtl ( ModeReason reason ) ;
2018-04-24 20:31:01 -03:00
bool start_command ( const AP_Mission : : Mission_Command & cmd ) ;
2017-12-10 22:50:49 -04:00
bool verify_command ( const AP_Mission : : Mission_Command & cmd ) ;
2018-04-24 20:31:01 -03:00
void exit_mission ( ) ;
2017-12-10 22:50:49 -04:00
2021-03-04 04:19:05 -04:00
bool check_for_mission_change ( ) ; // detect external changes to mission
2016-03-21 00:45:50 -03:00
void takeoff_run ( ) ;
void wp_run ( ) ;
void land_run ( ) ;
void rtl_run ( ) ;
void circle_run ( ) ;
void nav_guided_run ( ) ;
void loiter_run ( ) ;
2018-07-25 00:39:43 -03:00
void loiter_to_alt_run ( ) ;
2022-05-25 02:20:27 -03:00
void nav_attitude_time_run ( ) ;
2016-03-21 00:45:50 -03:00
2021-01-19 22:50:48 -04:00
Location loc_from_cmd ( const AP_Mission : : Mission_Command & cmd , const Location & default_loc ) const ;
2018-10-03 00:33:03 -03:00
2021-04-13 12:06:43 -03:00
SubMode _mode = SubMode : : TAKEOFF ; // controls which auto controller is run
2016-03-21 00:45:50 -03:00
2022-06-15 00:46:55 -03:00
bool shift_alt_to_current_alt ( Location & target_loc ) const ;
2017-12-10 22:50:49 -04:00
void do_takeoff ( const AP_Mission : : Mission_Command & cmd ) ;
void do_nav_wp ( const AP_Mission : : Mission_Command & cmd ) ;
2021-01-19 22:50:48 -04:00
bool set_next_wp ( const AP_Mission : : Mission_Command & current_cmd , const Location & default_loc ) ;
2017-12-10 22:50:49 -04:00
void do_land ( const AP_Mission : : Mission_Command & cmd ) ;
void do_loiter_unlimited ( const AP_Mission : : Mission_Command & cmd ) ;
void do_circle ( const AP_Mission : : Mission_Command & cmd ) ;
void do_loiter_time ( const AP_Mission : : Mission_Command & cmd ) ;
2018-07-25 00:39:43 -03:00
void do_loiter_to_alt ( const AP_Mission : : Mission_Command & cmd ) ;
2017-12-10 22:50:49 -04:00
void do_spline_wp ( const AP_Mission : : Mission_Command & cmd ) ;
2021-01-19 22:50:48 -04:00
void get_spline_from_cmd ( const AP_Mission : : Mission_Command & cmd , const Location & default_loc , Location & dest_loc , Location & next_dest_loc , bool & next_dest_loc_is_spline ) ;
2024-09-04 00:48:37 -03:00
# if AC_NAV_GUIDED
2017-12-10 22:50:49 -04:00
void do_nav_guided_enable ( const AP_Mission : : Mission_Command & cmd ) ;
void do_guided_limits ( const AP_Mission : : Mission_Command & cmd ) ;
# endif
void do_nav_delay ( const AP_Mission : : Mission_Command & cmd ) ;
void do_wait_delay ( const AP_Mission : : Mission_Command & cmd ) ;
void do_within_distance ( const AP_Mission : : Mission_Command & cmd ) ;
void do_yaw ( const AP_Mission : : Mission_Command & cmd ) ;
void do_change_speed ( const AP_Mission : : Mission_Command & cmd ) ;
void do_set_home ( const AP_Mission : : Mission_Command & cmd ) ;
void do_roi ( const AP_Mission : : Mission_Command & cmd ) ;
void do_mount_control ( const AP_Mission : : Mission_Command & cmd ) ;
2024-08-07 00:05:01 -03:00
# if HAL_PARACHUTE_ENABLED
2017-12-10 22:50:49 -04:00
void do_parachute ( const AP_Mission : : Mission_Command & cmd ) ;
# endif
2023-03-03 00:13:14 -04:00
# if AP_WINCH_ENABLED
2017-12-10 22:50:49 -04:00
void do_winch ( const AP_Mission : : Mission_Command & cmd ) ;
2018-02-10 10:23:06 -04:00
# endif
2017-12-10 22:50:49 -04:00
void do_payload_place ( const AP_Mission : : Mission_Command & cmd ) ;
void do_RTL ( void ) ;
2022-02-17 08:59:16 -04:00
# if AP_SCRIPTING_ENABLED
void do_nav_script_time ( const AP_Mission : : Mission_Command & cmd ) ;
# endif
2022-05-25 02:20:27 -03:00
void do_nav_attitude_time ( const AP_Mission : : Mission_Command & cmd ) ;
2017-12-10 22:50:49 -04:00
bool verify_takeoff ( ) ;
bool verify_land ( ) ;
bool verify_payload_place ( ) ;
bool verify_loiter_unlimited ( ) ;
2019-02-25 16:38:38 -04:00
bool verify_loiter_time ( const AP_Mission : : Mission_Command & cmd ) ;
2021-02-01 12:26:21 -04:00
bool verify_loiter_to_alt ( ) const ;
2017-12-10 22:50:49 -04:00
bool verify_RTL ( ) ;
bool verify_wait_delay ( ) ;
bool verify_within_distance ( ) ;
bool verify_yaw ( ) ;
bool verify_nav_wp ( const AP_Mission : : Mission_Command & cmd ) ;
bool verify_circle ( const AP_Mission : : Mission_Command & cmd ) ;
bool verify_spline_wp ( const AP_Mission : : Mission_Command & cmd ) ;
2024-09-04 00:48:37 -03:00
# if AC_NAV_GUIDED
2017-12-10 22:50:49 -04:00
bool verify_nav_guided_enable ( const AP_Mission : : Mission_Command & cmd ) ;
# endif
bool verify_nav_delay ( const AP_Mission : : Mission_Command & cmd ) ;
2022-02-17 08:59:16 -04:00
# if AP_SCRIPTING_ENABLED
bool verify_nav_script_time ( ) ;
# endif
2022-05-25 02:20:27 -03:00
bool verify_nav_attitude_time ( const AP_Mission : : Mission_Command & cmd ) ;
2017-12-10 22:50:49 -04:00
// Loiter control
uint16_t loiter_time_max ; // How long we should stay in Loiter Mode for mission scripting (time in seconds)
uint32_t loiter_time ; // How long have we been loitering - The start time in millis
2018-07-25 00:39:43 -03:00
struct {
bool reached_destination_xy : 1 ;
bool loiter_start_done : 1 ;
bool reached_alt : 1 ;
float alt_error_cm ;
int32_t alt ;
} loiter_to_alt ;
2017-12-10 22:50:49 -04:00
// Delay the next navigation command
2019-08-08 11:10:20 -03:00
uint32_t nav_delay_time_max_ms ; // used for delaying the navigation commands (eg land,takeoff etc.)
uint32_t nav_delay_time_start_ms ;
2017-12-10 22:50:49 -04:00
// Delay Mission Scripting Command
int32_t condition_value ; // used in condition commands (eg delay, change alt, etc.)
uint32_t condition_start ;
2023-11-28 07:36:19 -04:00
// Land within Auto state
2019-12-01 19:25:32 -04:00
enum class State {
FlyToLocation = 0 ,
Descending = 1
} ;
State state = State : : FlyToLocation ;
2017-12-10 22:50:49 -04:00
2021-03-04 01:04:02 -04:00
bool waiting_to_start ; // true if waiting for vehicle to be armed or EKF origin before starting mission
2021-03-04 04:19:05 -04:00
2021-07-24 20:25:18 -03:00
// True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode
bool auto_RTL ;
2022-02-17 08:59:16 -04:00
# if AP_SCRIPTING_ENABLED
// nav_script_time command variables
struct {
bool done ; // true once lua script indicates it has completed
uint16_t id ; // unique id to avoid race conditions between commands and lua scripts
uint32_t start_ms ; // system time nav_script_time command was received (used for timeout)
uint8_t command ; // command number provided by mission command
uint8_t timeout_s ; // timeout (in seconds) provided by mission command
float arg1 ; // 1st argument provided by mission command
float arg2 ; // 2nd argument provided by mission command
2022-10-13 21:37:52 -03:00
int16_t arg3 ; // 3rd argument provided by mission command
int16_t arg4 ; // 4th argument provided by mission command
2022-02-17 08:59:16 -04:00
} nav_scripting ;
# endif
2022-05-25 02:20:27 -03:00
// nav attitude time command variables
struct {
int16_t roll_deg ; // target roll angle in degrees. provided by mission command
int8_t pitch_deg ; // target pitch angle in degrees. provided by mission command
int16_t yaw_deg ; // target yaw angle in degrees. provided by mission command
float climb_rate ; // climb rate in m/s. provided by mission command
2022-07-10 08:11:07 -03:00
uint32_t start_ms ; // system time that nav attitude time command was received (used for timeout)
2022-05-25 02:20:27 -03:00
} nav_attitude_time ;
2023-11-28 07:36:08 -04:00
// desired speeds
struct {
float xy ; // desired speed horizontally in m/s. 0 if unset
float up ; // desired speed upwards in m/s. 0 if unset
float down ; // desired speed downwards in m/s. 0 if unset
} desired_speed_override ;
2016-03-21 00:45:50 -03:00
} ;
2016-03-21 07:41:37 -03:00
2024-09-04 00:48:37 -03:00
# if AUTOTUNE_ENABLED
2018-12-12 23:53:56 -04:00
/*
wrapper class for AC_AutoTune
*/
2020-05-02 01:21:22 -03:00
# if FRAME_CONFIG == HELI_FRAME
class AutoTune : public AC_AutoTune_Heli
# else
class AutoTune : public AC_AutoTune_Multi
# endif
2018-12-12 23:53:56 -04:00
{
public :
bool init ( ) override ;
void run ( ) override ;
protected :
bool position_ok ( ) override ;
float get_pilot_desired_climb_rate_cms ( void ) const override ;
2019-02-28 07:10:05 -04:00
void get_pilot_desired_rp_yrate_cd ( float & roll_cd , float & pitch_cd , float & yaw_rate_cds ) override ;
2018-12-12 23:53:56 -04:00
void init_z_limits ( ) override ;
2023-07-13 21:58:09 -03:00
# if HAL_LOGGING_ENABLED
2018-12-21 22:26:45 -04:00
void log_pids ( ) override ;
2023-07-13 21:58:09 -03:00
# endif
2018-12-12 23:53:56 -04:00
} ;
2017-12-10 23:51:13 -04:00
class ModeAutoTune : public Mode {
2016-03-21 07:41:37 -03:00
2021-03-24 21:15:20 -03:00
// ParametersG2 sets a pointer within our autotune object:
friend class ParametersG2 ;
2016-03-21 07:41:37 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : AUTOTUNE ; }
2016-03-21 07:41:37 -03:00
bool init ( bool ignore_checks ) override ;
2021-04-20 09:36:36 -03:00
void exit ( ) override ;
2018-12-12 23:53:56 -04:00
void run ( ) override ;
2019-04-18 20:41:30 -03:00
2017-12-10 23:10:48 -04:00
bool requires_GPS ( ) const override { return false ; }
2016-03-21 07:41:37 -03:00
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; }
2017-12-10 23:10:48 -04:00
bool is_autopilot ( ) const override { return false ; }
2019-04-18 20:41:30 -03:00
2024-08-04 19:37:09 -03:00
AutoTune autotune ;
2016-03-21 07:41:37 -03:00
2017-12-10 23:10:48 -04:00
protected :
const char * name ( ) const override { return " AUTOTUNE " ; }
const char * name4 ( ) const override { return " ATUN " ; }
} ;
# endif
2017-12-10 23:51:13 -04:00
class ModeBrake : public Mode {
2016-03-21 21:42:21 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : BRAKE ; }
2016-03-21 21:42:21 -03:00
bool init ( bool ignore_checks ) override ;
2017-11-09 22:54:34 -04:00
void run ( ) override ;
2016-03-21 21:42:21 -03:00
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; } ;
2016-03-21 21:42:21 -03:00
bool is_autopilot ( ) const override { return false ; }
2017-12-10 23:10:48 -04:00
void timeout_to_loiter_ms ( uint32_t timeout_ms ) ;
2016-03-21 21:42:21 -03:00
protected :
2017-12-10 23:10:48 -04:00
const char * name ( ) const override { return " BRAKE " ; }
const char * name4 ( ) const override { return " BRAK " ; }
2016-03-21 21:42:21 -03:00
private :
2017-12-10 23:10:48 -04:00
uint32_t _timeout_start ;
uint32_t _timeout_ms ;
2016-03-21 21:42:21 -03:00
} ;
2016-03-21 23:13:34 -03:00
2017-12-10 23:51:13 -04:00
class ModeCircle : public Mode {
2016-03-21 23:13:34 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : CIRCLE ; }
2016-03-21 23:13:34 -03:00
bool init ( bool ignore_checks ) override ;
2017-11-09 22:54:34 -04:00
void run ( ) override ;
2016-03-21 23:13:34 -03:00
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; } ;
2016-03-21 23:13:34 -03:00
bool is_autopilot ( ) const override { return true ; }
2017-12-10 23:10:48 -04:00
protected :
2016-03-21 23:13:34 -03:00
2017-12-10 23:10:48 -04:00
const char * name ( ) const override { return " CIRCLE " ; }
const char * name4 ( ) const override { return " CIRC " ; }
2018-02-07 22:21:09 -04:00
uint32_t wp_distance ( ) const override ;
int32_t wp_bearing ( ) const override ;
2017-12-10 23:10:48 -04:00
private :
// Circle
2020-01-06 18:37:04 -04:00
bool speed_changing = false ; // true when the roll stick is being held to facilitate stopping at 0 rate
2017-12-10 23:10:48 -04:00
} ;
2017-12-10 23:51:13 -04:00
class ModeDrift : public Mode {
2017-12-10 23:10:48 -04:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : DRIFT ; }
2017-12-10 23:10:48 -04:00
2018-02-08 18:50:59 -04:00
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
2017-12-10 23:10:48 -04:00
2018-02-08 18:50:59 -04:00
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return true ; } ;
2018-02-08 18:50:59 -04:00
bool is_autopilot ( ) const override { return false ; }
2017-12-10 23:10:48 -04:00
protected :
const char * name ( ) const override { return " DRIFT " ; }
const char * name4 ( ) const override { return " DRIF " ; }
private :
float get_throttle_assist ( float velz , float pilot_throttle_scaled ) ;
} ;
2017-12-10 23:51:13 -04:00
class ModeFlip : public Mode {
2017-12-10 23:10:48 -04:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : FLIP ; }
2017-12-10 23:10:48 -04:00
2018-02-08 18:50:59 -04:00
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
2017-12-10 23:10:48 -04:00
2018-02-08 18:50:59 -04:00
bool requires_GPS ( ) const override { return false ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; } ;
2018-02-08 18:50:59 -04:00
bool is_autopilot ( ) const override { return false ; }
2024-07-25 02:12:05 -03:00
bool crash_check_enabled ( ) const override { return false ; }
2017-12-10 23:10:48 -04:00
protected :
const char * name ( ) const override { return " FLIP " ; }
const char * name4 ( ) const override { return " FLIP " ; }
private :
// Flip
2019-02-02 00:19:03 -04:00
Vector3f orig_attitude ; // original vehicle attitude before flip
2019-04-08 00:20:47 -03:00
enum class FlipState : uint8_t {
Start ,
Roll ,
Pitch_A ,
Pitch_B ,
Recover ,
Abandon
2019-02-02 00:19:03 -04:00
} ;
FlipState _state ; // current state of flip
2019-09-07 20:33:56 -03:00
Mode : : Number orig_control_mode ; // flight mode when flip was initated
2019-02-02 00:19:03 -04:00
uint32_t start_time_ms ; // time since flip began
int8_t roll_dir ; // roll direction (-1 = roll left, 1 = roll right)
int8_t pitch_dir ; // pitch direction (-1 = pitch forward, 1 = pitch back)
2017-12-10 23:10:48 -04:00
} ;
2024-09-04 00:48:37 -03:00
# if MODE_FLOWHOLD_ENABLED
2018-02-08 08:21:10 -04:00
/*
class to support FLOWHOLD mode , which is a position hold mode using
optical flow directly , avoiding the need for a rangefinder
*/
class ModeFlowHold : public Mode {
public :
// need a constructor for parameters
ModeFlowHold ( void ) ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : FLOWHOLD ; }
2018-02-08 08:21:10 -04:00
bool init ( bool ignore_checks ) override ;
void run ( void ) override ;
bool requires_GPS ( ) const override { return false ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return true ; } ;
2018-02-08 08:21:10 -04:00
bool is_autopilot ( ) const override { return false ; }
2016-05-01 00:29:31 -03:00
bool has_user_takeoff ( bool must_navigate ) const override {
return ! must_navigate ;
}
2020-02-10 22:17:19 -04:00
bool allows_flip ( ) const override { return true ; }
2018-02-08 08:21:10 -04:00
static const struct AP_Param : : GroupInfo var_info [ ] ;
protected :
const char * name ( ) const override { return " FLOWHOLD " ; }
const char * name4 ( ) const override { return " FHLD " ; }
private :
// FlowHold states
enum FlowHoldModeState {
FlowHold_MotorStopped ,
FlowHold_Takeoff ,
FlowHold_Flying ,
FlowHold_Landed
} ;
// calculate attitude from flow data
void flow_to_angle ( Vector2f & bf_angle ) ;
2024-07-27 09:14:08 -03:00
LowPassFilterConstDtVector2f flow_filter ;
2018-02-08 08:21:10 -04:00
bool flowhold_init ( bool ignore_checks ) ;
void flowhold_run ( ) ;
void flowhold_flow_to_angle ( Vector2f & angle , bool stick_input ) ;
void update_height_estimate ( void ) ;
// minimum assumed height
2019-04-04 02:41:10 -03:00
const float height_min = 0.1f ;
2018-02-08 08:21:10 -04:00
// maximum scaling height
2019-04-04 02:41:10 -03:00
const float height_max = 3.0f ;
2018-02-08 08:21:10 -04:00
AP_Float flow_max ;
2019-04-04 02:41:10 -03:00
AC_PI_2D flow_pi_xy { 0.2f , 0.3f , 3000 , 5 , 0.0025f } ;
2018-02-08 08:21:10 -04:00
AP_Float flow_filter_hz ;
AP_Int8 flow_min_quality ;
AP_Int8 brake_rate_dps ;
float quality_filtered ;
uint8_t log_counter ;
bool limited ;
Vector2f xy_I ;
// accumulated INS delta velocity in north-east form since last flow update
Vector2f delta_velocity_ne ;
// last flow rate in radians/sec in north-east axis
Vector2f last_flow_rate_rps ;
// timestamp of last flow data
uint32_t last_flow_ms ;
float last_ins_height ;
float height_offset ;
// are we braking after pilot input?
bool braking ;
// last time there was significant stick input
uint32_t last_stick_input_ms ;
} ;
2022-08-16 23:18:38 -03:00
# endif // MODE_FLOWHOLD_ENABLED
2018-02-08 08:21:10 -04:00
2017-12-10 23:51:13 -04:00
class ModeGuided : public Mode {
2017-12-10 23:10:48 -04:00
public :
2023-08-08 21:28:58 -03:00
# if AP_EXTERNAL_CONTROL_ENABLED
friend class AP_ExternalControl_Copter ;
# endif
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : GUIDED ; }
2017-12-10 23:10:48 -04:00
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override ;
2017-12-10 23:10:48 -04:00
bool is_autopilot ( ) const override { return true ; }
2016-05-01 00:29:31 -03:00
bool has_user_takeoff ( bool must_navigate ) const override { return true ; }
2019-04-18 20:41:30 -03:00
bool in_guided_mode ( ) const override { return true ; }
2017-12-10 23:10:48 -04:00
2019-09-08 20:08:02 -03:00
bool requires_terrain_failsafe ( ) const override { return true ; }
2022-01-18 14:01:02 -04:00
// Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
// attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
// IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
// ang_vel: angular velocity (rad/s)
// climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless
// use_thrust: IF true: climb_rate_cms_or_thrust represents thrust
// IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
void set_angle ( const Quaternion & attitude_quat , const Vector3f & ang_vel , float climb_rate_cms_or_thrust , bool use_thrust ) ;
2020-02-16 05:42:31 -04:00
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 , bool terrain_alt = false ) ;
2019-01-01 22:54:31 -04:00
bool set_destination ( const Location & 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 ) ;
2021-07-06 18:02:26 -03:00
bool get_wp ( Location & loc ) const override ;
2021-05-11 01:33:13 -03:00
void set_accel ( const Vector3f & acceleration , 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 ) ;
2018-02-06 02:16:09 -04:00
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 ) ;
2021-05-11 01:33:13 -03:00
void set_velaccel ( const Vector3f & velocity , const Vector3f & acceleration , 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 ) ;
2017-12-10 23:10:48 -04:00
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 ) ;
2021-05-11 01:33:13 -03:00
bool set_destination_posvelaccel ( const Vector3f & destination , const Vector3f & velocity , const Vector3f & acceleration , bool use_yaw = false , float yaw_cd = 0.0 , bool use_yaw_rate = false , float yaw_rate_cds = 0.0 , bool yaw_relative = false ) ;
// get position, velocity and acceleration targets
const Vector3p & get_target_pos ( ) const ;
const Vector3f & get_target_vel ( ) const ;
const Vector3f & get_target_accel ( ) const ;
2017-12-10 23:10:48 -04:00
2021-04-15 01:14:12 -03:00
// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
bool set_attitude_target_provides_thrust ( ) const ;
2021-05-11 01:33:13 -03:00
bool stabilizing_pos_xy ( ) const ;
bool stabilizing_vel_xy ( ) const ;
2021-09-07 09:30:25 -03:00
bool use_wpnav_for_position_control ( ) const ;
2021-04-15 01:14:12 -03:00
2017-12-10 23:10:48 -04:00
void limit_clear ( ) ;
void limit_init_time_and_pos ( ) ;
2016-03-21 23:13:34 -03:00
void limit_set ( uint32_t timeout_ms , float alt_min_cm , float alt_max_cm , float horiz_max_cm ) ;
bool limit_check ( ) ;
2018-04-30 06:50:04 -03:00
bool is_taking_off ( ) const override ;
2022-07-27 06:20:29 -03:00
bool set_speed_xy ( float speed_xy_cms ) override ;
bool set_speed_up ( float speed_up_cms ) override ;
bool set_speed_down ( float speed_down_cms ) override ;
2018-04-30 06:50:04 -03:00
2022-02-10 23:05:43 -04:00
// initialises position controller to implement take-off
// takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
2020-03-06 22:28:43 -04:00
bool do_user_takeoff_start ( float takeoff_alt_cm ) override ;
2016-03-21 23:13:34 -03:00
2021-03-30 00:36:25 -03:00
enum class SubMode {
TakeOff ,
WP ,
2021-09-07 09:30:25 -03:00
Pos ,
2021-05-11 01:33:13 -03:00
PosVelAccel ,
VelAccel ,
Accel ,
2021-03-30 00:36:25 -03:00
Angle ,
} ;
SubMode submode ( ) const { return guided_mode ; }
2016-03-21 23:13:34 -03:00
void angle_control_start ( ) ;
void angle_control_run ( ) ;
2021-09-09 01:05:49 -03:00
// return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rate control
2021-07-09 02:02:07 -03:00
uint32_t get_timeout_ms ( ) const ;
2021-09-03 03:22:25 -03:00
bool use_pilot_yaw ( ) const override ;
2022-01-27 14:36:21 -04:00
// pause continue in guided mode
bool pause ( ) override ;
bool resume ( ) override ;
2022-04-14 06:58:27 -03:00
// true if weathervaning is allowed in guided
2024-09-04 00:48:37 -03:00
# if WEATHERVANE_ENABLED
2022-04-14 06:58:27 -03:00
bool allows_weathervaning ( void ) const override ;
# endif
2016-03-21 23:13:34 -03:00
protected :
const char * name ( ) const override { return " GUIDED " ; }
const char * name4 ( ) const override { return " GUID " ; }
2017-12-05 21:28:32 -04:00
uint32_t wp_distance ( ) const override ;
int32_t wp_bearing ( ) const override ;
2018-05-23 12:34:24 -03:00
float crosstrack_error ( ) const override ;
2017-12-05 21:28:32 -04:00
2016-03-21 23:13:34 -03:00
private :
2020-10-19 01:39:51 -03:00
// enum for GUID_OPTIONS parameter
2024-05-23 07:25:30 -03:00
enum class Option : uint32_t {
2021-05-11 01:33:13 -03:00
AllowArmingFromTX = ( 1U < < 0 ) ,
2020-11-12 20:15:53 -04:00
// this bit is still available, pilot yaw was mapped to bit 2 for symmetry with auto
2021-05-11 01:33:13 -03:00
IgnorePilotYaw = ( 1U < < 2 ) ,
2021-04-15 01:14:12 -03:00
SetAttitudeTarget_ThrustAsThrust = ( 1U < < 3 ) ,
2021-05-11 01:33:13 -03:00
DoNotStabilizePositionXY = ( 1U < < 4 ) ,
DoNotStabilizeVelocityXY = ( 1U < < 5 ) ,
2021-09-07 09:30:25 -03:00
WPNavUsedForPosControl = ( 1U < < 6 ) ,
2022-04-14 06:58:27 -03:00
AllowWeatherVaning = ( 1U < < 7 )
2020-10-14 02:25:41 -03:00
} ;
2024-05-23 07:25:30 -03:00
// returns true if the Guided-mode-option is set (see GUID_OPTIONS)
bool option_is_enabled ( Option option ) const ;
2021-09-07 09:30:25 -03:00
// wp controller
void wp_control_start ( ) ;
void wp_control_run ( ) ;
2021-07-08 01:11:54 -03:00
void pva_control_start ( ) ;
2016-03-21 23:13:34 -03:00
void pos_control_start ( ) ;
2021-05-11 01:33:13 -03:00
void accel_control_start ( ) ;
void velaccel_control_start ( ) ;
void posvelaccel_control_start ( ) ;
2016-03-21 23:13:34 -03:00
void takeoff_run ( ) ;
void pos_control_run ( ) ;
2021-05-11 01:33:13 -03:00
void accel_control_run ( ) ;
void velaccel_control_run ( ) ;
2022-01-27 05:00:15 -04:00
void pause_control_run ( ) ;
2021-05-11 01:33:13 -03:00
void posvelaccel_control_run ( ) ;
2016-03-21 23:13:34 -03:00
void set_yaw_state ( bool use_yaw , float yaw_cd , bool use_yaw_rate , float yaw_rate_cds , bool relative_angle ) ;
// controls which controller is run (pos or vel):
2021-03-30 00:36:25 -03:00
SubMode guided_mode = SubMode : : TakeOff ;
2021-03-05 03:03:56 -04:00
bool send_notification ; // used to send one time notification to ground station
2021-09-17 22:33:58 -03:00
bool takeoff_complete ; // true once takeoff has completed (used to trigger retracting of landing gear)
2022-01-27 14:36:21 -04:00
// guided mode is paused or not
bool _paused ;
2016-03-21 23:13:34 -03:00
} ;
2016-03-22 00:24:56 -03:00
2017-12-10 23:51:13 -04:00
class ModeGuidedNoGPS : public ModeGuided {
2017-12-10 23:10:48 -04:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using ModeGuided : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : GUIDED_NOGPS ; }
2017-12-10 23:10:48 -04:00
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
2019-02-05 21:33:36 -04:00
bool requires_GPS ( ) const override { return false ; }
2017-12-10 23:10:48 -04:00
bool has_manual_throttle ( ) const override { return false ; }
bool is_autopilot ( ) const override { return true ; }
protected :
const char * name ( ) const override { return " GUIDED_NOGPS " ; }
const char * name4 ( ) const override { return " GNGP " ; }
private :
} ;
2016-03-22 00:24:56 -03:00
2017-12-10 23:51:13 -04:00
class ModeLand : public Mode {
2016-03-22 00:24:56 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : LAND ; }
2016-03-22 00:24:56 -03:00
bool init ( bool ignore_checks ) override ;
2017-11-09 22:54:34 -04:00
void run ( ) override ;
2016-03-22 00:24:56 -03:00
bool requires_GPS ( ) const override { return false ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; } ;
2016-03-22 00:24:56 -03:00
bool is_autopilot ( ) const override { return true ; }
2019-04-18 20:41:30 -03:00
2018-04-30 06:50:04 -03:00
bool is_landing ( ) const override { return true ; } ;
2016-03-22 00:24:56 -03:00
void do_not_use_GPS ( ) ;
2020-05-01 00:59:07 -03:00
// returns true if LAND mode is trying to control X/Y position
bool controlling_position ( ) const { return control_position ; }
void set_land_pause ( bool new_value ) { land_pause = new_value ; }
2016-03-22 00:24:56 -03:00
protected :
const char * name ( ) const override { return " LAND " ; }
const char * name4 ( ) const override { return " LAND " ; }
private :
void gps_run ( ) ;
void nogps_run ( ) ;
2020-05-01 00:59:07 -03:00
bool control_position ; // true if we are using an external reference to control position
uint32_t land_start_time ;
bool land_pause ;
2016-03-22 00:24:56 -03:00
} ;
2016-03-22 03:42:17 -03:00
2017-12-10 23:51:13 -04:00
class ModeLoiter : public Mode {
2017-12-10 23:10:48 -04:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : LOITER ; }
2017-12-10 23:10:48 -04:00
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return true ; } ;
2017-12-10 23:10:48 -04:00
bool is_autopilot ( ) const override { return false ; }
2016-05-01 00:29:31 -03:00
bool has_user_takeoff ( bool must_navigate ) const override { return true ; }
2020-02-10 22:06:57 -04:00
bool allows_autotune ( ) const override { return true ; }
2017-12-10 23:10:48 -04:00
2024-03-06 00:18:38 -04:00
# if FRAME_CONFIG == HELI_FRAME
bool allows_inverted ( ) const override { return true ; } ;
# endif
2023-03-22 05:45:41 -03:00
# if AC_PRECLAND_ENABLED
2017-12-10 23:10:48 -04:00
void set_precision_loiter_enabled ( bool value ) { _precision_loiter_enabled = value ; }
# endif
protected :
const char * name ( ) const override { return " LOITER " ; }
const char * name4 ( ) const override { return " LOIT " ; }
2018-02-07 22:21:09 -04:00
uint32_t wp_distance ( ) const override ;
int32_t wp_bearing ( ) const override ;
2021-07-20 21:38:05 -03:00
float crosstrack_error ( ) const override { return pos_control - > crosstrack_error ( ) ; }
2017-12-10 23:10:48 -04:00
2023-03-22 05:45:41 -03:00
# if AC_PRECLAND_ENABLED
2017-12-10 23:10:48 -04:00
bool do_precision_loiter ( ) ;
void precision_loiter_xy ( ) ;
# endif
private :
2023-03-22 05:45:41 -03:00
# if AC_PRECLAND_ENABLED
2017-12-10 23:10:48 -04:00
bool _precision_loiter_enabled ;
2022-01-30 07:04:42 -04:00
bool _precision_loiter_active ; // true if user has switched on prec loiter
2017-12-10 23:10:48 -04:00
# endif
} ;
2017-12-10 23:51:13 -04:00
class ModePosHold : public Mode {
2017-12-10 23:10:48 -04:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : POSHOLD ; }
2017-12-10 23:10:48 -04:00
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return true ; } ;
2017-12-10 23:10:48 -04:00
bool is_autopilot ( ) const override { return false ; }
2016-05-01 00:29:31 -03:00
bool has_user_takeoff ( bool must_navigate ) const override { return true ; }
2020-02-10 22:06:57 -04:00
bool allows_autotune ( ) const override { return true ; }
2017-12-10 23:10:48 -04:00
protected :
const char * name ( ) const override { return " POSHOLD " ; }
const char * name4 ( ) const override { return " PHLD " ; }
private :
2019-04-22 22:54:48 -03:00
void update_pilot_lean_angle ( float & lean_angle_filtered , float & lean_angle_raw ) ;
2019-07-07 10:13:43 -03:00
float mix_controls ( float mix_ratio , float first_control , float second_control ) ;
void update_brake_angle_from_velocity ( float & brake_angle , float velocity ) ;
2020-11-05 21:42:17 -04:00
void init_wind_comp_estimate ( ) ;
2019-04-22 22:54:48 -03:00
void update_wind_comp_estimate ( ) ;
2019-07-07 10:13:43 -03:00
void get_wind_comp_lean_angles ( float & roll_angle , float & pitch_angle ) ;
2019-04-22 22:54:48 -03:00
void roll_controller_to_pilot_override ( ) ;
void pitch_controller_to_pilot_override ( ) ;
2017-12-10 23:10:48 -04:00
2019-04-22 23:14:35 -03:00
enum class RPMode {
PILOT_OVERRIDE = 0 , // pilot is controlling this axis (i.e. roll or pitch)
BRAKE , // this axis is braking towards zero
BRAKE_READY_TO_LOITER , // this axis has completed braking and is ready to enter loiter mode (both modes must be this value before moving to next stage)
BRAKE_TO_LOITER , // both vehicle's axis (roll and pitch) are transitioning from braking to loiter mode (braking and loiter controls are mixed)
LOITER , // both vehicle axis are holding position
CONTROLLER_TO_PILOT_OVERRIDE // pilot has input controls on this axis and this axis is transitioning to pilot override (other axis will transition to brake if no pilot input)
2019-04-22 22:48:41 -03:00
} ;
2019-04-22 23:14:35 -03:00
RPMode roll_mode ;
RPMode pitch_mode ;
2019-04-22 22:48:41 -03:00
// pilot input related variables
float pilot_roll ; // pilot requested roll angle (filtered to slow returns to zero)
float pilot_pitch ; // pilot requested roll angle (filtered to slow returns to zero)
// braking related variables
2019-04-08 21:58:11 -03:00
struct {
uint8_t time_updated_roll : 1 ; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
uint8_t time_updated_pitch : 1 ; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
float gain ; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from rate)
float roll ; // target roll angle during braking periods
float pitch ; // target pitch angle during braking periods
int16_t timeout_roll ; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
int16_t timeout_pitch ; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
float angle_max_roll ; // maximum lean angle achieved during braking. Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
float angle_max_pitch ; // maximum lean angle achieved during braking Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
int16_t to_loiter_timer ; // cycles to mix brake and loiter controls in POSHOLD_TO_LOITER
} brake ;
2019-04-22 22:48:41 -03:00
// loiter related variables
int16_t controller_to_pilot_timer_roll ; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
int16_t controller_to_pilot_timer_pitch ; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
2019-07-07 10:13:43 -03:00
float controller_final_roll ; // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
float controller_final_pitch ; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
2019-04-22 22:48:41 -03:00
// wind compensation related variables
Vector2f wind_comp_ef ; // wind compensation in earth frame, filtered lean angles from position controller
2019-07-07 10:13:43 -03:00
float wind_comp_roll ; // roll angle to compensate for wind
float wind_comp_pitch ; // pitch angle to compensate for wind
2019-04-22 22:48:41 -03:00
uint16_t wind_comp_start_timer ; // counter to delay start of wind compensation for a short time after loiter is engaged
int8_t wind_comp_timer ; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz
// final output
2019-07-07 10:13:43 -03:00
float roll ; // final roll angle sent to attitude controller
float pitch ; // final pitch angle sent to attitude controller
2019-04-22 22:48:41 -03:00
2017-12-10 23:10:48 -04:00
} ;
2016-03-22 03:42:17 -03:00
2017-12-10 23:51:13 -04:00
class ModeRTL : public Mode {
2016-03-22 03:42:17 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : RTL ; }
2016-03-22 03:42:17 -03:00
bool init ( bool ignore_checks ) override ;
2017-11-09 22:54:34 -04:00
void run ( ) override {
2016-03-22 03:42:17 -03:00
return run ( true ) ;
}
2017-11-09 22:54:34 -04:00
void run ( bool disarm_on_land ) ;
2016-03-22 03:42:17 -03:00
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; } ;
2016-03-22 03:42:17 -03:00
bool is_autopilot ( ) const override { return true ; }
2019-09-08 20:08:02 -03:00
bool requires_terrain_failsafe ( ) const override { return true ; }
2019-06-05 07:47:32 -03:00
// for reporting to GCS
2021-07-06 18:02:26 -03:00
bool get_wp ( Location & loc ) const override ;
2019-06-05 07:47:32 -03:00
2021-09-03 03:22:25 -03:00
bool use_pilot_yaw ( ) const override ;
2023-04-19 09:28:51 -03:00
bool set_speed_xy ( float speed_xy_cms ) override ;
bool set_speed_up ( float speed_up_cms ) override ;
bool set_speed_down ( float speed_down_cms ) override ;
2019-12-11 23:36:28 -04:00
// RTL states
2021-04-13 12:22:04 -03:00
enum class SubMode : uint8_t {
2021-01-26 07:10:11 -04:00
STARTING ,
INITIAL_CLIMB ,
RETURN_HOME ,
LOITER_AT_HOME ,
FINAL_DESCENT ,
LAND
2019-12-11 23:36:28 -04:00
} ;
2021-04-13 12:22:04 -03:00
SubMode state ( ) { return _state ; }
2016-03-22 03:42:17 -03:00
// this should probably not be exposed
2021-02-01 12:26:21 -04:00
bool state_complete ( ) const { return _state_complete ; }
2016-03-22 03:42:17 -03:00
2020-09-02 20:31:08 -03:00
virtual bool is_landing ( ) const override ;
2016-03-22 03:42:17 -03:00
void restart_without_terrain ( ) ;
2019-12-11 23:36:49 -04:00
// enum for RTL_ALT_TYPE parameter
2022-07-16 11:11:58 -03:00
enum class RTLAltType : int8_t {
2024-09-04 23:35:23 -03:00
RELATIVE = 0 ,
TERRAIN = 1
2019-12-11 23:36:49 -04:00
} ;
ModeRTL : : RTLAltType get_alt_type ( ) const ;
2016-03-22 03:42:17 -03:00
protected :
const char * name ( ) const override { return " RTL " ; }
const char * name4 ( ) const override { return " RTL " ; }
2019-06-05 07:47:32 -03:00
// for reporting to GCS
2018-02-07 22:21:09 -04:00
uint32_t wp_distance ( ) const override ;
int32_t wp_bearing ( ) const override ;
2018-05-23 12:34:24 -03:00
float crosstrack_error ( ) const override { return wp_nav - > crosstrack_error ( ) ; }
2017-12-05 21:28:32 -04:00
2017-09-10 20:46:51 -03:00
void descent_start ( ) ;
void descent_run ( ) ;
void land_start ( ) ;
void land_run ( bool disarm_on_land ) ;
void set_descent_target_alt ( uint32_t alt ) { rtl_path . descent_target . alt = alt ; }
2016-03-22 03:42:17 -03:00
private :
void climb_start ( ) ;
void return_start ( ) ;
void climb_return_run ( ) ;
void loiterathome_start ( ) ;
void loiterathome_run ( ) ;
2018-10-25 23:10:32 -03:00
void build_path ( ) ;
void compute_return_target ( ) ;
2016-03-22 03:42:17 -03:00
2021-04-13 12:22:04 -03:00
SubMode _state = SubMode : : INITIAL_CLIMB ; // records state of rtl (initial climb, returning home, etc)
2016-03-22 03:42:17 -03:00
bool _state_complete = false ; // set to true if the current state is completed
struct {
// NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain
2019-01-01 22:54:31 -04:00
Location origin_point ;
Location climb_target ;
Location return_target ;
Location descent_target ;
2016-03-22 03:42:17 -03:00
bool land ;
} rtl_path ;
2019-12-11 23:36:49 -04:00
// return target alt type
enum class ReturnTargetAltType {
2020-09-02 20:39:23 -03:00
RELATIVE = 0 ,
RANGEFINDER = 1 ,
TERRAINDATABASE = 2
2019-12-11 23:36:49 -04:00
} ;
2016-03-22 03:42:17 -03:00
// Loiter timer - Records how long we have been in loiter
2018-10-25 22:28:03 -03:00
uint32_t _loiter_start_time ;
2018-10-25 23:10:32 -03:00
bool terrain_following_allowed ;
2020-11-12 20:15:53 -04:00
// enum for RTL_OPTIONS parameter
enum class Options : int32_t {
// First pair of bits are still available, pilot yaw was mapped to bit 2 for symmetry with auto
IgnorePilotYaw = ( 1U < < 2 ) ,
} ;
2016-03-22 03:42:17 -03:00
} ;
2016-03-22 03:56:08 -03:00
2017-12-10 23:51:13 -04:00
class ModeSmartRTL : public ModeRTL {
2016-03-22 03:56:08 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using ModeRTL : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : SMART_RTL ; }
2016-03-22 03:56:08 -03:00
2017-12-10 23:10:48 -04:00
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
2016-03-22 03:56:08 -03:00
2017-12-10 23:10:48 -04:00
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; }
2017-12-10 23:10:48 -04:00
bool is_autopilot ( ) const override { return true ; }
void save_position ( ) ;
2021-04-20 09:36:36 -03:00
void exit ( ) override ;
2016-03-22 03:56:08 -03:00
2020-09-02 20:31:08 -03:00
bool is_landing ( ) const override ;
2021-09-03 03:27:49 -03:00
bool use_pilot_yaw ( ) const override ;
2020-09-02 20:31:08 -03:00
2021-01-26 07:37:55 -04:00
// Safe RTL states
2021-04-13 12:09:24 -03:00
enum class SubMode : uint8_t {
2021-01-26 07:37:55 -04:00
WAIT_FOR_PATH_CLEANUP ,
PATH_FOLLOW ,
PRELAND_POSITION ,
DESCEND ,
LAND
} ;
2016-03-22 03:56:08 -03:00
protected :
2017-12-10 23:10:48 -04:00
const char * name ( ) const override { return " SMARTRTL " ; }
const char * name4 ( ) const override { return " SRTL " ; }
2019-06-05 07:47:32 -03:00
// for reporting to GCS
2021-07-06 18:02:26 -03:00
bool get_wp ( Location & loc ) const override ;
2018-02-07 22:21:09 -04:00
uint32_t wp_distance ( ) const override ;
int32_t wp_bearing ( ) const override ;
2018-05-23 12:34:24 -03:00
float crosstrack_error ( ) const override { return wp_nav - > crosstrack_error ( ) ; }
2016-03-22 03:56:08 -03:00
private :
2017-12-10 23:10:48 -04:00
void wait_cleanup_run ( ) ;
void path_follow_run ( ) ;
void pre_land_position_run ( ) ;
void land ( ) ;
2021-04-13 12:09:24 -03:00
SubMode smart_rtl_state = SubMode : : PATH_FOLLOW ;
2016-03-22 03:56:08 -03:00
2020-10-22 21:28:41 -03:00
// keep track of how long we have failed to get another return
// point while following our path home. If we take too long we
// may choose to land the vehicle.
uint32_t path_follow_last_pop_fail_ms ;
2024-07-23 09:43:59 -03:00
// backup last popped point so that it can be restored to the path
// if vehicle exits SmartRTL mode before reaching home. invalid if zero
Vector3f dest_NED_backup ;
2016-03-22 03:56:08 -03:00
} ;
2016-03-22 05:07:23 -03:00
2017-12-10 23:51:13 -04:00
class ModeSport : public Mode {
2016-03-22 05:07:23 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : SPORT ; }
2016-03-22 05:07:23 -03:00
2018-02-08 18:50:59 -04:00
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
2016-03-22 05:07:23 -03:00
2018-02-08 18:50:59 -04:00
bool requires_GPS ( ) const override { return false ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return true ; } ;
2018-02-08 18:50:59 -04:00
bool is_autopilot ( ) const override { return false ; }
2016-05-01 00:29:31 -03:00
bool has_user_takeoff ( bool must_navigate ) const override {
return ! must_navigate ;
}
2016-03-22 05:07:23 -03:00
protected :
const char * name ( ) const override { return " SPORT " ; }
const char * name4 ( ) const override { return " SPRT " ; }
private :
} ;
2016-03-22 07:41:38 -03:00
2017-12-10 23:51:13 -04:00
class ModeStabilize : public Mode {
2016-03-22 07:41:38 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : STABILIZE ; }
2016-03-22 07:41:38 -03:00
2017-11-09 22:54:34 -04:00
virtual void run ( ) override ;
2016-03-22 07:41:38 -03:00
2018-02-08 18:50:59 -04:00
bool requires_GPS ( ) const override { return false ; }
bool has_manual_throttle ( ) const override { return true ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return true ; } ;
2018-02-08 18:50:59 -04:00
bool is_autopilot ( ) const override { return false ; }
2021-02-07 00:58:50 -04:00
bool allows_save_trim ( ) const override { return true ; }
2020-02-10 22:06:57 -04:00
bool allows_autotune ( ) const override { return true ; }
2020-02-10 22:17:19 -04:00
bool allows_flip ( ) const override { return true ; }
2016-03-22 07:41:38 -03:00
protected :
2017-12-10 23:10:48 -04:00
const char * name ( ) const override { return " STABILIZE " ; }
const char * name4 ( ) const override { return " STAB " ; }
2016-03-22 20:36:52 -03:00
private :
} ;
2016-03-22 21:51:39 -03:00
2017-12-10 23:10:48 -04:00
# if FRAME_CONFIG == HELI_FRAME
2017-12-10 23:51:13 -04:00
class ModeStabilize_Heli : public ModeStabilize {
2016-03-22 21:51:39 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using ModeStabilize : : Mode ;
2016-03-22 21:51:39 -03:00
bool init ( bool ignore_checks ) override ;
2017-11-09 22:54:34 -04:00
void run ( ) override ;
2016-03-22 21:51:39 -03:00
2024-02-29 19:31:38 -04:00
bool allows_inverted ( ) const override { return true ; } ;
2016-03-22 21:51:39 -03:00
protected :
private :
} ;
# endif
2016-03-22 22:00:19 -03:00
2019-07-29 04:55:40 -03:00
class ModeSystemId : public Mode {
public :
ModeSystemId ( void ) ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : SYSTEMID ; }
2019-07-29 04:55:40 -03:00
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
2022-04-06 00:58:38 -03:00
void exit ( ) override ;
2019-07-29 04:55:40 -03:00
2021-03-25 07:20:53 -03:00
bool requires_GPS ( ) const override { return false ; }
bool has_manual_throttle ( ) const override { return true ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; } ;
2019-07-29 04:55:40 -03:00
bool is_autopilot ( ) const override { return false ; }
2019-10-15 02:38:24 -03:00
bool logs_attitude ( ) const override { return true ; }
2021-03-25 07:20:53 -03:00
2022-07-05 00:08:57 -03:00
void set_magnitude ( float input ) { waveform_magnitude . set ( input ) ; }
2019-07-29 04:55:40 -03:00
static const struct AP_Param : : GroupInfo var_info [ ] ;
2022-03-14 23:53:57 -03:00
Chirp chirp_input ;
2019-07-29 04:55:40 -03:00
protected :
const char * name ( ) const override { return " SYSTEMID " ; }
const char * name4 ( ) const override { return " SYSI " ; }
private :
2019-10-15 02:38:24 -03:00
2021-02-01 12:26:21 -04:00
void log_data ( ) const ;
2024-02-13 23:26:31 -04:00
bool is_poscontrol_axis_type ( ) const ;
2019-10-15 02:38:24 -03:00
enum class AxisType {
2024-01-28 00:06:20 -04:00
NONE = 0 , // none
INPUT_ROLL = 1 , // angle input roll axis is being excited
INPUT_PITCH = 2 , // angle pitch axis is being excited
INPUT_YAW = 3 , // angle yaw axis is being excited
RECOVER_ROLL = 4 , // angle roll axis is being excited
RECOVER_PITCH = 5 , // angle pitch axis is being excited
RECOVER_YAW = 6 , // angle yaw axis is being excited
RATE_ROLL = 7 , // rate roll axis is being excited
RATE_PITCH = 8 , // rate pitch axis is being excited
RATE_YAW = 9 , // rate yaw axis is being excited
MIX_ROLL = 10 , // mixer roll axis is being excited
MIX_PITCH = 11 , // mixer pitch axis is being excited
MIX_YAW = 12 , // mixer pitch axis is being excited
MIX_THROTTLE = 13 , // mixer throttle axis is being excited
DISTURB_POS_LAT = 14 , // lateral body axis measured position is being excited
DISTURB_POS_LONG = 15 , // longitudinal body axis measured position is being excited
DISTURB_VEL_LAT = 16 , // lateral body axis measured velocity is being excited
DISTURB_VEL_LONG = 17 , // longitudinal body axis measured velocity is being excited
2024-05-13 20:31:42 -03:00
INPUT_VEL_LAT = 18 , // lateral body axis commanded velocity is being excited
INPUT_VEL_LONG = 19 , // longitudinal body axis commanded velocity is being excited
2019-07-29 04:55:40 -03:00
} ;
2019-12-02 22:34:19 -04:00
AP_Int8 axis ; // Controls which axis are being excited. Set to non-zero to display other parameters
2019-10-15 02:38:24 -03:00
AP_Float waveform_magnitude ; // Magnitude of chirp waveform
AP_Float frequency_start ; // Frequency at the start of the chirp
AP_Float frequency_stop ; // Frequency at the end of the chirp
AP_Float time_fade_in ; // Time to reach maximum amplitude of chirp
AP_Float time_record ; // Time taken to complete the chirp waveform
AP_Float time_fade_out ; // Time to reach zero amplitude after chirp finishes
bool att_bf_feedforward ; // Setting of attitude_control->get_bf_feedforward
float waveform_time ; // Time reference for waveform
float waveform_sample ; // Current waveform sample
float waveform_freq_rads ; // Instantaneous waveform frequency
float time_const_freq ; // Time at constant frequency before chirp starts
int8_t log_subsample ; // Subsample multiple for logging.
2023-08-18 00:56:25 -03:00
Vector2f target_vel ; // target velocity for position controller modes
2024-02-14 20:01:40 -04:00
Vector2f target_pos ; // target positon
Vector2f input_vel_last ; // last cycle input velocity
2019-07-29 04:55:40 -03:00
// System ID states
2019-10-15 02:38:24 -03:00
enum class SystemIDModeState {
SYSTEMID_STATE_STOPPED ,
SYSTEMID_STATE_TESTING
} systemid_state ;
2019-07-29 04:55:40 -03:00
} ;
2016-03-22 22:00:19 -03:00
2017-12-10 23:51:13 -04:00
class ModeThrow : public Mode {
2016-08-02 23:30:27 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : THROW ; }
2016-08-02 23:30:27 -03:00
bool init ( bool ignore_checks ) override ;
2017-11-09 22:54:34 -04:00
void run ( ) override ;
2016-08-02 23:30:27 -03:00
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return true ; } ;
2016-08-02 23:30:27 -03:00
bool is_autopilot ( ) const override { return false ; }
2017-12-13 05:57:38 -04:00
// Throw types
2020-12-05 03:10:00 -04:00
enum class ThrowType {
Upward = 0 ,
Drop = 1
2017-12-13 05:57:38 -04:00
} ;
2016-08-02 23:30:27 -03:00
2020-12-05 03:18:28 -04:00
enum class PreThrowMotorState {
STOPPED = 0 ,
RUNNING = 1 ,
} ;
2016-08-02 23:30:27 -03:00
protected :
const char * name ( ) const override { return " THROW " ; }
const char * name4 ( ) const override { return " THRW " ; }
private :
bool throw_detected ( ) ;
2021-07-02 03:06:42 -03:00
bool throw_position_good ( ) const ;
bool throw_height_good ( ) const ;
bool throw_attitude_good ( ) const ;
2016-08-02 23:30:27 -03:00
2017-12-13 05:57:38 -04:00
// Throw stages
enum ThrowModeStage {
Throw_Disarmed ,
Throw_Detecting ,
2021-07-02 03:37:03 -03:00
Throw_Wait_Throttle_Unlimited ,
2017-12-13 05:57:38 -04:00
Throw_Uprighting ,
Throw_HgtStabilise ,
Throw_PosHold
} ;
2016-08-02 23:30:27 -03:00
ThrowModeStage stage = Throw_Disarmed ;
ThrowModeStage prev_stage = Throw_Disarmed ;
uint32_t last_log_ms ;
bool nextmode_attempted ;
uint32_t free_fall_start_ms ; // system time free fall was detected
float free_fall_start_velz ; // vertical velocity when free fall was detected
} ;
2016-08-02 23:41:10 -03:00
2024-09-04 00:48:37 -03:00
# if MODE_TURTLE_ENABLED
2021-07-27 16:51:19 -03:00
class ModeTurtle : public Mode {
public :
// inherit constructors
using Mode : : Mode ;
Number mode_number ( ) const override { return Number : : TURTLE ; }
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
void exit ( ) override ;
bool requires_GPS ( ) const override { return false ; }
bool has_manual_throttle ( ) const override { return true ; }
bool allows_arming ( AP_Arming : : Method method ) const override ;
bool is_autopilot ( ) const override { return false ; }
void change_motor_direction ( bool reverse ) ;
2021-08-14 06:12:18 -03:00
void output_to_motors ( ) override ;
2021-07-27 16:51:19 -03:00
protected :
const char * name ( ) const override { return " TURTLE " ; }
const char * name4 ( ) const override { return " TRTL " ; }
2021-08-14 06:12:18 -03:00
private :
2022-10-20 15:34:13 -03:00
void arm_motors ( ) ;
void disarm_motors ( ) ;
2021-08-14 06:12:18 -03:00
float motors_output ;
Vector2f motors_input ;
2022-11-19 13:33:16 -04:00
uint32_t last_throttle_warning_output_ms ;
2021-07-27 16:51:19 -03:00
} ;
# endif
2018-02-05 22:44:05 -04:00
// modes below rely on Guided mode so must be declared at the end (instead of in alphabetical order)
2016-08-02 23:41:10 -03:00
2017-12-10 23:51:13 -04:00
class ModeAvoidADSB : public ModeGuided {
2017-09-10 20:46:51 -03:00
public :
2018-02-07 22:21:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using ModeGuided : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : AVOID_ADSB ; }
2017-09-10 20:46:51 -03:00
bool init ( bool ignore_checks ) override ;
2017-11-09 22:54:34 -04:00
void run ( ) override ;
2017-09-10 20:46:51 -03:00
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; }
2017-09-10 20:46:51 -03:00
bool is_autopilot ( ) const override { return true ; }
2017-12-10 23:10:48 -04:00
bool set_velocity ( const Vector3f & velocity_neu ) ;
2017-09-10 20:46:51 -03:00
protected :
2017-12-10 23:10:48 -04:00
const char * name ( ) const override { return " AVOID_ADSB " ; }
const char * name4 ( ) const override { return " AVOI " ; }
2017-12-05 21:28:32 -04:00
2017-09-10 20:46:51 -03:00
private :
} ;
2017-12-26 19:09:11 -04:00
2024-09-04 00:48:37 -03:00
# if MODE_FOLLOW_ENABLED
2018-02-05 22:44:05 -04:00
class ModeFollow : public ModeGuided {
2017-12-26 19:09:11 -04:00
public :
2018-02-06 02:16:09 -04:00
// inherit constructor
2019-05-09 23:18:49 -03:00
using ModeGuided : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : FOLLOW ; }
2017-12-26 19:09:11 -04:00
bool init ( bool ignore_checks ) override ;
2021-04-20 09:36:36 -03:00
void exit ( ) override ;
2017-12-26 19:09:11 -04:00
void run ( ) override ;
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; }
2017-12-26 19:09:11 -04:00
bool is_autopilot ( ) const override { return true ; }
protected :
2018-02-05 22:44:05 -04:00
const char * name ( ) const override { return " FOLLOW " ; }
const char * name4 ( ) const override { return " FOLL " ; }
2019-06-05 07:47:32 -03:00
// for reporting to GCS
2021-07-06 18:02:26 -03:00
bool get_wp ( Location & loc ) const override ;
2018-12-11 05:37:30 -04:00
uint32_t wp_distance ( ) const override ;
int32_t wp_bearing ( ) const override ;
2018-02-06 02:16:09 -04:00
uint32_t last_log_ms ; // system time of last time desired velocity was logging
2017-12-26 19:09:11 -04:00
} ;
2023-08-11 09:27:37 -03:00
# endif
2018-09-07 01:23:33 -03:00
class ModeZigZag : public Mode {
public :
2020-05-11 23:17:21 -03:00
ModeZigZag ( void ) ;
2018-09-07 01:23:33 -03:00
2019-11-28 16:21:07 -04:00
// Inherit constructor
2019-05-09 23:18:49 -03:00
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : ZIGZAG ; }
2018-09-07 01:23:33 -03:00
2020-02-22 09:58:42 -04:00
enum class Destination : uint8_t {
A , // Destination A
B , // Destination B
} ;
2020-04-03 01:48:23 -03:00
enum class Direction : uint8_t {
FORWARD , // moving forward from the yaw direction
RIGHT , // moving right from the yaw direction
BACKWARD , // moving backward from the yaw direction
LEFT , // moving left from the yaw direction
} zigzag_direction ;
2018-09-07 01:23:33 -03:00
bool init ( bool ignore_checks ) override ;
2021-04-20 09:36:36 -03:00
void exit ( ) override ;
2018-09-07 01:23:33 -03:00
void run ( ) override ;
2020-04-07 12:23:27 -03:00
// auto control methods. copter flies grid pattern
2020-04-03 01:48:23 -03:00
void run_auto ( ) ;
2020-04-07 12:23:27 -03:00
void suspend_auto ( ) ;
void init_auto ( ) ;
2018-09-07 01:23:33 -03:00
bool requires_GPS ( ) const override { return true ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return true ; }
2018-09-07 01:23:33 -03:00
bool is_autopilot ( ) const override { return true ; }
2023-06-13 23:01:32 -03:00
bool has_user_takeoff ( bool must_navigate ) const override { return true ; }
2018-09-07 01:23:33 -03:00
2020-02-22 09:58:42 -04:00
// save current position as A or B. If both A and B have been saved move to the one specified
void save_or_move_to_destination ( Destination ab_dest ) ;
2018-09-29 06:25:31 -03:00
// return manual control to the pilot
2019-04-11 01:34:30 -03:00
void return_to_manual_control ( bool maintain_target ) ;
2018-09-07 01:23:33 -03:00
2020-05-11 23:17:21 -03:00
static const struct AP_Param : : GroupInfo var_info [ ] ;
2018-09-07 01:23:33 -03:00
protected :
const char * name ( ) const override { return " ZIGZAG " ; }
const char * name4 ( ) const override { return " ZIGZ " ; }
2021-05-16 02:21:47 -03:00
uint32_t wp_distance ( ) const override ;
int32_t wp_bearing ( ) const override ;
float crosstrack_error ( ) const override ;
2018-09-07 01:23:33 -03:00
private :
void auto_control ( ) ;
void manual_control ( ) ;
2018-09-29 06:25:31 -03:00
bool reached_destination ( ) ;
2020-02-22 09:58:42 -04:00
bool calculate_next_dest ( Destination ab_dest , bool use_wpnav_alt , Vector3f & next_dest , bool & terrain_alt ) const ;
2020-04-03 01:38:35 -03:00
void spray ( bool b ) ;
2020-04-03 01:48:23 -03:00
bool calculate_side_dest ( Vector3f & next_dest , bool & terrain_alt ) const ;
void move_to_side ( ) ;
2018-09-07 01:23:33 -03:00
2018-09-29 06:25:31 -03:00
Vector2f dest_A ; // in NEU frame in cm relative to ekf origin
Vector2f dest_B ; // in NEU frame in cm relative to ekf origin
2020-04-07 12:23:27 -03:00
Vector3f current_dest ; // current target destination (use for resume after suspending)
bool current_terr_alt ;
2018-09-07 01:23:33 -03:00
2020-05-11 23:17:21 -03:00
// parameters
AP_Int8 _auto_enabled ; // top level enable/disable control
2022-11-01 00:11:31 -03:00
# if HAL_SPRAYER_ENABLED
2020-05-11 23:17:21 -03:00
AP_Int8 _spray_enabled ; // auto spray enable/disable
# endif
AP_Int8 _wp_delay ; // delay for zigzag waypoint
AP_Float _side_dist ; // sideways distance
AP_Int8 _direction ; // sideways direction
AP_Int16 _line_num ; // total number of lines
2020-04-03 01:48:23 -03:00
enum ZigZagState {
2018-09-29 06:25:31 -03:00
STORING_POINTS , // storing points A and B, pilot has manual control
AUTO , // after A and B defined, pilot toggle the switch from one side to the other, vehicle flies autonomously
MANUAL_REGAIN // pilot toggle the switch to middle position, has manual control
} stage ;
2018-09-07 01:23:33 -03:00
2020-04-03 01:48:23 -03:00
enum AutoState {
2020-04-07 12:23:27 -03:00
MANUAL , // not in ZigZag Auto
2020-04-03 01:48:23 -03:00
AB_MOVING , // moving from A to B or from B to A
SIDEWAYS , // moving to sideways
} auto_stage ;
2018-09-29 06:25:31 -03:00
uint32_t reach_wp_time_ms = 0 ; // time since vehicle reached destination (or zero if not yet reached)
2020-04-03 01:48:23 -03:00
Destination ab_dest_stored ; // store the current destination
bool is_auto ; // enable zigzag auto feature which is automate both AB and sideways
2020-04-03 04:51:59 -03:00
uint16_t line_count = 0 ; // current line number
int16_t line_num = 0 ; // target line number
2020-04-07 12:23:27 -03:00
bool is_suspended ; // true if zigzag auto is suspended
2018-09-07 01:23:33 -03:00
} ;
2019-11-28 16:21:07 -04:00
2024-09-04 00:48:37 -03:00
# if MODE_AUTOROTATE_ENABLED
2019-11-28 16:21:07 -04:00
class ModeAutorotate : public Mode {
public :
// inherit constructor
using Mode : : Mode ;
2020-01-30 02:10:15 -04:00
Number mode_number ( ) const override { return Number : : AUTOROTATE ; }
2019-11-28 16:21:07 -04:00
bool init ( bool ignore_checks ) override ;
void run ( ) override ;
bool is_autopilot ( ) const override { return true ; }
bool requires_GPS ( ) const override { return false ; }
bool has_manual_throttle ( ) const override { return false ; }
2020-12-31 20:49:01 -04:00
bool allows_arming ( AP_Arming : : Method method ) const override { return false ; } ;
2019-11-28 16:21:07 -04:00
static const struct AP_Param : : GroupInfo var_info [ ] ;
protected :
const char * name ( ) const override { return " AUTOROTATE " ; }
const char * name4 ( ) const override { return " AROT " ; }
private :
// --- Internal variables ---
float _initial_rpm ; // Head speed recorded at initiation of flight mode (RPM)
float _target_head_speed ; // The terget head main rotor head speed. Normalised by main rotor set point
float _desired_v_z ; // Desired vertical
int32_t _pitch_target ; // Target pitch attitude to pass to attitude controller
2020-04-18 22:32:01 -03:00
uint32_t _entry_time_start_ms ; // Time remaining until entry phase moves on to glide phase
2019-11-28 16:21:07 -04:00
float _hs_decay ; // The head accerleration during the entry phase
float _bail_time ; // Timer for exiting the bail out phase (s)
2020-04-18 22:32:01 -03:00
uint32_t _bail_time_start_ms ; // Time at start of bail out
2019-11-28 16:21:07 -04:00
float _target_climb_rate_adjust ; // Target vertical acceleration used during bail out phase
float _target_pitch_adjust ; // Target pitch rate used during bail out phase
enum class Autorotation_Phase {
ENTRY ,
SS_GLIDE ,
FLARE ,
TOUCH_DOWN ,
BAIL_OUT } phase_switch ;
enum class Navigation_Decision {
USER_CONTROL_STABILISED ,
STRAIGHT_AHEAD ,
INTO_WIND ,
NEAREST_RALLY } nav_pos_switch ;
// --- Internal flags ---
struct controller_flags {
bool entry_initial : 1 ;
bool ss_glide_initial : 1 ;
bool flare_initial : 1 ;
bool touch_down_initial : 1 ;
bool straight_ahead_initial : 1 ;
bool level_initial : 1 ;
bool break_initial : 1 ;
bool bail_out_initial : 1 ;
bool bad_rpm : 1 ;
} _flags ;
struct message_flags {
bool bad_rpm : 1 ;
} _msg_flags ;
//--- Internal functions ---
void warning_message ( uint8_t message_n ) ; //Handles output messages to the terminal
} ;
2019-12-01 19:25:32 -04:00
# endif