Line data Source code
1 : #pragma once
2 :
3 : #include "Sub.h"
4 : class Parameters;
5 : class ParametersG2;
6 :
7 : class GCS_Sub;
8 :
9 : // Guided modes
10 : enum GuidedSubMode {
11 : Guided_WP,
12 : Guided_Velocity,
13 : Guided_PosVelAccel,
14 : Guided_Angle,
15 : };
16 :
17 : // Auto modes
18 : enum AutoSubMode {
19 : Auto_WP,
20 : Auto_CircleMoveToEdge,
21 : Auto_Circle,
22 : Auto_NavGuided,
23 : Auto_Loiter,
24 : Auto_TerrainRecover
25 : };
26 :
27 : // RTL states
28 : enum RTLState {
29 : RTL_InitialClimb,
30 : RTL_ReturnHome,
31 : RTL_LoiterAtHome,
32 : RTL_FinalDescent,
33 : RTL_Land
34 : };
35 :
36 : class Mode
37 : {
38 :
39 : public:
40 :
41 : // Auto Pilot Modes enumeration
42 : enum class Number : uint8_t {
43 : STABILIZE = 0, // manual angle with manual depth/throttle
44 : ACRO = 1, // manual body-frame angular rate with manual depth/throttle
45 : ALT_HOLD = 2, // manual angle with automatic depth/throttle
46 : AUTO = 3, // fully automatic waypoint control using mission commands
47 : GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
48 : CIRCLE = 7, // automatic circular flight with automatic throttle
49 : SURFACE = 9, // automatically return to surface, pilot maintains horizontal control
50 : POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
51 : MANUAL = 19, // Pass-through input with no stabilization
52 : MOTOR_DETECT = 20, // Automatically detect motors orientation
53 : SURFTRAK = 21 // Track distance above seafloor (hold range)
54 : // Mode number 30 reserved for "offboard" for external/lua control.
55 : };
56 :
57 : // constructor
58 : Mode(void);
59 :
60 : // do not allow copying
61 : CLASS_NO_COPY(Mode);
62 :
63 : // child classes should override these methods
64 0 : virtual bool init(bool ignore_checks) { return true; }
65 : virtual void run() = 0;
66 : virtual bool requires_GPS() const = 0;
67 : virtual bool requires_altitude() const = 0;
68 : virtual bool allows_arming(bool from_gcs) const = 0;
69 0 : virtual bool is_autopilot() const { return false; }
70 34260 : virtual bool in_guided_mode() const { return false; }
71 :
72 : // return a string for this flightmode
73 : virtual const char *name() const = 0;
74 : virtual const char *name4() const = 0;
75 :
76 : // returns a unique number specific to this mode
77 : virtual Mode::Number number() const = 0;
78 :
79 : // pilot input processing
80 : void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
81 :
82 :
83 : protected:
84 :
85 : // navigation support functions
86 0 : virtual void run_autopilot() {}
87 :
88 : // helper functions
89 : bool is_disarmed_or_landed() const;
90 :
91 : // functions to control landing
92 : // in modes that support landing
93 : void land_run_horizontal_control();
94 : void land_run_vertical_control(bool pause_descent = false);
95 :
96 : // convenience references to avoid code churn in conversion:
97 : Parameters &g;
98 : ParametersG2 &g2;
99 : AP_AHRS &ahrs;
100 : AP_Motors6DOF &motors;
101 : RC_Channel *&channel_roll;
102 : RC_Channel *&channel_pitch;
103 : RC_Channel *&channel_throttle;
104 : RC_Channel *&channel_yaw;
105 : RC_Channel *&channel_forward;
106 : RC_Channel *&channel_lateral;
107 : AC_PosControl *position_control;
108 : AC_AttitudeControl_Sub *attitude_control;
109 : // TODO: channels
110 : float &G_Dt;
111 :
112 : public:
113 :
114 : // pass-through functions to reduce code churn on conversion;
115 : // these are candidates for moving into the Mode base
116 : // class.
117 : bool set_mode(Mode::Number mode, ModeReason reason);
118 : GCS_Sub &gcs();
119 :
120 : // end pass-through functions
121 : };
122 :
123 : class ModeManual : public Mode
124 : {
125 :
126 : public:
127 : // inherit constructor
128 : using Mode::Mode;
129 : virtual void run() override;
130 : bool init(bool ignore_checks) override;
131 24056 : bool requires_GPS() const override { return false; }
132 24149 : bool requires_altitude() const override { return false; }
133 0 : bool allows_arming(bool from_gcs) const override { return true; }
134 107 : bool is_autopilot() const override { return false; }
135 :
136 : protected:
137 :
138 0 : const char *name() const override { return "Manual"; }
139 107 : const char *name4() const override { return "MANU"; }
140 0 : Mode::Number number() const override { return Mode::Number::MANUAL; }
141 : };
142 :
143 :
144 : class ModeAcro : public Mode
145 : {
146 :
147 : public:
148 : // inherit constructor
149 : using Mode::Mode;
150 :
151 : virtual void run() override;
152 :
153 : bool init(bool ignore_checks) override;
154 20 : bool requires_GPS() const override { return false; }
155 21 : bool requires_altitude() const override { return false; }
156 0 : bool allows_arming(bool from_gcs) const override { return true; }
157 1 : bool is_autopilot() const override { return false; }
158 :
159 : protected:
160 :
161 0 : const char *name() const override { return "Acro"; }
162 1 : const char *name4() const override { return "ACRO"; }
163 0 : Mode::Number number() const override { return Mode::Number::ACRO; }
164 : };
165 :
166 :
167 : class ModeStabilize : public Mode
168 : {
169 :
170 : public:
171 : // inherit constructor
172 : using Mode::Mode;
173 :
174 : virtual void run() override;
175 :
176 : bool init(bool ignore_checks) override;
177 460 : bool requires_GPS() const override { return false; }
178 463 : bool requires_altitude() const override { return false; }
179 0 : bool allows_arming(bool from_gcs) const override { return true; }
180 7 : bool is_autopilot() const override { return false; }
181 :
182 : protected:
183 :
184 0 : const char *name() const override { return "Stabilize"; }
185 7 : const char *name4() const override { return "STAB"; }
186 0 : Mode::Number number() const override { return Mode::Number::STABILIZE; }
187 : };
188 :
189 :
190 : class ModeAlthold : public Mode
191 : {
192 :
193 : public:
194 : // inherit constructor
195 : using Mode::Mode;
196 :
197 : virtual void run() override;
198 :
199 : bool init(bool ignore_checks) override;
200 7218 : bool requires_GPS() const override { return false; }
201 7230 : bool requires_altitude() const override { return true; }
202 0 : bool allows_arming(bool from_gcs) const override { return true; }
203 16 : bool is_autopilot() const override { return false; }
204 : void control_depth();
205 :
206 : protected:
207 :
208 : void run_pre();
209 : void run_post();
210 :
211 0 : const char *name() const override { return "Depth Hold"; }
212 12 : const char *name4() const override { return "ALTH"; }
213 0 : Mode::Number number() const override { return Mode::Number::ALT_HOLD; }
214 : };
215 :
216 :
217 : class ModeSurftrak : public ModeAlthold
218 : {
219 :
220 : public:
221 : // constructor
222 : ModeSurftrak();
223 :
224 : void run() override;
225 :
226 : bool init(bool ignore_checks) override;
227 :
228 107554 : float get_rangefinder_target_cm() const WARN_IF_UNUSED { return rangefinder_target_cm; }
229 : bool set_rangefinder_target_cm(float target_cm);
230 :
231 : protected:
232 :
233 0 : const char *name() const override { return "Surftrak"; }
234 4 : const char *name4() const override { return "STRK"; }
235 0 : Mode::Number number() const override { return Mode::Number::SURFTRAK; }
236 :
237 : private:
238 :
239 : void reset();
240 : void control_range();
241 : void update_surface_offset();
242 :
243 : float rangefinder_target_cm;
244 :
245 : bool pilot_in_control; // pilot is moving up/down
246 : float pilot_control_start_z_cm; // alt when pilot took control
247 : };
248 :
249 : class ModeGuided : public Mode
250 : {
251 :
252 : public:
253 : // inherit constructor
254 : using Mode::Mode;
255 :
256 : virtual void run() override;
257 :
258 : bool init(bool ignore_checks) override;
259 4302 : bool requires_GPS() const override { return true; }
260 8 : bool requires_altitude() const override { return true; }
261 0 : bool allows_arming(bool from_gcs) const override { return true; }
262 6 : bool is_autopilot() const override { return true; }
263 17815 : bool in_guided_mode() const override { return true; }
264 : bool guided_limit_check();
265 : void guided_limit_init_time_and_pos();
266 : void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
267 : void guided_set_angle(const Quaternion&, float);
268 : void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
269 : bool guided_set_posvelaccel(const Vector3f& destination_neu_cm, const Vector3f& velocity_neu_cms, const Vector3f& accel_neu_cmss);
270 : bool guided_set_posvelaccel(const Vector3f& destination_neu_cm, const Vector3f& velocity_neu_cms, const Vector3f& accel_neu_cmss, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
271 : bool guided_set_destination(const Location&);
272 : bool guided_set_destination(const Vector3f& destination_neu_cm, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
273 : void guided_set_velocity(const Vector3f& velocity_neu_cms);
274 : void guided_set_velocity(const Vector3f& velocity_neu_cms, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
275 : void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
276 : // fills pos with the current Guided_PosVelAccel position target (NEU cm
277 : // relative to EKF origin). returns false if not in Guided_PosVelAccel.
278 : // velocity target not exposed; base GCS sends 0 with VX/VY/VZ_IGNORE.
279 : bool get_posvelaccel_target_NEU_cm(Vector3f &pos) const;
280 : float get_auto_heading();
281 : void guided_limit_clear();
282 : void set_auto_yaw_mode(autopilot_yaw_mode yaw_mode);
283 :
284 : protected:
285 :
286 16 : const char *name() const override { return "Guided"; }
287 6 : const char *name4() const override { return "GUID"; }
288 0 : Mode::Number number() const override { return Mode::Number::GUIDED; }
289 :
290 : autopilot_yaw_mode get_default_auto_yaw_mode(bool rtl) const;
291 :
292 : private:
293 : void guided_pos_control_run();
294 : void guided_vel_control_run();
295 : void guided_posvelaccel_control_run();
296 : void guided_angle_control_run();
297 : void guided_takeoff_run();
298 : void guided_pos_control_start();
299 : void guided_vel_control_start();
300 : void guided_posvelaccel_control_start();
301 : void guided_angle_control_start();
302 : };
303 :
304 :
305 :
306 : class ModeAuto : public ModeGuided
307 : {
308 :
309 : public:
310 : // inherit constructor
311 : using ModeGuided::ModeGuided;
312 :
313 : virtual void run() override;
314 :
315 : bool init(bool ignore_checks) override;
316 13637 : bool requires_GPS() const override { return true; }
317 14 : bool requires_altitude() const override { return true; }
318 0 : bool allows_arming(bool from_gcs) const override { return true; }
319 9 : bool is_autopilot() const override { return true; }
320 : bool auto_loiter_start();
321 : void auto_wp_start(const Vector3f& destination_neu_cm);
322 : void auto_wp_start(const Location& dest_loc);
323 : void auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn);
324 : void auto_circle_start();
325 : void auto_nav_guided_start();
326 : void set_auto_yaw_roi(const Location &roi_location);
327 : void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle);
328 : void set_yaw_rate(float turn_rate_dps);
329 : bool auto_terrain_recover_start();
330 :
331 : protected:
332 :
333 46 : const char *name() const override { return "Auto"; }
334 9 : const char *name4() const override { return "AUTO"; }
335 0 : Mode::Number number() const override { return Mode::Number::AUTO; }
336 :
337 : private:
338 : void auto_wp_run();
339 : void auto_circle_run();
340 : void auto_nav_guided_run();
341 : void auto_loiter_run();
342 : void auto_terrain_recover_run();
343 : };
344 :
345 :
346 : class ModePoshold : public ModeAlthold
347 : {
348 :
349 : public:
350 : // inherit constructor
351 : using ModeAlthold::ModeAlthold;
352 :
353 : virtual void run() override;
354 :
355 : bool init(bool ignore_checks) override;
356 :
357 2176 : bool requires_GPS() const override { return true; }
358 8 : bool requires_altitude() const override { return true; }
359 0 : bool allows_arming(bool from_gcs) const override { return true; }
360 6 : bool is_autopilot() const override { return true; }
361 :
362 : protected:
363 :
364 16 : const char *name() const override { return "Position Hold"; }
365 6 : const char *name4() const override { return "POSH"; }
366 0 : Mode::Number number() const override { return Mode::Number::POSHOLD; }
367 :
368 : private:
369 :
370 : void control_horizontal();
371 : };
372 :
373 :
374 : class ModeCircle : public Mode
375 : {
376 :
377 : public:
378 : // inherit constructor
379 : using Mode::Mode;
380 :
381 : virtual void run() override;
382 :
383 : bool init(bool ignore_checks) override;
384 128 : bool requires_GPS() const override { return true; }
385 14 : bool requires_altitude() const override { return true; }
386 0 : bool allows_arming(bool from_gcs) const override { return true; }
387 8 : bool is_autopilot() const override { return true; }
388 :
389 : protected:
390 :
391 47 : const char *name() const override { return "Circle"; }
392 8 : const char *name4() const override { return "CIRC"; }
393 0 : Mode::Number number() const override { return Mode::Number::CIRCLE; }
394 : };
395 :
396 : class ModeSurface : public Mode
397 : {
398 :
399 : public:
400 : // inherit constructor
401 : using Mode::Mode;
402 :
403 : virtual void run() override;
404 :
405 : bool init(bool ignore_checks) override;
406 616 : bool requires_GPS() const override { return false; }
407 618 : bool requires_altitude() const override { return false; }
408 0 : bool allows_arming(bool from_gcs) const override { return true; }
409 6 : bool is_autopilot() const override { return true; }
410 :
411 : protected:
412 0 : const char *name() const override { return "Surface"; }
413 6 : const char *name4() const override { return "SURF"; }
414 0 : Mode::Number number() const override { return Mode::Number::SURFACE; }
415 : bool nobaro_mode;
416 : };
417 :
418 :
419 : class ModeMotordetect : public Mode
420 : {
421 :
422 : public:
423 : // inherit constructor
424 : using Mode::Mode;
425 :
426 : virtual void run() override;
427 :
428 : bool init(bool ignore_checks) override;
429 0 : bool requires_GPS() const override { return false; }
430 0 : bool requires_altitude() const override { return false; }
431 0 : bool allows_arming(bool from_gcs) const override { return true; }
432 0 : bool is_autopilot() const override { return true; }
433 :
434 : protected:
435 :
436 0 : const char *name() const override { return "Motor Detection"; }
437 0 : const char *name4() const override { return "DETE"; }
438 0 : Mode::Number number() const override { return Mode::Number::MOTOR_DETECT; }
439 : };
|
