diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 8d2675d199..4458fc34d4 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1051,7 +1051,6 @@ const clivalue_t valueTable[] = { { "anti_gravity_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) }, { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) }, { "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) }, - { "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforwardTransition) }, { "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, { "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, { "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) }, @@ -1150,12 +1149,13 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_FEEDFORWARD + { "feedforward_transition", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforwardTransition) }, { "feedforward_averaging", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FEEDFORWARD_AVERAGING }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_averaging) }, { "feedforward_smoothing", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) }, { "feedforward_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_jitter_factor) }, + { "feedforward_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_boost) }, { "feedforward_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_max_rate_limit) }, #endif - { "feedforward_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_boost) }, #ifdef USE_DYN_IDLE { "dyn_idle_min_rpm", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_idle_min_rpm) }, diff --git a/src/main/flight/feedforward.c b/src/main/flight/feedforward.c index c912752802..36ab6ab8b9 100644 --- a/src/main/flight/feedforward.c +++ b/src/main/flight/feedforward.c @@ -75,11 +75,11 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward const float feedforwardJitterFactor = pidGetFeedforwardJitterFactor(); float feedforward; - // calculate an attenuator from average of two most recent rcCommand deltas vs jitter threshold if (axis == FD_ROLL) { DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference = steps of 50 mean 2000 RC steps } rcCommandDelta = fabsf(rcCommandDelta); + // calculate the jitter attenuator from average of two most recent abs rcCommand deltas vs jitter threshold float jitterAttenuator = 1.0f; if (feedforwardJitterFactor) { if (rcCommandDelta < feedforwardJitterFactor) { @@ -161,6 +161,10 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward } else { setpointDelta[axis] = feedforward; } + + // apply feedforward transition + setpointDelta[axis] *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1.0f; + } return setpointDelta[axis]; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6ca00b0446..5695697674 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -256,12 +256,12 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -float pidGetFeedforwardBoostFactor() +#ifdef USE_FEEDFORWARD +float pidGetFeedforwardTransitionFactor() { - return pidRuntime.feedforwardBoostFactor; + return pidRuntime.feedforwardTransitionFactor; } -#ifdef USE_FEEDFORWARD float pidGetFeedforwardSmoothFactor() { return pidRuntime.feedforwardSmoothFactor; @@ -271,6 +271,11 @@ float pidGetFeedforwardJitterFactor() { return pidRuntime.feedforwardJitterFactor; } + +float pidGetFeedforwardBoostFactor() +{ + return pidRuntime.feedforwardBoostFactor; +} #endif void pidResetIterm(void) @@ -1105,9 +1110,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim if (feedforwardGain > 0) { // halve feedforward in Level mode since stick sensitivity is weaker by about half feedforwardGain *= FLIGHT_MODE(ANGLE_MODE) ? 0.5f : 1.0f; - // no transition if feedforwardTransition == 0 - float transition = pidRuntime.feedforwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * pidRuntime.feedforwardTransition) : 1; - float feedForward = feedforwardGain * transition * pidSetpointDelta * pidRuntime.pidFrequency; + // transition now calculated in feedforward.c when new RC data arrives + float feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency; #ifdef USE_FEEDFORWARD pidData[axis].F = shouldApplyFeedforwardLimits(axis) ? diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f99140cbae..5fcd4ebc94 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -161,7 +161,6 @@ typedef struct pidProfile_s { uint16_t crash_delay; // ms uint8_t crash_recovery_angle; // degrees uint8_t crash_recovery_rate; // degree/second - uint8_t feedforwardTransition; // Feedforward attenuation around centre sticks uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase uint16_t itermLimit; uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz @@ -197,7 +196,6 @@ typedef struct pidProfile_s { uint8_t motor_output_limit; // Upper limit of the motor output (percent) int8_t auto_profile_cell_count; // Cell count for this profile to be used with if auto PID profile switching is used uint8_t transient_throttle_limit; // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise - uint8_t feedforward_boost; // amount of setpoint acceleration to add to feedforward, 10 means 100% added char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile uint8_t dyn_idle_min_rpm; // minimum motor speed enforced by the dynamic idle controller @@ -206,10 +204,13 @@ typedef struct pidProfile_s { uint8_t dyn_idle_d_gain; // D gain for corrections around rapid changes in rpm uint8_t dyn_idle_max_increase; // limit on maximum possible increase in motor idle drive during active control + uint8_t feedforwardTransition; // Feedforward attenuation around centre sticks uint8_t feedforward_averaging; // Number of packets to average when averaging is on - uint8_t feedforward_max_rate_limit; // Maximum setpoint rate percentage for feedforward uint8_t feedforward_smooth_factor; // Amount of lowpass type smoothing for feedforward steps 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 dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount @@ -284,10 +285,8 @@ typedef struct pidRuntime_s { float antiGravityOsdCutoff; float antiGravityThrottleHpf; float antiGravityPBoost; - float feedforwardBoostFactor; float itermAccelerator; uint16_t itermAcceleratorGain; - float feedforwardTransition; pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT]; float levelGain; float horizonGain; @@ -385,9 +384,11 @@ typedef struct pidRuntime_s { #endif #ifdef USE_FEEDFORWARD + float feedforwardTransitionFactor; feedforwardAveraging_t feedforwardAveraging; float feedforwardSmoothFactor; float feedforwardJitterFactor; + float feedforwardBoostFactor; #endif } pidRuntime_t; @@ -443,4 +444,5 @@ float pidGetPidFrequency(); float pidGetFeedforwardBoostFactor(); float pidGetFeedforwardSmoothFactor(); float pidGetFeedforwardJitterFactor(); +float pidGetFeedforwardTransitionFactor(); float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo); diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 72243eadf7..6bed63ad07 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -276,11 +276,6 @@ void pidUpdateFeedforwardLpf(uint16_t filterCutoff) void pidInitConfig(const pidProfile_t *pidProfile) { - if (pidProfile->feedforwardTransition == 0) { - pidRuntime.feedforwardTransition = 0; - } else { - pidRuntime.feedforwardTransition = 100.0f / pidProfile->feedforwardTransition; - } for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P; pidRuntime.pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I; @@ -428,6 +423,11 @@ void pidInitConfig(const pidProfile_t *pidProfile) pidRuntime.feedforwardSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f; } pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor; + if (pidProfile->feedforwardTransition == 0) { + pidRuntime.feedforwardTransitionFactor = 0; + } else { + pidRuntime.feedforwardTransitionFactor = 100.0f / pidProfile->feedforwardTransition; + } feedforwardInit(pidProfile); #endif diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 947cf549fd..afb5894f0f 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -101,7 +101,10 @@ extern "C" { { UNUSED(newRcFrame); UNUSED(feedforwardAveraging); - return simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis]; + const float feedforwardTransitionFactor = pidGetFeedforwardTransitionFactor(); + float setpointDelta = simulatedSetpointRate[axis] - simulatedPrevSetpointRate[axis]; + setpointDelta *= feedforwardTransitionFactor > 0 ? MIN(1.0f, getRcDeflectionAbs(axis) * feedforwardTransitionFactor) : 1; + return setpointDelta; } bool shouldApplyFeedforwardLimits(int axis) {