Merge remote-tracking branch 'origin/autotune'

Makefile

@@ -112,6 +112,7 @@ COMMON_SRC	 = build_config.c \
 		   drivers/serial_common.c \
 		   drivers/sound_beeper.c \
 		   drivers/system_common.c \
+		   flight_autotune.c \
 		   flight_common.c \
 		   flight_imu.c \
 		   flight_mixer.c \

docs/Autotune.md

@@ -0,0 +1,53 @@
+# Autotune
+
+Autotune helps to automatically tune your multirotor.
+
+## Configuration.
+
+Autotune only works in HORIZON or ANGLE mode, before using auto-tune it's best you setup so there is as little drift as possible.
+Autotuning is best on a full battery in good flying conditions, i.e. no or minimal wind.
+
+Configure a two position switch on your transmitter to activate the AUTOTUNE and HORIZON or ANGLE modes using the auxilary configuration.
+You may find a momentary switch more suitable than a toggle switch.
+
+
+## Using autotuning
+
+Turn off the autotune switch.  If the autotune switch is on while not armed the warning LED will flash and you cannot arm.
+
+Launch the multirotor.
+
+Turn on/hold the autotune switch on your transmitter.
+
+Observe roll left/right.  A beep code will sound on the buzzer.
+
+Turn off/release the switch while still flying to stop this phase of tuning.
+
+PID settings will have been updated for ROLL/YAW.
+
+Turn on/hold the switch again.
+
+Observe pitch forwards/backwards.  A beep code will sound on the buzzer.
+
+Turn off/release the switch while still flying to stop this phase of tuning.
+
+PID settings will have been updated for PITCH/YAW.
+
+PIDS are updated, fly and see if it's better.
+
+If it's worse, flip the switch again to restore previous pids that were present prior to arming.
+
+Land.
+
+Verify results via an app while power still applied if desired.
+
+Cutting the power will loose the unsaved pids.
+
+If you're happy with the pids then while disarmed flip the autotune switch again to save all settings then flip it back so you can arm again.
+
+A beeper will sound indicating the settings are saved.
+
+
+# References
+
+* Brad Quick for the initial Autotune algorithm in BradWii.

src/buzzer.c

@@ -39,7 +39,6 @@ void buzzer(bool warn_vbat)
     static uint8_t beeperOnBox;
     static uint8_t warn_noGPSfix = 0;
     static failsafeBuzzerWarnings_e warn_failsafe = FAILSAFE_IDLE;
-    static uint8_t warn_runtime = 0;
 
     //=====================  BeeperOn via rcOptions =====================
     if (rcOptions[BOXBEEPERON]) {       // unconditional beeper on via AUXn switch
@@ -88,8 +87,8 @@ void buzzer(bool warn_vbat)
         beep_code('S','S','S','M');                 // beeperon
     else if (warn_vbat)
         beep_code('S','M','M','D');
-    else if (warn_runtime == 1 && f.ARMED)
+    else if (f.AUTOTUNE_MODE)
-        beep_code('S','S','M','N');                 // Runtime warning
+        beep_code('S','M','S','M');
     else if (toggleBeep > 0)
         beep(50);                                   // fast confirmation beep
     else {

src/flight_autotune.c

@@ -0,0 +1,369 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "drivers/system_common.h"
+
+#include "flight_common.h"
+
+extern int16_t debug[4];
+
+/*
+ * Authors
+ * Brad Quick - initial implementation in BradWii
+ * Dominic Clifton - baseflight port & cleanup.
+ *
+ * Autotune in BradWii thread here: http://www.rcgroups.com/forums/showthread.php?t=1922423
+ *
+ * We start with two input parameters. The first is our target angle. By default it's 20 degrees, so we will bank to 20 degrees,
+ * see how the system reacts, then bank to -20 degrees and evaluate again. We repeat this over and over. The second input is
+ * how much oscillation we can tolerate. This can range from 2 degrees to 5 or more degrees. This defaults to 4 degrees. The
+ * higher this value is, the more agressive the result of the tuning will be.
+ *
+ * First, we turn the I gain down to zero so that we don't have to try to figure out how much overshoot is caused by the I term
+ * vs. the P term.
+ *
+ * Then, we move to the target of 20 degrees and analyze the results. Our goal is to have no overshoot and to keep the bounce
+ * back within the 4 degrees. By working to get zero overshoot, we can isolate the effects of the P and D terms. If we don't
+ * overshoot, then the P term never works in the opposite direction, so we know that any bounce we get is caused by the D term.
+ *
+ * If we overshoot the target 20 degrees, then we know our P term is too high or our D term is too low. We can determine
+ * which one to change by looking at how much bounce back (or the amplitude of the oscillation) we get. If it bounces back
+ * more than the 4 degrees, then our D is already too high, so we can't increase it, so instead we decrease P.
+ *
+ * If we undershoot, then either our P is too low or our D is too high. Again, we can determine which to change by looking at
+ * how much bounce we get.
+ *
+ * Once we have the P and D terms set, we then set the I term by repeating the same test above and measuring the overshoot.
+ * If our maximum oscillation is set to 4 degrees, then we keep increasing the I until we get an overshoot of 2 degrees, so
+ * that our oscillations are now centered around our target (in theory).
+ *
+ * In the BradWii software, it alternates between doing the P and D step and doing the I step so you can quit whenever you
+ * want without having to tell it specifically to do the I term. The sequence is actually P&D, P&D, I, P&D, P&D, I...
+ *
+ * Note: The 4 degrees mentioned above is the value of AUTOTUNE_MAX_OSCILLATION.  In the BradWii code at the time of writing
+ * the default value was 1.0f instead of 4.0f.
+ *
+ * To adjust how aggressive the tuning is, adjust the AUTOTUNEMAXOSCILLATION value.  A larger value will result in more
+ * aggressive tuning. A lower value will result in softer tuning. It will rock back and forth between -AUTOTUNE_TARGET_ANGLE
+ * and AUTOTUNE_TARGET_ANGLE degrees
+ * AUTOTUNE_D_MULTIPLIER is a multiplier that puts in a little extra D when autotuning is done. This helps damp  the wobbles
+ * after a quick angle change.
+ * Always autotune on a full battery.
+ */
+
+#define AUTOTUNE_MAX_OSCILLATION_ANGLE 2.0f
+#define AUTOTUNE_TARGET_ANGLE 20.0f
+#define AUTOTUNE_D_MULTIPLIER 1.2f
+#define AUTOTUNE_SETTLING_DELAY_MS 250  // 1/4 of a second.
+
+#define AUTOTUNE_INCREASE_MULTIPLIER 1.03f
+#define AUTOTUNE_DECREASE_MULTIPLIER 0.97f
+
+#define AUTOTUNE_MINIMUM_I_VALUE 0.001f
+
+#define YAW_GAIN_MULTIPLIER 2.0f
+
+
+typedef enum {
+    PHASE_IDLE = 0,
+    PHASE_TUNE_ROLL,
+    PHASE_TUNE_PITCH,
+    PHASE_SAVE_OR_RESTORE_PIDS,
+} autotunePhase_e;
+
+static const pidIndex_e angleIndexToPidIndexMap[] = {
+    PIDROLL,
+    PIDPITCH
+};
+
+#define AUTOTUNE_PHASE_MAX PHASE_SAVE_OR_RESTORE_PIDS
+#define AUTOTUNE_PHASE_COUNT (AUTOTUNE_PHASE_MAX + 1)
+
+#define FIRST_TUNE_PHASE PHASE_TUNE_ROLL
+
+static pidProfile_t *pidProfile;
+static pidProfile_t pidBackup;
+static uint8_t pidController;
+static uint8_t pidIndex;
+static bool rising;
+static uint8_t cycleCount; // TODO can we replace this with an enum to improve readability.
+static uint32_t timeoutAt;
+static angle_index_t autoTuneAngleIndex;
+static autotunePhase_e phase = PHASE_IDLE;
+static autotunePhase_e nextPhase = FIRST_TUNE_PHASE;
+
+static float targetAngle = 0;
+static float targetAngleAtPeak;
+static float secondPeakAngle, firstPeakAngle; // deci dgrees, 180 deg = 1800
+
+typedef struct fp_pid {
+    float p;
+    float i;
+    float d;
+} fp_pid_t;
+
+static fp_pid_t pid;
+
+// These are used to convert between multiwii integer values to the float pid values used by the autotuner.
+#define MULTIWII_P_MULTIPLIER 10.0f    // e.g 0.4 * 10 = 40
+#define MULTIWII_I_MULTIPLIER 1000.0f  // e.g 0.030 * 1000 = 30
+// Note there is no D multiplier since D values are stored and used AS-IS
+
+bool isAutotuneIdle(void)
+{
+    return phase == PHASE_IDLE;
+}
+
+static void startNewCycle(void)
+{
+    rising = !rising;
+    secondPeakAngle = firstPeakAngle = 0;
+}
+
+static void updatePidIndex(void)
+{
+    pidIndex = angleIndexToPidIndexMap[autoTuneAngleIndex];
+}
+
+static void updateTargetAngle(void)
+{
+    if (rising) {
+        targetAngle = AUTOTUNE_TARGET_ANGLE;
+    }
+    else  {
+        targetAngle = -AUTOTUNE_TARGET_ANGLE;
+    }
+
+#if 0
+    debug[2] = DEGREES_TO_DECIDEGREES(targetAngle);
+#endif
+}
+
+float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle)
+{
+    float currentAngle;
+
+    if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) {
+        return errorAngle;
+    }
+
+    if (pidController == 2) {
+        // TODO support new baseflight pid controller
+        return errorAngle;
+    }
+
+    debug[0] = 0;
+
+#if 0
+    debug[1] = inclination->rawAngles[angleIndex];
+#endif
+
+    updatePidIndex();
+
+    if (rising) {
+        currentAngle = DECIDEGREES_TO_DEGREES(inclination->raw[angleIndex]);
+    } else {
+        targetAngle = -targetAngle;
+        currentAngle = DECIDEGREES_TO_DEGREES(-inclination->raw[angleIndex]);
+    }
+
+#if 1
+    debug[1] = DEGREES_TO_DECIDEGREES(currentAngle);
+    debug[2] = DEGREES_TO_DECIDEGREES(targetAngle);
+#endif
+
+    if (firstPeakAngle == 0) {
+        // The peak will be when our angular velocity is negative.  To be sure we are in the right place,
+        // we also check to make sure our angle position is greater than zero.
+
+        if (currentAngle > secondPeakAngle) {
+            // we are still going up
+            secondPeakAngle = currentAngle;
+            targetAngleAtPeak = targetAngle;
+
+            debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle);
+
+        } else if (secondPeakAngle > 0) {
+            if (cycleCount == 0) {
+                // when checking the I value, we would like to overshoot the target position by half of the max oscillation.
+                if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) {
+                    pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
+                } else {
+                    pid.i *= AUTOTUNE_DECREASE_MULTIPLIER;
+                    if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) {
+                        pid.i = AUTOTUNE_MINIMUM_I_VALUE;
+                    }
+                }
+
+                // go back to checking P and D
+                cycleCount = 1;
+                pidProfile->I8[pidIndex] = 0;
+                startNewCycle();
+            } else {
+                // we are checking P and D values
+                // set up to look for the 2nd peak
+                firstPeakAngle = currentAngle;
+                timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS;
+            }
+        }
+    } else {
+        // we saw the first peak. looking for the second
+
+        if (currentAngle < firstPeakAngle) {
+            firstPeakAngle = currentAngle;
+            debug[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle);
+        }
+
+        float oscillationAmplitude = secondPeakAngle - firstPeakAngle;
+
+        uint32_t now = millis();
+        int32_t signedDiff = now - timeoutAt;
+        bool timedOut = signedDiff >= 0L;
+
+        // stop looking for the 2nd peak if we time out or if we change direction again after moving by more than half the maximum oscillation
+        if (timedOut || (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 && currentAngle > firstPeakAngle)) {
+            // analyze the data
+            // Our goal is to have zero overshoot and to have AUTOTUNEMAXOSCILLATION amplitude
+
+            if (secondPeakAngle > targetAngleAtPeak) {
+                // overshot
+                debug[0] = 1;
+
+#ifdef PREFER_HIGH_GAIN_SOLUTION
+                if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE) {
+                    // we have too much oscillation, so we can't increase D, so decrease P
+                    pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
+                } else {
+                    // we don't have too much oscillation, so we can increase D
+                    pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
+                }
+#else
+				pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
+				pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
+#endif
+            } else {
+                // undershot
+                debug[0] = 2;
+
+                if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE) {
+                    // we have too much oscillation
+                    pid.d *= AUTOTUNE_DECREASE_MULTIPLIER;
+                } else {
+                    // we don't have too much oscillation
+                    pid.p *= AUTOTUNE_INCREASE_MULTIPLIER;
+                }
+            }
+
+            pidProfile->P8[pidIndex] = pid.p * MULTIWII_P_MULTIPLIER;
+            pidProfile->D8[pidIndex] = pid.d;
+
+            // switch to the other direction and start a new cycle
+            startNewCycle();
+
+            if (++cycleCount == 3) {
+                // switch to testing I value
+                cycleCount = 0;
+
+                pidProfile->I8[pidIndex] = pid.i  * MULTIWII_I_MULTIPLIER;
+            }
+        }
+    }
+
+    if (angleIndex == AI_ROLL) {
+        debug[0] += 100;
+    }
+
+    updateTargetAngle();
+
+    return targetAngle - DECIDEGREES_TO_DEGREES(inclination->raw[angleIndex]);
+}
+
+void autotuneReset(void)
+{
+    targetAngle = 0;
+    phase = PHASE_IDLE;
+    nextPhase = FIRST_TUNE_PHASE;
+}
+
+void backupPids(pidProfile_t *pidProfileToTune)
+{
+    memcpy(&pidBackup, pidProfileToTune, sizeof(pidBackup));
+}
+
+void restorePids(pidProfile_t *pidProfileToTune)
+{
+    memcpy(pidProfileToTune, &pidBackup, sizeof(pidBackup));
+}
+
+void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse)
+{
+    phase = nextPhase;
+
+    if (phase == PHASE_SAVE_OR_RESTORE_PIDS) {
+        restorePids(pidProfileToTune);
+        return;
+    }
+
+    if (phase == FIRST_TUNE_PHASE) {
+        backupPids(pidProfileToTune);
+    }
+
+    if (phase == PHASE_TUNE_ROLL) {
+        autoTuneAngleIndex = AI_ROLL;
+    } if (phase == PHASE_TUNE_PITCH) {
+        autoTuneAngleIndex = AI_PITCH;
+    }
+
+    rising = true;
+    cycleCount = 1;
+    secondPeakAngle = firstPeakAngle = 0;
+
+    pidProfile = pidProfileToTune;
+    pidController = pidControllerInUse;
+
+    updatePidIndex();
+    updateTargetAngle();
+
+    pid.p = pidProfile->P8[pidIndex] / MULTIWII_P_MULTIPLIER;
+    pid.i = pidProfile->I8[pidIndex] / MULTIWII_I_MULTIPLIER;
+    // divide by D multiplier to get our working value.  We'll multiply by D multiplier when we are done.
+    pid.d = pidProfile->D8[pidIndex] * (1.0f / AUTOTUNE_D_MULTIPLIER);
+
+    pidProfile->D8[pidIndex] = pid.d;
+    pidProfile->I8[pidIndex] = 0;
+}
+
+void autotuneEndPhase(void)
+{
+    if (phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) {
+
+        // we leave P alone, just update I and D
+
+        pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER;
+
+        // multiply by D multiplier.  The best D is usually a little higher than what the algroithm produces.
+        pidProfile->D8[pidIndex] = (pid.d * AUTOTUNE_D_MULTIPLIER);
+
+        pidProfile->P8[PIDYAW] = pidProfile->P8[PIDROLL] * YAW_GAIN_MULTIPLIER;
+        pidProfile->I8[PIDYAW] = pidProfile->I8[PIDROLL];
+        pidProfile->D8[PIDYAW] = pidProfile->D8[PIDROLL];
+    }
+
+    if (phase == AUTOTUNE_PHASE_MAX) {
+        phase = PHASE_IDLE;
+        nextPhase = FIRST_TUNE_PHASE;
+    } else {
+        nextPhase++;
+    }
+}
+
+bool havePidsBeenUpdatedByAutotune(void)
+{
+    return targetAngle != 0;
+}
+

src/flight_autotune.h

@@ -0,0 +1,11 @@
+#pragma once
+
+void autotuneReset();
+void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse);
+float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle);
+void autotuneEndPhase();
+
+bool isAutotuneIdle(void);
+bool hasAutotunePhaseCompleted(void);
+bool havePidsBeenUpdatedByAutotune(void);
+

src/flight_common.c

@@ -12,6 +12,7 @@
 #include "sensors_gyro.h"
 #include "rc_controls.h"
 #include "flight_common.h"
+#include "flight_autotune.h"
 #include "gps_common.h"
 
 extern uint16_t cycleTime;
@@ -55,6 +56,12 @@ void resetErrorGyro(void)
     errorGyroIf[YAW] = 0;
 }
 
+const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
+
+bool shouldAutotune(void)
+{
+    return f.ARMED && f.AUTOTUNE_MODE;
+}
 
 static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
@@ -72,19 +79,27 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
     // ----------PID controller----------
     for (axis = 0; axis < 3; axis++) {
         // -----Get the desired angle rate depending on flight mode
-        if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
+        if (axis == FD_YAW) {
+            // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
             AngleRate = (float)((controlRateConfig->yawRate + 10) * rcCommand[YAW]) / 50.0f;
          } else {
             // calculate error and limit the angle to 50 degrees max inclination
             errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
-            if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
+
+            if (shouldAutotune()) {
+                errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle);
+            }
+
+            if (f.ANGLE_MODE) {
+                // it's the ANGLE mode - control is angle based, so control loop is needed
+                AngleRate = errorAngle * pidProfile->A_level;
+            } else {
+                //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
                 AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
                 if (f.HORIZON_MODE) {
-                    // mix up angle error to desired AngleRateTmp to add a little auto-level feel
+                    // mix up angle error to desired AngleRate to add a little auto-level feel
                     AngleRate += errorAngle * pidProfile->H_level;
                 }
-            } else { // it's the ANGLE mode - control is angle based, so control loop is needed
-                AngleRate = errorAngle * pidProfile->A_level;
             }
         }
 
@@ -123,6 +138,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
     }
 
 }
+
 static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
 {
@@ -138,9 +154,14 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
     for (axis = 0; axis < 3; axis++) {
         if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
-            // 50 degrees max inclination
+            // observe max inclination
             errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
                     +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
+
+            if (shouldAutotune()) {
+                errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
+            }
+
             PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
             PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
 
@@ -205,6 +226,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
             errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int) max_angle_inclination),
                     +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
 
+            if (shouldAutotune()) {
+                errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
+            }
+
             if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
                 AngleRateTmp = ((int32_t)(controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
                 if (f.HORIZON_MODE) {

src/flight_common.h

@@ -1,7 +1,7 @@
 #pragma once
 
 
-enum {
+typedef enum {
     PIDROLL,
     PIDPITCH,
     PIDYAW,
@@ -13,7 +13,7 @@ enum {
     PIDMAG,
     PIDVEL,
     PID_ITEM_COUNT
-};
+} pidIndex_e;
 
 typedef struct pidProfile_s {
     uint8_t P8[PID_ITEM_COUNT];
@@ -27,7 +27,7 @@ typedef struct pidProfile_s {
     float H_level;
 } pidProfile_t;
 
-enum {
+typedef enum {
     AI_ROLL = 0,
     AI_PITCH,
 } angle_index_t;
@@ -35,7 +35,7 @@ enum {
 #define ANGLE_INDEX_COUNT 2
 
 // See http://en.wikipedia.org/wiki/Flight_dynamics
-enum {
+typedef enum {
     FD_ROLL = 0,
     FD_PITCH,
     FD_YAW
@@ -98,6 +98,9 @@ typedef union {
 } rollAndPitchInclination_t;
 
 
+#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
+#define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f)
+
 extern rollAndPitchInclination_t inclination;
 
 extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];

src/mw.c

@@ -22,6 +22,7 @@
 #include "failsafe.h"
 #include "flight_imu.h"
 #include "flight_common.h"
+#include "flight_autotune.h"
 #include "flight_mixer.h"
 #include "gimbal.h"
 #include "gps_common.h"
@@ -78,6 +79,41 @@ bool AccInflightCalibrationSavetoEEProm = false;
 bool AccInflightCalibrationActive = false;
 uint16_t InflightcalibratingA = 0;
 
+void updateAutotuneState(void)
+{
+    static bool landedAfterAutoTuning = false;
+    static bool autoTuneWasUsed = false;
+
+    if (rcOptions[BOXAUTOTUNE]) {
+        if (!f.AUTOTUNE_MODE) {
+            if (f.ARMED) {
+                if (isAutotuneIdle() || landedAfterAutoTuning) {
+                    autotuneReset();
+                    landedAfterAutoTuning = false;
+                }
+                autotuneBeginNextPhase(&currentProfile.pidProfile, currentProfile.pidController);
+                f.AUTOTUNE_MODE = 1;
+                autoTuneWasUsed = true;
+            } else {
+                if (havePidsBeenUpdatedByAutotune()) {
+                    saveAndReloadCurrentProfileToCurrentProfileSlot();
+                    autotuneReset();
+                }
+            }
+        }
+        return;
+    }
+
+    if (f.AUTOTUNE_MODE) {
+        autotuneEndPhase();
+        f.AUTOTUNE_MODE = 0;
+    }
+
+    if (!f.ARMED && autoTuneWasUsed) {
+        landedAfterAutoTuning = true;
+    }
+}
+
 bool isCalibrating()
 {
 #ifdef BARO
@@ -206,7 +242,11 @@ void annexCode(void)
 
         f.OK_TO_ARM = 1;
 
-        if (!f.ARMED && !f.SMALL_ANGLE) {
+        if (!f.SMALL_ANGLE) {
+            f.OK_TO_ARM = 0;
+        }
+
+        if (rcOptions[BOXAUTOTUNE]) {
             f.OK_TO_ARM = 0;
         }
 
@@ -603,6 +643,8 @@ void loop(void)
         cycleTime = (int32_t)(currentTime - previousTime);
         previousTime = currentTime;
 
+        updateAutotuneState();
+
 #ifdef MAG
         if (sensors(SENSOR_MAG)) {
             if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {

src/runtime_config.h

@@ -22,6 +22,7 @@ enum {
     BOXGOV,
     BOXOSD,
     BOXTELEMETRY,
+    BOXAUTOTUNE,
     CHECKBOX_ITEM_COUNT
 };
 
@@ -46,6 +47,7 @@ typedef struct flags_t {
     uint8_t CALIBRATE_MAG;
     uint8_t VARIO_MODE;
     uint8_t FIXED_WING;                     // set when in flying_wing or airplane mode. currently used by althold selection code
+    uint8_t AUTOTUNE_MODE;
 } flags_t;
 
 extern flags_t f;

src/serial_common.c

@@ -78,12 +78,12 @@ typedef struct serialPortSearchResult_s {
     uint8_t startSerialPortFunctionIndex; // used by findNextSerialPort
 } serialPortSearchResult_t;
 
-static serialPortFunctionList_t serialPortFunctionList = {
+static const serialPortFunctionList_t serialPortFunctionList = {
         4,
         serialPortFunctions
 };
 
-serialPortFunctionList_t *getSerialPortFunctionList(void)
+const serialPortFunctionList_t *getSerialPortFunctionList(void)
 {
     return &serialPortFunctionList;
 }

src/serial_common.h

@@ -113,7 +113,7 @@ bool isSerialConfigValid(serialConfig_t *serialConfig);
 bool doesConfigurationUsePort(serialConfig_t *serialConfig, serialPortIdentifier_e portIdentifier);
 bool isSerialPortFunctionShared(serialPortFunction_e functionToUse, uint16_t functionMask);
 
-serialPortFunctionList_t *getSerialPortFunctionList(void);
+const serialPortFunctionList_t *getSerialPortFunctionList(void);
 
 void evaluateOtherData(uint8_t sr);
 void handleSerial(void);

src/serial_msp.c

@@ -139,6 +139,7 @@ struct box_t {
     { BOXGOV, "GOVERNOR;", 18 },
     { BOXOSD, "OSD SW;", 19 },
     { BOXTELEMETRY, "TELEMETRY;", 20 },
+    { BOXAUTOTUNE, "AUTOTUNE;", 21 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -315,7 +316,7 @@ static void openAllMSPSerialPorts(serialConfig_t *serialConfig)
     } while (port);
 
     // XXX this function might help with adding support for MSP on more than one port, if not delete it.
-    serialPortFunctionList_t *serialPortFunctionList = getSerialPortFunctionList();
+    const serialPortFunctionList_t *serialPortFunctionList = getSerialPortFunctionList();
     UNUSED(serialPortFunctionList);
 }
 
@@ -367,6 +368,8 @@ void mspInit(serialConfig_t *serialConfig)
     if (feature(FEATURE_TELEMETRY && masterConfig.telemetryConfig.telemetry_switch))
         availableBoxes[idx++] = BOXTELEMETRY;
 
+    availableBoxes[idx++] = BOXAUTOTUNE;
+
     numberBoxItems = idx;
 
     openAllMSPSerialPorts(serialConfig);
@@ -513,6 +516,7 @@ static void evaluateCommand(void)
             rcOptions[BOXGOV] << BOXGOV |
             rcOptions[BOXOSD] << BOXOSD |
             rcOptions[BOXTELEMETRY] << BOXTELEMETRY |
+            rcOptions[BOXAUTOTUNE] << BOXAUTOTUNE |
             f.ARMED << BOXARM;
         for (i = 0; i < numberBoxItems; i++) {
             int flag = (tmp & (1 << availableBoxes[i]));