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