Further 2khz Stability Enhancements

src/main/drivers/gyro_sync.h

@@ -5,6 +5,8 @@
  *      Author: borisb
  */
 
+#define INTERRUPT_WAIT_TIME 10
+
 extern uint32_t targetLooptime;
 
 bool gyroSyncCheckUpdate(void);

src/main/flight/pid.c

@@ -94,7 +94,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     float ITerm,PTerm,DTerm;
     int32_t stickPosAil, stickPosEle, mostDeflectedPos;
     static float lastError[3];
-    static float delta1[3], delta2[3];
+    static float delta1[3], delta2[3], delta3[3], delta4[3], delta5[3];
     float delta, deltaSum;
     int axis;
     float horizonLevelStrength = 1;
@@ -194,8 +194,17 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         delta *= (1.0f / dT);
 
         if (!pidProfile->dterm_cut_hz) {
-            // add moving average here to reduce noise
-            deltaSum = (delta1[axis] + delta2[axis] + delta) / 3;
+            // add moving average here to reduce noise. More averaging needed for 2khz mode
+            deltaSum = delta1[axis] + delta2[axis] + delta;
+            if (targetLooptime < 1000) {
+                deltaSum += delta3[axis] + delta4[axis] + delta5[axis];
+                delta5[axis] = delta4[axis];
+                delta4[axis] = delta3[axis];
+                delta3[axis] = delta2[axis];
+                deltaSum /= 6;
+            } else {
+                deltaSum /= 3;
+            }
             delta2[axis] = delta1[axis];
             delta1[axis] = delta;
         } else {
@@ -234,7 +243,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
     int32_t errorAngle;
     int axis;
     int32_t delta, deltaSum;
-    static int32_t delta1[3], delta2[3];
+    static int32_t delta1[3], delta2[3], delta3[3], delta4[3], delta5[3];;
     int32_t PTerm, ITerm, DTerm;
     static int32_t lastError[3] = { 0, 0, 0 };
     static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
@@ -337,8 +346,15 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
 
         if (!pidProfile->dterm_cut_hz) {
-            // add moving average here to reduce noise
+            // add moving average here to reduce noise (More moving average required for 2khz mode)
             deltaSum = delta1[axis] + delta2[axis] + delta;
+            if (targetLooptime < 1000) {
+                deltaSum += delta3[axis] + delta4[axis] + delta5[axis];
+                delta5[axis] = delta4[axis];
+                delta4[axis] = delta3[axis];
+                delta3[axis] = delta2[axis];
+                deltaSum /= 2;  // Get same scaling
+            }
             delta2[axis] = delta1[axis];
             delta1[axis] = delta;
         } else {

src/main/main.c

@@ -541,7 +541,7 @@ int main(void) {
     init();
 
     /* Setup scheduler */
-    rescheduleTask(TASK_GYROPID, targetLooptime);
+    rescheduleTask(TASK_GYROPID, targetLooptime - INTERRUPT_WAIT_TIME);
 
     setTaskEnabled(TASK_GYROPID, true);
     setTaskEnabled(TASK_ACCEL, sensors(SENSOR_ACC));

src/main/mw.c

@@ -91,7 +91,7 @@ enum {
     ALIGN_MAG = 2
 };
 
-#define JITTER_DEBUG 0  // Specify debug value for jitter debug
+//#define JITTER_DEBUG 0  // Specify debug value for jitter debug
 
 /* VBAT monitoring interval (in microseconds) - 1s*/
 #define VBATINTERVAL (6 * 3500)
@@ -644,17 +644,6 @@ void processRx(void)
 static bool haveProcessedAnnexCodeOnce = false;
 #endif
 
-// Function for loop trigger
-bool taskMainPidLoopCheck(uint32_t currentDeltaTime) {
-	bool loopTrigger = false;
-
-    if (gyroSyncCheckUpdate() || (currentDeltaTime >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
-            loopTrigger = true;
-    }
-
-    return loopTrigger;
-}
-
 void taskMainPidLoop(void)
 {
     cycleTime = getTaskDeltaTime(TASK_SELF);
@@ -748,6 +737,16 @@ void taskMainPidLoop(void)
 #endif
 }
 
+// Function for loop trigger
+void taskMainPidLoopCheck(void) {
+    while (1) {
+        if (gyroSyncCheckUpdate() || (getTaskDeltaTime(TASK_SELF) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
+            taskMainPidLoop();
+            break;
+        }
+    }
+}
+
 void taskUpdateAccelerometer(void)
 {
     imuUpdateAccelerometer(&currentProfile->accelerometerTrims);

src/main/scheduler.c

@@ -27,7 +27,7 @@
 
 #include "drivers/system.h"
 
-#define SCHEDULER_DEBUG
+//#define SCHEDULER_DEBUG
 
 cfTaskId_e currentTaskId = TASK_NONE;
 
@@ -62,8 +62,7 @@ typedef struct {
 #endif
 } cfTask_t;
 
-bool taskMainPidLoopCheck(uint32_t currentDeltaTime);
-void taskMainPidLoop(void);
+void taskMainPidLoopCheck(void);
 void taskUpdateAccelerometer(void);
 void taskHandleSerial(void);
 void taskUpdateBeeper(void);
@@ -91,8 +90,7 @@ static cfTask_t cfTasks[TASK_COUNT] = {
 
     [TASK_GYROPID] = {
         .taskName = "GYRO/PID",
-        .checkFunc = taskMainPidLoopCheck,
-        .taskFunc = taskMainPidLoop,
+        .taskFunc = taskMainPidLoopCheck,
         .desiredPeriod = 1000,
         .staticPriority = TASK_PRIORITY_REALTIME,
     },