ArduCopter/mode.cpp

   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 Mode::Mode(void) :
  12     g(copter.g),
  13     g2(copter.g2),
  14     wp_nav(copter.wp_nav),
  15     loiter_nav(copter.loiter_nav),
  16     pos_control(copter.pos_control),
  17     inertial_nav(copter.inertial_nav),
  18     ahrs(copter.ahrs),
  19     attitude_control(copter.attitude_control),
  20     motors(copter.motors),
  21     channel_roll(copter.channel_roll),
  22     channel_pitch(copter.channel_pitch),
  23     channel_throttle(copter.channel_throttle),
  24     channel_yaw(copter.channel_yaw),
  25     G_Dt(copter.G_Dt)
  26 { };
  27
  28 #if AC_PAYLOAD_PLACE_ENABLED
  29 PayloadPlace Mode::payload_place;
  30 #endif
  31
  32 // return the static controller object corresponding to supplied mode
  33 Mode *Copter::mode_from_mode_num(const Mode::Number mode)
  34 {
  35
  36     switch (mode) {
  37 #if MODE_ACRO_ENABLED
  38         case Mode::Number::ACRO:
  39             return &mode_acro;
  40 #endif
  41
  42         case Mode::Number::STABILIZE:
  43             return &mode_stabilize;
  44
  45         case Mode::Number::ALT_HOLD:
  46             return &mode_althold;
  47
  48 #if MODE_AUTO_ENABLED
  49         case Mode::Number::AUTO:
  50             return &mode_auto;
  51 #endif
  52
  53 #if MODE_CIRCLE_ENABLED
  54         case Mode::Number::CIRCLE:
  55             return &mode_circle;
  56 #endif
  57
  58 #if MODE_LOITER_ENABLED
  59         case Mode::Number::LOITER:
  60             return &mode_loiter;
  61 #endif
  62
  63 #if MODE_GUIDED_ENABLED
  64         case Mode::Number::GUIDED:
  65             return &mode_guided;
  66 #endif
  67
  68         case Mode::Number::LAND:
  69             return &mode_land;
  70
  71 #if MODE_RTL_ENABLED
  72         case Mode::Number::RTL:
  73             return &mode_rtl;
  74 #endif
  75
  76 #if MODE_DRIFT_ENABLED
  77         case Mode::Number::DRIFT:
  78             return &mode_drift;
  79 #endif
  80
  81 #if MODE_SPORT_ENABLED
  82         case Mode::Number::SPORT:
  83             return &mode_sport;
  84 #endif
  85
  86 #if MODE_FLIP_ENABLED
  87         case Mode::Number::FLIP:
  88             return &mode_flip;
  89 #endif
  90
  91 #if AUTOTUNE_ENABLED
  92         case Mode::Number::AUTOTUNE:
  93             return &mode_autotune;
  94 #endif
  95
  96 #if MODE_POSHOLD_ENABLED
  97         case Mode::Number::POSHOLD:
  98             return &mode_poshold;
  99 #endif
 100
 101 #if MODE_BRAKE_ENABLED
 102         case Mode::Number::BRAKE:
 103             return &mode_brake;
 104 #endif
 105
 106 #if MODE_THROW_ENABLED
 107         case Mode::Number::THROW:
 108             return &mode_throw;
 109 #endif
 110
 111 #if HAL_ADSB_ENABLED
 112         case Mode::Number::AVOID_ADSB:
 113             return &mode_avoid_adsb;
 114 #endif
 115
 116 #if MODE_GUIDED_NOGPS_ENABLED
 117         case Mode::Number::GUIDED_NOGPS:
 118             return &mode_guided_nogps;
 119 #endif
 120
 121 #if MODE_SMARTRTL_ENABLED
 122         case Mode::Number::SMART_RTL:
 123             return &mode_smartrtl;
 124 #endif
 125
 126 #if MODE_FLOWHOLD_ENABLED
 127         case Mode::Number::FLOWHOLD:
 128             return (Mode *)g2.mode_flowhold_ptr;
 129 #endif
 130
 131 #if MODE_FOLLOW_ENABLED
 132         case Mode::Number::FOLLOW:
 133             return &mode_follow;
 134 #endif
 135
 136 #if MODE_ZIGZAG_ENABLED
 137         case Mode::Number::ZIGZAG:
 138             return &mode_zigzag;
 139 #endif
 140
 141 #if MODE_SYSTEMID_ENABLED
 142         case Mode::Number::SYSTEMID:
 143             return (Mode *)g2.mode_systemid_ptr;
 144 #endif
 145
 146 #if MODE_AUTOROTATE_ENABLED
 147         case Mode::Number::AUTOROTATE:
 148             return &mode_autorotate;
 149 #endif
 150
 151 #if MODE_TURTLE_ENABLED
 152         case Mode::Number::TURTLE:
 153             return &mode_turtle;
 154 #endif
 155
 156         default:
 157             break;
 158     }
 159
 160 #if MODE_GUIDED_ENABLED && AP_SCRIPTING_ENABLED
 161     // Check registered custom modes
 162     for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
 163         if ((mode_guided_custom[i] != nullptr) && (mode_guided_custom[i]->mode_number() == mode)) {
 164             return mode_guided_custom[i];
 165         }
 166     }
 167 #endif
 168
 169     return nullptr;
 170 }
 171
 172
 173 // called when an attempt to change into a mode is unsuccessful:
 174 void Copter::mode_change_failed(const Mode *mode, const char *reason)
 175 {
 176     gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to %s failed: %s", mode->name(), reason);
 177     LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode->mode_number()));
 178     // make sad noise
 179     if (copter.ap.initialised) {
 180         AP_Notify::events.user_mode_change_failed = 1;
 181     }
 182 }
 183
 184 // Check if this mode can be entered from the GCS
 185 bool Copter::gcs_mode_enabled(const Mode::Number mode_num)
 186 {
 187     // List of modes that can be blocked, index is bit number in parameter bitmask
 188     static const uint8_t mode_list [] {
 189         (uint8_t)Mode::Number::STABILIZE,
 190         (uint8_t)Mode::Number::ACRO,
 191         (uint8_t)Mode::Number::ALT_HOLD,
 192         (uint8_t)Mode::Number::AUTO,
 193         (uint8_t)Mode::Number::GUIDED,
 194         (uint8_t)Mode::Number::LOITER,
 195         (uint8_t)Mode::Number::CIRCLE,
 196         (uint8_t)Mode::Number::DRIFT,
 197         (uint8_t)Mode::Number::SPORT,
 198         (uint8_t)Mode::Number::FLIP,
 199         (uint8_t)Mode::Number::AUTOTUNE,
 200         (uint8_t)Mode::Number::POSHOLD,
 201         (uint8_t)Mode::Number::BRAKE,
 202         (uint8_t)Mode::Number::THROW,
 203         (uint8_t)Mode::Number::AVOID_ADSB,
 204         (uint8_t)Mode::Number::GUIDED_NOGPS,
 205         (uint8_t)Mode::Number::SMART_RTL,
 206         (uint8_t)Mode::Number::FLOWHOLD,
 207         (uint8_t)Mode::Number::FOLLOW,
 208         (uint8_t)Mode::Number::ZIGZAG,
 209         (uint8_t)Mode::Number::SYSTEMID,
 210         (uint8_t)Mode::Number::AUTOROTATE,
 211         (uint8_t)Mode::Number::AUTO_RTL,
 212         (uint8_t)Mode::Number::TURTLE
 213     };
 214
 215     if (!block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list))) {
 216         return true;
 217     }
 218
 219     // Mode disabled, try and grab a mode name to give a better warning.
 220     Mode *new_flightmode = mode_from_mode_num(mode_num);
 221     if (new_flightmode != nullptr) {
 222         mode_change_failed(new_flightmode, "GCS entry disabled (FLTMODE_GCSBLOCK)");
 223     } else {
 224         notify_no_such_mode((uint8_t)mode_num);
 225     }
 226
 227     return false;
 228 }
 229
 230 // set_mode - change flight mode and perform any necessary initialisation
 231 // optional force parameter used to force the flight mode change (used only first time mode is set)
 232 // returns true if mode was successfully set
 233 // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
 234 bool Copter::set_mode(Mode::Number mode, ModeReason reason)
 235 {
 236     // update last reason
 237     const ModeReason last_reason = _last_reason;
 238     _last_reason = reason;
 239
 240     // return immediately if we are already in the desired mode
 241     if (mode == flightmode->mode_number()) {
 242         control_mode_reason = reason;
 243         // set yaw rate time constant during autopilot startup
 244         if (reason == ModeReason::INITIALISED && mode == Mode::Number::STABILIZE) {
 245             attitude_control->set_yaw_rate_tc(g2.command_model_pilot.get_rate_tc());
 246         }
 247         // make happy noise
 248         if (copter.ap.initialised && (reason != last_reason)) {
 249             AP_Notify::events.user_mode_change = 1;
 250         }
 251         return true;
 252     }
 253
 254     // Check if GCS mode change is disabled via parameter
 255     if ((reason == ModeReason::GCS_COMMAND) && !gcs_mode_enabled(mode)) {
 256         return false;
 257     }
 258
 259 #if MODE_AUTO_ENABLED
 260     if (mode == Mode::Number::AUTO_RTL) {
 261         // Special case for AUTO RTL, not a true mode, just AUTO in disguise
 262         // Attempt to join return path, fallback to do-land-start
 263         return mode_auto.return_path_or_jump_to_landing_sequence_auto_RTL(reason);
 264     }
 265 #endif
 266
 267     Mode *new_flightmode = mode_from_mode_num(mode);
 268     if (new_flightmode == nullptr) {
 269         notify_no_such_mode((uint8_t)mode);
 270         return false;
 271     }
 272
 273     bool ignore_checks = !motors->armed();   // allow switching to any mode if disarmed.  We rely on the arming check to perform
 274
 275 #if FRAME_CONFIG == HELI_FRAME
 276     // do not allow helis to enter a non-manual throttle mode if the
 277     // rotor runup is not complete
 278     if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()) {
 279         mode_change_failed(new_flightmode, "runup not complete");
 280         return false;
 281     }
 282 #endif
 283
 284 #if FRAME_CONFIG != HELI_FRAME
 285     // ensure vehicle doesn't leap off the ground if a user switches
 286     // into a manual throttle mode from a non-manual-throttle mode
 287     // (e.g. user arms in guided, raises throttle to 1300 (not enough to
 288     // trigger auto takeoff), then switches into manual):
 289     bool user_throttle = new_flightmode->has_manual_throttle();
 290 #if MODE_DRIFT_ENABLED
 291     if (new_flightmode == &mode_drift) {
 292         user_throttle = true;
 293     }
 294 #endif
 295     if (!ignore_checks &&
 296         ap.land_complete &&
 297         user_throttle &&
 298         !copter.flightmode->has_manual_throttle() &&
 299         new_flightmode->get_pilot_desired_throttle() > copter.get_non_takeoff_throttle()) {
 300         mode_change_failed(new_flightmode, "throttle too high");
 301         return false;
 302     }
 303 #endif
 304
 305     if (!ignore_checks &&
 306         new_flightmode->requires_GPS() &&
 307         !copter.position_ok()) {
 308         mode_change_failed(new_flightmode, "requires position");
 309         return false;
 310     }
 311
 312     // check for valid altitude if old mode did not require it but new one does
 313     // we only want to stop changing modes if it could make things worse
 314     if (!ignore_checks &&
 315         !copter.ekf_alt_ok() &&
 316         flightmode->has_manual_throttle() &&
 317         !new_flightmode->has_manual_throttle()) {
 318         mode_change_failed(new_flightmode, "need alt estimate");
 319         return false;
 320     }
 321
 322 #if AP_FENCE_ENABLED
 323     // may not be allowed to change mode if recovering from fence breach
 324     if (!ignore_checks &&
 325         fence.enabled() &&
 326         fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) &&
 327         fence.get_breaches() &&
 328         motors->armed() &&
 329         get_control_mode_reason() == ModeReason::FENCE_BREACHED &&
 330         !ap.land_complete) {
 331         mode_change_failed(new_flightmode, "in fence recovery");
 332         return false;
 333     }
 334 #endif
 335
 336     if (!new_flightmode->init(ignore_checks)) {
 337         mode_change_failed(new_flightmode, "init failed");
 338         return false;
 339     }
 340
 341     // perform any cleanup required by previous flight mode
 342     exit_mode(flightmode, new_flightmode);
 343
 344     // update flight mode
 345     flightmode = new_flightmode;
 346     control_mode_reason = reason;
 347 #if HAL_LOGGING_ENABLED
 348     logger.Write_Mode((uint8_t)flightmode->mode_number(), reason);
 349 #endif
 350     gcs().send_message(MSG_HEARTBEAT);
 351
 352 #if HAL_ADSB_ENABLED
 353     adsb.set_is_auto_mode((mode == Mode::Number::AUTO) || (mode == Mode::Number::RTL) || (mode == Mode::Number::GUIDED));
 354 #endif
 355
 356 #if AP_FENCE_ENABLED
 357     if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) {
 358         // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
 359         // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
 360         // but it should be harmless to disable the fence temporarily in these situations as well
 361         fence.manual_recovery_start();
 362     }
 363 #endif
 364
 365 #if AP_CAMERA_ENABLED
 366     camera.set_is_auto_mode(flightmode->mode_number() == Mode::Number::AUTO);
 367 #endif
 368
 369     // set rate shaping time constants
 370 #if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
 371     attitude_control->set_roll_pitch_rate_tc(g2.command_model_acro_rp.get_rate_tc());
 372 #endif
 373     attitude_control->set_yaw_rate_tc(g2.command_model_pilot.get_rate_tc());
 374 #if MODE_ACRO_ENABLED || MODE_DRIFT_ENABLED
 375     if (mode== Mode::Number::ACRO || mode== Mode::Number::DRIFT) {
 376         attitude_control->set_yaw_rate_tc(g2.command_model_acro_y.get_rate_tc());
 377     }
 378 #endif
 379
 380     // update notify object
 381     notify_flight_mode();
 382
 383     // make happy noise
 384     if (copter.ap.initialised) {
 385         AP_Notify::events.user_mode_change = 1;
 386     }
 387
 388     // return success
 389     return true;
 390 }
 391
 392 bool Copter::set_mode(const uint8_t new_mode, const ModeReason reason)
 393 {
 394     static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
 395 #ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
 396     if (reason == ModeReason::GCS_COMMAND && copter.failsafe.radio) {
 397         // don't allow mode changes while in radio failsafe
 398         return false;
 399     }
 400 #endif
 401     return copter.set_mode(static_cast<Mode::Number>(new_mode), reason);
 402 }
 403
 404 // update_flight_mode - calls the appropriate attitude controllers based on flight mode
 405 // called at 100hz or more
 406 void Copter::update_flight_mode()
 407 {
 408 #if AP_RANGEFINDER_ENABLED
 409     surface_tracking.invalidate_for_logging();  // invalidate surface tracking alt, flight mode will set to true if used
 410 #endif
 411     attitude_control->landed_gain_reduction(copter.ap.land_complete); // Adjust gains when landed to attenuate ground oscillation
 412
 413     flightmode->run();
 414 }
 415
 416 // exit_mode - high level call to organise cleanup as a flight mode is exited
 417 void Copter::exit_mode(Mode *&old_flightmode,
 418                        Mode *&new_flightmode)
 419 {
 420     // smooth throttle transition when switching from manual to automatic flight modes
 421     if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && motors->armed() && !ap.land_complete) {
 422         // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
 423         set_accel_throttle_I_from_pilot_throttle();
 424     }
 425
 426     // cancel any takeoffs in progress
 427     old_flightmode->takeoff_stop();
 428
 429     // perform cleanup required for each flight mode
 430     old_flightmode->exit();
 431
 432 #if FRAME_CONFIG == HELI_FRAME
 433     // firmly reset the flybar passthrough to false when exiting acro mode.
 434     if (old_flightmode == &mode_acro) {
 435         attitude_control->use_flybar_passthrough(false, false);
 436         motors->set_acro_tail(false);
 437     }
 438
 439     // if we are changing from a mode that did not use manual throttle,
 440     // stab col ramp value should be pre-loaded to the correct value to avoid a twitch
 441     // heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes
 442     if (!old_flightmode->has_manual_throttle()){
 443         if (new_flightmode == &mode_stabilize){
 444             input_manager.set_stab_col_ramp(1.0);
 445         } else if (new_flightmode == &mode_acro){
 446             input_manager.set_stab_col_ramp(0.0);
 447         }
 448     }
 449
 450     // Make sure inverted flight is disabled if not supported in the new mode
 451     if (!new_flightmode->allows_inverted()) {
 452         attitude_control->set_inverted_flight(false);
 453     }
 454 #endif //HELI_FRAME
 455 }
 456
 457 // notify_flight_mode - sets notify object based on current flight mode.  Only used for OreoLED notify device
 458 void Copter::notify_flight_mode() {
 459     AP_Notify::flags.autopilot_mode = flightmode->is_autopilot();
 460     AP_Notify::flags.flight_mode = (uint8_t)flightmode->mode_number();
 461     notify.set_flight_mode_str(flightmode->name4());
 462 }
 463
 464 // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
 465 // returns desired angle in centi-degrees
 466 void Mode::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const
 467 {
 468     // throttle failsafe check
 469     if (copter.failsafe.radio || !rc().has_ever_seen_rc_input()) {
 470         roll_out_cd = 0.0;
 471         pitch_out_cd = 0.0;
 472         return;
 473     }
 474
 475     //transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command
 476     float roll_out_deg;
 477     float pitch_out_deg;
 478     rc_input_to_roll_pitch(channel_roll->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), channel_pitch->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), angle_max_cd * 0.01,  angle_limit_cd * 0.01, roll_out_deg, pitch_out_deg);
 479
 480     // Convert to centi-degrees
 481     roll_out_cd = roll_out_deg * 100.0;
 482     pitch_out_cd = pitch_out_deg * 100.0;
 483 }
 484
 485 // transform pilot's roll or pitch input into a desired velocity
 486 Vector2f Mode::get_pilot_desired_velocity(float vel_max) const
 487 {
 488     Vector2f vel;
 489
 490     // throttle failsafe check
 491     if (copter.failsafe.radio || !rc().has_ever_seen_rc_input()) {
 492         return vel;
 493     }
 494     // fetch roll and pitch inputs
 495     float roll_out = channel_roll->get_control_in();
 496     float pitch_out = channel_pitch->get_control_in();
 497
 498     // convert roll and pitch inputs to -1 to +1 range
 499     float scaler = 1.0 / (float)ROLL_PITCH_YAW_INPUT_MAX;
 500     roll_out *= scaler;
 501     pitch_out *= scaler;
 502
 503     // convert roll and pitch inputs into velocity in NE frame
 504     vel = Vector2f(-pitch_out, roll_out);
 505     if (vel.is_zero()) {
 506         return vel;
 507     }
 508     copter.rotate_body_frame_to_NE(vel.x, vel.y);
 509
 510     // Transform square input range to circular output
 511     // vel_scaler is the vector to the edge of the +- 1.0 square in the direction of the current input
 512     Vector2f vel_scaler = vel / MAX(fabsf(vel.x), fabsf(vel.y));
 513     // We scale the output by the ratio of the distance to the square to the unit circle and multiply by vel_max
 514     vel *= vel_max / vel_scaler.length();
 515     return vel;
 516 }
 517
 518 bool Mode::_TakeOff::triggered(const float target_climb_rate) const
 519 {
 520     if (!copter.ap.land_complete) {
 521         // can't take off if we're already flying
 522         return false;
 523     }
 524     if (target_climb_rate <= 0.0f) {
 525         // can't takeoff unless we want to go up...
 526         return false;
 527     }
 528
 529     if (copter.motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
 530         // hold aircraft on the ground until rotor speed runup has finished
 531         return false;
 532     }
 533
 534     return true;
 535 }
 536
 537 bool Mode::is_disarmed_or_landed() const
 538 {
 539     if (!motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) {
 540         return true;
 541     }
 542     return false;
 543 }
 544
 545 void Mode::zero_throttle_and_relax_ac(bool spool_up)
 546 {
 547     if (spool_up) {
 548         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 549     } else {
 550         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
 551     }
 552     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
 553     attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
 554 }
 555
 556 void Mode::zero_throttle_and_hold_attitude()
 557 {
 558     // run attitude controller
 559     attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
 560     attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
 561 }
 562
 563 // handle situations where the vehicle is on the ground waiting for takeoff
 564 // force_throttle_unlimited should be true in cases where we want to keep the motors spooled up
 565 // (instead of spooling down to ground idle).  This is required for tradheli's in Guided and Auto
 566 // where we always want the motor spooled up in Guided or Auto mode.  Tradheli's main rotor stops
 567 // when spooled down to ground idle.
 568 // ultimately it forces the motor interlock to be obeyed in auto and guided modes when on the ground.
 569 void Mode::make_safe_ground_handling(bool force_throttle_unlimited)
 570 {
 571     if (force_throttle_unlimited) {
 572         // keep rotors turning
 573         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 574     } else {
 575         // spool down to ground idle
 576         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
 577     }
 578
 579     // aircraft is landed, integrator terms must be reset regardless of spool state
 580     attitude_control->reset_rate_controller_I_terms_smoothly();
 581
 582     switch (motors->get_spool_state()) {
 583     case AP_Motors::SpoolState::SHUT_DOWN:
 584     case AP_Motors::SpoolState::GROUND_IDLE:
 585         // reset yaw targets and rates during idle states
 586         attitude_control->reset_yaw_target_and_rate();
 587         break;
 588     case AP_Motors::SpoolState::SPOOLING_UP:
 589     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
 590     case AP_Motors::SpoolState::SPOOLING_DOWN:
 591         // while transitioning though active states continue to operate normally
 592         break;
 593     }
 594
 595     pos_control->relax_velocity_controller_xy();
 596     pos_control->update_xy_controller();
 597     pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
 598     pos_control->update_z_controller();
 599     // we may need to move this out
 600     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
 601 }
 602
 603 /*
 604   get a height above ground estimate for landing
 605  */
 606 int32_t Mode::get_alt_above_ground_cm(void)
 607 {
 608     int32_t alt_above_ground_cm;
 609     if (copter.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) {
 610         return alt_above_ground_cm;
 611     }
 612     if (!pos_control->is_active_xy()) {
 613         return copter.current_loc.alt;
 614     }
 615     if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) {
 616         return alt_above_ground_cm;
 617     }
 618
 619     // Assume the Earth is flat:
 620     return copter.current_loc.alt;
 621 }
 622
 623 void Mode::land_run_vertical_control(bool pause_descent)
 624 {
 625     float cmb_rate = 0;
 626     bool ignore_descent_limit = false;
 627     if (!pause_descent) {
 628
 629         // do not ignore limits until we have slowed down for landing
 630         ignore_descent_limit = (MAX(g2.land_alt_low,100) > get_alt_above_ground_cm()) || copter.ap.land_complete_maybe;
 631
 632         float max_land_descent_velocity;
 633         if (g.land_speed_high > 0) {
 634             max_land_descent_velocity = -g.land_speed_high;
 635         } else {
 636             max_land_descent_velocity = pos_control->get_max_speed_down_cms();
 637         }
 638
 639         // Don't speed up for landing.
 640         max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
 641
 642         // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low.
 643         cmb_rate = sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt);
 644
 645         // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
 646         cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
 647
 648 #if AC_PRECLAND_ENABLED
 649         const bool navigating = pos_control->is_active_xy();
 650         bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating;
 651
 652         if (doing_precision_landing) {
 653             // prec landing is active
 654             Vector2f target_pos;
 655             float target_error_cm = 0.0f;
 656             if (copter.precland.get_target_position_cm(target_pos)) {
 657                 const Vector2f current_pos = inertial_nav.get_position_xy_cm();
 658                 // target is this many cm away from the vehicle
 659                 target_error_cm = (target_pos - current_pos).length();
 660             }
 661             // check if we should descend or not
 662             const float max_horiz_pos_error_cm = copter.precland.get_max_xy_error_before_descending_cm();
 663             Vector3f target_pos_meas;
 664             copter.precland.get_target_position_measurement_cm(target_pos_meas);
 665             if (target_error_cm > max_horiz_pos_error_cm && !is_zero(max_horiz_pos_error_cm)) {
 666                 // doing precland but too far away from the obstacle
 667                 // do not descend
 668                 cmb_rate = 0.0f;
 669             } else if (target_pos_meas.z > 35.0f && target_pos_meas.z < 200.0f && !copter.precland.do_fast_descend()) {
 670                 // very close to the ground and doing prec land, lets slow down to make sure we land on target
 671                 // compute desired descent velocity
 672                 const float precland_acceptable_error_cm = 15.0f;
 673                 const float precland_min_descent_speed_cms = 10.0f;
 674                 const float max_descent_speed_cms = abs(g.land_speed)*0.5f;
 675                 const float land_slowdown = MAX(0.0f, target_error_cm*(max_descent_speed_cms/precland_acceptable_error_cm));
 676                 cmb_rate = MIN(-precland_min_descent_speed_cms, -max_descent_speed_cms+land_slowdown);
 677             }
 678         }
 679 #endif
 680     }
 681
 682     // update altitude target and call position controller
 683     pos_control->land_at_climb_rate_cm(cmb_rate, ignore_descent_limit);
 684     pos_control->update_z_controller();
 685 }
 686
 687 void Mode::land_run_horizontal_control()
 688 {
 689     Vector2f vel_correction;
 690
 691     // relax loiter target if we might be landed
 692     if (copter.ap.land_complete_maybe) {
 693         pos_control->soften_for_landing_xy();
 694     }
 695
 696     // process pilot inputs
 697     if (!copter.failsafe.radio) {
 698         if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
 699             LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT);
 700             // exit land if throttle is high
 701             if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
 702                 set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
 703             }
 704         }
 705
 706         if (g.land_repositioning) {
 707             // apply SIMPLE mode transform to pilot inputs
 708             update_simple_mode();
 709
 710             // convert pilot input to reposition velocity
 711             // use half maximum acceleration as the maximum velocity to ensure aircraft will
 712             // stop from full reposition speed in less than 1 second.
 713             const float max_pilot_vel = wp_nav->get_wp_acceleration() * 0.5;
 714             vel_correction = get_pilot_desired_velocity(max_pilot_vel);
 715
 716             // record if pilot has overridden roll or pitch
 717             if (!vel_correction.is_zero()) {
 718                 if (!copter.ap.land_repo_active) {
 719                     LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE);
 720                 }
 721                 copter.ap.land_repo_active = true;
 722 #if AC_PRECLAND_ENABLED
 723             } else {
 724                 // no override right now, check if we should allow precland
 725                 if (copter.precland.allow_precland_after_reposition()) {
 726                     copter.ap.land_repo_active = false;
 727                 }
 728 #endif
 729             }
 730         }
 731     }
 732
 733     // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle
 734     copter.ap.prec_land_active = false;
 735 #if AC_PRECLAND_ENABLED
 736     copter.ap.prec_land_active = !copter.ap.land_repo_active && copter.precland.target_acquired();
 737     // run precision landing
 738     if (copter.ap.prec_land_active) {
 739         Vector2f target_pos, target_vel;
 740         if (!copter.precland.get_target_position_cm(target_pos)) {
 741             target_pos = inertial_nav.get_position_xy_cm();
 742         }
 743          // get the velocity of the target
 744         copter.precland.get_target_velocity_cms(inertial_nav.get_velocity_xy_cms(), target_vel);
 745
 746         Vector2f zero;
 747         Vector2p landing_pos = target_pos.topostype();
 748         // target vel will remain zero if landing target is stationary
 749         pos_control->input_pos_vel_accel_xy(landing_pos, target_vel, zero);
 750     }
 751 #endif
 752
 753     if (!copter.ap.prec_land_active) {
 754         Vector2f accel;
 755         pos_control->input_vel_accel_xy(vel_correction, accel);
 756     }
 757
 758     // run pos controller
 759     pos_control->update_xy_controller();
 760     Vector3f thrust_vector = pos_control->get_thrust_vector();
 761
 762     if (g2.wp_navalt_min > 0) {
 763         // user has requested an altitude below which navigation
 764         // attitude is limited. This is used to prevent commanded roll
 765         // over on landing, which particularly affects helicopters if
 766         // there is any position estimate drift after touchdown. We
 767         // limit attitude to 7 degrees below this limit and linearly
 768         // interpolate for 1m above that
 769         const float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(),
 770                                                      g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U);
 771         const float thrust_vector_max = sinf(radians(attitude_limit_cd * 0.01f)) * GRAVITY_MSS * 100.0f;
 772         const float thrust_vector_mag = thrust_vector.xy().length();
 773         if (thrust_vector_mag > thrust_vector_max) {
 774             float ratio = thrust_vector_max / thrust_vector_mag;
 775             thrust_vector.x *= ratio;
 776             thrust_vector.y *= ratio;
 777
 778             // tell position controller we are applying an external limit
 779             pos_control->set_externally_limited_xy();
 780         }
 781     }
 782
 783     // call attitude controller
 784     attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.get_heading());
 785
 786 }
 787
 788 // run normal or precision landing (if enabled)
 789 // pause_descent is true if vehicle should not descend
 790 void Mode::land_run_normal_or_precland(bool pause_descent)
 791 {
 792 #if AC_PRECLAND_ENABLED
 793     if (pause_descent || !copter.precland.enabled()) {
 794         // we don't want to start descending immediately or prec land is disabled
 795         // in both cases just run simple land controllers
 796         land_run_horiz_and_vert_control(pause_descent);
 797     } else {
 798         // prec land is enabled and we have not paused descent
 799         // the state machine takes care of the entire prec landing procedure
 800         precland_run();
 801     }
 802 #else
 803     land_run_horiz_and_vert_control(pause_descent);
 804 #endif
 805 }
 806
 807 #if AC_PRECLAND_ENABLED
 808 // Go towards a position commanded by prec land state machine in order to retry landing
 809 // The passed in location is expected to be NED and in m
 810 void Mode::precland_retry_position(const Vector3f &retry_pos)
 811 {
 812     if (!copter.failsafe.radio) {
 813         if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
 814             LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT);
 815             // exit land if throttle is high
 816             if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
 817                 set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
 818             }
 819         }
 820
 821         // allow user to take control during repositioning. Note: copied from land_run_horizontal_control()
 822         // To-Do: this code exists at several different places in slightly different forms and that should be fixed
 823         if (g.land_repositioning) {
 824             float target_roll = 0.0f;
 825             float target_pitch = 0.0f;
 826             // convert pilot input to lean angles
 827             get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
 828
 829             // record if pilot has overridden roll or pitch
 830             if (!is_zero(target_roll) || !is_zero(target_pitch)) {
 831                 if (!copter.ap.land_repo_active) {
 832                     LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE);
 833                 }
 834                 // this flag will be checked by prec land state machine later and any further landing retires will be cancelled
 835                 copter.ap.land_repo_active = true;
 836             }
 837         }
 838     }
 839
 840     Vector3p retry_pos_NEU{retry_pos.x, retry_pos.y, retry_pos.z * -1.0f};
 841     // pos controller expects input in NEU cm's
 842     retry_pos_NEU = retry_pos_NEU * 100.0f;
 843     pos_control->input_pos_xyz(retry_pos_NEU, 0.0f, 1000.0f);
 844
 845     // run position controllers
 846     pos_control->update_xy_controller();
 847     pos_control->update_z_controller();
 848
 849     // call attitude controller
 850     attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 851
 852 }
 853
 854 // Run precland statemachine. This function should be called from any mode that wants to do precision landing.
 855 // This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
 856 void Mode::precland_run()
 857 {
 858     // if user is taking control, we will not run the statemachine, and simply land (may or may not be on target)
 859     if (!copter.ap.land_repo_active) {
 860         // This will get updated later to a retry pos if needed
 861         Vector3f retry_pos;
 862
 863         switch (copter.precland_statemachine.update(retry_pos)) {
 864         case AC_PrecLand_StateMachine::Status::RETRYING:
 865             // we want to retry landing by going to another position
 866             precland_retry_position(retry_pos);
 867             break;
 868
 869         case AC_PrecLand_StateMachine::Status::FAILSAFE: {
 870             // we have hit a failsafe. Failsafe can only mean two things, we either want to stop permanently till user takes over or land
 871             switch (copter.precland_statemachine.get_failsafe_actions()) {
 872             case AC_PrecLand_StateMachine::FailSafeAction::DESCEND:
 873                 // descend normally, prec land target is definitely not in sight
 874                 land_run_horiz_and_vert_control();
 875                 break;
 876             case AC_PrecLand_StateMachine::FailSafeAction::HOLD_POS:
 877                 // sending "true" in this argument will stop the descend
 878                 land_run_horiz_and_vert_control(true);
 879                 break;
 880             }
 881             break;
 882         }
 883         case AC_PrecLand_StateMachine::Status::ERROR:
 884             // should never happen, is certainly a bug. Report then descend
 885             INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
 886             FALLTHROUGH;
 887         case AC_PrecLand_StateMachine::Status::DESCEND:
 888             // run land controller. This will descend towards the target if prec land target is in sight
 889             // else it will just descend vertically
 890             land_run_horiz_and_vert_control();
 891             break;
 892         }
 893     } else {
 894         // just land, since user has taken over controls, it does not make sense to run any retries or failsafe measures
 895         land_run_horiz_and_vert_control();
 896     }
 897 }
 898 #endif
 899
 900 float Mode::throttle_hover() const
 901 {
 902     return motors->get_throttle_hover();
 903 }
 904
 905 // transform pilot's manual throttle input to make hover throttle mid stick
 906 // used only for manual throttle modes
 907 // thr_mid should be in the range 0 to 1
 908 // returns throttle output 0 to 1
 909 float Mode::get_pilot_desired_throttle() const
 910 {
 911     const float thr_mid = throttle_hover();
 912     int16_t throttle_control = channel_throttle->get_control_in();
 913
 914     int16_t mid_stick = copter.get_throttle_mid();
 915     // protect against unlikely divide by zero
 916     if (mid_stick <= 0) {
 917         mid_stick = 500;
 918     }
 919
 920     // ensure reasonable throttle values
 921     throttle_control = constrain_int16(throttle_control,0,1000);
 922
 923     // calculate normalised throttle input
 924     float throttle_in;
 925     if (throttle_control < mid_stick) {
 926         throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
 927     } else {
 928         throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
 929     }
 930
 931     const float expo = constrain_float(-(thr_mid-0.5f)/0.375f, -0.5f, 1.0f);
 932     // calculate the output throttle using the given expo function
 933     float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
 934     return throttle_out;
 935 }
 936
 937 float Mode::get_avoidance_adjusted_climbrate(float target_rate)
 938 {
 939 #if AP_AVOIDANCE_ENABLED
 940     AP::ac_avoid()->adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), target_rate, G_Dt);
 941     return target_rate;
 942 #else
 943     return target_rate;
 944 #endif
 945 }
 946
 947 // send output to the motors, can be overridden by subclasses
 948 void Mode::output_to_motors()
 949 {
 950     motors->output();
 951 }
 952
 953 Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms)
 954 {
 955     // Alt Hold State Machine Determination
 956     if (!motors->armed()) {
 957         // the aircraft should moved to a shut down state
 958         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
 959
 960         // transition through states as aircraft spools down
 961         switch (motors->get_spool_state()) {
 962
 963         case AP_Motors::SpoolState::SHUT_DOWN:
 964             return AltHoldModeState::MotorStopped;
 965
 966         case AP_Motors::SpoolState::GROUND_IDLE:
 967             return AltHoldModeState::Landed_Ground_Idle;
 968
 969         default:
 970             return AltHoldModeState::Landed_Pre_Takeoff;
 971         }
 972
 973     } else if (takeoff.running() || takeoff.triggered(target_climb_rate_cms)) {
 974         // the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED
 975         // the aircraft should progress through the take off procedure
 976         return AltHoldModeState::Takeoff;
 977
 978     } else if (!copter.ap.auto_armed || copter.ap.land_complete) {
 979         // the aircraft is armed and landed
 980         if (target_climb_rate_cms < 0.0f && !copter.ap.using_interlock) {
 981             // the aircraft should move to a ground idle state
 982             motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
 983
 984         } else {
 985             // the aircraft should prepare for imminent take off
 986             motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 987         }
 988
 989         if (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
 990             // the aircraft is waiting in ground idle
 991             return AltHoldModeState::Landed_Ground_Idle;
 992
 993         } else {
 994             // the aircraft can leave the ground at any time
 995             return AltHoldModeState::Landed_Pre_Takeoff;
 996         }
 997
 998     } else {
 999         // the aircraft is in a flying state
1000         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1001         return AltHoldModeState::Flying;
1002     }
1003 }
1004
1005 // transform pilot's yaw input into a desired yaw rate
1006 // returns desired yaw rate in centi-degrees per second
1007 float Mode::get_pilot_desired_yaw_rate() const
1008 {
1009     // throttle failsafe check
1010     if (copter.failsafe.radio || !rc().has_ever_seen_rc_input()) {
1011         return 0.0f;
1012     }
1013
1014     // Get yaw input
1015     const float yaw_in = channel_yaw->norm_input_dz();
1016
1017     // convert pilot input to the desired yaw rate
1018     return g2.command_model_pilot.get_rate() * 100.0 * input_expo(yaw_in, g2.command_model_pilot.get_expo());
1019 }
1020
1021 // pass-through functions to reduce code churn on conversion;
1022 // these are candidates for moving into the Mode base
1023 // class.
1024 float Mode::get_pilot_desired_climb_rate(float throttle_control)
1025 {
1026     return copter.get_pilot_desired_climb_rate(throttle_control);
1027 }
1028
1029 float Mode::get_non_takeoff_throttle()
1030 {
1031     return copter.get_non_takeoff_throttle();
1032 }
1033
1034 void Mode::update_simple_mode(void) {
1035     copter.update_simple_mode();
1036 }
1037
1038 bool Mode::set_mode(Mode::Number mode, ModeReason reason)
1039 {
1040     return copter.set_mode(mode, reason);
1041 }
1042
1043 void Mode::set_land_complete(bool b)
1044 {
1045     return copter.set_land_complete(b);
1046 }
1047
1048 GCS_Copter &Mode::gcs()
1049 {
1050     return copter.gcs();
1051 }
1052
1053 uint16_t Mode::get_pilot_speed_dn()
1054 {
1055     return copter.get_pilot_speed_dn();
1056 }
1057
1058 // Return stopping point as a location with above origin alt frame
1059 Location Mode::get_stopping_point() const
1060 {
1061     Vector3p stopping_point_NEU;
1062     copter.pos_control->get_stopping_point_xy_cm(stopping_point_NEU.xy());
1063     copter.pos_control->get_stopping_point_z_cm(stopping_point_NEU.z);
1064     return Location { stopping_point_NEU.tofloat(), Location::AltFrame::ABOVE_ORIGIN };
1065 }