Merged in f5soh/librepilot/LP-401_LP-411_Autotune (pull request #357)

LP-401 and LP-411 Autotune

flight/modules/AutoTune/autotune.c

@@ -146,6 +146,7 @@ static bool moduleEnabled;
 
 // Private functions
 static void AtNewGyroData(UAVObjEvent *ev);
+static bool AutoTuneFoundInFMS();
 static void AutoTuneTask(void *parameters);
 static void AfInit(float X[AF_NUMX], float P[AF_NUMP]);
 static void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in);
@@ -153,35 +154,28 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode);
 static uint8_t CheckSettings();
 static uint8_t CheckSettingsRaw();
 static void ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise);
+static void FlightModeSettingsUpdatedCb(UAVObjEvent *ev);
 static void InitSystemIdent(bool loadDefaults);
 static void ProportionPidsSmoothToQuick();
 static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle);
 static void UpdateStabilizationDesired(bool doingIdent);
 
 
-static void flightModeSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
-{
-    FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
-
-    FlightModeSettingsFlightModePositionGet(fms);
-    for (uint8_t i = 0; i < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM; ++i) {
-        if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) {
-            ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
-            break;
-        }
-    }
-}
-
 /**
  * Initialise the module, called on startup
  * \returns 0 on success or -1 if initialisation failed
  */
 int32_t AutoTuneInitialize(void)
 {
-    // Create a queue, connect to manual control command and flightstatus
+    // do this here since module can become disabled for several reasons
-#ifdef MODULE_AutoTune_BUILTIN
+    // even for MODULE_AutoTune_BUILTIN
+    FlightModeSettingsInitialize();
+
+#if defined(MODULE_AutoTune_BUILTIN)
     moduleEnabled = true;
 #else
+    // HwSettings is only used right here, so init here
+    HwSettingsInitialize();
     HwSettingsOptionalModulesData optionalModules;
     HwSettingsOptionalModulesGet(&optionalModules);
     if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) {
@@ -191,32 +185,33 @@ int32_t AutoTuneInitialize(void)
         // that allows PIDs to be adjusted in flight
         moduleEnabled = true;
     } else {
-        // if the user did not enable the autotune module
+        // if the user did not manually enable the autotune module
         // do it for them if they have autotune on their flight mode switch
-        FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
+        moduleEnabled = AutoTuneFoundInFMS();
-        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 /* ifdef MODULE_AutoTune_BUILTIN */
+#endif /* defined(MODULE_AutoTune_BUILTIN) */
 
     if (moduleEnabled) {
+        AccessoryDesiredInitialize();
+        ActuatorDesiredInitialize();
+        FlightStatusInitialize();
+        GyroStateInitialize();
+        ManualControlCommandInitialize();
+        StabilizationBankInitialize();
+        StabilizationSettingsBank1Initialize();
+        StabilizationSettingsBank2Initialize();
+        StabilizationSettingsBank3Initialize();
         SystemIdentSettingsInitialize();
         SystemIdentStateInitialize();
+
         atQueue = xQueueCreate(AT_QUEUE_NUMELEM, sizeof(struct at_queued_data));
         if (!atQueue) {
             moduleEnabled = false;
         }
     }
-
     if (!moduleEnabled) {
-        FlightModeSettingsConnectCallback(flightModeSettingsUpdatedCb);
+        // only need to watch for enabling AutoTune in FMS if AutoTune module is _not_ running
+        FlightModeSettingsConnectCallback(FlightModeSettingsUpdatedCb);
     }
 
     return 0;
@@ -577,6 +572,26 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
 }
 
 
+// FlightModeSettings callback
+// determine if autotune is enabled in the flight mode switch
+static bool AutoTuneFoundInFMS()
+{
+    bool found = false;
+    FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
+    uint8_t num_flightMode;
+
+    FlightModeSettingsFlightModePositionGet(fms);
+    ManualControlSettingsFlightModeNumberGet(&num_flightMode);
+    for (uint8_t i = 0; i < num_flightMode; ++i) {
+        if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) {
+            found = true;
+            break;
+        }
+    }
+    return found;
+}
+
+
 // gyro sensor callback
 // get gyro data and actuatordesired into a packet
 // and put it in the queue for later processing
@@ -624,6 +639,16 @@ static void AtNewGyroData(UAVObjEvent *ev)
 }
 
 
+// this callback is only enabled if the AutoTune module is not running
+// if it sees that AutoTune was added to the FMS it issues BOOT and ? alarms
+static void FlightModeSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    if (AutoTuneFoundInFMS()) {
+        ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
+    }
+}
+
+
 // check for the user quickly toggling the flight mode switch
 // into and out of AutoTune, 3 times
 // that is a signal that the user wants to try the next PID settings
@@ -925,11 +950,29 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
     // inner loop as a single order lpf. Set the outer loop to be
     // critically damped;
     const float zeta_o = 1.3f;
-    const float kp_o   = 1.0f / 4.0f / (zeta_o * zeta_o) / (1.0f / wn);
+    float kp_o = 1.0f / 4.0f / (zeta_o * zeta_o) / (1.0f / wn);
-    const float ki_o   = 0.75f * kp_o / (2.0f * M_PI_F * tau * 10.0f);
+
+    // Except, if this is very high, we may be slew rate limited and pick
+    // up oscillation that way.  Fix it with very soft clamping.
+    // (dRonin) MaximumRate defaults to 350, 6.5 corresponds to where we begin
+    // clamping rate ourselves.  ESCs, etc, it depends upon gains
+    // and any pre-emphasis they do.   Still give ourselves partial credit
+    // for inner loop bandwidth.
+
+    // In dRonin, MaximumRate defaults to 350 and they begin clamping at outer Kp 6.5
+    // To avoid oscillation, find the minimum rate, calculate the ratio of that to 350,
+    // and scale (linearly) with that.  Skip yaw.  There is no outer yaw in the GUI.
+    const uint16_t minRate = MIN(stabSettingsBank.MaximumRate.Roll, stabSettingsBank.MaximumRate.Pitch);
+    const float kp_o_clamp = systemIdentSettings.OuterLoopKpSoftClamp * ((float)minRate / 350.0f);
+    if (kp_o > kp_o_clamp) {
+        kp_o = kp_o_clamp - sqrtf(kp_o_clamp) + sqrtf(kp_o);
+    }
+    kp_o *= 0.95f; // Pick up some margin.
+    // Add a zero at 1/15th the innermost bandwidth.
+    const float ki_o = 0.75f * kp_o / (2.0f * M_PI_F * tau * 15.0f);
 
     float kpMax = 0.0f;
-    float betaMinLn    = 1000.0f;
+    float betaMinLn  = 1000.0f;
     StabilizationBankRollRatePIDData volatile *rollPitchPid = NULL; // satisfy compiler warning only
 
     for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) {

shared/uavobjectdefinition/systemidentsettings.xml

@@ -54,7 +54,8 @@
         <!-- Cliff sluggish 500 quad thinks that yaw P should be about 5.8 times roll/pitch P, but can easily (and better) live with 2.6 -->
         <field name="YawToRollPitchPIDRatioMin" units="" type="float" elements="1" defaultvalue="1.0" description="Setting: Yaw PID will be at least this times Pitch PID (if enabled)"/>
         <field name="YawToRollPitchPIDRatioMax" units="" type="float" elements="1" defaultvalue="2.5" description="Setting: Yaw PID will be at most this times Pitch PID (if enabled)"/>
-        <field name="DerivativeFactor" units="" type="float" elements="1" defaultvalue="1.0" limits="%BE:0:1" description="Setting: Reduce this toward zero if you have D term oscillations, it will reduce PID D terms"/>
+        <field name="DerivativeFactor" units="" type="float" elements="1" defaultvalue="1.0" limits="%BE:0:1" description="Setting: Multiplicative factor. If you have D term oscillations, reduce it toward zero and it will reduce PID D terms."/>
+        <field name="OuterLoopKpSoftClamp" units="" type="float" elements="1" defaultvalue="6.5" limits="%BE:0:100" description="Setting: Change this to change the outer loop Kp and Ki limiting"/>
         <field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="3" limits="%BE:1:3" description="Setting: Which bank the calculated PIDs will be stored in after tuning"/>
         <field name="TuningDuration" units="s" type="uint8" elements="1" defaultvalue="60" limits="%BI:0" description="Setting: Duration of the tuning motions (expert)"/>
         <!-- SmoothQuickSource: the smooth vs. quick PID selector -->