From 28ad66502af5541a2b60b28c49e138933d8e8bd5 Mon Sep 17 00:00:00 2001
From: Vladimir Zidar <mr_w@mindnever.org>
Date: Fri, 8 Sep 2017 09:49:20 +0200
Subject: [PATCH] LP-549 Disable Iterm zeroing at low throttle when ASWA is
 enabled. LP-549 Enable ASWA only after initial throttle threshold has been
 reached to allow Iterm to be zeroed with zero throttle while on ground.

---
 flight/modules/ManualControl/manualcontrol.c      | 3 ++-
 flight/modules/Stabilization/innerloop.c          | 7 ++++++-
 shared/uavobjectdefinition/flightmodesettings.xml | 2 +-
 3 files changed, 9 insertions(+), 3 deletions(-)

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."/>
-
+        
         <field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
         <field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
         <field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>