Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

Feedforward improvements for 4.6 (#13576)

Browse source 
* feedforward update for 4.6

improve jitter reduction method
don't interpolate CRSF protocol
replace 'boost' with highpass element for yaw FF
make yaw highpass element CLI adjustable
add yaw feedforward sustain params to CLI and BBE
refactoring and unit test fix

* implement review suggestions, start on the filter struct

* Attempt PT1 filter init method

* fix silly error, scale time constant correctly

improve gain linearisation at shorter time constants

* fix averaging init

* Review suggestions from PL

* Improve filter initialisation

much better :-)

* re-name prevPacketDuplicate to prevPacketWasADuplicate

* Add review comments - thanks Jan and Mark!

* cast gyro.gyroDebugAxis to int

* A better fix than int cast

* implement review comments from PL

also hopefully improved some comments.

* increase PG to 10, expecting the Disarm PR to use 9

* two always win against one ;-)

* remove inappropriate comment, remove space

* update comments and review suggestions from PL
This commit is contained in:
ctzsnooze 2024-08-25 17:34:27 +10:00 • committed by GitHub
parent 9e3fce54d3
commit f2bd6e69b2
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
8 changed files with 179 additions and 109 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -1508,6 +1508,8 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, "%d", currentPidProfile->feedforward_max_rate_limit);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FEEDFORWARD, "%d", currentPidProfile->pid[PID_LEVEL].F);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FF_SMOOTHING_MS, "%d", currentPidProfile->angle_feedforward_smoothing_ms);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_YAW_HOLD_GAIN, "%d", currentPidProfile->feedforward_yaw_hold_gain);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_YAW_HOLD_TIME, "%d", currentPidProfile->feedforward_yaw_hold_time);
#endif
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_LIMIT, "%d", currentPidProfile->angle_limit);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_EARTH_REF, "%d", currentPidProfile->angle_earth_ref);

2
src/main/cli/settings.c
View file

@ -1231,6 +1231,8 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_FEEDFORWARD_JITTER_FACTOR, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_jitter_factor) },
{ PARAM_NAME_FEEDFORWARD_BOOST, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_boost) },
{ PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 200}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_max_rate_limit) },
{ PARAM_NAME_FEEDFORWARD_YAW_HOLD_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 100}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_yaw_hold_gain) },
{ PARAM_NAME_FEEDFORWARD_YAW_HOLD_TIME, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {10, 250}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_yaw_hold_time) },
#endif
#ifdef USE_DYN_IDLE

2
src/main/fc/parameter_names.h
View file

@ -115,6 +115,8 @@
#define PARAM_NAME_FEEDFORWARD_JITTER_FACTOR "feedforward_jitter_factor"
#define PARAM_NAME_FEEDFORWARD_BOOST "feedforward_boost"
#define PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT "feedforward_max_rate_limit"
#define PARAM_NAME_FEEDFORWARD_YAW_HOLD_GAIN "feedforward_yaw_hold_gain"
#define PARAM_NAME_FEEDFORWARD_YAW_HOLD_TIME "feedforward_yaw_hold_time"
#define PARAM_NAME_DYN_IDLE_MIN_RPM "dyn_idle_min_rpm"
#define PARAM_NAME_DYN_IDLE_P_GAIN "dyn_idle_p_gain"
#define PARAM_NAME_DYN_IDLE_I_GAIN "dyn_idle_i_gain"

236
src/main/fc/rc.c
View file

@ -85,12 +85,15 @@ enum {
#ifdef USE_FEEDFORWARD
static float feedforwardSmoothed[3];
static float feedforwardRaw[3];
static uint16_t feedforwardAveraging;
typedef struct laggedMovingAverageCombined_s {
laggedMovingAverage_t filter;
float buf[4];
} laggedMovingAverageCombined_t;
laggedMovingAverageCombined_t feedforwardDeltaAvg[XYZ_AXIS_COUNT];
static pt1Filter_t feedforwardYawHoldLpf;
float getFeedforward(int axis)
{
#ifdef USE_RC_SMOOTHING_FILTER
@ -489,141 +492,177 @@ static FAST_CODE void processRcSmoothingFilter(void)
}
#endif // USE_RC_SMOOTHING_FILTER
NOINLINE void initAveraging(uint16_t feedforwardAveraging)
{
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
laggedMovingAverageInit(&feedforwardDeltaAvg[i].filter, feedforwardAveraging + 1, (float *)&feedforwardDeltaAvg[i].buf[0]);
}
}
FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, int axis)
#ifdef USE_FEEDFORWARD
FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, flight_dynamics_index_t axis)
{
const float rxInterval = currentRxIntervalUs * 1e-6f; // seconds
float rxRate = currentRxRateHz;
static float prevRxInterval;
static float prevRcCommand[3];
float rxRate = currentRxRateHz; // 1e6f / currentRxIntervalUs;
static float prevRcCommand[3]; // for rcCommandDelta test
static float prevRcCommandDeltaAbs[3]; // for duplicate interpolation
static float prevSetpoint[3]; // equals raw unless interpolated
static float prevSetpointSpeed[3];
static float prevAcceleration[3]; // for duplicate interpolation
static bool prevDuplicatePacket[3]; // to identify multiple identical packets
static uint16_t feedforwardAveraging = 0;
static float prevSetpoint[3]; // equals raw unless extrapolated forward
static float prevSetpointSpeed[3]; // for setpointDelta calculation
static float prevSetpointSpeedDelta[3]; // for duplicate extrapolation
static bool isPrevPacketDuplicate[3]; // to identify multiple identical packets
if (feedforwardAveraging != pid->feedforwardAveraging) {
feedforwardAveraging = pid->feedforwardAveraging;
initAveraging(feedforwardAveraging);
}
const float rcCommandDeltaAbs = fabsf(rcCommand[axis] - prevRcCommand[axis]);
const float rcCommandDelta = rcCommand[axis] - prevRcCommand[axis];
prevRcCommand[axis] = rcCommand[axis];
float rcCommandDeltaAbs = fabsf(rcCommandDelta);
float setpoint = rawSetpoint[axis];
float setpointSpeed = (setpoint - prevSetpoint[axis]);
const float setpoint = rawSetpoint[axis];
const float setpointDelta = setpoint - prevSetpoint[axis];
prevSetpoint[axis] = setpoint;
float setpointAcceleration = 0.0f;
float setpointSpeed = 0.0f;
float setpointSpeedDelta = 0.0f;
float feedforward = 0.0f;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint * 10.0f)); // un-smoothed final feedforward
}
// attenuators
float zeroTheAcceleration = 1.0f;
float jitterAttenuator = 1.0f;
if (pid->feedforwardJitterFactor) {
if (rcCommandDeltaAbs < pid->feedforwardJitterFactor) {
jitterAttenuator = MAX(1.0f - (rcCommandDeltaAbs + prevRcCommandDeltaAbs[axis]) * pid->feedforwardJitterFactorInv, 0.0f);
// note that feedforwardJitterFactorInv includes a divide by 2 to average the two previous rcCommandDeltaAbs values
jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
}
}
prevRcCommandDeltaAbs[axis] = rcCommandDeltaAbs;
// interpolate setpoint if necessary
if (rcCommandDeltaAbs) {
if (pid->feedforwardInterpolate) {
static float prevRxInterval;
// for Rx links which send frequent duplicate data packets, use a per-axis duplicate test
// extrapolate setpointSpeed when a duplicate is detected, to minimise steps in feedforward
const bool isDuplicate = rcCommandDeltaAbs == 0;
if (!isDuplicate) {
// movement!
if (prevDuplicatePacket[axis] == true) {
// but, if the packet before this was also a duplicate,
// calculate setpointSpeed over the last two intervals
if (isPrevPacketDuplicate[axis]) {
rxRate = 1.0f / (rxInterval + prevRxInterval);
zeroTheAcceleration = 0.0f;
// don't add acceleration, empirically seems better on FrSky
}
setpointSpeed *= rxRate;
prevDuplicatePacket[axis] = false;
setpointSpeed = setpointDelta * rxRate;
isPrevPacketDuplicate[axis] = isDuplicate;
} else {
// no movement!
if (prevDuplicatePacket[axis] == false) {
// first duplicate after movement
setpointSpeed = prevSetpointSpeed[axis];
if (fabsf(setpoint) < 0.95f * maxRcRate[axis]) {
setpointSpeed += prevAcceleration[axis];
// no movement
if (!isPrevPacketDuplicate[axis]) {
// extrapolate a replacement setpointSpeed value for the first duplicate after normal movement
// but not when about to hit max deflection
if (fabsf(setpoint) < 0.90f * maxRcRate[axis]) {
// this is a single packet duplicate, and we assume that it is of approximately normal duration
// hence no multiplication of prevSetpointSpeedDelta by rxInterval / prevRxInterval
setpointSpeed = prevSetpointSpeed[axis] + prevSetpointSpeedDelta[axis];
// pretend that there was stick movement also, to hold the same jitter value
rcCommandDeltaAbs = prevRcCommandDeltaAbs[axis];
}
zeroTheAcceleration = 0.0f; // force acceleration to zero
} else {
// second and subsequent duplicates after movement should be zeroed
// for second and all subsequent duplicates...
// force setpoint speed to zero
setpointSpeed = 0.0f;
prevSetpointSpeed[axis] = 0.0f;
zeroTheAcceleration = 0.0f; // force acceleration to zero
// zero the acceleration by setting previous speed to zero
// feedforward will smoothly decay and be attenuated by the jitter reduction value for zero rcCommandDelta
prevSetpointSpeed[axis] = 0.0f; // zero acceleration later on
}
prevDuplicatePacket[axis] = true;
isPrevPacketDuplicate[axis] = isDuplicate;
}
prevRxInterval = rxInterval;
} else {
// don't interpolate for radio systems that rarely send duplicate packets, eg CRSF/ELRS
setpointSpeed = setpointDelta * rxRate;
}
// calculate jitterAttenuation factor
// The intent is to attenuate feedforward when absolute rcCommandDelta is small, ie when sticks move very slowly
// Greater feedforward_jitter_factor values widen the attenuation range, and increase the suppression at center
// Stick input is the average of the previous two absolute rcCommandDelta values
// Output is jitterAttenuator, a value 0-1.0 that is a simple multiplier of the final feedforward value
// For the CLI user setting of feedforward_jitter_factor:
// User setting of 0 returns feedforwardJitterFactorInv = 1.0 (and disables the function)
// User setting of 1 returns feedforwardJitterFactorInv = 0.5
// User setting of 9 returns feedforwardJitterFactorInv = 0.1
// rcCommandDelta has 500 unit values either side of center stick position
// For a 250Hz link, a one second stick sweep center->max returns rcCommandDelta around 2.0
// For a user jitter reduction setting of 2, the jitterAttenuator value ranges linearly
// from 0.33 when rcCommandDelta is close to zero, up to 1.0 for rcCommandDelta of 2.0 or more
// For a user jitter reduction setting of 9, the jitterAttenuator value ranges linearly
// from 0.1 when rcCommandDelta is close to zero, up to 1.0 for rcCommandDelta is 9.0 or more
// note that the jitter reduction multiplies the final smoothed value of feedforward
// allowing residual smooth feedforward offsets even if the sticks are not moving
// this is an improvement on the previous version which 'chopped' FF to zero when sticks stopped moving
float jitterAttenuator = ((rcCommandDeltaAbs + prevRcCommandDeltaAbs[axis]) * 0.5f + 1.0f) * pid->feedforwardJitterFactorInv;
jitterAttenuator = MIN(jitterAttenuator, 1.0f);
prevRcCommandDeltaAbs[axis] = rcCommandDeltaAbs;
// smooth the setpointSpeed value
setpointSpeed = prevSetpointSpeed[axis] + pid->feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
// calculate acceleration and attenuate
setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * rxRate * 0.01f;
// calculate setpointDelta from smoothed setpoint speed
setpointSpeedDelta = setpointSpeed - prevSetpointSpeed[axis];
prevSetpointSpeed[axis] = setpointSpeed;
// smooth the acceleration element (effectively a second order filter) and apply jitter reduction
setpointAcceleration = prevAcceleration[axis] + pid->feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
prevAcceleration[axis] = setpointAcceleration * zeroTheAcceleration;
setpointAcceleration = setpointAcceleration * pid->feedforwardBoostFactor * jitterAttenuator * zeroTheAcceleration;
// smooth the setpointDelta element (effectively a second order filter since incoming setpoint was already smoothed)
setpointSpeedDelta = prevSetpointSpeedDelta[axis] + pid->feedforwardSmoothFactor * (setpointSpeedDelta - prevSetpointSpeedDelta[axis]);
prevSetpointSpeedDelta[axis] = setpointSpeedDelta;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(setpointSpeed * 0.1f)); // base feedforward without acceleration
// apply gain factor to delta and adjust for rxRate
const float feedforwardBoost = setpointSpeedDelta * rxRate * pid->feedforwardBoostFactor;
feedforward = setpointSpeed;
if (axis == FD_ROLL || axis == FD_PITCH) {
// for pitch and roll, add feedforwardBoost to deal with motor lag
feedforward += feedforwardBoost;
// apply jitter reduction multiplier to reduce noise by attenuating when sticks move slowly
feedforward *= jitterAttenuator;
// pull feedforward back towards zero as sticks approach max if in same direction
// to avoid overshooting on the outwards leg of a fast roll or flip
if (pid->feedforwardMaxRateLimit && feedforward * setpoint > 0.0f) {
const float limit = (maxRcRate[axis] - fabsf(setpoint)) * pid->feedforwardMaxRateLimit;
feedforward = (limit > 0.0f) ? constrainf(feedforward, -limit, limit) : 0.0f;
}
float feedforward = setpointSpeed + setpointAcceleration;
} else {
// for yaw, apply jitter reduction only to the base feedforward delta element
// can't be applied to the 'sustained' element or jitter values will divide it down too much when sticks are still
feedforward *= jitterAttenuator;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(feedforward * 0.1f));
// un-smoothed feedforward including acceleration but before limiting, transition, averaging, and jitter reduction
// instead of adding setpoint acceleration, which is too aggressive for yaw,
// add a slow-fading high-pass filtered setpoint element
// this provides a 'sustained boost' with low noise
// it mimics the normal sustained yaw motor drive requirements, reducing P and I and hence reducing bounceback
// this doesn't add significant noise to feedforward
// too little yaw FF causes iTerm windup and slow bounce back when stopping a hard yaw
// too much causes fast bounce back when stopping a hard yaw
// calculate lowpass filter gain factor from user specified time constant
const float gain = pt1FilterGainFromDelay(pid->feedforwardYawHoldTime, rxInterval);
pt1FilterUpdateCutoff(&feedforwardYawHoldLpf, gain);
const float setpointLpfYaw = pt1FilterApply(&feedforwardYawHoldLpf, setpoint);
// subtract lowpass from input to get highpass of setpoint for sustained yaw 'boost'
const float feedforwardYawHold = pid->feedforwardYawHoldGain * (setpoint - setpointLpfYaw);
DEBUG_SET(DEBUG_FEEDFORWARD, 6, lrintf(feedforward * 0.01f)); // basic yaw feedforward without hold element
DEBUG_SET(DEBUG_FEEDFORWARD, 7, lrintf(feedforwardYawHold * 0.01f)); // yaw feedforward hold element
feedforward += feedforwardYawHold;
// NB: yaw doesn't need max rate limiting since it rarely overshoots
}
// apply feedforward transition
// apply feedforward transition, if configured. Archaic (better to use jitter reduction)
const bool useTransition = (pid->feedforwardTransition != 0.0f) && (rcDeflectionAbs[axis] < pid->feedforwardTransition);
if (useTransition) {
feedforward *= rcDeflectionAbs[axis] * pid->feedforwardTransitionInv;
}
// apply averaging
if (axis == gyro.gyroDebugAxis) {
DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint)); // un-smoothed (raw) setpoint value used for FF
DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(setpointSpeed * 0.01f)); // smoothed and extrapolated basic feedfoward element
DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(feedforwardBoost * 0.01f)); // acceleration (boost) smoothed
DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 10.0f));
DEBUG_SET(DEBUG_FEEDFORWARD, 4, lrintf(jitterAttenuator * 100.0f)); // jitter attenuation percent
DEBUG_SET(DEBUG_FEEDFORWARD, 5, (int16_t)(isPrevPacketDuplicate[axis])); // previous packet was a duplicate
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, lrintf(jitterAttenuator * 100.0f)); // jitter attenuation factor in percent
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, lrintf(maxRcRate[axis])); // max Setpoint rate (badly named)
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, lrintf(setpoint)); // setpoint used for FF
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 3, lrintf(feedforward * 0.01f)); // un-smoothed final feedforward
}
// apply averaging to final values, for additional smoothing if needed; not shown in logs
if (feedforwardAveraging) {
feedforward = laggedMovingAverageUpdate(&feedforwardDeltaAvg[axis].filter, feedforward);
}
// apply jitter reduction
feedforward *= jitterAttenuator;
// apply max rate limiting
if (pid->feedforwardMaxRateLimit && axis < FD_YAW) {
if (feedforward * setpoint > 0.0f) { // in same direction
const float limit = (maxRcRate[axis] - fabsf(setpoint)) * pid->feedforwardMaxRateLimit;
feedforward = (limit > 0.0f) ? constrainf(feedforward, -limit, limit) : 0.0f;
}
}
feedforwardRaw[axis] = feedforward;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(feedforwardRaw[axis] * 0.1f)); // un-smoothed final feedforward
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, lrintf(jitterAttenuator * 100.0f)); // un-smoothed final feedforward
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, lrintf(maxRcRate[axis])); // un-smoothed final feedforward
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, lrintf(setpoint)); // un-smoothed final feedforward
DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 3, lrintf(feedforwardRaw[axis])); // un-smoothed final feedforward
}
}
#endif // USE_FEEDFORWARD
FAST_CODE void processRcCommand(void)
{
@ -686,7 +725,6 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
isRxDataNew = true;
for (int axis = 0; axis < 3; axis++) {
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f);
if (axis == ROLL || axis == PITCH) {
@ -807,11 +845,19 @@ void initRcProcessing(void)
break;
}
for (int i = 0; i < 3; i++) {
#ifdef USE_FEEDFORWARD
feedforwardAveraging = pidRuntime.feedforwardAveraging;
pt1FilterInit(&feedforwardYawHoldLpf, 0.0f);
#endif // USE_FEEDFORWARD
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
maxRcRate[i] = applyRates(i, 1.0f, 1.0f);
#ifdef USE_FEEDFORWARD
feedforwardSmoothed[i] = 0.0f;
feedforwardRaw[i] = 0.0f;
if (feedforwardAveraging) {
laggedMovingAverageInit(&feedforwardDeltaAvg[i].filter, feedforwardAveraging + 1, (float *)&feedforwardDeltaAvg[i].buf[0]);
}
#endif // USE_FEEDFORWARD
}

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

@ -116,7 +116,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 9);
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 10);
void resetPidProfile(pidProfile_t *pidProfile)
{
@ -237,6 +237,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.spa_width = { 0, 0, 0 },
.spa_mode = { 0, 0, 0 },
.landing_disarm_threshold = 0, // relatively safe values are around 100
.feedforward_yaw_hold_gain = 15, // zero disables; 15-20 is OK for 5in
.feedforward_yaw_hold_time = 100, // a value of 100 is a time constant of about 100ms, and is OK for a 5in; smaller values decay faster, eg for smaller props
);
#ifndef USE_D_MIN

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

@ -228,6 +228,8 @@ typedef struct pidProfile_s {
uint8_t feedforward_jitter_factor; // Number of RC steps below which to attenuate feedforward
uint8_t feedforward_boost; // amount of setpoint acceleration to add to feedforward, 10 means 100% added
uint8_t feedforward_max_rate_limit; // Maximum setpoint rate percentage for feedforward
uint8_t feedforward_yaw_hold_gain; // Amount of sustained high-pass yaw setpoint to add to feedforward, zero disables
uint8_t feedforward_yaw_hold_time ; // Time constant of the sustained yaw hold element in ms to add to feed forward, higher values decay slower
uint8_t dterm_lpf1_dyn_expo; // set the curve for dynamic dterm lowpass filter
uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calculation is gyro based in level mode
@ -437,6 +439,9 @@ typedef struct pidRuntime_s {
float feedforwardTransition;
float feedforwardTransitionInv;
uint8_t feedforwardMaxRateLimit;
float feedforwardYawHoldGain;
float feedforwardYawHoldTime;
bool feedforwardInterpolate; // Whether to interpolate an FF value for duplicate/identical data values
pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT];
#endif

12
src/main/flight/pid_init.c
View file

@ -426,10 +426,16 @@ void pidInitConfig(const pidProfile_t *pidProfile)
pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
pidRuntime.feedforwardSmoothFactor = 1.0f - (0.01f * pidProfile->feedforward_smooth_factor);
pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor;
pidRuntime.feedforwardJitterFactorInv = 1.0f / (2.0f * pidProfile->feedforward_jitter_factor);
// the extra division by 2 is to average the sum of the two previous rcCommandAbs values
pidRuntime.feedforwardBoostFactor = 0.1f * pidProfile->feedforward_boost;
pidRuntime.feedforwardJitterFactorInv = 1.0f / (1.0f + pidProfile->feedforward_jitter_factor);
pidRuntime.feedforwardBoostFactor = 0.001f * pidProfile->feedforward_boost;
pidRuntime.feedforwardMaxRateLimit = pidProfile->feedforward_max_rate_limit;
pidRuntime.feedforwardInterpolate = !(rxRuntimeState.serialrxProvider == SERIALRX_CRSF);
pidRuntime.feedforwardYawHoldTime = 0.001f * pidProfile->feedforward_yaw_hold_time; // input time constant in milliseconds, converted to seconds
pidRuntime.feedforwardYawHoldGain = pidProfile->feedforward_yaw_hold_gain;
// normalise/maintain boost when time constant is small, 1.5x at 50ms, 2x at 25ms, almost 3x at 10ms
if (pidProfile->feedforward_yaw_hold_time < 100) {
pidRuntime.feedforwardYawHoldGain *= 150.0f / (float)(pidProfile->feedforward_yaw_hold_time + 50);
}
#endif
pidRuntime.levelRaceMode = pidProfile->level_race_mode;

5
src/test/unit/pid_unittest.cc
View file

@ -71,6 +71,9 @@ extern "C" {
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/rx.h"
#include "rx/rx.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
@ -78,6 +81,8 @@ extern "C" {
gyro_t gyro;
attitudeEulerAngles_t attitude;
rxRuntimeState_t rxRuntimeState = {};
PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2);