src/main/drivers/accgyro_mpu6050.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 #include "build_config.h"
  24 #include "debug.h"
  25
  26 #include "common/maths.h"
  27
  28 #include "nvic.h"
  29
  30 #include "system.h"
  31 #include "gpio.h"
  32 #include "exti.h"
  33 #include "bus_i2c.h"
  34
  35 #include "sensor.h"
  36 #include "accgyro.h"
  37 #include "accgyro_mpu.h"
  38 #include "accgyro_mpu6050.h"
  39
  40 extern uint8_t mpuLowPassFilter;
  41
  42 //#define DEBUG_MPU_DATA_READY_INTERRUPT
  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(void);
  54 static void mpu6050GyroInit(uint16_t lpf);
  55
  56 bool mpu6050AccDetect(acc_t *acc)
  57 {
  58     if (mpuDetectionResult.sensor != MPU_60x0) {
  59         return false;
  60     }
  61
  62     acc->init = mpu6050AccInit;
  63     acc->read = mpuAccRead;
  64     acc->revisionCode = (mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
  65
  66     return true;
  67 }
  68
  69 bool mpu6050GyroDetect(gyro_t *gyro)
  70 {
  71     if (mpuDetectionResult.sensor != MPU_60x0) {
  72         return false;
  73     }
  74     gyro->init = mpu6050GyroInit;
  75     gyro->read = mpuGyroRead;
  76
  77     // 16.4 dps/lsb scalefactor
  78     gyro->scale = 1.0f / 16.4f;
  79
  80     return true;
  81 }
  82
  83 static void mpu6050AccInit(void)
  84 {
  85     mpuIntExtiInit();
  86
  87     switch (mpuDetectionResult.resolution) {
  88         case MPU_HALF_RESOLUTION:
  89             acc_1G = 256 * 8;
  90             break;
  91         case MPU_FULL_RESOLUTION:
  92             acc_1G = 512 * 8;
  93             break;
  94     }
  95 }
  96
  97 static void mpu6050GyroInit(uint16_t lpf)
  98 {
  99     bool ack;
 100
 101     mpuIntExtiInit();
 102
 103     uint8_t mpuLowPassFilter = determineMPULPF(lpf);
 104
 105     ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
 106     delay(100);
 107     ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
 108     ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
 109     delay(15); //PLL Settling time when changing CLKSEL is max 10ms.  Use 15ms to be sure
 110     ack = mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
 111     ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
 112
 113     // ACC Init stuff.
 114     // Accel scale 8g (4096 LSB/g)
 115     ack = mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
 116
 117     ack = mpuConfiguration.write(MPU_RA_INT_PIN_CFG,
 118             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
 119
 120 #ifdef USE_MPU_DATA_READY_SIGNAL
 121     ack = mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
 122 #endif
 123     UNUSED(ack);
 124 }