src/main/drivers/compass/compass_ak8963.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
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #if defined(USE_MAG_AK8963) || defined(USE_MAG_SPI_AK8963)
  29
  30 #include "build/debug.h"
  31
  32 #include "common/axis.h"
  33 #include "common/maths.h"
  34 #include "common/utils.h"
  35
  36 #include "drivers/bus.h"
  37 #include "drivers/bus_i2c.h"
  38 #include "drivers/bus_i2c_busdev.h"
  39 #include "drivers/bus_spi.h"
  40 #include "drivers/io.h"
  41 #include "drivers/sensor.h"
  42 #include "drivers/time.h"
  43
  44 #include "drivers/compass/compass.h"
  45
  46 #include "drivers/accgyro/accgyro.h"
  47 #include "drivers/accgyro/accgyro_mpu.h"
  48 #include "drivers/accgyro/accgyro_mpu6500.h"
  49 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
  50 #include "drivers/accgyro/accgyro_spi_mpu9250.h"
  51 #include "drivers/compass/compass_ak8963.h"
  52
  53 #include "scheduler/scheduler.h"
  54
  55 // This sensor is also available also part of the MPU-9250 connected to the secondary I2C bus.
  56
  57 // 10 MHz max SPI frequency
  58 #define AK8963_MAX_SPI_CLK_HZ 10000000
  59
  60 // AK8963, mag sensor address
  61 #define AK8963_MAG_I2C_ADDRESS          0x0C
  62 #define AK8963_DEVICE_ID                0x48
  63
  64 // Registers
  65 #define AK8963_MAG_REG_WIA              0x00
  66 #define AK8963_MAG_REG_INFO             0x01
  67 #define AK8963_MAG_REG_ST1              0x02
  68 #define AK8963_MAG_REG_HXL              0x03
  69 #define AK8963_MAG_REG_HXH              0x04
  70 #define AK8963_MAG_REG_HYL              0x05
  71 #define AK8963_MAG_REG_HYH              0x06
  72 #define AK8963_MAG_REG_HZL              0x07
  73 #define AK8963_MAG_REG_HZH              0x08
  74 #define AK8963_MAG_REG_ST2              0x09
  75 #define AK8963_MAG_REG_CNTL1            0x0A
  76 #define AK8963_MAG_REG_CNTL2            0x0B
  77 #define AK8963_MAG_REG_ASCT             0x0C // self test
  78 #define AK8963_MAG_REG_I2CDIS           0x0F
  79 #define AK8963_MAG_REG_ASAX             0x10 // Fuse ROM x-axis sensitivity adjustment value
  80 #define AK8963_MAG_REG_ASAY             0x11 // Fuse ROM y-axis sensitivity adjustment value
  81 #define AK8963_MAG_REG_ASAZ             0x12 // Fuse ROM z-axis sensitivity adjustment value
  82
  83 #define READ_FLAG                       0x80
  84 #define I2C_SLV0_EN                     0x80
  85
  86 #define ST1_DATA_READY                  0x01
  87 #define ST1_DATA_OVERRUN                0x02
  88
  89 #define ST2_MAG_SENSOR_OVERFLOW         0x08
  90
  91 #define CNTL1_MODE_POWER_DOWN           0x00
  92 #define CNTL1_MODE_ONCE                 0x01
  93 #define CNTL1_MODE_CONT1                0x02
  94 #define CNTL1_MODE_CONT2                0x06
  95 #define CNTL1_MODE_SELF_TEST            0x08
  96 #define CNTL1_MODE_FUSE_ROM             0x0F
  97 #define CNTL1_BIT_14_BIT                0x00
  98 #define CNTL1_BIT_16_BIT                0x10
  99
 100 #define CNTL2_SOFT_RESET                0x01
 101
 102 #define I2CDIS_DISABLE_MASK             0x1D
 103
 104 #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
 105
 106 static bool ak8963SpiWriteRegisterDelay(const extDevice_t *dev, uint8_t reg, uint8_t data)
 107 {
 108     spiWriteReg(dev, reg, data);
 109     delayMicroseconds(10);
 110     return true;
 111 }
 112
 113 static bool ak8963SlaveReadRegisterBuffer(const extDevice_t *slaveDev, uint8_t reg, uint8_t *buf, uint8_t len)
 114 {
 115     extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master;
 116
 117     ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_ADDR, slaveDev->busType_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read
 118     ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_REG, reg);                             // set I2C slave register
 119     ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN);     // read number of bytes
 120     delay(4);
 121     __disable_irq();
 122     bool ack = spiReadRegMskBufRB(dev, MPU_RA_EXT_SENS_DATA_00, buf, len);            // read I2C
 123     __enable_irq();
 124     return ack;
 125 }
 126
 127 static bool ak8963SlaveWriteRegister(const extDevice_t *slaveDev, uint8_t reg, uint8_t data)
 128 {
 129     extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master;
 130
 131     ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_ADDR, slaveDev->busType_u.mpuSlave.address); // set I2C slave address for write
 132     ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_REG, reg);                             // set I2C slave register
 133     ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_DO, data);                             // set I2C sLave value
 134     ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_CTRL, (1 & 0x0F) | I2C_SLV0_EN);       // write 1 byte
 135     return true;
 136 }
 137
 138 typedef struct queuedReadState_s {
 139     bool waiting;
 140     uint8_t len;
 141     uint32_t readStartedAt;                                                                 // time read was queued in micros.
 142 } queuedReadState_t;
 143
 144 static queuedReadState_t queuedRead = { false, 0, 0};
 145
 146 static bool ak8963SlaveStartRead(const extDevice_t *slaveDev, uint8_t reg, uint8_t len)
 147 {
 148     if (queuedRead.waiting) {
 149         return false;
 150     }
 151
 152     extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master;
 153
 154     queuedRead.len = len;
 155
 156     ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_ADDR, slaveDev->busType_u.mpuSlave.address | READ_FLAG);  // set I2C slave address for read
 157     ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_REG, reg);                             // set I2C slave register
 158     ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN);    // read number of bytes
 159
 160     queuedRead.readStartedAt = micros();
 161     queuedRead.waiting = true;
 162
 163     return true;
 164 }
 165
 166 static uint32_t ak8963SlaveQueuedReadTimeRemaining(void)
 167 {
 168     if (!queuedRead.waiting) {
 169         return 0;
 170     }
 171
 172     int32_t timeSinceStarted = micros() - queuedRead.readStartedAt;
 173
 174     int32_t timeRemaining = 8000 - timeSinceStarted;
 175
 176     if (timeRemaining < 0) {
 177         return 0;
 178     }
 179
 180     return timeRemaining;
 181 }
 182
 183 static bool ak8963SlaveCompleteRead(const extDevice_t *slaveDev, uint8_t *buf)
 184 {
 185     uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining();
 186
 187     extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master;
 188
 189     if (timeRemaining > 0) {
 190         delayMicroseconds(timeRemaining);
 191     }
 192
 193     queuedRead.waiting = false;
 194
 195     return spiReadRegMskBufRB(dev, MPU_RA_EXT_SENS_DATA_00, buf, queuedRead.len);            // read I2C buffer
 196 }
 197
 198 static bool ak8963SlaveReadData(const extDevice_t *dev, uint8_t *buf)
 199 {
 200     typedef enum {
 201         CHECK_STATUS = 0,
 202         WAITING_FOR_STATUS,
 203         WAITING_FOR_DATA
 204     } ak8963ReadState_e;
 205
 206     static ak8963ReadState_e state = CHECK_STATUS;
 207
 208     bool ack = false;
 209
 210     // we currently need a different approach for the MPU9250 connected via SPI.
 211     // we cannot use the ak8963SlaveReadRegisterBuffer() method for SPI, it is to slow and blocks for far too long.
 212
 213     bool retry = true;
 214
 215 restart:
 216     switch (state) {
 217         case CHECK_STATUS: {
 218             ak8963SlaveStartRead(dev, AK8963_MAG_REG_ST1, 1);
 219             state = WAITING_FOR_STATUS;
 220             return false;
 221         }
 222
 223         case WAITING_FOR_STATUS: {
 224             uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining();
 225             if (timeRemaining) {
 226                 return false;
 227             }
 228
 229             ack = ak8963SlaveCompleteRead(dev, &buf[0]);
 230
 231             uint8_t status = buf[0];
 232
 233             if (!ack || (status & ST1_DATA_READY) == 0) {
 234                 // too early. queue the status read again
 235                 state = CHECK_STATUS;
 236                 if (retry) {
 237                     retry = false;
 238                     goto restart;
 239                }
 240                return false;
 241             }
 242
 243             // read the 6 bytes of data and the status2 register
 244             ak8963SlaveStartRead(dev, AK8963_MAG_REG_HXL, 7);
 245
 246             state = WAITING_FOR_DATA;
 247             return false;
 248         }
 249
 250         case WAITING_FOR_DATA: {
 251             uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining();
 252             if (timeRemaining) {
 253                 return false;
 254             }
 255
 256             ack = ak8963SlaveCompleteRead(dev, &buf[0]);
 257             state = CHECK_STATUS;
 258         }
 259     }
 260
 261     return ack;
 262 }
 263 #endif
 264
 265 static bool ak8963ReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *buf, uint8_t len)
 266 {
 267 #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
 268     if (dev->bus->busType == BUS_TYPE_MPU_SLAVE) {
 269         return ak8963SlaveReadRegisterBuffer(dev, reg, buf, len);
 270     }
 271 #endif
 272     return busReadRegisterBuffer(dev, reg, buf, len);
 273 }
 274
 275 static bool ak8963WriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data)
 276 {
 277 #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
 278     if (dev->bus->busType == BUS_TYPE_MPU_SLAVE) {
 279         return ak8963SlaveWriteRegister(dev, reg, data);
 280     }
 281 #endif
 282     return busWriteRegister(dev, reg, data);
 283 }
 284
 285 static bool ak8963DirectReadData(const extDevice_t *dev, uint8_t *buf)
 286 {
 287     uint8_t status;
 288
 289     bool ack = ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ST1, &status, 1);
 290
 291     if (!ack || (status & ST1_DATA_READY) == 0) {
 292         return false;
 293     }
 294
 295     return ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_HXL, buf, 7);
 296 }
 297
 298 static int16_t parseMag(const uint8_t *raw, int16_t gain)
 299 {
 300   int ret = (int16_t)(raw[1] << 8 | raw[0]) * gain / 256;
 301   return constrain(ret, INT16_MIN, INT16_MAX);
 302 }
 303
 304 static bool ak8963Read(magDev_t *mag, int16_t *magData)
 305 {
 306     bool ack = false;
 307     uint8_t buf[7];
 308
 309     extDevice_t *dev = &mag->dev;
 310
 311     switch (dev->bus->busType) {
 312 #if defined(USE_MAG_SPI_AK8963) || defined(USE_MAG_AK8963)
 313     case BUS_TYPE_I2C:
 314     case BUS_TYPE_SPI:
 315         ack = ak8963DirectReadData(dev, buf);
 316         break;
 317 #endif
 318
 319 #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
 320     case BUS_TYPE_MPU_SLAVE:
 321         ack = ak8963SlaveReadData(dev, buf);
 322         break;
 323 #endif
 324     default:
 325         break;
 326     }
 327
 328     uint8_t status2 = buf[6];
 329     if (!ack) {
 330         return false;
 331     }
 332
 333     ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE); // start reading again    uint8_t status2 = buf[6];
 334
 335     if (status2 & ST2_MAG_SENSOR_OVERFLOW) {
 336         return false;
 337     }
 338
 339     magData[X] = parseMag(buf + 0, mag->magGain[X]);
 340     magData[Y] = parseMag(buf + 2, mag->magGain[Y]);
 341     magData[Z] = parseMag(buf + 4, mag->magGain[Z]);
 342
 343     return true;
 344 }
 345
 346 static bool ak8963Init(magDev_t *mag)
 347 {
 348     uint8_t asa[3];
 349     uint8_t status;
 350
 351     extDevice_t *dev = &mag->dev;
 352
 353     busDeviceRegister(dev);
 354
 355     ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN);               // power down before entering fuse mode
 356     ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM);                 // Enter Fuse ROM access mode
 357     ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ASAX, asa, sizeof(asa));                // Read the x-, y-, and z-axis calibration values
 358
 359     mag->magGain[X] = asa[X] + 128;
 360     mag->magGain[Y] = asa[Y] + 128;
 361     mag->magGain[Z] = asa[Z] + 128;
 362
 363     ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN);               // power down after reading.
 364
 365     // Clear status registers
 366     ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ST1, &status, 1);
 367     ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ST2, &status, 1);
 368
 369     // Trigger first measurement
 370     ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE);
 371     mag->magOdrHz = 50; // arbitrary value, need to check what ODR is actually returned
 372     return true;
 373 }
 374
 375 void ak8963BusInit(const extDevice_t *dev)
 376 {
 377     switch (dev->bus->busType) {
 378 #ifdef USE_MAG_AK8963
 379     case BUS_TYPE_I2C:
 380         UNUSED(dev);
 381         break;
 382 #endif
 383
 384 #ifdef USE_MAG_SPI_AK8963
 385     case BUS_TYPE_SPI:
 386         IOHi(dev->busType_u.spi.csnPin);                                                  // Disable
 387         IOInit(dev->busType_u.spi.csnPin, OWNER_COMPASS_CS, 0);
 388         IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
 389         spiSetClkDivisor(dev, spiCalculateDivider(AK8963_MAX_SPI_CLK_HZ));
 390         break;
 391 #endif
 392
 393 #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
 394     case BUS_TYPE_MPU_SLAVE:
 395
 396         // Disable DMA on gyro as this upsets slave access timing
 397         spiDmaEnable(dev->bus->busType_u.mpuSlave.master, false);
 398
 399         // initialize I2C master via SPI bus
 400         ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
 401         ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
 402         ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_USER_CTRL, 0x30);   // I2C master mode, SPI mode only
 403         break;
 404 #endif
 405     default:
 406         break;
 407     }
 408 }
 409
 410 void ak8963BusDeInit(const extDevice_t *dev)
 411 {
 412     switch (dev->bus->busType) {
 413 #ifdef USE_MAG_AK8963
 414     case BUS_TYPE_I2C:
 415         UNUSED(dev);
 416         break;
 417 #endif
 418
 419 #ifdef USE_MAG_SPI_AK8963
 420     case BUS_TYPE_SPI:
 421         spiPreinitByIO(dev->busType_u.spi.csnPin);
 422         break;
 423 #endif
 424
 425 #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
 426     case BUS_TYPE_MPU_SLAVE:
 427         ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR);
 428         break;
 429 #endif
 430     default:
 431         break;
 432     }
 433 }
 434
 435 bool ak8963Detect(magDev_t *mag)
 436 {
 437     uint8_t sig = 0;
 438
 439     extDevice_t *dev = &mag->dev;
 440
 441     if ((dev->bus->busType == BUS_TYPE_I2C || dev->bus->busType == BUS_TYPE_MPU_SLAVE) && dev->busType_u.mpuSlave.address == 0) {
 442         dev->busType_u.mpuSlave.address = AK8963_MAG_I2C_ADDRESS;
 443     }
 444
 445     ak8963BusInit(dev);
 446
 447     ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL2, CNTL2_SOFT_RESET);                    // reset MAG
 448     delay(4);
 449
 450     bool ack = ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_WIA, &sig, 1);               // check for AK8963
 451
 452     if (ack && sig == AK8963_DEVICE_ID) // 0x48 / 01001000 / 'H'
 453     {
 454         mag->init = ak8963Init;
 455         mag->read = ak8963Read;
 456
 457         return true;
 458     }
 459
 460     ak8963BusDeInit(dev);
 461
 462     return false;
 463 }
 464 #endif