src/main/drivers/accgyro/accgyro_mpu6050.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 #if defined(USE_ACC_MPU6050) || defined(USE_GYRO_MPU6050)
  28
  29 #include "build/debug.h"
  30
  31 #include "common/maths.h"
  32 #include "common/utils.h"
  33
  34 #include "drivers/bus_i2c.h"
  35 #include "drivers/exti.h"
  36 #include "drivers/nvic.h"
  37 #include "drivers/sensor.h"
  38 #include "drivers/time.h"
  39
  40 #include "accgyro.h"
  41 #include "accgyro_mpu.h"
  42 #include "accgyro_mpu6050.h"
  43
  44 // MPU6050, Standard address 0x68
  45 // MPU_INT on PB13 on rev4 Naze32 hardware
  46 #define MPU6050_ADDRESS         0x68
  47
  48 #define DMP_MEM_START_ADDR 0x6E
  49 #define DMP_MEM_R_W 0x6F
  50
  51 #define MPU6050_SMPLRT_DIV      0       // 8000Hz
  52
  53 static void mpu6050AccInit(accDev_t *acc)
  54 {
  55     switch (acc->mpuDetectionResult.resolution) {
  56         case MPU_HALF_RESOLUTION:
  57             acc->acc_1G = 256 * 4;
  58             break;
  59         case MPU_FULL_RESOLUTION:
  60             acc->acc_1G = 512 * 4;
  61             break;
  62     }
  63 }
  64
  65 bool mpu6050AccDetect(accDev_t *acc)
  66 {
  67     if (acc->mpuDetectionResult.sensor != MPU_60x0) {
  68         return false;
  69     }
  70
  71     acc->initFn = mpu6050AccInit;
  72     acc->readFn = mpuAccRead;
  73     acc->revisionCode = (acc->mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
  74
  75     return true;
  76 }
  77
  78 static void mpu6050GyroInit(gyroDev_t *gyro)
  79 {
  80     extDevice_t *dev = &gyro->dev;
  81
  82     mpuGyroInit(gyro);
  83
  84     busWriteRegister(dev, MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
  85     delay(100);
  86     busWriteRegister(dev, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
  87     busWriteRegister(dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
  88     delay(15); //PLL Settling time when changing CLKSEL is max 10ms.  Use 15ms to be sure
  89     busWriteRegister(dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
  90     busWriteRegister(dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
  91
  92     // ACC Init stuff.
  93     // Accel scale 8g (4096 LSB/g)
  94     busWriteRegister(dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
  95
  96     busWriteRegister(dev, MPU_RA_INT_PIN_CFG,
  97             0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG   -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
  98
  99     busWriteRegister(&gyro->dev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
 100 }
 101
 102 bool mpu6050GyroDetect(gyroDev_t *gyro)
 103 {
 104     if (gyro->mpuDetectionResult.sensor != MPU_60x0) {
 105         return false;
 106     }
 107     gyro->initFn = mpu6050GyroInit;
 108     gyro->readFn = mpuGyroRead;
 109
 110     gyro->scale = GYRO_SCALE_2000DPS;
 111
 112     return true;
 113 }
 114 #endif