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