From f7c38af7fc04360edd457382b8c67bc38a616ce4 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 27 May 2014 00:33:28 +0100 Subject: [PATCH] Initial commit for implementation of autotune. --- Makefile | 1 + docs/Autotune.md | 46 ++++++++++++++++++++++++++++ src/config.c | 2 +- src/flight_autotune.c | 71 +++++++++++++++++++++++++++++++++++++++++++ src/flight_autotune.h | 9 ++++++ src/flight_common.c | 21 +++++++++++++ src/flight_common.h | 4 +-- src/mw.c | 27 ++++++++++++++++ src/runtime_config.h | 2 ++ src/serial_msp.c | 4 +++ 10 files changed, 184 insertions(+), 3 deletions(-) create mode 100644 docs/Autotune.md create mode 100644 src/flight_autotune.c create mode 100644 src/flight_autotune.h diff --git a/Makefile b/Makefile index ab782e43a..1df847528 100644 --- a/Makefile +++ b/Makefile @@ -112,6 +112,7 @@ COMMON_SRC = build_config.c \ drivers/serial_common.c \ drivers/sound_beeper.c \ drivers/system_common.c \ + flight_autotune.c \ flight_common.c \ flight_imu.c \ flight_mixer.c \ diff --git a/docs/Autotune.md b/docs/Autotune.md new file mode 100644 index 000000000..305e21209 --- /dev/null +++ b/docs/Autotune.md @@ -0,0 +1,46 @@ +# Autotune + +Autotune helps to automatically tune your multirotor. + +## Configuration. + +Autotune only works in HORIZON or ANGLE mode. + +Configure a switch on your transmitter to activate the AUTOTUNE and HORIZON or ANGLE modes using the auxilary configuration. + +## Using autotuning + +Launch the multirotor. + +Flip the autotune switch on your transmitter. + +Observe roll/left/right a few times. + +PID settings will have been updated for ROLL/YAW. + +Disable the switch while still flying. + +Enable the switch again. + +Observe pitch forwards/backwards a few times. + +PID settings will have been updated for PITCH/YAW. + +Disable the switch again. + +PIDS are tunes, fly and see if it's better. + +Land. + +Verify results via an app while power still applied if desired. + +Cutting the power will loose the unsaved pids. + +If you're happy with the pids then while disarmed flip the autotune switch again to save all settings. + +A beeper will sound indicating the settings are saved. + + +# References + +* Brad Quick for the initial Autotune algorithm in BradWii. \ No newline at end of file diff --git a/src/config.c b/src/config.c index 0043eb967..091320627 100755 --- a/src/config.c +++ b/src/config.c @@ -58,7 +58,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon #define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) // use the last flash pages for storage master_t masterConfig; // master config struct with data independent from profiles profile_t currentProfile; // profile config struct -static const uint8_t EEPROM_CONF_VERSION = 69; +static const uint8_t EEPROM_CONF_VERSION = 70; static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims) { diff --git a/src/flight_autotune.c b/src/flight_autotune.c new file mode 100644 index 000000000..46a8e27d8 --- /dev/null +++ b/src/flight_autotune.c @@ -0,0 +1,71 @@ +#include +#include +#include + +#include "common/axis.h" +#include "common/maths.h" + +#include "flight_common.h" + +// To adjust how agressive the tuning is, adjust the AUTOTUNEMAXOSCILLATION value. A larger +// value will result in more agressive 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 1.0 +#define AUTOTUNE_TARGET_ANGLE 20.0 +#define AUTOTUNE_D_MULTIPLIER 1.2 + +static pidProfile_t *pidProfile; +static uint8_t pidController; +static bool rising; +static float targetAngle = 0; + +static angle_index_t autoTuneAngleIndex; + +float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle) +{ + if (autoTuneAngleIndex != angleIndex) { + // Not tuning this angle yet. + return errorAngle; + } + + // TODO autotune! + + if (rising) { + targetAngle = AUTOTUNE_TARGET_ANGLE; + } + else { + targetAngle = -AUTOTUNE_TARGET_ANGLE; + } + + + return targetAngle - inclination->rawAngles[angleIndex]; +} + +void autotuneReset(void) +{ + targetAngle = 0; + rising = true; + autoTuneAngleIndex = AI_ROLL; +} + +void autotuneBegin(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse) +{ + autotuneReset(); + + pidProfile = pidProfileToTune; + pidController = pidControllerInUse; +} + +void autotuneEnd(void) +{ + +} + +bool havePidsBeenUpdatedByAutotune(void) +{ + return targetAngle != 0; +} + diff --git a/src/flight_autotune.h b/src/flight_autotune.h new file mode 100644 index 000000000..b3aa65c38 --- /dev/null +++ b/src/flight_autotune.h @@ -0,0 +1,9 @@ +#pragma once + +void autotuneReset(); +void autotuneBegin(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse); +float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle); +void autotuneEnd(); + +bool havePidsBeenUpdatedByAutotune(void); + diff --git a/src/flight_common.c b/src/flight_common.c index 8fc361881..398321a0b 100644 --- a/src/flight_common.c +++ b/src/flight_common.c @@ -12,6 +12,7 @@ #include "sensors_gyro.h" #include "rc_controls.h" #include "flight_common.h" +#include "flight_autotune.h" #include "gps_common.h" extern uint16_t cycleTime; @@ -55,6 +56,12 @@ void resetErrorGyro(void) errorGyroIf[YAW] = 0; } +angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; + +bool shouldAutotune(void) +{ + return f.ARMED && f.AUTOTUNE_MODE; +} static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) @@ -77,6 +84,11 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control } else { // calculate error and limit the angle to 50 degrees max inclination errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - inclination.rawAngles[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here + + if (shouldAutotune()) { + errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle); + } + if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate if (f.HORIZON_MODE) { @@ -141,6 +153,11 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // 50 degrees max inclination errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; + + if (shouldAutotune()) { + errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle); + } + 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); @@ -205,6 +222,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here + if (shouldAutotune()) { + errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle); + } + 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; if (f.HORIZON_MODE) { diff --git a/src/flight_common.h b/src/flight_common.h index 31b2576cd..f5767a16b 100644 --- a/src/flight_common.h +++ b/src/flight_common.h @@ -27,7 +27,7 @@ typedef struct pidProfile_s { float H_level; } pidProfile_t; -enum { +typedef enum { AI_ROLL = 0, AI_PITCH, } angle_index_t; @@ -35,7 +35,7 @@ enum { #define ANGLE_INDEX_COUNT 2 // See http://en.wikipedia.org/wiki/Flight_dynamics -enum { +typedef enum { FD_ROLL = 0, FD_PITCH, FD_YAW diff --git a/src/mw.c b/src/mw.c index 0d5f359a8..b738c7bde 100755 --- a/src/mw.c +++ b/src/mw.c @@ -22,6 +22,7 @@ #include "failsafe.h" #include "flight_imu.h" #include "flight_common.h" +#include "flight_autotune.h" #include "flight_mixer.h" #include "gimbal.h" #include "gps_common.h" @@ -78,6 +79,30 @@ bool AccInflightCalibrationSavetoEEProm = false; bool AccInflightCalibrationActive = false; uint16_t InflightcalibratingA = 0; +void updateAutotuneState(void) +{ + if (rcOptions[BOXAUTOTUNE]) { + if (!f.AUTOTUNE_MODE) { + if (f.ARMED) { + autotuneBegin(¤tProfile.pidProfile, currentProfile.pidController); + f.AUTOTUNE_MODE = 1; + } else { + if (havePidsBeenUpdatedByAutotune()) { + //writeEEPROM(); + blinkLedAndSoundBeeper(5, 50, 1); + autotuneReset(); + } + } + } + return; + } + + if (f.AUTOTUNE_MODE) { + autotuneEnd(); + f.AUTOTUNE_MODE = 0; + } +} + bool isCalibrating() { #ifdef BARO @@ -603,6 +628,8 @@ void loop(void) cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; + updateAutotuneState(); + #ifdef MAG if (sensors(SENSOR_MAG)) { if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) { diff --git a/src/runtime_config.h b/src/runtime_config.h index 2d8b37aeb..e64c8b5a3 100644 --- a/src/runtime_config.h +++ b/src/runtime_config.h @@ -22,6 +22,7 @@ enum { BOXGOV, BOXOSD, BOXTELEMETRY, + BOXAUTOTUNE, CHECKBOX_ITEM_COUNT }; @@ -46,6 +47,7 @@ typedef struct flags_t { uint8_t CALIBRATE_MAG; uint8_t VARIO_MODE; uint8_t FIXED_WING; // set when in flying_wing or airplane mode. currently used by althold selection code + uint8_t AUTOTUNE_MODE; } flags_t; extern flags_t f; diff --git a/src/serial_msp.c b/src/serial_msp.c index 847bab7ab..f19b28962 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -139,6 +139,7 @@ struct box_t { { BOXGOV, "GOVERNOR;", 18 }, { BOXOSD, "OSD SW;", 19 }, { BOXTELEMETRY, "TELEMETRY;", 20 }, + { BOXAUTOTUNE, "AUTOTUNE;", 21 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -367,6 +368,8 @@ void mspInit(serialConfig_t *serialConfig) if (feature(FEATURE_TELEMETRY && masterConfig.telemetryConfig.telemetry_switch)) availableBoxes[idx++] = BOXTELEMETRY; + availableBoxes[idx++] = BOXAUTOTUNE; + numberBoxItems = idx; openAllMSPSerialPorts(serialConfig); @@ -513,6 +516,7 @@ static void evaluateCommand(void) rcOptions[BOXGOV] << BOXGOV | rcOptions[BOXOSD] << BOXOSD | rcOptions[BOXTELEMETRY] << BOXTELEMETRY | + rcOptions[BOXAUTOTUNE] << BOXAUTOTUNE | f.ARMED << BOXARM; for (i = 0; i < numberBoxItems; i++) { int flag = (tmp & (1 << availableBoxes[i]));