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 resetGForceStats(void) {
 533     acc.maxG = 0.0f;
 534
 535     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 536         acc.extremes[axis].min = 100;
 537         acc.extremes[axis].max = -100;
 538     }
 539 }
 540
 541 void accUpdate(void)
 542 {
 543 #ifdef USE_SIMULATOR
 544     if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
 545         //output: acc.accADCf
 546         //unused: acc.dev.ADCRaw[], acc.accClipCount, acc.accVibeSq[]
 547         return;
 548     }
 549 #endif
 550     if (!acc.dev.readFn(&acc.dev)) {
 551         return;
 552     }
 553     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 554         accADC[axis] = acc.dev.ADCRaw[axis];
 555         DEBUG_SET(DEBUG_ACC, axis, accADC[axis]);
 556     }
 557
 558     if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) {
 559         performAcclerationCalibration();
 560         applyAccelerationZero();
 561     }
 562
 563     applySensorAlignment(accADC, accADC, acc.dev.accAlign);
 564     applyBoardAlignment(accADC);
 565
 566     // Calculate acceleration readings in G's
 567     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 568         acc.accADCf[axis] = (float)accADC[axis] / acc.dev.acc_1G;
 569     }
 570
 571     // Before filtering check for clipping and vibration levels
 572     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) {
 573         acc.isClipped = true;
 574         acc.accClipCount++;
 575     }
 576     else {
 577         acc.isClipped = false;
 578     }
 579
 580     // Calculate vibration levels
 581     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 582         // filter accel at 5hz
 583         const float accFloorFilt = pt1FilterApply(&accVibeFloorFilter[axis], acc.accADCf[axis]);
 584
 585         // calc difference from this sample and 5hz filtered value, square and filter at 2hz
 586         const float accDiff = acc.accADCf[axis] - accFloorFilt;
 587         acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
 588         acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
 589     }
 590
 591     // Filter acceleration
 592     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 593         acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
 594     }
 595
 596     if (accelerometerConfig()->acc_notch_hz) {
 597         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 598             acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]);
 599         }
 600     }
 601
 602 }
 603
 604 // Record extremes: min/max for each axis and acceleration vector modulus
 605 void updateAccExtremes(void)
 606 {
 607     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 608         if (acc.accADCf[axis] < acc.extremes[axis].min) acc.extremes[axis].min = acc.accADCf[axis];
 609         if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis];
 610     }
 611
 612     float gforce = calc_length_pythagorean_3D(acc.accADCf[X], acc.accADCf[Y], acc.accADCf[Z]);
 613     if (gforce > acc.maxG) acc.maxG = gforce;
 614 }
 615
 616 void accGetVibrationLevels(fpVector3_t *accVibeLevels)
 617 {
 618     accVibeLevels->x = fast_fsqrtf(acc.accVibeSq[X]);
 619     accVibeLevels->y = fast_fsqrtf(acc.accVibeSq[Y]);
 620     accVibeLevels->z = fast_fsqrtf(acc.accVibeSq[Z]);
 621 }
 622
 623 float accGetVibrationLevel(void)
 624 {
 625     return acc.accVibe;
 626 }
 627
 628 uint32_t accGetClipCount(void)
 629 {
 630     return acc.accClipCount;
 631 }
 632
 633 bool accIsClipped(void)
 634 {
 635     return acc.isClipped;
 636 }
 637
 638 void accSetCalibrationValues(void)
 639 {
 640     if (!ARMING_FLAG(SIMULATOR_MODE_SITL) &&
 641         ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) &&
 642         (accelerometerConfig()->accGain.raw[X] == 4096) && (accelerometerConfig()->accGain.raw[Y] == 4096) &&(accelerometerConfig()->accGain.raw[Z] == 4096))) {
 643         DISABLE_STATE(ACCELEROMETER_CALIBRATED);
 644     }
 645     else {
 646         ENABLE_STATE(ACCELEROMETER_CALIBRATED);
 647     }
 648 }
 649
 650 void accInitFilters(void)
 651 {
 652     accSoftLpfFilterApplyFn = nullFilterApply;
 653
 654     if (acc.accTargetLooptime && accelerometerConfig()->acc_lpf_hz) {
 655
 656         switch (accelerometerConfig()->acc_soft_lpf_type)
 657         {
 658         case FILTER_PT1:
 659             accSoftLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
 660             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 661                 accSoftLpfFilter[axis] = &accFilter[axis].pt1;
 662                 pt1FilterInit(accSoftLpfFilter[axis], accelerometerConfig()->acc_lpf_hz, US2S(acc.accTargetLooptime));
 663             }
 664             break;
 665         case FILTER_BIQUAD:
 666             accSoftLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
 667             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 668                 accSoftLpfFilter[axis] = &accFilter[axis].biquad;
 669                 biquadFilterInitLPF(accSoftLpfFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime);
 670             }
 671             break;
 672         }
 673
 674     }
 675
 676     const float accDt = US2S(acc.accTargetLooptime);
 677     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 678         pt1FilterInit(&accVibeFloorFilter[axis], ACC_VIBE_FLOOR_FILT_HZ, accDt);
 679         pt1FilterInit(&accVibeFilter[axis], ACC_VIBE_FILT_HZ, accDt);
 680     }
 681
 682     STATIC_FASTRAM biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
 683     accNotchFilterApplyFn = nullFilterApply;
 684
 685     if (acc.accTargetLooptime && accelerometerConfig()->acc_notch_hz) {
 686         accNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
 687         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 688             accNotchFilter[axis] = &accFilterNotch[axis];
 689             biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff);
 690         }
 691     }
 692
 693 }
 694
 695 bool accIsHealthy(void)
 696 {
 697     return true;
 698 }