// Function for loop trigger FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs) { static uint32_t pidUpdateCounter = 0; #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC) if (lockMainPID() != 0) return; #endif // DEBUG_PIDLOOP, timings for: // 0 - gyroUpdate() // 1 - subTaskPidController() // 2 - subTaskMotorUpdate() // 3 - subTaskPidSubprocesses() gyroUpdate(currentTimeUs); DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs); if (pidUpdateCounter++ % pidConfig()->pid_process_denom == 0) { subTaskRcCommand(currentTimeUs); subTaskPidController(currentTimeUs); subTaskMotorUpdate(currentTimeUs); subTaskPidSubprocesses(currentTimeUs); } if (debugMode == DEBUG_CYCLETIME) { debug[0] = getTaskDeltaTime(TASK_SELF); debug[1] = averageSystemLoadPercent; } }
// Function for loop trigger void taskMainPidLoop(timeUs_t currentTimeUs) { static bool runTaskMainSubprocesses; static uint8_t pidUpdateCountdown; #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC) if(lockMainPID() != 0) return; #endif if (debugMode == DEBUG_CYCLETIME) { debug[0] = getTaskDeltaTime(TASK_SELF); debug[1] = averageSystemLoadPercent; } if (runTaskMainSubprocesses) { subTaskMainSubprocesses(currentTimeUs); runTaskMainSubprocesses = false; } // DEBUG_PIDLOOP, timings for: // 0 - gyroUpdate() // 1 - pidController() // 2 - subTaskMainSubprocesses() // 3 - subTaskMotorUpdate() uint32_t startTime = 0; if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} gyroUpdate(); DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - startTime); if (pidUpdateCountdown) { pidUpdateCountdown--; } else { pidUpdateCountdown = setPidUpdateCountDown(); subTaskPidController(currentTimeUs); subTaskMotorUpdate(); runTaskMainSubprocesses = true; } }