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

ArduPlane: add failsafe protections to Mode Takeoff

Browse Source
This commit is contained in:
Henry Wurzburg 2023-08-08 08:24:11 -05:00 committed by Andrew Tridgell
parent af34b1e43b
commit e9414d69ff
4 changed files with 22 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/Plane.h
View File

@ -218,6 +218,9 @@ private:
// external failsafe boards during baro and airspeed calibration
bool in_calibration;
// are we currently in long failsafe but have postponed it in MODE TAKEOFF until min level alt is reached
bool long_failsafe_pending;
// GCS selection
GCS_Plane _gcs; // avoid using this; use gcs()
GCS_Plane &gcs() { return _gcs; }

11
ArduPlane/events.cpp
View File

@ -107,6 +107,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason)
{
// This is how to handle a long loss of control signal failsafe.
// If the GCS is locked up we allow control to revert to RC
RC_Channels::clear_overrides();
@ -124,6 +125,14 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
case Mode::Number::CIRCLE:
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;
@ -196,7 +205,6 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
case Mode::Number::QRTL:
case Mode::Number::LOITER_ALT_QLAND:
#endif
case Mode::Number::TAKEOFF:
case Mode::Number::INITIALISING:
break;
}
@ -217,6 +225,7 @@ void Plane::failsafe_short_off_event(ModeReason reason)
void Plane::failsafe_long_off_event(ModeReason reason)
{
long_failsafe_pending = false;
// We're back in radio contact with RC or GCS
if (reason == ModeReason:: GCS_FAILSAFE) {
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Off");

3
ArduPlane/mode.cpp
View File

@ -89,6 +89,9 @@ bool Mode::enter()
// initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands
plane.new_airspeed_cm = -1;
// clear postponed long failsafe if mode change (from GCS) occurs before recall of long failsafe
plane.long_failsafe_pending = false;
#if HAL_QUADPLANE_ENABLED
quadplane.mode_enter();

6
ArduPlane/mode_takeoff.cpp
View File

@ -1,5 +1,6 @@
#include "mode.h"
#include "Plane.h"
#include <GCS_MAVLink/GCS.h>
/*
mode takeoff parameters
@ -150,6 +151,11 @@ void ModeTakeoff::update()
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
//check if in long failsafe, if it is recall long failsafe now to get fs action via events call
if (plane.long_failsafe_pending) {
plane.long_failsafe_pending = false;
plane.failsafe_long_on_event(FAILSAFE_LONG, ModeReason::MODE_TAKEOFF_FAILSAFE);
}
}
}