Skip to content
Open
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
4 changes: 2 additions & 2 deletions ArduPlane/Attitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -112,8 +112,8 @@ bool Plane::stick_mixing_enabled(void)
}
}

if (failsafe.rc_failsafe && g.fs_action_short == FS_ACTION_SHORT_FBWA) {
// don't do stick mixing in FBWA glide mode
// No stick mixing in glide failsafe
if (control_mode == &mode_glide_failsafe) {
return false;
}

Expand Down
2 changes: 2 additions & 0 deletions ArduPlane/GCS_MAVLink_Plane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ uint8_t GCS_MAVLINK_Plane::base_mode() const
#endif
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::CRUISE:
case Mode::Number::GLIDE_FAILSAFE:
_base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case Mode::Number::AUTO:
Expand Down Expand Up @@ -1309,6 +1310,7 @@ uint8_t GCS_MAVLINK_Plane::send_available_mode(uint8_t index) const
#if MODE_AUTOLAND_ENABLED
&plane.mode_autoland,
#endif
&plane.mode_glide_failsafe,
};

const uint8_t fw_mode_count = ARRAY_SIZE(fw_modes);
Expand Down
1 change: 1 addition & 0 deletions ArduPlane/GCS_Plane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CRUISE:
case Mode::Number::GLIDE_FAILSAFE:
rate_controlled = true;
attitude_stabilized = true;
break;
Expand Down
10 changes: 5 additions & 5 deletions ArduPlane/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -438,15 +438,15 @@ const AP_Param::Info Plane::var_info[] = {

// @Param: FS_SHORT_ACTN
// @DisplayName: Short failsafe action
// @Description: The action to take on a short failsafe event. A short failsafe event can be triggered instantly by loss of RC control or by throttle value (see THR_FS_VALUE). If in CIRCLE or RTL mode this parameter is ignored. A short failsafe event in stabilization and manual modes will cause a change to CIRCLE mode if FS_SHORT_ACTN is 0 or 1, a change to FBWA mode with zero throttle if FS_SHORT_ACTN is 2, and a change to FBWB mode if FS_SHORT_ACTN is 4. In all other modes (AUTO, GUIDED and LOITER) a short failsafe event will cause no mode change if FS_SHORT_ACTN is set to 0, will cause a change to CIRCLE mode if set to 1, will change to FBWA mode with zero throttle if set to 2, or will change to FBWB if set to 4. Please see the documentation for FS_LONG_ACTN for the behaviour after FS_LONG_TIMEOUT seconds of failsafe. This parameter only applies to failsafes during fixed wing modes. Quadplane modes will switch to QLAND unless Q_OPTIONS bit 5(QRTL) or 20(RTL) are set.
// @Values: 0:CIRCLE/no change(if already in AUTO|GUIDED|LOITER),1:CIRCLE,2:FBWA at zero throttle,3:Disable,4:FBWB
// @Description: The action to take on a short failsafe event. A short failsafe event can be triggered instantly by loss of RC control or by throttle value (see THR_FS_VALUE). If in CIRCLE or RTL mode this parameter is ignored. A short failsafe event in stabilization and manual modes will cause a change to CIRCLE mode if FS_SHORT_ACTN is 0 or 1, a change to Glide FS mode (zero throttle, wings level) if FS_SHORT_ACTN is 2, and a change to FBWB mode if FS_SHORT_ACTN is 4. In all other modes (AUTO, GUIDED and LOITER) a short failsafe event will cause no mode change if FS_SHORT_ACTN is set to 0, will cause a change to CIRCLE mode if set to 1, will change to Glide FS mode if set to 2, or will change to FBWB if set to 4. Please see the documentation for FS_LONG_ACTN for the behaviour after FS_LONG_TIMEOUT seconds of failsafe. This parameter only applies to failsafes during fixed wing modes. Quadplane modes will switch to QLAND unless Q_OPTIONS bit 5(QRTL) or 20(RTL) are set.
// @Values: 0:CIRCLE/no change(if already in AUTO|GUIDED|LOITER),1:CIRCLE,2:Glide FS,3:Disable,4:FBWB
// @User: Standard
GSCALAR(fs_action_short, "FS_SHORT_ACTN", FS_ACTION_SHORT_BESTGUESS),

// @Param: FS_LONG_ACTN
// @DisplayName: Long failsafe action
// @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event. If the aircraft was in a stabilization or manual mode when failsafe started and a long failsafe occurs then it will change to RTL mode if FS_LONG_ACTN is 0 or 1, and will change to FBWA if FS_LONG_ACTN is set to 2. If the aircraft was in an auto mode (such as AUTO or GUIDED) when the failsafe started then it will continue in the auto mode if FS_LONG_ACTN is set to 0, will change to RTL mode if FS_LONG_ACTN is set to 1 and will change to FBWA mode if FS_LONG_ACTN is set to 2. If FS_LONG_ACTN is set to 3, the parachute will be deployed (make sure the chute is configured and enabled). If FS_LONG_ACTN is set to 4 the aircraft will switch to mode AUTO with the current waypoint if it is not already in mode AUTO, unless it is in the middle of a landing sequence. If FS_LONG_ACTN is set to 5, will switch to AUTOLAND mode if possible, otherwise RTL mode. This parameter only applies to failsafes during fixed wing modes. Quadplane modes will switch to QLAND unless Q_OPTIONS bit 5 (QRTL) or 20(RTL) are set.
// @Values: 0:Continue,1:ReturnToLaunch,2:Glide,3:Deploy Parachute,4:Auto,5:AUTOLAND
// @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event. If the aircraft was in a stabilization or manual mode when failsafe started and a long failsafe occurs then it will change to RTL mode if FS_LONG_ACTN is 0 or 1, and will change to Glide FS mode (zero throttle, wings level) if FS_LONG_ACTN is set to 2. If the aircraft was in an auto mode (such as AUTO or GUIDED) when the failsafe started then it will continue in the auto mode if FS_LONG_ACTN is set to 0, will change to RTL mode if FS_LONG_ACTN is set to 1 and will change to Glide FS mode if FS_LONG_ACTN is set to 2. If FS_LONG_ACTN is set to 3, the parachute will be deployed (make sure the chute is configured and enabled). If FS_LONG_ACTN is set to 4 the aircraft will switch to mode AUTO with the current waypoint if it is not already in mode AUTO, unless it is in the middle of a landing sequence. If FS_LONG_ACTN is set to 5, will switch to AUTOLAND mode if possible, otherwise RTL mode. This parameter only applies to failsafes during fixed wing modes. Quadplane modes will switch to QLAND unless Q_OPTIONS bit 5 (QRTL) or 20(RTL) are set.
// @Values: 0:Continue,1:ReturnToLaunch,2:Glide FS,3:Deploy Parachute,4:Auto,5:AUTOLAND
// @User: Standard
GSCALAR(fs_action_long, "FS_LONG_ACTN", FS_ACTION_LONG_CONTINUE),

Expand Down Expand Up @@ -476,7 +476,7 @@ const AP_Param::Info Plane::var_info[] = {
// @Param: FLTMODE1
// @DisplayName: FlightMode1
// @Description: Flight mode for switch position 1 (910 to 1230 and above 2049)
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO,24:THERMAL,25:Loiter to QLand,26:AUTOLAND
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO,24:THERMAL,25:Loiter to QLand,26:AUTOLAND,27:Glide FS
// @User: Standard
GARRAY(flight_modes, 0, "FLTMODE1", FLIGHT_MODE_1),

Expand Down
2 changes: 2 additions & 0 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -182,6 +182,7 @@ class Plane : public AP_Vehicle {
#if MODE_AUTOLAND_ENABLED
friend class ModeAutoLand;
#endif
friend class ModeGlideFailsafe;
#if AP_EXTERNAL_CONTROL_ENABLED
friend class AP_ExternalControl_Plane;
#endif
Expand Down Expand Up @@ -340,6 +341,7 @@ class Plane : public AP_Vehicle {
#if HAL_SOARING_ENABLED
ModeThermal mode_thermal;
#endif
ModeGlideFailsafe mode_glide_failsafe;

#if AP_QUICKTUNE_ENABLED
AP_Quicktune quicktune;
Expand Down
4 changes: 3 additions & 1 deletion ArduPlane/control_modes.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,9 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
ret = &mode_loiter_qland;
break;
#endif // HAL_QUADPLANE_ENABLED

case Mode::Number::GLIDE_FAILSAFE:
ret = &mode_glide_failsafe;
break;
}
return ret;
}
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/defines.h
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ enum gcs_failsafe {
enum failsafe_action_short {
FS_ACTION_SHORT_BESTGUESS = 0, // CIRCLE/no change(if already in AUTO|GUIDED|LOITER)
FS_ACTION_SHORT_CIRCLE = 1,
FS_ACTION_SHORT_FBWA = 2,
FS_ACTION_SHORT_GLIDE = 2,
FS_ACTION_SHORT_DISABLED = 3,
FS_ACTION_SHORT_FBWB = 4,
};
Expand Down
39 changes: 19 additions & 20 deletions ArduPlane/events.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,12 @@ void Plane::rc_failsafe_short_on_event()
case Mode::Number::AUTOTUNE:
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));
case Mode::Number::TRAINING:
case Mode::Number::GLIDE_FAILSAFE:
// emergency landing switch overrides normal action to allow out of range landing
if (plane.emergency_landing || (g.fs_action_short == FS_ACTION_SHORT_GLIDE)) {
IGNORE_RETURN(set_mode(mode_glide_failsafe, ModeReason::RADIO_FAILSAFE));

} else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
IGNORE_RETURN(set_mode(mode_fbwb, ModeReason::RADIO_FAILSAFE));
} else {
Expand Down Expand Up @@ -81,8 +80,8 @@ void Plane::rc_failsafe_short_on_event()
case Mode::Number::THERMAL:
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 (g.fs_action_short == FS_ACTION_SHORT_GLIDE) {
IGNORE_RETURN(set_mode(mode_glide_failsafe, ModeReason::RADIO_FAILSAFE));
} else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
IGNORE_RETURN(set_mode(mode_fbwb, ModeReason::RADIO_FAILSAFE));
} else {
Expand Down Expand Up @@ -133,23 +132,22 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
case Mode::Number::LOITER:
case Mode::Number::THERMAL:
case Mode::Number::TAKEOFF:
case Mode::Number::GLIDE_FAILSAFE:
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 initial 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) {
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)) {
IGNORE_RETURN(set_mode(mode_glide_failsafe, reason));

} 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 +202,8 @@ 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));
IGNORE_RETURN(set_mode(mode_glide_failsafe, 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 @@ -247,9 +246,9 @@ 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)) {
gcs().send_text(MAV_SEVERITY_INFO,"Flight mode %s restored",control_mode->name());
}
}
}

Expand Down
19 changes: 19 additions & 0 deletions ArduPlane/mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,7 @@ class Mode
#if MODE_AUTOLAND_ENABLED
AUTOLAND = 26,
#endif
GLIDE_FAILSAFE = 27,

// Mode number 30 reserved for "offboard" for external/lua control.
};
Expand Down Expand Up @@ -1087,3 +1088,21 @@ class ModeThermal: public Mode
};

#endif

class ModeGlideFailsafe : public Mode
{
public:

Number mode_number() const override { return Number::GLIDE_FAILSAFE; }
const char *name() const override { return "Glide FS"; }
const char *name4() const override { return "GLFS"; }

// methods that affect movement of the vehicle in this mode
void update() override;

void run() override;

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

};
7 changes: 1 addition & 6 deletions ArduPlane/mode_fbwa.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,7 @@ 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) {
// 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);
}

RC_Channel *chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FBWA_TAILDRAGGER);
if (chan != nullptr) {
// check for the user enabling FBWA taildrag takeoff mode
Expand Down
24 changes: 24 additions & 0 deletions ArduPlane/mode_glide_failsafe.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
#include "mode.h"
#include "Plane.h"

void ModeGlideFailsafe::update()
{
plane.nav_roll_cd = 0;
plane.nav_pitch_cd = 0;
}

void ModeGlideFailsafe::run()
{
// Run base class function
Mode::run();

// Always zero throttle
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
}

// true if throttle min/max limits should be applied
bool ModeGlideFailsafe::use_throttle_limits() const
{
// Never apply throttle limits in glide failsafe
return false;
}
3 changes: 2 additions & 1 deletion ArduPlane/system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -230,10 +230,11 @@ bool Plane::gcs_mode_enabled(const Mode::Number mode_num) const
#else
0xFF,
#endif // MODE_AUTOLAND_ENABLED
(uint8_t)Mode::Number::GLIDE_FAILSAFE,
};

// Place holders should mean that array is always the same size
static_assert(ARRAY_SIZE(mode_list) == 22, "mode_list placeholders incorrect");
static_assert(ARRAY_SIZE(mode_list) == 23, "mode_list placeholders incorrect");

return !block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list));
}
Expand Down
Loading
Loading