From 78dffae5a19014109f3830bdd302ffdcde0a2aa1 Mon Sep 17 00:00:00 2001
From: Cliff Geerdes <cliffsjunk@att.net>
Date: Mon, 7 Mar 2016 21:57:29 -0500
Subject: [PATCH] LP-76 disable stabHandler for AutoTune mode

---
 flight/modules/AutoTune/autotune.c               | 8 ++++++--
 flight/modules/ManualControl/stabilizedhandler.c | 9 +++++++++
 2 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c
index 548c2f2e0..2e2e1c402 100644
--- a/flight/modules/AutoTune/autotune.c
+++ b/flight/modules/AutoTune/autotune.c
@@ -798,7 +798,8 @@ static void UpdateSystemIdent(const float *X, const float *noise,
 
 static void UpdateStabilizationDesired(bool doingIdent) {
     StabilizationDesiredData stabDesired;
-    StabilizationDesiredGet(&stabDesired);
+    // unneeded since we are setting everything in this uavo
+    //StabilizationDesiredGet(&stabDesired);
 
     ManualControlCommandData manual_control_command;
     ManualControlCommandGet(&manual_control_command);
@@ -806,7 +807,8 @@ static void UpdateStabilizationDesired(bool doingIdent) {
     stabDesired.Roll = manual_control_command.Roll * rollMax;
     stabDesired.Pitch = manual_control_command.Pitch * pitchMax;
     stabDesired.Yaw = manual_control_command.Yaw * manualRate.Yaw;
-    stabDesired.Thrust = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle;
+    //stabDesired.Thrust = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle;
+    stabDesired.Thrust = manual_control_command.Thrust;
 
     if (doingIdent) {
         stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
@@ -829,6 +831,7 @@ static uint8_t CheckSettings()
 {
     uint8_t retVal = 0;
 
+#if 1
     // Check the axis gains
     // Extreme values: Your roll or pitch gain was lower than expected. This will result in large PID values.
     if (systemIdentData.Beta.Roll < 6) {
@@ -847,6 +850,7 @@ static uint8_t CheckSettings()
     else if (expf(systemIdentData.Tau) < 0.008f) {
         retVal |= 8;
     }
+#endif
 
     return retVal;
 }
diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c
index 4ec0c5cc2..f7e4132f3 100644
--- a/flight/modules/ManualControl/stabilizedhandler.c
+++ b/flight/modules/ManualControl/stabilizedhandler.c
@@ -49,6 +49,7 @@ static float applyExpo(float value, float expo);
 static uint8_t currentFpvTiltAngle = 0;
 static float cosAngle = 0.0f;
 static float sinAngle = 0.0f;
+#if 0
 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 static FlightModeSettingsStabilization1SettingsData autotuneSettings = {
     .Roll   = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT,
@@ -65,6 +66,7 @@ static FlightModeSettingsStabilization1SettingsData attitudeSettings = {
 };
 #endif
 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
+#endif
 
 static float applyExpo(float value, float expo)
 {
@@ -100,6 +102,7 @@ void stabilizedHandler(__attribute__((unused)) bool newinit)
         StabilizationDesiredInitialize();
         StabilizationBankInitialize();
     }
+
     ManualControlCommandData cmd;
     ManualControlCommandGet(&cmd);
 
@@ -157,12 +160,18 @@ void stabilizedHandler(__attribute__((unused)) bool newinit)
         break;
 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
     case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
+#if 0
 //        if (SystemIdentHandle()) {
             stab_settings = (uint8_t *)&autotuneSettings;
 //        } else {
 //            stab_settings = (uint8_t *)&attitudeSettings;
 //        }
         break;
+#else
+        // let autotune.c handle it
+        // because it must switch to Attitude after <user configurable> seconds
+        return;
+#endif
 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
     default:
         // Major error, this should not occur because only enter this block when one of these is true