mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 01:05:27 +03:00
Autotune phase changes.
Add an additional phase to allow previous pid settings to be restored, in flight, after auto tuning.
This commit is contained in:
Dominic Clifton
parent
f7c38af7fc
commit
4c764aacf4
5 changed files with 262 additions and 26 deletions
|
|
@ -1,38 +1,68 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "drivers/system_common.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.
|
||||
extern int16_t debug[4];
|
||||
|
||||
// To adjust how aggressive the tuning is, adjust the AUTOTUNEMAXOSCILLATION 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 1.0
|
||||
#define AUTOTUNE_TARGET_ANGLE 20.0
|
||||
#define AUTOTUNE_D_MULTIPLIER 1.2
|
||||
|
||||
#define AUTOTUNE_MAX_OSCILLATION 1.0f
|
||||
#define AUTOTUNE_TARGET_ANGLE 20.0f
|
||||
#define AUTOTUNE_D_MULTIPLIER 1.2f
|
||||
#define AUTOTUNE_SETTLING_DELAY_MS 250 // 1/4 of a second.
|
||||
|
||||
typedef enum {
|
||||
PHASE_IDLE = 0,
|
||||
PHASE_TUNE_ROLL,
|
||||
PHASE_TUNE_PITCH,
|
||||
PHASE_SAVE_OR_RESTORE_PIDS,
|
||||
} autotunePhase_e;
|
||||
|
||||
#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 bool rising;
|
||||
static float targetAngle = 0;
|
||||
|
||||
static uint8_t cycleCount; // TODO can we replace this with an enum to improve readability.
|
||||
static uint32_t timeoutAt;
|
||||
static angle_index_t autoTuneAngleIndex;
|
||||
static autotunePhase_e phase = PHASE_IDLE;
|
||||
static autotunePhase_e nextPhase = FIRST_TUNE_PHASE;
|
||||
|
||||
float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle)
|
||||
static float targetAngle = 0;
|
||||
static float targetAngleAtPeak;
|
||||
static float secondPeakAngle, firstPeakAngle; // deci dgrees, 180 deg = 1800
|
||||
|
||||
bool isAutotuneIdle(void)
|
||||
{
|
||||
if (autoTuneAngleIndex != angleIndex) {
|
||||
// Not tuning this angle yet.
|
||||
return errorAngle;
|
||||
return phase == PHASE_IDLE;
|
||||
}
|
||||
|
||||
// TODO autotune!
|
||||
static void startNewCycle(void)
|
||||
{
|
||||
rising = !rising;
|
||||
secondPeakAngle = firstPeakAngle = 0;
|
||||
}
|
||||
|
||||
static void updateTargetAngle(void)
|
||||
{
|
||||
if (rising) {
|
||||
targetAngle = AUTOTUNE_TARGET_ANGLE;
|
||||
}
|
||||
|
|
@ -40,28 +70,221 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
|
|||
targetAngle = -AUTOTUNE_TARGET_ANGLE;
|
||||
}
|
||||
|
||||
#if 0
|
||||
debug[2] = DEGREES_TO_DECIDEGREES(targetAngle);
|
||||
#endif
|
||||
}
|
||||
|
||||
return targetAngle - inclination->rawAngles[angleIndex];
|
||||
float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle)
|
||||
{
|
||||
if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) {
|
||||
return errorAngle;
|
||||
}
|
||||
|
||||
debug[0] = 0;
|
||||
|
||||
#if 0
|
||||
debug[1] = inclination->rawAngles[angleIndex];
|
||||
#endif
|
||||
|
||||
float currentAngle;
|
||||
|
||||
if (rising) {
|
||||
currentAngle = DECIDEGREES_TO_DEGREES(inclination->rawAngles[angleIndex]);
|
||||
} else {
|
||||
targetAngle = -targetAngle;
|
||||
currentAngle = DECIDEGREES_TO_DEGREES(-inclination->rawAngles[angleIndex]);
|
||||
}
|
||||
|
||||
#if 1
|
||||
debug[1] = DEGREES_TO_DECIDEGREES(currentAngle);
|
||||
debug[2] = DEGREES_TO_DECIDEGREES(targetAngle);
|
||||
#endif
|
||||
|
||||
if (firstPeakAngle == 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 > secondPeakAngle) {
|
||||
// we are still going up
|
||||
secondPeakAngle = currentAngle;
|
||||
targetAngleAtPeak = targetAngle;
|
||||
|
||||
debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle);
|
||||
|
||||
} else if (secondPeakAngle > 0) {
|
||||
if (cycleCount == 0) {
|
||||
// when checking the I value, we would like to overshoot the target position by half of the max oscillation.
|
||||
// if (currentangle-targetangle<(FPAUTOTUNEMAXOSCILLATION>>1)) {
|
||||
// currentivalueshifted=lib_fp_multiply(currentivalueshifted,AUTOTUNEINCREASEMULTIPLIER);
|
||||
// } else {
|
||||
// currentivalueshifted=lib_fp_multiply(currentivalueshifted,AUTOTUNEDECREASEMULTIPLIER);
|
||||
// if (currentivalueshifted<AUTOTUNEMINIMUMIVALUE) currentivalueshifted=AUTOTUNEMINIMUMIVALUE;
|
||||
// }
|
||||
|
||||
// go back to checking P and D
|
||||
cycleCount = 1;
|
||||
// usersettings.pid_igain[autotuneindex]=0;
|
||||
startNewCycle();
|
||||
} else {
|
||||
// we are checking P and D values
|
||||
// set up to look for the 2nd peak
|
||||
firstPeakAngle = currentAngle;
|
||||
timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// we saw the first peak. looking for the second
|
||||
|
||||
if (currentAngle < firstPeakAngle) {
|
||||
firstPeakAngle = currentAngle;
|
||||
debug[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle);
|
||||
}
|
||||
|
||||
float oscillationAmplitude = secondPeakAngle - firstPeakAngle;
|
||||
|
||||
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 / 2 && currentAngle > firstPeakAngle)) {
|
||||
// analyze the data
|
||||
// Our goal is to have zero overshoot and to have AUTOTUNEMAXOSCILLATION amplitude
|
||||
|
||||
|
||||
if (secondPeakAngle > targetAngleAtPeak) {
|
||||
// overshot
|
||||
debug[0] = 1;
|
||||
|
||||
#ifdef PREFER_HIGH_GAIN_SOLUTION
|
||||
if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION) {
|
||||
// we have too much oscillation, so we can't increase D, so decrease P
|
||||
#endif
|
||||
// decrease P
|
||||
//currentpvalueshifted=lib_fp_multiply(currentpvalueshifted, AUTOTUNEDECREASEMULTIPLIER); // TODO
|
||||
|
||||
#ifdef PREFER_HIGH_GAIN_SOLUTION
|
||||
} else {
|
||||
// we don't have too much oscillation, so we can increase D"
|
||||
#endif
|
||||
|
||||
// increase D
|
||||
//currentdvalueshifted=lib_fp_multiply(currentdvalueshifted, AUTOTUNEINCREASEMULTIPLIER); // TODO
|
||||
#ifdef PREFER_HIGH_GAIN_SOLUTION
|
||||
}
|
||||
#endif
|
||||
} else {
|
||||
// undershot
|
||||
debug[0] = 2;
|
||||
|
||||
if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION) {
|
||||
// we have too much oscillation, so we should lower D
|
||||
//currentdvalueshifted=lib_fp_multiply(currentdvalueshifted, AUTOTUNEDECREASEMULTIPLIER); // TODO
|
||||
} else {
|
||||
// we don't have too much oscillation, so we increase P
|
||||
//currentpvalueshifted=lib_fp_multiply(currentpvalueshifted, AUTOTUNEINCREASEMULTIPLIER); // TODO
|
||||
}
|
||||
}
|
||||
|
||||
//usersettings.pid_pgain[autotuneindex]=currentpvalueshifted>>AUTOTUNESHIFT; // TODO
|
||||
//usersettings.pid_dgain[autotuneindex]=currentdvalueshifted>>AUTOTUNESHIFT; // TODO
|
||||
|
||||
// switch to the other direction and start a new cycle
|
||||
startNewCycle();
|
||||
|
||||
if (++cycleCount == 3) {
|
||||
// switch to testing I value
|
||||
cycleCount = 0;
|
||||
|
||||
//usersettings.pid_igain[autotuneindex]=currentivalueshifted>>AUTOTUNESHIFT; // TODO
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (angleIndex == AI_ROLL) {
|
||||
debug[0] += 100;
|
||||
}
|
||||
|
||||
updateTargetAngle();
|
||||
|
||||
return targetAngle - DECIDEGREES_TO_DEGREES(inclination->rawAngles[angleIndex]);
|
||||
}
|
||||
|
||||
void autotuneReset(void)
|
||||
{
|
||||
targetAngle = 0;
|
||||
rising = true;
|
||||
autoTuneAngleIndex = AI_ROLL;
|
||||
phase = PHASE_IDLE;
|
||||
nextPhase = FIRST_TUNE_PHASE;
|
||||
}
|
||||
|
||||
void autotuneBegin(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse)
|
||||
void backupPids(pidProfile_t *pidProfileToTune)
|
||||
{
|
||||
autotuneReset();
|
||||
memcpy(&pidBackup, pidProfileToTune, sizeof(pidBackup));
|
||||
}
|
||||
|
||||
void restorePids(pidProfile_t *pidProfileToTune)
|
||||
{
|
||||
memcpy(pidProfileToTune, &pidBackup, sizeof(pidBackup));
|
||||
}
|
||||
|
||||
void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse)
|
||||
{
|
||||
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;
|
||||
cycleCount = 1;
|
||||
secondPeakAngle = firstPeakAngle = 0;
|
||||
|
||||
pidProfile = pidProfileToTune;
|
||||
pidController = pidControllerInUse;
|
||||
|
||||
updateTargetAngle();
|
||||
|
||||
// TODO
|
||||
// currentpvalueshifted=usersettings.pid_pgain[autotuneindex]<<AUTOTUNESHIFT;
|
||||
// currentivalueshifted=usersettings.pid_igain[autotuneindex]<<AUTOTUNESHIFT;
|
||||
// // divide by D multiplier to get our working value. We'll multiply by D multiplier when we are done.
|
||||
// usersettings.pid_dgain[autotuneindex]=lib_fp_multiply(usersettings.pid_dgain[autotuneindex],FPONEOVERAUTOTUNE_D_MULTIPLIER);
|
||||
// currentdvalueshifted=usersettings.pid_dgain[autotuneindex]<<AUTOTUNESHIFT;
|
||||
//
|
||||
// usersettings.pid_igain[autotuneindex]=0;
|
||||
|
||||
}
|
||||
|
||||
void autotuneEnd(void)
|
||||
void autotuneEndPhase(void)
|
||||
{
|
||||
// TODO
|
||||
// usersettings.pid_igain[autotuneindex]=currentivalueshifted>>AUTOTUNESHIFT;
|
||||
//
|
||||
// // multiply by D multiplier. The best D is usually a little higher than what the algroithm produces.
|
||||
// usersettings.pid_dgain[autotuneindex]=lib_fp_multiply(currentdvalueshifted,FPAUTOTUNE_D_MULTIPLIER)>>AUTOTUNESHIFT;
|
||||
//
|
||||
// usersettings.pid_igain[YAWINDEX]=usersettings.pid_igain[ROLLINDEX];
|
||||
// usersettings.pid_dgain[YAWINDEX]=usersettings.pid_dgain[ROLLINDEX];
|
||||
// usersettings.pid_pgain[YAWINDEX]=lib_fp_multiply(usersettings.pid_pgain[ROLLINDEX],YAWGAINMULTIPLIER);
|
||||
|
||||
if (phase == AUTOTUNE_PHASE_MAX) {
|
||||
phase = PHASE_IDLE;
|
||||
nextPhase = FIRST_TUNE_PHASE;
|
||||
} else {
|
||||
nextPhase++;
|
||||
}
|
||||
}
|
||||
|
||||
bool havePidsBeenUpdatedByAutotune(void)
|
||||
|
|
|
|||
|
|
@ -1,9 +1,11 @@
|
|||
#pragma once
|
||||
|
||||
void autotuneReset();
|
||||
void autotuneBegin(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse);
|
||||
void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse);
|
||||
float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle);
|
||||
void autotuneEnd();
|
||||
void autotuneEndPhase();
|
||||
|
||||
bool isAutotuneIdle(void);
|
||||
bool hasAutotunePhaseCompleted(void);
|
||||
bool havePidsBeenUpdatedByAutotune(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -135,6 +135,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
|
|||
}
|
||||
|
||||
}
|
||||
|
||||
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
|
||||
{
|
||||
|
|
@ -155,7 +156,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
+max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis];
|
||||
|
||||
if (shouldAutotune()) {
|
||||
errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle);
|
||||
errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(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
|
||||
|
|
@ -223,7 +224,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
|||
+max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here
|
||||
|
||||
if (shouldAutotune()) {
|
||||
errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle);
|
||||
errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
|
||||
}
|
||||
|
||||
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
|
||||
|
|
|
|||
|
|
@ -98,6 +98,9 @@ typedef union {
|
|||
} rollAndPitchInclination_t;
|
||||
|
||||
|
||||
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
|
||||
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f)
|
||||
|
||||
extern rollAndPitchInclination_t inclination;
|
||||
|
||||
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
|
||||
|
|
|
|||
13
src/mw.c
13
src/mw.c
|
|
@ -84,7 +84,10 @@ void updateAutotuneState(void)
|
|||
if (rcOptions[BOXAUTOTUNE]) {
|
||||
if (!f.AUTOTUNE_MODE) {
|
||||
if (f.ARMED) {
|
||||
autotuneBegin(¤tProfile.pidProfile, currentProfile.pidController);
|
||||
if (isAutotuneIdle()) {
|
||||
autotuneReset();
|
||||
}
|
||||
autotuneBeginNextPhase(¤tProfile.pidProfile, currentProfile.pidController);
|
||||
f.AUTOTUNE_MODE = 1;
|
||||
} else {
|
||||
if (havePidsBeenUpdatedByAutotune()) {
|
||||
|
|
@ -98,7 +101,7 @@ void updateAutotuneState(void)
|
|||
}
|
||||
|
||||
if (f.AUTOTUNE_MODE) {
|
||||
autotuneEnd();
|
||||
autotuneEndPhase();
|
||||
f.AUTOTUNE_MODE = 0;
|
||||
}
|
||||
}
|
||||
|
|
@ -231,7 +234,11 @@ void annexCode(void)
|
|||
|
||||
f.OK_TO_ARM = 1;
|
||||
|
||||
if (!f.ARMED && !f.SMALL_ANGLE) {
|
||||
if (!f.SMALL_ANGLE) {
|
||||
f.OK_TO_ARM = 0;
|
||||
}
|
||||
|
||||
if (rcOptions[BOXAUTOTUNE]) {
|
||||
f.OK_TO_ARM = 0;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue