ArduPlane/events.cpp

   1 #include "Plane.h"
   2
   3 // returns true if the vehicle is in landing sequence.  Intended only
   4 // for use in failsafe code.
   5 bool Plane::failsafe_in_landing_sequence() const
   6 {
   7     if (flight_stage == AP_FixedWing::FlightStage::LAND) {
   8         return true;
   9     }
  10 #if HAL_QUADPLANE_ENABLED
  11     if (quadplane.in_vtol_land_sequence()) {
  12         return true;
  13     }
  14 #endif
  15     if (mission.get_in_landing_sequence_flag()) {
  16         return true;
  17     }
  18     return false;
  19 }
  20
  21 void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason)
  22 {
  23     // This is how to handle a short loss of control signal failsafe.
  24     failsafe.state = fstype;
  25     failsafe.short_timer_ms = millis();
  26     failsafe.saved_mode_number = control_mode->mode_number();
  27     switch (control_mode->mode_number())
  28     {
  29     case Mode::Number::MANUAL:
  30     case Mode::Number::STABILIZE:
  31     case Mode::Number::ACRO:
  32     case Mode::Number::FLY_BY_WIRE_A:
  33     case Mode::Number::AUTOTUNE:
  34     case Mode::Number::FLY_BY_WIRE_B:
  35     case Mode::Number::CRUISE:
  36     case Mode::Number::TRAINING:
  37         if(plane.emergency_landing) {
  38             set_mode(mode_fbwa, reason); // emergency landing switch overrides normal action to allow out of range landing
  39             break;
  40         }
  41         if(g.fs_action_short == FS_ACTION_SHORT_FBWA) {
  42             set_mode(mode_fbwa, reason);
  43         } else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
  44             set_mode(mode_fbwb, reason);
  45         } else {
  46             set_mode(mode_circle, reason); // circle if action = 0 or 1
  47         }
  48         break;
  49
  50 #if HAL_QUADPLANE_ENABLED
  51     case Mode::Number::QSTABILIZE:
  52     case Mode::Number::QLOITER:
  53     case Mode::Number::QHOVER:
  54 #if QAUTOTUNE_ENABLED
  55     case Mode::Number::QAUTOTUNE:
  56 #endif
  57     case Mode::Number::QACRO:
  58         if (quadplane.option_is_set(QuadPlane::OPTION::FS_RTL)) {
  59             set_mode(mode_rtl, reason);
  60         } else if (quadplane.option_is_set(QuadPlane::OPTION::FS_QRTL)) {
  61             set_mode(mode_qrtl, reason);
  62         } else {
  63             set_mode(mode_qland, reason);
  64         }
  65         break;
  66 #endif // HAL_QUADPLANE_ENABLED
  67
  68     case Mode::Number::AUTO: {
  69         if (failsafe_in_landing_sequence()) {
  70             // don't failsafe in a landing sequence
  71             break;
  72         }
  73         FALLTHROUGH;
  74     }
  75     case Mode::Number::AVOID_ADSB:
  76     case Mode::Number::GUIDED:
  77     case Mode::Number::LOITER:
  78     case Mode::Number::THERMAL:
  79         if (g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) { // if acton = 0(BESTGUESS) this group of modes take no action
  80             failsafe.saved_mode_number = control_mode->mode_number();
  81             if (g.fs_action_short == FS_ACTION_SHORT_FBWA) {
  82                 set_mode(mode_fbwa, reason);
  83             } else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
  84                 set_mode(mode_fbwb, reason);
  85             } else {
  86                 set_mode(mode_circle, reason);
  87             }
  88         }
  89          break;
  90     case Mode::Number::CIRCLE:  // these modes never take any short failsafe action and continue
  91     case Mode::Number::TAKEOFF:
  92     case Mode::Number::RTL:
  93 #if HAL_QUADPLANE_ENABLED
  94     case Mode::Number::QLAND:
  95     case Mode::Number::QRTL:
  96     case Mode::Number::LOITER_ALT_QLAND:
  97 #endif
  98     case Mode::Number::INITIALISING:
  99         break;
 100     }
 101     if (failsafe.saved_mode_number != control_mode->mode_number()) {
 102         gcs().send_text(MAV_SEVERITY_WARNING, "RC Short Failsafe: switched to %s", control_mode->name());
 103     } else {
 104         gcs().send_text(MAV_SEVERITY_WARNING, "RC Short Failsafe On");
 105     }
 106 }
 107
 108 void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason)
 109 {
 110
 111     // This is how to handle a long loss of control signal failsafe.
 112     //  If the GCS is locked up we allow control to revert to RC
 113     RC_Channels::clear_overrides();
 114     failsafe.state = fstype;
 115     switch (control_mode->mode_number())
 116     {
 117     case Mode::Number::MANUAL:
 118     case Mode::Number::STABILIZE:
 119     case Mode::Number::ACRO:
 120     case Mode::Number::FLY_BY_WIRE_A:
 121     case Mode::Number::AUTOTUNE:
 122     case Mode::Number::FLY_BY_WIRE_B:
 123     case Mode::Number::CRUISE:
 124     case Mode::Number::TRAINING:
 125     case Mode::Number::CIRCLE:
 126     case Mode::Number::LOITER:
 127     case Mode::Number::THERMAL:
 128     case Mode::Number::TAKEOFF:
 129         if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && !(g.fs_action_long == FS_ACTION_LONG_GLIDE || g.fs_action_long == FS_ACTION_LONG_PARACHUTE)) {
 130             // don't failsafe if in inital climb of TAKEOFF mode and FS action is not parachute or glide
 131             // long failsafe will be re-called if still in fs after initial climb
 132             long_failsafe_pending = true;
 133             break;
 134         }
 135
 136         if(plane.emergency_landing) {
 137             set_mode(mode_fbwa, reason); // emergency landing switch overrides normal action to allow out of range landing
 138             break;
 139         }
 140         if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
 141 #if PARACHUTE == ENABLED
 142             parachute_release();
 143 #endif
 144         } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
 145             set_mode(mode_fbwa, reason);
 146         } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
 147             set_mode(mode_auto, reason);
 148         } else {
 149             set_mode(mode_rtl, reason);
 150         }
 151         break;
 152
 153 #if HAL_QUADPLANE_ENABLED
 154     case Mode::Number::QSTABILIZE:
 155     case Mode::Number::QHOVER:
 156     case Mode::Number::QLOITER:
 157     case Mode::Number::QACRO:
 158 #if QAUTOTUNE_ENABLED
 159     case Mode::Number::QAUTOTUNE:
 160 #endif
 161         if (quadplane.option_is_set(QuadPlane::OPTION::FS_RTL)) {
 162             set_mode(mode_rtl, reason);
 163         } else if (quadplane.option_is_set(QuadPlane::OPTION::FS_QRTL)) {
 164             set_mode(mode_qrtl, reason);
 165         } else {
 166             set_mode(mode_qland, reason);
 167         }
 168         break;
 169 #endif  // HAL_QUADPLANE_ENABLED
 170
 171     case Mode::Number::AUTO:
 172         if (failsafe_in_landing_sequence()) {
 173             // don't failsafe in a landing sequence
 174             break;
 175         }
 176
 177 #if HAL_QUADPLANE_ENABLED
 178         if (quadplane.in_vtol_takeoff()) {
 179             set_mode(mode_qland, reason);
 180             // QLAND if in VTOL takeoff
 181             break;
 182         }
 183 #endif
 184         FALLTHROUGH;
 185
 186     case Mode::Number::AVOID_ADSB:
 187     case Mode::Number::GUIDED:
 188
 189         if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
 190 #if PARACHUTE == ENABLED
 191             parachute_release();
 192 #endif
 193         } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
 194             set_mode(mode_fbwa, reason);
 195         } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
 196             set_mode(mode_auto, reason);
 197         } else if (g.fs_action_long == FS_ACTION_LONG_RTL) {
 198             set_mode(mode_rtl, reason);
 199         }
 200         break;
 201
 202     case Mode::Number::RTL:
 203         if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
 204             set_mode(mode_auto, reason);
 205         }
 206         break;
 207 #if HAL_QUADPLANE_ENABLED
 208     case Mode::Number::QLAND:
 209     case Mode::Number::QRTL:
 210     case Mode::Number::LOITER_ALT_QLAND:
 211 #endif
 212     case Mode::Number::INITIALISING:
 213         break;
 214     }
 215     gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe On: %s", (reason == ModeReason:: GCS_FAILSAFE) ? "GCS" : "RC Long", control_mode->name());
 216 }
 217
 218 void Plane::failsafe_short_off_event(ModeReason reason)
 219 {
 220     // We're back in radio contact
 221     gcs().send_text(MAV_SEVERITY_WARNING, "Short Failsafe Cleared");
 222     failsafe.state = FAILSAFE_NONE;
 223     // restore entry mode if desired but check that our current mode is still due to failsafe
 224     if (control_mode_reason == ModeReason::RADIO_FAILSAFE) {
 225        set_mode_by_number(failsafe.saved_mode_number, ModeReason::RADIO_FAILSAFE_RECOVERY);
 226        gcs().send_text(MAV_SEVERITY_INFO,"Flight mode %s restored",control_mode->name());
 227     }
 228 }
 229
 230 void Plane::failsafe_long_off_event(ModeReason reason)
 231 {
 232     long_failsafe_pending = false;
 233     // We're back in radio contact with RC or GCS
 234     if (reason == ModeReason:: GCS_FAILSAFE) {
 235         gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Off");
 236     }
 237     else {
 238         gcs().send_text(MAV_SEVERITY_WARNING, "RC Long Failsafe Cleared");
 239     }
 240     failsafe.state = FAILSAFE_NONE;
 241 }
 242
 243 void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
 244 {
 245     switch ((Failsafe_Action)action) {
 246 #if HAL_QUADPLANE_ENABLED
 247         case Failsafe_Action_Loiter_alt_QLand:
 248             if (quadplane.available()) {
 249                 plane.set_mode(mode_loiter_qland, ModeReason::BATTERY_FAILSAFE);
 250                 break;
 251             }
 252             FALLTHROUGH;
 253
 254         case Failsafe_Action_QLand:
 255             if (quadplane.available()) {
 256                 plane.set_mode(mode_qland, ModeReason::BATTERY_FAILSAFE);
 257                 break;
 258             }
 259             FALLTHROUGH;
 260 #endif // HAL_QUADPLANE_ENABLED
 261         case Failsafe_Action_Land: {
 262             bool already_landing = flight_stage == AP_FixedWing::FlightStage::LAND;
 263 #if HAL_QUADPLANE_ENABLED
 264             if (control_mode == &mode_qland || control_mode == &mode_loiter_qland) {
 265                 already_landing = true;
 266             }
 267 #endif
 268             if (!already_landing && plane.have_position) {
 269                 // never stop a landing if we were already committed
 270                 if (plane.mission.is_best_land_sequence(plane.current_loc)) {
 271                     // continue mission as it will reach a landing in less distance
 272                     plane.mission.set_in_landing_sequence_flag(true);
 273                     break;
 274                 }
 275                 if (plane.mission.jump_to_landing_sequence(plane.current_loc)) {
 276                     plane.set_mode(mode_auto, ModeReason::BATTERY_FAILSAFE);
 277                     break;
 278                 }
 279             }
 280             FALLTHROUGH;
 281         }
 282         case Failsafe_Action_RTL: {
 283             bool already_landing = flight_stage == AP_FixedWing::FlightStage::LAND;
 284 #if HAL_QUADPLANE_ENABLED
 285             if (control_mode == &mode_qland || control_mode == &mode_loiter_qland ||
 286                 quadplane.in_vtol_land_sequence()) {
 287                 already_landing = true;
 288             }
 289 #endif
 290             if (!already_landing) {
 291                 // never stop a landing if we were already committed
 292                 if ((g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) && plane.have_position && plane.mission.is_best_land_sequence(plane.current_loc)) {
 293                     // continue mission as it will reach a landing in less distance
 294                     plane.mission.set_in_landing_sequence_flag(true);
 295                     break;
 296                 }
 297                 set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE);
 298                 aparm.throttle_cruise.load();
 299             }
 300             break;
 301         }
 302
 303         case Failsafe_Action_Terminate:
 304 #if AP_ADVANCEDFAILSAFE_ENABLED
 305             char battery_type_str[17];
 306             snprintf(battery_type_str, 17, "%s battery", type_str);
 307             afs.gcs_terminate(true, battery_type_str);
 308 #else
 309             arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);
 310 #endif
 311             break;
 312
 313         case Failsafe_Action_Parachute:
 314 #if PARACHUTE == ENABLED
 315             parachute_release();
 316 #endif
 317             break;
 318
 319         case Failsafe_Action_None:
 320             // don't actually do anything, however we should still flag the system as having hit a failsafe
 321             // and ensure all appropriate flags are going off to the user
 322             break;
 323     }
 324 }