Jitter enhancements
This commit is contained in:
parent
d8e62d0508
commit
af6f9de903
|
@ -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) {
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue