Initial commit for implementation of autotune.

src/config.c

@@ -58,7 +58,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
 #define FLASH_WRITE_ADDR                (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) // use the last flash pages for storage
master_t masterConfig;      // master config struct with data independent from profiles
 profile_t currentProfile;   // profile config struct
 
-static const uint8_t EEPROM_CONF_VERSION = 69;
+static const uint8_t EEPROM_CONF_VERSION = 70;
 
 static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims)
 {

src/flight_autotune.c

@@ -0,0 +1,71 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "flight_common.h"
+
+// To adjust how agressive the tuning is, adjust the AUTOTUNEMAXOSCILLATION value.  A larger
+// value will result in more agressive 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 1.0
+#define AUTOTUNE_TARGET_ANGLE 20.0
+#define AUTOTUNE_D_MULTIPLIER 1.2
+
+static pidProfile_t *pidProfile;
+static uint8_t pidController;
+static bool rising;
+static float targetAngle = 0;
+
+static angle_index_t autoTuneAngleIndex;
+
+float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle)
+{
+    if (autoTuneAngleIndex != angleIndex) {
+        // Not tuning this angle yet.
+        return errorAngle;
+    }
+
+    // TODO autotune!
+
+    if (rising) {
+        targetAngle = AUTOTUNE_TARGET_ANGLE;
+    }
+    else  {
+        targetAngle = -AUTOTUNE_TARGET_ANGLE;
+    }
+
+
+    return targetAngle - inclination->rawAngles[angleIndex];
+}
+
+void autotuneReset(void)
+{
+    targetAngle = 0;
+    rising = true;
+    autoTuneAngleIndex = AI_ROLL;
+}
+
+void autotuneBegin(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse)
+{
+    autotuneReset();
+
+    pidProfile = pidProfileToTune;
+    pidController = pidControllerInUse;
+}
+
+void autotuneEnd(void)
+{
+
+}
+
+bool havePidsBeenUpdatedByAutotune(void)
+{
+    return targetAngle != 0;
+}
+

src/flight_autotune.h

@@ -0,0 +1,9 @@
+#pragma once
+
+void autotuneReset();
+void autotuneBegin(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse);
+float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle);
+void autotuneEnd();
+
+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;
 }
 
+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)
@@ -77,6 +84,11 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
          } else {
             // calculate error and limit the angle to 50 degrees max inclination
             errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - inclination.rawAngles[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
+
+            if (shouldAutotune()) {
+                errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle);
+            }
+
             if (!f.ANGLE_MODE) { //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) {
@@ -141,6 +153,11 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
             // 50 degrees max inclination
             errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
                     +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis];
+
+            if (shouldAutotune()) {
+                errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, 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 +222,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.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here
 
+            if (shouldAutotune()) {
+                errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, 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

@@ -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

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,30 @@ bool AccInflightCalibrationSavetoEEProm = false;
 bool AccInflightCalibrationActive = false;
 uint16_t InflightcalibratingA = 0;
 
+void updateAutotuneState(void)
+{
+    if (rcOptions[BOXAUTOTUNE]) {
+        if (!f.AUTOTUNE_MODE) {
+            if (f.ARMED) {
+                autotuneBegin(&currentProfile.pidProfile, currentProfile.pidController);
+                f.AUTOTUNE_MODE = 1;
+            } else {
+                if (havePidsBeenUpdatedByAutotune()) {
+                    //writeEEPROM();
+                    blinkLedAndSoundBeeper(5, 50, 1);
+                    autotuneReset();
+                }
+            }
+        }
+        return;
+    }
+
+    if (f.AUTOTUNE_MODE) {
+        autotuneEnd();
+        f.AUTOTUNE_MODE = 0;
+    }
+}
+
 bool isCalibrating()
 {
 #ifdef BARO
@@ -603,6 +628,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_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 }
 };
 
@@ -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]));