src/main/drivers/compass/compass_mpu9250.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 <string.h>
  21
  22 #include <math.h>
  23
  24 #include "platform.h"
  25
  26 #include "build/debug.h"
  27
  28 #include "common/axis.h"
  29 #include "common/maths.h"
  30 #include "common/utils.h"
  31
  32 #include "drivers/bus.h"
  33 #include "drivers/sensor.h"
  34 #include "drivers/system.h"
  35 #include "drivers/time.h"
  36
  37 #include "drivers/accgyro/accgyro.h"
  38 #include "drivers/accgyro/accgyro_mpu.h"
  39
  40 #include "drivers/compass/compass.h"
  41 #include "drivers/compass/compass_mpu9250.h"
  42
  43 #if defined(USE_MAG_MPU9250) && defined(USE_IMU_MPU9250)
  44
  45 // No separate hardware descriptor needed. Hardware descriptor initialization is handled by GYRO driver
  46
  47 // AK8963, mag sensor address
  48 #define AK8963_MAG_I2C_ADDRESS          0x0C
  49 #define AK8963_DEVICE_ID                0x48
  50
  51 // Registers
  52 #define AK8963_MAG_REG_WHO_AM_I         0x00
  53 #define AK8963_MAG_REG_INFO             0x01
  54 #define AK8963_MAG_REG_STATUS1          0x02
  55 #define AK8963_MAG_REG_HXL              0x03
  56 #define AK8963_MAG_REG_HXH              0x04
  57 #define AK8963_MAG_REG_HYL              0x05
  58 #define AK8963_MAG_REG_HYH              0x06
  59 #define AK8963_MAG_REG_HZL              0x07
  60 #define AK8963_MAG_REG_HZH              0x08
  61 #define AK8963_MAG_REG_STATUS2          0x09
  62 #define AK8963_MAG_REG_CNTL             0x0a
  63 #define AK8963_MAG_REG_ASCT             0x0c // self test
  64 #define AK8963_MAG_REG_ASAX             0x10 // Fuse ROM x-axis sensitivity adjustment value
  65 #define AK8963_MAG_REG_ASAY             0x11 // Fuse ROM y-axis sensitivity adjustment value
  66 #define AK8963_MAG_REG_ASAZ             0x12 // Fuse ROM z-axis sensitivity adjustment value
  67
  68 #define READ_FLAG                       0x80
  69
  70 #define STATUS1_DATA_READY              0x01
  71 #define STATUS1_DATA_OVERRUN            0x02
  72
  73 #define STATUS2_MAG_SENSOR_OVERFLOW     0x08
  74
  75 #define CNTL_MODE_POWER_DOWN            0x00
  76 #define CNTL_MODE_ONCE                  0x01
  77 #define CNTL_MODE_CONT1                 0x02
  78 #define CNTL_MODE_CONT2                 0x06
  79 #define CNTL_MODE_SELF_TEST             0x08
  80 #define CNTL_MODE_FUSE_ROM              0x0F
  81 #define CNTL_BIT_14_BIT                 0x00
  82 #define CNTL_BIT_16_BIT                 0x10
  83
  84 #define DETECTION_MAX_RETRY_COUNT   5
  85
  86 typedef enum {
  87     CHECK_STATUS = 0,
  88     WAITING_FOR_STATUS,
  89     WAITING_FOR_DATA
  90 } mpu9250CompassReadState_e;
  91
  92 typedef struct {
  93     mpu9250CompassReadState_e   state;
  94     bool                        waiting;
  95     uint8_t                     len;
  96     timeUs_t                    readStartedAt; // time read was queued in micros.
  97 } mpu9250CompassReadContext_s;
  98
  99 static int16_t magGain[3];
 100 static mpu9250CompassReadContext_s  ctx;
 101 static bool lastReadResult = false;
 102 static int16_t cachedMagData[3];
 103
 104 static bool mpu9250SlaveI2CRead(magDev_t * mag, uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len)
 105 {
 106     // Setting up MPU9250's I2C master to read from AK8963 via internal I2C bus
 107     busWrite(mag->busDev, MPU_RA_I2C_SLV0_ADDR, addr | READ_FLAG);     // set I2C slave address for read
 108     busWrite(mag->busDev, MPU_RA_I2C_SLV0_REG, reg);                   // set I2C slave register
 109     busWrite(mag->busDev, MPU_RA_I2C_SLV0_CTRL, len | 0x80);           // read number of bytes
 110     delay(10);                                                      // wait for transaction to complete
 111     busReadBuf(mag->busDev, MPU_RA_EXT_SENS_DATA_00, buf, len);        // read I2C
 112     return true;
 113 }
 114
 115 static bool mpu9250SlaveI2CWrite(magDev_t * mag, uint8_t addr, uint8_t reg, uint8_t data)
 116 {
 117     // Setting up MPU9250's I2C master to write to AK8963 via internal I2C bus
 118     busWrite(mag->busDev, MPU_RA_I2C_SLV0_ADDR, addr);         // set I2C slave address for write
 119     busWrite(mag->busDev, MPU_RA_I2C_SLV0_REG, reg);           // set I2C slave register
 120     busWrite(mag->busDev, MPU_RA_I2C_SLV0_DO, data);           // set I2C salve value
 121     busWrite(mag->busDev, MPU_RA_I2C_SLV0_CTRL, 0x81);         // write 1 byte
 122     return true;
 123 }
 124
 125 static bool mpu9250SlaveI2CStartRead(magDev_t * mag, uint8_t addr, uint8_t reg, uint8_t len)
 126 {
 127     if (ctx.waiting) {
 128         return false;
 129     }
 130
 131     ctx.len = len;
 132
 133     busWrite(mag->busDev, MPU_RA_I2C_SLV0_ADDR, addr | READ_FLAG);     // set I2C slave address for read
 134     busWrite(mag->busDev, MPU_RA_I2C_SLV0_REG, reg);                   // set I2C slave register
 135     busWrite(mag->busDev, MPU_RA_I2C_SLV0_CTRL, len | 0x80);           // read number of bytes
 136
 137     ctx.readStartedAt = micros();
 138     ctx.waiting = true;
 139     return true;
 140 }
 141
 142 static timeDelta_t mpu9250SlaveI2CReadTimeRemaining(void)
 143 {
 144     if (!ctx.waiting) {
 145         return 0;
 146     }
 147
 148     timeDelta_t timeSinceStarted = micros() - ctx.readStartedAt;
 149     timeDelta_t timeRemaining = 8000 - timeSinceStarted;
 150
 151     if (timeRemaining < 0) {
 152         return 0;
 153     }
 154
 155     return timeRemaining;
 156 }
 157
 158 static bool mpu9250SlaveI2CCompleteRead(magDev_t * mag, uint8_t * buf)
 159 {
 160     timeDelta_t timeRemaining = mpu9250SlaveI2CReadTimeRemaining();
 161
 162     if (timeRemaining > 0) {
 163         delayMicroseconds(timeRemaining);
 164     }
 165
 166     ctx.waiting = false;
 167     busReadBuf(mag->busDev, MPU_RA_EXT_SENS_DATA_00, buf, ctx.len);
 168     return true;
 169 }
 170
 171 static bool mpu9250CompassInit(magDev_t * mag)
 172 {
 173     uint8_t calibration[3];
 174     uint8_t status;
 175
 176     // Do init at low speed
 177     busSetSpeed(mag->busDev, BUS_SPEED_INITIALIZATION);
 178
 179     mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
 180     delay(20);
 181
 182     mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode
 183     delay(10);
 184
 185     mpu9250SlaveI2CRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ASAX, calibration, sizeof(calibration)); // Read the x-, y-, and z-axis calibration values
 186     delay(10);
 187
 188     magGain[X] = calibration[X] + 128;
 189     magGain[Y] = calibration[Y] + 128;
 190     magGain[Z] = calibration[Z] + 128;
 191
 192     mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
 193     delay(10);
 194
 195     // Clear status registers
 196     mpu9250SlaveI2CRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, &status, 1);
 197     mpu9250SlaveI2CRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, &status, 1);
 198
 199     // Trigger first measurement
 200     mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_CONT1);
 201
 202     busSetSpeed(mag->busDev, BUS_SPEED_FAST);
 203     return true;
 204 }
 205
 206 static int16_t parseMag(uint8_t *raw, int16_t gain) {
 207   int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256;
 208   return constrain(ret, INT16_MIN, INT16_MAX);
 209 }
 210
 211 static bool mpu9250CompassRead(magDev_t * mag)
 212 {
 213     bool ack = false;
 214     uint8_t buf[7];
 215
 216     // set magData to latest cached value
 217     memcpy(&mag->magADCRaw, cachedMagData, sizeof(cachedMagData));
 218
 219     bool reprocess;
 220
 221     do {
 222         reprocess = false;
 223
 224         switch (ctx.state) {
 225             case CHECK_STATUS:
 226                 mpu9250SlaveI2CStartRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1);
 227                 ctx.state = WAITING_FOR_STATUS;
 228                 return lastReadResult;
 229
 230             case WAITING_FOR_STATUS: {
 231                 uint32_t timeRemaining = mpu9250SlaveI2CReadTimeRemaining();
 232                 if (timeRemaining) {
 233                     return lastReadResult;
 234                 }
 235
 236                 ack = mpu9250SlaveI2CCompleteRead(mag, &buf[0]);
 237                 uint8_t status = buf[0];
 238
 239                 if (!ack || ((status & STATUS1_DATA_READY) == 0 && (status & STATUS1_DATA_OVERRUN) == 0)) {
 240                     // too early. queue the status read again
 241                     ctx.state = CHECK_STATUS;
 242                     reprocess = true;
 243                     return lastReadResult;
 244                 }
 245
 246                 // read the 6 bytes of data and the status2 register
 247                 mpu9250SlaveI2CStartRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7);
 248                 ctx.state = WAITING_FOR_DATA;
 249                 return lastReadResult;
 250             }
 251
 252             case WAITING_FOR_DATA: {
 253                 uint32_t timeRemaining = mpu9250SlaveI2CReadTimeRemaining();
 254                 if (timeRemaining) {
 255                     return lastReadResult;
 256                 }
 257
 258                 ack = mpu9250SlaveI2CCompleteRead(mag, &buf[0]);
 259                 break;
 260             }
 261         }
 262     } while(reprocess);
 263
 264     uint8_t status2 = buf[6];
 265     if (!ack || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) {
 266         ctx.state = CHECK_STATUS;
 267         lastReadResult = false;
 268         return lastReadResult;
 269     }
 270
 271     mag->magADCRaw[X] = -parseMag(buf + 0, magGain[X]);
 272     mag->magADCRaw[Y] = parseMag(buf + 2, magGain[Y]);
 273     mag->magADCRaw[Z] = -parseMag(buf + 4, magGain[Z]);
 274
 275     memcpy(cachedMagData, &mag->magADCRaw, sizeof(cachedMagData));
 276     ctx.state = CHECK_STATUS;
 277     lastReadResult = true;
 278
 279     return lastReadResult;
 280 }
 281
 282 bool mpu9250CompassDetect(magDev_t * mag)
 283 {
 284     // Compass on MPU9250 is only supported if MPU9250 is connected to SPI bus
 285     // FIXME: We need to use gyro_to_use here, not mag_to_use
 286     mag->busDev = busDeviceOpen(BUSTYPE_SPI, DEVHW_MPU9250, mag->magSensorToUse);
 287     if (mag->busDev == NULL) {
 288         return false;
 289     }
 290
 291     // Check if Gyro driver initialized the chip
 292     mpuContextData_t * ctx = busDeviceGetScratchpadMemory(mag->busDev);
 293     if (ctx->chipMagicNumber != 0x9250) {
 294         return false;
 295     }
 296
 297     busSetSpeed(mag->busDev, BUS_SPEED_INITIALIZATION);
 298     for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
 299         bool ack = false;
 300         uint8_t sig = 0;
 301
 302         // Initialize I2C master via SPI bus (MPU9250)
 303         busWrite(mag->busDev, MPU_RA_INT_PIN_CFG, 0x10);       // INT_ANYRD_2CLEAR
 304         delay(15);
 305
 306         busWrite(mag->busDev, MPU_RA_I2C_MST_CTRL, 0x0D);      // I2C multi-master / 400kHz
 307         delay(15);
 308
 309         busWrite(mag->busDev, MPU_RA_USER_CTRL, 0x30);         // I2C master mode, SPI mode only
 310         delay(15);
 311
 312         // check for AK8963
 313         ack = mpu9250SlaveI2CRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, &sig, 1);
 314         if (ack && sig == AK8963_DEVICE_ID) { // 0x48 / 01001000 / 'H'
 315             mag->init = mpu9250CompassInit;
 316             mag->read = mpu9250CompassRead;
 317             return true;
 318         }
 319     }
 320
 321     busSetSpeed(mag->busDev, BUS_SPEED_FAST);
 322     return false;
 323 }
 324
 325 #endif