Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

FW autotune improvements

Browse source
This commit is contained in:
Alexander van Saase 2021-02-15 23:23:59 +01:00
parent e5dae31be1
commit 06ade636fb
6 changed files with 210 additions and 99 deletions
Download patch file Download diff file Expand all files Collapse all files

9
docs/Settings.md
View file

@ -109,11 +109,14 @@
| frsky_pitch_roll | OFF | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | | frsky_pitch_roll | OFF | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data |
| frsky_unit | METRIC | Not used? [METRIC/IMPERIAL] | | frsky_unit | METRIC | Not used? [METRIC/IMPERIAL] |
| frsky_vfas_precision | 0 | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method | | frsky_vfas_precision | 0 | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method |
| fw_autotune_convergence_rate | 10 | How much autotune adjusts the gains on each update. Setting this too high can cause erratically changing gains during autotune. |
| fw_autotune_detect_time | 250 | Time [ms] to detect sustained undershoot or overshoot |
| fw_autotune_ff_to_i_tc | 600 | FF to I time (defines time for I to reach the same level of response as FF) [ms] | | fw_autotune_ff_to_i_tc | 600 | FF to I time (defines time for I to reach the same level of response as FF) [ms] |
| fw_autotune_ff_to_p_gain | 10 | FF to P gain (strength relationship) [%] | | fw_autotune_ff_to_p_gain | 10 | FF to P gain (strength relationship) [%] |
| fw_autotune_overshoot_time | 100 | Time [ms] to detect sustained overshoot | | fw_autotune_max_rate_deflection | 80 | The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. |
| fw_autotune_threshold | 50 | Threshold [%] of max rate to consider overshoot/undershoot detection | | fw_autotune_min_stick | 80 | Minimum stick input [%] to consider overshoot/undershoot detection |
| fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot | | fw_autotune_p_to_d_gain | 0 | P to D gain (strength relationship) [%] |
| fw_autotune_rate_adjustment | AUTO | `AUTO` and `LIMIT` adjust the rates to match the capabilities of the airplane, with `LIMIT` they are never increased above the starting rates setting. `FIXED` does not adjust the rates. Rates are not changed when tuning in `ANGLE` mode. |
| fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point | | fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point |
| fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH | | fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH |
| fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for ROLL | | fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for ROLL |

1
src/main/build/debug.h
View file

@ -83,5 +83,6 @@ typedef enum {
DEBUG_FW_D, DEBUG_FW_D,
DEBUG_IMU2, DEBUG_IMU2,
DEBUG_ALTITUDE, DEBUG_ALTITUDE,
DEBUG_AUTOTUNE,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

51
src/main/fc/settings.yaml
View file

@ -87,7 +87,7 @@ tables:
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
"IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE"] "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE", "AUTOTUNE"]
- name: async_mode - name: async_mode
values: ["NONE", "GYRO", "ALL"] values: ["NONE", "GYRO", "ALL"]
- name: aux_operator - name: aux_operator
@ -163,6 +163,9 @@ tables:
values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"]
- name: osd_plus_code_short - name: osd_plus_code_short
values: ["0", "2", "4", "6"] values: ["0", "2", "4", "6"]
- name: autotune_rate_adjustment
enum: autotune_rate_adjustment_e
values: ["FIXED", "LIMIT", "AUTO"]
groups: groups:
- name: PG_GYRO_CONFIG - name: PG_GYRO_CONFIG
@ -2049,22 +2052,16 @@ groups:
type: pidAutotuneConfig_t type: pidAutotuneConfig_t
condition: USE_AUTOTUNE_FIXED_WING condition: USE_AUTOTUNE_FIXED_WING
members: members:
- name: fw_autotune_overshoot_time - name: fw_autotune_detect_time
description: "Time [ms] to detect sustained overshoot" description: "Time [ms] to detect sustained undershoot or overshoot"
default_value: 100 default_value: 250
field: fw_overshoot_time field: fw_detect_time
min: 50 min: 50
max: 500 max: 500
- name: fw_autotune_undershoot_time - name: fw_autotune_min_stick
description: "Time [ms] to detect sustained undershoot" description: "Minimum stick input [%] to consider overshoot/undershoot detection"
default_value: 200 default_value: 80
field: fw_undershoot_time field: fw_min_stick
min: 50
max: 500
- name: fw_autotune_threshold
description: "Threshold [%] of max rate to consider overshoot/undershoot detection"
default_value: 50
field: fw_max_rate_threshold
min: 0 min: 0
max: 100 max: 100
- name: fw_autotune_ff_to_p_gain - name: fw_autotune_ff_to_p_gain
@ -2073,12 +2070,36 @@ groups:
field: fw_ff_to_p_gain field: fw_ff_to_p_gain
min: 0 min: 0
max: 100 max: 100
- name: fw_autotune_p_to_d_gain
description: "P to D gain (strength relationship) [%]"
default_value: 0
field: fw_p_to_d_gain
min: 0
max: 200
- name: fw_autotune_ff_to_i_tc - name: fw_autotune_ff_to_i_tc
description: "FF to I time (defines time for I to reach the same level of response as FF) [ms]" description: "FF to I time (defines time for I to reach the same level of response as FF) [ms]"
default_value: 600 default_value: 600
field: fw_ff_to_i_time_constant field: fw_ff_to_i_time_constant
min: 100 min: 100
max: 5000 max: 5000
- name: fw_autotune_rate_adjustment
description: "`AUTO` and `LIMIT` adjust the rates to match the capabilities of the airplane, with `LIMIT` they are never increased above the starting rates setting. `FIXED` does not adjust the rates. Rates are not changed when tuning in `ANGLE` mode."
default_value: "AUTO"
field: fw_rate_adjustment
table: autotune_rate_adjustment
type: uint8_t
- name: fw_autotune_convergence_rate
description: "How much autotune adjusts the gains on each update. Setting this too high can cause erratically changing gains during autotune."
default_value: 10
field: fw_convergence_rate
min: 1
max: 50
- name: fw_autotune_max_rate_deflection
description: "The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`."
default_value: 80
field: fw_max_rate_deflection
min: 50
max: 100
- name: PG_POSITION_ESTIMATION_CONFIG - name: PG_POSITION_ESTIMATION_CONFIG
type: positionEstimationConfig_t type: positionEstimationConfig_t

8
src/main/flight/pid.c
View file

@ -406,7 +406,7 @@ static float calculateFixedWingTPAFactor(uint16_t throttle)
// tpa_rate is amount of curve TPA applied to PIDs // tpa_rate is amount of curve TPA applied to PIDs
// tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned) // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned)
if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue()) { if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) {
if (throttle > getThrottleIdleValue()) { if (throttle > getThrottleIdleValue()) {
// Calculate TPA according to throttle // Calculate TPA according to throttle
tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f);
@ -717,14 +717,14 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
} }
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit);
#ifdef USE_AUTOTUNE_FIXED_WING #ifdef USE_AUTOTUNE_FIXED_WING
if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) { if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm); autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, constrainf(newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit));
} }
#endif #endif
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit);
#ifdef USE_BLACKBOX #ifdef USE_BLACKBOX
axisPID_P[axis] = newPTerm; axisPID_P[axis] = newPTerm;
axisPID_I[axis] = pidState->errorGyroIf; axisPID_I[axis] = pidState->errorGyroIf;

17
src/main/flight/pid.h
View file

@ -24,7 +24,7 @@
#define PID_SUM_LIMIT_MIN 100 #define PID_SUM_LIMIT_MIN 100
#define PID_SUM_LIMIT_MAX 1000 #define PID_SUM_LIMIT_MAX 1000
#define PID_SUM_LIMIT_DEFAULT 500 #define PID_SUM_LIMIT_DEFAULT 500
#define PID_SUM_LIMIT_YAW_DEFAULT 350 #define PID_SUM_LIMIT_YAW_DEFAULT 400
#define HEADING_HOLD_RATE_LIMIT_MIN 10 #define HEADING_HOLD_RATE_LIMIT_MIN 10
#define HEADING_HOLD_RATE_LIMIT_MAX 250 #define HEADING_HOLD_RATE_LIMIT_MAX 250
@ -164,13 +164,22 @@ typedef struct pidProfile_s {
} pidProfile_t; } pidProfile_t;
typedef struct pidAutotuneConfig_s { typedef struct pidAutotuneConfig_s {
uint16_t fw_overshoot_time; // Time [ms] to detect sustained overshoot uint16_t fw_detect_time; // Time [ms] to detect sustained undershoot or overshoot
uint16_t fw_undershoot_time; // Time [ms] to detect sustained undershoot uint8_t fw_min_stick; // Minimum stick input required to update rates and gains
uint8_t fw_max_rate_threshold; // Threshold [%] of max rate to consider autotune detection
uint8_t fw_ff_to_p_gain; // FF to P gain (strength relationship) [%] uint8_t fw_ff_to_p_gain; // FF to P gain (strength relationship) [%]
uint8_t fw_p_to_d_gain; // P to D gain (strength relationship) [%]
uint16_t fw_ff_to_i_time_constant; // FF to I time (defines time for I to reach the same level of response as FF) [ms] uint16_t fw_ff_to_i_time_constant; // FF to I time (defines time for I to reach the same level of response as FF) [ms]
uint8_t fw_rate_adjustment; // Adjust rate settings during autotune?
uint8_t fw_convergence_rate; // Convergence rate
uint8_t fw_max_rate_deflection; // Percentage of max mixer output used for calculating the rates
} pidAutotuneConfig_t; } pidAutotuneConfig_t;
typedef enum {
FIXED,
LIMIT,
AUTO,
} fw_autotune_rate_adjustment_e;
PG_DECLARE_PROFILE(pidProfile_t, pidProfile); PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig); PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig);

223
src/main/flight/pid_autotune.c
View file

@ -25,6 +25,8 @@
#include "blackbox/blackbox.h" #include "blackbox/blackbox.h"
#include "blackbox/blackbox_fielddefs.h" #include "blackbox/blackbox_fielddefs.h"
#include "build/debug.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h" #include "common/utils.h"
@ -44,20 +46,23 @@
#include "flight/pid.h" #include "flight/pid.h"
#define AUTOTUNE_FIXED_WING_INTEGRATOR_TC 600
#define AUTOTUNE_FIXED_WING_DECREASE_STEP 8 // 8%
#define AUTOTUNE_FIXED_WING_INCREASE_STEP 5 // 5%
#define AUTOTUNE_FIXED_WING_MIN_FF 10 #define AUTOTUNE_FIXED_WING_MIN_FF 10
#define AUTOTUNE_FIXED_WING_MAX_FF 200 #define AUTOTUNE_FIXED_WING_MAX_FF 200
#define AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE 40
#define AUTOTUNE_FIXED_WING_MIN_YAW_RATE 10
#define AUTOTUNE_FIXED_WING_MAX_RATE 720
PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 2);
PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig,
.fw_overshoot_time = SETTING_FW_AUTOTUNE_OVERSHOOT_TIME_DEFAULT, .fw_detect_time = SETTING_FW_AUTOTUNE_DETECT_TIME_DEFAULT,
.fw_undershoot_time = SETTING_FW_AUTOTUNE_UNDERSHOOT_TIME_DEFAULT, .fw_min_stick = SETTING_FW_AUTOTUNE_MIN_STICK_DEFAULT,
.fw_max_rate_threshold = SETTING_FW_AUTOTUNE_THRESHOLD_DEFAULT,
.fw_ff_to_p_gain = SETTING_FW_AUTOTUNE_FF_TO_P_GAIN_DEFAULT, .fw_ff_to_p_gain = SETTING_FW_AUTOTUNE_FF_TO_P_GAIN_DEFAULT,
.fw_ff_to_i_time_constant = SETTING_FW_AUTOTUNE_FF_TO_I_TC_DEFAULT, .fw_ff_to_i_time_constant = SETTING_FW_AUTOTUNE_FF_TO_I_TC_DEFAULT,
.fw_p_to_d_gain = SETTING_FW_AUTOTUNE_P_TO_D_GAIN_DEFAULT,
.fw_rate_adjustment = SETTING_FW_AUTOTUNE_RATE_ADJUSTMENT_DEFAULT,
.fw_convergence_rate = SETTING_FW_AUTOTUNE_CONVERGENCE_RATE_DEFAULT,
.fw_max_rate_deflection = SETTING_FW_AUTOTUNE_MAX_RATE_DEFLECTION_DEFAULT,
); );
typedef enum { typedef enum {
@ -69,11 +74,15 @@ typedef enum {
typedef struct { typedef struct {
pidAutotuneState_e state; pidAutotuneState_e state;
timeMs_t stateEnterTime; timeMs_t stateEnterTime;
bool pidSaturated;
float gainP; float gainP;
float gainI; float gainI;
float gainD;
float gainFF; float gainFF;
float rate;
float initialRate;
float maxAbsDesiredRateDps;
float maxAbsReachedRateDps;
float maxAbsPidOutput;
} pidAutotuneData_t; } pidAutotuneData_t;
#define AUTOTUNE_SAVE_PERIOD 5000 // Save interval is 5 seconds - when we turn off autotune we'll restore values from previous update at most 5 sec ago #define AUTOTUNE_SAVE_PERIOD 5000 // Save interval is 5 seconds - when we turn off autotune we'll restore values from previous update at most 5 sec ago
@ -89,8 +98,9 @@ void autotuneUpdateGains(pidAutotuneData_t * data)
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pidBankMutable()->pid[axis].P = lrintf(data[axis].gainP); pidBankMutable()->pid[axis].P = lrintf(data[axis].gainP);
pidBankMutable()->pid[axis].I = lrintf(data[axis].gainI); pidBankMutable()->pid[axis].I = lrintf(data[axis].gainI);
pidBankMutable()->pid[axis].D = 0; pidBankMutable()->pid[axis].D = lrintf(data[axis].gainD);
pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF); pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF);
((controlRateConfig_t *)currentControlRateProfile)->stabilized.rates[axis] = lrintf(data[axis].rate/10.0f);
} }
schedulePidGainsUpdate(); schedulePidGainsUpdate();
} }
@ -114,10 +124,15 @@ void autotuneStart(void)
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
tuneCurrent[axis].gainP = pidBank()->pid[axis].P; tuneCurrent[axis].gainP = pidBank()->pid[axis].P;
tuneCurrent[axis].gainI = pidBank()->pid[axis].I; tuneCurrent[axis].gainI = pidBank()->pid[axis].I;
tuneCurrent[axis].gainD = pidBank()->pid[axis].D;
tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF; tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF;
tuneCurrent[axis].pidSaturated = false; tuneCurrent[axis].rate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
tuneCurrent[axis].stateEnterTime = millis(); tuneCurrent[axis].stateEnterTime = millis();
tuneCurrent[axis].state = DEMAND_TOO_LOW; tuneCurrent[axis].state = DEMAND_TOO_LOW;
tuneCurrent[axis].initialRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
tuneCurrent[axis].maxAbsDesiredRateDps = 0;
tuneCurrent[axis].maxAbsReachedRateDps = 0;
tuneCurrent[axis].maxAbsPidOutput = 0;
} }
memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT); memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT);
@ -164,27 +179,35 @@ static void blackboxLogAutotuneEvent(adjustmentFunction_e adjustmentFunction, in
void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput) void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput)
{ {
const timeMs_t currentTimeMs = millis(); const timeMs_t currentTimeMs = millis();
const float convergenceRate = pidAutotuneConfig()->fw_convergence_rate / 100.0f;
float maxDesiredRateDps = tuneCurrent[axis].rate;
float gainFF = tuneCurrent[axis].gainFF;
const float absDesiredRateDps = fabsf(desiredRateDps); const float absDesiredRateDps = fabsf(desiredRateDps);
float maxDesiredRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; const float absReachedRateDps = fabsf(reachedRateDps);
const float absPidOutput = fabsf(pidOutput);
const bool correctDirection = (desiredRateDps>0) == (reachedRateDps>0);
const float stickInput = absDesiredRateDps / maxDesiredRateDps;
float pidSumTarget;
float pidSumLimit;
float rateFullStick;
float pidOutputRequired;
pidAutotuneState_e newState; pidAutotuneState_e newState;
// Use different max desired rate in ANGLE for pitch and roll // Use different max desired rate in ANGLE for pitch and roll
// Maximum reasonable error in ANGLE mode is 200% of angle inclination (control dublet), but we are conservative and tune for control singlet. // Maximum reasonable error in ANGLE mode is 200% of angle inclination (control dublet), but we are conservative and tune for control singlet.
if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) {
float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER; float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER;
maxDesiredRate = MIN(maxDesiredRate, maxDesiredRateInAngleMode); maxDesiredRateDps = MIN(maxDesiredRateDps, maxDesiredRateInAngleMode);
} }
if (fabsf(pidOutput) >= pidProfile()->pidSumLimit) { if (stickInput < (pidAutotuneConfig()->fw_min_stick / 100.0f) || !correctDirection) {
// If we have saturated the pid output by P+FF don't increase the gain // We can make decisions only when we are giving at least 80% stick input and the airplane is rotating in the requested direction
tuneCurrent[axis].pidSaturated = true;
}
if (absDesiredRateDps < (pidAutotuneConfig()->fw_max_rate_threshold / 100.0f) * maxDesiredRate) {
// We can make decisions only when we are demanding at least 50% of max configured rate
newState = DEMAND_TOO_LOW; newState = DEMAND_TOO_LOW;
} }
else if (fabsf(reachedRateDps) > absDesiredRateDps) { else if (absReachedRateDps > absDesiredRateDps) {
newState = DEMAND_OVERSHOOT; newState = DEMAND_OVERSHOOT;
} }
else { else {
@ -192,60 +215,114 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
} }
if (newState != tuneCurrent[axis].state) { if (newState != tuneCurrent[axis].state) {
const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime;
bool gainsUpdated = false;
switch (tuneCurrent[axis].state) {
case DEMAND_TOO_LOW:
break;
case DEMAND_OVERSHOOT:
if (stateTimeMs >= pidAutotuneConfig()->fw_overshoot_time) {
tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f;
if (tuneCurrent[axis].gainFF < AUTOTUNE_FIXED_WING_MIN_FF) {
tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MIN_FF;
}
gainsUpdated = true;
}
break;
case DEMAND_UNDERSHOOT:
if (stateTimeMs >= pidAutotuneConfig()->fw_undershoot_time && !tuneCurrent[axis].pidSaturated) {
tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f;
if (tuneCurrent[axis].gainFF > AUTOTUNE_FIXED_WING_MAX_FF) {
tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MAX_FF;
}
gainsUpdated = true;
}
break;
}
if (gainsUpdated) {
// Set P-gain to 10% of FF gain (quite agressive - FIXME)
tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);
// Set integrator gain to reach the same response as FF gain in 0.667 second
tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f);
autotuneUpdateGains(tuneCurrent);
switch (axis) {
case FD_ROLL:
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF);
break;
case FD_PITCH:
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF);
break;
case FD_YAW:
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF);
break;
}
}
// Change state and reset saturation flag
tuneCurrent[axis].state = newState; tuneCurrent[axis].state = newState;
tuneCurrent[axis].stateEnterTime = currentTimeMs; tuneCurrent[axis].stateEnterTime = currentTimeMs;
tuneCurrent[axis].pidSaturated = false; tuneCurrent[axis].maxAbsDesiredRateDps = 0;
tuneCurrent[axis].maxAbsReachedRateDps = 0;
tuneCurrent[axis].maxAbsPidOutput = 0;
}
switch (tuneCurrent[axis].state){
case DEMAND_TOO_LOW:
break;
case DEMAND_UNDERSHOOT:
case DEMAND_OVERSHOOT:
{
const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime;
bool gainsUpdated = false;
bool ratesUpdated = false;
// Record max values
tuneCurrent[axis].maxAbsDesiredRateDps = MAX(tuneCurrent[axis].maxAbsDesiredRateDps, absDesiredRateDps);
tuneCurrent[axis].maxAbsReachedRateDps = MAX(tuneCurrent[axis].maxAbsReachedRateDps, absReachedRateDps);
tuneCurrent[axis].maxAbsPidOutput = MAX(tuneCurrent[axis].maxAbsPidOutput, absPidOutput);
if (stateTimeMs >= pidAutotuneConfig()->fw_detect_time) {
if (pidAutotuneConfig()->fw_rate_adjustment != FIXED && !FLIGHT_MODE(ANGLE_MODE)) {
// Tuning the rates is not compatible with ANGLE mode
// Target 80% control surface deflection to leave some room for P and I to work
pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit;
pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit;
// Theoretically achievable rate with target deflection
rateFullStick = (pidSumTarget / tuneCurrent[axis].maxAbsPidOutput) * tuneCurrent[axis].maxAbsReachedRateDps;
// Rate update
if (rateFullStick > (maxDesiredRateDps + 10.0f)) {
maxDesiredRateDps += 10.0f;
} else if (rateFullStick < (maxDesiredRateDps - 10.0f)) {
maxDesiredRateDps -= 10.0f;
}
// Constrain to safe values
uint16_t minRate = (axis == FD_YAW) ? AUTOTUNE_FIXED_WING_MIN_YAW_RATE : AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE;
uint16_t maxRate = (pidAutotuneConfig()->fw_rate_adjustment == AUTO) ? AUTOTUNE_FIXED_WING_MAX_RATE : MAX(tuneCurrent[axis].initialRate, minRate);
tuneCurrent[axis].rate = constrainf(maxDesiredRateDps, minRate, maxRate);
ratesUpdated = true;
}
// Update FF towards value needed to achieve current rate target
pidOutputRequired = MIN(tuneCurrent[axis].maxAbsPidOutput * (tuneCurrent[axis].maxAbsDesiredRateDps / tuneCurrent[axis].maxAbsReachedRateDps), pidSumLimit);
gainFF += (pidOutputRequired / tuneCurrent[axis].maxAbsDesiredRateDps * FP_PID_RATE_FF_MULTIPLIER - gainFF) * convergenceRate * stickInput;
tuneCurrent[axis].gainFF = constrainf(gainFF, AUTOTUNE_FIXED_WING_MIN_FF, AUTOTUNE_FIXED_WING_MAX_FF);
gainsUpdated = true;
// Reset state flag
tuneCurrent[axis].state = DEMAND_TOO_LOW;
}
if (gainsUpdated) {
// Set P-gain to 10% of FF gain (quite agressive - FIXME)
tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);
// Set D-gain relative to P-gain (0 for now)
tuneCurrent[axis].gainD = tuneCurrent[axis].gainP * (pidAutotuneConfig()->fw_p_to_d_gain / 100.0f);
// Set integrator gain to reach the same response as FF gain in 0.667 second
tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f);
autotuneUpdateGains(tuneCurrent);
switch (axis) {
case FD_ROLL:
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF);
break;
case FD_PITCH:
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF);
break;
case FD_YAW:
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF);
break;
}
// Debug
DEBUG_SET(DEBUG_AUTOTUNE, axis + 3, pidOutputRequired);
}
if (ratesUpdated) {
autotuneUpdateGains(tuneCurrent);
switch (axis) {
case FD_ROLL:
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_RATE, tuneCurrent[axis].rate);
break;
case FD_PITCH:
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_RATE, tuneCurrent[axis].rate);
break;
case FD_YAW:
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_RATE, tuneCurrent[axis].rate);
break;
}
// Debug
DEBUG_SET(DEBUG_AUTOTUNE, axis, rateFullStick);
}
}
} }
} }
#endif #endif