ArduCopter/mode.h

   1 #pragma once
   2
   3 #include "Copter.h"
   4 #include <AP_Math/chirp.h>
   5 #include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
   6
   7 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
   8 #include "afs_copter.h"
   9 #endif
  10
  11 class Parameters;
  12 class ParametersG2;
  13
  14 class GCS_Copter;
  15
  16 // object shared by both Guided and Auto for takeoff.
  17 // position controller controls vehicle but the user can control the yaw.
  18 class _AutoTakeoff {
  19 public:
  20     void run();
  21     void start(float complete_alt_cm, bool terrain_alt);
  22     bool get_completion_pos(Vector3p& pos_neu_cm);
  23
  24     bool complete;          // true when takeoff is complete
  25
  26 private:
  27     // altitude above-ekf-origin below which auto takeoff does not control horizontal position
  28     bool no_nav_active;
  29     float no_nav_alt_cm;
  30
  31     // auto takeoff variables
  32     float complete_alt_cm;  // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt)
  33     bool terrain_alt;       // true if altitudes are above terrain
  34     Vector3p complete_pos;  // target takeoff position as offset from ekf origin in cm
  35 };
  36
  37 #if AC_PAYLOAD_PLACE_ENABLED
  38 class PayloadPlace {
  39 public:
  40     void run();
  41     void start_descent();
  42     bool verify();
  43
  44     enum class State : uint8_t {
  45         FlyToLocation,
  46         Descent_Start,
  47         Descent,
  48         Release,
  49         Releasing,
  50         Delay,
  51         Ascent_Start,
  52         Ascent,
  53         Done,
  54     };
  55
  56     // these are set by the Mission code:
  57     State state = State::Descent_Start; // records state of payload place
  58     float descent_max_cm;
  59
  60 private:
  61
  62     uint32_t descent_established_time_ms; // milliseconds
  63     uint32_t place_start_time_ms; // milliseconds
  64     float descent_thrust_level;
  65     float descent_start_altitude_cm;
  66     float descent_speed_cms;
  67 };
  68 #endif
  69
  70 class Mode {
  71     friend class PayloadPlace;
  72
  73 public:
  74
  75     // Auto Pilot Modes enumeration
  76     enum class Number : uint8_t {
  77         STABILIZE =     0,  // manual airframe angle with manual throttle
  78         ACRO =          1,  // manual body-frame angular rate with manual throttle
  79         ALT_HOLD =      2,  // manual airframe angle with automatic throttle
  80         AUTO =          3,  // fully automatic waypoint control using mission commands
  81         GUIDED =        4,  // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
  82         LOITER =        5,  // automatic horizontal acceleration with automatic throttle
  83         RTL =           6,  // automatic return to launching point
  84         CIRCLE =        7,  // automatic circular flight with automatic throttle
  85         LAND =          9,  // automatic landing with horizontal position control
  86         DRIFT =        11,  // semi-autonomous position, yaw and throttle control
  87         SPORT =        13,  // manual earth-frame angular rate control with manual throttle
  88         FLIP =         14,  // automatically flip the vehicle on the roll axis
  89         AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
  90         POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
  91         BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
  92         THROW =        18,  // throw to launch mode using inertial/GPS system, no pilot input
  93         AVOID_ADSB =   19,  // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
  94         GUIDED_NOGPS = 20,  // guided mode but only accepts attitude and altitude
  95         SMART_RTL =    21,  // SMART_RTL returns to home by retracing its steps
  96         FLOWHOLD  =    22,  // FLOWHOLD holds position with optical flow without rangefinder
  97         FOLLOW    =    23,  // follow attempts to follow another vehicle or ground station
  98         ZIGZAG    =    24,  // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
  99         SYSTEMID  =    25,  // System ID mode produces automated system identification signals in the controllers
 100         AUTOROTATE =   26,  // Autonomous autorotation
 101         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
 102         TURTLE =       28,  // Flip over after crash
 103
 104         // Mode number 127 reserved for the "drone show mode" in the Skybrush
 105         // fork at https://github.com/skybrush-io/ardupilot
 106     };
 107
 108     // constructor
 109     Mode(void);
 110
 111     // do not allow copying
 112     CLASS_NO_COPY(Mode);
 113
 114     friend class _AutoTakeoff;
 115
 116     // returns a unique number specific to this mode
 117     virtual Number mode_number() const = 0;
 118
 119     // child classes should override these methods
 120     virtual bool init(bool ignore_checks) {
 121         return true;
 122     }
 123     virtual void exit() {};
 124     virtual void run() = 0;
 125     virtual bool requires_GPS() const = 0;
 126     virtual bool has_manual_throttle() const = 0;
 127     virtual bool allows_arming(AP_Arming::Method method) const = 0;
 128     virtual bool is_autopilot() const { return false; }
 129     virtual bool has_user_takeoff(bool must_navigate) const { return false; }
 130     virtual bool in_guided_mode() const { return false; }
 131     virtual bool logs_attitude() const { return false; }
 132     virtual bool allows_save_trim() const { return false; }
 133     virtual bool allows_autotune() const { return false; }
 134     virtual bool allows_flip() const { return false; }
 135     virtual bool crash_check_enabled() const { return true; }
 136
 137 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
 138     // Return the type of this mode for use by advanced failsafe
 139     virtual AP_AdvancedFailsafe_Copter::control_mode afs_mode() const { return AP_AdvancedFailsafe_Copter::control_mode::AFS_STABILIZED; }
 140 #endif
 141
 142     // Return true if the throttle high arming check can be skipped when arming from GCS or Scripting
 143     virtual bool allows_GCS_or_SCR_arming_with_throttle_high() const { return false; }
 144
 145 #if FRAME_CONFIG == HELI_FRAME
 146     virtual bool allows_inverted() const { return false; };
 147 #endif
 148
 149     // return a string for this flightmode
 150     virtual const char *name() const = 0;
 151     virtual const char *name4() const = 0;
 152
 153     bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate);
 154     virtual bool is_taking_off() const;
 155     static void takeoff_stop() { takeoff.stop(); }
 156
 157     virtual bool is_landing() const { return false; }
 158
 159     // mode requires terrain to be present to be functional
 160     virtual bool requires_terrain_failsafe() const { return false; }
 161
 162     // functions for reporting to GCS
 163     virtual bool get_wp(Location &loc) const { return false; };
 164     virtual int32_t wp_bearing() const { return 0; }
 165     virtual uint32_t wp_distance() const { return 0; }
 166     virtual float crosstrack_error() const { return 0.0f;}
 167
 168     // functions to support MAV_CMD_DO_CHANGE_SPEED
 169     virtual bool set_speed_xy(float speed_xy_cms) {return false;}
 170     virtual bool set_speed_up(float speed_xy_cms) {return false;}
 171     virtual bool set_speed_down(float speed_xy_cms) {return false;}
 172
 173     int32_t get_alt_above_ground_cm(void);
 174
 175     // pilot input processing
 176     void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const;
 177     Vector2f get_pilot_desired_velocity(float vel_max) const;
 178     float get_pilot_desired_yaw_rate() const;
 179     float get_pilot_desired_throttle() const;
 180
 181     // returns climb target_rate reduced to avoid obstacles and
 182     // altitude fence
 183     float get_avoidance_adjusted_climbrate(float target_rate);
 184
 185     // send output to the motors, can be overridden by subclasses
 186     virtual void output_to_motors();
 187
 188     // returns true if pilot's yaw input should be used to adjust vehicle's heading
 189     virtual bool use_pilot_yaw() const {return true; }
 190
 191     // pause and resume a mode
 192     virtual bool pause() { return false; };
 193     virtual bool resume() { return false; };
 194
 195     // handle situations where the vehicle is on the ground waiting for takeoff
 196     void make_safe_ground_handling(bool force_throttle_unlimited = false);
 197
 198     // true if weathervaning is allowed in the current mode
 199 #if WEATHERVANE_ENABLED
 200     virtual bool allows_weathervaning() const { return false; }
 201 #endif
 202
 203 protected:
 204
 205     // helper functions
 206     bool is_disarmed_or_landed() const;
 207     void zero_throttle_and_relax_ac(bool spool_up = false);
 208     void zero_throttle_and_hold_attitude();
 209
 210     // Return stopping point as a location with above origin alt frame
 211     Location get_stopping_point() const;
 212
 213     // functions to control normal landing.  pause_descent is true if vehicle should not descend
 214     void land_run_horizontal_control();
 215     void land_run_vertical_control(bool pause_descent = false);
 216     void land_run_horiz_and_vert_control(bool pause_descent = false) {
 217         land_run_horizontal_control();
 218         land_run_vertical_control(pause_descent);
 219     }
 220
 221 #if AC_PAYLOAD_PLACE_ENABLED
 222     // payload place flight behaviour:
 223     static PayloadPlace payload_place;
 224 #endif
 225
 226     // run normal or precision landing (if enabled)
 227     // pause_descent is true if vehicle should not descend
 228     void land_run_normal_or_precland(bool pause_descent = false);
 229
 230 #if AC_PRECLAND_ENABLED
 231     // Go towards a position commanded by prec land state machine in order to retry landing
 232     // The passed in location is expected to be NED and in meters
 233     void precland_retry_position(const Vector3f &retry_pos);
 234
 235     // Run precland statemachine. This function should be called from any mode that wants to do precision landing.
 236     // This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
 237     void precland_run();
 238 #endif
 239
 240     // return expected input throttle setting to hover:
 241     virtual float throttle_hover() const;
 242
 243     // Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
 244     enum class AltHoldModeState {
 245         MotorStopped,
 246         Takeoff,
 247         Landed_Ground_Idle,
 248         Landed_Pre_Takeoff,
 249         Flying
 250     };
 251     AltHoldModeState get_alt_hold_state(float target_climb_rate_cms);
 252
 253     // convenience references to avoid code churn in conversion:
 254     Parameters &g;
 255     ParametersG2 &g2;
 256     AC_WPNav *&wp_nav;
 257     AC_Loiter *&loiter_nav;
 258     AC_PosControl *&pos_control;
 259     AP_InertialNav &inertial_nav;
 260     AP_AHRS &ahrs;
 261     AC_AttitudeControl *&attitude_control;
 262     MOTOR_CLASS *&motors;
 263     RC_Channel *&channel_roll;
 264     RC_Channel *&channel_pitch;
 265     RC_Channel *&channel_throttle;
 266     RC_Channel *&channel_yaw;
 267     float &G_Dt;
 268
 269     // note that we support two entirely different automatic takeoffs:
 270
 271     // "user-takeoff", which is available in modes such as ALT_HOLD
 272     // (see has_user_takeoff method).  "user-takeoff" is a simple
 273     // reach-altitude-based-on-pilot-input-or-parameter routine.
 274
 275     // "auto-takeoff" is used by both Guided and Auto, and is
 276     // basically waypoint navigation with pilot yaw permitted.
 277
 278     // user-takeoff support; takeoff state is shared across all mode instances
 279     class _TakeOff {
 280     public:
 281         void start(float alt_cm);
 282         void stop();
 283         void do_pilot_takeoff(float& pilot_climb_rate);
 284         bool triggered(float target_climb_rate) const;
 285
 286         bool running() const { return _running; }
 287     private:
 288         bool _running;
 289         float take_off_start_alt;
 290         float take_off_complete_alt;
 291     };
 292
 293     static _TakeOff takeoff;
 294
 295     virtual bool do_user_takeoff_start(float takeoff_alt_cm);
 296
 297     static _AutoTakeoff auto_takeoff;
 298
 299 public:
 300     // Navigation Yaw control
 301     class AutoYaw {
 302
 303     public:
 304
 305         // Autopilot Yaw Mode enumeration
 306         enum class Mode {
 307             HOLD =             0,  // hold zero yaw rate
 308             LOOK_AT_NEXT_WP =  1,  // point towards next waypoint (no pilot input accepted)
 309             ROI =              2,  // point towards a location held in roi (no pilot input accepted)
 310             FIXED =            3,  // point towards a particular angle (no pilot input accepted)
 311             LOOK_AHEAD =       4,  // point in the direction the copter is moving
 312             RESETTOARMEDYAW =  5,  // point towards heading at time motors were armed
 313             ANGLE_RATE =       6,  // turn at a specified rate from a starting angle
 314             RATE =             7,  // turn at a specified rate (held in auto_yaw_rate)
 315             CIRCLE =           8,  // use AC_Circle's provided yaw (used during Loiter-Turns commands)
 316             PILOT_RATE =       9,  // target rate from pilot stick
 317             WEATHERVANE =     10,  // yaw into wind
 318         };
 319
 320         // mode(): current method of determining desired yaw:
 321         Mode mode() const { return _mode; }
 322         void set_mode_to_default(bool rtl);
 323         void set_mode(Mode new_mode);
 324         Mode default_mode(bool rtl) const;
 325
 326         void set_rate(float new_rate_cds);
 327
 328         // set_roi(...): set a "look at" location:
 329         void set_roi(const Location &roi_location);
 330
 331         void set_fixed_yaw(float angle_deg,
 332                            float turn_rate_dps,
 333                            int8_t direction,
 334                            bool relative_angle);
 335
 336         void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
 337
 338         void set_yaw_angle_offset(const float yaw_angle_offset_d);
 339
 340         bool reached_fixed_yaw_target();
 341
 342 #if WEATHERVANE_ENABLED
 343         void update_weathervane(const int16_t pilot_yaw_cds);
 344 #endif
 345
 346         AC_AttitudeControl::HeadingCommand get_heading();
 347
 348     private:
 349
 350         // yaw_cd(): main product of AutoYaw; the heading:
 351         float yaw_cd();
 352
 353         // rate_cds(): desired yaw rate in centidegrees/second:
 354         float rate_cds();
 355
 356         // returns a yaw in degrees, direction of vehicle travel:
 357         float look_ahead_yaw();
 358
 359         float roi_yaw() const;
 360
 361         // auto flight mode's yaw mode
 362         Mode _mode = Mode::LOOK_AT_NEXT_WP;
 363         Mode _last_mode;
 364
 365         // Yaw will point at this location if mode is set to Mode::ROI
 366         Vector3f roi;
 367
 368         // yaw used for YAW_FIXED yaw_mode
 369         float _fixed_yaw_offset_cd;
 370
 371         // Deg/s we should turn
 372         float _fixed_yaw_slewrate_cds;
 373
 374         // time of the last yaw update
 375         uint32_t _last_update_ms;
 376
 377         // heading when in yaw_look_ahead_yaw
 378         float _look_ahead_yaw;
 379
 380         // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
 381         float _yaw_angle_cd;
 382         float _yaw_rate_cds;
 383         float _pilot_yaw_rate_cds;
 384     };
 385     static AutoYaw auto_yaw;
 386
 387     // pass-through functions to reduce code churn on conversion;
 388     // these are candidates for moving into the Mode base
 389     // class.
 390     float get_pilot_desired_climb_rate(float throttle_control);
 391     float get_non_takeoff_throttle(void);
 392     void update_simple_mode(void);
 393     bool set_mode(Mode::Number mode, ModeReason reason);
 394     void set_land_complete(bool b);
 395     GCS_Copter &gcs();
 396     uint16_t get_pilot_speed_dn(void);
 397     // end pass-through functions
 398 };
 399
 400
 401 #if MODE_ACRO_ENABLED
 402 class ModeAcro : public Mode {
 403
 404 public:
 405     // inherit constructor
 406     using Mode::Mode;
 407     Number mode_number() const override { return Number::ACRO; }
 408
 409     enum class Trainer {
 410         OFF = 0,
 411         LEVELING = 1,
 412         LIMITED = 2,
 413     };
 414
 415     enum class AcroOptions {
 416         AIR_MODE = 1 << 0,
 417         RATE_LOOP_ONLY = 1 << 1,
 418     };
 419
 420     virtual void run() override;
 421
 422     bool requires_GPS() const override { return false; }
 423     bool has_manual_throttle() const override { return true; }
 424     bool allows_arming(AP_Arming::Method method) const override { return true; };
 425     bool is_autopilot() const override { return false; }
 426     bool init(bool ignore_checks) override;
 427     void exit() override;
 428     // whether an air-mode aux switch has been toggled
 429     void air_mode_aux_changed();
 430     bool allows_save_trim() const override { return true; }
 431     bool allows_flip() const override { return true; }
 432     bool crash_check_enabled() const override { return false; }
 433
 434 protected:
 435
 436     const char *name() const override { return "ACRO"; }
 437     const char *name4() const override { return "ACRO"; }
 438
 439     // get_pilot_desired_angle_rates - transform pilot's normalised roll pitch and yaw input into a desired lean angle rates
 440     // inputs are -1 to 1 and the function returns desired angle rates in centi-degrees-per-second
 441     void get_pilot_desired_angle_rates(float roll_in, float pitch_in, float yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
 442
 443     float throttle_hover() const override;
 444
 445 private:
 446     bool disable_air_mode_reset;
 447 };
 448 #endif
 449
 450 #if FRAME_CONFIG == HELI_FRAME
 451 class ModeAcro_Heli : public ModeAcro {
 452
 453 public:
 454     // inherit constructor
 455     using ModeAcro::Mode;
 456
 457     bool init(bool ignore_checks) override;
 458     void run() override;
 459     void virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak);
 460
 461 protected:
 462 private:
 463 };
 464 #endif
 465
 466
 467 class ModeAltHold : public Mode {
 468
 469 public:
 470     // inherit constructor
 471     using Mode::Mode;
 472     Number mode_number() const override { return Number::ALT_HOLD; }
 473
 474     bool init(bool ignore_checks) override;
 475     void run() override;
 476
 477     bool requires_GPS() const override { return false; }
 478     bool has_manual_throttle() const override { return false; }
 479     bool allows_arming(AP_Arming::Method method) const override { return true; };
 480     bool is_autopilot() const override { return false; }
 481     bool has_user_takeoff(bool must_navigate) const override {
 482         return !must_navigate;
 483     }
 484     bool allows_autotune() const override { return true; }
 485     bool allows_flip() const override { return true; }
 486 #if FRAME_CONFIG == HELI_FRAME
 487     bool allows_inverted() const override { return true; };
 488 #endif
 489 protected:
 490
 491     const char *name() const override { return "ALT_HOLD"; }
 492     const char *name4() const override { return "ALTH"; }
 493
 494 private:
 495
 496 };
 497
 498 class ModeAuto : public Mode {
 499
 500 public:
 501     friend class PayloadPlace;  // in case wp_run is accidentally required
 502
 503     // inherit constructor
 504     using Mode::Mode;
 505     Number mode_number() const override { return auto_RTL? Number::AUTO_RTL : Number::AUTO; }
 506
 507     bool init(bool ignore_checks) override;
 508     void exit() override;
 509     void run() override;
 510
 511     bool requires_GPS() const override;
 512     bool has_manual_throttle() const override { return false; }
 513     bool allows_arming(AP_Arming::Method method) const override;
 514     bool is_autopilot() const override { return true; }
 515     bool in_guided_mode() const override { return _mode == SubMode::NAVGUIDED || _mode == SubMode::NAV_SCRIPT_TIME; }
 516 #if FRAME_CONFIG == HELI_FRAME
 517     bool allows_inverted() const override { return true; };
 518 #endif
 519
 520 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
 521     // Return the type of this mode for use by advanced failsafe
 522     AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
 523 #endif
 524
 525     // Return true if the throttle high arming check can be skipped when arming from GCS or Scripting
 526     bool allows_GCS_or_SCR_arming_with_throttle_high() const override { return true; }
 527
 528     // Auto modes
 529     enum class SubMode : uint8_t {
 530         TAKEOFF,
 531         WP,
 532         LAND,
 533         RTL,
 534         CIRCLE_MOVE_TO_EDGE,
 535         CIRCLE,
 536         NAVGUIDED,
 537         LOITER,
 538         LOITER_TO_ALT,
 539 #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED
 540         NAV_PAYLOAD_PLACE,
 541 #endif
 542         NAV_SCRIPT_TIME,
 543         NAV_ATTITUDE_TIME,
 544     };
 545
 546     // set submode.  returns true on success, false on failure
 547     void set_submode(SubMode new_submode);
 548
 549     // pause continue in auto mode
 550     bool pause() override;
 551     bool resume() override;
 552     bool paused() const;
 553
 554     bool loiter_start();
 555     void rtl_start();
 556     void takeoff_start(const Location& dest_loc);
 557     bool wp_start(const Location& dest_loc);
 558     void land_start();
 559     void circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn);
 560     void circle_start();
 561     void nav_guided_start();
 562
 563     bool is_landing() const override;
 564
 565     bool is_taking_off() const override;
 566     bool use_pilot_yaw() const override;
 567
 568     bool set_speed_xy(float speed_xy_cms) override;
 569     bool set_speed_up(float speed_up_cms) override;
 570     bool set_speed_down(float speed_down_cms) override;
 571
 572     bool requires_terrain_failsafe() const override { return true; }
 573
 574     void payload_place_start();
 575
 576     // for GCS_MAVLink to call:
 577     bool do_guided(const AP_Mission::Mission_Command& cmd);
 578
 579     // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
 580     bool jump_to_landing_sequence_auto_RTL(ModeReason reason);
 581
 582     // Join mission after DO_RETURN_PATH_START waypoint, if succeeds pretend to be Auto RTL mode
 583     bool return_path_start_auto_RTL(ModeReason reason);
 584
 585     // Try join return path else do land start
 586     bool return_path_or_jump_to_landing_sequence_auto_RTL(ModeReason reason);
 587
 588     // lua accessors for nav script time support
 589     bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4);
 590     void nav_script_time_done(uint16_t id);
 591
 592     AP_Mission mission{
 593         FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &),
 594         FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
 595         FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)};
 596
 597     // Mission change detector
 598     AP_Mission_ChangeDetector mis_change_detector;
 599
 600     // true if weathervaning is allowed in auto
 601 #if WEATHERVANE_ENABLED
 602     bool allows_weathervaning(void) const override;
 603 #endif
 604
 605 protected:
 606
 607     const char *name() const override { return auto_RTL? "AUTO RTL" : "AUTO"; }
 608     const char *name4() const override { return auto_RTL? "ARTL" : "AUTO"; }
 609
 610     uint32_t wp_distance() const override;
 611     int32_t wp_bearing() const override;
 612     float crosstrack_error() const override { return wp_nav->crosstrack_error();}
 613     bool get_wp(Location &loc) const override;
 614
 615 private:
 616
 617     enum class Option : int32_t {
 618         AllowArming                        = (1 << 0U),
 619         AllowTakeOffWithoutRaisingThrottle = (1 << 1U),
 620         IgnorePilotYaw                     = (1 << 2U),
 621         AllowWeatherVaning                 = (1 << 7U),
 622     };
 623     bool option_is_enabled(Option option) const;
 624
 625     // Enter auto rtl pseudo mode
 626     bool enter_auto_rtl(ModeReason reason);
 627
 628     bool start_command(const AP_Mission::Mission_Command& cmd);
 629     bool verify_command(const AP_Mission::Mission_Command& cmd);
 630     void exit_mission();
 631
 632     bool check_for_mission_change();    // detect external changes to mission
 633
 634     void takeoff_run();
 635     void wp_run();
 636     void land_run();
 637     void rtl_run();
 638     void circle_run();
 639     void nav_guided_run();
 640     void loiter_run();
 641     void loiter_to_alt_run();
 642     void nav_attitude_time_run();
 643
 644     // return the Location portion of a command.  If the command's lat and lon and/or alt are zero the default_loc's lat,lon and/or alt are returned instead
 645     Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const;
 646
 647     SubMode _mode = SubMode::TAKEOFF;   // controls which auto controller is run
 648
 649     bool shift_alt_to_current_alt(Location& target_loc) const;
 650
 651     // subtract position controller offsets from target location
 652     // should be used when the location will be used as a target for the position controller
 653     void subtract_pos_offsets(Location& target_loc) const;
 654
 655     void do_takeoff(const AP_Mission::Mission_Command& cmd);
 656     void do_nav_wp(const AP_Mission::Mission_Command& cmd);
 657     bool set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc);
 658     void do_land(const AP_Mission::Mission_Command& cmd);
 659     void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
 660     void do_circle(const AP_Mission::Mission_Command& cmd);
 661     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
 662     void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
 663     void do_spline_wp(const AP_Mission::Mission_Command& cmd);
 664     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);
 665 #if AC_NAV_GUIDED
 666     void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
 667     void do_guided_limits(const AP_Mission::Mission_Command& cmd);
 668 #endif
 669     void do_nav_delay(const AP_Mission::Mission_Command& cmd);
 670     void do_wait_delay(const AP_Mission::Mission_Command& cmd);
 671     void do_within_distance(const AP_Mission::Mission_Command& cmd);
 672     void do_yaw(const AP_Mission::Mission_Command& cmd);
 673     void do_change_speed(const AP_Mission::Mission_Command& cmd);
 674     void do_set_home(const AP_Mission::Mission_Command& cmd);
 675     void do_roi(const AP_Mission::Mission_Command& cmd);
 676     void do_mount_control(const AP_Mission::Mission_Command& cmd);
 677 #if HAL_PARACHUTE_ENABLED
 678     void do_parachute(const AP_Mission::Mission_Command& cmd);
 679 #endif
 680 #if AP_WINCH_ENABLED
 681     void do_winch(const AP_Mission::Mission_Command& cmd);
 682 #endif
 683     void do_payload_place(const AP_Mission::Mission_Command& cmd);
 684     void do_RTL(void);
 685 #if AP_SCRIPTING_ENABLED
 686     void do_nav_script_time(const AP_Mission::Mission_Command& cmd);
 687 #endif
 688     void do_nav_attitude_time(const AP_Mission::Mission_Command& cmd);
 689
 690     bool verify_takeoff();
 691     bool verify_land();
 692     bool verify_payload_place();
 693     bool verify_loiter_unlimited();
 694     bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
 695     bool verify_loiter_to_alt() const;
 696     bool verify_RTL();
 697     bool verify_wait_delay();
 698     bool verify_within_distance();
 699     bool verify_yaw();
 700     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
 701     bool verify_circle(const AP_Mission::Mission_Command& cmd);
 702     bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
 703 #if AC_NAV_GUIDED
 704     bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
 705 #endif
 706     bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
 707 #if AP_SCRIPTING_ENABLED
 708     bool verify_nav_script_time();
 709 #endif
 710     bool verify_nav_attitude_time(const AP_Mission::Mission_Command& cmd);
 711
 712     // Loiter control
 713     uint16_t loiter_time_max;                // How long we should stay in Loiter Mode for mission scripting (time in seconds)
 714     uint32_t loiter_time;                    // How long have we been loitering - The start time in millis
 715
 716     struct {
 717         bool reached_destination_xy : 1;
 718         bool loiter_start_done : 1;
 719         bool reached_alt : 1;
 720         float alt_error_cm;
 721         int32_t alt;
 722     } loiter_to_alt;
 723
 724     // Delay the next navigation command
 725     uint32_t nav_delay_time_max_ms;  // used for delaying the navigation commands (eg land,takeoff etc.)
 726     uint32_t nav_delay_time_start_ms;
 727
 728     // Delay Mission Scripting Command
 729     int32_t condition_value;  // used in condition commands (eg delay, change alt, etc.)
 730     uint32_t condition_start;
 731
 732     // Land within Auto state
 733     enum class State {
 734         FlyToLocation = 0,
 735         Descending = 1
 736     };
 737     State state = State::FlyToLocation;
 738
 739     bool waiting_to_start;  // true if waiting for vehicle to be armed or EKF origin before starting mission
 740
 741     // True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode
 742     bool auto_RTL;
 743
 744 #if AP_SCRIPTING_ENABLED
 745     // nav_script_time command variables
 746     struct {
 747         bool done;          // true once lua script indicates it has completed
 748         uint16_t id;        // unique id to avoid race conditions between commands and lua scripts
 749         uint32_t start_ms;  // system time nav_script_time command was received (used for timeout)
 750         uint8_t command;    // command number provided by mission command
 751         uint8_t timeout_s;  // timeout (in seconds) provided by mission command
 752         float arg1;         // 1st argument provided by mission command
 753         float arg2;         // 2nd argument provided by mission command
 754         int16_t arg3;       // 3rd argument provided by mission command
 755         int16_t arg4;       // 4th argument provided by mission command
 756     } nav_scripting;
 757 #endif
 758
 759     // nav attitude time command variables
 760     struct {
 761         int16_t roll_deg;   // target roll angle in degrees.  provided by mission command
 762         int8_t pitch_deg;   // target pitch angle in degrees.  provided by mission command
 763         int16_t yaw_deg;    // target yaw angle in degrees.  provided by mission command
 764         float climb_rate;   // climb rate in m/s. provided by mission command
 765         uint32_t start_ms;  // system time that nav attitude time command was received (used for timeout)
 766     } nav_attitude_time;
 767
 768     // desired speeds
 769     struct {
 770         float xy;     // desired speed horizontally in m/s. 0 if unset
 771         float up;     // desired speed upwards in m/s. 0 if unset
 772         float down;   // desired speed downwards in m/s. 0 if unset
 773     } desired_speed_override;
 774 };
 775
 776 #if AUTOTUNE_ENABLED
 777 /*
 778   wrapper class for AC_AutoTune
 779  */
 780
 781 #if FRAME_CONFIG == HELI_FRAME
 782 class AutoTune : public AC_AutoTune_Heli
 783 #else
 784 class AutoTune : public AC_AutoTune_Multi
 785 #endif
 786 {
 787 public:
 788     bool init() override;
 789     void run() override;
 790
 791 protected:
 792     bool position_ok() override;
 793     float get_pilot_desired_climb_rate_cms(void) const override;
 794     void get_pilot_desired_rp_yrate_cd(float &roll_cd, float &pitch_cd, float &yaw_rate_cds) override;
 795     void init_z_limits() override;
 796 #if HAL_LOGGING_ENABLED
 797     void log_pids() override;
 798 #endif
 799 };
 800
 801 class ModeAutoTune : public Mode {
 802
 803     // ParametersG2 sets a pointer within our autotune object:
 804     friend class ParametersG2;
 805
 806 public:
 807     // inherit constructor
 808     using Mode::Mode;
 809     Number mode_number() const override { return Number::AUTOTUNE; }
 810
 811     bool init(bool ignore_checks) override;
 812     void exit() override;
 813     void run() override;
 814
 815     bool requires_GPS() const override { return false; }
 816     bool has_manual_throttle() const override { return false; }
 817     bool allows_arming(AP_Arming::Method method) const override { return false; }
 818     bool is_autopilot() const override { return false; }
 819
 820     AutoTune autotune;
 821
 822 protected:
 823
 824     const char *name() const override { return "AUTOTUNE"; }
 825     const char *name4() const override { return "ATUN"; }
 826 };
 827 #endif
 828
 829
 830 class ModeBrake : public Mode {
 831
 832 public:
 833     // inherit constructor
 834     using Mode::Mode;
 835     Number mode_number() const override { return Number::BRAKE; }
 836
 837     bool init(bool ignore_checks) override;
 838     void run() override;
 839
 840     bool requires_GPS() const override { return true; }
 841     bool has_manual_throttle() const override { return false; }
 842     bool allows_arming(AP_Arming::Method method) const override { return false; };
 843     bool is_autopilot() const override { return false; }
 844
 845     void timeout_to_loiter_ms(uint32_t timeout_ms);
 846
 847 protected:
 848
 849     const char *name() const override { return "BRAKE"; }
 850     const char *name4() const override { return "BRAK"; }
 851
 852 private:
 853
 854     uint32_t _timeout_start;
 855     uint32_t _timeout_ms;
 856
 857 };
 858
 859
 860 class ModeCircle : public Mode {
 861
 862 public:
 863     // inherit constructor
 864     using Mode::Mode;
 865     Number mode_number() const override { return Number::CIRCLE; }
 866
 867     bool init(bool ignore_checks) override;
 868     void run() override;
 869
 870     bool requires_GPS() const override { return true; }
 871     bool has_manual_throttle() const override { return false; }
 872     bool allows_arming(AP_Arming::Method method) const override { return false; };
 873     bool is_autopilot() const override { return true; }
 874
 875 protected:
 876
 877     const char *name() const override { return "CIRCLE"; }
 878     const char *name4() const override { return "CIRC"; }
 879
 880     uint32_t wp_distance() const override;
 881     int32_t wp_bearing() const override;
 882
 883 private:
 884
 885     // Circle
 886     bool speed_changing = false;     // true when the roll stick is being held to facilitate stopping at 0 rate
 887 };
 888
 889
 890 class ModeDrift : public Mode {
 891
 892 public:
 893     // inherit constructor
 894     using Mode::Mode;
 895     Number mode_number() const override { return Number::DRIFT; }
 896
 897     bool init(bool ignore_checks) override;
 898     void run() override;
 899
 900     bool requires_GPS() const override { return true; }
 901     bool has_manual_throttle() const override { return false; }
 902     bool allows_arming(AP_Arming::Method method) const override { return true; };
 903     bool is_autopilot() const override { return false; }
 904
 905 protected:
 906
 907     const char *name() const override { return "DRIFT"; }
 908     const char *name4() const override { return "DRIF"; }
 909
 910 private:
 911
 912     float get_throttle_assist(float velz, float pilot_throttle_scaled);
 913
 914 };
 915
 916
 917 class ModeFlip : public Mode {
 918
 919 public:
 920     // inherit constructor
 921     using Mode::Mode;
 922     Number mode_number() const override { return Number::FLIP; }
 923
 924     bool init(bool ignore_checks) override;
 925     void run() override;
 926
 927     bool requires_GPS() const override { return false; }
 928     bool has_manual_throttle() const override { return false; }
 929     bool allows_arming(AP_Arming::Method method) const override { return false; };
 930     bool is_autopilot() const override { return false; }
 931     bool crash_check_enabled() const override { return false; }
 932
 933 protected:
 934
 935     const char *name() const override { return "FLIP"; }
 936     const char *name4() const override { return "FLIP"; }
 937
 938 private:
 939
 940     // Flip
 941     Vector3f orig_attitude;         // original vehicle attitude before flip
 942
 943     enum class FlipState : uint8_t {
 944         Start,
 945         Roll,
 946         Pitch_A,
 947         Pitch_B,
 948         Recover,
 949         Abandon
 950     };
 951     FlipState _state;               // current state of flip
 952     Mode::Number   orig_control_mode;   // flight mode when flip was initated
 953     uint32_t  start_time_ms;          // time since flip began
 954     int8_t    roll_dir;            // roll direction (-1 = roll left, 1 = roll right)
 955     int8_t    pitch_dir;           // pitch direction (-1 = pitch forward, 1 = pitch back)
 956 };
 957
 958
 959 #if MODE_FLOWHOLD_ENABLED
 960 /*
 961   class to support FLOWHOLD mode, which is a position hold mode using
 962   optical flow directly, avoiding the need for a rangefinder
 963  */
 964
 965 class ModeFlowHold : public Mode {
 966 public:
 967     // need a constructor for parameters
 968     ModeFlowHold(void);
 969     Number mode_number() const override { return Number::FLOWHOLD; }
 970
 971     bool init(bool ignore_checks) override;
 972     void run(void) override;
 973
 974     bool requires_GPS() const override { return false; }
 975     bool has_manual_throttle() const override { return false; }
 976     bool allows_arming(AP_Arming::Method method) const override { return true; };
 977     bool is_autopilot() const override { return false; }
 978     bool has_user_takeoff(bool must_navigate) const override {
 979         return !must_navigate;
 980     }
 981     bool allows_flip() const override { return true; }
 982
 983     static const struct AP_Param::GroupInfo var_info[];
 984
 985 protected:
 986     const char *name() const override { return "FLOWHOLD"; }
 987     const char *name4() const override { return "FHLD"; }
 988
 989 private:
 990
 991     // FlowHold states
 992     enum FlowHoldModeState {
 993         FlowHold_MotorStopped,
 994         FlowHold_Takeoff,
 995         FlowHold_Flying,
 996         FlowHold_Landed
 997     };
 998
 999     // calculate attitude from flow data
1000     void flow_to_angle(Vector2f &bf_angle);
1001
1002     LowPassFilterConstDtVector2f flow_filter;
1003
1004     bool flowhold_init(bool ignore_checks);
1005     void flowhold_run();
1006     void flowhold_flow_to_angle(Vector2f &angle, bool stick_input);
1007     void update_height_estimate(void);
1008
1009     // minimum assumed height
1010     const float height_min = 0.1f;
1011
1012     // maximum scaling height
1013     const float height_max = 3.0f;
1014
1015     AP_Float flow_max;
1016     AC_PI_2D flow_pi_xy{0.2f, 0.3f, 3000, 5, 0.0025f};
1017     AP_Float flow_filter_hz;
1018     AP_Int8  flow_min_quality;
1019     AP_Int8  brake_rate_dps;
1020
1021     float quality_filtered;
1022
1023     uint8_t log_counter;
1024     bool limited;
1025     Vector2f xy_I;
1026
1027     // accumulated INS delta velocity in north-east form since last flow update
1028     Vector2f delta_velocity_ne;
1029
1030     // last flow rate in radians/sec in north-east axis
1031     Vector2f last_flow_rate_rps;
1032
1033     // timestamp of last flow data
1034     uint32_t last_flow_ms;
1035
1036     float last_ins_height;
1037     float height_offset;
1038
1039     // are we braking after pilot input?
1040     bool braking;
1041
1042     // last time there was significant stick input
1043     uint32_t last_stick_input_ms;
1044 };
1045 #endif // MODE_FLOWHOLD_ENABLED
1046
1047
1048 class ModeGuided : public Mode {
1049
1050 public:
1051 #if AP_EXTERNAL_CONTROL_ENABLED
1052     friend class AP_ExternalControl_Copter;
1053 #endif
1054
1055     // inherit constructor
1056     using Mode::Mode;
1057     Number mode_number() const override { return Number::GUIDED; }
1058
1059     bool init(bool ignore_checks) override;
1060     void run() override;
1061
1062     bool requires_GPS() const override { return true; }
1063     bool has_manual_throttle() const override { return false; }
1064     bool allows_arming(AP_Arming::Method method) const override;
1065     bool is_autopilot() const override { return true; }
1066     bool has_user_takeoff(bool must_navigate) const override { return true; }
1067     bool in_guided_mode() const override { return true; }
1068
1069     bool requires_terrain_failsafe() const override { return true; }
1070
1071 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
1072     // Return the type of this mode for use by advanced failsafe
1073     AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
1074 #endif
1075
1076     // Return true if the throttle high arming check can be skipped when arming from GCS or Scripting
1077     bool allows_GCS_or_SCR_arming_with_throttle_high() const override { return true; }
1078
1079     // Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
1080     // attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
1081     //                IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
1082     // ang_vel: angular velocity (rad/s)
1083     // climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless
1084     // use_thrust: IF true: climb_rate_cms_or_thrust represents thrust
1085     //             IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
1086     void set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust);
1087
1088     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);
1089     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);
1090     bool get_wp(Location &loc) const override;
1091     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);
1092     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);
1093     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);
1094     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);
1095     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);
1096
1097     // get position, velocity and acceleration targets
1098     const Vector3p& get_target_pos() const;
1099     const Vector3f& get_target_vel() const;
1100     const Vector3f& get_target_accel() const;
1101
1102     // returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
1103     bool set_attitude_target_provides_thrust() const;
1104     bool stabilizing_pos_xy() const;
1105     bool stabilizing_vel_xy() const;
1106     bool use_wpnav_for_position_control() const;
1107
1108     void limit_clear();
1109     void limit_init_time_and_pos();
1110     void limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
1111     bool limit_check();
1112
1113     bool is_taking_off() const override;
1114
1115     bool set_speed_xy(float speed_xy_cms) override;
1116     bool set_speed_up(float speed_up_cms) override;
1117     bool set_speed_down(float speed_down_cms) override;
1118
1119     // initialises position controller to implement take-off
1120     // takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
1121     bool do_user_takeoff_start(float takeoff_alt_cm) override;
1122
1123     enum class SubMode {
1124         TakeOff,
1125         WP,
1126         Pos,
1127         PosVelAccel,
1128         VelAccel,
1129         Accel,
1130         Angle,
1131     };
1132
1133     SubMode submode() const { return guided_mode; }
1134
1135     void angle_control_start();
1136     void angle_control_run();
1137
1138     // return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rate control
1139     uint32_t get_timeout_ms() const;
1140
1141     bool use_pilot_yaw() const override;
1142
1143     // pause continue in guided mode
1144     bool pause() override;
1145     bool resume() override;
1146
1147     // true if weathervaning is allowed in guided
1148 #if WEATHERVANE_ENABLED
1149     bool allows_weathervaning(void) const override;
1150 #endif
1151
1152 protected:
1153
1154     const char *name() const override { return "GUIDED"; }
1155     const char *name4() const override { return "GUID"; }
1156
1157     uint32_t wp_distance() const override;
1158     int32_t wp_bearing() const override;
1159     float crosstrack_error() const override;
1160
1161 private:
1162
1163     // enum for GUID_OPTIONS parameter
1164     enum class Option : uint32_t {
1165         AllowArmingFromTX   = (1U << 0),
1166         // this bit is still available, pilot yaw was mapped to bit 2 for symmetry with auto
1167         IgnorePilotYaw      = (1U << 2),
1168         SetAttitudeTarget_ThrustAsThrust = (1U << 3),
1169         DoNotStabilizePositionXY = (1U << 4),
1170         DoNotStabilizeVelocityXY = (1U << 5),
1171         WPNavUsedForPosControl = (1U << 6),
1172         AllowWeatherVaning = (1U << 7)
1173     };
1174
1175     // returns true if the Guided-mode-option is set (see GUID_OPTIONS)
1176     bool option_is_enabled(Option option) const;
1177
1178     // wp controller
1179     void wp_control_start();
1180     void wp_control_run();
1181
1182     void pva_control_start();
1183     void pos_control_start();
1184     void accel_control_start();
1185     void velaccel_control_start();
1186     void posvelaccel_control_start();
1187     void takeoff_run();
1188     void pos_control_run();
1189     void accel_control_run();
1190     void velaccel_control_run();
1191     void pause_control_run();
1192     void posvelaccel_control_run();
1193     void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
1194
1195     // controls which controller is run (pos or vel):
1196     static SubMode guided_mode;
1197     static bool send_notification;     // used to send one time notification to ground station
1198     static bool takeoff_complete;      // true once takeoff has completed (used to trigger retracting of landing gear)
1199
1200     // guided mode is paused or not
1201     static bool _paused;
1202 };
1203
1204 #if AP_SCRIPTING_ENABLED
1205 // Mode which behaves as guided with custom mode number and name
1206 class ModeGuidedCustom : public ModeGuided {
1207 public:
1208     // constructor registers custom number and names
1209     ModeGuidedCustom(const Number _number, const char* _full_name, const char* _short_name);
1210
1211     bool init(bool ignore_checks) override;
1212
1213     Number mode_number() const override { return number; }
1214
1215     const char *name() const override { return full_name; }
1216     const char *name4() const override { return short_name; }
1217
1218     // State object which can be edited by scripting
1219     AP_Vehicle::custom_mode_state state;
1220
1221 private:
1222     const Number number;
1223     const char* full_name;
1224     const char* short_name;
1225 };
1226 #endif
1227
1228 class ModeGuidedNoGPS : public ModeGuided {
1229
1230 public:
1231     // inherit constructor
1232     using ModeGuided::Mode;
1233     Number mode_number() const override { return Number::GUIDED_NOGPS; }
1234
1235     bool init(bool ignore_checks) override;
1236     void run() override;
1237
1238     bool requires_GPS() const override { return false; }
1239     bool has_manual_throttle() const override { return false; }
1240     bool is_autopilot() const override { return true; }
1241
1242 protected:
1243
1244     const char *name() const override { return "GUIDED_NOGPS"; }
1245     const char *name4() const override { return "GNGP"; }
1246
1247 private:
1248
1249 };
1250
1251
1252 class ModeLand : public Mode {
1253
1254 public:
1255     // inherit constructor
1256     using Mode::Mode;
1257     Number mode_number() const override { return Number::LAND; }
1258
1259     bool init(bool ignore_checks) override;
1260     void run() override;
1261
1262     bool requires_GPS() const override { return false; }
1263     bool has_manual_throttle() const override { return false; }
1264     bool allows_arming(AP_Arming::Method method) const override { return false; };
1265     bool is_autopilot() const override { return true; }
1266
1267     bool is_landing() const override { return true; };
1268
1269 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
1270     // Return the type of this mode for use by advanced failsafe
1271     AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
1272 #endif
1273
1274     void do_not_use_GPS();
1275
1276     // returns true if LAND mode is trying to control X/Y position
1277     bool controlling_position() const { return control_position; }
1278
1279     void set_land_pause(bool new_value) { land_pause = new_value; }
1280
1281 protected:
1282
1283     const char *name() const override { return "LAND"; }
1284     const char *name4() const override { return "LAND"; }
1285
1286 private:
1287
1288     void gps_run();
1289     void nogps_run();
1290
1291     bool control_position; // true if we are using an external reference to control position
1292
1293     uint32_t land_start_time;
1294     bool land_pause;
1295 };
1296
1297
1298 class ModeLoiter : public Mode {
1299
1300 public:
1301     // inherit constructor
1302     using Mode::Mode;
1303     Number mode_number() const override { return Number::LOITER; }
1304
1305     bool init(bool ignore_checks) override;
1306     void run() override;
1307
1308     bool requires_GPS() const override { return true; }
1309     bool has_manual_throttle() const override { return false; }
1310     bool allows_arming(AP_Arming::Method method) const override { return true; };
1311     bool is_autopilot() const override { return false; }
1312     bool has_user_takeoff(bool must_navigate) const override { return true; }
1313     bool allows_autotune() const override { return true; }
1314
1315 #if FRAME_CONFIG == HELI_FRAME
1316     bool allows_inverted() const override { return true; };
1317 #endif
1318
1319 #if AC_PRECLAND_ENABLED
1320     void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; }
1321 #endif
1322
1323 protected:
1324
1325     const char *name() const override { return "LOITER"; }
1326     const char *name4() const override { return "LOIT"; }
1327
1328     uint32_t wp_distance() const override;
1329     int32_t wp_bearing() const override;
1330     float crosstrack_error() const override { return pos_control->crosstrack_error();}
1331
1332 #if AC_PRECLAND_ENABLED
1333     bool do_precision_loiter();
1334     void precision_loiter_xy();
1335 #endif
1336
1337 private:
1338
1339 #if AC_PRECLAND_ENABLED
1340     bool _precision_loiter_enabled;
1341     bool _precision_loiter_active; // true if user has switched on prec loiter
1342 #endif
1343
1344 };
1345
1346
1347 class ModePosHold : public Mode {
1348
1349 public:
1350     // inherit constructor
1351     using Mode::Mode;
1352     Number mode_number() const override { return Number::POSHOLD; }
1353
1354     bool init(bool ignore_checks) override;
1355     void run() override;
1356
1357     bool requires_GPS() const override { return true; }
1358     bool has_manual_throttle() const override { return false; }
1359     bool allows_arming(AP_Arming::Method method) const override { return true; };
1360     bool is_autopilot() const override { return false; }
1361     bool has_user_takeoff(bool must_navigate) const override { return true; }
1362     bool allows_autotune() const override { return true; }
1363
1364 protected:
1365
1366     const char *name() const override { return "POSHOLD"; }
1367     const char *name4() const override { return "PHLD"; }
1368
1369 private:
1370
1371     void update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw);
1372     float mix_controls(float mix_ratio, float first_control, float second_control);
1373     void update_brake_angle_from_velocity(float &brake_angle, float velocity);
1374     void init_wind_comp_estimate();
1375     void update_wind_comp_estimate();
1376     void get_wind_comp_lean_angles(float &roll_angle, float &pitch_angle);
1377     void roll_controller_to_pilot_override();
1378     void pitch_controller_to_pilot_override();
1379
1380     enum class RPMode {
1381         PILOT_OVERRIDE=0,            // pilot is controlling this axis (i.e. roll or pitch)
1382         BRAKE,                       // this axis is braking towards zero
1383         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)
1384         BRAKE_TO_LOITER,             // both vehicle's axis (roll and pitch) are transitioning from braking to loiter mode (braking and loiter controls are mixed)
1385         LOITER,                      // both vehicle axis are holding position
1386         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)
1387     };
1388
1389     RPMode roll_mode;
1390     RPMode pitch_mode;
1391
1392     // pilot input related variables
1393     float pilot_roll;                         // pilot requested roll angle (filtered to slow returns to zero)
1394     float pilot_pitch;                        // pilot requested roll angle (filtered to slow returns to zero)
1395
1396     // braking related variables
1397     struct {
1398         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
1399         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
1400
1401         float gain;                         // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from rate)
1402         float roll;                         // target roll angle during braking periods
1403         float pitch;                        // target pitch angle during braking periods
1404         int16_t timeout_roll;               // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
1405         int16_t timeout_pitch;              // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
1406         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
1407         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
1408         int16_t to_loiter_timer;            // cycles to mix brake and loiter controls in POSHOLD_TO_LOITER
1409     } brake;
1410
1411     // loiter related variables
1412     int16_t controller_to_pilot_timer_roll;     // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
1413     int16_t controller_to_pilot_timer_pitch;    // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
1414     float controller_final_roll;                // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
1415     float controller_final_pitch;               // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
1416
1417     // wind compensation related variables
1418     Vector2f wind_comp_ef;                      // wind compensation in earth frame, filtered lean angles from position controller
1419     float wind_comp_roll;                       // roll angle to compensate for wind
1420     float wind_comp_pitch;                      // pitch angle to compensate for wind
1421     uint16_t wind_comp_start_timer;             // counter to delay start of wind compensation for a short time after loiter is engaged
1422     int8_t  wind_comp_timer;                    // counter to reduce wind comp roll/pitch lean angle calcs to 10hz
1423
1424     // final output
1425     float roll;   // final roll angle sent to attitude controller
1426     float pitch;  // final pitch angle sent to attitude controller
1427
1428 };
1429
1430
1431 class ModeRTL : public Mode {
1432
1433 public:
1434     // inherit constructor
1435     using Mode::Mode;
1436     Number mode_number() const override { return Number::RTL; }
1437
1438     bool init(bool ignore_checks) override;
1439     void run() override {
1440         return run(true);
1441     }
1442     void run(bool disarm_on_land);
1443
1444     bool requires_GPS() const override { return true; }
1445     bool has_manual_throttle() const override { return false; }
1446     bool allows_arming(AP_Arming::Method method) const override { return false; };
1447     bool is_autopilot() const override { return true; }
1448
1449     bool requires_terrain_failsafe() const override { return true; }
1450
1451 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
1452     // Return the type of this mode for use by advanced failsafe
1453     AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
1454 #endif
1455
1456     // for reporting to GCS
1457     bool get_wp(Location &loc) const override;
1458
1459     bool use_pilot_yaw() const override;
1460
1461     bool set_speed_xy(float speed_xy_cms) override;
1462     bool set_speed_up(float speed_up_cms) override;
1463     bool set_speed_down(float speed_down_cms) override;
1464
1465     // RTL states
1466     enum class SubMode : uint8_t {
1467         STARTING,
1468         INITIAL_CLIMB,
1469         RETURN_HOME,
1470         LOITER_AT_HOME,
1471         FINAL_DESCENT,
1472         LAND
1473     };
1474     SubMode state() { return _state; }
1475
1476     // this should probably not be exposed
1477     bool state_complete() const { return _state_complete; }
1478
1479     virtual bool is_landing() const override;
1480
1481     void restart_without_terrain();
1482
1483     // enum for RTL_ALT_TYPE parameter
1484     enum class RTLAltType : int8_t {
1485         RELATIVE = 0,
1486         TERRAIN = 1
1487     };
1488     ModeRTL::RTLAltType get_alt_type() const;
1489
1490 protected:
1491
1492     const char *name() const override { return "RTL"; }
1493     const char *name4() const override { return "RTL "; }
1494
1495     // for reporting to GCS
1496     uint32_t wp_distance() const override;
1497     int32_t wp_bearing() const override;
1498     float crosstrack_error() const override { return wp_nav->crosstrack_error();}
1499
1500     void descent_start();
1501     void descent_run();
1502     void land_start();
1503     void land_run(bool disarm_on_land);
1504
1505     void set_descent_target_alt(uint32_t alt) { rtl_path.descent_target.alt = alt; }
1506
1507 private:
1508
1509     void climb_start();
1510     void return_start();
1511     void climb_return_run();
1512     void loiterathome_start();
1513     void loiterathome_run();
1514     void build_path();
1515     void compute_return_target();
1516
1517     SubMode _state = SubMode::INITIAL_CLIMB;  // records state of rtl (initial climb, returning home, etc)
1518     bool _state_complete = false; // set to true if the current state is completed
1519
1520     struct {
1521         // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain
1522         Location origin_point;
1523         Location climb_target;
1524         Location return_target;
1525         Location descent_target;
1526         bool land;
1527     } rtl_path;
1528
1529     // return target alt type
1530     enum class ReturnTargetAltType {
1531         RELATIVE = 0,
1532         RANGEFINDER = 1,
1533         TERRAINDATABASE = 2
1534     };
1535
1536     // Loiter timer - Records how long we have been in loiter
1537     uint32_t _loiter_start_time;
1538
1539     bool terrain_following_allowed;
1540
1541     // enum for RTL_OPTIONS parameter
1542     enum class Options : int32_t {
1543         // First pair of bits are still available, pilot yaw was mapped to bit 2 for symmetry with auto
1544         IgnorePilotYaw    = (1U << 2),
1545     };
1546
1547 };
1548
1549
1550 class ModeSmartRTL : public ModeRTL {
1551
1552 public:
1553     // inherit constructor
1554     using ModeRTL::Mode;
1555     Number mode_number() const override { return Number::SMART_RTL; }
1556
1557     bool init(bool ignore_checks) override;
1558     void run() override;
1559
1560     bool requires_GPS() const override { return true; }
1561     bool has_manual_throttle() const override { return false; }
1562     bool allows_arming(AP_Arming::Method method) const override { return false; }
1563     bool is_autopilot() const override { return true; }
1564
1565     void save_position();
1566     void exit() override;
1567
1568     bool is_landing() const override;
1569     bool use_pilot_yaw() const override;
1570
1571     // Safe RTL states
1572     enum class SubMode : uint8_t {
1573         WAIT_FOR_PATH_CLEANUP,
1574         PATH_FOLLOW,
1575         PRELAND_POSITION,
1576         DESCEND,
1577         LAND
1578     };
1579
1580 protected:
1581
1582     const char *name() const override { return "SMARTRTL"; }
1583     const char *name4() const override { return "SRTL"; }
1584
1585     // for reporting to GCS
1586     bool get_wp(Location &loc) const override;
1587     uint32_t wp_distance() const override;
1588     int32_t wp_bearing() const override;
1589     float crosstrack_error() const override { return wp_nav->crosstrack_error();}
1590
1591 private:
1592
1593     void wait_cleanup_run();
1594     void path_follow_run();
1595     void pre_land_position_run();
1596     void land();
1597     SubMode smart_rtl_state = SubMode::PATH_FOLLOW;
1598
1599     // keep track of how long we have failed to get another return
1600     // point while following our path home.  If we take too long we
1601     // may choose to land the vehicle.
1602     uint32_t path_follow_last_pop_fail_ms;
1603
1604     // backup last popped point so that it can be restored to the path
1605     // if vehicle exits SmartRTL mode before reaching home. invalid if zero
1606     Vector3f dest_NED_backup;
1607 };
1608
1609
1610 class ModeSport : public Mode {
1611
1612 public:
1613     // inherit constructor
1614     using Mode::Mode;
1615     Number mode_number() const override { return Number::SPORT; }
1616
1617     bool init(bool ignore_checks) override;
1618     void run() override;
1619
1620     bool requires_GPS() const override { return false; }
1621     bool has_manual_throttle() const override { return false; }
1622     bool allows_arming(AP_Arming::Method method) const override { return true; };
1623     bool is_autopilot() const override { return false; }
1624     bool has_user_takeoff(bool must_navigate) const override {
1625         return !must_navigate;
1626     }
1627
1628 protected:
1629
1630     const char *name() const override { return "SPORT"; }
1631     const char *name4() const override { return "SPRT"; }
1632
1633 private:
1634
1635 };
1636
1637
1638 class ModeStabilize : public Mode {
1639
1640 public:
1641     // inherit constructor
1642     using Mode::Mode;
1643     Number mode_number() const override { return Number::STABILIZE; }
1644
1645     virtual void run() override;
1646
1647     bool requires_GPS() const override { return false; }
1648     bool has_manual_throttle() const override { return true; }
1649     bool allows_arming(AP_Arming::Method method) const override { return true; };
1650     bool is_autopilot() const override { return false; }
1651     bool allows_save_trim() const override { return true; }
1652     bool allows_autotune() const override { return true; }
1653     bool allows_flip() const override { return true; }
1654
1655 protected:
1656
1657     const char *name() const override { return "STABILIZE"; }
1658     const char *name4() const override { return "STAB"; }
1659
1660 private:
1661
1662 };
1663
1664 #if FRAME_CONFIG == HELI_FRAME
1665 class ModeStabilize_Heli : public ModeStabilize {
1666
1667 public:
1668     // inherit constructor
1669     using ModeStabilize::Mode;
1670
1671     bool init(bool ignore_checks) override;
1672     void run() override;
1673
1674     bool allows_inverted() const override { return true; };
1675
1676 protected:
1677
1678 private:
1679
1680 };
1681 #endif
1682
1683 class ModeSystemId : public Mode {
1684
1685 public:
1686     ModeSystemId(void);
1687     Number mode_number() const override { return Number::SYSTEMID; }
1688
1689     bool init(bool ignore_checks) override;
1690     void run() override;
1691     void exit() override;
1692
1693     bool requires_GPS() const override { return false; }
1694     bool has_manual_throttle() const override { return true; }
1695     bool allows_arming(AP_Arming::Method method) const override { return false; };
1696     bool is_autopilot() const override { return false; }
1697     bool logs_attitude() const override { return true; }
1698
1699     void set_magnitude(float input) { waveform_magnitude.set(input); }
1700
1701     static const struct AP_Param::GroupInfo var_info[];
1702
1703     Chirp chirp_input;
1704
1705 protected:
1706
1707     const char *name() const override { return "SYSTEMID"; }
1708     const char *name4() const override { return "SYSI"; }
1709
1710 private:
1711
1712     void log_data() const;
1713     bool is_poscontrol_axis_type() const;
1714
1715     enum class AxisType {
1716         NONE = 0,               // none
1717         INPUT_ROLL = 1,         // angle input roll axis is being excited
1718         INPUT_PITCH = 2,        // angle pitch axis is being excited
1719         INPUT_YAW = 3,          // angle yaw axis is being excited
1720         RECOVER_ROLL = 4,       // angle roll axis is being excited
1721         RECOVER_PITCH = 5,      // angle pitch axis is being excited
1722         RECOVER_YAW = 6,        // angle yaw axis is being excited
1723         RATE_ROLL = 7,          // rate roll axis is being excited
1724         RATE_PITCH = 8,         // rate pitch axis is being excited
1725         RATE_YAW = 9,           // rate yaw axis is being excited
1726         MIX_ROLL = 10,          // mixer roll axis is being excited
1727         MIX_PITCH = 11,         // mixer pitch axis is being excited
1728         MIX_YAW = 12,           // mixer pitch axis is being excited
1729         MIX_THROTTLE = 13,      // mixer throttle axis is being excited
1730         DISTURB_POS_LAT = 14,   // lateral body axis measured position is being excited
1731         DISTURB_POS_LONG = 15,  // longitudinal body axis measured position is being excited
1732         DISTURB_VEL_LAT = 16,   // lateral body axis measured velocity is being excited
1733         DISTURB_VEL_LONG = 17,  // longitudinal body axis measured velocity is being excited
1734         INPUT_VEL_LAT = 18,     // lateral body axis commanded velocity is being excited
1735         INPUT_VEL_LONG = 19,    // longitudinal body axis commanded velocity is being excited
1736     };
1737
1738     AP_Int8 axis;               // Controls which axis are being excited. Set to non-zero to display other parameters
1739     AP_Float waveform_magnitude;// Magnitude of chirp waveform
1740     AP_Float frequency_start;   // Frequency at the start of the chirp
1741     AP_Float frequency_stop;    // Frequency at the end of the chirp
1742     AP_Float time_fade_in;      // Time to reach maximum amplitude of chirp
1743     AP_Float time_record;       // Time taken to complete the chirp waveform
1744     AP_Float time_fade_out;     // Time to reach zero amplitude after chirp finishes
1745
1746     bool att_bf_feedforward;    // Setting of attitude_control->get_bf_feedforward
1747     float waveform_time;        // Time reference for waveform
1748     float waveform_sample;      // Current waveform sample
1749     float waveform_freq_rads;   // Instantaneous waveform frequency
1750     float time_const_freq;      // Time at constant frequency before chirp starts
1751     int8_t log_subsample;       // Subsample multiple for logging.
1752     Vector2f target_vel;        // target velocity for position controller modes
1753     Vector2f target_pos;       // target positon
1754     Vector2f input_vel_last;    // last cycle input velocity
1755     // System ID states
1756     enum class SystemIDModeState {
1757         SYSTEMID_STATE_STOPPED,
1758         SYSTEMID_STATE_TESTING
1759     } systemid_state;
1760 };
1761
1762 class ModeThrow : public Mode {
1763
1764 public:
1765     // inherit constructor
1766     using Mode::Mode;
1767     Number mode_number() const override { return Number::THROW; }
1768
1769     bool init(bool ignore_checks) override;
1770     void run() override;
1771
1772     bool requires_GPS() const override { return true; }
1773     bool has_manual_throttle() const override { return false; }
1774     bool allows_arming(AP_Arming::Method method) const override { return true; };
1775     bool is_autopilot() const override { return false; }
1776
1777     // Throw types
1778     enum class ThrowType {
1779         Upward = 0,
1780         Drop = 1
1781     };
1782
1783     enum class PreThrowMotorState {
1784         STOPPED = 0,
1785         RUNNING = 1,
1786     };
1787
1788 protected:
1789
1790     const char *name() const override { return "THROW"; }
1791     const char *name4() const override { return "THRW"; }
1792
1793 private:
1794
1795     bool throw_detected();
1796     bool throw_position_good() const;
1797     bool throw_height_good() const;
1798     bool throw_attitude_good() const;
1799
1800     // Throw stages
1801     enum ThrowModeStage {
1802         Throw_Disarmed,
1803         Throw_Detecting,
1804         Throw_Wait_Throttle_Unlimited,
1805         Throw_Uprighting,
1806         Throw_HgtStabilise,
1807         Throw_PosHold
1808     };
1809
1810     ThrowModeStage stage = Throw_Disarmed;
1811     ThrowModeStage prev_stage = Throw_Disarmed;
1812     uint32_t last_log_ms;
1813     bool nextmode_attempted;
1814     uint32_t free_fall_start_ms;    // system time free fall was detected
1815     float free_fall_start_velz;     // vertical velocity when free fall was detected
1816 };
1817
1818 #if MODE_TURTLE_ENABLED
1819 class ModeTurtle : public Mode {
1820
1821 public:
1822     // inherit constructors
1823     using Mode::Mode;
1824     Number mode_number() const override { return Number::TURTLE; }
1825
1826     bool init(bool ignore_checks) override;
1827     void run() override;
1828     void exit() override;
1829
1830     bool requires_GPS() const override { return false; }
1831     bool has_manual_throttle() const override { return true; }
1832     bool allows_arming(AP_Arming::Method method) const override;
1833     bool is_autopilot() const override { return false; }
1834     void change_motor_direction(bool reverse);
1835     void output_to_motors() override;
1836
1837 protected:
1838     const char *name() const override { return "TURTLE"; }
1839     const char *name4() const override { return "TRTL"; }
1840
1841 private:
1842     void arm_motors();
1843     void disarm_motors();
1844
1845     float motors_output;
1846     Vector2f motors_input;
1847     uint32_t last_throttle_warning_output_ms;
1848 };
1849 #endif
1850
1851 // modes below rely on Guided mode so must be declared at the end (instead of in alphabetical order)
1852
1853 class ModeAvoidADSB : public ModeGuided {
1854
1855 public:
1856     // inherit constructor
1857     using ModeGuided::Mode;
1858     Number mode_number() const override { return Number::AVOID_ADSB; }
1859
1860     bool init(bool ignore_checks) override;
1861     void run() override;
1862
1863     bool requires_GPS() const override { return true; }
1864     bool has_manual_throttle() const override { return false; }
1865     bool allows_arming(AP_Arming::Method method) const override { return false; }
1866     bool is_autopilot() const override { return true; }
1867
1868     bool set_velocity(const Vector3f& velocity_neu);
1869
1870 protected:
1871
1872     const char *name() const override { return "AVOID_ADSB"; }
1873     const char *name4() const override { return "AVOI"; }
1874
1875 private:
1876
1877 };
1878
1879 #if MODE_FOLLOW_ENABLED
1880 class ModeFollow : public ModeGuided {
1881
1882 public:
1883
1884     // inherit constructor
1885     using ModeGuided::Mode;
1886     Number mode_number() const override { return Number::FOLLOW; }
1887
1888     bool init(bool ignore_checks) override;
1889     void exit() override;
1890     void run() override;
1891
1892     bool requires_GPS() const override { return true; }
1893     bool has_manual_throttle() const override { return false; }
1894     bool allows_arming(AP_Arming::Method method) const override { return false; }
1895     bool is_autopilot() const override { return true; }
1896
1897 protected:
1898
1899     const char *name() const override { return "FOLLOW"; }
1900     const char *name4() const override { return "FOLL"; }
1901
1902     // for reporting to GCS
1903     bool get_wp(Location &loc) const override;
1904     uint32_t wp_distance() const override;
1905     int32_t wp_bearing() const override;
1906
1907     uint32_t last_log_ms;   // system time of last time desired velocity was logging
1908 };
1909 #endif
1910
1911 class ModeZigZag : public Mode {
1912
1913 public:
1914     ModeZigZag(void);
1915
1916     // Inherit constructor
1917     using Mode::Mode;
1918     Number mode_number() const override { return Number::ZIGZAG; }
1919
1920     enum class Destination : uint8_t {
1921         A,  // Destination A
1922         B,  // Destination B
1923     };
1924
1925     enum class Direction : uint8_t {
1926         FORWARD,        // moving forward from the yaw direction
1927         RIGHT,          // moving right from the yaw direction
1928         BACKWARD,       // moving backward from the yaw direction
1929         LEFT,           // moving left from the yaw direction
1930     } zigzag_direction;
1931
1932     bool init(bool ignore_checks) override;
1933     void exit() override;
1934     void run() override;
1935
1936     // auto control methods.  copter flies grid pattern
1937     void run_auto();
1938     void suspend_auto();
1939     void init_auto();
1940
1941     bool requires_GPS() const override { return true; }
1942     bool has_manual_throttle() const override { return false; }
1943     bool allows_arming(AP_Arming::Method method) const override { return true; }
1944     bool is_autopilot() const override { return true; }
1945     bool has_user_takeoff(bool must_navigate) const override { return true; }
1946
1947     // save current position as A or B.  If both A and B have been saved move to the one specified
1948     void save_or_move_to_destination(Destination ab_dest);
1949
1950     // return manual control to the pilot
1951     void return_to_manual_control(bool maintain_target);
1952
1953     static const struct AP_Param::GroupInfo var_info[];
1954
1955 protected:
1956
1957     const char *name() const override { return "ZIGZAG"; }
1958     const char *name4() const override { return "ZIGZ"; }
1959     uint32_t wp_distance() const override;
1960     int32_t wp_bearing() const override;
1961     float crosstrack_error() const override;
1962
1963 private:
1964
1965     void auto_control();
1966     void manual_control();
1967     bool reached_destination();
1968     bool calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const;
1969     void spray(bool b);
1970     bool calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) const;
1971     void move_to_side();
1972
1973     Vector2f dest_A;    // in NEU frame in cm relative to ekf origin
1974     Vector2f dest_B;    // in NEU frame in cm relative to ekf origin
1975     Vector3f current_dest; // current target destination (use for resume after suspending)
1976     bool current_terr_alt;
1977
1978     // parameters
1979     AP_Int8  _auto_enabled;    // top level enable/disable control
1980 #if HAL_SPRAYER_ENABLED
1981     AP_Int8  _spray_enabled;   // auto spray enable/disable
1982 #endif
1983     AP_Int8  _wp_delay;        // delay for zigzag waypoint
1984     AP_Float _side_dist;       // sideways distance
1985     AP_Int8  _direction;       // sideways direction
1986     AP_Int16 _line_num;        // total number of lines
1987
1988     enum ZigZagState {
1989         STORING_POINTS, // storing points A and B, pilot has manual control
1990         AUTO,           // after A and B defined, pilot toggle the switch from one side to the other, vehicle flies autonomously
1991         MANUAL_REGAIN   // pilot toggle the switch to middle position, has manual control
1992     } stage;
1993
1994     enum AutoState {
1995         MANUAL,         // not in ZigZag Auto
1996         AB_MOVING,      // moving from A to B or from B to A
1997         SIDEWAYS,       // moving to sideways
1998     } auto_stage;
1999
2000     uint32_t reach_wp_time_ms = 0;  // time since vehicle reached destination (or zero if not yet reached)
2001     Destination ab_dest_stored;     // store the current destination
2002     bool is_auto;                   // enable zigzag auto feature which is automate both AB and sideways
2003     uint16_t line_count = 0;        // current line number
2004     int16_t line_num = 0;           // target line number
2005     bool is_suspended;              // true if zigzag auto is suspended
2006 };
2007
2008 #if MODE_AUTOROTATE_ENABLED
2009 class ModeAutorotate : public Mode {
2010
2011 public:
2012
2013     // inherit constructor
2014     using Mode::Mode;
2015     Number mode_number() const override { return Number::AUTOROTATE; }
2016
2017     bool init(bool ignore_checks) override;
2018     void run() override;
2019
2020     bool is_autopilot() const override { return true; }
2021     bool requires_GPS() const override { return false; }
2022     bool has_manual_throttle() const override { return false; }
2023     bool allows_arming(AP_Arming::Method method) const override { return false; };
2024
2025     static const struct AP_Param::GroupInfo  var_info[];
2026
2027 protected:
2028
2029     const char *name() const override { return "AUTOROTATE"; }
2030     const char *name4() const override { return "AROT"; }
2031
2032 private:
2033
2034     // --- Internal variables ---
2035     float _initial_rpm;             // Head speed recorded at initiation of flight mode (RPM)
2036     float _target_head_speed;       // The terget head main rotor head speed.  Normalised by main rotor set point
2037     float _desired_v_z;             // Desired vertical
2038     int32_t _pitch_target;          // Target pitch attitude to pass to attitude controller
2039     uint32_t _entry_time_start_ms;  // Time remaining until entry phase moves on to glide phase
2040     float _hs_decay;                // The head accerleration during the entry phase
2041
2042     enum class Autorotation_Phase {
2043         ENTRY,
2044         SS_GLIDE,
2045         FLARE,
2046         TOUCH_DOWN,
2047         LANDED } phase_switch;
2048
2049     enum class Navigation_Decision {
2050         USER_CONTROL_STABILISED,
2051         STRAIGHT_AHEAD,
2052         INTO_WIND,
2053         NEAREST_RALLY} nav_pos_switch;
2054
2055     // --- Internal flags ---
2056     struct controller_flags {
2057             bool entry_initial             : 1;
2058             bool ss_glide_initial          : 1;
2059             bool flare_initial             : 1;
2060             bool touch_down_initial        : 1;
2061             bool landed_initial            : 1;
2062             bool straight_ahead_initial    : 1;
2063             bool level_initial             : 1;
2064             bool break_initial             : 1;
2065             bool bad_rpm                   : 1;
2066     } _flags;
2067
2068     struct message_flags {
2069             bool bad_rpm                   : 1;
2070     } _msg_flags;
2071
2072     //--- Internal functions ---
2073     void warning_message(uint8_t message_n);    //Handles output messages to the terminal
2074
2075 };
2076 #endif