Revert "Remove GTUNE"

This reverts commit a16ac7fbe7.

Makefile

@@ -326,6 +326,7 @@ COMMON_SRC = build_config.c \
 		   $(DEVICE_STDPERIPH_SRC)
 
 HIGHEND_SRC = \
+		   flight/gtune.c \
 		   flight/navigation.c \
 		   flight/gps_conversion.c \
 		   common/colorconversion.c \

src/main/blackbox/blackbox.c

@@ -1206,6 +1206,11 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
                 blackboxWriteSignedVB(data->inflightAdjustment.newValue);
             }
         break;
+        case FLIGHT_LOG_EVENT_GTUNE_RESULT:
+            blackboxWrite(data->gtuneCycleResult.gtuneAxis);
+            blackboxWriteSignedVB(data->gtuneCycleResult.gtuneGyroAVG);
+            blackboxWriteS16(data->gtuneCycleResult.gtuneNewP);
+        break;
         case FLIGHT_LOG_EVENT_LOGGING_RESUME:
             blackboxWriteUnsignedVB(data->loggingResume.logIteration);
             blackboxWriteUnsignedVB(data->loggingResume.currentTime);

src/main/blackbox/blackbox_fielddefs.h

@@ -105,6 +105,7 @@ typedef enum FlightLogEvent {
     FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
     FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
     FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
+    FLIGHT_LOG_EVENT_GTUNE_RESULT = 20,
     FLIGHT_LOG_EVENT_LOG_END = 255
 } FlightLogEvent;
 

src/main/config/config.c

@@ -194,6 +194,17 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->H_level = 4.0f;
     pidProfile->H_sensitivity = 75;
 
+#ifdef GTUNE
+    pidProfile->gtune_lolimP[ROLL] = 10;          // [0..200] Lower limit of ROLL P during G tune.
+    pidProfile->gtune_lolimP[PITCH] = 10;         // [0..200] Lower limit of PITCH P during G tune.
+    pidProfile->gtune_lolimP[YAW] = 10;           // [0..200] Lower limit of YAW P during G tune.
+    pidProfile->gtune_hilimP[ROLL] = 100;         // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis.
+    pidProfile->gtune_hilimP[PITCH] = 100;        // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis.
+    pidProfile->gtune_hilimP[YAW] = 100;          // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis.
+    pidProfile->gtune_pwr = 0;                    // [0..10] Strength of adjustment
+    pidProfile->gtune_settle_time = 450;          // [200..1000] Settle time in ms
+    pidProfile->gtune_average_cycles = 16;        // [8..128] Number of looptime cycles used for gyro average calculation
+#endif
 }
 
 #ifdef GPS

src/main/config/runtime_config.h

@@ -42,6 +42,7 @@ typedef enum {
     PASSTHRU_MODE   = (1 << 8),
     SONAR_MODE      = (1 << 9),
     FAILSAFE_MODE   = (1 << 10),
+    GTUNE_MODE      = (1 << 11),
 } flightModeFlags_e;
 
 extern uint16_t flightModeFlags;

src/main/flight/gtune.c

@@ -0,0 +1,211 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+
+#ifdef GTUNE
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "drivers/system.h"
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
+
+#include "sensors/sensors.h"
+#include "sensors/gyro.h"
+#include "sensors/acceleration.h"
+
+#include "flight/pid.h"
+#include "flight/imu.h"
+
+#include "config/config.h"
+#include "blackbox/blackbox.h"
+
+#include "io/rc_controls.h"
+
+#include "config/runtime_config.h"
+
+extern uint16_t cycleTime;
+extern uint8_t motorCount;
+
+/*
+ ****************************************************************************
+ ***                    G_Tune                                            ***
+ ****************************************************************************
+	G_Tune Mode
+	This is the multiwii implementation of ZERO-PID Algorithm
+	http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html
+	The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com)
+
+	You may use/modify this algorithm on your own risk, kindly refer to above link in any future distribution.
+ */
+
+/*
+   version 1.0.0: MIN & Maxis & Tuned Band
+   version 1.0.1:
+                a. error is gyro reading not rc - gyro.
+                b. OldError = Error no averaging.
+                c. No Min Maxis BOUNDRY
+    version 1.0.2:
+                a. no boundaries
+                b. I - Factor tune.
+                c. time_skip
+
+   Crashpilot: Reduced to just P tuning in a predefined range - so it is not "zero pid" anymore.
+   Tuning is limited to just work when stick is centered besides that YAW is tuned in non Acro as well.
+   See also:
+   http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2
+   http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190
+   Gyrosetting 2000DPS
+   GyroScale = (1 / 16,4 ) * RADX(see board.h) = 0,001064225154 digit per rad/s
+
+    pidProfile->gtune_lolimP[ROLL]   = 10;  [0..200] Lower limit of ROLL P during G tune.
+    pidProfile->gtune_lolimP[PITCH]  = 10;  [0..200] Lower limit of PITCH P during G tune.
+    pidProfile->gtune_lolimP[YAW]    = 10;  [0..200] Lower limit of YAW P during G tune.
+    pidProfile->gtune_hilimP[ROLL]   = 100; [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis.
+    pidProfile->gtune_hilimP[PITCH]  = 100; [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis.
+    pidProfile->gtune_hilimP[YAW]    = 100; [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axisis.
+    pidProfile->gtune_pwr            = 0;   [0..10] Strength of adjustment
+    pidProfile->gtune_settle_time    = 450; [200..1000] Settle time in ms
+    pidProfile->gtune_average_cycles = 16;  [8..128] Number of looptime cycles used for gyro average calculation
+*/
+
+static pidProfile_t *pidProfile;
+static int16_t delay_cycles;
+static int16_t time_skip[3];
+static int16_t OldError[3], result_P64[3];
+static int32_t AvgGyro[3];
+static bool floatPID;
+
+void updateDelayCycles(void)
+{
+    delay_cycles = -(((int32_t)pidProfile->gtune_settle_time * 1000) / cycleTime);
+}
+
+void init_Gtune(pidProfile_t *pidProfileToTune)
+{
+    uint8_t i;
+
+    pidProfile = pidProfileToTune;
+	if (pidProfile->pidController == 2) {
+	    floatPID = true;                                                        // LuxFloat is using float values for PID settings
+	} else {
+	    floatPID = false;
+	}
+	updateDelayCycles();
+	for (i = 0; i < 3; i++) {
+        if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning
+            pidProfile->gtune_hilimP[i] = 0;                                    // Disable YAW tuning for everything below a quadcopter
+        }
+        if (floatPID) {
+            if((pidProfile->P_f[i] * 10.0f) < pidProfile->gtune_lolimP[i]) {
+                pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10.0f);
+            }
+            result_P64[i] = (int16_t)pidProfile->P_f[i] << 6;                   // 6 bit extra resolution for P.
+        } else {
+            if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) {
+                pidProfile->P8[i] = pidProfile->gtune_lolimP[i];
+            }
+            result_P64[i] = (int16_t)pidProfile->P8[i] << 6;                    // 6 bit extra resolution for P.
+        }
+        OldError[i] = 0;
+        time_skip[i] = delay_cycles;
+    }
+}
+
+void calculate_Gtune(uint8_t axis)
+{
+    int16_t error, diff_G, threshP;
+
+    if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) {  // Block tuning on stick input. Always allow G-Tune on YAW, Roll & Pitch only in acromode
+        OldError[axis] = 0;
+        time_skip[axis] = delay_cycles;                                         // Some settle time after stick center. default 450ms
+    } else {
+        if (!time_skip[axis]) AvgGyro[axis] = 0;
+        time_skip[axis]++;
+        if (time_skip[axis] > 0) {
+            if (axis == FD_YAW) {
+                AvgGyro[axis] += 32 * ((int16_t)gyroADC[axis] / 32);           // Chop some jitter and average
+            } else {
+                AvgGyro[axis] += 128 * ((int16_t)gyroADC[axis] / 128);         // Chop some jitter and average
+            }
+        }
+        if (time_skip[axis] == pidProfile->gtune_average_cycles) {              // Looptime cycles for gyro average calculation. default 16.
+            AvgGyro[axis] /= time_skip[axis];                                   // AvgGyro[axis] has now very clean gyrodata
+            time_skip[axis] = 0;
+            if (axis == FD_YAW) {
+                threshP = 20;
+                error = -AvgGyro[axis];
+            } else {
+                threshP = 10;
+                error = AvgGyro[axis];
+            }
+            if (pidProfile->gtune_hilimP[axis] && error && OldError[axis] && error != OldError[axis]) {  // Don't run when not needed or pointless to do so
+                diff_G = ABS(error) - ABS(OldError[axis]);
+                if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) {
+                    if (diff_G > threshP) {
+                        if (axis == FD_YAW) {
+                            result_P64[axis] += 256 + pidProfile->gtune_pwr;    // YAW ends up at low limit on float PID, give it some more to work with.
+                        } else {
+                            result_P64[axis] += 64 + pidProfile->gtune_pwr;     // Shift balance a little on the plus side.
+                        }
+                    } else {
+                        if (diff_G < -threshP) {
+                            if (axis == FD_YAW) {
+                                result_P64[axis] -= 64 + pidProfile->gtune_pwr;
+                            } else {
+                                result_P64[axis] -= 32;
+                            }
+                        }
+                    }
+                } else {
+                    if (ABS(diff_G) > threshP && axis != FD_YAW) {
+                        result_P64[axis] -= 32;                                 // Don't use antiwobble for YAW
+                    }
+                }
+                int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]);
+
+#ifdef BLACKBOX
+                if (feature(FEATURE_BLACKBOX)) {
+                    flightLogEvent_gtuneCycleResult_t eventData;
+
+                    eventData.gtuneAxis = axis;
+                    eventData.gtuneGyroAVG = AvgGyro[axis];
+                    eventData.gtuneNewP = newP;                                 // for float PID the logged P value is still mutiplyed by 10
+                    blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
+                }
+#endif
+
+                if (floatPID) {
+                    pidProfile->P_f[axis] = (float)newP / 10.0f;                // new P value for float PID
+                } else {
+                    pidProfile->P8[axis] = newP;                                // new P value
+                }
+            }
+            OldError[axis] = error;
+        }
+    }
+}
+
+#endif
+

src/main/flight/gtune.h

@@ -0,0 +1,21 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+void init_Gtune(pidProfile_t *pidProfileToTune);
+void calculate_Gtune(uint8_t axis);

src/main/flight/pid.c

@@ -44,7 +44,7 @@
 #include "flight/pid.h"
 #include "flight/imu.h"
 #include "flight/navigation.h"
-
+#include "flight/gtune.h"
 
 #include "config/runtime_config.h"
 
@@ -255,6 +255,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
 
         if (lowThrottlePidReduction) axisPID[axis] /= 4;
 
+#ifdef GTUNE
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+            calculate_Gtune(axis);
+        }
+#endif
+
 #ifdef BLACKBOX
         axisPID_P[axis] = PTerm;
         axisPID_I[axis] = ITerm;
@@ -364,6 +370,11 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
 
         if (lowThrottlePidReduction) axisPID[axis] /= 4;
 
+#ifdef GTUNE
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+            calculate_Gtune(axis);
+        }
+#endif
 
 #ifdef BLACKBOX
         axisPID_P[axis] = PTerm;
@@ -396,6 +407,12 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
 
     if (lowThrottlePidReduction) axisPID[FD_YAW] /= 4;
 
+#ifdef GTUNE
+    if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+        calculate_Gtune(FD_YAW);
+    }
+#endif
+
 #ifdef BLACKBOX
     axisPID_P[FD_YAW] = PTerm;
     axisPID_I[FD_YAW] = ITerm;
@@ -528,6 +545,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
 
         if (lowThrottlePidReduction) axisPID[axis] /= 4;
 
+#ifdef GTUNE
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+             calculate_Gtune(axis);
+        }
+#endif
 
 #ifdef BLACKBOX
         axisPID_P[axis] = PTerm;

src/main/flight/pid.h

@@ -75,6 +75,13 @@ typedef struct pidProfile_s {
     uint16_t yaw_p_limit;
     uint8_t dterm_average_count;            // Configurable delta count for dterm
 
+#ifdef GTUNE
+    uint8_t  gtune_lolimP[3];               // [0..200] Lower limit of P during G tune
+    uint8_t  gtune_hilimP[3];               // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis.
+    uint8_t  gtune_pwr;                     // [0..10] Strength of adjustment
+    uint16_t gtune_settle_time;             // [200..1000] Settle time in ms
+    uint8_t  gtune_average_cycles;          // [8..128] Number of looptime cycles used for gyro average calculation
+#endif
 } pidProfile_t;
 
 extern int16_t axisPID[XYZ_AXIS_COUNT];

src/main/io/i2c_bst.c

@@ -322,7 +322,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXGOV, "GOVERNOR;", 18 },
     { BOXOSD, "OSD SW;", 19 },
     { BOXTELEMETRY, "TELEMETRY;", 20 },
-   // { BOXGTUNE, "GTUNE;", 21 },
+    { BOXGTUNE, "GTUNE;", 21 },
     { BOXSONAR, "SONAR;", 22 },
     { BOXSERVO1, "SERVO1;", 23 },
     { BOXSERVO2, "SERVO2;", 24 },
@@ -584,6 +584,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
 		            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
 		            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
 		            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
+		            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
 		            IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
 		            IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
 		            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
@@ -1688,6 +1689,7 @@ bool writeFCModeToBST(void)
 	       IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
 	       IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
 	       IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
+	       IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
 	       IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
 	       IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
 	       IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |

src/main/io/serial_cli.c

@@ -564,6 +564,17 @@ const clivalue_t valueTable[] = {
     { "nav_speed_max",              VAR_UINT16 | MASTER_VALUE, &masterConfig.gpsProfile.nav_speed_max, .config.minmax = { 10,  2000 } },
     { "nav_slew_rate",              VAR_UINT8  | MASTER_VALUE, &masterConfig.gpsProfile.nav_slew_rate, .config.minmax = { 0,  100 } },
 #endif
+#ifdef GTUNE
+    { "gtune_loP_rll",              VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10,  200 } },
+    { "gtune_loP_ptch",             VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], .config.minmax = { 10,  200 } },
+    { "gtune_loP_yw",               VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], .config.minmax = { 10,  200 } },
+    { "gtune_hiP_rll",              VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], .config.minmax = { 0,  200 } },
+    { "gtune_hiP_ptch",             VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], .config.minmax = { 0,  200 } },
+    { "gtune_hiP_yw",               VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], .config.minmax = { 0,  200 } },
+    { "gtune_pwr",                  VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_pwr, .config.minmax = { 0,  10 } },
+    { "gtune_settle_time",          VAR_UINT16 | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_settle_time, .config.minmax = { 200,  1000 } },
+    { "gtune_average_cycles",       VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_average_cycles, .config.minmax = { 8,  128 } },
+#endif
 
     { "serialrx_provider",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX } },
     { "spektrum_sat_bind",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED,  SPEKTRUM_SAT_BIND_MAX} },

src/main/io/serial_msp.c

@@ -167,7 +167,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXGOV, "GOVERNOR;", 18 },
     { BOXOSD, "OSD SW;", 19 },
     { BOXTELEMETRY, "TELEMETRY;", 20 },
-   // { BOXGTUNE, "GTUNE;", 21 },
+    { BOXGTUNE, "GTUNE;", 21 },
     { BOXSONAR, "SONAR;", 22 },
     { BOXSERVO1, "SERVO1;", 23 },
     { BOXSERVO2, "SERVO2;", 24 },
@@ -566,6 +566,10 @@ void mspInit(serialConfig_t *serialConfig)
         activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
     }
 
+#ifdef GTUNE
+    activeBoxIds[activeBoxIdCount++] = BOXGTUNE;
+#endif
+
     memset(mspPorts, 0x00, sizeof(mspPorts));
     mspAllocateSerialPorts(serialConfig);
 }
@@ -599,6 +603,7 @@ static uint32_t packFlightModeFlags(void)
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
+        IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
         IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
         IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |

src/main/mw.c

@@ -78,6 +78,7 @@
 #include "flight/imu.h"
 #include "flight/altitudehold.h"
 #include "flight/failsafe.h"
+#include "flight/gtune.h"
 #include "flight/navigation.h"
 
 #include "config/runtime_config.h"
@@ -134,6 +135,30 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
     saveConfigAndNotify();
 }
 
+#ifdef GTUNE
+
+void updateGtuneState(void)
+{
+    static bool GTuneWasUsed = false;
+
+    if (IS_RC_MODE_ACTIVE(BOXGTUNE)) {
+        if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+            ENABLE_FLIGHT_MODE(GTUNE_MODE);
+            init_Gtune(&currentProfile->pidProfile);
+            GTuneWasUsed = true;
+        }
+        if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) {
+            saveConfigAndNotify();
+            GTuneWasUsed = false;
+        }
+    } else {
+        if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
+            DISABLE_FLIGHT_MODE(GTUNE_MODE);
+        }
+    }
+}
+#endif
+
 bool isCalibrating()
 {
 #ifdef BARO
@@ -633,6 +658,10 @@ void taskMainPidLoop(void)
         }
 #endif
 
+#ifdef GTUNE
+        updateGtuneState();
+#endif
+
 #if defined(BARO) || defined(SONAR)
         if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
             if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {

src/main/target/ALIENFLIGHTF3/target.h

@@ -152,6 +152,9 @@
 
 //#define BLACKBOX
 #define SERIAL_RX
+//#define GPS
+#define GTUNE
+//#define DISPLAY
 #define USE_SERVOS
 #define USE_CLI
 

src/main/target/CHEBUZZF3/target.h

@@ -162,6 +162,7 @@
 #endif
 
 #define BLACKBOX
+#define GTUNE
 #define TELEMETRY
 #define SERIAL_RX
 #define USE_SERVOS

src/main/target/CJMCU/target.h

@@ -77,3 +77,6 @@
 #define SKIP_CLI_COMMAND_HELP
 #endif
 
+//#undef USE_CLI
+#define GTUNE
+//#define BLACKBOX

src/main/target/COLIBRI_RACE/target.h

@@ -153,6 +153,7 @@
 
 #define BLACKBOX
 #define GPS
+#define GTUNE
 #define LED_STRIP
 
 #define LED_STRIP_TIMER TIM16

src/main/target/EUSTM32F103RC/target.h

@@ -123,6 +123,7 @@
 #define LED_STRIP_TIMER TIM3
 
 #define BLACKBOX
+#define GTUNE
 #define TELEMETRY
 #define SERIAL_RX
 #define USE_SERVOS

src/main/target/IRCFUSIONF3/target.h

@@ -151,6 +151,7 @@
 #define BLACKBOX
 #define DISPLAY
 #define GPS
+#define GTUNE
 #define SERIAL_RX
 #define TELEMETRY
 #define USE_SERVOS

src/main/target/LUX_RACE/target.h

@@ -129,6 +129,7 @@
 
 #define BLACKBOX
 #define GPS
+#define GTUNE
 #define LED_STRIP
 
 #define LED_STRIP_TIMER TIM16

src/main/target/MOTOLAB/target.h

@@ -122,6 +122,7 @@
 #define BLACKBOX
 #define SERIAL_RX
 //#define GPS
+#define GTUNE
 #define DISPLAY
 #define USE_SERVOS
 #define USE_FLASHFS

src/main/target/NAZE/target.h

@@ -175,6 +175,8 @@
 #define WS2811_DMA_TC_FLAG           DMA1_FLAG_TC6
 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER
 
+//#define GPS
+#define GTUNE
 #define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX

src/main/target/NAZE32PRO/target.h

@@ -42,6 +42,7 @@
 
 #define BLACKBOX
 #define GPS
+#define GTUNE
 #define SERIAL_RX
 #define TELEMETRY
 #define USE_SERVOS

src/main/target/PORT103R/target.h

@@ -151,6 +151,7 @@
 
 #define BLACKBOX
 #define GPS
+#define GTUNE
 #define SERIAL_RX
 #define TELEMETRY
 #define USE_SERVOS

src/main/target/RMDO/target.h

@@ -143,6 +143,7 @@
 #define WS2811_DMA_HANDLER_IDENTIFER    DMA1_CH2_HANDLER
 
 #define BLACKBOX
+#define GTUNE
 #define TELEMETRY
 #define SERIAL_RX
 #define USE_SERVOS

src/main/target/SPARKY/target.h

@@ -120,6 +120,7 @@
 
 #define BLACKBOX
 #define GPS
+#define GTUNE
 #define DISPLAY
 #define SERIAL_RX
 #define TELEMETRY

src/main/target/SPRACINGF3/target.h

@@ -152,6 +152,7 @@
 #define BLACKBOX
 #define DISPLAY
 #define GPS
+#define GTUNE
 #define SERIAL_RX
 #define TELEMETRY
 #define USE_SERVOS

src/main/target/STM32F3DISCOVERY/target.h

@@ -154,6 +154,7 @@
 
 #define BLACKBOX
 #define GPS
+#define GTUNE
 #define LED_STRIP
 #define LED_STRIP_TIMER TIM16
 #define TELEMETRY