Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ArduPlane/Attitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,7 @@ bool Plane::stick_mixing_enabled(void)
}
}

if (failsafe.rc_failsafe && g.fs_action_short == FS_ACTION_SHORT_FBWA) {
if (in_fbwa_glide_failsafe()) {
Comment thread
Georacer marked this conversation as resolved.
Resolved

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I traced the code and you are now stopping the mixing on FS_ACTION_LONG_GLIDE, which seems reasonable.

One thing: you usually clear should_glide in every void Plane::rc_failsafe_short_off_event(). Should you be doing the same in void Plane::failsafe_long_off_event(ModeReason reason)?

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Its only done in rc_failsafe_short_off_event because it restores the flight mode. Originally entering FBWA would clear the glide flag, but you may have been flying in FBWA then had a glide failsafe (also in FBWA) then be restored to FBWA, in this case the glide is not cleared because the mode entry code it not run (since were already in the mode).

For the long failsafe there is no expectation of restoring flight mode so I think it would be unexpected to have throttle suddenly come to life.

I guess the way to really make this clear would be to have a glide failsafe as its own mode.....

// don't do stick mixing in FBWA glide mode
return false;
}
Expand Down
1 change: 1 addition & 0 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -1078,6 +1078,7 @@ class Plane : public AP_Vehicle {
void failsafe_long_off_event(ModeReason reason);
void handle_battery_failsafe(const char* type_str, const int8_t action);
bool failsafe_in_landing_sequence() const; // returns true if the vehicle is in landing sequence. Intended only for use in failsafe code.
bool in_fbwa_glide_failsafe() const; // return true if currently in a fbwa glide failsafe

#if AP_FENCE_ENABLED
// fence.cpp
Expand Down
47 changes: 29 additions & 18 deletions ArduPlane/events.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,12 +33,11 @@ void Plane::rc_failsafe_short_on_event()
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CRUISE:
case Mode::Number::TRAINING:
if(plane.emergency_landing) {
IGNORE_RETURN(set_mode(mode_fbwa, ModeReason::RADIO_FAILSAFE)); // emergency landing switch overrides normal action to allow out of range landing
break;
}
if(g.fs_action_short == FS_ACTION_SHORT_FBWA) {
IGNORE_RETURN(set_mode(mode_fbwa, ModeReason::RADIO_FAILSAFE));
// emergency landing switch overrides normal action to allow out of range landing
if (plane.emergency_landing || (g.fs_action_short == FS_ACTION_SHORT_FBWA)) {
if (set_mode(mode_fbwa, ModeReason::RADIO_FAILSAFE)) {
mode_fbwa.set_glide_failsafe(true);
}
} else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
IGNORE_RETURN(set_mode(mode_fbwb, ModeReason::RADIO_FAILSAFE));
} else {
Expand Down Expand Up @@ -82,7 +81,9 @@ void Plane::rc_failsafe_short_on_event()
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();
if (g.fs_action_short == FS_ACTION_SHORT_FBWA) {
IGNORE_RETURN(set_mode(mode_fbwa, ModeReason::RADIO_FAILSAFE));
if (set_mode(mode_fbwa, ModeReason::RADIO_FAILSAFE)) {
mode_fbwa.set_glide_failsafe(true);
}
} else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
IGNORE_RETURN(set_mode(mode_fbwb, ModeReason::RADIO_FAILSAFE));
} else {
Expand Down Expand Up @@ -140,16 +141,16 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
break;
}

if(plane.emergency_landing) {
IGNORE_RETURN(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) {
// emergency landing switch overrides normal action to allow out of range landing
if (plane.emergency_landing || (g.fs_action_long == FS_ACTION_LONG_GLIDE)) {
if (set_mode(mode_fbwa, reason)) {
mode_fbwa.set_glide_failsafe(true);
}

} else 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) {
IGNORE_RETURN(set_mode(mode_fbwa, reason));
} else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
IGNORE_RETURN(set_mode(mode_auto, reason));
#if MODE_AUTOLAND_ENABLED
Expand Down Expand Up @@ -204,7 +205,10 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
parachute_release();
#endif
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
IGNORE_RETURN(set_mode(mode_fbwa, reason));
if (set_mode(mode_fbwa, reason)) {
mode_fbwa.set_glide_failsafe(true);
}

} else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
IGNORE_RETURN(set_mode(mode_auto, reason));
#if MODE_AUTOLAND_ENABLED
Expand Down Expand Up @@ -247,9 +251,10 @@ void Plane::rc_failsafe_short_off_event()
failsafe.state = FAILSAFE_NONE;
// restore entry mode if desired but check that our current mode is still due to failsafe
if (control_mode_reason == ModeReason::RADIO_FAILSAFE) {
if (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 (set_mode_by_number(failsafe.saved_mode_number, ModeReason::RADIO_FAILSAFE_RECOVERY)) {
mode_fbwa.set_glide_failsafe(false);
gcs().send_text(MAV_SEVERITY_INFO,"Flight mode %s restored",control_mode->name());
}
}
}

Expand Down Expand Up @@ -362,3 +367,9 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
break;
}
}

// return true if currently in a fbwa glide failsafe
bool Plane::in_fbwa_glide_failsafe() const
{
return (control_mode == &mode_fbwa) && mode_fbwa.glide_failsafe_active();
}
24 changes: 24 additions & 0 deletions ArduPlane/mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -634,6 +634,30 @@ class ModeFBWA : public Mode
bool allows_autoland_direction_capture() const override { return true; }
#endif

// true if throttle min/max limits should be applied
bool use_throttle_limits() const override;

// Set glide failsafe flag
void set_glide_failsafe(bool b) {
should_glide = b;
}

// Return true if FBWA is in glide failsafe
bool glide_failsafe_active() const {
return should_glide;
}

protected:
// Prepare for mode entry
bool _enter() override;

// Cleanup on mode exit
void _exit() override;

private:
// True if FBWA should cut throttle and hold 0 roll and pitch due to glide failsafe
bool should_glide;

};

class ModeFBWB : public Mode
Expand Down
37 changes: 35 additions & 2 deletions ArduPlane/mode_fbwa.cpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,22 @@
#include "mode.h"
#include "Plane.h"

// Prepare for mode entry
bool ModeFBWA::_enter()
{
// Clear glide failsafe flag
should_glide = false;

return true;
}

// Cleanup on mode exit
void ModeFBWA::_exit()
{
// Clear glide failsafe flag
should_glide = false;
}

void ModeFBWA::update()
{
// set nav_roll and nav_pitch using sticks
Expand All @@ -17,11 +33,10 @@ void ModeFBWA::update()
if (plane.fly_inverted()) {
plane.nav_pitch_cd = -plane.nav_pitch_cd;
}
if (plane.failsafe.rc_failsafe && plane.g.fs_action_short == FS_ACTION_SHORT_FBWA) {
if (should_glide) {
// FBWA failsafe glide
plane.nav_roll_cd = 0;
plane.nav_pitch_cd = 0;
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::MIN);
Comment thread
Georacer marked this conversation as resolved.
Resolved
}
RC_Channel *chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FBWA_TAILDRAGGER);
if (chan != nullptr) {
Expand All @@ -41,5 +56,23 @@ void ModeFBWA::run()
// Run base class function and then output throttle
Mode::run();

// Always zero throttle in glide failsafe
if (should_glide) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
return;
}

output_pilot_throttle();
}

// true if throttle min/max limits should be applied
bool ModeFBWA::use_throttle_limits() const
Comment thread
Georacer marked this conversation as resolved.
Resolved
{
// Never apply throttle limits in glide failsafe
if (should_glide) {
return false;
}

// Otherwise use standard behaviour
return Mode::use_throttle_limits();
}
49 changes: 49 additions & 0 deletions Tools/autotest/arduplane.py
Original file line number Diff line number Diff line change
Expand Up @@ -1590,6 +1590,53 @@ def GCSFailsafe(self):
self.reboot_sitl()
self.end_subtest("Completed Parachute Failsafe test")

def FBWAGlideThrottle(self):
'''Ensure throttle is forced to zero in the FBWA glide failsafe'''
self.set_parameters({
"FS_SHORT_ACTN": 2, # short failsafe action: FBWA glide
"FS_LONG_ACTN": 2, # long failsafe action: FBWA glide, so we stay in the glide
"THR_MIN": 20, # non-zero so a zeroed throttle is distinct from throttle min
})
self.takeoff(50, mode='TAKEOFF') # takes off in FBWA demanding throttle
self.progress("Failing receiver to trigger FBWA glide")
self.set_parameter("SIM_RC_FAIL", 1) # no pulses
self.wait_mode("FBWA")
self.drain_mav() # make sure we have the latest throttle output
# throttle must be forced to zero (SERVO3_MIN), not held at THR_MIN
self.assert_servo_channel_value(3, self.get_parameter("SERVO3_MIN"))
self.set_parameter("SIM_RC_FAIL", 0)
self.fly_home_land_and_disarm()

def FBWAGlideThrottleReverse(self):
'''Ensure reverse thrust goes to zero, not full reverse, in the FBWA glide failsafe'''
# the reverse thrust sim model interprets throttle as an angle
# output: 1000 = full reverse, 1500 = zero thrust, 2000 = full forward
self.customise_SITL_commandline([], model="plane-revthrust", wipe=True)
self.set_parameters({
"THR_MIN": -100, # negative THR_MIN enables reverse thrust
"SERVO3_MIN": 1000,
"SERVO3_TRIM": 1500, # zero-thrust point for a reversible ESC
"SERVO3_MAX": 2000,
"FS_SHORT_ACTN": 2, # short failsafe action: FBWA glide
"FS_LONG_ACTN": 2, # long failsafe action: FBWA glide, so we stay in the glide
})
# take off in TAKEOFF mode: auto throttle avoids the reverse-thrust
# stick mapping and never commands reverse during the climb
self.takeoff(50, mode='TAKEOFF')
self.progress("Failing receiver to trigger FBWA glide")
self.set_parameter("SIM_RC_FAIL", 1) # no pulses
self.wait_mode("FBWA")
self.drain_mav() # make sure we have the latest throttle output
# zero thrust is SERVO3_TRIM (1500); the old code drove full reverse (SERVO3_MIN, 1000)
self.assert_servo_channel_value(
3,
self.get_parameter("SERVO3_TRIM"),
comparator=lambda got,
want: abs(got - want) <= 10
)
self.set_parameter("SIM_RC_FAIL", 0)
self.fly_home_land_and_disarm()

def TestGripperMission(self):
'''Test Gripper mission items'''
self.set_parameter("RTL_AUTOLAND", 1)
Expand Down Expand Up @@ -10839,6 +10886,8 @@ def tests1b(self):
self.AirspeedScripting,
self.MissionJumpTags,
Test(self.GCSFailsafe, speedup=8),
self.FBWAGlideThrottle,
self.FBWAGlideThrottleReverse,
self.RudderArmedTakeoffRequiresNeutralThrottle,
self.MAV_CMD_DO_AUTOTUNE_ENABLE,
self.MAV_CMD_NAV_ALTITUDE_WAIT,
Expand Down
Loading