Fix BB / Acc reading / Optimize scheduler

src/main/drivers/gyro_sync.h

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

src/main/io/serial_cli.c

@@ -555,7 +555,7 @@ const clivalue_t valueTable[] = {
 #ifdef CC3D
     { "enable_buzzer_p6",           VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.use_buzzer_p6, .config.lookup = { TABLE_OFF_ON } },
 #endif
-    { "motor_pwm_rate",             VAR_UINT16 | MASTER_VALUE,  &masterConfig.motor_pwm_rate, .config.minmax = { 50,  32000 } },
+    { "motor_pwm_rate",             VAR_UINT16 | MASTER_VALUE,  &masterConfig.motor_pwm_rate, .config.minmax = { 300,  5000 } },
     { "servo_pwm_rate",             VAR_UINT16 | MASTER_VALUE,  &masterConfig.servo_pwm_rate, .config.minmax = { 50,  498 } },
 
     { "disarm_kill_switch",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },

src/main/main.c

@@ -663,7 +663,7 @@ int main(void) {
     init();
 
     /* Setup scheduler */
-    rescheduleTask(TASK_GYROPID, targetLooptime - INTERRUPT_WAIT_TIME);
+    rescheduleTask(TASK_GYROPID, targetLooptime);
 
     setTaskEnabled(TASK_GYROPID, true);
     setTaskEnabled(TASK_MOTOR, true);

src/main/mw.c

@@ -724,6 +724,13 @@ void subTasksMainPidLoop(void) {
 }
 
 void taskMotorUpdate(void) {
+    if (debugMode == DEBUG_CYCLETIME) {
+        static uint32_t previousMotorUpdateTime;
+        uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
+        debug[2] = currentDeltaTime;
+        debug[3] = currentDeltaTime - previousMotorUpdateTime;
+        previousMotorUpdateTime = currentDeltaTime;
+    }
 
 #ifdef USE_SERVOS
     filterServos();
@@ -731,14 +738,6 @@ void taskMotorUpdate(void) {
 #endif
 
     if (motorControlEnable) {
-        if (debugMode == DEBUG_CYCLETIME) {
-            static uint32_t previousMotorUpdateTime;
-            uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
-            debug[2] = currentDeltaTime;
-            debug[3] = currentDeltaTime - previousMotorUpdateTime;
-            previousMotorUpdateTime = currentDeltaTime;
-        }
-
         writeMotors();
     }
 }
@@ -746,6 +745,7 @@ void taskMotorUpdate(void) {
 // Function for loop trigger
 void taskMainPidLoopCheck(void) {
     static uint32_t previousTime;
+    static bool runTaskMainSubprocesses;
 
     uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
 
@@ -761,9 +761,9 @@ void taskMainPidLoopCheck(void) {
         if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
             static uint8_t pidUpdateCountdown;
 
-            if (realTimeCycle) {
+            if (runTaskMainSubprocesses) {
                 subTasksMainPidLoop();
-                realTimeCycle = false;
+                runTaskMainSubprocesses = false;
             }
 
             imuUpdateGyro();
@@ -773,7 +773,7 @@ void taskMainPidLoopCheck(void) {
             } else {
                 pidUpdateCountdown = masterConfig.pid_process_denom - 1;
                 taskMainPidLoop();
-                //realTimeCycle = true;
+                runTaskMainSubprocesses = true;
             }
 
             break;

src/main/scheduler.c

@@ -33,7 +33,6 @@ cfTaskId_e currentTaskId = TASK_NONE;
 
 static uint32_t totalWaitingTasks;
 static uint32_t totalWaitingTasksSamples;
-static uint32_t realtimeGuardInterval;
 
 bool realTimeCycle = true;
 
@@ -101,21 +100,21 @@ static cfTask_t cfTasks[TASK_COUNT] = {
 	    .subTaskName = "GYRO",
         .taskFunc = taskMainPidLoopCheck,
         .desiredPeriod = 1000,
-        .staticPriority = TASK_PRIORITY_REALTIME,
+        .staticPriority = TASK_PRIORITY_HIGH,
     },
 
     [TASK_MOTOR] = {
         .taskName = "MOTOR",
         .taskFunc = taskMotorUpdate,
         .desiredPeriod = 1000,
-        .staticPriority = TASK_PRIORITY_HIGH,
+        .staticPriority = TASK_PRIORITY_REALTIME,
     },
 
     [TASK_ACCEL] = {
         .taskName = "ACCEL",
         .taskFunc = taskUpdateAccelerometer,
         .desiredPeriod = 1000000 / 100,
-        .staticPriority = TASK_PRIORITY_MEDIUM,
+        .staticPriority = TASK_PRIORITY_LOW,
     },
 
     [TASK_SERIAL] = {
@@ -129,14 +128,14 @@ static cfTask_t cfTasks[TASK_COUNT] = {
         .taskName = "BEEPER",
         .taskFunc = taskUpdateBeeper,
         .desiredPeriod = 1000000 / 100,     // 100 Hz
-        .staticPriority = TASK_PRIORITY_MEDIUM,
+        .staticPriority = TASK_PRIORITY_LOW,
     },
 
     [TASK_BATTERY] = {
         .taskName = "BATTERY",
         .taskFunc = taskUpdateBattery,
         .desiredPeriod = 1000000 / 50,      // 50 Hz
-        .staticPriority = TASK_PRIORITY_MEDIUM,
+        .staticPriority = TASK_PRIORITY_LOW,
     },
 
     [TASK_RX] = {
@@ -238,12 +237,8 @@ static cfTask_t cfTasks[TASK_COUNT] = {
 
 uint16_t averageSystemLoadPercent = 0;
 
-#define MAX_GUARD_INTERVAL 80
-#define MIN_GUARD_INTERVAL 0
-
 void taskSystem(void)
 {
-    uint8_t taskId;
 
     /* Calculate system load */
     if (totalWaitingTasksSamples > 0) {
@@ -251,19 +246,6 @@ void taskSystem(void)
         totalWaitingTasksSamples = 0;
         totalWaitingTasks = 0;
     }
-
-    /* Calculate guard interval */
-    uint32_t maxNonRealtimeTaskTime = 0;
-
-    for (taskId = 0; taskId < TASK_COUNT; taskId++) {
-        if (cfTasks[taskId].staticPriority != TASK_PRIORITY_REALTIME) {
-            maxNonRealtimeTaskTime = MAX(maxNonRealtimeTaskTime, cfTasks[taskId].averageExecutionTime);
-        }
-    }
-    realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, MIN_GUARD_INTERVAL, MAX_GUARD_INTERVAL);
-#if defined SCHEDULER_DEBUG
-    debug[2] = realtimeGuardInterval;
-#endif
 }
 
 #ifndef SKIP_TASK_STATISTICS
@@ -318,20 +300,12 @@ uint32_t getTaskDeltaTime(cfTaskId_e taskId)
 
 void scheduler(void)
 {
+    static uint16_t maxTaskCalculationReset = MAXT_TIME_TICKS_TO_RESET;
     uint8_t taskId;
     uint8_t selectedTaskId;
     uint8_t selectedTaskDynPrio;
     uint16_t waitingTasks = 0;
     uint32_t timeToNextRealtimeTask = UINT32_MAX;
-    uint32_t currentRealtimeGuardInterval;
-
-    static uint16_t maxTaskCalculationReset = MAXT_TIME_TICKS_TO_RESET;
-
-    if (realTimeCycle) {
-        currentRealtimeGuardInterval = realtimeGuardInterval;
-    } else {
-        currentRealtimeGuardInterval = MIN_GUARD_INTERVAL;
-    }
 
     /* Cache currentTime */
     currentTime = micros();
@@ -354,7 +328,7 @@ void scheduler(void)
         }
     }
 
-    bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > currentRealtimeGuardInterval);
+    bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > 0);
 
     /* Update task dynamic priorities */
     for (taskId = 0; taskId < TASK_COUNT; taskId++) {

src/main/scheduler.h

@@ -88,7 +88,6 @@ typedef enum {
 
 extern uint16_t cpuLoad;
 extern uint16_t averageSystemLoadPercent;
-extern bool realTimeCycle;
 
 void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo);
 void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);