void filterRc(bool isRXDataNew)
{
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
static biquad_t filteredCycleTimeState;
static bool filterInitialised;
uint16_t filteredCycleTime;
uint16_t rxRefreshRate;
// Set RC refresh rate for sampling and channels to filter
initRxRefreshRate(&rxRefreshRate);
// Calculate average cycle time (1Hz LPF on cycle time)
if (!filterInitialised) {
filterInitBiQuad(1, &filteredCycleTimeState, 0);
filterInitialised = true;
}
filteredCycleTime = filterApplyBiQuad((float) cycleTime, &filteredCycleTimeState);
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
if (isRXDataNew) {
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}
factor = rcInterpolationFactor - 1;
} else {
factor--;
}
// Interpolate steps of rcCommand
if (factor > 0) {
for (int channel=0; channel < 4; channel++) {
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
}
} else {
factor = 0;
}
}
void gyroUpdate(void)
{
int8_t axis;
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;
}
// Prepare a copy of int32_t gyroADC for mangling to prevent overflow
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = gyroADCRaw[axis];
if (gyroLpfCutHz) {
if (!gyroFilterInitialised) {
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (axis = 0; axis < 3; axis++) {
filterInitBiQuad(gyroLpfCutHz, &gyroFilterState[axis], 0);
}
gyroFilterInitialised = true;
}
}
if (gyroFilterInitialised) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = lrintf(filterApplyBiQuad((float) gyroADC[axis], &gyroFilterState[axis]));
}
}
}
alignSensors(gyroADC, gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();
}
void filterServos(void)
{
uint8_t servoIdx;
if (mixerConfig->servo_lowpass_enable) {
// Initialize servo lowpass filter (servos are calculated at looptime rate)
if (!servoFilterIsSet) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
filterInitBiQuad(mixerConfig->servo_lowpass_freq, &servoFitlerState[servoIdx], 0);
}
servoFilterIsSet = true;
}
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
// Apply servo lowpass filter and do sanity cheching
servo[servoIdx] = (int16_t) filterApplyBiQuad((float)servo[servoIdx], &servoFitlerState[servoIdx]);
}
}
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
}
}
