Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 11:59:58 +03:00
Code Issues Wiki Activity

feedforward transition moved to feedforward.c

Browse source
This commit is contained in:
ctzsnooze 2021-08-04 16:48:07 +10:00
parent 86aa5cc84e
commit 28d9d778a6
6 changed files with 33 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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

6
src/main/flight/feedforward.c
View file

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

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

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

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

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

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

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

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

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