RC Smoothing

fix loopcount

Improve AUX logic // initial factor set to 1

Better Delta approach

FIX AUX CALC

Update refresh rates

Conflicts:
	src/main/mw.c

src/main/mw.c

@@ -90,6 +90,8 @@ enum {
 /* for VBAT monitoring frequency */
 #define VBATFREQ 6        // to read battery voltage - nth number of loop iterations
 
+#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
@@ -105,6 +107,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
 
@@ -704,6 +708,110 @@ void filterGyro(void) {
         else {
 	        gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, dT);
         }
+
+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;
     }
 }
 
@@ -718,6 +826,7 @@ void loop(void)
 
     if (shouldProcessRx(currentTime)) {
         processRx();
+        isRXdataNew = true;
 
 #ifdef BARO
         // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
@@ -768,6 +877,8 @@ void loop(void)
             filterGyro();
         }
 
+        filterRc();
+
         annexCode();
 #if defined(BARO) || defined(SONAR)
         haveProcessedAnnexCodeOnce = true;

src/main/rx/rx.c

@@ -73,7 +73,7 @@ static uint32_t needRxSignalBefore = 0;
 
 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)