Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: add RTL as a short failsafe option

Browse Source
This commit is contained in:
Peter Barker 2024-10-26 13:08:45 +09:00
parent 380e9aa36b
commit 21c478e5a2
3 changed files with 51 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/Plane.h
View File

@ -360,6 +360,9 @@ private:
// saved flight mode
enum Mode::Number saved_mode_number;
// saved waypoint number
uint16_t saved_waypoint;
// A tracking variable for type of failsafe active
// Used for failsafe based on loss of RC signal or GCS signal
int16_t state;
@ -1040,6 +1043,7 @@ private:
// events.cpp
void failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason);
void failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason);
void failsafe_long_on_event_most_modes(enum failsafe_state fstype, ModeReason reason);
void failsafe_short_off_event(ModeReason reason);
void failsafe_long_off_event(ModeReason reason);
void handle_battery_failsafe(const char* type_str, const int8_t action);

1
ArduPlane/defines.h
View File

@ -37,6 +37,7 @@ enum failsafe_action_short {
FS_ACTION_SHORT_FBWA = 2,
FS_ACTION_SHORT_DISABLED = 3,
FS_ACTION_SHORT_FBWB = 4,
FS_ACTION_SHORT_RTL = 5,
};
enum failsafe_action_long {

69
ArduPlane/events.cpp
View File

@ -42,6 +42,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
set_mode(mode_fbwa, reason);
} else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
set_mode(mode_fbwb, reason);
} else if (g.fs_action_short == FS_ACTION_SHORT_RTL) {
failsafe.saved_waypoint = AP::mission()->get_current_nav_index();
set_mode(mode_rtl, reason);
} else {
set_mode(mode_circle, reason); // circle if action = 0 or 1
}
@ -82,6 +85,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
set_mode(mode_fbwa, reason);
} else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
set_mode(mode_fbwb, reason);
} else if (g.fs_action_short == FS_ACTION_SHORT_RTL) {
failsafe.saved_waypoint = AP::mission()->get_current_nav_index();
set_mode(mode_rtl, reason);
} else {
set_mode(mode_circle, reason);
}
@ -96,6 +102,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
case Mode::Number::LOITER_ALT_QLAND:
#endif
case Mode::Number::INITIALISING:
// do nothing
break;
}
if (failsafe.saved_mode_number != control_mode->mode_number()) {
@ -105,6 +112,32 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
}
}
void Plane::failsafe_long_on_event_most_modes(enum failsafe_state fstype, ModeReason reason)
{
if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && !(g.fs_action_long == FS_ACTION_LONG_GLIDE || g.fs_action_long == FS_ACTION_LONG_PARACHUTE)) {
// don't failsafe if in inital climb of TAKEOFF mode and FS action is not parachute or glide
// long failsafe will be re-called if still in fs after initial climb
long_failsafe_pending = true;
return;
}
if(plane.emergency_landing) {
set_mode(mode_fbwa, reason); // emergency landing switch overrides normal action to allow out of range landing
return;
}
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
#if HAL_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_AUTO) {
set_mode(mode_auto, reason);
} else {
set_mode(mode_rtl, reason);
}
}
void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason)
{
@ -126,28 +159,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
case Mode::Number::LOITER:
case Mode::Number::THERMAL:
case Mode::Number::TAKEOFF:
if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && !(g.fs_action_long == FS_ACTION_LONG_GLIDE || g.fs_action_long == FS_ACTION_LONG_PARACHUTE)) {
// don't failsafe if in inital climb of TAKEOFF mode and FS action is not parachute or glide
// long failsafe will be re-called if still in fs after initial climb
long_failsafe_pending = true;
break;
}
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) {
#if HAL_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_AUTO) {
set_mode(mode_auto, reason);
} else {
set_mode(mode_rtl, reason);
}
failsafe_long_on_event_most_modes(fstype, reason);
break;
#if HAL_QUADPLANE_ENABLED
@ -169,8 +181,13 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::AUTO:
if (g.fs_action_short == FS_ACTION_SHORT_RTL) {
failsafe_long_on_event_most_modes(fstype, reason);
break;
}
if (failsafe_in_landing_sequence()) {
// don't failsafe in a landing sequence
// do nothing if in a landing sequence
break;
}
@ -202,6 +219,8 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
case Mode::Number::RTL:
if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
set_mode(mode_auto, reason);
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
set_mode(mode_fbwa, reason);
}
break;
#if HAL_QUADPLANE_ENABLED
@ -225,6 +244,10 @@ void Plane::failsafe_short_off_event(ModeReason reason)
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());
}
if (failsafe.saved_mode_number == Mode::Number::AUTO) {
gcs().send_text(MAV_SEVERITY_INFO,"Waypoint %u restored", failsafe.saved_waypoint);
AP::mission()->set_current_cmd(failsafe.saved_waypoint);
}
}
void Plane::failsafe_long_off_event(ModeReason reason)