Merge pull request #10626 from ctzsnooze/rc-tidyup-interpolated-setpoint

Remove non-required interpolated_sp parameters, simplify calling FF recalculation

src/main/fc/rc.c

@@ -61,9 +61,6 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom
 #ifdef USE_INTERPOLATED_SP
 // Setpoint in degrees/sec before RC-Smoothing is applied
 static float rawSetpoint[XYZ_AXIS_COUNT];
-// Stick deflection [-1.0, 1.0] before RC-Smoothing is applied
-static float rawDeflection[XYZ_AXIS_COUNT];
-static float oldRcCommand[XYZ_AXIS_COUNT];
 #endif
 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
 static float throttlePIDAttenuation;
@@ -75,7 +72,7 @@ static float rcCommandDivider = 500.0f;
 static float rcCommandYawDivider = 500.0f;
 
 FAST_DATA_ZERO_INIT uint8_t interpolationChannels;
-static FAST_DATA_ZERO_INIT uint32_t rcFrameNumber;
+static FAST_DATA_ZERO_INIT bool newRxDataForFF;
 
 enum {
     ROLL_FLAG = 1 << ROLL,
@@ -99,12 +96,18 @@ enum {
 static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
 #endif // USE_RC_SMOOTHING_FILTER
 
-uint32_t getRcFrameNumber()
+bool getShouldUpdateFf()
+// only used in pid.c when interpolated_sp is active to initiate a new FF value
 {
-    return rcFrameNumber;
+    const bool updateFf = newRxDataForFF;
+    if (newRxDataForFF == true){
+        newRxDataForFF = false;
+    }
+    return updateFf;
 }
 
 float getSetpointRate(int axis)
+// only used in pid.c to provide setpointRate for the crash recovery function
 {
     return setpointRate[axis];
 }
@@ -130,11 +133,6 @@ float getRawSetpoint(int axis)
     return rawSetpoint[axis];
 }
 
-float getRawDeflection(int axis)
-{
-    return rawDeflection[axis];
-}
-
 #endif
 
 #define THROTTLE_LOOKUP_LENGTH 12
@@ -235,11 +233,6 @@ float applyCurve(int axis, float deflection)
     return applyRates(axis, deflection, fabsf(deflection));
 }
 
-float getRcCurveSlope(int axis, float deflection)
-{
-    return (applyCurve(axis, deflection + 0.01f) - applyCurve(axis, deflection)) * 100.0f;
-}
-
 static void calculateSetpointRate(int axis)
 {
     float angleRate;
@@ -691,7 +684,7 @@ FAST_CODE void processRcCommand(void)
     uint8_t updatedChannel;
 
     if (isRxDataNew) {
-        rcFrameNumber++;
+        newRxDataForFF = true;
     }
 
     if (isRxDataNew && pidAntiGravityEnabled()) {
@@ -701,7 +694,6 @@ FAST_CODE void processRcCommand(void)
 #ifdef USE_INTERPOLATED_SP
     if (isRxDataNew) {
         for (int i = FD_ROLL; i <= FD_YAW; i++) {
-            oldRcCommand[i] = rcCommand[i];
             float rcCommandf;
             if (i == FD_YAW) {
                 rcCommandf = rcCommand[i] / rcCommandYawDivider;
@@ -710,7 +702,6 @@ FAST_CODE void processRcCommand(void)
             }
             const float rcCommandfAbs = fabsf(rcCommandf);
             rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs);
-            rawDeflection[i] = rcCommandf;
         }
     }
 #endif

src/main/fc/rc.h

@@ -51,9 +51,7 @@ rcSmoothingFilter_t *getRcSmoothingData(void);
 bool rcSmoothingAutoCalculate(void);
 bool rcSmoothingInitializationComplete(void);
 float getRawSetpoint(int axis);
-float getRawDeflection(int axis);
 float applyCurve(int axis, float deflection);
-uint32_t getRcFrameNumber();
-float getRcCurveSlope(int axis, float deflection);
+bool getShouldUpdateFf();
 void updateRcRefreshRate(timeUs_t currentTimeUs);
 uint16_t getCurrentRxRefreshRate(void);

src/main/flight/pid.c

@@ -803,9 +803,6 @@ static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchT
 void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
 {
     static float previousGyroRateDterm[XYZ_AXIS_COUNT];
-#ifdef USE_INTERPOLATED_SP
-    static FAST_DATA_ZERO_INIT uint32_t lastFrameNumber;
-#endif
     static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
 
 #if defined(USE_ACC)
@@ -919,8 +916,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 
 #ifdef USE_INTERPOLATED_SP
     bool newRcFrame = false;
-    if (lastFrameNumber != getRcFrameNumber()) {
-        lastFrameNumber = getRcFrameNumber();
+    if (getShouldUpdateFf()) {
         newRcFrame = true;
     }
 #endif