diff --git a/Makefile b/Makefile
index af26074383..ebbe6b583e 100644
--- a/Makefile
+++ b/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 \
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index ec7b4754bc..65d5759038 100644
--- a/src/main/blackbox/blackbox.c
+++ b/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);
diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h
index 048dfae08b..0bee570f20 100644
--- a/src/main/blackbox/blackbox_fielddefs.h
+++ b/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;
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 614923e87a..fa9ca0532d 100755
--- a/src/main/config/config.c
+++ b/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
diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h
index 857ebea59f..6b4dee82e5 100644
--- a/src/main/config/runtime_config.h
+++ b/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;
diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
new file mode 100644
index 0000000000..d32a93ad8d
--- /dev/null
+++ b/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 .
+ */
+
+#include
+#include
+#include
+#include
+
+#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
+
diff --git a/src/main/flight/gtune.h b/src/main/flight/gtune.h
new file mode 100644
index 0000000000..f580c7c125
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+
+void init_Gtune(pidProfile_t *pidProfileToTune);
+void calculate_Gtune(uint8_t axis);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 5cb8d43998..c9a0c726f9 100644
--- a/src/main/flight/pid.c
+++ b/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;
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 665e187d5d..98de99238a 100644
--- a/src/main/flight/pid.h
+++ b/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];
diff --git a/src/main/io/i2c_bst.c b/src/main/io/i2c_bst.c
index 1b373134d1..c31fcf3e5a 100644
--- a/src/main/io/i2c_bst.c
+++ b/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 |
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index b5fe24da90..8283c41978 100644
--- a/src/main/io/serial_cli.c
+++ b/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} },
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 3adf064b12..9d621c8533 100644
--- a/src/main/io/serial_msp.c
+++ b/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 |
diff --git a/src/main/mw.c b/src/main/mw.c
index 38d280466d..7e8afbb49a 100644
--- a/src/main/mw.c
+++ b/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(¤tProfile->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)) {
diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h
index 6f89f8da98..121002a5e6 100644
--- a/src/main/target/ALIENFLIGHTF3/target.h
+++ b/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
diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h
index 211d107083..15b54d015c 100644
--- a/src/main/target/CHEBUZZF3/target.h
+++ b/src/main/target/CHEBUZZF3/target.h
@@ -162,6 +162,7 @@
#endif
#define BLACKBOX
+#define GTUNE
#define TELEMETRY
#define SERIAL_RX
#define USE_SERVOS
diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h
index a54de79049..cf32393b42 100644
--- a/src/main/target/CJMCU/target.h
+++ b/src/main/target/CJMCU/target.h
@@ -77,3 +77,6 @@
#define SKIP_CLI_COMMAND_HELP
#endif
+//#undef USE_CLI
+#define GTUNE
+//#define BLACKBOX
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index 279e2984d5..624350c49f 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/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
diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h
index d89c4d9b42..389acbbb04 100644
--- a/src/main/target/EUSTM32F103RC/target.h
+++ b/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
diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h
index b0d282f595..8be6707444 100644
--- a/src/main/target/IRCFUSIONF3/target.h
+++ b/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
diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h
index 3053df83b2..935657b67e 100644
--- a/src/main/target/LUX_RACE/target.h
+++ b/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
diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h
index b5d4f89bbf..f28ee90786 100644
--- a/src/main/target/MOTOLAB/target.h
+++ b/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
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index e83fd605ce..cb38ed09ca 100644
--- a/src/main/target/NAZE/target.h
+++ b/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
diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h
index 95a5679186..7cb85713d4 100644
--- a/src/main/target/NAZE32PRO/target.h
+++ b/src/main/target/NAZE32PRO/target.h
@@ -42,6 +42,7 @@
#define BLACKBOX
#define GPS
+#define GTUNE
#define SERIAL_RX
#define TELEMETRY
#define USE_SERVOS
diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h
index 3f7d1b48fd..07ce1e2b2c 100644
--- a/src/main/target/PORT103R/target.h
+++ b/src/main/target/PORT103R/target.h
@@ -151,6 +151,7 @@
#define BLACKBOX
#define GPS
+#define GTUNE
#define SERIAL_RX
#define TELEMETRY
#define USE_SERVOS
diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h
index 5e5f6b9f7e..35fb7cda5d 100644
--- a/src/main/target/RMDO/target.h
+++ b/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
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index f69ba2c959..829ed90456 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -120,6 +120,7 @@
#define BLACKBOX
#define GPS
+#define GTUNE
#define DISPLAY
#define SERIAL_RX
#define TELEMETRY
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index 7c9d401f5d..b47a4fb318 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/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
diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h
index 755b8396ff..1b07900aff 100644
--- a/src/main/target/STM32F3DISCOVERY/target.h
+++ b/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