src/main/sensors/acceleration.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21 #include <math.h>
  22
  23 #include "platform.h"
  24
  25 FILE_COMPILE_FOR_SPEED
  26
  27 #include "build/debug.h"
  28
  29 #include "common/axis.h"
  30 #include "common/filter.h"
  31 #include "common/maths.h"
  32 #include "common/utils.h"
  33 #include "common/calibration.h"
  34
  35 #include "config/config_reset.h"
  36 #include "config/parameter_group.h"
  37 #include "config/parameter_group_ids.h"
  38
  39 #include "drivers/accgyro/accgyro.h"
  40 #include "drivers/accgyro/accgyro_mpu.h"
  41 #include "drivers/accgyro/accgyro_mpu6000.h"
  42 #include "drivers/accgyro/accgyro_mpu6500.h"
  43 #include "drivers/accgyro/accgyro_mpu9250.h"
  44
  45 #include "drivers/accgyro/accgyro_bmi088.h"
  46 #include "drivers/accgyro/accgyro_bmi160.h"
  47 #include "drivers/accgyro/accgyro_bmi270.h"
  48 #include "drivers/accgyro/accgyro_icm20689.h"
  49 #include "drivers/accgyro/accgyro_icm42605.h"
  50 #include "drivers/accgyro/accgyro_fake.h"
  51 #include "drivers/sensor.h"
  52
  53 #include "fc/config.h"
  54 #include "fc/runtime_config.h"
  55 #include "fc/settings.h"
  56
  57 #include "io/beeper.h"
  58
  59 #include "sensors/acceleration.h"
  60 #include "sensors/battery.h"
  61 #include "sensors/boardalignment.h"
  62 #include "sensors/gyro.h"
  63 #include "sensors/sensors.h"
  64
  65 #ifdef USE_HARDWARE_REVISION_DETECTION
  66 #include "hardware_revision.h"
  67 #endif
  68
  69
  70 FASTRAM acc_t acc;                       // acc access functions
  71
  72 STATIC_FASTRAM zeroCalibrationVector_t zeroCalibration;
  73
  74 STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT];
  75
  76 STATIC_FASTRAM filter_t accFilter[XYZ_AXIS_COUNT];
  77 STATIC_FASTRAM filterApplyFnPtr accSoftLpfFilterApplyFn;
  78 STATIC_FASTRAM void *accSoftLpfFilter[XYZ_AXIS_COUNT];
  79
  80 static EXTENDED_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
  81 static EXTENDED_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
  82
  83 static EXTENDED_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
  84 static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
  85
  86 PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 5);
  87
  88 void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
  89 {
  90     RESET_CONFIG_2(accelerometerConfig_t, instance,
  91         .acc_hardware = SETTING_ACC_HARDWARE_DEFAULT,
  92         .acc_lpf_hz = SETTING_ACC_LPF_HZ_DEFAULT,
  93         .acc_notch_hz = SETTING_ACC_NOTCH_HZ_DEFAULT,
  94         .acc_notch_cutoff = SETTING_ACC_NOTCH_CUTOFF_DEFAULT,
  95         .acc_soft_lpf_type = SETTING_ACC_LPF_TYPE_DEFAULT
  96     );
  97     RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accZero,
  98         .raw[X] = SETTING_ACCZERO_X_DEFAULT,
  99         .raw[Y] = SETTING_ACCZERO_Y_DEFAULT,
 100         .raw[Z] = SETTING_ACCZERO_Z_DEFAULT
 101     );
 102     RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accGain,
 103          .raw[X] = SETTING_ACCGAIN_X_DEFAULT,
 104          .raw[Y] = SETTING_ACCGAIN_Y_DEFAULT,
 105          .raw[Z] = SETTING_ACCGAIN_Z_DEFAULT
 106     );
 107 }
 108
 109 static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 110 {
 111     accelerationSensor_e accHardware = ACC_NONE;
 112
 113     dev->accAlign = ALIGN_DEFAULT;
 114
 115     requestedSensors[SENSOR_INDEX_ACC] = accHardwareToUse;
 116
 117     switch (accHardwareToUse) {
 118     case ACC_AUTODETECT:
 119         FALLTHROUGH;
 120
 121 #ifdef USE_IMU_MPU6000
 122     case ACC_MPU6000:
 123         if (mpu6000AccDetect(dev)) {
 124             accHardware = ACC_MPU6000;
 125             break;
 126         }
 127         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 128         if (accHardwareToUse != ACC_AUTODETECT) {
 129             break;
 130         }
 131         FALLTHROUGH;
 132 #endif
 133
 134 #if defined(USE_IMU_MPU6500)
 135     case ACC_MPU6500:
 136         if (mpu6500AccDetect(dev)) {
 137             accHardware = ACC_MPU6500;
 138             break;
 139         }
 140         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 141         if (accHardwareToUse != ACC_AUTODETECT) {
 142             break;
 143         }
 144         FALLTHROUGH;
 145 #endif
 146
 147 #if defined(USE_IMU_MPU9250)
 148     case ACC_MPU9250:
 149         if (mpu9250AccDetect(dev)) {
 150             accHardware = ACC_MPU9250;
 151             break;
 152         }
 153         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 154         if (accHardwareToUse != ACC_AUTODETECT) {
 155             break;
 156         }
 157         FALLTHROUGH;
 158 #endif
 159
 160 #if defined(USE_IMU_BMI160)
 161     case ACC_BMI160:
 162         if (bmi160AccDetect(dev)) {
 163             accHardware = ACC_BMI160;
 164             break;
 165         }
 166         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 167         if (accHardwareToUse != ACC_AUTODETECT) {
 168             break;
 169         }
 170         FALLTHROUGH;
 171 #endif
 172
 173 #if defined(USE_IMU_BMI088)
 174     case ACC_BMI088:
 175         if (bmi088AccDetect(dev)) {
 176             accHardware = ACC_BMI088;
 177             break;
 178         }
 179         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 180         if (accHardwareToUse != ACC_AUTODETECT) {
 181             break;
 182         }
 183         FALLTHROUGH;
 184 #endif
 185
 186 #ifdef USE_IMU_ICM20689
 187     case ACC_ICM20689:
 188         if (icm20689AccDetect(dev)) {
 189             accHardware = ACC_ICM20689;
 190             break;
 191         }
 192         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 193         if (accHardwareToUse != ACC_AUTODETECT) {
 194             break;
 195         }
 196         FALLTHROUGH;
 197 #endif
 198
 199 #ifdef USE_IMU_ICM42605
 200     case ACC_ICM42605:
 201         if (icm42605AccDetect(dev)) {
 202             accHardware = ACC_ICM42605;
 203             break;
 204         }
 205         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 206         if (accHardwareToUse != ACC_AUTODETECT) {
 207             break;
 208         }
 209         FALLTHROUGH;
 210 #endif
 211
 212 #ifdef USE_IMU_BMI270
 213     case ACC_BMI270:
 214         if (bmi270AccDetect(dev)) {
 215             accHardware = ACC_BMI270;
 216             break;
 217         }
 218         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 219         if (accHardwareToUse != ACC_AUTODETECT) {
 220             break;
 221         }
 222         FALLTHROUGH;
 223 #endif
 224
 225 #ifdef USE_IMU_FAKE
 226     case ACC_FAKE:
 227         if (fakeAccDetect(dev)) {
 228             accHardware = ACC_FAKE;
 229             break;
 230         }
 231         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 232         if (accHardwareToUse != ACC_AUTODETECT) {
 233             break;
 234         }
 235         FALLTHROUGH;
 236 #endif
 237
 238     default:
 239     case ACC_NONE: // disable ACC
 240         accHardware = ACC_NONE;
 241         break;
 242     }
 243
 244     if (accHardware == ACC_NONE) {
 245         return false;
 246     }
 247
 248     detectedSensors[SENSOR_INDEX_ACC] = accHardware;
 249     sensorsSet(SENSOR_ACC);
 250     return true;
 251 }
 252
 253 bool accInit(uint32_t targetLooptime)
 254 {
 255     memset(&acc, 0, sizeof(acc));
 256
 257     // Set inertial sensor tag (for dual-gyro selection)
 258 #ifdef USE_DUAL_GYRO
 259     acc.dev.imuSensorToUse = gyroConfig()->gyro_to_use;     // Use the same selection from gyroConfig()
 260 #else
 261     acc.dev.imuSensorToUse = 0;
 262 #endif
 263
 264     if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
 265         return false;
 266     }
 267
 268     acc.dev.acc_1G = 256; // set default
 269     acc.dev.initFn(&acc.dev);
 270     acc.accTargetLooptime = targetLooptime;
 271     acc.accClipCount = 0;
 272     accInitFilters();
 273
 274     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 275         acc.extremes[axis].min = 100;
 276         acc.extremes[axis].max = -100;
 277     }
 278
 279     return true;
 280 }
 281
 282 static bool calibratedPosition[6];
 283 static int32_t accSamples[6][3];
 284
 285 uint8_t accGetCalibrationAxisFlags(void)
 286 {
 287     if (STATE(ACCELEROMETER_CALIBRATED)) {
 288         return 0x3F;    // All 6 bits are set
 289     }
 290
 291     static const uint8_t bitMap[6] = { 0, 1, 3, 5, 2, 4 };      // A mapping of bits to match position indexes in Configurator
 292     uint8_t flags = 0;
 293     for (int i = 0; i < 6; i++) {
 294         if (calibratedPosition[i]) {
 295             flags |= (1 << bitMap[i]);
 296         }
 297     }
 298
 299     return flags;
 300 }
 301
 302 static int getPrimaryAxisIndex(int32_t accADCData[3])
 303 {
 304     // Work on a copy so we don't mess with accADC data
 305     int32_t sample[3];
 306
 307     applySensorAlignment(sample, accADCData, acc.dev.accAlign);
 308
 309     // Tolerate up to atan(1 / 1.5) = 33 deg tilt (in worst case 66 deg separation between points)
 310     if ((ABS(sample[Z]) / 1.5f) > ABS(sample[X]) && (ABS(sample[Z]) / 1.5f) > ABS(sample[Y])) {
 311         //Z-axis
 312         return (sample[Z] > 0) ? 0 : 1;
 313     }
 314     else if ((ABS(sample[X]) / 1.5f) > ABS(sample[Y]) && (ABS(sample[X]) / 1.5f) > ABS(sample[Z])) {
 315         //X-axis
 316         return (sample[X] > 0) ? 2 : 3;
 317     }
 318     else if ((ABS(sample[Y]) / 1.5f) > ABS(sample[X]) && (ABS(sample[Y]) / 1.5f) > ABS(sample[Z])) {
 319         //Y-axis
 320         return (sample[Y] > 0) ? 4 : 5;
 321     }
 322     else
 323         return -1;
 324 }
 325
 326 void accStartCalibration(void)
 327 {
 328     int positionIndex = getPrimaryAxisIndex(accADC);
 329
 330     // Fail if we can't detect the side
 331     if (positionIndex < 0) {
 332         return;
 333     }
 334
 335     // Fail if we have accelerometer fully calibrated and are NOT starting with TOP-UP position
 336     if (STATE(ACCELEROMETER_CALIBRATED) && positionIndex != 0) {
 337         return;
 338     }
 339
 340     // Top+up and first calibration cycle, reset everything
 341     if (positionIndex == 0) {
 342         for (int axis = 0; axis < 6; axis++) {
 343             calibratedPosition[axis] = false;
 344             accSamples[axis][X] = 0;
 345             accSamples[axis][Y] = 0;
 346             accSamples[axis][Z] = 0;
 347         }
 348
 349         DISABLE_STATE(ACCELEROMETER_CALIBRATED);
 350     }
 351
 352     // Tolerate 5% variance in accelerometer readings
 353     zeroCalibrationStartV(&zeroCalibration, CALIBRATING_ACC_TIME_MS, acc.dev.acc_1G * 0.05f, true);
 354 }
 355
 356 static bool allOrientationsHaveCalibrationDataCollected(void)
 357 {
 358     // Return true only if we have calibration data for all 6 positions
 359     return calibratedPosition[0] && calibratedPosition[1] && calibratedPosition[2] &&
 360            calibratedPosition[3] && calibratedPosition[4] && calibratedPosition[5];
 361 }
 362
 363 bool accIsCalibrationComplete(void)
 364 {
 365     return zeroCalibrationIsCompleteV(&zeroCalibration);
 366 }
 367
 368 static void performAcclerationCalibration(void)
 369 {
 370     // Shortcut - no need to do any math if acceleromter is marked as calibrated
 371     if (STATE(ACCELEROMETER_CALIBRATED)) {
 372         return;
 373     }
 374
 375     // If zero calibration logic is finished - no need to do anything
 376     if (accIsCalibrationComplete()) {
 377         return;
 378     }
 379
 380     fpVector3_t v;
 381     int positionIndex = getPrimaryAxisIndex(accADC);
 382
 383     // Check if sample is usable
 384     if (positionIndex < 0) {
 385         return;
 386     }
 387
 388     if (!calibratedPosition[positionIndex]) {
 389         v.v[X] = accADC[X];
 390         v.v[Y] = accADC[Y];
 391         v.v[Z] = accADC[Z];
 392
 393         zeroCalibrationAddValueV(&zeroCalibration, &v);
 394
 395         if (zeroCalibrationIsCompleteV(&zeroCalibration)) {
 396             if (zeroCalibrationIsSuccessfulV(&zeroCalibration)) {
 397                 zeroCalibrationGetZeroV(&zeroCalibration, &v);
 398
 399                 accSamples[positionIndex][X] = v.v[X];
 400                 accSamples[positionIndex][Y] = v.v[Y];
 401                 accSamples[positionIndex][Z] = v.v[Z];
 402
 403                 calibratedPosition[positionIndex] = true;
 404             }
 405             else {
 406                 calibratedPosition[positionIndex] = false;
 407             }
 408
 409             beeperConfirmationBeeps(2);
 410         }
 411     }
 412
 413     if (allOrientationsHaveCalibrationDataCollected()) {
 414         sensorCalibrationState_t calState;
 415         float accTmp[3];
 416         bool calFailed = false;
 417
 418         /* Calculate offset */
 419         sensorCalibrationResetState(&calState);
 420
 421         for (int axis = 0; axis < 6; axis++) {
 422             sensorCalibrationPushSampleForOffsetCalculation(&calState, accSamples[axis]);
 423         }
 424
 425         if (!sensorCalibrationSolveForOffset(&calState, accTmp)) {
 426             accTmp[X] = 0.0f;
 427             accTmp[Y] = 0.0f;
 428             accTmp[Z] = 0.0f;
 429             calFailed = true;
 430         }
 431
 432         accelerometerConfigMutable()->accZero.raw[X] = lrintf(accTmp[X]);
 433         accelerometerConfigMutable()->accZero.raw[Y] = lrintf(accTmp[Y]);
 434         accelerometerConfigMutable()->accZero.raw[Z] = lrintf(accTmp[Z]);
 435
 436         /* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
 437         sensorCalibrationResetState(&calState);
 438
 439         for (int axis = 0; axis < 6; axis++) {
 440             int32_t accSample[3];
 441
 442             accSample[X] = accSamples[axis][X] - accelerometerConfig()->accZero.raw[X];
 443             accSample[Y] = accSamples[axis][Y] - accelerometerConfig()->accZero.raw[Y];
 444             accSample[Z] = accSamples[axis][Z] - accelerometerConfig()->accZero.raw[Z];
 445
 446             sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.dev.acc_1G);
 447         }
 448
 449         if (!sensorCalibrationSolveForScale(&calState, accTmp)) {
 450             accTmp[X] = 1.0f;
 451             accTmp[Y] = 1.0f;
 452             accTmp[Z] = 1.0f;
 453             calFailed = true;
 454         }
 455
 456         for (int axis = 0; axis < 3; axis++) {
 457             accelerometerConfigMutable()->accGain.raw[axis] = lrintf(accTmp[axis] * 4096);
 458         }
 459
 460         if (calFailed) {
 461             // If failed - don't save and also invalidate the calibration data for all positions
 462             for (int axis = 0; axis < 6; axis++) {
 463                 calibratedPosition[axis] = false;
 464             }
 465         }
 466         else {
 467             // saveConfigAndNotify will trigger eepromREAD and in turn call back the accelerometer gain validation
 468             // that will set ENABLE_STATE(ACCELEROMETER_CALIBRATED) if all is good
 469             saveConfigAndNotify();
 470         }
 471     }
 472 }
 473
 474 static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)
 475 {
 476     accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
 477     accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
 478     accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
 479 }
 480
 481 /*
 482  * Calculate measured acceleration in body frame in m/s^2
 483  */
 484 void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
 485 {
 486     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 487         measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
 488     }
 489 }
 490
 491 /*
 492  * Return g's
 493  */
 494 const acc_extremes_t* accGetMeasuredExtremes(void)
 495 {
 496     return (const acc_extremes_t *)&acc.extremes;
 497 }
 498
 499 float accGetMeasuredMaxG(void)
 500 {
 501     return acc.maxG;
 502 }
 503
 504 void accUpdate(void)
 505 {
 506 #ifdef USE_SIMULATOR
 507     if (ARMING_FLAG(SIMULATOR_MODE)) {
 508         //output: acc.accADCf
 509         //unused: acc.dev.ADCRaw[], acc.accClipCount, acc.accVibeSq[]
 510         return;
 511     }
 512 #endif
 513     if (!acc.dev.readFn(&acc.dev)) {
 514         return;
 515     }
 516
 517     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 518         accADC[axis] = acc.dev.ADCRaw[axis];
 519         DEBUG_SET(DEBUG_ACC, axis, accADC[axis]);
 520     }
 521
 522     performAcclerationCalibration();
 523
 524     applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
 525
 526     applySensorAlignment(accADC, accADC, acc.dev.accAlign);
 527     applyBoardAlignment(accADC);
 528
 529     // Calculate acceleration readings in G's
 530     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 531         acc.accADCf[axis] = (float)accADC[axis] / acc.dev.acc_1G;
 532     }
 533
 534     // Before filtering check for clipping and vibration levels
 535     if (fabsf(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) {
 536         acc.isClipped = true;
 537         acc.accClipCount++;
 538     }
 539     else {
 540         acc.isClipped = false;
 541     }
 542
 543     // Calculate vibration levels
 544     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 545         // filter accel at 5hz
 546         const float accFloorFilt = pt1FilterApply(&accVibeFloorFilter[axis], acc.accADCf[axis]);
 547
 548         // calc difference from this sample and 5hz filtered value, square and filter at 2hz
 549         const float accDiff = acc.accADCf[axis] - accFloorFilt;
 550         acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
 551     }
 552
 553     // Filter acceleration
 554     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 555         acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
 556     }
 557
 558     if (accelerometerConfig()->acc_notch_hz) {
 559         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 560             acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]);
 561         }
 562     }
 563
 564 }
 565
 566 // Record extremes: min/max for each axis and acceleration vector modulus
 567 void updateAccExtremes(void)
 568 {
 569     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 570         if (acc.accADCf[axis] < acc.extremes[axis].min) acc.extremes[axis].min = acc.accADCf[axis];
 571         if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis];
 572     }
 573
 574     float gforce = calc_length_pythagorean_3D(acc.accADCf[X], acc.accADCf[Y], acc.accADCf[Z]);
 575     if (gforce > acc.maxG) acc.maxG = gforce;
 576 }
 577
 578 void accGetVibrationLevels(fpVector3_t *accVibeLevels)
 579 {
 580     accVibeLevels->x = fast_fsqrtf(acc.accVibeSq[X]);
 581     accVibeLevels->y = fast_fsqrtf(acc.accVibeSq[Y]);
 582     accVibeLevels->z = fast_fsqrtf(acc.accVibeSq[Z]);
 583 }
 584
 585 float accGetVibrationLevel(void)
 586 {
 587     return fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
 588 }
 589
 590 uint32_t accGetClipCount(void)
 591 {
 592     return acc.accClipCount;
 593 }
 594
 595 bool accIsClipped(void)
 596 {
 597     return acc.isClipped;
 598 }
 599
 600 void accSetCalibrationValues(void)
 601 {
 602     if ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) &&
 603         (accelerometerConfig()->accGain.raw[X] == 4096) && (accelerometerConfig()->accGain.raw[Y] == 4096) &&(accelerometerConfig()->accGain.raw[Z] == 4096)) {
 604         DISABLE_STATE(ACCELEROMETER_CALIBRATED);
 605     }
 606     else {
 607         ENABLE_STATE(ACCELEROMETER_CALIBRATED);
 608     }
 609 }
 610
 611 void accInitFilters(void)
 612 {
 613     accSoftLpfFilterApplyFn = nullFilterApply;
 614
 615     if (acc.accTargetLooptime && accelerometerConfig()->acc_lpf_hz) {
 616
 617         switch (accelerometerConfig()->acc_soft_lpf_type)
 618         {
 619         case FILTER_PT1:
 620             accSoftLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
 621             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 622                 accSoftLpfFilter[axis] = &accFilter[axis].pt1;
 623                 pt1FilterInit(accSoftLpfFilter[axis], accelerometerConfig()->acc_lpf_hz, US2S(acc.accTargetLooptime));
 624             }
 625             break;
 626         case FILTER_BIQUAD:
 627             accSoftLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
 628             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 629                 accSoftLpfFilter[axis] = &accFilter[axis].biquad;
 630                 biquadFilterInitLPF(accSoftLpfFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime);
 631             }
 632             break;
 633         }
 634
 635     }
 636
 637     const float accDt = US2S(acc.accTargetLooptime);
 638     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 639         pt1FilterInit(&accVibeFloorFilter[axis], ACC_VIBE_FLOOR_FILT_HZ, accDt);
 640         pt1FilterInit(&accVibeFilter[axis], ACC_VIBE_FILT_HZ, accDt);
 641     }
 642
 643     STATIC_FASTRAM biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
 644     accNotchFilterApplyFn = nullFilterApply;
 645
 646     if (acc.accTargetLooptime && accelerometerConfig()->acc_notch_hz) {
 647         accNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
 648         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 649             accNotchFilter[axis] = &accFilterNotch[axis];
 650             biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff);
 651         }
 652     }
 653
 654 }
 655
 656 bool accIsHealthy(void)
 657 {
 658     return true;
 659 }