From af6f9de9039542b590ee71b97b76ea6a74794010 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 20 Sep 2015 23:55:22 +0200 Subject: [PATCH] Jitter enhancements --- src/main/flight/pid.c | 7 ++++--- src/main/mw.c | 3 ++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a0458c06e2..7004deed77 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -29,6 +29,7 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "drivers/gyro_sync.h" #include "sensors/sensors.h" #include "sensors/gyro.h" @@ -210,7 +211,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); } - DTerm = constrainf((delta / 3) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); + DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); @@ -755,7 +756,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Time correction (to avoid different I scaling for different builds based on average cycle time) // is normalized to cycle time = 2048. - errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis]; + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -768,7 +769,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // would be scaled by different dt each time. Division by dT fixes that. - delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6; + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6; // Dterm delta low pass if (pidProfile->dterm_cut_hz) { diff --git a/src/main/mw.c b/src/main/mw.c index 6a10852335..3f8ebe9cc3 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -93,6 +93,7 @@ enum { /* IBat monitoring interval (in microseconds) - 6 default looptimes */ #define IBATINTERVAL (6 * 3500) #define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro +#define PREVENT_RX_PROCESS_PRE_LOOP_TRIGGER 90 // Prevent RX processing before expected loop trigger uint32_t currentTime = 0; uint32_t previousTime = 0; @@ -733,7 +734,7 @@ void loop(void) updateRx(currentTime); - if (shouldProcessRx(currentTime)) { + if (shouldProcessRx(currentTime) && !((int32_t)(currentTime - (loopTime - PREVENT_RX_PROCESS_PRE_LOOP_TRIGGER)) >= 0)) { processRx(); isRXDataNew = true;