void voltageMeterESCInit(void)
{
#ifdef USE_ESC_SENSOR
memset(&voltageMeterESCState, 0, sizeof(voltageMeterESCState_t));
biquadFilterInitLPF(&voltageMeterESCState.filter, VBAT_LPF_FREQ, 50000); //50HZ Update
#endif
}
void filterServos(void)
{
static int16_t servoIdx;
static bool servoFilterIsSet;
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
#if defined(MIXER_DEBUG)
uint32_t startTime = micros();
#endif
if (servoMixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
}
}
#if defined(MIXER_DEBUG)
debug[0] = (int16_t)(micros() - startTime);
#endif
}
void voltageMeterInit(void)
{
for (uint8_t i = 0; i < MAX_VOLTAGE_METERS && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) {
// store the battery voltage with some other recent battery voltage readings
voltageMeterState_t *state = &voltageMeterStates[i];
biquadFilterInitLPF(&state->vbatFilterState, VBATT_LPF_FREQ, 50000);
}
}
void voltageMeterADCInit(void)
{
for (uint8_t i = 0; i < MAX_VOLTAGE_SENSOR_ADC && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) {
// store the battery voltage with some other recent battery voltage readings
voltageMeterADCState_t *state = &voltageMeterADCStates[i];
memset(state, 0, sizeof(voltageMeterADCState_t));
biquadFilterInitLPF(&state->filter, VBAT_LPF_FREQ, 50000);
}
}
void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz)
{
filterApplyFnPtr *lowpassFilterApplyFn;
gyroLowpassFilter_t *lowpassFilter = NULL;
switch (slot) {
case FILTER_LOWPASS:
lowpassFilterApplyFn = &gyroSensor->lowpassFilterApplyFn;
lowpassFilter = gyroSensor->lowpassFilter;
break;
case FILTER_LOWPASS2:
lowpassFilterApplyFn = &gyroSensor->lowpass2FilterApplyFn;
lowpassFilter = gyroSensor->lowpass2Filter;
break;
default:
return;
}
// Establish some common constants
const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
const float gyroDt = gyro.targetLooptime * 1e-6f;
// Gain could be calculated a little later as it is specific to the pt1/bqrcf2/fkf branches
const float gain = pt1FilterGain(lpfHz, gyroDt);
// Dereference the pointer to null before checking valid cutoff and filter
// type. It will be overridden for positive cases.
*lowpassFilterApplyFn = nullFilterApply;
// If lowpass cutoff has been specified and is less than the Nyquist frequency
if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
switch (type) {
case FILTER_PT1:
*lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterInit(&lowpassFilter[axis].pt1FilterState, gain);
}
break;
case FILTER_BIQUAD:
#ifdef USE_DYN_LPF
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
#else
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
#endif
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
}
break;
}
}
}
void gyroInit(void)
{
if (gyroConfig()->gyro_soft_lpf_hz) { // Initialisation needs to happen once sampling rate is known
const uint16_t gyroPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz);
for (int axis = 0; axis < 3; axis++) {
biquadFilterInitNotch(&gyroFilterNotch[axis], gyroPeriodUs, gyroConfig()->gyro_soft_notch_hz, gyroConfig()->gyro_soft_notch_cutoff_hz);
if (gyroConfig()->gyro_soft_type == FILTER_BIQUAD) {
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig()->gyro_soft_lpf_hz, gyroPeriodUs);
} else {
const float gyroDt = (float)gyroPeriodUs * 0.000001f;
pt1FilterInit(&gyroFilterPt1[axis], gyroConfig()->gyro_soft_lpf_hz, gyroDt);
}
}
}
}
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 biquadFilter_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) {
biquadFilterInitLPF(&filteredCycleTimeState, 1, gyro.targetLooptime);
filterInitialised = true;
}
filteredCycleTime = biquadFilterApply(&filteredCycleTimeState, (float) cycleTime);
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;
}
}
static void updateBatteryVoltage(void)
{
static biquadFilter_t vbatFilter;
static bool vbatFilterIsInitialised;
// store the battery voltage with some other recent battery voltage readings
uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
if (!vbatFilterIsInitialised) {
biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
vbatFilterIsInitialised = true;
}
vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
vbat = batteryAdcToVoltage(vbatSample);
if (debugMode == DEBUG_BATTERY) debug[1] = vbat;
}
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
int16_t accADCRaw[XYZ_AXIS_COUNT];
if (!acc.read(accADCRaw)) {
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
accSmooth[axis] = accADCRaw[axis];
}
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
accFilterInitialised = true;
}
}
if (accFilterInitialised) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
}
}
}
alignSensors(accSmooth, accSmooth, accAlign);
if (!isAccelerationCalibrationComplete()) {
performAcclerationCalibration(rollAndPitchTrims);
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
performInflightAccelerationCalibration(rollAndPitchTrims);
}
applyAccelerationTrims(accelerationTrims);
}
void gyroInit(void)
{
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
if (gyroSoftLpfType == FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else if (gyroSoftLpfType == FILTER_PT1)
gyroDt = (float) gyro.targetLooptime * 0.000001f;
else
firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
if ((gyroSoftNotchHz1 || gyroSoftNotchHz2) && gyro.targetLooptime) {
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
}
}
}
static bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfig)
{
dev->mpuIntExtiConfig = extiConfig;
gyroSensor_e gyroHardware = GYRO_AUTODETECT;
dev->gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_AUTODETECT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(dev)) {
gyroHardware = GYRO_MPU6050;
#ifdef GYRO_MPU6050_ALIGN
dev->gyroAlign = GYRO_MPU6050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(dev)) {
gyroHardware = GYRO_L3G4200D;
#ifdef GYRO_L3G4200D_ALIGN
dev->gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(dev)) {
gyroHardware = GYRO_MPU3050;
#ifdef GYRO_MPU3050_ALIGN
dev->gyroAlign = GYRO_MPU3050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(dev)) {
gyroHardware = GYRO_L3GD20;
#ifdef GYRO_L3GD20_ALIGN
dev->gyroAlign = GYRO_L3GD20_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(dev)) {
gyroHardware = GYRO_MPU6000;
#ifdef GYRO_MPU6000_ALIGN
dev->gyroAlign = GYRO_MPU6000_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU6500:
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#ifdef USE_GYRO_SPI_MPU6500
if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
#else
if (mpu6500GyroDetect(dev)) {
#endif
gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
dev->gyroAlign = GYRO_MPU6500_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(dev)) {
gyroHardware = GYRO_FAKE;
break;
}
#endif
; // fallthrough
case GYRO_NONE:
gyroHardware = GYRO_NONE;
}
addBootlogEvent6(BOOT_EVENT_GYRO_DETECTION, BOOT_EVENT_FLAGS_NONE, gyroHardware, 0, 0, 0);
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
bool gyroInit(const gyroConfig_t *gyroConfigToUse)
{
gyroConfig = gyroConfigToUse;
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetect(&gyro.dev);
mpuReset = gyro.dev.mpuConfiguration.reset;
#endif
if (!gyroDetect(&gyro.dev, extiConfig)) {
return false;
}
// After refactoring this function is always called after gyro sampling rate is known, so
// no additional condition is required
// Set gyro sample rate before driver initialisation
gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->looptime, gyroConfig->gyro_lpf, gyroConfig->gyroSync, gyroConfig->gyroSyncDenominator);
// driver initialisation
gyro.dev.init(&gyro.dev);
if (gyroConfig->gyro_soft_lpf_hz) {
for (int axis = 0; axis < 3; axis++) {
#ifdef ASYNC_GYRO_PROCESSING
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, getGyroUpdateRate());
#else
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
#endif
}
}
return true;
}
