diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c
index 8e167d6f4..fa9cf6978 100644
--- a/flight/modules/ManualControl/manualcontrol.c
+++ b/flight/modules/ManualControl/manualcontrol.c
@@ -66,6 +66,7 @@
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.04f
#define ALWAYSTABILIZEACCESSORY_THRESHOLD 0.05f
+#define ALWAYSTABILIZETHROTTLE_THRESHOLD 0.2f
// defined handlers
@@ -499,7 +500,7 @@ static void manualControlTask(void)
if (alwaysStabilizedSwitch) {
if (acc.AccessoryVal <= -ALWAYSTABILIZEACCESSORY_THRESHOLD) {
newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_FALSE;
- } else if (acc.AccessoryVal >= ALWAYSTABILIZEACCESSORY_THRESHOLD) {
+ } else if ((acc.AccessoryVal >= ALWAYSTABILIZEACCESSORY_THRESHOLD) && (cmd.Thrust >= ALWAYSTABILIZETHROTTLE_THRESHOLD)) { // && Thrust (or Throttle?) above threshold
newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE;
}
} else {
diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c
index a50e6e6ec..58228728c 100644
--- a/flight/modules/Stabilization/innerloop.c
+++ b/flight/modules/Stabilization/innerloop.c
@@ -449,10 +449,15 @@ static void stabilizationInnerloopTask()
{
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
+ FlightStatusAlwaysStabilizeWhenArmedOptions alwaysStabilizeWhenArmed;
+ FlightStatusAlwaysStabilizeWhenArmedGet(&alwaysStabilizeWhenArmed);
+
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);
if (armed != FLIGHTSTATUS_ARMED_ARMED ||
- ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
+ ((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) &&
+ (throttleDesired < 0) &&
+ (alwaysStabilizeWhenArmed != FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE))) {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml
index 4ecbfea4a..42d50e142 100644
--- a/shared/uavobjectdefinition/flightmodesettings.xml
+++ b/shared/uavobjectdefinition/flightmodesettings.xml
@@ -82,7 +82,7 @@
options="Disabled,Accessory 0,Accessory 1,Accessory 2,Accessory 3"
defaultvalue="Disabled"
description="For Multirotors. Always stabilize no matter the throttle setting when vehicle is armed. Does not work when vehicle is set to Always Armed."/>
-
+