diff --git a/Makefile b/Makefile index 9aa6c8668c..3f8ff67b29 100644 --- a/Makefile +++ b/Makefile @@ -127,7 +127,6 @@ COMMON_SRC = build_config.c \ common/typeconversion.c \ main.c \ mw.c \ - flight/autotune.c \ flight/altitudehold.c \ flight/failsafe.c \ flight/flight.c \ @@ -156,7 +155,8 @@ COMMON_SRC = build_config.c \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) -HIGHEND_SRC = io/gps.c \ +HIGHEND_SRC = flight/autotune.c \ + io/gps.c \ io/gps_conversion.c \ io/ledstrip.c \ rx/sbus.c \ diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 769365da04..e0b8c883b0 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -20,6 +20,10 @@ #include #include +#include "platform.h" + +#ifdef AUTOTUNE + #include "common/axis.h" #include "common/maths.h" @@ -384,3 +388,4 @@ bool havePidsBeenUpdatedByAutotune(void) return targetAngle != 0; } +#endif diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 355f7f184a..47a574c824 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -76,10 +76,12 @@ void resetErrorGyro(void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; +#ifdef AUTOTUNE bool shouldAutotune(void) { return f.ARMED && f.AUTOTUNE_MODE; } +#endif static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) @@ -109,9 +111,12 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination), +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 (f.ANGLE_MODE) { // it's the ANGLE mode - control is angle based, so control loop is needed @@ -186,9 +191,11 @@ 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); @@ -259,9 +266,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), +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 (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID AngleRateTmp = ((int32_t)(controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4; diff --git a/src/main/mw.c b/src/main/mw.c index 404f02f093..094d683b39 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -100,6 +100,8 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD saveAndReloadCurrentProfileToCurrentProfileSlot(); } +#ifdef AUTOTUNE + void updateAutotuneState(void) { static bool landedAfterAutoTuning = false; @@ -134,6 +136,7 @@ void updateAutotuneState(void) landedAfterAutoTuning = true; } } +#endif bool isCalibrating() { @@ -590,7 +593,10 @@ void loop(void) #ifdef BARO haveProcessedAnnexCodeOnce = true; #endif + +#ifdef AUTOTUNE updateAutotuneState(); +#endif #ifdef MAG if (sensors(SENSOR_MAG)) { diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 3932a41d1a..f9278159d1 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -34,3 +34,4 @@ #define TELEMETRY #define SOFT_SERIAL #define SERIAL_RX +#define AUTOTUNE diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index aea15d759b..0b587ac5ba 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -49,3 +49,4 @@ #define TELEMETRY #define SOFT_SERIAL #define SERIAL_RX +#define AUTOTUNE diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 0cef23144b..fabc8efdcb 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -55,3 +55,4 @@ #define TELEMETRY #define SOFT_SERIAL #define SERIAL_RX +#define AUTOTUNE diff --git a/src/main/target/NAZE32PRO/target.h b/src/main/target/NAZE32PRO/target.h index b3afeb2067..27d9a39ea9 100644 --- a/src/main/target/NAZE32PRO/target.h +++ b/src/main/target/NAZE32PRO/target.h @@ -39,3 +39,4 @@ #define TELEMETRY #define SOFT_SERIAL #define SERIAL_RX +#define AUTOTUNE diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index ffcd87f208..61b49fa942 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -53,3 +53,5 @@ #define TELEMETRY #define SOFT_SERIAL #define SERIAL_RX +#define AUTOTUNE + diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 7b566bb97c..f9cd353714 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -51,3 +51,4 @@ #define TELEMETRY #define SOFT_SERIAL #define SERIAL_RX +#define AUTOTUNE