Line data Source code
1 : #include "Copter.h"
2 :
3 : /*
4 : * High level calls to set and update flight modes logic for individual
5 : * flight modes is in control_acro.cpp, control_stabilize.cpp, etc
6 : */
7 :
8 : /*
9 : constructor for Mode object
10 : */
11 24025 : Mode::Mode(void) :
12 24025 : g(copter.g),
13 24025 : g2(copter.g2),
14 24025 : wp_nav(copter.wp_nav),
15 24025 : loiter_nav(copter.loiter_nav),
16 24025 : pos_control(copter.pos_control),
17 24025 : ahrs(copter.ahrs),
18 24025 : attitude_control(copter.attitude_control),
19 24025 : motors(copter.motors),
20 24025 : channel_roll(copter.channel_roll),
21 24025 : channel_pitch(copter.channel_pitch),
22 24025 : channel_throttle(copter.channel_throttle),
23 24025 : channel_yaw(copter.channel_yaw),
24 24025 : G_Dt(copter.G_Dt)
25 24025 : { };
26 :
27 : #if AC_PAYLOAD_PLACE_ENABLED
28 : PayloadPlace Mode::payload_place;
29 : #endif
30 :
31 : // return the static controller object corresponding to supplied mode
32 1386 : Mode *Copter::mode_from_mode_num(const Mode::Number mode)
33 : {
34 :
35 1386 : switch (mode) {
36 : #if MODE_ACRO_ENABLED
37 10 : case Mode::Number::ACRO:
38 10 : return &mode_acro;
39 : #endif
40 :
41 164 : case Mode::Number::STABILIZE:
42 164 : return &mode_stabilize;
43 :
44 : #if MODE_ALTHOLD_ENABLED
45 175 : case Mode::Number::ALT_HOLD:
46 175 : return &mode_althold;
47 : #endif
48 :
49 : #if MODE_AUTO_ENABLED
50 98 : case Mode::Number::AUTO:
51 98 : return &mode_auto;
52 : #endif
53 :
54 : #if MODE_CIRCLE_ENABLED
55 22 : case Mode::Number::CIRCLE:
56 22 : return &mode_circle;
57 : #endif
58 :
59 : #if MODE_LOITER_ENABLED
60 215 : case Mode::Number::LOITER:
61 215 : return &mode_loiter;
62 : #endif
63 :
64 : #if MODE_GUIDED_ENABLED
65 160 : case Mode::Number::GUIDED:
66 160 : return &mode_guided;
67 : #endif
68 :
69 102 : case Mode::Number::LAND:
70 102 : return &mode_land;
71 :
72 : #if MODE_RTL_ENABLED
73 271 : case Mode::Number::RTL:
74 271 : return &mode_rtl;
75 : #endif
76 :
77 : #if MODE_DRIFT_ENABLED
78 3 : case Mode::Number::DRIFT:
79 3 : return &mode_drift;
80 : #endif
81 :
82 : #if MODE_SPORT_ENABLED
83 0 : case Mode::Number::SPORT:
84 0 : return &mode_sport;
85 : #endif
86 :
87 : #if MODE_FLIP_ENABLED
88 47 : case Mode::Number::FLIP:
89 47 : return &mode_flip;
90 : #endif
91 :
92 : #if AUTOTUNE_ENABLED
93 45 : case Mode::Number::AUTOTUNE:
94 45 : return &mode_autotune;
95 : #endif
96 :
97 : #if MODE_POSHOLD_ENABLED
98 12 : case Mode::Number::POSHOLD:
99 12 : return &mode_poshold;
100 : #endif
101 :
102 : #if MODE_BRAKE_ENABLED
103 9 : case Mode::Number::BRAKE:
104 9 : return &mode_brake;
105 : #endif
106 :
107 : #if MODE_THROW_ENABLED
108 6 : case Mode::Number::THROW:
109 6 : return &mode_throw;
110 : #endif
111 :
112 : #if AP_ADSB_AVOIDANCE_ENABLED
113 2 : case Mode::Number::AVOID_ADSB:
114 2 : return &mode_avoid_adsb;
115 : #endif
116 :
117 : #if MODE_GUIDED_NOGPS_ENABLED
118 2 : case Mode::Number::GUIDED_NOGPS:
119 2 : return &mode_guided_nogps;
120 : #endif
121 :
122 : #if MODE_SMARTRTL_ENABLED
123 16 : case Mode::Number::SMART_RTL:
124 16 : return &mode_smartrtl;
125 : #endif
126 :
127 : #if MODE_FLOWHOLD_ENABLED
128 1 : case Mode::Number::FLOWHOLD:
129 1 : return (Mode *)g2.mode_flowhold_ptr;
130 : #endif
131 :
132 : #if MODE_FOLLOW_ENABLED
133 7 : case Mode::Number::FOLLOW:
134 7 : return &mode_follow;
135 : #endif
136 :
137 : #if MODE_ZIGZAG_ENABLED
138 4 : case Mode::Number::ZIGZAG:
139 4 : return &mode_zigzag;
140 : #endif
141 :
142 : #if MODE_SYSTEMID_ENABLED
143 0 : case Mode::Number::SYSTEMID:
144 0 : return (Mode *)g2.mode_systemid_ptr;
145 : #endif
146 :
147 : #if MODE_AUTOROTATE_ENABLED
148 1 : case Mode::Number::AUTOROTATE:
149 1 : return &mode_autorotate;
150 : #endif
151 :
152 : #if MODE_TURTLE_ENABLED
153 0 : case Mode::Number::TURTLE:
154 0 : return &mode_turtle;
155 : #endif
156 :
157 14 : default:
158 14 : break;
159 : }
160 :
161 : #if MODE_GUIDED_ENABLED && AP_SCRIPTING_ENABLED
162 : // Check registered custom modes
163 24 : for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
164 22 : if ((mode_guided_custom[i] != nullptr) && (mode_guided_custom[i]->mode_number() == mode)) {
165 12 : return mode_guided_custom[i];
166 : }
167 : }
168 : #endif
169 :
170 2 : return nullptr;
171 : }
172 :
173 :
174 : // called when an attempt to change into a mode is unsuccessful:
175 117 : void Copter::mode_change_failed(const Mode *mode, const char *reason)
176 : {
177 117 : gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to %s failed: %s", mode->name(), reason);
178 117 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode->mode_number()));
179 : // make sad noise
180 117 : if (copter.ap.initialised) {
181 117 : AP_Notify::events.user_mode_change_failed = 1;
182 : }
183 117 : }
184 :
185 : // Check if this mode can be entered from the GCS
186 1515494 : bool Copter::gcs_mode_enabled(const Mode::Number mode_num)
187 : {
188 : // List of modes that can be blocked, index is bit number in parameter bitmask
189 : static const uint8_t mode_list [] {
190 : (uint8_t)Mode::Number::STABILIZE,
191 : (uint8_t)Mode::Number::ACRO,
192 : (uint8_t)Mode::Number::ALT_HOLD,
193 : (uint8_t)Mode::Number::AUTO,
194 : (uint8_t)Mode::Number::GUIDED,
195 : (uint8_t)Mode::Number::LOITER,
196 : (uint8_t)Mode::Number::CIRCLE,
197 : (uint8_t)Mode::Number::DRIFT,
198 : (uint8_t)Mode::Number::SPORT,
199 : (uint8_t)Mode::Number::FLIP,
200 : (uint8_t)Mode::Number::AUTOTUNE,
201 : (uint8_t)Mode::Number::POSHOLD,
202 : (uint8_t)Mode::Number::BRAKE,
203 : (uint8_t)Mode::Number::THROW,
204 : (uint8_t)Mode::Number::AVOID_ADSB,
205 : (uint8_t)Mode::Number::GUIDED_NOGPS,
206 : (uint8_t)Mode::Number::SMART_RTL,
207 : (uint8_t)Mode::Number::FLOWHOLD,
208 : (uint8_t)Mode::Number::FOLLOW,
209 : (uint8_t)Mode::Number::ZIGZAG,
210 : (uint8_t)Mode::Number::SYSTEMID,
211 : (uint8_t)Mode::Number::AUTOROTATE,
212 : (uint8_t)Mode::Number::AUTO_RTL,
213 : (uint8_t)Mode::Number::TURTLE
214 : };
215 :
216 1515494 : return !block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list));
217 : }
218 :
219 : // Return mask of enabled modes, order does not matter, its just for tracking changes
220 75223 : uint32_t Copter::get_available_mode_enabled_mask() const
221 : {
222 75223 : const Mode* modes[] {
223 : #if MODE_AUTO_ENABLED
224 : &copter.mode_auto,
225 : #endif
226 : #if MODE_ACRO_ENABLED
227 : &copter.mode_acro,
228 : #endif
229 : &copter.mode_stabilize,
230 : #if MODE_ALTHOLD_ENABLED
231 : &copter.mode_althold,
232 : #endif
233 : #if MODE_CIRCLE_ENABLED
234 : &copter.mode_circle,
235 : #endif
236 : #if MODE_LOITER_ENABLED
237 : &copter.mode_loiter,
238 : #endif
239 : #if MODE_GUIDED_ENABLED
240 : &copter.mode_guided,
241 : #endif
242 : &copter.mode_land,
243 : #if MODE_RTL_ENABLED
244 : &copter.mode_rtl,
245 : #endif
246 : #if MODE_DRIFT_ENABLED
247 : &copter.mode_drift,
248 : #endif
249 : #if MODE_SPORT_ENABLED
250 : &copter.mode_sport,
251 : #endif
252 : #if MODE_FLIP_ENABLED
253 : &copter.mode_flip,
254 : #endif
255 : #if AUTOTUNE_ENABLED
256 : &copter.mode_autotune,
257 : #endif
258 : #if MODE_POSHOLD_ENABLED
259 : &copter.mode_poshold,
260 : #endif
261 : #if MODE_BRAKE_ENABLED
262 : &copter.mode_brake,
263 : #endif
264 : #if MODE_THROW_ENABLED
265 : &copter.mode_throw,
266 : #endif
267 : #if AP_ADSB_AVOIDANCE_ENABLED
268 : &copter.mode_avoid_adsb,
269 : #endif
270 : #if MODE_GUIDED_NOGPS_ENABLED
271 : &copter.mode_guided_nogps,
272 : #endif
273 : #if MODE_SMARTRTL_ENABLED
274 : &copter.mode_smartrtl,
275 : #endif
276 : #if MODE_FLOWHOLD_ENABLED
277 75223 : (Mode*)copter.g2.mode_flowhold_ptr,
278 : #endif
279 : #if MODE_FOLLOW_ENABLED
280 : &copter.mode_follow,
281 : #endif
282 : #if MODE_ZIGZAG_ENABLED
283 : &copter.mode_zigzag,
284 : #endif
285 : #if MODE_SYSTEMID_ENABLED
286 75223 : (Mode *)copter.g2.mode_systemid_ptr,
287 : #endif
288 : #if MODE_AUTOROTATE_ENABLED
289 : &copter.mode_autorotate,
290 : #endif
291 : #if MODE_TURTLE_ENABLED
292 : &copter.mode_turtle,
293 : #endif
294 75223 : };
295 :
296 : static_assert(ARRAY_SIZE(modes) <= 32, "Flight modes must fit in 32 bit bitmask");
297 :
298 75223 : uint32_t mask = 0;
299 1880575 : for (uint8_t i = 0; i < ARRAY_SIZE(modes); i++) {
300 1805352 : const Mode* mode = modes[i];
301 :
302 : // the check here must be the same as the one in `send_available_mode`
303 1805352 : const bool user_selectable = mode->enabled() && copter.gcs_mode_enabled(mode->mode_number());
304 :
305 1805352 : if (user_selectable) {
306 1514248 : mask |= 1U << i;
307 : }
308 : }
309 :
310 75223 : return mask;
311 : }
312 :
313 : // set_mode - change flight mode and perform any necessary initialisation
314 : // optional force parameter used to force the flight mode change (used only first time mode is set)
315 : // returns true if mode was successfully set
316 : // compiled-in ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT modes can always be set successfully,
317 : // but the return state of other flight modes should be checked and the caller should deal with failures appropriately
318 3804 : bool Copter::set_mode(Mode::Number mode, ModeReason reason)
319 : {
320 : // update last reason
321 3804 : const ModeReason last_reason = _last_reason;
322 3804 : _last_reason = reason;
323 :
324 : // return immediately if we are already in the desired mode
325 3804 : if (mode == flightmode->mode_number()) {
326 2406 : control_mode_reason = reason;
327 : // set yaw rate time constant during autopilot startup
328 2406 : if (reason == ModeReason::INITIALISED && mode == Mode::Number::STABILIZE) {
329 996 : attitude_control->set_yaw_rate_tc(g2.command_model_pilot_y.get_rate_tc());
330 : }
331 : // make happy noise
332 2406 : if (copter.ap.initialised && (reason != last_reason)) {
333 1323 : AP_Notify::events.user_mode_change = 1;
334 : }
335 2406 : return true;
336 : }
337 :
338 : // Check if GCS mode change is disabled via parameter
339 1398 : if ((reason == ModeReason::GCS_COMMAND) && !gcs_mode_enabled(mode)) {
340 : // Mode disabled, try and grab a mode name to give a better warning.
341 0 : const Mode *new_flightmode = mode_from_mode_num(mode);
342 0 : if (new_flightmode == nullptr) {
343 0 : notify_no_such_mode((uint8_t)mode);
344 : } else {
345 0 : mode_change_failed(new_flightmode, "GCS entry disabled (FLTMODE_GCSBLOCK)");
346 : }
347 0 : return false;
348 : }
349 :
350 : #if MODE_AUTO_ENABLED
351 1398 : if (mode == Mode::Number::AUTO_RTL) {
352 : // Special case for AUTO RTL, not a true mode, just AUTO in disguise
353 : // Attempt to join return path, fallback to do-land-start
354 13 : return mode_auto.return_path_or_jump_to_landing_sequence_auto_RTL(reason);
355 : }
356 : #endif
357 :
358 1385 : Mode *new_flightmode = mode_from_mode_num(mode);
359 1385 : if (new_flightmode == nullptr) {
360 1 : notify_no_such_mode((uint8_t)mode);
361 1 : return false;
362 : }
363 :
364 1384 : bool ignore_checks = !motors->armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
365 :
366 : #if FRAME_CONFIG == HELI_FRAME
367 : // do not allow helis to enter a non-manual throttle mode if the
368 : // rotor runup is not complete
369 139 : if (!ignore_checks && !new_flightmode->has_manual_throttle() && motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
370 20 : mode_change_failed(new_flightmode, "runup not complete");
371 20 : return false;
372 : }
373 : #endif
374 :
375 : #if FRAME_CONFIG != HELI_FRAME
376 : // ensure vehicle doesn't leap off the ground if a user switches
377 : // into a manual throttle mode from a non-manual-throttle mode
378 : // (e.g. user arms in guided, raises throttle to 1300 (not enough to
379 : // trigger auto takeoff), then switches into manual):
380 1245 : bool user_throttle = new_flightmode->has_manual_throttle();
381 : #if MODE_DRIFT_ENABLED
382 1245 : if (new_flightmode == &mode_drift) {
383 2 : user_throttle = true;
384 : }
385 : #endif
386 3122 : if (!ignore_checks &&
387 632 : ap.land_complete &&
388 6 : user_throttle &&
389 1881 : !copter.flightmode->has_manual_throttle() &&
390 4 : new_flightmode->get_pilot_desired_throttle() > copter.get_non_takeoff_throttle()) {
391 3 : mode_change_failed(new_flightmode, "throttle too high");
392 3 : return false;
393 : }
394 : #endif
395 :
396 665 : if (!ignore_checks &&
397 2026 : new_flightmode->requires_position() &&
398 425 : !copter.position_ok()) {
399 4 : mode_change_failed(new_flightmode, "requires position");
400 4 : return false;
401 : }
402 :
403 : // check for valid altitude if old mode did not require it but new one does
404 : // we only want to stop changing modes if it could make things worse
405 3375 : if (!ignore_checks &&
406 672 : !copter.ekf_alt_ok() &&
407 2029 : flightmode->has_manual_throttle() &&
408 11 : !new_flightmode->has_manual_throttle()) {
409 11 : mode_change_failed(new_flightmode, "need alt estimate");
410 11 : return false;
411 : }
412 :
413 : #if AP_FENCE_ENABLED
414 : // may not be allowed to change mode if recovering from fence breach
415 650 : if (!ignore_checks &&
416 699 : fence.enabled() &&
417 49 : fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) &&
418 0 : fence.get_breaches() &&
419 0 : motors->armed() &&
420 1996 : get_control_mode_reason() == ModeReason::FENCE_BREACHED &&
421 0 : !ap.land_complete) {
422 0 : mode_change_failed(new_flightmode, "in fence recovery");
423 0 : return false;
424 : }
425 : #endif
426 :
427 1346 : if (rc().in_rc_failsafe() && !new_flightmode->allows_entry_in_rc_failsafe()) {
428 1 : mode_change_failed(new_flightmode, "in RC failsafe");
429 1 : return false;
430 : }
431 :
432 1345 : if (!new_flightmode->init(ignore_checks)) {
433 78 : mode_change_failed(new_flightmode, "init failed");
434 78 : return false;
435 : }
436 :
437 : // perform any cleanup required by previous flight mode
438 1267 : exit_mode(flightmode, new_flightmode);
439 :
440 : // update flight mode
441 1267 : flightmode = new_flightmode;
442 1267 : control_mode_reason = reason;
443 : #if HAL_LOGGING_ENABLED
444 1267 : logger.Write_Mode((uint8_t)flightmode->mode_number(), reason);
445 : #endif
446 1267 : gcs().send_message(MSG_HEARTBEAT);
447 :
448 : #if HAL_ADSB_ENABLED
449 1267 : adsb.set_is_auto_mode((mode == Mode::Number::AUTO) || (mode == Mode::Number::RTL) || (mode == Mode::Number::GUIDED));
450 : #endif
451 :
452 : #if AP_FENCE_ENABLED
453 1267 : if (fence.get_action() != AC_Fence::Action::REPORT_ONLY) {
454 : // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
455 : // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
456 : // but it should be harmless to disable the fence temporarily in these situations as well
457 1259 : fence.manual_recovery_start();
458 : }
459 : #endif
460 :
461 : #if AP_CAMERA_ENABLED
462 1267 : camera.set_is_auto_mode(flightmode->mode_number() == Mode::Number::AUTO);
463 : #endif
464 :
465 : // set rate shaping time constants
466 : #if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
467 1267 : attitude_control->set_roll_pitch_rate_tc(g2.command_model_acro_rp.get_rate_tc());
468 : #endif
469 1267 : attitude_control->set_yaw_rate_tc(g2.command_model_pilot_y.get_rate_tc());
470 : #if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
471 1267 : if (mode== Mode::Number::ACRO || mode== Mode::Number::DRIFT) {
472 11 : attitude_control->set_yaw_rate_tc(g2.command_model_acro_y.get_rate_tc());
473 : }
474 : #endif
475 :
476 : // update notify object
477 1267 : notify_flight_mode();
478 :
479 : // make happy noise
480 1267 : if (copter.ap.initialised) {
481 1265 : AP_Notify::events.user_mode_change = 1;
482 : }
483 :
484 : // return success
485 1267 : return true;
486 : }
487 :
488 1656 : bool Copter::set_mode(const uint8_t new_mode, const ModeReason reason)
489 : {
490 : static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
491 : #ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
492 : if (reason == ModeReason::GCS_COMMAND && copter.failsafe.radio) {
493 : // don't allow mode changes while in radio failsafe
494 : return false;
495 : }
496 : #endif
497 1656 : return copter.set_mode(static_cast<Mode::Number>(new_mode), reason);
498 : }
499 :
500 : // update_flight_mode - calls the appropriate attitude controllers based on flight mode
501 : // called at 100hz or more
502 30412448 : void Copter::update_flight_mode()
503 : {
504 : #if AP_RANGEFINDER_ENABLED
505 30412448 : surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used
506 : #endif
507 30412448 : attitude_control->landed_gain_reduction(copter.ap.land_complete); // Adjust gains when landed to attenuate ground oscillation
508 :
509 : // set ekf reset handling method
510 30412448 : pos_control->set_reset_handling_method(flightmode->move_vehicle_on_ekf_reset() ? AC_PosControl::EKFResetMethod::MoveVehicle : AC_PosControl::EKFResetMethod::MoveTarget);
511 :
512 30412448 : flightmode->run();
513 30412448 : }
514 :
515 : // exit_mode - high level call to organise cleanup as a flight mode is exited
516 1267 : void Copter::exit_mode(Mode *&old_flightmode,
517 : Mode *&new_flightmode)
518 : {
519 : // smooth throttle transition when switching from manual to automatic flight modes
520 1267 : if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && motors->armed() && !ap.land_complete) {
521 : // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
522 57 : set_accel_throttle_I_from_pilot_throttle();
523 : }
524 :
525 : // cancel any takeoffs in progress
526 1267 : old_flightmode->takeoff_stop();
527 :
528 : // perform cleanup required for each flight mode
529 1267 : old_flightmode->exit();
530 :
531 : #if FRAME_CONFIG == HELI_FRAME
532 : //last collective output
533 87 : input_manager.set_last_coll_output(motors->get_throttle());
534 :
535 : // if we are changing from a mode that did not use manual throttle,
536 : // collective ramp functions should be called to blend the transition
537 87 : if (new_flightmode->has_manual_throttle()) {
538 16 : input_manager.set_collective_ramp(1.0);
539 : }
540 :
541 : // Make sure inverted flight is disabled if not supported in the new mode
542 87 : if (!new_flightmode->allows_inverted()) {
543 44 : attitude_control->set_inverted_flight(false);
544 : }
545 : #endif //HELI_FRAME
546 1267 : }
547 :
548 : // notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device
549 2268 : void Copter::notify_flight_mode() {
550 2268 : AP_Notify::flags.autopilot_mode = flightmode->is_autopilot();
551 2268 : AP_Notify::flags.flight_mode = (uint8_t)flightmode->mode_number();
552 2268 : notify.set_flight_mode_str(flightmode->name4());
553 2268 : }
554 :
555 : // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
556 : // returns desired angle in radians
557 15560762 : void Mode::get_pilot_desired_lean_angles_rad(float &roll_out_rad, float &pitch_out_rad, float angle_max_rad, float angle_limit_rad) const
558 : {
559 : // throttle failsafe check
560 15560762 : if (!rc().has_valid_input()) {
561 29653 : roll_out_rad = 0.0;
562 29653 : pitch_out_rad = 0.0;
563 29653 : return;
564 : }
565 :
566 : // fetch roll and pitch inputs
567 15531109 : float roll_in_norm = channel_roll->norm_input_dz();
568 15531109 : float pitch_in_norm = channel_pitch->norm_input_dz();
569 :
570 : // apply SIMPLE mode transform to pilot inputs
571 15531109 : apply_simple_mode(roll_in_norm, pitch_in_norm);
572 :
573 : // transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command
574 15531109 : rc_input_to_roll_pitch_rad(roll_in_norm, pitch_in_norm, angle_max_rad, angle_limit_rad, roll_out_rad, pitch_out_rad);
575 : }
576 :
577 : // transform pilot's roll or pitch input into a desired velocity
578 4313890 : Vector2f Mode::get_pilot_desired_velocity(float vel_max) const
579 : {
580 4313890 : Vector2f vel_ne_ms;
581 :
582 4313890 : if (!rc().has_valid_input()) {
583 0 : return vel_ne_ms;
584 : }
585 : // fetch roll and pitch inputs
586 4313890 : float roll_out_norm = channel_roll->norm_input_dz();
587 4313890 : float pitch_out_norm = channel_pitch->norm_input_dz();
588 :
589 : // apply SIMPLE mode transform to pilot inputs
590 4313890 : apply_simple_mode(roll_out_norm, pitch_out_norm);
591 :
592 : // convert roll and pitch inputs into velocity in NE frame
593 4313890 : vel_ne_ms = Vector2f(-pitch_out_norm, roll_out_norm);
594 4313890 : if (vel_ne_ms.is_zero()) {
595 4292523 : return vel_ne_ms;
596 : }
597 21367 : vel_ne_ms = copter.ahrs.body_to_earth2D(vel_ne_ms);
598 :
599 : // Transform square input range to circular output
600 : // vel_scalar is the vector to the edge of the +- 1.0 square in the direction of the current input
601 21367 : Vector2f vel_scalar = vel_ne_ms / MAX(fabsf(vel_ne_ms.x), fabsf(vel_ne_ms.y));
602 : // We scale the output by the ratio of the distance to the square to the unit circle and multiply by vel_max
603 21367 : vel_ne_ms *= vel_max / vel_scalar.length();
604 21367 : return vel_ne_ms;
605 : }
606 :
607 4741379 : bool Mode::_TakeOff::triggered_ms(const float target_climb_rate_ms) const
608 : {
609 4741379 : if (!copter.ap.land_complete) {
610 : // can't take off if we're already flying
611 4338666 : return false;
612 : }
613 402713 : if (target_climb_rate_ms <= 0.0f) {
614 : // can't takeoff unless we want to go up...
615 273540 : return false;
616 : }
617 :
618 129173 : if (copter.motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
619 : // hold aircraft on the ground until rotor speed runup has finished
620 129008 : return false;
621 : }
622 :
623 165 : return true;
624 : }
625 :
626 10843302 : bool Mode::is_disarmed_or_landed() const
627 : {
628 10843302 : if (!motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) {
629 1746406 : return true;
630 : }
631 9096896 : return false;
632 : }
633 :
634 813 : void Mode::zero_throttle_and_relax_ac(bool spool_up)
635 : {
636 813 : if (spool_up) {
637 0 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
638 : } else {
639 813 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
640 : }
641 813 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(0.0f, 0.0f, 0.0f);
642 813 : attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
643 813 : }
644 :
645 0 : void Mode::zero_throttle_and_hold_attitude()
646 : {
647 : // run attitude controller
648 0 : attitude_control->input_rate_bf_roll_pitch_yaw_rads(0.0f, 0.0f, 0.0f);
649 0 : attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
650 0 : }
651 :
652 : // handle situations where the vehicle is on the ground waiting for takeoff
653 : // force_throttle_unlimited should be true in cases where we want to keep the motors spooled up
654 : // (instead of spooling down to ground idle). This is required for tradheli's in Guided and Auto
655 : // where we always want the motor spooled up in Guided or Auto mode. Tradheli's main rotor stops
656 : // when spooled down to ground idle.
657 : // ultimately it forces the motor interlock to be obeyed in auto and guided modes when on the ground.
658 1999875 : void Mode::make_safe_ground_handling(bool force_throttle_unlimited)
659 : {
660 1999875 : if (force_throttle_unlimited) {
661 : // keep rotors turning
662 8636 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
663 : } else {
664 : // spool down to ground idle
665 1991239 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
666 : }
667 :
668 : // aircraft is landed, integrator terms must be reset regardless of spool state
669 1999875 : attitude_control->reset_rate_controller_I_terms_smoothly();
670 :
671 1999875 : switch (motors->get_spool_state()) {
672 1831331 : case AP_Motors::SpoolState::SHUT_DOWN:
673 : case AP_Motors::SpoolState::GROUND_IDLE:
674 : // reset yaw targets and rates during idle states
675 1831331 : attitude_control->reset_yaw_target_and_rate();
676 1831331 : break;
677 168544 : case AP_Motors::SpoolState::SPOOLING_UP:
678 : case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
679 : case AP_Motors::SpoolState::SPOOLING_DOWN:
680 : // while transitioning though active states continue to operate normally
681 168544 : break;
682 : }
683 :
684 1999875 : pos_control->NE_relax_velocity_controller();
685 1999875 : pos_control->NE_update_controller();
686 1999875 : pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero
687 1999875 : pos_control->D_update_controller();
688 : // we may need to move this out
689 1999875 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(0.0f, 0.0f, 0.0f);
690 1999875 : }
691 :
692 : /*
693 : get a height above ground estimate for landing
694 : */
695 8870124 : float Mode::get_alt_above_ground_m(void) const
696 : {
697 : float alt_above_ground_m;
698 8870124 : if (copter.get_rangefinder_height_interpolated_m(alt_above_ground_m)) {
699 509087 : return alt_above_ground_m;
700 : }
701 8361037 : if (!copter.current_loc.initialised()) {
702 : // current loc uninitialised during startup, return zero
703 104 : return 0;
704 : }
705 8360933 : if (copter.current_loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_m)) {
706 7679931 : return alt_above_ground_m;
707 : }
708 :
709 : // Assume the Earth is flat:
710 681002 : return copter.current_loc.alt * 0.01;
711 : }
712 :
713 4542700 : void Mode::land_run_vertical_control(bool pause_descent)
714 : {
715 4542700 : float climb_rate_ms = 0;
716 4542700 : bool ignore_descent_limit = false;
717 4542700 : if (!pause_descent) {
718 :
719 : // do not ignore limits until we have slowed down for landing
720 4514392 : const float land_alt_low_m = copter.mode_land.get_land_alt_low_m();
721 4514392 : ignore_descent_limit = (MAX(land_alt_low_m, 1) > get_alt_above_ground_m()) || copter.ap.land_complete_maybe;
722 :
723 : float max_land_descent_speed_ms;
724 4514392 : const float land_speed_high_ms = copter.mode_land.get_land_speed_high_ms();
725 4514392 : if (land_speed_high_ms > 0) {
726 20648 : max_land_descent_speed_ms = land_speed_high_ms;
727 : } else {
728 4493744 : max_land_descent_speed_ms = pos_control->get_max_speed_down_ms();
729 : }
730 :
731 : // Don't speed up for landing.
732 4514392 : const float land_speed_ms = copter.mode_land.get_land_speed_ms();
733 4514392 : max_land_descent_speed_ms = MAX(max_land_descent_speed_ms, fabsf(land_speed_ms));
734 :
735 : // Compute a vertical velocity demand such that the vehicle approaches land_alt_low. Without the below constraint, this would cause the vehicle to hover at land_alt_low.
736 4514392 : climb_rate_ms = sqrt_controller(MAX(land_alt_low_m, 1) - get_alt_above_ground_m(), pos_control->D_get_pos_p().kP(), pos_control->D_get_max_accel_mss(), G_Dt);
737 :
738 : // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
739 4514392 : climb_rate_ms = constrain_float(climb_rate_ms, -max_land_descent_speed_ms, -fabsf(land_speed_ms));
740 :
741 : #if AC_PRECLAND_ENABLED
742 4514392 : const bool navigating = pos_control->NE_is_active();
743 4514392 : bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating;
744 :
745 4514392 : if (doing_precision_landing) {
746 : // prec landing is active
747 34267 : Vector2p target_pos_ne_m;
748 34267 : float target_error_m = 0.0f;
749 34267 : if (copter.precland.get_target_position_m(target_pos_ne_m)) {
750 34267 : const Vector2p current_pos_ne_m = pos_control->get_pos_estimate_NED_m().xy();
751 : // target is this many m away from the vehicle
752 34267 : target_error_m = (target_pos_ne_m - current_pos_ne_m).tofloat().length();
753 : }
754 : // check if we should descend or not
755 34267 : const float max_horiz_pos_error_m = copter.precland.get_max_xy_error_before_descending_m();
756 34267 : Vector3f target_pos_meas_ned_m;
757 34267 : copter.precland.get_target_position_measurement_NED_m(target_pos_meas_ned_m);
758 34267 : if (target_error_m > max_horiz_pos_error_m && !is_zero(max_horiz_pos_error_m)) {
759 : // doing precland but too far away from the obstacle
760 : // do not descend
761 2128 : climb_rate_ms = 0.0f;
762 32139 : } else if (target_pos_meas_ned_m.z > 0.35 && target_pos_meas_ned_m.z < 2.0 && !copter.precland.do_fast_descend()) {
763 : // very close to the ground and doing prec land, lets slow down to make sure we land on target
764 : // compute desired descent velocity
765 10320 : const float precland_acceptable_error_m = 0.15;
766 10320 : const float precland_min_descent_speed_ms = 0.1;
767 10320 : const float max_descent_speed_ms = fabsf(land_speed_ms) * 0.5;
768 10320 : const float land_slowdown_ms = MAX(0.0f, target_error_m * (max_descent_speed_ms / precland_acceptable_error_m));
769 10320 : climb_rate_ms = MIN(-precland_min_descent_speed_ms, -max_descent_speed_ms + land_slowdown_ms);
770 : }
771 : }
772 : #endif
773 : }
774 :
775 : // update altitude target and call position controller
776 4542700 : pos_control->D_set_pos_target_from_climb_rate_ms(climb_rate_ms, ignore_descent_limit);
777 4542700 : pos_control->D_update_controller();
778 4542700 : }
779 :
780 4484987 : void Mode::land_run_horizontal_control()
781 : {
782 4484987 : Vector2f vel_correction_ms;
783 :
784 : // relax loiter target if we might be landed
785 4484987 : if (copter.ap.land_complete_maybe) {
786 118449 : pos_control->NE_soften_for_landing();
787 : }
788 :
789 : // process pilot inputs
790 4484987 : if (rc().has_valid_input()) {
791 4295438 : if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
792 0 : LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT);
793 : // exit land if throttle is high
794 0 : if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
795 : #if MODE_ALTHOLD_ENABLED
796 0 : set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
797 : #endif
798 : }
799 : }
800 :
801 4295438 : if (g.land_repositioning) {
802 : // convert pilot input to reposition velocity
803 : // use half maximum acceleration as the maximum velocity to ensure aircraft will
804 : // stop from full reposition speed in less than 1 second.
805 4295438 : const float max_pilot_vel_ms = wp_nav->get_wp_acceleration_mss() * 0.5;
806 4295438 : vel_correction_ms = get_pilot_desired_velocity(max_pilot_vel_ms);
807 :
808 : // record if pilot has overridden roll or pitch
809 4295438 : if (!vel_correction_ms.is_zero()) {
810 21367 : if (!copter.ap.land_repo_active) {
811 4 : LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE);
812 : }
813 21367 : copter.ap.land_repo_active = true;
814 : #if AC_PRECLAND_ENABLED
815 : } else {
816 : // no override right now, check if we should allow precland
817 4274071 : if (copter.precland.allow_precland_after_reposition()) {
818 0 : copter.ap.land_repo_active = false;
819 : }
820 : #endif
821 : }
822 : }
823 : }
824 :
825 : // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle
826 4484987 : copter.ap.prec_land_active = false;
827 : #if AC_PRECLAND_ENABLED
828 4484987 : copter.ap.prec_land_active = !copter.ap.land_repo_active && copter.precland.target_acquired();
829 : // run precision landing
830 4484987 : if (copter.ap.prec_land_active) {
831 34267 : Vector2p target_pos_ne_m;
832 34267 : Vector2f target_vel_ne_ms;
833 34267 : if (!copter.precland.get_target_position_m(target_pos_ne_m)) {
834 0 : target_pos_ne_m = pos_control->get_pos_estimate_NED_m().xy();
835 : }
836 : // get the velocity of the target
837 34267 : copter.precland.get_target_velocity_ms(pos_control->get_vel_estimate_NED_ms().xy(), target_vel_ne_ms);
838 :
839 34267 : Vector2f accel_ne_zero;
840 : // target vel will remain zero if landing target is stationary
841 34267 : pos_control->input_pos_vel_accel_NE_m(target_pos_ne_m, target_vel_ne_ms, accel_ne_zero);
842 : }
843 : #endif
844 :
845 4484987 : if (!copter.ap.prec_land_active) {
846 4450720 : Vector2f accel_ne_zero;
847 4450720 : pos_control->input_vel_accel_NE_m(vel_correction_ms, accel_ne_zero);
848 : }
849 :
850 : // run pos controller
851 4484987 : pos_control->NE_update_controller();
852 4484987 : Vector3f thrust_vector = pos_control->get_thrust_vector();
853 :
854 : // call attitude controller
855 4484987 : attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.get_heading());
856 :
857 4484987 : }
858 :
859 : // run normal or precision landing (if enabled)
860 : // pause_descent is true if vehicle should not descend
861 4453760 : void Mode::land_run_normal_or_precland(bool pause_descent)
862 : {
863 : #if AC_PRECLAND_ENABLED
864 4453760 : if (pause_descent || !copter.precland.enabled()) {
865 : // we don't want to start descending immediately or prec land is disabled
866 : // in both cases just run simple land controllers
867 4389903 : land_run_horiz_and_vert_control(pause_descent);
868 : } else {
869 : // prec land is enabled and we have not paused descent
870 : // the state machine takes care of the entire prec landing procedure
871 63857 : precland_run();
872 : }
873 : #else
874 : land_run_horiz_and_vert_control(pause_descent);
875 : #endif
876 4453760 : }
877 :
878 : #if AC_PRECLAND_ENABLED
879 : // Go towards a position commanded by prec land state machine in order to retry landing
880 : // The passed in location is expected to be NED and in m
881 0 : void Mode::precland_retry_position(const Vector3p &retry_pos_ned_m)
882 : {
883 0 : if (rc().has_valid_input()) {
884 0 : if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
885 0 : LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT);
886 : // exit land if throttle is high
887 0 : if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
888 : #if MODE_ALTHOLD_ENABLED
889 0 : set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
890 : #endif
891 : }
892 : }
893 :
894 : // allow user to take control during repositioning. Note: copied from land_run_horizontal_control()
895 : // To-Do: this code exists at several different places in slightly different forms and that should be fixed
896 0 : if (g.land_repositioning) {
897 0 : float target_roll_rad = 0.0f;
898 0 : float target_pitch_rad = 0.0f;
899 : // convert pilot input to lean angles
900 0 : get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, loiter_nav->get_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
901 :
902 : // record if pilot has overridden roll or pitch
903 0 : if (!is_zero(target_roll_rad) || !is_zero(target_pitch_rad)) {
904 0 : if (!copter.ap.land_repo_active) {
905 0 : LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE);
906 : }
907 : // this flag will be checked by prec land state machine later and any further landing retires will be cancelled
908 0 : copter.ap.land_repo_active = true;
909 : }
910 : }
911 : }
912 :
913 0 : pos_control->input_pos_NED_m(retry_pos_ned_m, 0.0f, 10.0);
914 :
915 : // run position controllers
916 0 : pos_control->NE_update_controller();
917 0 : pos_control->D_update_controller();
918 :
919 : // call attitude controller
920 0 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
921 0 : }
922 :
923 : // Run precland statemachine. This function should be called from any mode that wants to do precision landing.
924 : // This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
925 63857 : void Mode::precland_run()
926 : {
927 : // if user is taking control, we will not run the statemachine, and simply land (may or may not be on target)
928 63857 : if (!copter.ap.land_repo_active) {
929 : // This will get updated later to a retry pos if needed
930 63857 : Vector3p retry_pos_ned_m;
931 :
932 63857 : switch (copter.precland_statemachine.update(retry_pos_ned_m)) {
933 0 : case AC_PrecLand_StateMachine::Status::RETRYING:
934 : // we want to retry landing by going to another position
935 0 : precland_retry_position(retry_pos_ned_m);
936 0 : break;
937 :
938 17160 : case AC_PrecLand_StateMachine::Status::FAILSAFE: {
939 : // we have hit a failsafe. Failsafe can only mean two things, we either want to stop permanently till user takes over or land
940 17160 : switch (copter.precland_statemachine.get_failsafe_actions()) {
941 11560 : case AC_PrecLand_StateMachine::FailSafeAction::DESCEND:
942 : // descend normally, prec land target is definitely not in sight
943 11560 : land_run_horiz_and_vert_control();
944 11560 : break;
945 5600 : case AC_PrecLand_StateMachine::FailSafeAction::HOLD_POS:
946 : // sending "true" in this argument will stop the descend
947 5600 : land_run_horiz_and_vert_control(true);
948 5600 : break;
949 : }
950 17160 : break;
951 : }
952 0 : case AC_PrecLand_StateMachine::Status::ERROR:
953 : // should never happen, is certainly a bug. Report then descend
954 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
955 : FALLTHROUGH;
956 46697 : case AC_PrecLand_StateMachine::Status::DESCEND:
957 : // run land controller. This will descend towards the target if prec land target is in sight
958 : // else it will just descend vertically
959 46697 : land_run_horiz_and_vert_control();
960 46697 : break;
961 : }
962 : } else {
963 : // just land, since user has taken over controls, it does not make sense to run any retries or failsafe measures
964 0 : land_run_horiz_and_vert_control();
965 : }
966 63857 : }
967 : #endif
968 :
969 6599531 : float Mode::throttle_hover() const
970 : {
971 6599531 : return motors->get_throttle_hover();
972 : }
973 :
974 : // transform pilot's manual throttle input to make hover throttle mid stick
975 : // used only for manual throttle modes
976 : // thr_mid should be in the range 0 to 1
977 : // returns throttle output 0 to 1
978 6599531 : float Mode::get_pilot_desired_throttle() const
979 : {
980 6599531 : int16_t mid_stick = copter.get_throttle_mid();
981 : // protect against unlikely divide by zero
982 6599531 : if (mid_stick <= 0) {
983 0 : mid_stick = 500;
984 : }
985 :
986 6599531 : int16_t throttle_control = channel_throttle->get_control_in();
987 : // ensure reasonable throttle values
988 6599531 : throttle_control = constrain_int16(throttle_control,0,1000);
989 :
990 : // calculate normalised throttle input
991 : float throttle_in;
992 6599531 : if (throttle_control < mid_stick) {
993 6003595 : throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
994 : } else {
995 595936 : throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
996 : }
997 :
998 6599531 : const float thr_mid = throttle_hover();
999 6599531 : const float expo = constrain_float(-(thr_mid-0.5f)/0.375f, -0.5f, 1.0f);
1000 : // calculate the output throttle using the given expo function
1001 6599531 : float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
1002 6599531 : return throttle_out;
1003 : }
1004 :
1005 6299913 : float Mode::get_avoidance_adjusted_climbrate_ms(float target_rate_ms)
1006 : {
1007 : #if AP_AVOIDANCE_ENABLED
1008 6299913 : float target_rate_cms = target_rate_ms * 100.0;
1009 6299913 : AP::ac_avoid()->adjust_velocity_z(pos_control->D_get_pos_p().kP(), pos_control->D_get_max_accel_mss() * 100.0, target_rate_cms, G_Dt);
1010 6299913 : return target_rate_cms * 0.01;
1011 : #else
1012 : return target_rate_ms;
1013 : #endif
1014 : }
1015 :
1016 : // send output to the motors, can be overridden by subclasses
1017 30577541 : void Mode::output_to_motors()
1018 : {
1019 30577541 : motors->output();
1020 30577541 : }
1021 :
1022 6227204 : Mode::AltHoldModeState Mode::get_alt_hold_state_D_ms(float target_climb_rate_ms)
1023 : {
1024 : // Alt Hold State Machine Determination
1025 6227204 : if (!motors->armed()) {
1026 : // the aircraft should moved to a shut down state
1027 1414239 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
1028 :
1029 : // transition through states as aircraft spools down
1030 1414239 : switch (motors->get_spool_state()) {
1031 :
1032 1414239 : case AP_Motors::SpoolState::SHUT_DOWN:
1033 1414239 : return AltHoldModeState::MotorStopped;
1034 :
1035 0 : case AP_Motors::SpoolState::GROUND_IDLE:
1036 0 : return AltHoldModeState::Landed_Ground_Idle;
1037 :
1038 0 : default:
1039 0 : return AltHoldModeState::Landed_Pre_Takeoff;
1040 : }
1041 :
1042 4812965 : } else if (takeoff.running() || takeoff.triggered_ms(target_climb_rate_ms)) {
1043 : // the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED
1044 : // the aircraft should progress through the take off procedure
1045 71751 : return AltHoldModeState::Takeoff;
1046 :
1047 4741214 : } else if (!copter.ap.auto_armed || copter.ap.land_complete) {
1048 : // the aircraft is armed and landed
1049 402548 : if (target_climb_rate_ms < 0.0f && !copter.ap.using_interlock) {
1050 : // the aircraft should move to a ground idle state
1051 57892 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
1052 : } else {
1053 : // the aircraft should prepare for imminent take off
1054 344656 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1055 : }
1056 :
1057 402548 : if (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
1058 : // the aircraft is waiting in ground idle
1059 72035 : return AltHoldModeState::Landed_Ground_Idle;
1060 :
1061 : } else {
1062 : // the aircraft can leave the ground at any time
1063 330513 : return AltHoldModeState::Landed_Pre_Takeoff;
1064 : }
1065 :
1066 : } else {
1067 : // the aircraft is in a flying state
1068 4338666 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1069 4338666 : return AltHoldModeState::Flying;
1070 : }
1071 : }
1072 :
1073 : // transform pilot's yaw input into a desired yaw rate
1074 : // returns desired yaw rate in centi-degrees per second
1075 27029361 : float Mode::get_pilot_desired_yaw_rate_rads() const
1076 : {
1077 27029361 : if (!rc().has_valid_input()) {
1078 29652 : return 0.0f;
1079 : }
1080 :
1081 : // Get yaw input
1082 26999709 : const float yaw_in_norm = channel_yaw->norm_input_dz();
1083 :
1084 : // convert pilot input to the desired yaw rate
1085 26999709 : return radians(g2.command_model_pilot_y.get_rate()) * input_expo(yaw_in_norm, g2.command_model_pilot_y.get_expo());
1086 : }
1087 :
1088 : // pass-through functions to reduce code churn on conversion;
1089 : // these are candidates for moving into the Mode base
1090 : // class.
1091 :
1092 : // Returns the pilot’s commanded climb rate in m/s.
1093 6292556 : float Mode::get_pilot_desired_climb_rate_ms() const
1094 : {
1095 6292556 : return copter.get_pilot_desired_climb_rate_ms();
1096 : }
1097 :
1098 : // Returns half the hover throttle.
1099 0 : float Mode::get_non_takeoff_throttle() const
1100 : {
1101 0 : return copter.get_non_takeoff_throttle();
1102 : }
1103 :
1104 : // Rotates roll/pitch pilot input if simple or super simple mode is active.
1105 : // roll and pitch may be in any units/scale; the rotation is scale-independent
1106 19844999 : void Mode::apply_simple_mode(float &roll, float &pitch) const
1107 : {
1108 : // exit immediately if not in simple mode
1109 19844999 : if (copter.simple_mode == Copter::SimpleMode::NONE) {
1110 19824251 : return;
1111 : }
1112 :
1113 : float roll_out, pitch_out;
1114 :
1115 20748 : if (copter.simple_mode == Copter::SimpleMode::SIMPLE) {
1116 : // rotate roll, pitch input by -initial simple heading (i.e. north facing)
1117 20748 : roll_out = roll*copter.simple_cos_yaw - pitch*copter.simple_sin_yaw;
1118 20748 : pitch_out = roll*copter.simple_sin_yaw + pitch*copter.simple_cos_yaw;
1119 : } else {
1120 : // rotate roll, pitch input by -super simple heading (reverse of heading to home)
1121 0 : roll_out = roll*copter.super_simple_cos_yaw - pitch*copter.super_simple_sin_yaw;
1122 0 : pitch_out = roll*copter.super_simple_sin_yaw + pitch*copter.super_simple_cos_yaw;
1123 : }
1124 :
1125 : // rotate roll, pitch input from north facing to vehicle's perspective
1126 20748 : roll = roll_out*copter.ahrs.cos_yaw() + pitch_out*copter.ahrs.sin_yaw();
1127 20748 : pitch = -roll_out*copter.ahrs.sin_yaw() + pitch_out*copter.ahrs.cos_yaw();
1128 : }
1129 :
1130 : // Requests a mode change with the specified reason; returns true if accepted.
1131 10 : bool Mode::set_mode(Mode::Number mode, ModeReason reason)
1132 : {
1133 10 : return copter.set_mode(mode, reason);
1134 : }
1135 :
1136 : // Sets the “land complete” state flag.
1137 168864 : void Mode::set_land_complete(bool b)
1138 : {
1139 168864 : return copter.set_land_complete(b);
1140 : }
1141 :
1142 : // Returns a reference to the GCS interface for Copter.
1143 1846 : GCS_Copter &Mode::gcs() const
1144 : {
1145 1846 : return copter.gcs();
1146 : }
1147 :
1148 : // Returns the pilot’s maximum upward speed in m/s.
1149 9782788 : float Mode::get_pilot_speed_up_ms() const
1150 : {
1151 9782788 : return g2.pilot_speed_up_ms;
1152 : }
1153 :
1154 : // Returns the pilot’s maximum downward speed in m/s.
1155 9782788 : float Mode::get_pilot_speed_dn_ms() const
1156 : {
1157 9782788 : return copter.get_pilot_speed_dn_ms();
1158 : }
1159 :
1160 : // Returns the pilot’s vertical acceleration limit in m/s².
1161 9782788 : float Mode::get_pilot_accel_D_mss() const
1162 : {
1163 9782788 : return g2.pilot_accel_d_mss;
1164 : }
1165 :
1166 : // Return stopping point as a location with above origin alt frame
1167 319 : Location Mode::get_stopping_point() const
1168 : {
1169 319 : Vector3p stopping_point_ned_m;
1170 319 : copter.pos_control->get_stopping_point_NE_m(stopping_point_ned_m.xy());
1171 319 : copter.pos_control->get_stopping_point_D_m(stopping_point_ned_m.z);
1172 319 : return Location::from_ekf_offset_NED_m(stopping_point_ned_m, Location::AltFrame::ABOVE_ORIGIN);
1173 : }
|
