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); } }