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 mpuDetectionResult_t mpuDetectionResult;
  41 extern uint8_t mpuLowPassFilter;
  42
  43 //#define DEBUG_MPU_DATA_READY_INTERRUPT
  44
  45 // MPU6050, Standard address 0x68
  46 // MPU_INT on PB13 on rev4 Naze32 hardware
  47 #define MPU6050_ADDRESS         0x68
  48
  49 #define DMP_MEM_START_ADDR 0x6E
  50 #define DMP_MEM_R_W 0x6F
  51
  52 // RA = Register Address
  53
  54 #define MPU_RA_XG_OFFS_TC       0x00    //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
  55 #define MPU_RA_YG_OFFS_TC       0x01    //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
  56 #define MPU_RA_ZG_OFFS_TC       0x02    //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
  57 #define MPU_RA_X_FINE_GAIN      0x03    //[7:0] X_FINE_GAIN
  58 #define MPU_RA_Y_FINE_GAIN      0x04    //[7:0] Y_FINE_GAIN
  59 #define MPU_RA_Z_FINE_GAIN      0x05    //[7:0] Z_FINE_GAIN
  60 #define MPU_RA_XA_OFFS_H        0x06    //[15:0] XA_OFFS
  61 #define MPU_RA_XA_OFFS_L_TC     0x07
  62 #define MPU_RA_YA_OFFS_H        0x08    //[15:0] YA_OFFS
  63 #define MPU_RA_YA_OFFS_L_TC     0x09
  64 #define MPU_RA_ZA_OFFS_H        0x0A    //[15:0] ZA_OFFS
  65 #define MPU_RA_ZA_OFFS_L_TC     0x0B
  66 #define MPU_RA_PRODUCT_ID       0x0C    // Product ID Register
  67 #define MPU_RA_XG_OFFS_USRH     0x13    //[15:0] XG_OFFS_USR
  68 #define MPU_RA_XG_OFFS_USRL     0x14
  69 #define MPU_RA_YG_OFFS_USRH     0x15    //[15:0] YG_OFFS_USR
  70 #define MPU_RA_YG_OFFS_USRL     0x16
  71 #define MPU_RA_ZG_OFFS_USRH     0x17    //[15:0] ZG_OFFS_USR
  72 #define MPU_RA_ZG_OFFS_USRL     0x18
  73 #define MPU_RA_SMPLRT_DIV       0x19
  74 #define MPU_RA_CONFIG           0x1A
  75 #define MPU_RA_GYRO_CONFIG      0x1B
  76 #define MPU_RA_ACCEL_CONFIG     0x1C
  77 #define MPU_RA_FF_THR           0x1D
  78 #define MPU_RA_FF_DUR           0x1E
  79 #define MPU_RA_MOT_THR          0x1F
  80 #define MPU_RA_MOT_DUR          0x20
  81 #define MPU_RA_ZRMOT_THR        0x21
  82 #define MPU_RA_ZRMOT_DUR        0x22
  83 #define MPU_RA_FIFO_EN          0x23
  84 #define MPU_RA_I2C_MST_CTRL     0x24
  85 #define MPU_RA_I2C_SLV0_ADDR    0x25
  86 #define MPU_RA_I2C_SLV0_REG     0x26
  87 #define MPU_RA_I2C_SLV0_CTRL    0x27
  88 #define MPU_RA_I2C_SLV1_ADDR    0x28
  89 #define MPU_RA_I2C_SLV1_REG     0x29
  90 #define MPU_RA_I2C_SLV1_CTRL    0x2A
  91 #define MPU_RA_I2C_SLV2_ADDR    0x2B
  92 #define MPU_RA_I2C_SLV2_REG     0x2C
  93 #define MPU_RA_I2C_SLV2_CTRL    0x2D
  94 #define MPU_RA_I2C_SLV3_ADDR    0x2E
  95 #define MPU_RA_I2C_SLV3_REG     0x2F
  96 #define MPU_RA_I2C_SLV3_CTRL    0x30
  97 #define MPU_RA_I2C_SLV4_ADDR    0x31
  98 #define MPU_RA_I2C_SLV4_REG     0x32
  99 #define MPU_RA_I2C_SLV4_DO      0x33
 100 #define MPU_RA_I2C_SLV4_CTRL    0x34
 101 #define MPU_RA_I2C_SLV4_DI      0x35
 102 #define MPU_RA_I2C_MST_STATUS   0x36
 103 #define MPU_RA_INT_PIN_CFG      0x37
 104 #define MPU_RA_INT_ENABLE       0x38
 105 #define MPU_RA_DMP_INT_STATUS   0x39
 106 #define MPU_RA_INT_STATUS       0x3A
 107 #define MPU_RA_ACCEL_XOUT_H     0x3B
 108 #define MPU_RA_ACCEL_XOUT_L     0x3C
 109 #define MPU_RA_ACCEL_YOUT_H     0x3D
 110 #define MPU_RA_ACCEL_YOUT_L     0x3E
 111 #define MPU_RA_ACCEL_ZOUT_H     0x3F
 112 #define MPU_RA_ACCEL_ZOUT_L     0x40
 113 #define MPU_RA_TEMP_OUT_H       0x41
 114 #define MPU_RA_TEMP_OUT_L       0x42
 115 #define MPU_RA_GYRO_XOUT_H      0x43
 116 #define MPU_RA_GYRO_XOUT_L      0x44
 117 #define MPU_RA_GYRO_YOUT_H      0x45
 118 #define MPU_RA_GYRO_YOUT_L      0x46
 119 #define MPU_RA_GYRO_ZOUT_H      0x47
 120 #define MPU_RA_GYRO_ZOUT_L      0x48
 121 #define MPU_RA_EXT_SENS_DATA_00 0x49
 122 #define MPU_RA_MOT_DETECT_STATUS    0x61
 123 #define MPU_RA_I2C_SLV0_DO      0x63
 124 #define MPU_RA_I2C_SLV1_DO      0x64
 125 #define MPU_RA_I2C_SLV2_DO      0x65
 126 #define MPU_RA_I2C_SLV3_DO      0x66
 127 #define MPU_RA_I2C_MST_DELAY_CTRL   0x67
 128 #define MPU_RA_SIGNAL_PATH_RESET    0x68
 129 #define MPU_RA_MOT_DETECT_CTRL      0x69
 130 #define MPU_RA_USER_CTRL        0x6A
 131 #define MPU_RA_PWR_MGMT_1       0x6B
 132 #define MPU_RA_PWR_MGMT_2       0x6C
 133 #define MPU_RA_BANK_SEL         0x6D
 134 #define MPU_RA_MEM_START_ADDR   0x6E
 135 #define MPU_RA_MEM_R_W          0x6F
 136 #define MPU_RA_DMP_CFG_1        0x70
 137 #define MPU_RA_DMP_CFG_2        0x71
 138 #define MPU_RA_FIFO_COUNTH      0x72
 139 #define MPU_RA_FIFO_COUNTL      0x73
 140 #define MPU_RA_FIFO_R_W         0x74
 141 #define MPU_RA_WHO_AM_I         0x75
 142
 143 // RF = Register Flag
 144 #define MPU_RF_DATA_RDY_EN (1 << 0)
 145
 146 #define MPU6050_SMPLRT_DIV      0       // 8000Hz
 147
 148 static void mpu6050AccInit(void);
 149 static void mpu6050GyroInit(void);
 150
 151 bool mpu6050AccDetect(acc_t *acc)
 152 {
 153     if (mpuDetectionResult.sensor != MPU_60x0) {
 154         return false;
 155     }
 156
 157     acc->init = mpu6050AccInit;
 158     acc->read = mpuAccRead;
 159     acc->revisionCode = (mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
 160
 161     return true;
 162 }
 163
 164 bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
 165 {
 166     if (mpuDetectionResult.sensor != MPU_60x0) {
 167         return false;;
 168     }
 169     gyro->init = mpu6050GyroInit;
 170     gyro->read = mpuGyroRead;
 171
 172     // 16.4 dps/lsb scalefactor
 173     gyro->scale = 1.0f / 16.4f;
 174
 175     configureMPULPF(lpf);
 176
 177     return true;
 178 }
 179
 180 static void mpu6050AccInit(void)
 181 {
 182     mpuIntExtiInit();
 183
 184     switch (mpuDetectionResult.resolution) {
 185         case MPU_HALF_RESOLUTION:
 186             acc_1G = 256 * 8;
 187             break;
 188         case MPU_FULL_RESOLUTION:
 189             acc_1G = 512 * 8;
 190             break;
 191     }
 192 }
 193
 194 static void mpu6050GyroInit(void)
 195 {
 196     mpuIntExtiInit();
 197
 198     bool ack;
 199     ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
 200     delay(100);
 201     ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
 202     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)
 203     delay(15); //PLL Settling time when changing CLKSEL is max 10ms.  Use 15ms to be sure
 204     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)
 205     ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
 206
 207     // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
 208     // Accel scale 8g (4096 LSB/g)
 209     ack = mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
 210
 211     ack = mpuConfiguration.write(MPU_RA_INT_PIN_CFG,
 212             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
 213
 214 #ifdef USE_MPU_DATA_READY_SIGNAL
 215     ack = mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
 216 #endif
 217     UNUSED(ack);
 218 }