Taskmain rework part II

INT wait

Fix auto settings F3

src/main/config/config.c

@@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 126;
+static const uint8_t EEPROM_CONF_VERSION = 127;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -415,6 +415,8 @@ static void resetConf(void)
 
     masterConfig.pid_process_denom = 1;
 
+    masterConfig.debug_mode = 0;
+
     resetAccelerometerTrims(&masterConfig.accZero);
 
     resetSensorAlignment(&masterConfig.sensorAlignmentConfig);

src/main/config/config_master.h

@@ -67,6 +67,8 @@ typedef struct master_t {
 
     uint8_t pid_process_denom;              // Processing denominator for PID controller vs gyro sampling rate
 
+    uint8_t debug_mode;                     // Processing denominator for PID controller vs gyro sampling rate
+
     gyroConfig_t gyroConfig;
 
     uint8_t mag_hardware;                   // Which mag hardware to use on boards with more than one device

src/main/drivers/gyro_sync.h

@@ -5,7 +5,7 @@
  *      Author: borisb
  */
 
-#define INTERRUPT_WAIT_TIME 5
+#define INTERRUPT_WAIT_TIME 4
 
 extern uint32_t targetLooptime;
 

src/main/flight/pid.c

@@ -78,7 +78,7 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
 pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
 
 void setTargetPidLooptime(uint8_t pidProcessDenom) {
-	targetPidLooptime = targetLooptime / pidProcessDenom;
+	targetPidLooptime = targetLooptime * pidProcessDenom;
 }
 
 void pidResetErrorAngle(void)

src/main/io/serial_cli.c

@@ -526,6 +526,7 @@ const clivalue_t valueTable[] = {
     { "rc_smoothing",               VAR_INT8   | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON } },
     { "roll_yaw_cam_mix_degrees",   VAR_UINT8  | MASTER_VALUE,  &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0,  50 } },
     { "max_aux_channels",           VAR_UINT8  | MASTER_VALUE,  &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0,  13 } },
+    { "debug_mode",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.debug_mode, .config.lookup = { TABLE_OFF_ON } },
 
     { "min_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } },
     { "max_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } },
@@ -2581,24 +2582,31 @@ static void cliTasks(char *cmdline)
             uint16_t taskFrequency;
             uint16_t subTaskFrequency;
 
-            if (taskId == TASK_GYROPID) {
-                subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f));
-                taskFrequency = subTaskFrequency / masterConfig.pid_process_denom;
-            } else {
-                taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f));
-            }
-
             uint32_t taskTotalTime = taskInfo.totalExecutionTime / 1000;
 
-            cliPrintf("%d - (%s) max: %dus, avg: %dus, rate: %dhz, total: ", taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency);
+            if (taskId == TASK_GYROPID) {
+                subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f));
+                if (masterConfig.pid_process_denom > 1) {
+                    taskFrequency = subTaskFrequency / masterConfig.pid_process_denom;
+                    cliPrintf("%d - (%s) ", taskId, taskInfo.taskName);
+                } else {
+                    taskFrequency = subTaskFrequency;
+                    cliPrintf("%d - (%s/%s) ", taskId, taskInfo.subTaskName, taskInfo.taskName);
+                }
+            } else {
+                taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f));
+                cliPrintf("%d - (%s) ", taskId, taskInfo.taskName);
+            }
+
+            cliPrintf("max: %dus, avg: %dus, rate: %dhz, total: ", taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency);
 
             if (taskTotalTime >= 1000) {
-                cliPrintf("%ds", taskTotalTime / 1000);
+                cliPrintf("%dsec", taskTotalTime / 1000);
             } else {
                 cliPrintf("%dms", taskTotalTime);
             }
 
-            if (taskId == TASK_GYROPID) cliPrintf(" (%s rate: %dhz)", taskInfo.subTaskName, subTaskFrequency);
+            if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) cliPrintf("\r\n- - (%s)  rate: %dhz", taskInfo.subTaskName, subTaskFrequency);
             cliPrintf("\r\n", taskTotalTime);
         }
     }

src/main/io/serial_msp.c

@@ -114,7 +114,10 @@ void setGyroSamplingSpeed(uint16_t looptime) {
             gyroSampleRate = 125;
             maxDivider = 8;
             masterConfig.pid_process_denom = 1;
-            if (looptime < 250) {
+            masterConfig.acc_hardware = 0;
+            masterConfig.baro_hardware = 0;
+            masterConfig.mag_hardware = 0;
+            if (looptime < 375) {
                 masterConfig.acc_hardware = 1;
                 masterConfig.baro_hardware = 1;
                 masterConfig.mag_hardware = 1;
@@ -123,6 +126,9 @@ void setGyroSamplingSpeed(uint16_t looptime) {
         } else {
             masterConfig.gyro_lpf = 1;
             masterConfig.pid_process_denom = 1;
+            masterConfig.acc_hardware = 0;
+            masterConfig.baro_hardware = 0;
+            masterConfig.mag_hardware = 0;
         }
 #else
         if (looptime < 1000) {
@@ -133,14 +139,18 @@ void setGyroSamplingSpeed(uint16_t looptime) {
             gyroSampleRate = 125;
             maxDivider = 8;
             masterConfig.pid_process_denom = 1;
+            masterConfig.emf_avoidance = 0;
+            if (currentProfile->pidProfile.pidController == 2) masterConfig.pid_process_denom = 2;
             if (looptime < 250) {
                 masterConfig.pid_process_denom = 3;
-            } else if (looptime < 500) {
+                masterConfig.emf_avoidance = 1;
+            } else if (looptime < 375) {
                 if (currentProfile->pidProfile.pidController == 2) {
                     masterConfig.pid_process_denom = 3;
                 } else {
                     masterConfig.pid_process_denom = 2;
                 }
+                masterConfig.emf_avoidance = 1;
             }
         } else {
             masterConfig.gyro_lpf = 1;

src/main/mw.c

@@ -286,10 +286,6 @@ void annexCode(void)
         rcCommand[PITCH] = rcCommand_PITCH;
     }
 
-    if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
-        filterRc();
-    }
-
     // experimental scaling of RC command to FPV cam angle
     if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) {
         scaleRcCommandToFpvCamAngle();
@@ -644,59 +640,6 @@ static bool haveProcessedAnnexCodeOnce = false;
 
 void taskMainPidLoop(void)
 {
-    imuUpdateAttitude();
-
-    annexCode();
-
-#if defined(BARO) || defined(SONAR)
-    haveProcessedAnnexCodeOnce = true;
-#endif
-
-#ifdef MAG
-        if (sensors(SENSOR_MAG)) {
-            updateMagHold();
-        }
-#endif
-
-#ifdef GTUNE
-        updateGtuneState();
-#endif
-
-#if defined(BARO) || defined(SONAR)
-        if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
-            if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
-                applyAltHold(&masterConfig.airplaneConfig);
-            }
-        }
-#endif
-
-    // If we're armed, at minimum throttle, and we do arming via the
-    // sticks, do not process yaw input from the rx.  We do this so the
-    // motors do not spin up while we are trying to arm or disarm.
-    // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
-    if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
-#ifndef USE_QUAD_MIXER_ONLY
-#ifdef USE_SERVOS
-                && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
-#endif
-                && masterConfig.mixerMode != MIXER_AIRPLANE
-                && masterConfig.mixerMode != MIXER_FLYING_WING
-#endif
-    ) {
-        rcCommand[YAW] = 0;
-    }
-
-    if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
-        rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value);
-    }
-
-#ifdef GPS
-    if (sensors(SENSOR_GPS)) {
-        if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
-            updateGpsStateForHomeAndHoldMode();
-        }
-    }
-#endif
 
     // PID - note this is function pointer set by setPIDController()
     pid_controller(
@@ -714,30 +657,98 @@ void taskMainPidLoop(void)
     writeServos();
 #endif
 
+    if (masterConfig.debug_mode) {
+        static uint32_t previousMotorUpdateTime, motorCycleTime;
+
+        motorCycleTime = micros() - previousMotorUpdateTime;
+        previousMotorUpdateTime = micros();
+        debug[2] = motorCycleTime;
+        debug[3] = motorCycleTime - targetPidLooptime;
+    }
+
     if (motorControlEnable) {
         writeMotors();
     }
+}
 
-#ifdef USE_SDCARD
+void subTasksPreMainPidLoop(void) {
-    afatfs_poll();
+    imuUpdateAttitude();
-#endif
 
-#ifdef BLACKBOX
+    if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
-    if (!cliMode && feature(FEATURE_BLACKBOX)) {
+        filterRc();
-        handleBlackbox();
     }
-#endif
 
-#ifdef TRANSPONDER
+    #if defined(BARO) || defined(SONAR)
-    updateTransponder();
+        haveProcessedAnnexCodeOnce = true;
-#endif
+    #endif
+
+    #ifdef MAG
+            if (sensors(SENSOR_MAG)) {
+                updateMagHold();
+            }
+    #endif
+
+    #ifdef GTUNE
+            updateGtuneState();
+    #endif
+
+    #if defined(BARO) || defined(SONAR)
+            if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
+                if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
+                    applyAltHold(&masterConfig.airplaneConfig);
+                }
+            }
+    #endif
+
+        // If we're armed, at minimum throttle, and we do arming via the
+        // sticks, do not process yaw input from the rx.  We do this so the
+        // motors do not spin up while we are trying to arm or disarm.
+        // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
+        if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
+    #ifndef USE_QUAD_MIXER_ONLY
+    #ifdef USE_SERVOS
+                    && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
+    #endif
+                    && masterConfig.mixerMode != MIXER_AIRPLANE
+                    && masterConfig.mixerMode != MIXER_FLYING_WING
+    #endif
+        ) {
+            rcCommand[YAW] = 0;
+        }
+
+        if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
+            rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value);
+        }
+
+    #ifdef GPS
+        if (sensors(SENSOR_GPS)) {
+            if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
+                updateGpsStateForHomeAndHoldMode();
+            }
+        }
+    #endif
+}
+
+void subTasksPostMainPidLoop(void) {
+    #ifdef USE_SDCARD
+        afatfs_poll();
+    #endif
+
+    #ifdef BLACKBOX
+        if (!cliMode && feature(FEATURE_BLACKBOX)) {
+            handleBlackbox();
+        }
+    #endif
+
+    #ifdef TRANSPONDER
+        updateTransponder();
+    #endif
 }
 
 // Function for loop trigger
 void taskMainPidLoopCheck(void) {
     static uint32_t previousTime;
     static uint8_t pidUpdateCountdown;
-    static uint32_t previousPidUpdateTime, pidCycleTime;
 
     if (!pidUpdateCountdown) pidUpdateCountdown = masterConfig.pid_process_denom;
 
@@ -746,33 +757,32 @@ void taskMainPidLoopCheck(void) {
     cycleTime = micros() - previousTime;
     previousTime = micros();
 
-    // Debugging parameters
+    if (masterConfig.debug_mode) {
-    debug[0] = cycleTime;
+        // Debugging parameters
-    debug[1] = cycleTime - targetLooptime;
+        debug[0] = cycleTime;
-    debug[2] = averageSystemLoadPercent;
+        debug[1] = averageSystemLoadPercent;
-    debug[3] = pidCycleTime;
+    }
 
-    while (1) {
+    while (true) {
         if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
 
             imuUpdateGyro();
 
-            switch (pidUpdateCountdown) {
+            if (pidUpdateCountdown == 2 || masterConfig.pid_process_denom == 1) {
-                case(1):
+                subTasksPreMainPidLoop();
-                    pidCycleTime = micros() - previousPidUpdateTime;
+                realTimeCycle = true;  // Set realtime guard interval
-                    previousPidUpdateTime = micros();
-
-                    pidUpdateCountdown = masterConfig.pid_process_denom;
-                    taskMainPidLoop();
-                    if (masterConfig.pid_process_denom > 1) realTimeCycle = false;
-                    break;
-                case(2):
-				    realTimeCycle = true;
-                    pidUpdateCountdown--;
-                    break;
-                default:
-                    pidUpdateCountdown--;
             }
+
+            if (pidUpdateCountdown == 1) {
+                pidUpdateCountdown = masterConfig.pid_process_denom;
+                taskMainPidLoop();
+                if (masterConfig.pid_process_denom > 1) realTimeCycle = false;
+            } else {
+                pidUpdateCountdown--;
+            }
+
+            if (realTimeCycle) subTasksPostMainPidLoop();
+
             break;
         }
     }
@@ -843,6 +853,7 @@ void taskUpdateRxMain(void)
         }
     }
 #endif
+    annexCode();
 }
 
 #ifdef GPS