src/main/sensors/acceleration_init.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_ACC
  29
  30 #include "build/debug.h"
  31
  32 #include "common/axis.h"
  33 #include "common/filter.h"
  34 #include "common/utils.h"
  35
  36 #include "config/config_reset.h"
  37 #include "config/feature.h"
  38
  39 #include "drivers/accgyro/accgyro.h"
  40 #include "drivers/accgyro/accgyro_fake.h"
  41 #include "drivers/accgyro/accgyro_mpu.h"
  42 #include "drivers/accgyro/accgyro_mpu3050.h"
  43 #include "drivers/accgyro/accgyro_mpu6050.h"
  44 #include "drivers/accgyro/accgyro_mpu6500.h"
  45 #include "drivers/accgyro/accgyro_spi_bmi160.h"
  46 #include "drivers/accgyro/accgyro_spi_bmi270.h"
  47 #include "drivers/accgyro/accgyro_spi_icm20649.h"
  48 #include "drivers/accgyro/accgyro_spi_icm20689.h"
  49 #include "drivers/accgyro/accgyro_spi_icm426xx.h"
  50 #include "drivers/accgyro/accgyro_spi_lsm6dso.h"
  51 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
  52 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
  53 #include "drivers/accgyro/accgyro_spi_mpu9250.h"
  54
  55 #ifdef USE_ACC_ADXL345
  56 #include "drivers/accgyro_legacy/accgyro_adxl345.h"
  57 #endif
  58
  59 #ifdef USE_ACC_BMA280
  60 #include "drivers/accgyro_legacy/accgyro_bma280.h"
  61 #endif
  62
  63 #ifdef USE_ACC_LSM303DLHC
  64 #include "drivers/accgyro_legacy/accgyro_lsm303dlhc.h"
  65 #endif
  66
  67 #ifdef USE_ACC_MMA8452
  68 #include "drivers/accgyro_legacy/accgyro_mma845x.h"
  69 #endif
  70
  71 #include "drivers/bus_spi.h"
  72
  73 #include "config/config.h"
  74 #include "fc/runtime_config.h"
  75
  76 #include "io/beeper.h"
  77
  78 #include "pg/gyrodev.h"
  79 #include "pg/pg.h"
  80 #include "pg/pg_ids.h"
  81
  82 #include "sensors/boardalignment.h"
  83 #include "sensors/gyro.h"
  84 #include "sensors/gyro_init.h"
  85 #include "sensors/sensors.h"
  86
  87 #include "acceleration_init.h"
  88
  89 #define CALIBRATING_ACC_CYCLES              400
  90
  91 FAST_DATA_ZERO_INIT accelerationRuntime_t accelerationRuntime;
  92
  93 void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
  94 {
  95     RESET_CONFIG_2(rollAndPitchTrims_t, rollAndPitchTrims,
  96         .values.roll = 0,
  97         .values.pitch = 0,
  98     );
  99 }
 100
 101 static void setConfigCalibrationCompleted(void)
 102 {
 103     accelerometerConfigMutable()->accZero.values.calibrationCompleted = 1;
 104 }
 105
 106 bool accHasBeenCalibrated(void)
 107 {
 108     return accelerometerConfig()->accZero.values.calibrationCompleted;
 109 }
 110
 111 void accResetRollAndPitchTrims(void)
 112 {
 113     resetRollAndPitchTrims(&accelerometerConfigMutable()->accelerometerTrims);
 114 }
 115
 116 static void resetFlightDynamicsTrims(flightDynamicsTrims_t *accZero)
 117 {
 118     accZero->values.roll = 0;
 119     accZero->values.pitch = 0;
 120     accZero->values.yaw = 0;
 121     accZero->values.calibrationCompleted = 0;
 122 }
 123
 124 void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
 125 {
 126     RESET_CONFIG_2(accelerometerConfig_t, instance,
 127         .acc_lpf_hz = 25, // ATTITUDE/IMU runs at 100Hz (acro) or 500Hz (level modes) so we need to set 50 Hz (or lower) to avoid aliasing
 128         .acc_hardware = ACC_DEFAULT,
 129         .acc_high_fsr = false,
 130     );
 131     resetRollAndPitchTrims(&instance->accelerometerTrims);
 132     resetFlightDynamicsTrims(&instance->accZero);
 133 }
 134
 135 PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 2);
 136
 137 extern uint16_t InflightcalibratingA;
 138 extern bool AccInflightCalibrationMeasurementDone;
 139 extern bool AccInflightCalibrationSavetoEEProm;
 140 extern bool AccInflightCalibrationActive;
 141
 142
 143 bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 144 {
 145     accelerationSensor_e accHardware = ACC_NONE;
 146
 147 #ifdef USE_ACC_ADXL345
 148     drv_adxl345_config_t acc_params;
 149 #endif
 150
 151 retry:
 152
 153     switch (accHardwareToUse) {
 154     case ACC_DEFAULT:
 155         FALLTHROUGH;
 156
 157 #ifdef USE_ACC_ADXL345
 158     case ACC_ADXL345: // ADXL345
 159         acc_params.useFifo = false;
 160         acc_params.dataRate = 800; // unused currently
 161         if (adxl345Detect(&acc_params, dev)) {
 162             accHardware = ACC_ADXL345;
 163             break;
 164         }
 165         FALLTHROUGH;
 166 #endif
 167
 168 #ifdef USE_ACC_LSM303DLHC
 169     case ACC_LSM303DLHC:
 170         if (lsm303dlhcAccDetect(dev)) {
 171             accHardware = ACC_LSM303DLHC;
 172             break;
 173         }
 174         FALLTHROUGH;
 175 #endif
 176
 177 #ifdef USE_ACC_MPU6050
 178     case ACC_MPU6050: // MPU6050
 179         if (mpu6050AccDetect(dev)) {
 180             accHardware = ACC_MPU6050;
 181             break;
 182         }
 183         FALLTHROUGH;
 184 #endif
 185
 186 #ifdef USE_ACC_MMA8452
 187     case ACC_MMA8452: // MMA8452
 188         if (mma8452Detect(dev)) {
 189             accHardware = ACC_MMA8452;
 190             break;
 191         }
 192         FALLTHROUGH;
 193 #endif
 194
 195 #ifdef USE_ACC_BMA280
 196     case ACC_BMA280: // BMA280
 197         if (bma280Detect(dev)) {
 198             accHardware = ACC_BMA280;
 199             break;
 200         }
 201         FALLTHROUGH;
 202 #endif
 203
 204 #ifdef USE_ACC_SPI_MPU6000
 205     case ACC_MPU6000:
 206         if (mpu6000SpiAccDetect(dev)) {
 207             accHardware = ACC_MPU6000;
 208             break;
 209         }
 210         FALLTHROUGH;
 211 #endif
 212
 213 #ifdef USE_ACC_SPI_MPU9250
 214     case ACC_MPU9250:
 215         if (mpu9250SpiAccDetect(dev)) {
 216             accHardware = ACC_MPU9250;
 217             break;
 218         }
 219         FALLTHROUGH;
 220 #endif
 221
 222     case ACC_MPU6500:
 223     case ACC_ICM20601:
 224     case ACC_ICM20602:
 225     case ACC_ICM20608G:
 226 #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
 227 #ifdef USE_ACC_SPI_MPU6500
 228         if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) {
 229 #else
 230         if (mpu6500AccDetect(dev)) {
 231 #endif
 232             switch (dev->mpuDetectionResult.sensor) {
 233             case MPU_9250_SPI:
 234                 accHardware = ACC_MPU9250;
 235                 break;
 236             case ICM_20601_SPI:
 237                 accHardware = ACC_ICM20601;
 238                 break;
 239             case ICM_20602_SPI:
 240                 accHardware = ACC_ICM20602;
 241                 break;
 242             case ICM_20608_SPI:
 243                 accHardware = ACC_ICM20608G;
 244                 break;
 245             default:
 246                 accHardware = ACC_MPU6500;
 247             }
 248             break;
 249         }
 250 #endif
 251         FALLTHROUGH;
 252
 253 #ifdef USE_ACC_SPI_ICM20649
 254     case ACC_ICM20649:
 255         if (icm20649SpiAccDetect(dev)) {
 256             accHardware = ACC_ICM20649;
 257             break;
 258         }
 259         FALLTHROUGH;
 260 #endif
 261
 262 #ifdef USE_ACC_SPI_ICM20689
 263     case ACC_ICM20689:
 264         if (icm20689SpiAccDetect(dev)) {
 265             accHardware = ACC_ICM20689;
 266             break;
 267         }
 268         FALLTHROUGH;
 269 #endif
 270
 271 #if defined(USE_ACC_SPI_ICM42605) || defined(USE_ACC_SPI_ICM42688P)
 272     case ACC_ICM42605:
 273     case ACC_ICM42688P:
 274         if (icm426xxSpiAccDetect(dev)) {
 275             switch (dev->mpuDetectionResult.sensor) {
 276             case ICM_42605_SPI:
 277                 accHardware = ACC_ICM42605;
 278                 break;
 279             case ICM_42688P_SPI:
 280                 accHardware = ACC_ICM42688P;
 281                 break;
 282             default:
 283                 accHardware = ACC_NONE;
 284                 break;
 285             }
 286             break;
 287         }
 288         FALLTHROUGH;
 289 #endif
 290
 291 #ifdef USE_ACCGYRO_BMI160
 292     case ACC_BMI160:
 293         if (bmi160SpiAccDetect(dev)) {
 294             accHardware = ACC_BMI160;
 295             break;
 296         }
 297         FALLTHROUGH;
 298 #endif
 299
 300 #ifdef USE_ACCGYRO_BMI270
 301     case ACC_BMI270:
 302         if (bmi270SpiAccDetect(dev)) {
 303             accHardware = ACC_BMI270;
 304             break;
 305         }
 306         FALLTHROUGH;
 307 #endif
 308
 309 #ifdef USE_ACCGYRO_LSM6DSO
 310     case ACC_LSM6DSO:
 311         if (lsm6dsoSpiAccDetect(dev)) {
 312             accHardware = ACC_LSM6DSO;
 313             break;
 314         }
 315         FALLTHROUGH;
 316 #endif
 317
 318 #ifdef USE_FAKE_ACC
 319     case ACC_FAKE:
 320         if (fakeAccDetect(dev)) {
 321             accHardware = ACC_FAKE;
 322             break;
 323         }
 324         FALLTHROUGH;
 325 #endif
 326
 327     default:
 328     case ACC_NONE: // disable ACC
 329         accHardware = ACC_NONE;
 330         break;
 331     }
 332
 333     // Found anything? Check if error or ACC is really missing.
 334     if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) {
 335         // Nothing was found and we have a forced sensor that isn't present.
 336         accHardwareToUse = ACC_DEFAULT;
 337         goto retry;
 338     }
 339
 340     if (accHardware == ACC_NONE) {
 341         return false;
 342     }
 343
 344     detectedSensors[SENSOR_INDEX_ACC] = accHardware;
 345     sensorsSet(SENSOR_ACC);
 346     return true;
 347 }
 348
 349 void accInitFilters(void)
 350 {
 351     // Only set the lowpass cutoff if the ACC sample rate is detected otherwise
 352     // the filter initialization is not defined (sample rate = 0)
 353     accelerationRuntime.accLpfCutHz = (acc.sampleRateHz) ? accelerometerConfig()->acc_lpf_hz : 0;
 354     if (accelerationRuntime.accLpfCutHz) {
 355         const float k = pt2FilterGain(accelerationRuntime.accLpfCutHz, 1.0f / acc.sampleRateHz);
 356         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 357             pt2FilterInit(&accelerationRuntime.accFilter[axis], k);
 358         }
 359     }
 360 }
 361
 362 bool accInit(uint16_t accSampleRateHz)
 363 {
 364     memset(&acc, 0, sizeof(acc));
 365     // copy over the common gyro mpu settings
 366     acc.dev.gyro = gyroActiveDev();
 367     acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
 368     acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr;
 369
 370     // Copy alignment from active gyro, as all production boards use acc-gyro-combi chip.
 371     // Exception is STM32F411DISCOVERY, and (may be) handled in future enhancement.
 372
 373     sensor_align_e alignment = gyroDeviceConfig(0)->alignment;
 374     const sensorAlignment_t* customAlignment = &gyroDeviceConfig(0)->customAlignment;
 375
 376 #ifdef USE_MULTI_GYRO
 377     if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) {
 378         alignment = gyroDeviceConfig(1)->alignment;
 379
 380         customAlignment = &gyroDeviceConfig(1)->customAlignment;
 381     }
 382 #endif
 383     acc.dev.accAlign = alignment;
 384     buildRotationMatrixFromAlignment(customAlignment, &acc.dev.rotationMatrix);
 385
 386     if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
 387         return false;
 388     }
 389     acc.dev.acc_1G = 256; // set default
 390     acc.dev.initFn(&acc.dev); // driver initialisation
 391     acc.dev.acc_1G_rec = 1.0f / acc.dev.acc_1G;
 392
 393     acc.sampleRateHz = accSampleRateHz;
 394     accInitFilters();
 395     return true;
 396 }
 397
 398 void accStartCalibration(void)
 399 {
 400     accelerationRuntime.calibratingA = CALIBRATING_ACC_CYCLES;
 401 }
 402
 403 bool accIsCalibrationComplete(void)
 404 {
 405     return accelerationRuntime.calibratingA == 0;
 406 }
 407
 408 static bool isOnFinalAccelerationCalibrationCycle(void)
 409 {
 410     return accelerationRuntime.calibratingA == 1;
 411 }
 412
 413 static bool isOnFirstAccelerationCalibrationCycle(void)
 414 {
 415     return accelerationRuntime.calibratingA == CALIBRATING_ACC_CYCLES;
 416 }
 417
 418 void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
 419 {
 420     static int32_t a[3];
 421
 422     for (int axis = 0; axis < 3; axis++) {
 423
 424         // Reset a[axis] at start of calibration
 425         if (isOnFirstAccelerationCalibrationCycle()) {
 426             a[axis] = 0;
 427         }
 428
 429         // Sum up CALIBRATING_ACC_CYCLES readings
 430         a[axis] += acc.accADC[axis];
 431
 432         // Reset global variables to prevent other code from using un-calibrated data
 433         acc.accADC[axis] = 0;
 434         accelerationRuntime.accelerationTrims->raw[axis] = 0;
 435     }
 436
 437     if (isOnFinalAccelerationCalibrationCycle()) {
 438         // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
 439         accelerationRuntime.accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
 440         accelerationRuntime.accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
 441         accelerationRuntime.accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc.dev.acc_1G;
 442
 443         resetRollAndPitchTrims(rollAndPitchTrims);
 444         setConfigCalibrationCompleted();
 445
 446         saveConfigAndNotify();
 447     }
 448
 449     accelerationRuntime.calibratingA--;
 450 }
 451
 452 void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
 453 {
 454     static int32_t b[3];
 455     static int16_t accZero_saved[3] = { 0, 0, 0 };
 456     static rollAndPitchTrims_t angleTrim_saved = { { 0, 0 } };
 457
 458     // Saving old zeropoints before measurement
 459     if (InflightcalibratingA == 50) {
 460         accZero_saved[X] = accelerationRuntime.accelerationTrims->raw[X];
 461         accZero_saved[Y] = accelerationRuntime.accelerationTrims->raw[Y];
 462         accZero_saved[Z] = accelerationRuntime.accelerationTrims->raw[Z];
 463         angleTrim_saved.values.roll = rollAndPitchTrims->values.roll;
 464         angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch;
 465     }
 466     if (InflightcalibratingA > 0) {
 467         for (int axis = 0; axis < 3; axis++) {
 468             // Reset a[axis] at start of calibration
 469             if (InflightcalibratingA == 50)
 470                 b[axis] = 0;
 471             // Sum up 50 readings
 472             b[axis] += acc.accADC[axis];
 473             // Clear global variables for next reading
 474             acc.accADC[axis] = 0;
 475             accelerationRuntime.accelerationTrims->raw[axis] = 0;
 476         }
 477         // all values are measured
 478         if (InflightcalibratingA == 1) {
 479             AccInflightCalibrationActive = false;
 480             AccInflightCalibrationMeasurementDone = true;
 481             beeper(BEEPER_ACC_CALIBRATION); // indicate end of calibration
 482             // recover saved values to maintain current flight behaviour until new values are transferred
 483             accelerationRuntime.accelerationTrims->raw[X] = accZero_saved[X];
 484             accelerationRuntime.accelerationTrims->raw[Y] = accZero_saved[Y];
 485             accelerationRuntime.accelerationTrims->raw[Z] = accZero_saved[Z];
 486             rollAndPitchTrims->values.roll = angleTrim_saved.values.roll;
 487             rollAndPitchTrims->values.pitch = angleTrim_saved.values.pitch;
 488         }
 489         InflightcalibratingA--;
 490     }
 491     // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
 492     if (AccInflightCalibrationSavetoEEProm) {      // the aircraft is landed, disarmed and the combo has been done again
 493         AccInflightCalibrationSavetoEEProm = false;
 494         accelerationRuntime.accelerationTrims->raw[X] = b[X] / 50;
 495         accelerationRuntime.accelerationTrims->raw[Y] = b[Y] / 50;
 496         accelerationRuntime.accelerationTrims->raw[Z] = b[Z] / 50 - acc.dev.acc_1G;    // for nunchuck 200=1G
 497
 498         resetRollAndPitchTrims(rollAndPitchTrims);
 499         setConfigCalibrationCompleted();
 500
 501         saveConfigAndNotify();
 502     }
 503 }
 504
 505 void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
 506 {
 507     accelerationRuntime.accelerationTrims = accelerationTrimsToUse;
 508 }
 509
 510 void applyAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 511 {
 512     accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
 513     accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
 514 }
 515 #endif