src/main/drivers/accgyro/accgyro_mpu6000.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV 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  * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 /*
  19  * Authors:
  20  * Dominic Clifton - Cleanflight implementation
  21  * John Ihlein - Initial FF32 code
  22  * Konstantin Sharlaimov - busDevice refactoring
  23 */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27
  28 #include "platform.h"
  29 #include "build/debug.h"
  30
  31 #include "common/axis.h"
  32 #include "common/maths.h"
  33
  34 #include "drivers/system.h"
  35 #include "drivers/time.h"
  36 #include "drivers/io.h"
  37 #include "drivers/bus.h"
  38
  39 #include "drivers/sensor.h"
  40 #include "drivers/accgyro/accgyro.h"
  41 #include "drivers/accgyro/accgyro_mpu.h"
  42 #include "drivers/accgyro/accgyro_mpu6000.h"
  43
  44 #if defined(USE_IMU_MPU6000)
  45
  46 // Bits
  47 #define BIT_H_RESET                 0x80
  48 #define MPU_CLK_SEL_PLLGYROX        0x01
  49 #define MPU_CLK_SEL_PLLGYROZ        0x03
  50 #define BIT_I2C_IF_DIS              0x10
  51 #define BIT_GYRO                    3
  52 #define BIT_ACC                     2
  53 #define BIT_TEMP                    1
  54
  55 // Product ID Description for MPU6000
  56 // high 4 bits low 4 bits
  57 // Product Name Product Revision
  58 #define MPU6000ES_REV_C4 0x14
  59 #define MPU6000ES_REV_C5 0x15
  60 #define MPU6000ES_REV_D6 0x16
  61 #define MPU6000ES_REV_D7 0x17
  62 #define MPU6000ES_REV_D8 0x18
  63 #define MPU6000_REV_C4 0x54
  64 #define MPU6000_REV_C5 0x55
  65 #define MPU6000_REV_D6 0x56
  66 #define MPU6000_REV_D7 0x57
  67 #define MPU6000_REV_D8 0x58
  68 #define MPU6000_REV_D9 0x59
  69 #define MPU6000_REV_D10 0x5A
  70
  71 static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
  72 {
  73     busDevice_t * busDev = gyro->busDev;
  74     const gyroFilterAndRateConfig_t * config = mpuChooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs);
  75     gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
  76
  77     busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
  78
  79     // Device Reset
  80     busWrite(busDev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
  81     delay(150);
  82
  83     busWrite(busDev, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
  84     delay(150);
  85
  86     // Clock Source PPL with Z axis gyro reference
  87     busWrite(busDev, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
  88     delayMicroseconds(15);
  89
  90     // Disable Primary I2C Interface
  91     busWrite(busDev, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
  92     delayMicroseconds(15);
  93
  94     busWrite(busDev, MPU_RA_PWR_MGMT_2, 0x00);
  95     delayMicroseconds(15);
  96
  97     // Accel Sample Rate 1kHz
  98     // Gyroscope Output Rate =  1kHz when the DLPF is enabled
  99     busWrite(busDev, MPU_RA_SMPLRT_DIV, config->gyroConfigValues[1]);
 100     delayMicroseconds(15);
 101
 102     // Gyro +/- 2000 DPS Full Scale
 103     busWrite(busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
 104     delayMicroseconds(15);
 105
 106     // Accel +/- 16 G Full Scale
 107     busWrite(busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
 108     delayMicroseconds(15);
 109
 110     // Accel and Gyro DLPF Setting
 111     busWrite(busDev, MPU_RA_CONFIG, config->gyroConfigValues[0]);
 112     delayMicroseconds(1);
 113
 114     busSetSpeed(busDev, BUS_SPEED_FAST);
 115
 116     mpuGyroRead(gyro);
 117
 118     if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) {
 119         failureMode(FAILURE_GYRO_INIT_FAILED);
 120     }
 121 }
 122
 123 static void mpu6000AccInit(accDev_t *acc)
 124 {
 125     acc->acc_1G = 512 * 4;
 126 }
 127
 128 bool mpu6000AccDetect(accDev_t *acc)
 129 {
 130     acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_MPU6000, acc->imuSensorToUse);
 131     if (acc->busDev == NULL) {
 132         return false;
 133     }
 134
 135     mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
 136     if (ctx->chipMagicNumber != 0x6860) {
 137         return false;
 138     }
 139
 140     acc->initFn = mpu6000AccInit;
 141     acc->readFn = mpuAccReadScratchpad;
 142     acc->accAlign = acc->busDev->param;
 143
 144     return true;
 145 }
 146
 147 static bool mpu6000DeviceDetect(busDevice_t * busDev)
 148 {
 149     uint8_t in;
 150     uint8_t attemptsRemaining = 5;
 151
 152     busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
 153
 154     busWrite(busDev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
 155
 156     do {
 157         delay(150);
 158
 159         busRead(busDev, MPU_RA_WHO_AM_I, &in);
 160         if (in == MPU6000_WHO_AM_I_CONST) {
 161             break;
 162         }
 163         if (!attemptsRemaining) {
 164             return false;
 165         }
 166     } while (attemptsRemaining--);
 167
 168     busRead(busDev, MPU_RA_PRODUCT_ID, &in);
 169
 170     /* look for a product ID we recognise */
 171     switch (in) {
 172         case MPU6000ES_REV_C4:
 173         case MPU6000ES_REV_C5:
 174         case MPU6000_REV_C4:
 175         case MPU6000_REV_C5:
 176         case MPU6000ES_REV_D6:
 177         case MPU6000ES_REV_D7:
 178         case MPU6000ES_REV_D8:
 179         case MPU6000_REV_D6:
 180         case MPU6000_REV_D7:
 181         case MPU6000_REV_D8:
 182         case MPU6000_REV_D9:
 183         case MPU6000_REV_D10:
 184             return true;
 185     }
 186
 187     return false;
 188 }
 189
 190 bool mpu6000GyroDetect(gyroDev_t *gyro)
 191 {
 192     gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MPU6000, gyro->imuSensorToUse, OWNER_MPU);
 193     if (gyro->busDev == NULL) {
 194         return false;
 195     }
 196
 197     if (!mpu6000DeviceDetect(gyro->busDev)) {
 198         busDeviceDeInit(gyro->busDev);
 199         return false;
 200     }
 201
 202     // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
 203     mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
 204     ctx->chipMagicNumber = 0x6860;
 205
 206     gyro->initFn = mpu6000AccAndGyroInit;
 207     gyro->readFn = mpuGyroReadScratchpad;
 208     gyro->intStatusFn = gyroCheckDataReady;
 209     gyro->temperatureFn = mpuTemperatureReadScratchpad;
 210     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
 211     gyro->gyroAlign = gyro->busDev->param;
 212
 213     return true;
 214 }
 215
 216 #endif