CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/ArduSub/mode.h
Views: 1798
1
#pragma once

2


3
#include "Sub.h"

4
class Parameters;

5
class ParametersG2;

6


7
class GCS_Sub;

8


9
// Guided modes

10
enum GuidedSubMode {

11
    Guided_WP,

12
    Guided_Velocity,

13
    Guided_PosVel,

14
    Guided_Angle,

15
};

16


17
// Auto modes

18
enum AutoSubMode {

19
    Auto_WP,

20
    Auto_CircleMoveToEdge,

21
    Auto_Circle,

22
    Auto_NavGuided,

23
    Auto_Loiter,

24
    Auto_TerrainRecover

25
};

26


27
// RTL states

28
enum RTLState {

29
    RTL_InitialClimb,

30
    RTL_ReturnHome,

31
    RTL_LoiterAtHome,

32
    RTL_FinalDescent,

33
    RTL_Land

34
};

35


36
class Mode

37
{

38


39
public:

40


41
    // Auto Pilot Modes enumeration

42
    enum class Number : uint8_t {

43
        STABILIZE =     0,  // manual angle with manual depth/throttle

44
        ACRO =          1,  // manual body-frame angular rate with manual depth/throttle

45
        ALT_HOLD =      2,  // manual angle with automatic depth/throttle

46
        AUTO =          3,  // fully automatic waypoint control using mission commands

47
        GUIDED =        4,  // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands

48
        CIRCLE =        7,  // automatic circular flight with automatic throttle

49
        SURFACE =       9,  // automatically return to surface, pilot maintains horizontal control

50
        POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle

51
        MANUAL =       19,  // Pass-through input with no stabilization

52
        MOTOR_DETECT = 20,  // Automatically detect motors orientation

53
        SURFTRAK =     21   // Track distance above seafloor (hold range)

54
        // Mode number 30 reserved for "offboard" for external/lua control.

55
    };

56


57
    // constructor

58
    Mode(void);

59


60
    // do not allow copying

61
    CLASS_NO_COPY(Mode);

62


63
    // child classes should override these methods

64
    virtual bool init(bool ignore_checks) { return true; }

65
    virtual void run() = 0;

66
    virtual bool requires_GPS() const = 0;

67
    virtual bool has_manual_throttle() const = 0;

68
    virtual bool allows_arming(bool from_gcs) const = 0;

69
    virtual bool is_autopilot() const { return false; }

70
    virtual bool in_guided_mode() const { return false; }

71


72
    // return a string for this flightmode

73
    virtual const char *name() const = 0;

74
    virtual const char *name4() const = 0;

75


76
    // returns a unique number specific to this mode

77
    virtual Mode::Number number() const = 0;

78


79
    // functions for reporting to GCS

80
    virtual bool get_wp(Location &loc) { return false; }

81
    virtual int32_t wp_bearing() const { return 0; }

82
    virtual uint32_t wp_distance() const { return 0; }

83
    virtual float crosstrack_error() const { return 0.0f; }

84


85
  

86
    // pilot input processing

87
    void get_pilot_desired_accelerations(float &right_out, float &front_out) const;

88
    void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);

89


90


91
protected:

92


93
    // navigation support functions

94
    virtual void run_autopilot() {}

95


96
    // helper functions

97
    bool is_disarmed_or_landed() const;

98


99
    // functions to control landing

100
    // in modes that support landing

101
    void land_run_horizontal_control();

102
    void land_run_vertical_control(bool pause_descent = false);

103


104
    // convenience references to avoid code churn in conversion:

105
    Parameters &g;

106
    ParametersG2 &g2;

107
    AP_InertialNav &inertial_nav;

108
    AP_AHRS &ahrs;

109
    AP_Motors6DOF &motors;

110
    RC_Channel *&channel_roll;

111
    RC_Channel *&channel_pitch;

112
    RC_Channel *&channel_throttle;

113
    RC_Channel *&channel_yaw;

114
    RC_Channel *&channel_forward;

115
    RC_Channel *&channel_lateral;

116
    AC_PosControl *position_control;

117
    AC_AttitudeControl_Sub *attitude_control;

118
    // TODO: channels

119
    float &G_Dt;

120


121
public:

122
    // Navigation Yaw control

123
    class AutoYaw

124
    {

125


126
    public:

127
        // mode(): current method of determining desired yaw:

128
        autopilot_yaw_mode mode() const

129
        {

130
            return (autopilot_yaw_mode)_mode;

131
        }

132
        void set_mode_to_default(bool rtl);

133
        void set_mode(autopilot_yaw_mode new_mode);

134
        autopilot_yaw_mode default_mode(bool rtl) const;

135


136
        void set_rate(float new_rate_cds);

137


138
        // set_roi(...): set a "look at" location:

139
        void set_roi(const Location &roi_location);

140


141
        void set_fixed_yaw(float angle_deg,

142
                           float turn_rate_dps,

143
                           int8_t direction,

144
                           bool relative_angle);

145


146
    private:

147


148
        // yaw_cd(): main product of AutoYaw; the heading:

149
        float yaw_cd();

150


151
        // rate_cds(): desired yaw rate in centidegrees/second:

152
        float rate_cds();

153


154
        float look_ahead_yaw();

155
        float roi_yaw();

156


157
        // auto flight mode's yaw mode

158
        uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP;

159


160
        // Yaw will point at this location if mode is set to AUTO_YAW_ROI

161
        Vector3f roi;

162


163
        // bearing from current location to the ROI

164
        float _roi_yaw;

165


166
        // yaw used for YAW_FIXED yaw_mode

167
        int32_t _fixed_yaw;

168


169
        // Deg/s we should turn

170
        int16_t _fixed_yaw_slewrate;

171


172
        // heading when in yaw_look_ahead_yaw

173
        float _look_ahead_yaw;

174


175
        // used to reduce update rate to 100hz:

176
        uint8_t roi_yaw_counter;

177


178
        GuidedSubMode guided_mode;

179


180
    };

181
    static AutoYaw auto_yaw;

182


183
    // pass-through functions to reduce code churn on conversion;

184
    // these are candidates for moving into the Mode base

185
    // class.

186
    bool set_mode(Mode::Number mode, ModeReason reason);

187
    GCS_Sub &gcs();

188


189
    // end pass-through functions

190
};

191


192
class ModeManual : public Mode

193
{

194


195
public:

196
    // inherit constructor

197
    using Mode::Mode;

198
    virtual void run() override;

199
    bool init(bool ignore_checks) override;

200
    bool requires_GPS() const override { return false; }

201
    bool has_manual_throttle() const override { return true; }

202
    bool allows_arming(bool from_gcs) const override { return true; }

203
    bool is_autopilot() const override { return false; }

204


205
protected:

206


207
    const char *name() const override { return "MANUAL"; }

208
    const char *name4() const override { return "MANU"; }

209
    Mode::Number number() const override { return Mode::Number::MANUAL; }

210
};

211


212


213
class ModeAcro : public Mode

214
{

215


216
public:

217
    // inherit constructor

218
    using Mode::Mode;

219


220
    virtual void run() override;

221


222
    bool init(bool ignore_checks) override;

223
    bool requires_GPS() const override { return false; }

224
    bool has_manual_throttle() const override { return true; }

225
    bool allows_arming(bool from_gcs) const override { return true; }

226
    bool is_autopilot() const override { return false; }

227


228
protected:

229


230
    const char *name() const override { return "ACRO"; }

231
    const char *name4() const override { return "ACRO"; }

232
    Mode::Number number() const override { return Mode::Number::ACRO; }

233
};

234


235


236
class ModeStabilize : public Mode

237
{

238


239
public:

240
    // inherit constructor

241
    using Mode::Mode;

242


243
    virtual void run() override;

244


245
    bool init(bool ignore_checks) override;

246
    bool requires_GPS() const override { return false; }

247
    bool has_manual_throttle() const override { return true; }

248
    bool allows_arming(bool from_gcs) const override { return true; }

249
    bool is_autopilot() const override { return false; }

250


251
protected:

252


253
    const char *name() const override { return "STABILIZE"; }

254
    const char *name4() const override { return "STAB"; }

255
    Mode::Number number() const override { return Mode::Number::STABILIZE; }

256
};

257


258


259
class ModeAlthold : public Mode

260
{

261


262
public:

263
    // inherit constructor

264
    using Mode::Mode;

265


266
    virtual void run() override;

267


268
    bool init(bool ignore_checks) override;

269
    bool requires_GPS() const override { return false; }

270
    bool has_manual_throttle() const override { return false; }

271
    bool allows_arming(bool from_gcs) const override { return true; }

272
    bool is_autopilot() const override { return false; }

273
    void control_depth();

274


275
protected:

276


277
    void run_pre();

278
    void run_post();

279


280
    const char *name() const override { return "ALT_HOLD"; }

281
    const char *name4() const override { return "ALTH"; }

282
    Mode::Number number() const override { return Mode::Number::ALT_HOLD; }

283
};

284


285


286
class ModeSurftrak : public ModeAlthold

287
{

288


289
public:

290
    // constructor

291
    ModeSurftrak();

292


293
    void run() override;

294


295
    bool init(bool ignore_checks) override;

296


297
    float get_rangefinder_target_cm() const WARN_IF_UNUSED { return rangefinder_target_cm; }

298
    bool set_rangefinder_target_cm(float target_cm);

299


300
protected:

301


302
    const char *name() const override { return "SURFTRAK"; }

303
    const char *name4() const override { return "STRK"; }

304
    Mode::Number number() const override { return Mode::Number::SURFTRAK; }

305


306
private:

307


308
    void reset();

309
    void control_range();

310
    void update_surface_offset();

311


312
    float rangefinder_target_cm;

313


314
    bool pilot_in_control;            // pilot is moving up/down

315
    float pilot_control_start_z_cm;   // alt when pilot took control

316
};

317


318
class ModeGuided : public Mode

319
{

320


321
public:

322
    // inherit constructor

323
    using Mode::Mode;

324


325
    virtual void run() override;

326


327
    bool init(bool ignore_checks) override;

328
    bool requires_GPS() const override { return true; }

329
    bool has_manual_throttle() const override { return false; }

330
    bool allows_arming(bool from_gcs) const override { return true; }

331
    bool is_autopilot() const override { return true; }

332
    bool in_guided_mode() const override { return true; }

333
    bool guided_limit_check();

334
    void guided_limit_init_time_and_pos();

335
    void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);

336
    void guided_set_angle(const Quaternion&, float);

337
    void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);

338
    bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);

339
    bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);

340
    bool guided_set_destination(const Vector3f& destination);

341
    bool guided_set_destination(const Location&);

342
    bool guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);

343
    void guided_set_velocity(const Vector3f& velocity);

344
    void guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);

345
    void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);

346
    float get_auto_heading();

347
    void guided_limit_clear();

348
    void set_auto_yaw_mode(autopilot_yaw_mode yaw_mode);

349


350
protected:

351


352
    const char *name() const override { return "GUIDED"; }

353
    const char *name4() const override { return "GUID"; }

354
    Mode::Number number() const override { return Mode::Number::GUIDED; }

355


356
    autopilot_yaw_mode get_default_auto_yaw_mode(bool rtl) const;

357


358
private:

359
    void guided_pos_control_run();

360
    void guided_vel_control_run();

361
    void guided_posvel_control_run();

362
    void guided_angle_control_run();

363
    void guided_takeoff_run();

364
    void guided_pos_control_start();

365
    void guided_vel_control_start();

366
    void guided_posvel_control_start();

367
    void guided_angle_control_start();

368
};

369


370


371


372
class ModeAuto : public ModeGuided

373
{

374


375
public:

376
    // inherit constructor

377
    using ModeGuided::ModeGuided;

378


379
    virtual void run() override;

380


381
    bool init(bool ignore_checks) override;

382
    bool requires_GPS() const override { return true; }

383
    bool has_manual_throttle() const override { return false; }

384
    bool allows_arming(bool from_gcs) const override { return true; }

385
    bool is_autopilot() const override { return true; }

386
    bool auto_loiter_start();

387
    void auto_wp_start(const Vector3f& destination);

388
    void auto_wp_start(const Location& dest_loc);

389
    void auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn);

390
    void auto_circle_start();

391
    void auto_nav_guided_start();

392
    void set_auto_yaw_roi(const Location &roi_location);

393
    void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle);

394
    void set_yaw_rate(float turn_rate_dps);

395
    bool auto_terrain_recover_start();

396


397
protected:

398


399
    const char *name() const override { return "AUTO"; }

400
    const char *name4() const override { return "AUTO"; }

401
    Mode::Number number() const override { return Mode::Number::AUTO; }

402


403
private:

404
    void auto_wp_run();

405
    void auto_circle_run();

406
    void auto_nav_guided_run();

407
    void auto_loiter_run();

408
    void auto_terrain_recover_run();

409
};

410


411


412
class ModePoshold : public ModeAlthold

413
{

414


415
public:

416
    // inherit constructor

417
    using ModeAlthold::ModeAlthold;

418


419
    virtual void run() override;

420


421
    bool init(bool ignore_checks) override;

422


423
    bool requires_GPS() const override { return false; }

424
    bool has_manual_throttle() const override { return false; }

425
    bool allows_arming(bool from_gcs) const override { return true; }

426
    bool is_autopilot() const override { return true; }

427


428
protected:

429


430
    const char *name() const override { return "POSHOLD"; }

431
    const char *name4() const override { return "POSH"; }

432
    Mode::Number number() const override { return Mode::Number::POSHOLD; }

433
};

434


435


436
class ModeCircle : public Mode

437
{

438


439
public:

440
    // inherit constructor

441
    using Mode::Mode;

442


443
    virtual void run() override;

444


445
    bool init(bool ignore_checks) override;

446
    bool requires_GPS() const override { return true; }

447
    bool has_manual_throttle() const override { return false; }

448
    bool allows_arming(bool from_gcs) const override { return true; }

449
    bool is_autopilot() const override { return true; }

450


451
protected:

452


453
    const char *name() const override { return "CIRCLE"; }

454
    const char *name4() const override { return "CIRC"; }

455
    Mode::Number number() const override { return Mode::Number::CIRCLE; }

456
};

457


458
class ModeSurface : public Mode

459
{

460


461
public:

462
    // inherit constructor

463
    using Mode::Mode;

464


465
    virtual void run() override;

466


467
    bool init(bool ignore_checks) override;

468
    bool requires_GPS() const override { return true; }

469
    bool has_manual_throttle() const override { return false; }

470
    bool allows_arming(bool from_gcs) const override { return true; }

471
    bool is_autopilot() const override { return true; }

472


473
protected:

474


475
    const char *name() const override { return "SURFACE"; }

476
    const char *name4() const override { return "SURF"; }

477
    Mode::Number number() const override { return Mode::Number::CIRCLE; }

478
};

479


480


481
class ModeMotordetect : public Mode

482
{

483


484
public:

485
    // inherit constructor

486
    using Mode::Mode;

487


488
    virtual void run() override;

489


490
    bool init(bool ignore_checks) override;

491
    bool requires_GPS() const override { return false; }

492
    bool has_manual_throttle() const override { return false; }

493
    bool allows_arming(bool from_gcs) const override { return true; }

494
    bool is_autopilot() const override { return true; }

495


496
protected:

497


498
    const char *name() const override { return "MOTORDETECT"; }

499
    const char *name4() const override { return "DETE"; }

500
    Mode::Number number() const override { return Mode::Number::MOTOR_DETECT; }

501
};

502


503