src/main/drivers/accgyro_mpu6500.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 <stdlib.h>
  21
  22 #include "platform.h"
  23
  24 #include "common/axis.h"
  25 #include "common/maths.h"
  26
  27 #include "system.h"
  28 #include "exti.h"
  29 #include "gpio.h"
  30 #include "gyro_sync.h"
  31
  32 #include "sensor.h"
  33 #include "accgyro.h"
  34 #include "accgyro_mpu.h"
  35 #include "accgyro_mpu6500.h"
  36
  37 bool mpu6500AccDetect(acc_t *acc)
  38 {
  39     if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
  40         return false;
  41     }
  42
  43     acc->init = mpu6500AccInit;
  44     acc->read = mpuAccRead;
  45
  46     return true;
  47 }
  48
  49 bool mpu6500GyroDetect(gyro_t *gyro)
  50 {
  51     if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
  52         return false;
  53     }
  54
  55     gyro->init = mpu6500GyroInit;
  56     gyro->read = mpuGyroRead;
  57     gyro->intStatus = checkMPUDataReady;
  58
  59     // 16.4 dps/lsb scalefactor
  60     gyro->scale = 1.0f / 16.4f;
  61
  62     return true;
  63 }
  64
  65 void mpu6500AccInit(acc_t *acc)
  66 {
  67     mpuIntExtiInit();
  68
  69     acc->acc_1G = 512 * 4;
  70 }
  71
  72 void mpu6500GyroInit(uint8_t lpf)
  73 {
  74     mpuIntExtiInit();
  75
  76     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
  77     delay(100);
  78     mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07);
  79     delay(100);
  80     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
  81     delay(100);
  82     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
  83     delay(15);
  84     mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
  85     delay(15);
  86     mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
  87     delay(15);
  88     mpuConfiguration.write(MPU_RA_CONFIG, lpf);
  89     delay(15);
  90     mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
  91     delay(100);
  92
  93     // Data ready interrupt configuration
  94 #ifdef USE_MPU9250_MAG
  95     mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);  // INT_ANYRD_2CLEAR, BYPASS_EN
  96 #else
  97     mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR);  // INT_ANYRD_2CLEAR
  98 #endif
  99     delay(15);
 100
 101 #ifdef USE_MPU_DATA_READY_SIGNAL
 102     mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
 103 #endif
 104     delay(15);
 105 }