From d36da111b22731706316fd80789483057f0744dc Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 6 Oct 2015 19:38:02 +0100 Subject: [PATCH] Delete autotune. --- Makefile | 2 +- README.md | 1 - docs/Autotune.md | 43 -- docs/Board - CC3D.md | 1 - docs/Getting Started.md | 1 - docs/Modes.md | 1 - docs/PID tuning.md | 2 +- src/main/blackbox/blackbox.c | 20 - src/main/blackbox/blackbox_fielddefs.h | 34 +- src/main/config/runtime_config.h | 2 +- src/main/flight/autotune.c | 501 ---------------------- src/main/flight/autotune.h | 28 -- src/main/flight/pid.c | 43 -- src/main/io/rc_controls.h | 2 +- src/main/io/serial_msp.c | 7 +- src/main/mw.c | 47 -- src/main/target/ALIENWIIF3/target.h | 1 - src/main/target/CC3D/target.h | 2 - src/main/target/CHEBUZZF3/target.h | 1 - src/main/target/COLIBRI_RACE/target.h | 1 - src/main/target/EUSTM32F103RC/target.h | 1 - src/main/target/NAZE/target.h | 1 - src/main/target/NAZE32PRO/target.h | 1 - src/main/target/OLIMEXINO/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 - src/main/telemetry/smartport.c | 2 +- 30 files changed, 8 insertions(+), 743 deletions(-) delete mode 100644 docs/Autotune.md delete mode 100644 src/main/flight/autotune.c delete mode 100644 src/main/flight/autotune.h diff --git a/Makefile b/Makefile index 85244e39e..bee0cbcba 100755 --- a/Makefile +++ b/Makefile @@ -265,7 +265,7 @@ COMMON_SRC = build_config.c \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) -HIGHEND_SRC = flight/autotune.c \ +HIGHEND_SRC = \ flight/navigation.c \ flight/gps_conversion.c \ common/colorconversion.c \ diff --git a/README.md b/README.md index 11286b7f2..bc0b04144 100644 --- a/README.md +++ b/README.md @@ -35,7 +35,6 @@ Cleanflight also has additional features not found in baseflight. * MSP Telemetry. * Smartport Telemetry. * RSSI via ADC - Uses ADC to read PWM RSSI signals, tested with FrSky D4R-II and X8R. -* Autotune - ported from BradWii, experimental - feedback welcomed. * OLED Displays - Display information on: Battery voltage, profile, rate profile, version, sensors, RC, etc. * In-flight manual PID tuning and rate adjustment. * Rate profiles and in-flight selection of them. diff --git a/docs/Autotune.md b/docs/Autotune.md deleted file mode 100644 index 73aceee88..000000000 --- a/docs/Autotune.md +++ /dev/null @@ -1,43 +0,0 @@ -# Autotune - -Autotune helps to automatically tune your multirotor. - -WARNING: Autotune is an experimental feature. Currently enough feedback has been gathered and we do not recommend that anyone uses it until this warning is removed. Autotune may be replaced by G-Tune, Please see https://github.com/cleanflight/cleanflight/pull/568 for details. - -## 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. Autotune does not support pid_controller 2 or higher (pid_controller 0 is the Cleanflight default, pid_controller 1 will work for autotune as well). - -Configure a two position switch on your transmitter to activate the AUTOTUNE mode. Autotune may be done in ether one of the both only, HORIZON or ANGLE mode (will then apply on both modes). - - -## 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. - -1. Launch the multirotor. - -1. Phase 1: ROLL/YAW autotune. -Turn on/hold the autotune switch on your transmitter for approx 5 seconds. You can observe roll left/right while a beep code sounds on the beeper, when turning off the autotune switch, PID settings will have been updated for ROLL and YAW. - -1. Stay in air and re-align your copter for the following PITCH/YAW autotune. - -1. Phase 2: PITCH/YAW autotune. -Turn on/hold the switch again for approx 5 seconds. You can observe pitch forwards/backwards while a beep code sounds on the beeper, when turning off the autotune switch, PID settings will have been updated for PITCH and YAW. - -1. Keep flying and see if it's better. If it's worse then while still armed flip the switch again to restore previous PIDs that were present prior to arming. You can do this while still flying or after landing. - -1. Land & disarm. If desired you may verify results via an app while battery power still on. Cutting the power will lose the new unsaved PIDs. - -1. If you're happy with the PIDs then disarm (but leave the battery still on). - -1. Flip the autotune switch again (copter still under battery power) to save all settings. -A beeper will sound indicating the settings are saved. - -1. Then flip it back (so you can arm again). - - -# References - -* Brad Quick for the initial Autotune algorithm in BradWii. diff --git a/docs/Board - CC3D.md b/docs/Board - CC3D.md index 9a93fd3d7..f313ea149 100644 --- a/docs/Board - CC3D.md +++ b/docs/Board - CC3D.md @@ -123,7 +123,6 @@ The OpenPilot bootloader code also allows the remaining section of flash to be r OpenPilot Ground Station (GCS) via USB without requiring a USB to uart adapter. The following features are not available: - * Autotune * Display * Sonar diff --git a/docs/Getting Started.md b/docs/Getting Started.md index b7d4a452f..b0836255f 100644 --- a/docs/Getting Started.md +++ b/docs/Getting Started.md @@ -99,7 +99,6 @@ Some advanced configurations and features are documented in the following pages, * [Profiles](Profiles.md) * [PID tuning](PID tuning.md) * [In-flight Adjustments](Inflight Adjustments.md) -* [Autotune](Autotune.md) * [Blackbox logging](Blackbox.md) * [Using a Sonar](Sonar.md) * [Spektrum Bind](Spektrum bind.md) diff --git a/docs/Modes.md b/docs/Modes.md index 115f273e0..75b8ca6d7 100644 --- a/docs/Modes.md +++ b/docs/Modes.md @@ -25,7 +25,6 @@ auxillary receiver channels and other events such as failsafe detection. | 18 | 17 | GOV | | | 19 | 18 | OSD | Enable/Disable On-Screen-Display (OSD) | | 20 | 19 | TELEMETRY | Enable telemetry via switch | -| 21 | 20 | AUTOTUNE | Autotune Pitch/Roll PIDs | | 22 | 21 | SONAR | Altitude hold mode (sonar sensor only) | | 26 | 25 | BLACKBOX | Enable BlackBox logging | diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 8c41f4ce8..63f8498cf 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -107,7 +107,7 @@ This controller has code that attempts to compensate for variations in the loopt don't have to be retuned when the looptime setting changes. There were initially some problems with horizon mode, and sluggishness in acro mode, that were recently fixed by -nebbian in v1.6.0. The autotune feature does not work on this controller, so don't try to autotune it. +nebbian in v1.6.0. It is the first PID Controller designed for 32-bit processors and not derived from MultiWii. diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2f7ac5174..bd7b482e8 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1134,26 +1134,6 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) case FLIGHT_LOG_EVENT_SYNC_BEEP: blackboxWriteUnsignedVB(data->syncBeep.time); break; - case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START: - blackboxWrite(data->autotuneCycleStart.phase); - blackboxWrite(data->autotuneCycleStart.cycle | (data->autotuneCycleStart.rising ? 0x80 : 0)); - blackboxWrite(data->autotuneCycleStart.p); - blackboxWrite(data->autotuneCycleStart.i); - blackboxWrite(data->autotuneCycleStart.d); - break; - case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT: - blackboxWrite(data->autotuneCycleResult.flags); - blackboxWrite(data->autotuneCycleStart.p); - blackboxWrite(data->autotuneCycleStart.i); - blackboxWrite(data->autotuneCycleStart.d); - break; - case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS: - blackboxWriteS16(data->autotuneTargets.currentAngle); - blackboxWrite((uint8_t) data->autotuneTargets.targetAngle); - blackboxWrite((uint8_t) data->autotuneTargets.targetAngleAtPeak); - blackboxWriteS16(data->autotuneTargets.firstPeakAngle); - blackboxWriteS16(data->autotuneTargets.secondPeakAngle); - break; case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT: if (data->inflightAdjustment.floatFlag) { blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 72ffa8148..4da938e3d 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -103,9 +103,6 @@ typedef enum FlightLogFieldSign { typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_SYNC_BEEP = 0, - FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, - FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11, - FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12, FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13, FLIGHT_LOG_EVENT_LOGGING_RESUME = 14, FLIGHT_LOG_EVENT_LOG_END = 255 @@ -115,32 +112,6 @@ typedef struct flightLogEvent_syncBeep_t { uint32_t time; } flightLogEvent_syncBeep_t; -typedef struct flightLogEvent_autotuneCycleStart_t { - uint8_t phase; - uint8_t cycle; - uint8_t p; - uint8_t i; - uint8_t d; - uint8_t rising; -} flightLogEvent_autotuneCycleStart_t; - -#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT 1 -#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT 2 -#define FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG 128 - -typedef struct flightLogEvent_autotuneCycleResult_t { - uint8_t flags; - uint8_t p; - uint8_t i; - uint8_t d; -} flightLogEvent_autotuneCycleResult_t; - -typedef struct flightLogEvent_autotuneTargets_t { - uint16_t currentAngle; - int8_t targetAngle, targetAngleAtPeak; - uint16_t firstPeakAngle, secondPeakAngle; -} flightLogEvent_autotuneTargets_t; - typedef struct flightLogEvent_inflightAdjustment_t { uint8_t adjustmentFunction; bool floatFlag; @@ -153,12 +124,11 @@ typedef struct flightLogEvent_loggingResume_t { uint32_t currentTime; } flightLogEvent_loggingResume_t; +#define FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG 128 + typedef union flightLogEventData_t { flightLogEvent_syncBeep_t syncBeep; - flightLogEvent_autotuneCycleStart_t autotuneCycleStart; - flightLogEvent_autotuneCycleResult_t autotuneCycleResult; - flightLogEvent_autotuneTargets_t autotuneTargets; flightLogEvent_inflightAdjustment_t inflightAdjustment; flightLogEvent_loggingResume_t loggingResume; } flightLogEventData_t; diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index c5a1bff6a..857ebea59 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -38,7 +38,7 @@ typedef enum { GPS_HOME_MODE = (1 << 4), GPS_HOLD_MODE = (1 << 5), HEADFREE_MODE = (1 << 6), - AUTOTUNE_MODE = (1 << 7), + UNUSED_MODE = (1 << 7), // old autotune PASSTHRU_MODE = (1 << 8), SONAR_MODE = (1 << 9), FAILSAFE_MODE = (1 << 10), diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c deleted file mode 100644 index 17f3a21e4..000000000 --- a/src/main/flight/autotune.c +++ /dev/null @@ -1,501 +0,0 @@ -/* - * 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" -#include "build_config.h" -#include "debug.h" - -#ifdef AUTOTUNE - -#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/acceleration.h" - -#include "flight/pid.h" -#include "flight/imu.h" - -#include "config/config.h" -#include "blackbox/blackbox.h" - -/* - * 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_ANGLE. 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 AUTOTUNE_MAX_OSCILLATION_ANGLE 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; - -typedef enum { - CYCLE_TUNE_I = 0, - CYCLE_TUNE_PD, - CYCLE_TUNE_PD2 -} autotuneCycle_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 autotuneCycle_e cycle; -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 firstPeakAngle, secondPeakAngle; // in degrees - -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; -} - -#ifdef BLACKBOX - -static void autotuneLogCycleStart() -{ - if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_autotuneCycleStart_t eventData; - - eventData.phase = phase; - eventData.cycle = cycle; - eventData.p = pid.p * MULTIWII_P_MULTIPLIER; - eventData.i = pid.i * MULTIWII_I_MULTIPLIER; - eventData.d = pid.d; - eventData.rising = rising ? 1 : 0; - - blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData); - } -} - -static void autotuneLogAngleTargets(float currentAngle) -{ - if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_autotuneTargets_t eventData; - - // targetAngle is always just -AUTOTUNE_TARGET_ANGLE or +AUTOTUNE_TARGET_ANGLE so no need for float precision: - eventData.targetAngle = (int) targetAngle; - // and targetAngleAtPeak is set to targetAngle so it has the same small precision requirement: - eventData.targetAngleAtPeak = (int) targetAngleAtPeak; - - // currentAngle is integer decidegrees divided by 10, so just reverse that process to get an integer again: - eventData.currentAngle = roundf(currentAngle * 10); - // the peak angles are only ever set to currentAngle, so they get the same treatment: - eventData.firstPeakAngle = roundf(firstPeakAngle * 10); - eventData.secondPeakAngle = roundf(secondPeakAngle * 10); - - blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS, (flightLogEventData_t*)&eventData); - } -} - -#endif - -static void startNewCycle(void) -{ - rising = !rising; - firstPeakAngle = secondPeakAngle = 0; - -#ifdef BLACKBOX - autotuneLogCycleStart(); -#endif -} - -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; - bool overshot; - - if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) { - return errorAngle; - } - - if (IS_PID_CONTROLLER_FP_BASED(pidController)) { - // TODO support floating point based pid controllers - return errorAngle; - } - - -#ifdef DEBUG_AUTOTUNE - debug[0] = 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]); - } - -#ifdef DEBUG_AUTOTUNE - debug[1] = DEGREES_TO_DECIDEGREES(currentAngle); - debug[2] = DEGREES_TO_DECIDEGREES(targetAngle); -#endif - -#ifdef BLACKBOX - autotuneLogAngleTargets(currentAngle); -#endif - - if (secondPeakAngle == 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 > firstPeakAngle) { - // we are still going up - firstPeakAngle = currentAngle; - targetAngleAtPeak = targetAngle; - -#ifdef DEBUG_AUTOTUNE - debug[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle); -#endif - - } else if (firstPeakAngle > 0) { - switch (cycle) { - case CYCLE_TUNE_I: - // when checking the I value, we would like to overshoot the target position by half of the max oscillation. - overshot = currentAngle - targetAngle >= AUTOTUNE_MAX_OSCILLATION_ANGLE / 2; - - if (overshot) { - pid.i *= AUTOTUNE_DECREASE_MULTIPLIER; - if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) { - pid.i = AUTOTUNE_MINIMUM_I_VALUE; - } - } else { - pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; - } - -#ifdef BLACKBOX - if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_autotuneCycleResult_t eventData; - - eventData.flags = overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT: 0; - eventData.p = pidProfile->P8[pidIndex]; - eventData.i = pidProfile->I8[pidIndex]; - eventData.d = pidProfile->D8[pidIndex]; - - blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData); - } -#endif - - // go back to checking P and D - cycle = CYCLE_TUNE_PD; - pidProfile->I8[pidIndex] = 0; - startNewCycle(); - break; - - case CYCLE_TUNE_PD: - case CYCLE_TUNE_PD2: - // we are checking P and D values - // set up to look for the 2nd peak - secondPeakAngle = currentAngle; - timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS; - break; - } - } - } else { - // We saw the first peak while tuning PD, looking for the second - - if (currentAngle < secondPeakAngle) { - secondPeakAngle = currentAngle; -#ifdef DEBUG_AUTOTUNE - debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle); -#endif - } - - float oscillationAmplitude = firstPeakAngle - secondPeakAngle; - - 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 > secondPeakAngle)) { - // analyze the data - // Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude - - overshot = firstPeakAngle > targetAngleAtPeak; - if (overshot) { -#ifdef DEBUG_AUTOTUNE - debug[0] = 1; -#endif - -#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 { -#ifdef DEBUG_AUTOTUNE - debug[0] = 2; -#endif - 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; - -#ifdef BLACKBOX - if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_autotuneCycleResult_t eventData; - - eventData.flags = (overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT : 0) | (timedOut ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT : 0); - eventData.p = pidProfile->P8[pidIndex]; - eventData.i = pidProfile->I8[pidIndex]; - eventData.d = pidProfile->D8[pidIndex]; - - blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData); - } -#endif - - if (cycle == CYCLE_TUNE_PD2) { - // switch to testing I value - cycle = CYCLE_TUNE_I; - - pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER; - } else { - cycle = CYCLE_TUNE_PD2; - } - - // switch to the other direction for the new cycle - startNewCycle(); - } - } - -#ifdef DEBUG_AUTOTUNE - if (angleIndex == AI_ROLL) { - debug[0] += 100; - } -#endif - - 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) -{ - 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; - cycle = CYCLE_TUNE_PD; - firstPeakAngle = secondPeakAngle = 0; - - pidProfile = pidProfileToTune; - pidController = pidProfile->pidController; - - 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; - -#ifdef BLACKBOX - autotuneLogCycleStart(); -#endif -} - -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; -} - -#endif diff --git a/src/main/flight/autotune.h b/src/main/flight/autotune.h deleted file mode 100644 index 8d4296e3c..000000000 --- a/src/main/flight/autotune.h +++ /dev/null @@ -1,28 +0,0 @@ -/* - * 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 autotuneReset(); -void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune); -float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle); -void autotuneEndPhase(); - -bool isAutotuneIdle(void); -bool hasAutotunePhaseCompleted(void); -bool havePidsBeenUpdatedByAutotune(void); - diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 899015ce4..856c01db5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -41,7 +41,6 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/navigation.h" -#include "flight/autotune.h" #include "flight/filter.h" #include "config/runtime_config.h" @@ -96,13 +95,6 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static filterStatePt1_t PTermState[3], DTermState[3]; -#ifdef AUTOTUNE -bool shouldAutotune(void) -{ - return ARMING_FLAG(ARMED) && FLIGHT_MODE(AUTOTUNE_MODE); -} -#endif - static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { @@ -158,12 +150,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif -#ifdef AUTOTUNE - if (shouldAutotune()) { - errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle); - } -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { // it's the ANGLE mode - control is angle based, so control loop is needed AngleRate = errorAngle * pidProfile->A_level; @@ -259,12 +245,6 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; #endif -#ifdef AUTOTUNE - if (shouldAutotune()) { - errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); - } -#endif - 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); @@ -367,12 +347,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; #endif -#ifdef AUTOTUNE - if (shouldAutotune()) { - errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); - } -#endif - errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result @@ -473,12 +447,6 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; #endif -#ifdef AUTOTUNE - if (shouldAutotune()) { - errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); - } -#endif - 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); @@ -597,11 +565,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; #endif -#ifdef AUTOTUNE - if (shouldAutotune()) { - error = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(error))); - } -#endif PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f; PTermACC = constrain(PTermACC, -limitf, +limitf); @@ -754,12 +717,6 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here #endif -#ifdef AUTOTUNE - if (shouldAutotune()) { - errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); - } -#endif - if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4; if (FLIGHT_MODE(HORIZON_MODE)) { diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 8be258648..dbe552cbe 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -41,7 +41,7 @@ typedef enum { BOXGOV, BOXOSD, BOXTELEMETRY, - BOXAUTOTUNE, + // BOXAUTOTUNE, BOXSONAR, BOXSERVO1, BOXSERVO2, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index f383acbf8..fa2a3c14f 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -340,7 +340,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXGOV, "GOVERNOR;", 18 }, { BOXOSD, "OSD SW;", 19 }, { BOXTELEMETRY, "TELEMETRY;", 20 }, - { BOXAUTOTUNE, "AUTOTUNE;", 21 }, + //{ BOXAUTOTUNE, "AUTOTUNE;", 21 }, { BOXSONAR, "SONAR;", 22 }, { BOXSERVO1, "SERVO1;", 23 }, { BOXSERVO2, "SERVO2;", 24 }, @@ -677,10 +677,6 @@ void mspInit(serialConfig_t *serialConfig) if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; -#ifdef AUTOTUNE - activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE; -#endif - if (feature(FEATURE_SONAR)){ activeBoxIds[activeBoxIdCount++] = BOXSONAR; } @@ -821,7 +817,6 @@ static bool processOutCommand(uint8_t cmdMSP) 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(BOXAUTOTUNE)) << BOXAUTOTUNE | 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 4174d0e17..2ce13d722 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -70,7 +70,6 @@ #include "flight/imu.h" #include "flight/altitudehold.h" #include "flight/failsafe.h" -#include "flight/autotune.h" #include "flight/navigation.h" #include "flight/filter.h" @@ -122,44 +121,6 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD saveConfigAndNotify(); } -#ifdef AUTOTUNE - -void updateAutotuneState(void) -{ - static bool landedAfterAutoTuning = false; - static bool autoTuneWasUsed = false; - - if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { - if (!FLIGHT_MODE(AUTOTUNE_MODE)) { - if (ARMING_FLAG(ARMED)) { - if (isAutotuneIdle() || landedAfterAutoTuning) { - autotuneReset(); - landedAfterAutoTuning = false; - } - autotuneBeginNextPhase(¤tProfile->pidProfile); - ENABLE_FLIGHT_MODE(AUTOTUNE_MODE); - autoTuneWasUsed = true; - } else { - if (havePidsBeenUpdatedByAutotune()) { - saveConfigAndNotify(); - autotuneReset(); - } - } - } - return; - } - - if (FLIGHT_MODE(AUTOTUNE_MODE)) { - autotuneEndPhase(); - DISABLE_FLIGHT_MODE(AUTOTUNE_MODE); - } - - if (!ARMING_FLAG(ARMED) && autoTuneWasUsed) { - landedAfterAutoTuning = true; - } -} -#endif - bool isCalibrating() { #ifdef BARO @@ -278,10 +239,6 @@ void annexCode(void) DISABLE_ARMING_FLAG(OK_TO_ARM); } - if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { - DISABLE_ARMING_FLAG(OK_TO_ARM); - } - if (isCalibrating()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); @@ -796,10 +753,6 @@ void loop(void) haveProcessedAnnexCodeOnce = true; #endif -#ifdef AUTOTUNE - updateAutotuneState(); -#endif - #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h index 70b3367ee..0011ea4a3 100644 --- a/src/main/target/ALIENWIIF3/target.h +++ b/src/main/target/ALIENWIIF3/target.h @@ -114,7 +114,6 @@ #define SERIAL_RX //#define GPS //#define DISPLAY -#define AUTOTUNE #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 7f0043d7e..5744a5c1d 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -112,13 +112,11 @@ #define TELEMETRY #define SERIAL_RX #define SONAR -#define AUTOTUNE #define USE_SERVOS #define USE_CLI #if defined(OPBL) // disabled some features for OPBL build due to code size. -#undef AUTOTUNE #undef DISPLAY #undef SONAR #define SKIP_CLI_COMMAND_HELP diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index fdb41f8f0..f7bca39e1 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -121,6 +121,5 @@ #define BLACKBOX #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #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 cb35d1074..924cdbf39 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -157,6 +157,5 @@ #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 4ff778d96..2786a9cb6 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -124,7 +124,6 @@ #define BLACKBOX #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 52d9c5955..836ce7aab 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -177,7 +177,6 @@ #define BLACKBOX #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h index 6b35128d3..1359fa140 100644 --- a/src/main/target/NAZE32PRO/target.h +++ b/src/main/target/NAZE32PRO/target.h @@ -44,7 +44,6 @@ #define BLACKBOX #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index 3a2adb0c7..d1a74851c 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -109,7 +109,6 @@ #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #define BLACKBOX #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index e1af6a738..d80b4360f 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -151,6 +151,5 @@ #define BLACKBOX #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 2cefb62e6..652a46e10 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -155,7 +155,6 @@ #define BLACKBOX #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #define DISPLAY #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 1c835d2b0..59c695f17 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -107,7 +107,6 @@ #define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_7 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_4 -#define AUTOTUNE #define BLACKBOX #define TELEMETRY #define SERIAL_RX diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 939316884..bb99a9479 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -160,7 +160,6 @@ #define BLACKBOX #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #define DISPLAY #define USE_SERVOS #define USE_CLI diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 886d33f27..5eb5f969e 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -95,6 +95,5 @@ #define LED_STRIP_TIMER TIM16 #define TELEMETRY #define SERIAL_RX -#define AUTOTUNE #define USE_SERVOS #define USE_CLI diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index a8fd1678d..ece79e4f2 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -421,7 +421,7 @@ void handleSmartPortTelemetry(void) tmpi += 10; if (FLIGHT_MODE(HORIZON_MODE)) tmpi += 20; - if (FLIGHT_MODE(AUTOTUNE_MODE)) + if (FLIGHT_MODE(UNUSED_MODE)) tmpi += 40; if (FLIGHT_MODE(PASSTHRU_MODE)) tmpi += 40;