src/main/drivers/accgyro/accgyro_mpu6500.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 <stdlib.h>
  24
  25 #include "platform.h"
  26
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29
  30 #include "drivers/exti.h"
  31 #include "drivers/sensor.h"
  32 #include "drivers/time.h"
  33
  34 #include "accgyro.h"
  35 #include "accgyro_mpu.h"
  36 #include "accgyro_mpu6500.h"
  37
  38 void mpu6500AccInit(accDev_t *acc)
  39 {
  40     acc->acc_1G = 512 * 4;
  41 }
  42
  43 bool mpu6500AccDetect(accDev_t *acc)
  44 {
  45     if (acc->mpuDetectionResult.sensor != MPU_65xx_I2C) {
  46         return false;
  47     }
  48
  49     acc->initFn = mpu6500AccInit;
  50     acc->readFn = mpuAccRead;
  51
  52     return true;
  53 }
  54
  55 void mpu6500GyroInit(gyroDev_t *gyro)
  56 {
  57     extDevice_t *dev = &gyro->dev;
  58
  59     mpuGyroInit(gyro);
  60
  61     int gyro_range = INV_FSR_2000DPS;
  62     int accel_range = INV_FSR_16G;
  63
  64     if (gyro->mpuDetectionResult.sensor == ICM_20601_SPI) {
  65         gyro_range = gyro->gyro_high_fsr ? ICM_HIGH_RANGE_FSR_4000DPS : ICM_HIGH_RANGE_FSR_2000DPS;
  66         accel_range = ICM_HIGH_RANGE_FSR_16G;
  67     }
  68
  69     busWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
  70     delay(100);
  71     busWriteRegister(dev, MPU_RA_SIGNAL_PATH_RESET, 0x07);
  72     delay(100);
  73     busWriteRegister(dev, MPU_RA_PWR_MGMT_1, 0);
  74     delay(100);
  75     busWriteRegister(dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
  76     delay(15);
  77     busWriteRegister(dev, MPU_RA_GYRO_CONFIG, gyro_range << 3);
  78     delay(15);
  79     busWriteRegister(dev, MPU_RA_ACCEL_CONFIG, accel_range << 3);
  80     delay(15);
  81     busWriteRegister(dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
  82     delay(15);
  83     busWriteRegister(dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
  84     delay(100);
  85
  86     // Data ready interrupt configuration
  87 #ifdef USE_MPU9250_MAG
  88     busWriteRegister(dev, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);  // INT_ANYRD_2CLEAR, BYPASS_EN
  89 #else
  90     busWriteRegister(dev, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR);  // INT_ANYRD_2CLEAR
  91 #endif
  92     delay(15);
  93
  94     busWriteRegister(&gyro->dev, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
  95     delay(15);
  96 }
  97
  98 bool mpu6500GyroDetect(gyroDev_t *gyro)
  99 {
 100     if (gyro->mpuDetectionResult.sensor != MPU_65xx_I2C) {
 101         return false;
 102     }
 103
 104     gyro->initFn = mpu6500GyroInit;
 105     gyro->readFn = mpuGyroRead;
 106
 107     gyro->scale = GYRO_SCALE_2000DPS;
 108
 109     return true;
 110 }