ardupilot/ArduPlane/events.cpp

301 lines
10 KiB
C++
Raw Normal View History

2015-05-13 03:09:36 -03:00
#include "Plane.h"
// returns true if the vehicle is in landing sequence. Intended only
// for use in failsafe code.
bool Plane::failsafe_in_landing_sequence() const
{
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
return true;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_land_sequence()) {
return true;
}
#endif
return false;
}
void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason)
{
2012-08-16 21:50:15 -03:00
// This is how to handle a short loss of control signal failsafe.
failsafe.state = fstype;
failsafe.short_timer_ms = millis();
2021-11-02 21:06:13 -03:00
failsafe.saved_mode_number = control_mode->mode_number();
gcs().send_text(MAV_SEVERITY_WARNING, "RC Short Failsafe On");
switch (control_mode->mode_number())
2012-08-16 21:50:15 -03: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:
if(plane.emergency_landing) {
set_mode(mode_fbwa, reason); // emergency landing switch overrides normal action to allow out of range landing
break;
}
if(g.fs_action_short == FS_ACTION_SHORT_FBWA) {
set_mode(mode_fbwa, reason);
} else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
set_mode(mode_fbwb, reason);
} else {
2021-11-02 21:06:13 -03:00
set_mode(mode_circle, reason); // circle if action = 0 or 1
}
2021-11-02 21:06:13 -03:00
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %s", control_mode->name());
2012-08-16 21:50:15 -03:00
break;
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QSTABILIZE:
case Mode::Number::QLOITER:
case Mode::Number::QHOVER:
#if QAUTOTUNE_ENABLED
case Mode::Number::QAUTOTUNE:
#endif
case Mode::Number::QACRO:
if (quadplane.options & QuadPlane::OPTION_FS_RTL) {
set_mode(mode_rtl, reason);
} else if (quadplane.options & QuadPlane::OPTION_FS_QRTL) {
set_mode(mode_qrtl, reason);
} else {
set_mode(mode_qland, reason);
}
2021-11-02 21:06:13 -03:00
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %s", control_mode->name());
break;
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::AUTO: {
if (failsafe_in_landing_sequence()) {
// don't failsafe in a landing sequence
break;
}
FALLTHROUGH;
}
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::LOITER:
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
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) {
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);
} else {
set_mode(mode_circle, reason);
}
2021-11-02 21:06:13 -03:00
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %s", control_mode->name());
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
case Mode::Number::TAKEOFF:
case Mode::Number::RTL:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QLAND:
case Mode::Number::QRTL:
case Mode::Number::LOITER_ALT_QLAND:
#endif
case Mode::Number::INITIALISING:
2012-08-16 21:50:15 -03:00
break;
}
}
void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason)
{
2012-08-16 21:50:15 -03:00
// This is how to handle a long loss of control signal failsafe.
if (reason == ModeReason:: GCS_FAILSAFE) {
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe On");
}
else {
gcs().send_text(MAV_SEVERITY_WARNING, "RC Long Failsafe On");
}
2012-12-04 18:22:21 -04:00
// If the GCS is locked up we allow control to revert to RC
RC_Channels::clear_overrides();
failsafe.state = fstype;
switch (control_mode->mode_number())
2012-08-16 21:50:15 -03: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:
case Mode::Number::LOITER:
case Mode::Number::THERMAL:
if(plane.emergency_landing) {
set_mode(mode_fbwa, reason); // emergency landing switch overrides normal action to allow out of range landing
break;
}
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
2016-01-11 11:29:03 -04:00
#if PARACHUTE == ENABLED
parachute_release();
#endif
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
set_mode(mode_fbwa, reason);
} else {
set_mode(mode_rtl, reason);
}
2012-08-16 21:50:15 -03:00
break;
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLOITER:
case Mode::Number::QACRO:
#if QAUTOTUNE_ENABLED
case Mode::Number::QAUTOTUNE:
#endif
if (quadplane.options & QuadPlane::OPTION_FS_RTL) {
set_mode(mode_rtl, reason);
} else if (quadplane.options & QuadPlane::OPTION_FS_QRTL) {
set_mode(mode_qrtl, reason);
} else {
set_mode(mode_qland, reason);
}
break;
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::AUTO:
if (failsafe_in_landing_sequence()) {
// don't failsafe in a landing sequence
break;
}
FALLTHROUGH;
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
2016-01-11 11:29:03 -04:00
#if PARACHUTE == ENABLED
parachute_release();
#endif
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
set_mode(mode_fbwa, reason);
} else if (g.fs_action_long == FS_ACTION_LONG_RTL) {
set_mode(mode_rtl, reason);
2012-08-16 21:50:15 -03:00
}
break;
case Mode::Number::RTL:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QLAND:
case Mode::Number::QRTL:
case Mode::Number::LOITER_ALT_QLAND:
#endif
case Mode::Number::TAKEOFF:
case Mode::Number::INITIALISING:
2012-08-16 21:50:15 -03:00
break;
}
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %s", control_mode->name());
}
void Plane::failsafe_short_off_event(ModeReason reason)
{
2012-08-16 21:50:15 -03:00
// We're back in radio contact
gcs().send_text(MAV_SEVERITY_WARNING, "Short Failsafe Cleared");
failsafe.state = FAILSAFE_NONE;
2021-11-02 21:06:13 -03:00
//restore entry mode if desired but check that our current mode is still due to failsafe
if ( _last_reason == ModeReason::RADIO_FAILSAFE) {
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());
}
}
void Plane::failsafe_long_off_event(ModeReason reason)
2017-09-22 00:31:44 -03: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)
{
2017-11-09 18:34:12 -04:00
switch ((Failsafe_Action)action) {
#if HAL_QUADPLANE_ENABLED
case Failsafe_Action_Loiter_alt_QLand:
if (quadplane.available()) {
plane.set_mode(mode_loiter_qland, ModeReason::BATTERY_FAILSAFE);
break;
}
FALLTHROUGH;
case Failsafe_Action_QLand:
if (quadplane.available()) {
plane.set_mode(mode_qland, ModeReason::BATTERY_FAILSAFE);
break;
}
FALLTHROUGH;
#endif // HAL_QUADPLANE_ENABLED
case Failsafe_Action_Land: {
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qland || control_mode == &mode_loiter_qland) {
already_landing = true;
}
#endif
if (!already_landing) {
2017-11-09 18:34:12 -04:00
// never stop a landing if we were already committed
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()) {
plane.set_mode(mode_auto, ModeReason::BATTERY_FAILSAFE);
2017-11-09 18:34:12 -04:00
break;
}
2017-11-09 18:34:12 -04:00
}
FALLTHROUGH;
}
case Failsafe_Action_RTL: {
bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND;
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qland || control_mode == &mode_loiter_qland ||
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
if (g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START && 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;
}
set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE);
2017-11-09 18:34:12 -04:00
aparm.throttle_cruise.load();
}
break;
}
2017-11-09 18:34:12 -04:00
case Failsafe_Action_Terminate:
#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);
#else
2020-02-21 09:09:58 -04:00
arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);
#endif
2017-11-09 18:34:12 -04:00
break;
case Failsafe_Action_Parachute:
#if PARACHUTE == ENABLED
parachute_release();
#endif
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;
}
}