Line data Source code
1 : #pragma once
2 :
3 : #include "Rover.h"
4 :
5 : // pre-define ModeRTL so Auto can appear higher in this file
6 : class ModeRTL;
7 :
8 : class Mode
9 : {
10 : public:
11 :
12 : // Auto Pilot modes
13 : // ----------------
14 : enum class Number : uint8_t {
15 : MANUAL = 0,
16 : ACRO = 1,
17 : STEERING = 3,
18 : HOLD = 4,
19 : LOITER = 5,
20 : FOLLOW = 6,
21 : SIMPLE = 7,
22 : #if MODE_DOCK_ENABLED
23 : DOCK = 8,
24 : #endif
25 : CIRCLE = 9,
26 : AUTO = 10,
27 : RTL = 11,
28 : SMART_RTL = 12,
29 : GUIDED = 15,
30 : INITIALISING = 16,
31 : // Mode number 30 reserved for "offboard" for external/lua control.
32 : };
33 :
34 : // Constructor
35 : Mode();
36 :
37 : // do not allow copying
38 : CLASS_NO_COPY(Mode);
39 :
40 : // Return true if this mode is enabled, used by MAVLink available mode
41 125260 : virtual bool enabled() const { return true; }
42 :
43 : // enter this mode, returns false if we failed to enter
44 : bool enter();
45 :
46 : // perform any cleanups required:
47 : void exit();
48 :
49 : // returns a unique number specific to this mode
50 : virtual Number mode_number() const = 0;
51 :
52 : // returns full text name
53 : virtual const char *name() const = 0;
54 :
55 : // returns short text name (up to 4 bytes)
56 : virtual const char *name4() const = 0;
57 :
58 : //
59 : // methods that sub classes should override to affect movement of the vehicle in this mode
60 : //
61 :
62 : // convert user input to targets, implement high level control for this mode
63 : virtual void update() = 0;
64 :
65 : //
66 : // attributes of the mode
67 : //
68 :
69 : // return if in non-manual mode : Auto, Guided, RTL, SmartRTL
70 454942 : virtual bool is_autopilot_mode() const { return false; }
71 :
72 : // return if external control is allowed in this mode (Guided or Guided-within-Auto)
73 0 : virtual bool in_guided_mode() const { return false; }
74 :
75 : // returns true if vehicle can be armed or disarmed from the transmitter in this mode
76 3 : virtual bool allows_arming_from_transmitter() { return !is_autopilot_mode(); }
77 :
78 : // returns false if vehicle cannot be armed in this mode
79 15114 : virtual bool allows_arming() const { return true; }
80 :
81 218570 : bool allows_stick_mixing() const { return is_autopilot_mode(); }
82 :
83 : //
84 : // attributes for mavlink system status reporting
85 : //
86 :
87 : // returns true if any RC input is used
88 5340 : virtual bool has_manual_input() const { return false; }
89 :
90 : // true if heading is controlled
91 52514 : virtual bool attitude_stabilized() const { return true; }
92 :
93 : // true if mode requires position and/or velocity estimate
94 2078 : virtual bool requires_position() const { return true; }
95 107 : virtual bool requires_velocity() const { return true; }
96 :
97 : // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
98 : virtual float wp_bearing() const;
99 : virtual float nav_bearing() const;
100 : virtual float crosstrack_error_m() const;
101 : virtual float get_desired_lat_accel() const;
102 :
103 : // get speed error in m/s, not currently supported
104 50963 : float speed_error() const { return 0.0f; }
105 :
106 : //
107 : // navigation methods
108 : //
109 :
110 : // return straight-line distance (in meters) to destination
111 68683 : virtual float get_distance_to_destination() const { return 0.0f; }
112 :
113 : // return desired location (used in Guided, Auto, RTL, etc)
114 : // return true on success, false if there is no valid destination
115 75284 : virtual bool get_desired_location(Location& destination) const WARN_IF_UNUSED { return false; }
116 :
117 : // set desired location (used in Guided, Auto)
118 : // set next_destination (if known). If not provided vehicle stops at destination
119 : virtual bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED;
120 :
121 : // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
122 0 : virtual bool reached_destination() const { return true; }
123 :
124 : // get default speed for this mode (held in CRUISE_SPEED, WP_SPEED or RTL_SPEED)
125 : // rtl argument should be true if called from RTL or SmartRTL modes (handled here to avoid duplication)
126 : float get_speed_default(bool rtl = false) const;
127 :
128 : // set desired speed in m/s
129 0 : virtual bool set_desired_speed(float speed_ms) { return false; }
130 :
131 : // execute the mission in reverse (i.e. backing up)
132 : void set_reversed(bool value);
133 :
134 : // init reversed flag for autopilot mode
135 519 : virtual void init_reversed_flag() { if (is_autopilot_mode()) { set_reversed(false); } }
136 :
137 : // handle tacking request (from auxiliary switch) in sailboats
138 : virtual void handle_tack_request();
139 :
140 : protected:
141 :
142 : // subclasses override this to perform checks before entering the mode
143 400 : virtual bool _enter() { return true; }
144 :
145 : // subclasses override this to perform any required cleanup when exiting the mode
146 404 : virtual void _exit() { return; }
147 :
148 : // decode pilot steering and throttle inputs and return in steer_out and throttle_out arguments
149 : // steering_out is in the range -4500 ~ +4500 with positive numbers meaning rotate clockwise
150 : // throttle_out is in the range -100 ~ +100
151 : void get_pilot_desired_steering_and_throttle(float &steering_out, float &throttle_out) const;
152 :
153 : // decode pilot input steering and return steering_out and speed_out (in m/s)
154 : void get_pilot_desired_steering_and_speed(float &steering_out, float &speed_out) const;
155 :
156 : // decode pilot lateral movement input and return in lateral_out argument
157 : void get_pilot_desired_lateral(float &lateral_out) const;
158 :
159 : // decode pilot's input and return heading_out (in cd) and speed_out (in m/s)
160 : void get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out) const;
161 :
162 : // decode pilot roll and pitch inputs and return in roll_out and pitch_out arguments
163 : // outputs are in the range -1 to +1
164 : void get_pilot_desired_roll_and_pitch(float &roll_out, float &pitch_out) const;
165 :
166 : // decode pilot height inputs and return in height_out arguments
167 : // outputs are in the range -1 to +1
168 : void get_pilot_desired_walking_height(float &walking_height_out) const;
169 :
170 : // high level call to navigate to waypoint
171 : void navigate_to_waypoint();
172 :
173 : // calculate steering output given a turn rate
174 : // desired turn rate in radians/sec. Positive to the right.
175 : void calc_steering_from_turn_rate(float turn_rate);
176 :
177 : // calculate steering angle given a desired lateral acceleration
178 : void calc_steering_from_lateral_acceleration(float lat_accel, bool reversed = false);
179 :
180 : // calculate steering output to drive towards desired heading
181 : // rate_max is a maximum turn rate in deg/s. set to zero to use default turn rate limits
182 : void calc_steering_to_heading(float desired_heading_cd, float rate_max_degs = 0.0f);
183 :
184 : // calculates the amount of throttle that should be output based
185 : // on things like proximity to corners and current speed
186 : virtual void calc_throttle(float target_speed, bool avoidance_enabled);
187 :
188 : // performs a controlled stop. returns true once vehicle has stopped
189 : bool stop_vehicle();
190 :
191 : // estimate maximum vehicle speed (in m/s)
192 : // cruise_speed is in m/s, cruise_throttle should be in the range -1 to +1
193 : float calc_speed_max(float cruise_speed, float cruise_throttle) const;
194 :
195 : // calculate pilot input to nudge speed up or down
196 : // target_speed should be in meters/sec
197 : // reversed should be true if the vehicle is intentionally backing up which allows the pilot to increase the backing up speed by pulling the throttle stick down
198 : float calc_speed_nudge(float target_speed, bool reversed);
199 :
200 : protected:
201 :
202 : // decode pilot steering and throttle inputs and return in steer_out and throttle_out arguments
203 : // steering_out is in the range -4500 ~ +4500 with positive numbers meaning rotate clockwise
204 : // throttle_out is in the range -100 ~ +100
205 : void get_pilot_input(float &steering_out, float &throttle_out) const;
206 : void set_steering(float steering_value);
207 :
208 : // references to avoid code churn:
209 : class AP_AHRS &ahrs;
210 : class Parameters &g;
211 : class ParametersG2 &g2;
212 : class RC_Channel *&channel_steer;
213 : class RC_Channel *&channel_throttle;
214 : class RC_Channel *&channel_lateral;
215 : class RC_Channel *&channel_roll;
216 : class RC_Channel *&channel_pitch;
217 : class RC_Channel *&channel_walking_height;
218 : class AR_AttitudeControl &attitude_control;
219 :
220 : // private members for waypoint navigation
221 : float _distance_to_destination; // straight-line distance from vehicle to final destination in meters
222 : bool _reached_destination; // true once the vehicle has reached the destination
223 : float _desired_yaw_cd; // desired yaw in centi-degrees. used in Auto, Guided and Loiter
224 : };
225 :
226 :
227 : class ModeAcro : public Mode
228 : {
229 : public:
230 :
231 12690 : Number mode_number() const override { return Number::ACRO; }
232 0 : const char *name() const override { return "Acro"; }
233 16 : const char *name4() const override { return "ACRO"; }
234 :
235 : // methods that affect movement of the vehicle in this mode
236 : void update() override;
237 :
238 : // attributes for mavlink system status reporting
239 98 : bool has_manual_input() const override { return true; }
240 :
241 : // acro mode requires a velocity estimate for non skid-steer rovers
242 69 : bool requires_position() const override { return false; }
243 : bool requires_velocity() const override;
244 :
245 : // sailboats in acro mode support user manually initiating tacking from transmitter
246 : void handle_tack_request() override;
247 : };
248 :
249 :
250 : class ModeAuto : public Mode
251 : {
252 : public:
253 :
254 15538 : Number mode_number() const override { return Number::AUTO; }
255 0 : const char *name() const override { return "Auto"; }
256 40 : const char *name4() const override { return "AUTO"; }
257 :
258 : // methods that affect movement of the vehicle in this mode
259 : void update() override;
260 : void calc_throttle(float target_speed, bool avoidance_enabled) override;
261 :
262 : // attributes of the mode
263 179509 : bool is_autopilot_mode() const override { return true; }
264 :
265 : // return if external control is allowed in this mode (Guided or Guided-within-Auto)
266 0 : bool in_guided_mode() const override { return _submode == SubMode::Guided || _submode == SubMode::NavScriptTime; }
267 :
268 : // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
269 : float wp_bearing() const override;
270 : float nav_bearing() const override;
271 : float crosstrack_error_m() const override;
272 : float get_desired_lat_accel() const override;
273 :
274 : // return straight-line distance (in meters) to destination
275 : float get_distance_to_destination() const override;
276 :
277 : // get or set desired location
278 : bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
279 : bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED;
280 : bool reached_destination() const override;
281 :
282 : // set desired speed in m/s
283 : bool set_desired_speed(float speed_ms) override;
284 :
285 : // start RTL (within auto)
286 : void start_RTL();
287 :
288 : // lua accessors for nav script time support
289 : bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4);
290 : void nav_script_time_done(uint16_t id);
291 :
292 : //
293 40 : void init_reversed_flag() override {
294 40 : if (!mission.is_resume()) {
295 0 : set_reversed(false);
296 : }
297 40 : }
298 :
299 : AP_Mission mission{
300 : FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command&),
301 : FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command_callback, bool, const AP_Mission::Mission_Command&),
302 : FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)};
303 :
304 : enum class DoneBehaviour : uint8_t {
305 : HOLD = 0,
306 : LOITER = 1,
307 : ACRO = 2,
308 : MANUAL = 3,
309 : };
310 :
311 : protected:
312 :
313 : bool _enter() override;
314 : void _exit() override;
315 :
316 : enum SubMode: uint8_t {
317 : WP, // drive to a given location
318 : HeadingAndSpeed, // turn to a given heading
319 : RTL, // perform RTL within auto mode
320 : Loiter, // perform Loiter within auto mode
321 : Guided, // handover control to external navigation system from within auto mode
322 : Stop, // stop the vehicle as quickly as possible
323 : NavScriptTime, // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing
324 : Circle, // circle a given location
325 : } _submode;
326 :
327 : private:
328 :
329 : bool check_trigger(void);
330 : bool start_loiter();
331 : void start_guided(const Location& target_loc);
332 : void start_stop();
333 : void send_guided_position_target();
334 :
335 : bool start_command(const AP_Mission::Mission_Command& cmd);
336 : void exit_mission();
337 : bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
338 :
339 : bool verify_command(const AP_Mission::Mission_Command& cmd);
340 : void do_RTL(void);
341 : bool do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination);
342 : void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
343 : void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd);
344 : void do_nav_delay(const AP_Mission::Mission_Command& cmd);
345 : bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
346 : bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
347 : bool verify_RTL() const;
348 : bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
349 : bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
350 : bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
351 : bool verify_nav_set_yaw_speed();
352 : bool do_circle(const AP_Mission::Mission_Command& cmd);
353 : bool verify_circle(const AP_Mission::Mission_Command& cmd);
354 : void do_wait_delay(const AP_Mission::Mission_Command& cmd);
355 : void do_within_distance(const AP_Mission::Mission_Command& cmd);
356 : bool verify_wait_delay();
357 : bool verify_within_distance();
358 : void do_change_speed(const AP_Mission::Mission_Command& cmd);
359 : void do_set_home(const AP_Mission::Mission_Command& cmd);
360 : void do_set_reverse(const AP_Mission::Mission_Command& cmd);
361 : void do_guided_limits(const AP_Mission::Mission_Command& cmd);
362 : #if AP_SCRIPTING_ENABLED
363 : void do_nav_script_time(const AP_Mission::Mission_Command& cmd);
364 : bool verify_nav_script_time();
365 : #endif
366 :
367 : bool waiting_to_start; // true if waiting for EKF origin before starting mission
368 : bool auto_triggered; // true when auto has been triggered to start
369 :
370 : // HeadingAndSpeed sub mode variables
371 : float _desired_speed; // desired speed in HeadingAndSpeed submode
372 : bool _reached_heading; // true when vehicle has reached desired heading in TurnToHeading sub mode
373 :
374 : // Loiter control
375 : uint16_t loiter_duration; // How long we should loiter at the nav_waypoint (time in seconds)
376 : uint32_t loiter_start_time; // How long have we been loitering - The start time in millis
377 : bool previously_reached_wp; // set to true if we have EVER reached the waypoint
378 :
379 : // Guided-within-Auto variables
380 : struct {
381 : Location loc; // location target sent to external navigation
382 : bool valid; // true if loc is valid
383 : uint32_t last_sent_ms; // system time that target was last sent to offboard navigation
384 : } guided_target;
385 :
386 : // Conditional command
387 : // A value used in condition commands (eg delay, change alt, etc.)
388 : // For example in a change altitude command, it is the altitude to change to.
389 : int32_t condition_value;
390 : // A starting value used to check the status of a conditional command.
391 : // For example in a delay command the condition_start records that start time for the delay
392 : int32_t condition_start;
393 :
394 : // Delay the next navigation command
395 : uint32_t nav_delay_time_max_ms; // used for delaying the navigation commands
396 : uint32_t nav_delay_time_start_ms;
397 :
398 : #if AP_SCRIPTING_ENABLED
399 : // nav_script_time command variables
400 : struct {
401 : bool done; // true once lua script indicates it has completed
402 : uint16_t id; // unique id to avoid race conditions between commands and lua scripts
403 : uint32_t start_ms; // system time nav_script_time command was received (used for timeout)
404 : uint8_t command; // command number provided by mission command
405 : uint8_t timeout_s; // timeout (in seconds) provided by mission command
406 : float arg1; // 1st argument provided by mission command
407 : float arg2; // 2nd argument provided by mission command
408 : int16_t arg3; // 3rd argument provided by mission command
409 : int16_t arg4; // 4th argument provided by mission command
410 : } nav_scripting;
411 : #endif
412 :
413 : // Mission change detector
414 : AP_Mission_ChangeDetector mis_change_detector;
415 : };
416 :
417 : class ModeCircle : public Mode
418 : {
419 : public:
420 :
421 : // need a constructor for parameters
422 : ModeCircle();
423 :
424 : // Does not allow copies
425 : CLASS_NO_COPY(ModeCircle);
426 :
427 12526 : Number mode_number() const override { return Number::CIRCLE; }
428 0 : const char *name() const override { return "Circle"; }
429 0 : const char *name4() const override { return "CIRC"; }
430 :
431 : // return the distance at which the vehicle is considered to be on track along the circle
432 : float get_reached_distance() const;
433 :
434 : // initialise with specific center location, radius (in meters) and direction
435 : // replaces use of _enter when initialised from within Auto mode
436 : bool set_center(const Location& center_loc, float radius_m, bool dir_ccw);
437 :
438 : // methods that affect movement of the vehicle in this mode
439 : void update() override;
440 :
441 1978 : bool is_autopilot_mode() const override { return true; }
442 :
443 : // return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
444 : float wp_bearing() const override;
445 : float nav_bearing() const override;
446 828 : float crosstrack_error_m() const override { return dist_to_edge_m; }
447 : float get_desired_lat_accel() const override;
448 :
449 : // set desired speed in m/s
450 : bool set_desired_speed(float speed_ms) override;
451 :
452 : // return straight-line distance (in meters) to destination
453 828 : float get_distance_to_destination() const override { return _distance_to_destination; }
454 :
455 : // get or set desired location
456 : bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
457 :
458 : // return total angle in radians that vehicle has circled
459 : // fabsf is used so that full rotations in either direction are counted
460 1977 : float get_angle_total_rad() const { return fabsf(angle_total_rad); }
461 :
462 : static const struct AP_Param::GroupInfo var_info[];
463 :
464 : protected:
465 :
466 : AP_Float radius; // circle radius in meters
467 : AP_Float speed; // vehicle speed in m/s. If zero uses WP_SPEED
468 : AP_Int8 direction; // direction 0:clockwise, 1:counter-clockwise
469 :
470 : // initialise mode
471 : bool _enter() override;
472 :
473 : // Update position controller targets driving to the circle edge
474 : void update_drive_to_radius();
475 :
476 : // Update position controller targets while circling
477 : void update_circling();
478 :
479 : // initialise target_yaw_rad using the vehicle's position and yaw
480 : // if there is no current position estimate target_yaw_rad is set to vehicle yaw
481 : void init_target_yaw_rad();
482 :
483 : // limit config speed so that lateral acceleration is within limits
484 : // outputs warning to user if speed is reduced
485 : void check_config_speed();
486 :
487 : // ensure config radius is no smaller then vehicle's TURN_RADIUS
488 : // radius is increased if necessary and warning is output to the user
489 : void check_config_radius();
490 :
491 : // enum for Direction parameter
492 : enum class Direction {
493 : CW = 0,
494 : CCW = 1
495 : };
496 :
497 : // local members
498 : struct {
499 : Location center_loc; // circle center as a Location
500 : Vector2f center_pos; // circle center as an offset (in meters) from the EKF origin
501 : float radius; // circle radius
502 : float speed; // desired speed around circle in m/s
503 : Direction dir; // direction, 0:clockwise, 1:counter-clockwise
504 : } config;
505 : struct {
506 : float speed; // vehicle's target speed around circle in m/s
507 : float yaw_rad; // earth-frame angle of tarrget point on the circle
508 : Vector2p pos; // latest position target sent to position controller
509 : Vector2f vel; // latest velocity target sent to position controller
510 : Vector2f accel; // latest accel target sent to position controller
511 : } target;
512 : float angle_total_rad; // total angle in radians that vehicle has circled
513 : bool reached_edge; // true once vehicle has reached edge of circle
514 : float dist_to_edge_m; // distance to edge of circle in meters (equivalent to crosstrack error)
515 : bool tracking_back; // true if the vehicle is trying to track back onto the circle
516 : };
517 :
518 : class ModeGuided : public Mode
519 : {
520 : public:
521 : #if AP_EXTERNAL_CONTROL_ENABLED
522 : friend class AP_ExternalControl_Rover;
523 : #endif
524 :
525 14050 : Number mode_number() const override { return Number::GUIDED; }
526 0 : const char *name() const override { return "Guided"; }
527 34 : const char *name4() const override { return "GUID"; }
528 :
529 : // methods that affect movement of the vehicle in this mode
530 : void update() override;
531 :
532 : // attributes of the mode
533 73852 : bool is_autopilot_mode() const override { return true; }
534 :
535 : // return if external control is allowed in this mode (Guided or Guided-within-Auto)
536 2147 : bool in_guided_mode() const override { return true; }
537 :
538 : // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
539 : float wp_bearing() const override;
540 : float nav_bearing() const override;
541 : float crosstrack_error_m() const override;
542 : float get_desired_lat_accel() const override;
543 :
544 : // return straight-line distance (in meters) to destination
545 : float get_distance_to_destination() const override;
546 :
547 : // return true if vehicle has reached destination
548 : bool reached_destination() const override;
549 :
550 : // set desired speed in m/s
551 : bool set_desired_speed(float speed_ms) override;
552 :
553 : // get or set desired location
554 : bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
555 : bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED;
556 :
557 : // set desired heading and speed
558 : void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed);
559 :
560 : // set desired heading-delta, turn-rate and speed
561 : void set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed);
562 : void set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed);
563 :
564 : // set steering and throttle (-1 to +1). Only called from scripts
565 : void set_steering_and_throttle(float steering, float throttle);
566 :
567 : // vehicle start loiter
568 : bool start_loiter();
569 :
570 : // start stopping
571 : void start_stop();
572 :
573 : // guided limits
574 : void limit_set(uint32_t timeout_ms, float horiz_max);
575 : void limit_clear();
576 : void limit_init_time_and_location();
577 : bool limit_breached() const;
578 :
579 : // return guided mode timeout in milliseconds. Only used for velocity and acceleration control
580 : uint32_t get_timeout_ms() const;
581 :
582 : protected:
583 :
584 : enum class SubMode: uint8_t {
585 : WP,
586 : HeadingAndSpeed,
587 : TurnRateAndSpeed,
588 : Loiter,
589 : SteeringAndThrottle,
590 : Stop
591 : };
592 :
593 : // enum for GUID_OPTIONS parameter
594 : enum class Options : int32_t {
595 : SCurvesUsedForNavigation = (1U << 6)
596 : };
597 :
598 : bool _enter() override;
599 :
600 : // returns true if GUID_OPTIONS bit set to use scurve navigation instead of position controller input shaping
601 : // scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping)
602 : bool use_scurves_for_navigation() const;
603 :
604 : SubMode _guided_mode; // stores which GUIDED mode the vehicle is in
605 :
606 : // attitude control
607 : bool have_attitude_target; // true if we have a valid attitude target
608 : uint32_t _des_att_time_ms; // system time last call to set_desired_attitude was made (used for timeout)
609 : float _desired_yaw_rate_cds;// target turn rate centi-degrees per second
610 : bool send_notification; // used to send one time notification to ground station
611 : float _desired_speed; // desired speed used only in HeadingAndSpeed submode
612 :
613 : // direct steering and throttle control
614 : bool _have_strthr; // true if we have a valid direct steering and throttle inputs
615 : uint32_t _strthr_time_ms; // system time last call to set_steering_and_throttle was made (used for timeout)
616 : float _strthr_steering; // direct steering input in the range -1 to +1
617 : float _strthr_throttle; // direct throttle input in the range -1 to +1
618 :
619 : // limits
620 : struct {
621 : uint32_t timeout_ms;// timeout from the time that guided is invoked
622 : float horiz_max; // horizontal position limit in meters from where guided mode was initiated (0 = no limit)
623 : uint32_t start_time_ms; // system time in milliseconds that control was handed to the external computer
624 : Location start_loc; // starting location for checking horiz_max limit
625 : } limit;
626 : };
627 :
628 :
629 : class ModeHold : public Mode
630 : {
631 : public:
632 :
633 12734 : Number mode_number() const override { return Number::HOLD; }
634 0 : const char *name() const override { return "Hold"; }
635 25 : const char *name4() const override { return "HOLD"; }
636 :
637 : // methods that affect movement of the vehicle in this mode
638 : void update() override;
639 :
640 : // attributes for mavlink system status reporting
641 700 : bool attitude_stabilized() const override { return false; }
642 :
643 : // hold mode does not require position or velocity estimate
644 135 : bool requires_position() const override { return false; }
645 135 : bool requires_velocity() const override { return false; }
646 : };
647 :
648 : class ModeLoiter : public Mode
649 : {
650 : public:
651 :
652 12794 : Number mode_number() const override { return Number::LOITER; }
653 0 : const char *name() const override { return "Loiter"; }
654 10 : const char *name4() const override { return "LOIT"; }
655 :
656 : // methods that affect movement of the vehicle in this mode
657 : void update() override;
658 :
659 : // attributes of the mode
660 18728 : bool is_autopilot_mode() const override { return true; }
661 :
662 : // return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
663 4959 : float wp_bearing() const override { return _desired_yaw_cd * 0.01f; }
664 4716 : float nav_bearing() const override { return _desired_yaw_cd * 0.01f; }
665 4959 : float crosstrack_error_m() const override { return 0.0f; }
666 :
667 : // return desired location
668 : bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
669 :
670 : // return straight-line distance (in meters) to destination
671 4959 : float get_distance_to_destination() const override { return _distance_to_destination; }
672 :
673 : protected:
674 :
675 : bool _enter() override;
676 :
677 : Location _destination; // target location to hold position around
678 : float _desired_speed; // desired speed (ramped down from initial speed to zero)
679 : };
680 :
681 : class ModeManual : public Mode
682 : {
683 : public:
684 :
685 23306 : Number mode_number() const override { return Number::MANUAL; }
686 0 : const char *name() const override { return "Manual"; }
687 348 : const char *name4() const override { return "MANU"; }
688 :
689 : // methods that affect movement of the vehicle in this mode
690 : void update() override;
691 :
692 : // attributes for mavlink system status reporting
693 9007 : bool has_manual_input() const override { return true; }
694 72327 : bool attitude_stabilized() const override { return false; }
695 :
696 : // manual mode does not require position or velocity estimate
697 13022 : bool requires_position() const override { return false; }
698 13022 : bool requires_velocity() const override { return false; }
699 :
700 : protected:
701 :
702 : void _exit() override;
703 : };
704 :
705 :
706 : class ModeRTL : public Mode
707 : {
708 : public:
709 :
710 13354 : Number mode_number() const override { return Number::RTL; }
711 0 : const char *name() const override { return "RTL"; }
712 31 : const char *name4() const override { return "RTL"; }
713 :
714 : // methods that affect movement of the vehicle in this mode
715 : void update() override;
716 :
717 : // attributes of the mode
718 150590 : bool is_autopilot_mode() const override { return true; }
719 :
720 : // do not allow arming from this mode
721 98 : bool allows_arming() const override { return false; }
722 :
723 : // return desired location
724 : bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
725 :
726 : // return straight-line distance (in meters) to destination
727 23347 : float get_distance_to_destination() const override { return _distance_to_destination; }
728 : bool reached_destination() const override;
729 :
730 : // set desired speed in m/s
731 : bool set_desired_speed(float speed_ms) override;
732 :
733 : protected:
734 :
735 : bool _enter() override;
736 :
737 : bool send_notification; // used to send one time notification to ground station
738 : bool _loitering; // true if loitering at end of RTL
739 :
740 : };
741 :
742 : class ModeSmartRTL : public Mode
743 : {
744 : public:
745 :
746 12585 : Number mode_number() const override { return Number::SMART_RTL; }
747 0 : const char *name() const override { return "Smart RTL"; }
748 3 : const char *name4() const override { return "SRTL"; }
749 :
750 : // Return true if this mode is enabled, used by MAVLink available mode
751 : bool enabled() const override;
752 :
753 : // methods that affect movement of the vehicle in this mode
754 : void update() override;
755 :
756 : // attributes of the mode
757 5987 : bool is_autopilot_mode() const override { return true; }
758 :
759 : // do not allow arming from this mode
760 14 : bool allows_arming() const override { return false; }
761 :
762 : // return desired location
763 : bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
764 :
765 : // return straight-line distance (in meters) to destination
766 860 : float get_distance_to_destination() const override { return _distance_to_destination; }
767 0 : bool reached_destination() const override { return smart_rtl_state == SmartRTLState::StopAtHome; }
768 :
769 : // set desired speed in m/s
770 : bool set_desired_speed(float speed_ms) override;
771 :
772 : // save current position for use by the smart_rtl flight mode
773 : void save_position();
774 :
775 : protected:
776 :
777 : // Safe RTL states
778 : enum class SmartRTLState: uint8_t {
779 : WaitForPathCleanup,
780 : PathFollow,
781 : StopAtHome,
782 : Failure
783 : } smart_rtl_state;
784 :
785 : bool _enter() override;
786 : bool _load_point;
787 : bool _loitering; // true if loitering at end of SRTL
788 : };
789 :
790 :
791 :
792 : class ModeSteering : public Mode
793 : {
794 : public:
795 :
796 12657 : Number mode_number() const override { return Number::STEERING; }
797 0 : const char *name() const override { return "Steering"; }
798 11 : const char *name4() const override { return "STER"; }
799 :
800 : // methods that affect movement of the vehicle in this mode
801 : void update() override;
802 :
803 : // attributes for mavlink system status reporting
804 89 : bool has_manual_input() const override { return true; }
805 :
806 : // steering requires velocity but not position
807 61 : bool requires_position() const override { return false; }
808 61 : bool requires_velocity() const override { return true; }
809 :
810 : // return desired lateral acceleration
811 770 : float get_desired_lat_accel() const override { return _desired_lat_accel; }
812 :
813 : private:
814 :
815 : float _desired_lat_accel; // desired lateral acceleration calculated from pilot steering input
816 : };
817 :
818 : class ModeInitializing : public Mode
819 : {
820 : public:
821 :
822 1477 : Number mode_number() const override { return Number::INITIALISING; }
823 0 : const char *name() const override { return "Initialising"; }
824 285 : const char *name4() const override { return "INIT"; }
825 :
826 : // Return true if this mode is enabled, used by MAVLink available mode
827 12526 : bool enabled() const override { return false; };
828 :
829 : // methods that affect movement of the vehicle in this mode
830 0 : void update() override { }
831 :
832 : // do not allow arming from this mode
833 0 : bool allows_arming() const override { return false; }
834 :
835 : // attributes for mavlink system status reporting
836 1192 : bool has_manual_input() const override { return true; }
837 0 : bool attitude_stabilized() const override { return false; }
838 : protected:
839 0 : bool _enter() override { return false; };
840 : };
841 :
842 : #if MODE_FOLLOW_ENABLED
843 : class ModeFollow : public Mode
844 : {
845 : public:
846 :
847 0 : Number mode_number() const override { return Number::FOLLOW; }
848 0 : const char *name() const override { return "Follow"; }
849 0 : const char *name4() const override { return "FOLL"; }
850 :
851 : // Return true if this mode is enabled, used by MAVLink available mode
852 : bool enabled() const override;
853 :
854 : // methods that affect movement of the vehicle in this mode
855 : void update() override;
856 :
857 : // attributes of the mode
858 0 : bool is_autopilot_mode() const override { return true; }
859 :
860 : // return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
861 : float wp_bearing() const override;
862 0 : float nav_bearing() const override { return wp_bearing(); }
863 0 : float crosstrack_error_m() const override { return 0.0f; }
864 :
865 : // return desired location
866 0 : bool get_desired_location(Location& destination) const override WARN_IF_UNUSED { return false; }
867 :
868 : // return straight-line distance (in meters) to destination
869 : float get_distance_to_destination() const override;
870 :
871 : // set desired speed in m/s
872 : bool set_desired_speed(float speed_ms) override;
873 :
874 : protected:
875 :
876 : bool _enter() override;
877 : void _exit() override;
878 :
879 : float _desired_speed; // desired speed in m/s
880 : };
881 : #endif
882 :
883 : class ModeSimple : public Mode
884 : {
885 : public:
886 :
887 12526 : Number mode_number() const override { return Number::SIMPLE; }
888 0 : const char *name() const override { return "Simple"; }
889 0 : const char *name4() const override { return "SMPL"; }
890 :
891 : // methods that affect movement of the vehicle in this mode
892 : void update() override;
893 : void init_heading();
894 :
895 : // simple type enum used for SIMPLE_TYPE parameter
896 : enum simple_type {
897 : Simple_InitialHeading = 0,
898 : Simple_CardinalDirections = 1,
899 : };
900 :
901 : private:
902 :
903 : float _initial_heading_cd; // vehicle heading (in centi-degrees) at moment vehicle was armed
904 : float _desired_heading_cd; // latest desired heading (in centi-degrees) from pilot
905 : };
906 :
907 : #if MODE_DOCK_ENABLED
908 : class ModeDock : public Mode
909 : {
910 : public:
911 :
912 : // need a constructor for parameters
913 : ModeDock(void);
914 :
915 : // Does not allow copies
916 : CLASS_NO_COPY(ModeDock);
917 :
918 452 : Number mode_number() const override { return Number::DOCK; }
919 0 : const char *name() const override { return "Dock"; }
920 3 : const char *name4() const override { return "DOCK"; }
921 :
922 : // Return true if this mode is enabled, used by MAVLink available mode
923 : bool enabled() const override;
924 :
925 : // methods that affect movement of the vehicle in this mode
926 : void update() override;
927 :
928 19673 : bool is_autopilot_mode() const override { return true; }
929 :
930 : // return straight-line distance (in meters) to destination
931 2829 : float get_distance_to_destination() const override { return _distance_to_destination; }
932 :
933 : static const struct AP_Param::GroupInfo var_info[];
934 :
935 : protected:
936 :
937 : AP_Float speed; // dock mode speed
938 : AP_Float desired_dir; // desired direction of approach
939 : AP_Int8 hdg_corr_enable; // enable heading correction
940 : AP_Float hdg_corr_weight; // heading correction weight
941 : AP_Float stopping_dist; // how far away from the docking target should we start stopping
942 :
943 : bool _enter() override;
944 :
945 : // return reduced speed of vehicle based on error in position and current distance from the dock
946 : float apply_slowdown(float desired_speed);
947 :
948 : // calculate position of dock relative to the vehicle
949 : bool calc_dock_pos_rel_vehicle_NE_m(Vector2f &dock_pos_rel_vehicle_m) const;
950 :
951 : // we force the vehicle to use real dock target vector when this much close to the docking station
952 : const float _force_real_target_limit_m = 3.0f;
953 : // acceptable lateral error in vehicle's position with respect to dock. This is used while slowing down the vehicle
954 : const float _acceptable_pos_error_m = 0.2f;
955 :
956 : Vector2p _dock_pos_rel_origin_m; // position vector towards docking target relative to ekf origin
957 : Vector2f _desired_heading_NE; // unit vector in desired direction of docking
958 : bool _docking_complete = false; // flag to mark docking complete when we are close enough to the dock
959 : bool _loitering = false; // true if we are loitering after mission completion
960 : };
961 : #endif
|
