RC Smoothing

fix loopcount

Improve AUX logic // initial factor set to 1

Better Delta approach

FIX AUX CALC

Update refresh rates

src/main/mw.c

@@ -93,6 +93,8 @@ enum {
 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
 #define IBATINTERVAL (6 * 3500)       
 
+#define LOOP_DEADBAND 400 // Dead band for loop to modify to rcInterpolationFactor in RC Filtering for unstable looptimes
+
 uint32_t currentTime = 0;
 uint32_t previousTime = 0;
 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
@@ -107,6 +109,8 @@ static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the m
 
 extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 
+static bool isRXdataNew;
+
 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);            // pid controller function prototype
 
@@ -681,6 +685,112 @@ void processRx(void)
 
 }
 
+void getArmingChannel(modeActivationCondition_t *modeActivationConditions, uint8_t *armingChannel) {
+    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
+        modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
+        if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
+            *armingChannel = modeActivationCondition->auxChannelIndex +  NON_AUX_CHANNEL_COUNT;
+            break;
+        }
+    }
+}
+
+void filterRc(void){
+    static int16_t lastCommand[4] = { 0, 0, 0, 0 };
+    static int16_t deltaRC[4] = { 0, 0, 0, 0 };
+    static int16_t loop[5] = { 0, 0, 0, 0, 0 };
+    static int16_t factor, rcInterpolationFactor, loopAvg;
+    static uint32_t rxRefreshRate;
+    static int16_t lastAux, deltaAux;                            // last arming AUX position and delta for arming AUX
+	static uint8_t auxChannelToFilter;                           // AUX channel used for arming needs filtering when used
+	static int loopCount;
+
+    // Set RC refresh rate for sampling and channels to filter
+    if (!rxRefreshRate) {
+        if (feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) {
+            rxRefreshRate = 20000;
+
+            // AUX Channels to filter to replace PPM/PWM averaging
+            getArmingChannel(currentProfile->modeActivationConditions,&auxChannelToFilter);
+
+        }
+
+        // TODO Are there more different refresh rates?
+        else {
+    	    switch (masterConfig.rxConfig.serialrx_provider) {
+    	        case SERIALRX_SPEKTRUM1024:
+    	            rxRefreshRate = 22000;
+    	            break;
+    	        case SERIALRX_SPEKTRUM2048:
+    	            rxRefreshRate = 11000;
+    	            break;
+    	        case SERIALRX_SBUS:
+    	            rxRefreshRate = 11000;
+    	            break;
+    	        default:
+    	            rxRefreshRate = 10000;
+    	            break;
+            }
+        }
+
+        rcInterpolationFactor = 1; // Initial Factor before looptime average is calculated
+
+    }
+
+    // Averaging of cycleTime for more precise sampling
+    loop[loopCount] = cycleTime;
+    loopCount++;
+
+    // Start recalculating new rcInterpolationFactor every 5 loop iterations
+    if (loopCount > 4) {
+        uint16_t tmp = (loop[0] + loop[1] + loop[2] + loop[3] + loop[4]) / 5;
+
+        // Jitter tolerance to prevent rcInterpolationFactor jump too much
+        if (tmp > (loopAvg + LOOP_DEADBAND) || tmp < (loopAvg - LOOP_DEADBAND))  {
+            loopAvg = tmp;
+            rcInterpolationFactor = rxRefreshRate / loopAvg + 1;
+        }
+
+        loopCount = 0;
+     }
+
+    if (isRXdataNew) {
+        for (int channel=0; channel < 4; channel++) {
+        	deltaRC[channel] = rcData[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
+            lastCommand[channel] = rcData[channel];
+        }
+
+        // Read AUX channel (arm/disarm guard enhancement)
+        if (auxChannelToFilter) {
+            deltaAux = rcData[auxChannelToFilter] - (lastAux - deltaAux * factor/rcInterpolationFactor);
+            lastAux = rcData[auxChannelToFilter];
+        }
+
+        isRXdataNew = false;
+        factor = rcInterpolationFactor - 1;
+        }
+
+    else {
+        factor--;
+    }
+
+    // Interpolate steps of rcData
+    if (factor > 0) {
+        for (int channel=0; channel < 4; channel++) {
+            rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
+         }
+
+        // Interpolate steps of Aux
+        if (auxChannelToFilter) {
+            rcData[auxChannelToFilter] = lastAux - deltaAux * factor/rcInterpolationFactor;
+        }
+    }
+
+    else {
+        factor = 0;
+    }
+}
+
 void loop(void)
 {
     static uint32_t loopTime;
@@ -692,6 +802,7 @@ void loop(void)
 
     if (shouldProcessRx(currentTime)) {
         processRx();
+        isRXdataNew = true;
 
 #ifdef BARO
         // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
@@ -746,6 +857,8 @@ void loop(void)
             }
         }
 
+        filterRc();
+
         annexCode();
 #if defined(BARO) || defined(SONAR)
         haveProcessedAnnexCodeOnce = true;

src/main/rx/rx.c

@@ -75,7 +75,7 @@ static uint32_t needRxSignalBefore = 0;
 int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 
-#define PPM_AND_PWM_SAMPLE_COUNT 4
+#define PPM_AND_PWM_SAMPLE_COUNT 3
 
 #define DELAY_50_HZ (1000000 / 50)
 #define DELAY_10_HZ (1000000 / 10)