From 4eb6456ea17dce4530f1b31e3a41ce2024e1ca20 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Feb 2016 14:06:10 +0100 Subject: [PATCH 1/2] Revert "Remove GTUNE" This reverts commit a16ac7fbe77956b7349b5a02299546d9615fa3b4. --- Makefile | 1 + src/main/blackbox/blackbox.c | 5 + src/main/blackbox/blackbox_fielddefs.h | 1 + src/main/config/config.c | 11 ++ src/main/config/runtime_config.h | 1 + src/main/flight/gtune.c | 211 ++++++++++++++++++++++ src/main/flight/gtune.h | 21 +++ src/main/flight/pid.c | 24 ++- src/main/flight/pid.h | 7 + src/main/io/i2c_bst.c | 4 +- src/main/io/serial_cli.c | 11 ++ src/main/io/serial_msp.c | 7 +- src/main/mw.c | 29 +++ src/main/target/ALIENFLIGHTF3/target.h | 3 + src/main/target/CHEBUZZF3/target.h | 1 + src/main/target/CJMCU/target.h | 3 + src/main/target/COLIBRI_RACE/target.h | 1 + src/main/target/EUSTM32F103RC/target.h | 1 + src/main/target/IRCFUSIONF3/target.h | 1 + src/main/target/LUX_RACE/target.h | 1 + src/main/target/MOTOLAB/target.h | 1 + src/main/target/NAZE/target.h | 2 + src/main/target/NAZE32PRO/target.h | 1 + src/main/target/PORT103R/target.h | 1 + src/main/target/RMDO/target.h | 1 + src/main/target/SPARKY/target.h | 1 + src/main/target/SPRACINGF3/target.h | 1 + src/main/target/STM32F3DISCOVERY/target.h | 1 + 28 files changed, 350 insertions(+), 3 deletions(-) create mode 100644 src/main/flight/gtune.c create mode 100644 src/main/flight/gtune.h diff --git a/Makefile b/Makefile index af2607438..ebbe6b583 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 ec7b4754b..65d575903 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 048dfae08..0bee570f2 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 614923e87..fa9ca0532 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 857ebea59..6b4dee82e 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 000000000..d32a93ad8 --- /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 000000000..f580c7c12 --- /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 5cb8d4399..c9a0c726f 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 665e187d5..98de99238 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 1b373134d..c31fcf3e5 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 b5fe24da9..8283c4197 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 3adf064b1..9d621c853 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 38d280466..7e8afbb49 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 6f89f8da9..121002a5e 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 211d10708..15b54d015 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 a54de7904..cf32393b4 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 279e2984d..624350c49 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 d89c4d9b4..389acbbb0 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 b0d282f59..8be670744 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 3053df83b..935657b67 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 b5d4f89bb..f28ee9078 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 e83fd605c..cb38ed09c 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 95a567918..7cb85713d 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 3f7d1b48f..07ce1e2b2 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 5e5f6b9f7..35fb7cda5 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 f69ba2c95..829ed9045 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 7c9d401f5..b47a4fb31 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 755b8396f..1b07900af 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 From f2075888a32eb47965dfeec0f4729371d06e7b02 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Feb 2016 14:10:42 +0100 Subject: [PATCH 2/2] Remove gtune on a gentle way --- src/main/target/ALIENFLIGHTF3/target.h | 2 +- src/main/target/COLIBRI_RACE/target.h | 2 +- src/main/target/IRCFUSIONF3/target.h | 2 +- src/main/target/LUX_RACE/target.h | 2 +- src/main/target/MOTOLAB/target.h | 2 +- src/main/target/NAZE/target.h | 2 +- src/main/target/RMDO/target.h | 2 +- src/main/target/SPARKY/target.h | 2 +- src/main/target/SPRACINGF3/target.h | 2 +- src/main/target/STM32F3DISCOVERY/target.h | 2 +- 10 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 121002a5e..79365c62e 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -153,7 +153,7 @@ //#define BLACKBOX #define SERIAL_RX //#define GPS -#define GTUNE +//#define GTUNE //#define DISPLAY #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 624350c49..0fec63c0c 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -153,7 +153,7 @@ #define BLACKBOX #define GPS -#define GTUNE +//#define GTUNE #define LED_STRIP #define LED_STRIP_TIMER TIM16 diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 8be670744..7f288779f 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -151,7 +151,7 @@ #define BLACKBOX #define DISPLAY #define GPS -#define GTUNE +//#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 935657b67..a184c1b46 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -129,7 +129,7 @@ #define BLACKBOX #define GPS -#define GTUNE +//#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 f28ee9078..9a9eedc50 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -122,7 +122,7 @@ #define BLACKBOX #define SERIAL_RX //#define GPS -#define GTUNE +//#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 cb38ed09c..0a09a7bfa 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -176,7 +176,7 @@ #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER //#define GPS -#define GTUNE +//#define GTUNE #define BLACKBOX #define TELEMETRY #define SERIAL_RX diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 35fb7cda5..eca7c340d 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -143,7 +143,7 @@ #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define BLACKBOX -#define GTUNE +//#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 829ed9045..2c94162b5 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -120,7 +120,7 @@ #define BLACKBOX #define GPS -#define GTUNE +//#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 b47a4fb31..822473821 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -152,7 +152,7 @@ #define BLACKBOX #define DISPLAY #define GPS -#define GTUNE +//#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 1b07900af..5796239f4 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -154,7 +154,7 @@ #define BLACKBOX #define GPS -#define GTUNE +//#define GTUNE #define LED_STRIP #define LED_STRIP_TIMER TIM16 #define TELEMETRY