diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4aba7dc3d9..7eac0d7965 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -436,15 +436,17 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_ // We now use Acro Rates, transformed into the range +/- 1, to provide setpoints const float angleLimit = pidProfile->angle_limit; float angleFeedforward = 0.0f; + // if user changes rates profile, update the max setpoint for angle mode + const float maxSetpointRateInv = 1.0f / getMaxRcRate(axis); #ifdef USE_FEEDFORWARD - angleFeedforward = angleLimit * getFeedforward(axis) * pidRuntime.angleFeedforwardGain * pidRuntime.maxRcRateInv[axis]; + angleFeedforward = angleLimit * getFeedforward(axis) * pidRuntime.angleFeedforwardGain * maxSetpointRateInv; // angle feedforward must be heavily filtered, at the PID loop rate, with limited user control over time constant // it MUST be very delayed to avoid early overshoot and being too aggressive angleFeedforward = pt3FilterApply(&pidRuntime.angleFeedforwardPt3[axis], angleFeedforward); #endif - float angleTarget = angleLimit * currentPidSetpoint * pidRuntime.maxRcRateInv[axis]; + float angleTarget = angleLimit * currentPidSetpoint * maxSetpointRateInv; // use acro rates for the angle target in both horizon and angle modes, converted to -1 to +1 range using maxRate #ifdef USE_GPS_RESCUE diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 1159d9902a..c3943a8314 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -473,7 +473,6 @@ typedef struct pidRuntime_s { float angleEarthRef; float angleTarget[2]; bool axisInAngleMode[3]; - float maxRcRateInv[2]; #endif #ifdef USE_WING diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 158a65535d..412bf50159 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -251,7 +251,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) for (int axis = 0; axis < 2; axis++) { // ROLL and PITCH only pt3FilterInit(&pidRuntime.attitudeFilter[axis], k); pt3FilterInit(&pidRuntime.angleFeedforwardPt3[axis], k2); - pidRuntime.maxRcRateInv[axis] = 1.0f / getMaxRcRate(axis); } pidRuntime.angleYawSetpoint = 0.0f; #endif