LP-76 auto module enable, more sanity checks, SmoothQuick tweaks

flight/libraries/sanitycheck.c

@@ -167,6 +167,9 @@ int32_t configuration_check()
 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
             ADDSEVERITY(!gps_assisted);
+            // it would be fun to try autotune on a fixed wing
+            // but that should only be attempted by devs at first
+            ADDSEVERITY(multirotor);
             break;
 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
         default:

flight/modules/AutoTune/autotune.c

@@ -39,6 +39,7 @@
 #include <flightstatus.h>
 #include <manualcontrolcommand.h>
 #include <manualcontrolsettings.h>
+#include <flightmodesettings.h>
 #include <gyrostate.h>
 #include <actuatordesired.h>
 #include <stabilizationdesired.h>
@@ -145,7 +146,7 @@ static void UpdateSystemIdentState(const float *X, const float *noise, float dT_
 static void UpdateStabilizationDesired(bool doingIdent);
 static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode);
 static void InitSystemIdent(bool loadDefaults);
-static void InitSmoothQuick();
+static void InitSmoothQuick(bool loadToggle);
 
 
 /**
@@ -161,11 +162,26 @@ int32_t AutoTuneInitialize(void)
     HwSettingsOptionalModulesData optionalModules;
     HwSettingsOptionalModulesGet(&optionalModules);
     if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+        // even though the AutoTune module is automatically enabled
+        // (below, when the flight mode switch is configured to use autotune)
+        // there are use cases where the user may even want it enabled without being on the FMS
+        // that allows PIDs to be adjusted in flight
         moduleEnabled = true;
     } else {
+        // if the user did not enable the autotune module
+        // do it for them if they have autotune on their flight mode switch
+        FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
         moduleEnabled = false;
+        FlightModeSettingsInitialize();
+        FlightModeSettingsFlightModePositionGet(fms);
+        for (uint8_t i = 0; i < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM; ++i) {
+            if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) {
+                moduleEnabled = true;
+                break;
             }
-#endif
+        }
+    }
+#endif /* ifdef MODULE_AutoTune_BUILTIN */
 
     if (moduleEnabled) {
         SystemIdentSettingsInitialize();
@@ -226,7 +242,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
     // based on what is in SystemIdent
     // so that the user can use the PID smooth->quick slider in following flights
     InitSystemIdent(false);
-    InitSmoothQuick();
+    InitSmoothQuick(true);
 
     while (1) {
         uint32_t diffTime;
@@ -287,14 +303,27 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
             savePidNeeded = true;
         }
 
+        //////////////////////////////////////////////////////////////////////////////////////
+        // if configured to use a slider for smooth-quick and the autotune module is running
+        // (note that the module can be automatically or manually enabled)
+        // then the smooth-quick slider is always active (when not actually in autotune mode)
+        //
+        // when the slider is active it will immediately change the PIDs
+        // and it will schedule the PIDs to be written to permanent storage
+        //
+        // if the FC is disarmed, the perm write will happen on next loop
+        // but if the FC is armed, the perm write will only occur when the FC goes disarmed
+        //////////////////////////////////////////////////////////////////////////////////////
+
+        // we don't want it saving to permanent storage many times
+        // while the user is moving the knob once, so wait till the knob stops moving
+        static uint8_t savePidDelay;
         // any time we are not in AutoTune mode:
         // - the user may be using the accessory0-3 knob/slider to request PID changes
         // - the state machine needs to be reset
         // - the local version of Attitude mode gets skipped
         if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
-            // we don't want it saving to permanent storage many times
+            static bool savePidActive = false;
-            // while the user is moving the knob once, so wait till the knob stops moving
-            static uint8_t savePidDelay;
             // if accessory0-3 is configured as a PID changing slider/knob over the smooth to quick range
             // and FC is not currently running autotune
             // and accessory0-3 changed by at least 1/160 of full range (2)
@@ -313,12 +342,23 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
                     // this copies the PIDs to memory and makes them active
                     // but does not write them to permanent storage
                     ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f);
-                    // this schedules the first possible write of the PIDs to occur later (a fraction of a second)
+                    // don't save PID to perm storage the first time
+                    // that usually means at power up
+                    //
+                    // that keeps it from writing the same value at each boot
+                    // but means that it won't be permanent if they move the slider before FC power on
+                    // (note that the PIDs will be changed, just not saved permanently)
+                    if (savePidActive) {
+                        // this schedules the first possible write of the PIDs to occur a fraction of a second or so from now
+                        // and moves the scheduled time if it is already scheduled
                         savePidDelay = SMOOTH_QUICK_FLUSH_TICKS;
+                    } else {
+                        savePidActive = true;
+                    }
                 } else if (savePidDelay && --savePidDelay == 0) {
                     // this flags that the PIDs can be written to permanent storage right now
                     // but they will only be written when the FC is disarmed
-                    // so this means immediate or wait till FC is disarmed
+                    // so this means immediate (after NOT_AT_MODE_DELAY_MS) or wait till FC is disarmed
                     savePidNeeded = true;
                 }
             } else {
@@ -327,6 +367,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
             state = AT_INIT;
             vTaskDelay(NOT_AT_MODE_DELAY_MS / portTICK_RATE_MS);
             continue;
+        } else {
+            savePidDelay = 0;
         }
 
         switch (state) {
@@ -355,10 +397,16 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
                 // load SystemIdentSettings so that they can change it
                 // and do smooth-quick on changed values
                 InitSystemIdent(false);
-                // no InitSmoothQuick() here or the toggle switch gets reset even in a "quick toggle" that should increment it
+                InitSmoothQuick(false);
+                // wait for FC to arm in case they are doing this without a flight mode switch
+                // that causes the 2+ second delay that follows to happen after arming
+                // which gives them a chance to take off before the shakes start
+                // the FC must be armed and if we check here it also allows switchless setup to use autotune
+                if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
                     state = AT_INIT_DELAY2;
                     lastUpdateTime = xTaskGetTickCount();
                 }
+            }
             break;
 
         case AT_INIT_DELAY2:
@@ -373,8 +421,6 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
                 doingIdent = true;
                 // after an additional .5 seconds start capturing data
                 if (diffTime > INIT_TIME_DELAY2_MS) {
-                    // Only start when armed and flying
-                    if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
                     // Reset save status
                     // save SI data even if partial or bad, aids in diagnostics
                     saveSiNeeded  = true;
@@ -382,14 +428,13 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
                     // and the complete data has been sanity checked
                     savePidNeeded = false;
                     InitSystemIdent(true);
-                        InitSmoothQuick();
+                    InitSmoothQuick(true);
                     AfInit(gX, gP);
                     UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f);
                     measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000;
                     state = AT_START;
                 }
             }
-            }
             break;
 
         case AT_START:
@@ -587,13 +632,13 @@ static void InitSystemIdent(bool loadDefaults)
         // so that if they are changed there (mainly for future code changes), they will be changed here too
         SystemIdentStateSetDefaults(SystemIdentStateHandle(), 0);
         SystemIdentStateGet(&systemIdentState);
-        // Tau Beta and the Complete flag get default values
+        // Tau, Beta, and the Complete flag get default values
         // in preparation for running AutoTune
         systemIdentSettings.Tau = systemIdentState.Tau;
         memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
         systemIdentSettings.Complete = systemIdentState.Complete;
     } else {
-        // Tau Beta and the Complete flag get stored values
+        // Tau, Beta, and the Complete flag get stored values
         // so the user can fly another battery to select and test PIDs with the slider/knob
         systemIdentState.Tau = systemIdentSettings.Tau;
         memcpy(&systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData));
@@ -602,29 +647,42 @@ static void InitSystemIdent(bool loadDefaults)
 }
 
 
-static void InitSmoothQuick()
+static void InitSmoothQuick(bool loadToggle)
 {
     uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource;
 
-    // default to disable PID changing with flight mode switch and accessory0-3
-    accessoryToUse = -1;
-    flightModeSwitchTogglePosition = -1;
-    systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED;
     switch (SmoothQuickSource) {
     case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0
     case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1
     case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2
     case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3
+        // disable PID changing with flight mode switch
+        // ignore loadToggle if user is also switching to use knob as source
+        flightModeSwitchTogglePosition = -1;
         accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE;
         systemIdentSettings.SmoothQuickSource = SmoothQuickSource;
         break;
     case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points
     case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points
     case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points
+        // disable PID changing with accessory0-3
+        accessoryToUse = -1;
+        // don't allow init of current toggle position in the middle of 3x fms toggle
+        if (loadToggle) {
             // first test PID is in the middle of the smooth -> quick range
             flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
+        }
         systemIdentSettings.SmoothQuickSource = SmoothQuickSource;
         break;
+    case SMOOTH_QUICK_DISABLED:
+    default:
+        // disable PID changing with flight mode switch
+        // ignore loadToggle since user is disabling toggle
+        flightModeSwitchTogglePosition = -1;
+        // disable PID changing with accessory0-3
+        accessoryToUse = -1;
+        systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED;
+        break;
     }
 }
 
@@ -642,7 +700,7 @@ static void UpdateSystemIdentState(const float *X, const float *noise,
     systemIdentState.Bias.Pitch = X[11];
     systemIdentState.Bias.Yaw   = X[12];
     systemIdentState.Tau = X[9];
-    // 'settings' beta and tau have same value as state versions
+    // 'settings' beta and tau have same value as 'state' versions
     // the state version produces a GCS log
     // the settings version is remembered after power off/on
     systemIdentSettings.Tau = systemIdentState.Tau;

shared/uavobjectdefinition/systemidentsettings.xml

@@ -36,14 +36,18 @@
         <!-- <field name="YawPIDRatioFunction" units="" type="enum" elements="1" options="Disable,Limit" defaultvalue="Limit"/> -->
         <field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="2"/>
         <field name="TuningDuration" units="sec" type="uint8" elements="1" defaultvalue="60"/>
-        <!-- smooth vs. quick PID selector: -->
+        <!-- SmoothQuickSource: the smooth vs. quick PID selector -->
         <!-- 0 = disabled -->
         <!-- 10 thru 13 correspond to accessory0 -> accessory3 transmitter knobs -->
-        <!-- 23, 25, and 27 are discrete 3, 5, and 7 stop rount robin selectors incremented by quickly toggling the fms 3 times -->
+        <!-- an accessory knob works as expected, with full left being smoothest and full right being quickest -->
-        <!-- 23 (3 stops) means stops at 0%, 100%, -100% (repeat as you toggle) -->
+        <!-- 23, 25, and 27 are discrete 3, 5, and 7 position rount robin selectors -->
-        <!-- 25 (5 stops) means stops at 0%, 50%, 100%, -100%, -50% (repeat as you toggle) -->
+        <!-- incremented by quickly double toggling the fms 3 times (starting outside autotune mode -->
-        <!-- 27 (7 stops) means stops at 0%, 33%, 67%, 100%, -100%, -67%, -33% (repeat as you toggle) -->
+        <!-- with each double toggle going into and back out of autotune) to go to the next position -->
-        <!-- 25 is special in that the 3 middle values (-50, 0, 50) are exactly those that are recommended for smooth, normal, and quick responses -->
+        <!-- think of the positions on a scale from 0 to 100, with smoothest being 0 and quickest being 100, and you start out at 50 -->
+        <!-- 23 (3 stops) means stops at 50, then 100, then 0 then back to 50 -->
+        <!-- 25 (5 stops) means stops at 50, 75, 100, 0, 25 (repeat as you toggle) -->
+        <!-- 27 (7 stops) means stops at 50, 67, 83, 100, 0, 17, 33 (repeat as you toggle) -->
+        <!-- 25 is special in that the 3 middle values (25, 50, 75) are exactly those that are recommended for smooth, normal, and quick responses -->
         <field name="SmoothQuickSource" units="" type="uint8" elements="1" defaultvalue="25"/>
         <field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
         <field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>