ArduPlane/mode.cpp

   1 #include "Plane.h"
   2
   3 Mode::Mode() :
   4     ahrs(plane.ahrs)
   5 #if HAL_QUADPLANE_ENABLED
   6     , quadplane(plane.quadplane),
   7     pos_control(plane.quadplane.pos_control),
   8     attitude_control(plane.quadplane.attitude_control),
   9     loiter_nav(plane.quadplane.loiter_nav),
  10     poscontrol(plane.quadplane.poscontrol)
  11 #endif
  12 {
  13 }
  14
  15 void Mode::exit()
  16 {
  17     // call sub-classes exit
  18     _exit();
  19     // stop autotuning if not AUTOTUNE mode
  20     if (plane.control_mode != &plane.mode_autotune){
  21         plane.autotune_restore();
  22     }
  23
  24 }
  25
  26 bool Mode::enter()
  27 {
  28 #if AP_SCRIPTING_ENABLED
  29     // reset nav_scripting.enabled
  30     plane.nav_scripting.enabled = false;
  31 #endif
  32
  33     // cancel inverted flight
  34     plane.auto_state.inverted_flight = false;
  35
  36     // cancel waiting for rudder neutral
  37     plane.takeoff_state.waiting_for_rudder_neutral = false;
  38
  39     // don't cross-track when starting a mission
  40     plane.auto_state.next_wp_crosstrack = false;
  41
  42     // reset landing check
  43     plane.auto_state.checked_for_autoland = false;
  44
  45     // zero locked course
  46     plane.steer_state.locked_course_err = 0;
  47     plane.steer_state.locked_course = false;
  48
  49     // reset crash detection
  50     plane.crash_state.is_crashed = false;
  51     plane.crash_state.impact_detected = false;
  52
  53     // reset external attitude guidance
  54     plane.guided_state.last_forced_rpy_ms.zero();
  55     plane.guided_state.last_forced_throttle_ms = 0;
  56
  57 #if OFFBOARD_GUIDED == ENABLED
  58     plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range
  59     plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
  60     plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
  61     plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane.
  62     plane.guided_state.target_alt_time_ms = 0;
  63     plane.guided_state.last_target_alt = 0;
  64 #endif
  65
  66 #if AP_CAMERA_ENABLED
  67     plane.camera.set_is_auto_mode(this == &plane.mode_auto);
  68 #endif
  69
  70     // zero initial pitch and highest airspeed on mode change
  71     plane.auto_state.highest_airspeed = 0;
  72     plane.auto_state.initial_pitch_cd = ahrs.pitch_sensor;
  73
  74     // disable taildrag takeoff on mode change
  75     plane.auto_state.fbwa_tdrag_takeoff_mode = false;
  76
  77     // start with previous WP at current location
  78     plane.prev_WP_loc = plane.current_loc;
  79
  80     // new mode means new loiter
  81     plane.loiter.start_time_ms = 0;
  82
  83     // record time of mode change
  84     plane.last_mode_change_ms = AP_HAL::millis();
  85
  86     // set VTOL auto state
  87     plane.auto_state.vtol_mode = is_vtol_mode();
  88     plane.auto_state.vtol_loiter = false;
  89
  90     // initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands
  91     plane.new_airspeed_cm = -1;
  92
  93     // clear postponed long failsafe if mode change (from GCS) occurs before recall of long failsafe
  94     plane.long_failsafe_pending = false;
  95
  96 #if HAL_QUADPLANE_ENABLED
  97     quadplane.mode_enter();
  98 #endif
  99
 100 #if AP_TERRAIN_AVAILABLE
 101     plane.target_altitude.terrain_following_pending = false;
 102 #endif
 103
 104     bool enter_result = _enter();
 105
 106     if (enter_result) {
 107         // -------------------
 108         // these must be done AFTER _enter() because they use the results to set more flags
 109
 110         // start with throttle suppressed in auto_throttle modes
 111         plane.throttle_suppressed = does_auto_throttle();
 112 #if HAL_ADSB_ENABLED
 113         plane.adsb.set_is_auto_mode(does_auto_navigation());
 114 #endif
 115
 116         // set the nav controller stale AFTER _enter() so that we can check if we're currently in a loiter during the mode change
 117         plane.nav_controller->set_data_is_stale();
 118
 119         // reset steering integrator on mode change
 120         plane.steerController.reset_I();
 121
 122         // update RC failsafe, as mode change may have necessitated changing the failsafe throttle
 123         plane.control_failsafe();
 124
 125 #if AP_FENCE_ENABLED
 126         // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
 127         // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
 128         // but it should be harmless to disable the fence temporarily in these situations as well
 129         plane.fence.manual_recovery_start();
 130 #endif
 131         //reset mission if in landing sequence, disarmed, not flying, and have changed to a non-autothrottle mode to clear prearm
 132         if (plane.mission.get_in_landing_sequence_flag() &&
 133             !plane.is_flying() && !plane.arming.is_armed_and_safety_off() &&
 134             !plane.control_mode->does_auto_navigation()) {
 135            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "In landing sequence: mission reset");
 136            plane.mission.reset();
 137         }
 138
 139         // Make sure the flight stage is correct for the new mode
 140         plane.update_flight_stage();
 141     }
 142
 143     return enter_result;
 144 }
 145
 146 bool Mode::is_vtol_man_throttle() const
 147 {
 148 #if HAL_QUADPLANE_ENABLED
 149     if (plane.quadplane.tailsitter.is_in_fw_flight() &&
 150         plane.quadplane.assisted_flight) {
 151         // We are a tailsitter that has fully transitioned to Q-assisted forward flight.
 152         // In this case the forward throttle directly drives the vertical throttle so
 153         // set vertical throttle state to match the forward throttle state. Confusingly the booleans are inverted,
 154         // forward throttle uses 'does_auto_throttle' whereas vertical uses 'is_vtol_man_throttle'.
 155         return !does_auto_throttle();
 156     }
 157 #endif
 158     return false;
 159 }
 160
 161 void Mode::update_target_altitude()
 162 {
 163     Location target_location;
 164
 165     if (plane.landing.is_flaring()) {
 166         // during a landing flare, use TECS_LAND_SINK as a target sink
 167         // rate, and ignores the target altitude
 168         plane.set_target_altitude_location(plane.next_WP_loc);
 169     } else if (plane.landing.is_on_approach()) {
 170         plane.landing.setup_landing_glide_slope(plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.target_altitude.offset_cm);
 171         plane.landing.adjust_landing_slope_for_rangefinder_bump(plane.rangefinder_state, plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.auto_state.wp_distance, plane.target_altitude.offset_cm);
 172     } else if (plane.landing.get_target_altitude_location(target_location)) {
 173         plane.set_target_altitude_location(target_location);
 174 #if HAL_SOARING_ENABLED
 175     } else if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.get_throttle_suppressed()) {
 176         // Reset target alt to current alt, to prevent large altitude errors when gliding.
 177         plane.set_target_altitude_location(plane.current_loc);
 178         plane.reset_offset_altitude();
 179 #endif
 180     } else if (plane.reached_loiter_target()) {
 181         // once we reach a loiter target then lock to the final
 182         // altitude target
 183         plane.set_target_altitude_location(plane.next_WP_loc);
 184     } else if (plane.target_altitude.offset_cm != 0 &&
 185                !plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc)) {
 186         // control climb/descent rate
 187         plane.set_target_altitude_proportion(plane.next_WP_loc, 1.0f-plane.auto_state.wp_proportion);
 188
 189         // stay within the range of the start and end locations in altitude
 190         plane.constrain_target_altitude_location(plane.next_WP_loc, plane.prev_WP_loc);
 191     } else {
 192         plane.set_target_altitude_location(plane.next_WP_loc);
 193     }
 194 }
 195
 196 // returns true if the vehicle can be armed in this mode
 197 bool Mode::pre_arm_checks(size_t buflen, char *buffer) const
 198 {
 199     if (!_pre_arm_checks(buflen, buffer)) {
 200         if (strlen(buffer) == 0) {
 201             // If no message is provided add a generic one
 202             hal.util->snprintf(buffer, buflen, "mode not armable");
 203         }
 204         return false;
 205     }
 206
 207     return true;
 208 }
 209
 210 // Auto and Guided do not call this to bypass the q-mode check.
 211 bool Mode::_pre_arm_checks(size_t buflen, char *buffer) const
 212 {
 213 #if HAL_QUADPLANE_ENABLED
 214     if (plane.quadplane.enabled() && !is_vtol_mode() &&
 215             plane.quadplane.option_is_set(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO)) {
 216         hal.util->snprintf(buffer, buflen, "not Q mode");
 217         return false;
 218     }
 219 #endif
 220     return true;
 221 }
 222
 223 void Mode::run()
 224 {
 225     // Direct stick mixing functionality has been removed, so as not to remove all stick mixing from the user completely
 226     // the old direct option is now used to enable fbw mixing, this is easier than doing a param conversion.
 227     if ((plane.g.stick_mixing == StickMixing::FBW) || (plane.g.stick_mixing == StickMixing::DIRECT_REMOVED)) {
 228         plane.stabilize_stick_mixing_fbw();
 229     }
 230     plane.stabilize_roll();
 231     plane.stabilize_pitch();
 232     plane.stabilize_yaw();
 233 }
 234
 235 // Reset rate and steering controllers
 236 void Mode::reset_controllers()
 237 {
 238     // reset integrators
 239     plane.rollController.reset_I();
 240     plane.pitchController.reset_I();
 241     plane.yawController.reset_I();
 242
 243     // reset steering controls
 244     plane.steer_state.locked_course = false;
 245     plane.steer_state.locked_course_err = 0;
 246 }
 247
 248 bool Mode::is_taking_off() const
 249 {
 250     return (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF);
 251 }
 252
 253 // Helper to output to both k_rudder and k_steering servo functions
 254 void Mode::output_rudder_and_steering(float val)
 255 {
 256     SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, val);
 257     SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val);
 258 }
 259
 260 // Output pilot throttle, this is used in stabilized modes without auto throttle control
 261 // Direct mapping if THR_PASS_STAB is set
 262 // Otherwise apply curve for trim correction if configured
 263 void Mode::output_pilot_throttle()
 264 {
 265     if (plane.g.throttle_passthru_stabilize) {
 266         // THR_PASS_STAB set, direct mapping
 267         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true));
 268         return;
 269     }
 270
 271     // get throttle, but adjust center to output TRIM_THROTTLE if flight option set
 272     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_adjusted_throttle_input(true));
 273 }
 274
 275 // true if throttle min/max limits should be applied
 276 bool Mode::use_throttle_limits() const
 277 {
 278 #if AP_SCRIPTING_ENABLED
 279     if (plane.nav_scripting_active()) {
 280         return false;
 281     }
 282 #endif
 283
 284     if (this == &plane.mode_stabilize ||
 285         this == &plane.mode_training ||
 286         this == &plane.mode_acro ||
 287         this == &plane.mode_fbwa ||
 288         this == &plane.mode_autotune) {
 289         // a manual throttle mode
 290         return !plane.g.throttle_passthru_stabilize;
 291     }
 292
 293     if (is_guided_mode() && plane.guided_throttle_passthru) {
 294         // manual pass through of throttle while in GUIDED
 295         return false;
 296     }
 297
 298 #if HAL_QUADPLANE_ENABLED
 299     if (quadplane.in_vtol_mode()) {
 300         return quadplane.allow_forward_throttle_in_vtol_mode();
 301     }
 302 #endif
 303
 304     return true;
 305 }
 306
 307 // true if voltage correction should be applied to throttle
 308 bool Mode::use_battery_compensation() const
 309 {
 310 #if AP_SCRIPTING_ENABLED
 311     if (plane.nav_scripting_active()) {
 312         return false;
 313     }
 314 #endif
 315
 316     if (this == &plane.mode_stabilize ||
 317         this == &plane.mode_training ||
 318         this == &plane.mode_acro ||
 319         this == &plane.mode_fbwa ||
 320         this == &plane.mode_autotune) {
 321         // a manual throttle mode
 322         return false;
 323     }
 324
 325     if (is_guided_mode() && plane.guided_throttle_passthru) {
 326         // manual pass through of throttle while in GUIDED
 327         return false;
 328     }
 329
 330 #if HAL_QUADPLANE_ENABLED
 331     if (quadplane.in_vtol_mode()) {
 332         return false;
 333     }
 334 #endif
 335
 336     return true;
 337 }