rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Delete autotune.

This commit is contained in:
Dominic Clifton 2015-10-06 19:38:02 +01:00
parent 6e59eb235d
commit d36da111b2
30 changed files with 8 additions and 743 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -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 \

1
README.md
View File

@ -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.

43
docs/Autotune.md
View File

@ -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.

1
docs/Board - CC3D.md
View File

@ -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

1
docs/Getting Started.md
View File

@ -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)

1
docs/Modes.md
View File

@ -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 |

2
docs/PID tuning.md
View File

@ -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.

20
src/main/blackbox/blackbox.c
View File

@ -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);

34
src/main/blackbox/blackbox_fielddefs.h
View File

@ -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;

2
src/main/config/runtime_config.h
View File

@ -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),

501
src/main/flight/autotune.c
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#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

28
src/main/flight/autotune.h
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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);

43
src/main/flight/pid.c
View File

@ -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)) {

2
src/main/io/rc_controls.h
View File

@ -41,7 +41,7 @@ typedef enum {
BOXGOV,
BOXOSD,
BOXTELEMETRY,
BOXAUTOTUNE,
// BOXAUTOTUNE,
BOXSONAR,
BOXSERVO1,
BOXSERVO2,

7
src/main/io/serial_msp.c
View File

@ -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 |

47
src/main/mw.c
View File

@ -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(&currentProfile->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();

1
src/main/target/ALIENWIIF3/target.h
View File

@ -114,7 +114,6 @@
#define SERIAL_RX
//#define GPS
//#define DISPLAY
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

2
src/main/target/CC3D/target.h
View File

@ -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

1
src/main/target/CHEBUZZF3/target.h
View File

@ -121,6 +121,5 @@
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

1
src/main/target/COLIBRI_RACE/target.h
View File

@ -157,6 +157,5 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

1
src/main/target/EUSTM32F103RC/target.h
View File

@ -124,7 +124,6 @@
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

1
src/main/target/NAZE/target.h
View File

@ -177,7 +177,6 @@
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

1
src/main/target/NAZE32PRO/target.h
View File

@ -44,7 +44,6 @@
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

1
src/main/target/OLIMEXINO/target.h
View File

@ -109,7 +109,6 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define BLACKBOX
#define USE_SERVOS
#define USE_CLI

1
src/main/target/PORT103R/target.h
View File

@ -151,6 +151,5 @@
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

1
src/main/target/RMDO/target.h
View File

@ -155,7 +155,6 @@
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define DISPLAY
#define USE_SERVOS
#define USE_CLI

1
src/main/target/SPARKY/target.h
View File

@ -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

1
src/main/target/SPRACINGF3/target.h
View File

@ -160,7 +160,6 @@
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define DISPLAY
#define USE_SERVOS
#define USE_CLI

1
src/main/target/STM32F3DISCOVERY/target.h
View File

@ -95,6 +95,5 @@
#define LED_STRIP_TIMER TIM16
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

2
src/main/telemetry/smartport.c
View File

@ -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;