2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
// returns true if the vehicle is in landing sequence. Intended only
|
|
|
|
// for use in failsafe code.
|
|
|
|
bool Plane::failsafe_in_landing_sequence() const
|
|
|
|
{
|
2022-09-29 20:10:41 -03:00
|
|
|
if (flight_stage == AP_FixedWing::FlightStage::LAND) {
|
2021-09-10 03:28:21 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
#if HAL_QUADPLANE_ENABLED
|
|
|
|
if (quadplane.in_vtol_land_sequence()) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
#endif
|
2022-07-06 02:08:20 -03:00
|
|
|
if (mission.get_in_landing_sequence_flag()) {
|
|
|
|
return true;
|
|
|
|
}
|
2021-09-10 03:28:21 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2019-10-17 00:49:32 -03:00
|
|
|
void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-08-16 21:50:15 -03:00
|
|
|
// This is how to handle a short loss of control signal failsafe.
|
2013-07-19 01:11:16 -03:00
|
|
|
failsafe.state = fstype;
|
2017-10-27 17:21:28 -03:00
|
|
|
failsafe.short_timer_ms = millis();
|
2021-11-02 21:06:13 -03:00
|
|
|
failsafe.saved_mode_number = control_mode->mode_number();
|
2019-01-15 13:46:13 -04:00
|
|
|
switch (control_mode->mode_number())
|
2012-08-16 21:50:15 -03:00
|
|
|
{
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::MANUAL:
|
|
|
|
case Mode::Number::STABILIZE:
|
|
|
|
case Mode::Number::ACRO:
|
|
|
|
case Mode::Number::FLY_BY_WIRE_A:
|
|
|
|
case Mode::Number::AUTOTUNE:
|
|
|
|
case Mode::Number::FLY_BY_WIRE_B:
|
|
|
|
case Mode::Number::CRUISE:
|
2021-11-02 21:06:13 -03:00
|
|
|
case Mode::Number::TRAINING:
|
2021-10-24 02:20:43 -03:00
|
|
|
if(plane.emergency_landing) {
|
|
|
|
set_mode(mode_fbwa, reason); // emergency landing switch overrides normal action to allow out of range landing
|
|
|
|
break;
|
|
|
|
}
|
2017-12-06 19:02:11 -04:00
|
|
|
if(g.fs_action_short == FS_ACTION_SHORT_FBWA) {
|
2019-01-15 13:46:13 -04:00
|
|
|
set_mode(mode_fbwa, reason);
|
2021-10-17 21:32:10 -03:00
|
|
|
} else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
|
|
|
|
set_mode(mode_fbwb, reason);
|
2013-09-13 21:30:13 -03:00
|
|
|
} else {
|
2021-11-02 21:06:13 -03:00
|
|
|
set_mode(mode_circle, reason); // circle if action = 0 or 1
|
2013-09-13 21:30:13 -03:00
|
|
|
}
|
2012-08-16 21:50:15 -03:00
|
|
|
break;
|
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::QSTABILIZE:
|
|
|
|
case Mode::Number::QLOITER:
|
|
|
|
case Mode::Number::QHOVER:
|
2021-09-10 03:28:21 -03:00
|
|
|
#if QAUTOTUNE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::QAUTOTUNE:
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::QACRO:
|
2022-08-05 12:08:59 -03:00
|
|
|
if (quadplane.option_is_set(QuadPlane::OPTION::FS_RTL)) {
|
2022-04-11 15:15:17 -03:00
|
|
|
set_mode(mode_rtl, reason);
|
2022-08-05 12:08:59 -03:00
|
|
|
} else if (quadplane.option_is_set(QuadPlane::OPTION::FS_QRTL)) {
|
2019-06-11 15:00:32 -03:00
|
|
|
set_mode(mode_qrtl, reason);
|
|
|
|
} else {
|
|
|
|
set_mode(mode_qland, reason);
|
|
|
|
}
|
2015-12-26 03:45:42 -04:00
|
|
|
break;
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif // HAL_QUADPLANE_ENABLED
|
|
|
|
|
|
|
|
case Mode::Number::AUTO: {
|
|
|
|
if (failsafe_in_landing_sequence()) {
|
2020-02-17 01:36:56 -04:00
|
|
|
// don't failsafe in a landing sequence
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
FALLTHROUGH;
|
2021-09-10 03:28:21 -03:00
|
|
|
}
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::AVOID_ADSB:
|
|
|
|
case Mode::Number::GUIDED:
|
|
|
|
case Mode::Number::LOITER:
|
2020-09-16 04:46:56 -03:00
|
|
|
case Mode::Number::THERMAL:
|
2021-11-02 21:06:13 -03:00
|
|
|
if (g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) { // if acton = 0(BESTGUESS) this group of modes take no action
|
2019-01-15 13:46:13 -04:00
|
|
|
failsafe.saved_mode_number = control_mode->mode_number();
|
2021-11-02 21:06:13 -03:00
|
|
|
if (g.fs_action_short == FS_ACTION_SHORT_FBWA) {
|
2019-01-15 13:46:13 -04:00
|
|
|
set_mode(mode_fbwa, reason);
|
2021-11-02 21:06:13 -03:00
|
|
|
} else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
|
|
|
|
set_mode(mode_fbwb, reason);
|
2013-09-13 21:30:13 -03:00
|
|
|
} else {
|
2019-01-15 13:46:13 -04:00
|
|
|
set_mode(mode_circle, reason);
|
2013-09-13 21:30:13 -03:00
|
|
|
}
|
2012-08-16 21:50:15 -03:00
|
|
|
}
|
2021-11-02 21:06:13 -03:00
|
|
|
break;
|
|
|
|
case Mode::Number::CIRCLE: // these modes never take any short failsafe action and continue
|
2019-05-04 06:56:07 -03:00
|
|
|
case Mode::Number::TAKEOFF:
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::RTL:
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::QLAND:
|
|
|
|
case Mode::Number::QRTL:
|
2021-10-17 21:56:21 -03:00
|
|
|
case Mode::Number::LOITER_ALT_QLAND:
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::INITIALISING:
|
2012-08-16 21:50:15 -03:00
|
|
|
break;
|
|
|
|
}
|
2022-12-29 19:37:45 -04:00
|
|
|
if (failsafe.saved_mode_number != control_mode->mode_number()) {
|
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "RC Short Failsafe: switched to %s", control_mode->name());
|
|
|
|
} else {
|
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "RC Short Failsafe On");
|
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2019-10-17 00:49:32 -03:00
|
|
|
void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-08-16 21:50:15 -03:00
|
|
|
// This is how to handle a long loss of control signal failsafe.
|
2012-12-04 18:22:21 -04:00
|
|
|
// If the GCS is locked up we allow control to revert to RC
|
2018-04-03 23:17:05 -03:00
|
|
|
RC_Channels::clear_overrides();
|
2013-07-19 01:11:16 -03:00
|
|
|
failsafe.state = fstype;
|
2019-01-15 13:46:13 -04:00
|
|
|
switch (control_mode->mode_number())
|
2012-08-16 21:50:15 -03:00
|
|
|
{
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::MANUAL:
|
|
|
|
case Mode::Number::STABILIZE:
|
|
|
|
case Mode::Number::ACRO:
|
|
|
|
case Mode::Number::FLY_BY_WIRE_A:
|
|
|
|
case Mode::Number::AUTOTUNE:
|
|
|
|
case Mode::Number::FLY_BY_WIRE_B:
|
|
|
|
case Mode::Number::CRUISE:
|
|
|
|
case Mode::Number::TRAINING:
|
|
|
|
case Mode::Number::CIRCLE:
|
2020-12-26 18:07:20 -04:00
|
|
|
case Mode::Number::LOITER:
|
|
|
|
case Mode::Number::THERMAL:
|
2021-10-24 02:20:43 -03:00
|
|
|
if(plane.emergency_landing) {
|
|
|
|
set_mode(mode_fbwa, reason); // emergency landing switch overrides normal action to allow out of range landing
|
|
|
|
break;
|
|
|
|
}
|
2017-12-06 19:02:11 -04:00
|
|
|
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
|
2016-01-11 11:29:03 -04:00
|
|
|
#if PARACHUTE == ENABLED
|
|
|
|
parachute_release();
|
|
|
|
#endif
|
2017-12-06 19:02:11 -04:00
|
|
|
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
|
2019-01-15 13:46:13 -04:00
|
|
|
set_mode(mode_fbwa, reason);
|
2013-09-13 21:30:13 -03:00
|
|
|
} else {
|
2019-01-15 13:46:13 -04:00
|
|
|
set_mode(mode_rtl, reason);
|
2013-09-13 21:30:13 -03:00
|
|
|
}
|
2012-08-16 21:50:15 -03:00
|
|
|
break;
|
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::QSTABILIZE:
|
|
|
|
case Mode::Number::QHOVER:
|
|
|
|
case Mode::Number::QLOITER:
|
|
|
|
case Mode::Number::QACRO:
|
2021-09-10 03:28:21 -03:00
|
|
|
#if QAUTOTUNE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::QAUTOTUNE:
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2022-08-05 12:08:59 -03:00
|
|
|
if (quadplane.option_is_set(QuadPlane::OPTION::FS_RTL)) {
|
2022-04-11 15:15:17 -03:00
|
|
|
set_mode(mode_rtl, reason);
|
2022-08-05 12:08:59 -03:00
|
|
|
} else if (quadplane.option_is_set(QuadPlane::OPTION::FS_QRTL)) {
|
2019-06-11 15:00:32 -03:00
|
|
|
set_mode(mode_qrtl, reason);
|
|
|
|
} else {
|
|
|
|
set_mode(mode_qland, reason);
|
|
|
|
}
|
2015-12-26 03:45:42 -04:00
|
|
|
break;
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif // HAL_QUADPLANE_ENABLED
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::AUTO:
|
2021-09-10 03:28:21 -03:00
|
|
|
if (failsafe_in_landing_sequence()) {
|
2020-02-17 01:36:56 -04:00
|
|
|
// don't failsafe in a landing sequence
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
FALLTHROUGH;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::AVOID_ADSB:
|
|
|
|
case Mode::Number::GUIDED:
|
2017-12-06 19:02:11 -04:00
|
|
|
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
|
2016-01-11 11:29:03 -04:00
|
|
|
#if PARACHUTE == ENABLED
|
|
|
|
parachute_release();
|
|
|
|
#endif
|
2017-12-06 19:02:11 -04:00
|
|
|
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
|
2019-01-15 13:46:13 -04:00
|
|
|
set_mode(mode_fbwa, reason);
|
2017-12-06 19:02:11 -04:00
|
|
|
} else if (g.fs_action_long == FS_ACTION_LONG_RTL) {
|
2019-01-15 13:46:13 -04:00
|
|
|
set_mode(mode_rtl, reason);
|
2012-08-16 21:50:15 -03:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::RTL:
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::QLAND:
|
|
|
|
case Mode::Number::QRTL:
|
2021-10-17 21:56:21 -03:00
|
|
|
case Mode::Number::LOITER_ALT_QLAND:
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2019-05-04 06:56:07 -03:00
|
|
|
case Mode::Number::TAKEOFF:
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::INITIALISING:
|
2012-08-16 21:50:15 -03:00
|
|
|
break;
|
|
|
|
}
|
2022-12-29 19:37:45 -04:00
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe On: %s", (reason == ModeReason:: GCS_FAILSAFE) ? "GCS" : "RC Long", control_mode->name());
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2019-10-17 00:49:32 -03:00
|
|
|
void Plane::failsafe_short_off_event(ModeReason reason)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-08-16 21:50:15 -03:00
|
|
|
// We're back in radio contact
|
2022-02-26 09:40:12 -04:00
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Short Failsafe Cleared");
|
2013-07-19 01:11:16 -03:00
|
|
|
failsafe.state = FAILSAFE_NONE;
|
2022-07-08 18:51:21 -03:00
|
|
|
// restore entry mode if desired but check that our current mode is still due to failsafe
|
|
|
|
if (control_mode_reason == ModeReason::RADIO_FAILSAFE) {
|
2021-11-02 21:06:13 -03:00
|
|
|
set_mode_by_number(failsafe.saved_mode_number, ModeReason::RADIO_FAILSAFE_RECOVERY);
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"Flight mode %s restored",control_mode->name());
|
2022-07-08 18:51:21 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2019-10-17 00:49:32 -03:00
|
|
|
void Plane::failsafe_long_off_event(ModeReason reason)
|
2017-09-22 00:31:44 -03:00
|
|
|
{
|
2022-02-26 09:40:12 -04:00
|
|
|
// We're back in radio contact with RC or GCS
|
|
|
|
if (reason == ModeReason:: GCS_FAILSAFE) {
|
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Off");
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "RC Long Failsafe Cleared");
|
|
|
|
}
|
2017-09-22 00:31:44 -03:00
|
|
|
failsafe.state = FAILSAFE_NONE;
|
|
|
|
}
|
|
|
|
|
2017-11-09 18:34:12 -04:00
|
|
|
void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2017-11-09 18:34:12 -04:00
|
|
|
switch ((Failsafe_Action)action) {
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2021-09-27 16:39:18 -03:00
|
|
|
case Failsafe_Action_Loiter_alt_QLand:
|
|
|
|
if (quadplane.available()) {
|
2022-02-03 23:24:05 -04:00
|
|
|
plane.set_mode(mode_loiter_qland, ModeReason::BATTERY_FAILSAFE);
|
2021-09-27 16:39:18 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
FALLTHROUGH;
|
|
|
|
|
2018-11-23 19:00:31 -04:00
|
|
|
case Failsafe_Action_QLand:
|
|
|
|
if (quadplane.available()) {
|
2019-10-17 00:49:32 -03:00
|
|
|
plane.set_mode(mode_qland, ModeReason::BATTERY_FAILSAFE);
|
2018-11-23 19:00:31 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
FALLTHROUGH;
|
2021-09-27 16:39:18 -03:00
|
|
|
#endif // HAL_QUADPLANE_ENABLED
|
2021-09-10 03:28:21 -03:00
|
|
|
case Failsafe_Action_Land: {
|
2022-09-29 20:10:41 -03:00
|
|
|
bool already_landing = flight_stage == AP_FixedWing::FlightStage::LAND;
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2022-02-03 23:24:05 -04:00
|
|
|
if (control_mode == &mode_qland || control_mode == &mode_loiter_qland) {
|
2021-09-10 03:28:21 -03:00
|
|
|
already_landing = true;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
if (!already_landing) {
|
2017-11-09 18:34:12 -04:00
|
|
|
// never stop a landing if we were already committed
|
2020-02-17 19:45:52 -04:00
|
|
|
if (plane.mission.is_best_land_sequence()) {
|
|
|
|
// continue mission as it will reach a landing in less distance
|
|
|
|
plane.mission.set_in_landing_sequence_flag(true);
|
|
|
|
break;
|
|
|
|
}
|
2017-11-09 18:34:12 -04:00
|
|
|
if (plane.mission.jump_to_landing_sequence()) {
|
2019-10-17 00:49:32 -03:00
|
|
|
plane.set_mode(mode_auto, ModeReason::BATTERY_FAILSAFE);
|
2017-11-09 18:34:12 -04:00
|
|
|
break;
|
2019-01-23 12:12:01 -04:00
|
|
|
}
|
2017-11-09 18:34:12 -04:00
|
|
|
}
|
|
|
|
FALLTHROUGH;
|
2021-09-10 03:28:21 -03:00
|
|
|
}
|
|
|
|
case Failsafe_Action_RTL: {
|
2022-09-29 20:10:41 -03:00
|
|
|
bool already_landing = flight_stage == AP_FixedWing::FlightStage::LAND;
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2022-02-03 23:24:05 -04:00
|
|
|
if (control_mode == &mode_qland || control_mode == &mode_loiter_qland ||
|
2021-09-10 03:28:21 -03:00
|
|
|
quadplane.in_vtol_land_sequence()) {
|
|
|
|
already_landing = true;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
if (!already_landing) {
|
2017-11-09 18:34:12 -04:00
|
|
|
// never stop a landing if we were already committed
|
2022-03-22 21:29:29 -03:00
|
|
|
if (g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START && plane.mission.is_best_land_sequence()) {
|
2020-02-17 19:45:52 -04:00
|
|
|
// continue mission as it will reach a landing in less distance
|
|
|
|
plane.mission.set_in_landing_sequence_flag(true);
|
|
|
|
break;
|
|
|
|
}
|
2019-10-17 00:49:32 -03:00
|
|
|
set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE);
|
2017-11-09 18:34:12 -04:00
|
|
|
aparm.throttle_cruise.load();
|
|
|
|
}
|
|
|
|
break;
|
2021-09-10 03:28:21 -03:00
|
|
|
}
|
2019-01-23 12:12:01 -04:00
|
|
|
|
2017-11-09 18:34:12 -04:00
|
|
|
case Failsafe_Action_Terminate:
|
2019-01-08 21:05:40 -04:00
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
2017-11-09 18:34:12 -04:00
|
|
|
char battery_type_str[17];
|
|
|
|
snprintf(battery_type_str, 17, "%s battery", type_str);
|
|
|
|
afs.gcs_terminate(true, battery_type_str);
|
2019-01-08 21:05:40 -04:00
|
|
|
#else
|
2020-02-21 09:09:58 -04:00
|
|
|
arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);
|
2019-01-08 21:05:40 -04:00
|
|
|
#endif
|
2017-11-09 18:34:12 -04:00
|
|
|
break;
|
2019-01-23 12:12:01 -04:00
|
|
|
|
2019-02-11 10:20:38 -04:00
|
|
|
case Failsafe_Action_Parachute:
|
2019-08-20 21:56:45 -03:00
|
|
|
#if PARACHUTE == ENABLED
|
2019-02-11 10:20:38 -04:00
|
|
|
parachute_release();
|
2019-08-20 21:56:45 -03:00
|
|
|
#endif
|
2019-02-11 10:20:38 -04:00
|
|
|
break;
|
|
|
|
|
2017-11-09 18:34:12 -04:00
|
|
|
case Failsafe_Action_None:
|
|
|
|
// don't actually do anything, however we should still flag the system as having hit a failsafe
|
|
|
|
// and ensure all appropriate flags are going off to the user
|
|
|
|
break;
|
2015-06-20 18:26:31 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|