From fdee2e52283c7578f5e41a2680eea4bcc4c8a01f Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Wed, 16 May 2018 10:12:37 -0400 Subject: [PATCH] Break out rc interpolation as a separate task and run before pid controller and motor update Fixes an issue with motor "spikes" when rc interpolation was enabled for throttle. The problem was that the smoothing was happening too late in the sequence and the earlier process subTaskMotorUpdate() would use the unsmoothed throttle value in the mixer if new rx data had come in between the last and current PID loop. Also because the setPointRate was calculated at the end of the smoothing, the PID controller would always be using the value from the previous loop iteration. --- src/main/fc/fc_core.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 5a58801899..c21e167027 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -905,8 +905,6 @@ static NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs) rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value); } - processRcCommand(); - #ifdef USE_SDCARD afatfs_poll(); #endif @@ -958,6 +956,12 @@ static void subTaskMotorUpdate(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime); } +static void subTaskRcCommand(timeUs_t currentTimeUs) +{ + processRcCommand(); + UNUSED(currentTimeUs); +} + // Function for loop trigger FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs) { @@ -976,6 +980,7 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs); if (pidUpdateCounter++ % pidConfig()->pid_process_denom == 0) { + subTaskRcCommand(currentTimeUs); subTaskPidController(currentTimeUs); subTaskMotorUpdate(currentTimeUs); subTaskMainSubprocesses(currentTimeUs);