flight/pios/common/pios_mpu9250.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup PIOS_MPU9250 MPU9250 Functions
   6  * @brief Deals with the hardware interface to the 9 DOF sensor.
   7  * @{
   8  *
   9  * @file       pios_mp9250.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  11  * @brief      MPU9250 9-axis gyro, accel and mag chip
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  ******************************************************************************
  15  */
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include "pios.h"
  33 #include <pios_mpu9250.h>
  34 #ifdef PIOS_INCLUDE_MPU9250
  35 #include <stdint.h>
  36 #include <pios_constants.h>
  37 #include <pios_sensors.h>
  38 /* Global Variables */
  39
  40 enum pios_mpu9250_dev_magic {
  41     PIOS_MPU9250_DEV_MAGIC = 0x9da9b3ed,
  42 };
  43
  44 struct mpu9250_dev {
  45     uint32_t spi_id;
  46     uint32_t slave_num;
  47     QueueHandle_t queue;
  48     const struct pios_mpu9250_cfg *cfg;
  49     enum pios_mpu9250_range gyro_range;
  50     enum pios_mpu9250_accel_range accel_range;
  51     enum pios_mpu9250_filter filter;
  52     enum pios_mpu9250_dev_magic   magic;
  53     float mag_sens_adj[PIOS_MPU9250_MAG_ASA_NB_BYTE];
  54 };
  55
  56 #ifdef PIOS_MPU9250_ACCEL
  57 #define PIOS_MPU9250_ACCEL_SAMPLES_BYTES (6)
  58 #else
  59 #define PIOS_MPU9250_ACCEL_SAMPLES_BYTES (0)
  60 #endif
  61
  62 #ifdef PIOS_MPU9250_MAG
  63 #define PIOS_MPU9250_MAG_SAMPLES_BYTES   (8)
  64 #else
  65 #define PIOS_MPU9250_MAG_SAMPLES_BYTES   (0)
  66 #endif
  67
  68 #define PIOS_MPU9250_GYRO_SAMPLES_BYTES  (6)
  69 #define PIOS_MPU9250_TEMP_SAMPLES_BYTES  (2)
  70
  71 #define PIOS_MPU9250_SAMPLES_BYTES \
  72     (PIOS_MPU9250_ACCEL_SAMPLES_BYTES + \
  73      PIOS_MPU9250_GYRO_SAMPLES_BYTES + \
  74      PIOS_MPU9250_TEMP_SAMPLES_BYTES + \
  75      PIOS_MPU9250_MAG_SAMPLES_BYTES)
  76
  77 #ifdef PIOS_MPU9250_ACCEL
  78 #define PIOS_MPU9250_SENSOR_FIRST_REG    PIOS_MPU9250_ACCEL_X_OUT_MSB
  79 #else
  80 #define PIOS_MPU9250_SENSOR_FIRST_REG    PIOS_MPU9250_TEMP_OUT_MSB
  81 #endif
  82
  83 #if defined(PIOS_MPU9250_MAG) && !defined(PIOS_MPU9250_ACCEL)
  84 #error ERROR: PIOS_MPU9250_ACCEL not defined! THIS CONFIGURATION IS NOT SUPPORTED
  85 #endif
  86
  87 typedef union {
  88     uint8_t buffer[2 + PIOS_MPU9250_SAMPLES_BYTES];
  89     struct {
  90         uint8_t dummy;
  91 #ifdef PIOS_MPU9250_ACCEL
  92         uint8_t Accel_X_h;
  93         uint8_t Accel_X_l;
  94         uint8_t Accel_Y_h;
  95         uint8_t Accel_Y_l;
  96         uint8_t Accel_Z_h;
  97         uint8_t Accel_Z_l;
  98 #endif
  99         uint8_t Temperature_h;
 100         uint8_t Temperature_l;
 101         uint8_t Gyro_X_h;
 102         uint8_t Gyro_X_l;
 103         uint8_t Gyro_Y_h;
 104         uint8_t Gyro_Y_l;
 105         uint8_t Gyro_Z_h;
 106         uint8_t Gyro_Z_l;
 107 #ifdef PIOS_MPU9250_MAG
 108         uint8_t st1;
 109         uint8_t Mag_X_l;
 110         uint8_t Mag_X_h;
 111         uint8_t Mag_Y_l;
 112         uint8_t Mag_Y_h;
 113         uint8_t Mag_Z_l;
 114         uint8_t Mag_Z_h;
 115         uint8_t st2;
 116 #endif
 117     } data;
 118 } __attribute__((__packed__)) mpu9250_data_t;
 119
 120 #define GET_SENSOR_DATA(mpudataptr, sensor) (mpudataptr.data.sensor##_h << 8 | mpudataptr.data.sensor##_l)
 121
 122 static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0;
 123 static PIOS_SENSORS_3Axis_SensorsWithTemp *mag_data   = 0;
 124 static volatile bool mag_ready = false;
 125 #define SENSOR_COUNT         2
 126 #define SENSOR_DATA_SIZE     (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * SENSOR_COUNT)
 127 #define MAG_SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16))
 128 // ! Global structure for this device device
 129 static struct mpu9250_dev *dev;
 130 volatile bool mpu9250_configured = false;
 131 static mpu9250_data_t mpu9250_data;
 132
 133 // ! Private functions
 134 static struct mpu9250_dev *PIOS_MPU9250_alloc(const struct pios_mpu9250_cfg *cfg);
 135 static int32_t PIOS_MPU9250_Validate(struct mpu9250_dev *dev);
 136 static void PIOS_MPU9250_Config(struct pios_mpu9250_cfg const *cfg);
 137 static int32_t PIOS_MPU9250_SetReg(uint8_t address, uint8_t buffer);
 138 static int32_t PIOS_MPU9250_GetReg(uint8_t address);
 139 static void PIOS_MPU9250_SetSpeed(const bool fast);
 140 static bool PIOS_MPU9250_HandleData(uint32_t gyro_read_timestamp);
 141 static bool PIOS_MPU9250_ReadSensor(bool *woken);
 142 static int32_t PIOS_MPU9250_Test(void);
 143 #if defined(PIOS_MPU9250_MAG)
 144 static int32_t PIOS_MPU9250_Mag_Test(void);
 145 static int32_t PIOS_MPU9250_Mag_Init(void);
 146 #endif
 147
 148 /* Driver Framework interfaces */
 149 // Gyro/accel interface
 150 bool PIOS_MPU9250_Main_driver_Test(uintptr_t context);
 151 void PIOS_MPU9250_Main_driver_Reset(uintptr_t context);
 152 void PIOS_MPU9250_Main_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
 153 QueueHandle_t PIOS_MPU9250_Main_driver_get_queue(uintptr_t context);
 154
 155 const PIOS_SENSORS_Driver PIOS_MPU9250_Main_Driver = {
 156     .test      = PIOS_MPU9250_Main_driver_Test,
 157     .poll      = NULL,
 158     .fetch     = NULL,
 159     .reset     = PIOS_MPU9250_Main_driver_Reset,
 160     .get_queue = PIOS_MPU9250_Main_driver_get_queue,
 161     .get_scale = PIOS_MPU9250_Main_driver_get_scale,
 162     .is_polled = false,
 163 };
 164
 165 // mag sensor interface
 166 bool PIOS_MPU9250_Mag_driver_Test(uintptr_t context);
 167 void PIOS_MPU9250_Mag_driver_Reset(uintptr_t context);
 168 void PIOS_MPU9250_Mag_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
 169 void PIOS_MPU9250_Mag_driver_fetch(void *, uint8_t size, uintptr_t context);
 170 bool PIOS_MPU9250_Mag_driver_poll(uintptr_t context);
 171
 172 const PIOS_SENSORS_Driver PIOS_MPU9250_Mag_Driver = {
 173     .test      = PIOS_MPU9250_Mag_driver_Test,
 174     .poll      = PIOS_MPU9250_Mag_driver_poll,
 175     .fetch     = PIOS_MPU9250_Mag_driver_fetch,
 176     .reset     = PIOS_MPU9250_Mag_driver_Reset,
 177     .get_queue = NULL,
 178     .get_scale = PIOS_MPU9250_Mag_driver_get_scale,
 179     .is_polled = true,
 180 };
 181
 182 void PIOS_MPU9250_MainRegister()
 183 {
 184     PIOS_SENSORS_Register(&PIOS_MPU9250_Main_Driver, PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL, 0);
 185 }
 186
 187 void PIOS_MPU9250_MagRegister()
 188 {
 189     PIOS_SENSORS_Register(&PIOS_MPU9250_Mag_Driver, PIOS_SENSORS_TYPE_3AXIS_MAG, 0);
 190 }
 191 /**
 192  * @brief Allocate a new device
 193  */
 194 static struct mpu9250_dev *PIOS_MPU9250_alloc(const struct pios_mpu9250_cfg *cfg)
 195 {
 196     struct mpu9250_dev *mpu9250_dev;
 197
 198     mpu9250_dev = (struct mpu9250_dev *)pios_malloc(sizeof(*mpu9250_dev));
 199     PIOS_Assert(mpu9250_dev);
 200
 201     mpu9250_dev->magic = PIOS_MPU9250_DEV_MAGIC;
 202
 203     mpu9250_dev->queue = xQueueCreate(cfg->max_downsample + 1, SENSOR_DATA_SIZE);
 204     PIOS_Assert(mpu9250_dev->queue);
 205
 206     queue_data = (PIOS_SENSORS_3Axis_SensorsWithTemp *)pios_malloc(SENSOR_DATA_SIZE);
 207     PIOS_Assert(queue_data);
 208
 209     queue_data->count = SENSOR_COUNT;
 210
 211     mag_data = (PIOS_SENSORS_3Axis_SensorsWithTemp *)pios_malloc(MAG_SENSOR_DATA_SIZE);
 212     mag_data->count   = 1;
 213     PIOS_Assert(mag_data);
 214     return mpu9250_dev;
 215 }
 216
 217 /**
 218  * @brief Validate the handle to the spi device
 219  * @returns 0 for valid device or -1 otherwise
 220  */
 221 static int32_t PIOS_MPU9250_Validate(struct mpu9250_dev *vdev)
 222 {
 223     if (vdev == NULL) {
 224         return -1;
 225     }
 226     if (vdev->magic != PIOS_MPU9250_DEV_MAGIC) {
 227         return -2;
 228     }
 229     if (vdev->spi_id == 0) {
 230         return -3;
 231     }
 232     return 0;
 233 }
 234
 235 /**
 236  * @brief Initialize the MPU9250 3-axis gyro sensor.
 237  * @return 0 for success, -1 for failure
 238  */
 239 int32_t PIOS_MPU9250_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu9250_cfg *cfg)
 240 {
 241     dev = PIOS_MPU9250_alloc(cfg);
 242     if (dev == NULL) {
 243         return -1;
 244     }
 245
 246     dev->spi_id    = spi_id;
 247     dev->slave_num = slave_num;
 248     dev->cfg = cfg;
 249
 250     /* Configure the MPU9250 Sensor */
 251     PIOS_MPU9250_Config(cfg);
 252
 253     /* Set up EXTI line */
 254     PIOS_EXTI_Init(cfg->exti_cfg);
 255     return 0;
 256 }
 257
 258 /**
 259  * @brief Initialize the MPU9250 3-axis gyro sensor
 260  * \return none
 261  * \param[in] PIOS_MPU9250_ConfigTypeDef struct to be used to configure sensor.
 262  *
 263  */
 264 static void PIOS_MPU9250_Config(struct pios_mpu9250_cfg const *cfg)
 265 {
 266     uint8_t power;
 267
 268     while (PIOS_MPU9250_Test() != 0) {
 269         ;
 270     }
 271
 272     // Reset chip
 273     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_PWR_MGMT_REG, PIOS_MPU9250_PWRMGMT_IMU_RST) != 0) {
 274         ;
 275     }
 276
 277     PIOS_DELAY_WaitmS(100);
 278
 279     // Wake up the chip
 280     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_PWR_MGMT_REG, 0) != 0) {
 281         ;
 282     }
 283     // Reset sensors and fifo
 284     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_USER_CTRL_REG,
 285                                PIOS_MPU9250_USERCTL_DIS_I2C |
 286                                PIOS_MPU9250_USERCTL_SIG_COND) != 0) {
 287         ;
 288     }
 289     PIOS_DELAY_WaitmS(100);
 290
 291     // Power management configuration
 292     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) {
 293         ;
 294     }
 295
 296     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_USER_CTRL_REG, cfg->User_ctl) != 0) {
 297         ;
 298     }
 299
 300     // FIFO storage by default, do not include accelerometer and external sense data.
 301     power = PIOS_MPU9250_PWRMGMT2_DISABLE_ACCEL;
 302
 303 #if defined(PIOS_MPU9250_ACCEL)
 304
 305     power &= ~PIOS_MPU9250_PWRMGMT2_DISABLE_ACCEL;
 306 #endif
 307
 308     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_FIFO_EN_REG, cfg->Fifo_store) != 0) {
 309         ;
 310     }
 311     PIOS_MPU9250_SetReg(PIOS_MPU9250_PWR_MGMT2_REG, power);
 312
 313 #if defined(PIOS_MPU9250_ACCEL)
 314     PIOS_MPU9250_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter);
 315 #endif
 316
 317     // Interrupt configuration
 318     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_INT_CFG_REG, cfg->interrupt_cfg) != 0) {
 319         ;
 320     }
 321
 322 #ifdef PIOS_MPU9250_MAG
 323     PIOS_MPU9250_Mag_Init();
 324 #endif
 325
 326     // Interrupt enable
 327     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_INT_EN_REG, cfg->interrupt_en) != 0) {
 328         ;
 329     }
 330     if ((PIOS_MPU9250_GetReg(PIOS_MPU9250_INT_EN_REG)) != cfg->interrupt_en) {
 331         return;
 332     }
 333
 334     PIOS_MPU9250_GetReg(PIOS_MPU9250_INT_STATUS_REG);
 335
 336     mpu9250_configured = true;
 337 }
 338 /**
 339  * @brief Configures Gyro, accel and Filter ranges/setings
 340  * @return 0 if successful, -1 if device has not been initialized
 341  */
 342 int32_t PIOS_MPU9250_ConfigureRanges(
 343     enum pios_mpu9250_range gyroRange,
 344     enum pios_mpu9250_accel_range accelRange,
 345     enum pios_mpu9250_filter filterSetting)
 346 {
 347     if (dev == NULL) {
 348         return -1;
 349     }
 350
 351     // update filter settings
 352     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_DLPF_CFG_REG, filterSetting) != 0) {
 353         ;
 354     }
 355
 356     // Sample rate divider, chosen upon digital filtering settings
 357     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_SMPLRT_DIV_REG,
 358                                filterSetting == PIOS_MPU9250_LOWPASS_256_HZ ?
 359                                dev->cfg->Smpl_rate_div_no_dlp : dev->cfg->Smpl_rate_div_dlp) != 0) {
 360         ;
 361     }
 362
 363     dev->filter = filterSetting;
 364
 365     // Gyro range
 366     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_GYRO_CFG_REG, gyroRange) != 0) {
 367         ;
 368     }
 369
 370     dev->gyro_range = gyroRange;
 371 #if defined(PIOS_MPU9250_ACCEL)
 372     // Set the accel range
 373     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_ACCEL_CFG_REG, accelRange) != 0) {
 374         ;
 375     }
 376
 377     dev->accel_range = accelRange;
 378 #endif
 379     return 0;
 380 }
 381
 382 /**
 383  * @brief Claim the SPI bus for the accel communications and select this chip
 384  * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
 385  */
 386 static int32_t PIOS_MPU9250_ClaimBus(bool fast_spi)
 387 {
 388     if (PIOS_MPU9250_Validate(dev) != 0) {
 389         return -1;
 390     }
 391     if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
 392         return -2;
 393     }
 394     PIOS_MPU9250_SetSpeed(fast_spi);
 395     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
 396     return 0;
 397 }
 398
 399
 400 static void PIOS_MPU9250_SetSpeed(const bool fast)
 401 {
 402     if (fast) {
 403         PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->fast_prescaler);
 404     } else {
 405         PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->std_prescaler);
 406     }
 407 }
 408
 409 /**
 410  * @brief Claim the SPI bus for the accel communications and select this chip
 411  * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
 412  * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
 413  *                      task has is now eligible to run, else unchanged
 414  */
 415 static int32_t PIOS_MPU9250_ClaimBusISR(bool *woken, bool fast_spi)
 416 {
 417     if (PIOS_MPU9250_Validate(dev) != 0) {
 418         return -1;
 419     }
 420     if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
 421         return -2;
 422     }
 423     PIOS_MPU9250_SetSpeed(fast_spi);
 424     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
 425     return 0;
 426 }
 427
 428 /**
 429  * @brief Release the SPI bus for the accel communications and end the transaction
 430  * @return 0 if successful
 431  */
 432 static int32_t PIOS_MPU9250_ReleaseBus()
 433 {
 434     if (PIOS_MPU9250_Validate(dev) != 0) {
 435         return -1;
 436     }
 437     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
 438     return PIOS_SPI_ReleaseBus(dev->spi_id);
 439 }
 440
 441 /**
 442  * @brief Release the SPI bus for the accel communications and end the transaction
 443  * @return 0 if successful
 444  * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
 445  *                      task has is now eligible to run, else unchanged
 446  */
 447 static int32_t PIOS_MPU9250_ReleaseBusISR(bool *woken)
 448 {
 449     if (PIOS_MPU9250_Validate(dev) != 0) {
 450         return -1;
 451     }
 452     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
 453     return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
 454 }
 455
 456 /**
 457  * @brief Read a register from MPU9250
 458  * @returns The register value or -1 if failure to get bus
 459  * @param reg[in] Register address to be read
 460  */
 461 static int32_t PIOS_MPU9250_GetReg(uint8_t reg)
 462 {
 463     uint8_t data;
 464
 465     if (PIOS_MPU9250_ClaimBus(false) != 0) {
 466         return -1;
 467     }
 468
 469     PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte
 470     data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response
 471
 472     PIOS_MPU9250_ReleaseBus();
 473     return data;
 474 }
 475
 476 /**
 477  * @brief Writes one byte to the MPU9250
 478  * \param[in] reg Register address
 479  * \param[in] data Byte to write
 480  * \return 0 if operation was successful
 481  * \return -1 if unable to claim SPI bus
 482  * \return -2 if unable to send the command
 483  * \return -3 if unable to receive the response
 484  */
 485 static int32_t PIOS_MPU9250_SetReg(uint8_t reg, uint8_t data)
 486 {
 487     int ret = 0;
 488
 489     if (PIOS_MPU9250_ClaimBus(false) != 0) {
 490         return -1;
 491     }
 492
 493     PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg);
 494     // if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) {
 495     // PIOS_MPU9250_ReleaseBus();
 496     // return -2;
 497     // }
 498
 499     PIOS_SPI_TransferByte(dev->spi_id, data);
 500     // if (PIOS_SPI_TransferByte(dev->spi_id, data) != 0) {
 501     // PIOS_MPU9250_ReleaseBus();
 502     // return -3;
 503     // }
 504
 505     PIOS_MPU9250_ReleaseBus();
 506
 507     return ret;
 508 }
 509
 510
 511 /*
 512  * @brief Read the identification bytes from the MPU9250 sensor
 513  * \return ID read from MPU9250 or -1 if failure
 514  */
 515 int32_t PIOS_MPU9250_ReadID()
 516 {
 517     int32_t mpu9250_id = PIOS_MPU9250_GetReg(PIOS_MPU9250_WHOAMI);
 518
 519     if (mpu9250_id < 0) {
 520         return -1;
 521     }
 522     return mpu9250_id;
 523 }
 524
 525 static float PIOS_MPU9250_GetScale()
 526 {
 527     switch (dev->gyro_range) {
 528     case PIOS_MPU9250_SCALE_250_DEG:
 529         return 1.0f / 131.0f;
 530
 531     case PIOS_MPU9250_SCALE_500_DEG:
 532         return 1.0f / 65.5f;
 533
 534     case PIOS_MPU9250_SCALE_1000_DEG:
 535         return 1.0f / 32.8f;
 536
 537     case PIOS_MPU9250_SCALE_2000_DEG:
 538         return 1.0f / 16.4f;
 539     }
 540     return 0;
 541 }
 542
 543 static float PIOS_MPU9250_GetAccelScale()
 544 {
 545     switch (dev->accel_range) {
 546     case PIOS_MPU9250_ACCEL_2G:
 547         return PIOS_CONST_MKS_GRAV_ACCEL_F / 16384.0f;
 548
 549     case PIOS_MPU9250_ACCEL_4G:
 550         return PIOS_CONST_MKS_GRAV_ACCEL_F / 8192.0f;
 551
 552     case PIOS_MPU9250_ACCEL_8G:
 553         return PIOS_CONST_MKS_GRAV_ACCEL_F / 4096.0f;
 554
 555     case PIOS_MPU9250_ACCEL_16G:
 556         return PIOS_CONST_MKS_GRAV_ACCEL_F / 2048.0f;
 557     }
 558     return 0;
 559 }
 560
 561 /**
 562  * @brief Run self-test operation.
 563  * \return 0 if test succeeded
 564  * \return non-zero value if test failed
 565  */
 566 static int32_t PIOS_MPU9250_Test(void)
 567 {
 568     /* Verify that ID matches */
 569     int32_t mpu9250_id = PIOS_MPU9250_ReadID();
 570
 571     if (mpu9250_id < 0) {
 572         return -1;
 573     }
 574
 575     if (mpu9250_id != PIOS_MPU9250_GYRO_ACC_ID) {
 576         return -2;
 577     }
 578
 579     return 0;
 580 }
 581
 582 #if defined(PIOS_MPU9250_MAG)
 583 /**
 584  * @brief Read a mag register from MPU9250
 585  * @returns The register value or -1 if failure to get bus
 586  * @param reg[in] Register address to be read
 587  */
 588 static int32_t PIOS_MPU9250_Mag_GetReg(uint8_t reg)
 589 {
 590     int32_t data;
 591
 592     // Set the I2C slave address and read command.
 593     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV4_ADDR, PIOS_MPU9250_MAG_I2C_ADDR |
 594                                PIOS_MPU9250_MAG_I2C_READ_FLAG) != PIOS_MPU9250_MAG_OK) {
 595         ;
 596     }
 597
 598     // Set the address of the register to read.
 599     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV4_REG, reg) != PIOS_MPU9250_MAG_OK) {
 600         ;
 601     }
 602
 603     // Trigger the byte transfer.
 604     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV4_CTRL, PIOS_MPU9250_I2C_SLV_ENABLE) != PIOS_MPU9250_MAG_OK) {
 605         ;
 606     }
 607
 608     PIOS_DELAY_WaitmS(1);
 609
 610     // Read result.
 611     data = PIOS_MPU9250_GetReg(PIOS_MPU9250_I2C_SLV4_DI);
 612     PIOS_DELAY_WaitmS(1);
 613     return data;
 614 }
 615
 616 /**
 617  * @brief Writes one byte to the MPU9250
 618  * \param[in] reg Register address
 619  * \param[in] data Byte to write
 620  */
 621 static int32_t PIOS_MPU9250_Mag_SetReg(uint8_t reg, uint8_t data)
 622 {
 623     // Set the I2C slave address.
 624     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV4_ADDR, PIOS_MPU9250_MAG_I2C_ADDR) != PIOS_MPU9250_MAG_OK) {
 625         ;
 626     }
 627
 628     // Set the address of the register to write.
 629     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV4_REG, reg) != PIOS_MPU9250_MAG_OK) {
 630         ;
 631     }
 632
 633     // Set the byte to write.
 634     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV4_DO, data) != PIOS_MPU9250_MAG_OK) {
 635         ;
 636     }
 637
 638     // Trigger the byte transfer.
 639     while (PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV4_CTRL, PIOS_MPU9250_I2C_SLV_ENABLE) != PIOS_MPU9250_MAG_OK) {
 640         ;
 641     }
 642     PIOS_DELAY_WaitmS(1);
 643     return PIOS_MPU9250_MAG_OK;
 644 }
 645
 646 /**
 647  * @rief Get ASAx registers from fuse ROM
 648  *  Hadj = H*((ASA-128)*0.5/128+1)
 649  * \return 0 if test succeeded
 650  * \return non-zero value if test failed
 651  */
 652 static int32_t PIOS_MPU9250_Mag_Sensitivity(void)
 653 {
 654     int i;
 655
 656     /* Put mag in power down state before changing mode */
 657     PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL1, PIOS_MPU9250_MAG_POWER_DOWN_MODE);
 658     PIOS_DELAY_WaitmS(1);
 659
 660     /* Enable fuse ROM for access */
 661     PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL1, PIOS_MPU9250_MAG_FUSE_ROM_MODE);
 662     PIOS_DELAY_WaitmS(1);
 663
 664     if (PIOS_MPU9250_ClaimBus(false) != 0) {
 665         return -1;
 666     }
 667
 668     /* Set addres and read flag */
 669     PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV0_ADDR);
 670     PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_MAG_I2C_ADDR | PIOS_MPU9250_MAG_I2C_READ_FLAG);
 671
 672     /* Set the address of the register to read. */
 673     PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV0_REG);
 674     PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_ASAX);
 675
 676     /* Trigger the byte transfer. */
 677     PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV0_CTRL);
 678     PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV_ENABLE | 0x3);
 679
 680     PIOS_DELAY_WaitmS(1);
 681
 682     /* Read the mag data from SPI block */
 683     for (i = 0; i < 0x3; i++) {
 684         PIOS_SPI_TransferByte(dev->spi_id, (PIOS_MPU9250_EXT_SENS_DATA_00 | 0x80) + i);
 685         int32_t ret = PIOS_SPI_TransferByte(dev->spi_id, 0x0);
 686         if (ret < 0) {
 687             PIOS_MPU9250_ReleaseBus();
 688             return -1;
 689         }
 690         dev->mag_sens_adj[i] = 1.0f; // 1.0f + ((float)((uint8_t)ret - 128)) / 256.0f;
 691     }
 692
 693     PIOS_MPU9250_ReleaseBus();
 694
 695
 696     /* Put mag in power down state before changing mode */
 697     PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL1, PIOS_MPU9250_MAG_POWER_DOWN_MODE);
 698
 699     return PIOS_MPU9250_MAG_OK;
 700 }
 701
 702 /**
 703  * @brief Read a mag register from MPU9250
 704  * @returns The register value or -1 if failure to get bus
 705  * @param reg[in] Register address to be read
 706  */
 707 static int32_t PIOS_MPU9250_Mag_Init(void)
 708 {
 709     // I2C multi-master init.
 710     PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_MST_CTRL, PIOS_MPU9250_I2C_MST_P_NSR | PIOS_MPU9250_I2C_MST_CLOCK_400);
 711     PIOS_DELAY_WaitmS(1);
 712
 713     // Reset Mag.
 714     PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL2, PIOS_MPU9250_MAG_RESET);
 715     PIOS_DELAY_WaitmS(1);
 716
 717
 718     // read fuse ROM to get the sensitivity adjustment values.
 719     if (PIOS_MPU9250_Mag_Sensitivity() != PIOS_MPU9250_MAG_OK) {
 720         ;
 721     }
 722
 723     // Confirm Mag ID.
 724     while (false && (PIOS_MPU9250_Mag_Test() != PIOS_MPU9250_MAG_OK)) {
 725         ;
 726     }
 727
 728     // Make sure no other registers will be triggered before entering continuous mode.
 729     PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV4_CTRL, 0x0);
 730     PIOS_DELAY_WaitmS(1);
 731     PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV0_DO, 0x0);
 732     PIOS_DELAY_WaitmS(1);
 733
 734     // Making sure register are accessible.
 735     PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL1, PIOS_MPU9250_MAG_OUTPUT_16BITS | PIOS_MPU9250_MAG_CONTINUOUS_MODE2);
 736     PIOS_DELAY_WaitmS(1);
 737
 738     // Get ST1, the 6 mag data and ST2.
 739     // This is to save 2 SPI access.
 740     // Set the I2C slave address and read command.
 741     PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV0_ADDR, PIOS_MPU9250_MAG_I2C_ADDR | PIOS_MPU9250_MAG_I2C_READ_FLAG);
 742
 743     // Set the address of the register to read.
 744     PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV0_REG, PIOS_MPU9250_ST1);
 745
 746     // Trigger the byte transfer.
 747     PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV0_CTRL, PIOS_MPU9250_I2C_SLV_ENABLE | 0x8);
 748     PIOS_DELAY_WaitmS(1);
 749
 750     return PIOS_MPU9250_MAG_OK;
 751 }
 752
 753 /*
 754  * @brief Read the mag identification bytes from the MPU9250 sensor
 755  */
 756 int32_t PIOS_MPU9250_Mag_ReadID()
 757 {
 758     int32_t mpu9250_mag_id = PIOS_MPU9250_Mag_GetReg(PIOS_MPU9250_WIA);
 759
 760     if (mpu9250_mag_id < PIOS_MPU9250_MAG_OK) {
 761         return PIOS_MPU9250_ERR_MAG_READ_ID;
 762     }
 763     return mpu9250_mag_id;
 764 }
 765
 766 /**
 767  * @brief Run self-test operation.
 768  * \return 0 if test succeeded
 769  * \return non-zero value if test failed
 770  */
 771 static int32_t PIOS_MPU9250_Mag_Test(void)
 772 {
 773     /* Verify that ID matches */
 774     int32_t mpu9250_mag_id = PIOS_MPU9250_Mag_ReadID();
 775
 776     if (mpu9250_mag_id < PIOS_MPU9250_MAG_OK) {
 777         return PIOS_MPU9250_ERR_MAG_READ_ID;
 778     }
 779
 780     if (mpu9250_mag_id != PIOS_MPU9250_MAG_ID) {
 781         return PIOS_MPU9250_ERR_MAG_BAD_ID;
 782     }
 783
 784     /* TODO: run self-test */
 785
 786     return PIOS_MPU9250_MAG_OK;
 787 }
 788
 789
 790 /**
 791  * @brief Read the mag data.
 792  * \return true if data has been read from mpu
 793  * \return false on error
 794  */
 795 static bool PIOS_MPU9250_ReadMag(bool *woken)
 796 {
 797     if (PIOS_MPU9250_ClaimBusISR(woken, true) != 0) {
 798         return false;
 799     }
 800     // Trigger the byte transfer.
 801     PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV0_CTRL);
 802     PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV_ENABLE | 0x8);
 803
 804     PIOS_MPU9250_ReleaseBusISR(woken);
 805
 806     return true;
 807 }
 808 #endif /* if defined(PIOS_MPU9250_MAG) */
 809
 810 /**
 811  * @brief EXTI IRQ Handler.  Read all the data from onboard buffer
 812  * @return a boolean to the EXTI IRQ Handler wrapper indicating if a
 813  *         higher priority task is now eligible to run
 814  */
 815 bool PIOS_MPU9250_IRQHandler(void)
 816 {
 817     uint32_t gyro_read_timestamp = PIOS_DELAY_GetRaw();
 818     bool woken = false;
 819
 820     if (!mpu9250_configured) {
 821         return false;
 822     }
 823
 824 #if defined(PIOS_MPU9250_MAG)
 825     PIOS_MPU9250_ReadMag(&woken);
 826 #endif
 827
 828     if (PIOS_MPU9250_ReadSensor(&woken)) {
 829         woken |= PIOS_MPU9250_HandleData(gyro_read_timestamp);
 830     }
 831
 832     return woken;
 833 }
 834
 835 static bool PIOS_MPU9250_HandleData(uint32_t gyro_read_timestamp)
 836 {
 837     // Rotate the sensor to OP convention.  The datasheet defines X as towards the right
 838     // and Y as forward.  OP convention transposes this.  Also the Z is defined negatively
 839     // to our convention
 840     if (!queue_data) {
 841         return false;
 842     }
 843
 844 #ifdef PIOS_MPU9250_MAG
 845     bool mag_valid = mpu9250_data.data.st1 & PIOS_MPU9250_MAG_DATA_RDY;
 846 #endif
 847
 848     // Currently we only support rotations on top so switch X/Y accordingly
 849     switch (dev->cfg->orientation) {
 850     case PIOS_MPU9250_TOP_0DEG:
 851 #ifdef PIOS_MPU9250_ACCEL
 852         queue_data->sample[0].y = GET_SENSOR_DATA(mpu9250_data, Accel_X); // chip X
 853         queue_data->sample[0].x = GET_SENSOR_DATA(mpu9250_data, Accel_Y); // chip Y
 854 #endif
 855         queue_data->sample[1].y = GET_SENSOR_DATA(mpu9250_data, Gyro_X); // chip X
 856         queue_data->sample[1].x = GET_SENSOR_DATA(mpu9250_data, Gyro_Y); // chip Y
 857 #ifdef PIOS_MPU9250_MAG
 858         if (mag_valid) {
 859             mag_data->sample[0].y = GET_SENSOR_DATA(mpu9250_data, Mag_Y) * dev->mag_sens_adj[1]; // chip Y
 860             mag_data->sample[0].x = GET_SENSOR_DATA(mpu9250_data, Mag_X) * dev->mag_sens_adj[0]; // chip X
 861         }
 862 #endif
 863         break;
 864     case PIOS_MPU9250_TOP_90DEG:
 865         // -1 to bring it back to -32768 +32767 range
 866 #ifdef PIOS_MPU9250_ACCEL
 867         queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu9250_data, Accel_Y)); // chip Y
 868         queue_data->sample[0].x = GET_SENSOR_DATA(mpu9250_data, Accel_X); // chip X
 869 #endif
 870         queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu9250_data, Gyro_Y)); // chip Y
 871         queue_data->sample[1].x = GET_SENSOR_DATA(mpu9250_data, Gyro_X); // chip X
 872 #ifdef PIOS_MPU9250_MAG
 873         if (mag_valid) {
 874             mag_data->sample[0].y = GET_SENSOR_DATA(mpu9250_data, Mag_X) * dev->mag_sens_adj[0]; // chip X
 875             mag_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu9250_data, Mag_Y)) * dev->mag_sens_adj[1]; // chip Y
 876         }
 877
 878 #endif
 879         break;
 880     case PIOS_MPU9250_TOP_180DEG:
 881 #ifdef PIOS_MPU9250_ACCEL
 882         queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu9250_data, Accel_X)); // chip X
 883         queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu9250_data, Accel_Y)); // chip Y
 884 #endif
 885         queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu9250_data, Gyro_X)); // chip X
 886         queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu9250_data, Gyro_Y)); // chip Y
 887 #ifdef PIOS_MPU9250_MAG
 888         if (mag_valid) {
 889             mag_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu9250_data, Mag_Y)) * dev->mag_sens_adj[1]; // chip Y
 890             mag_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu9250_data, Mag_X)) * dev->mag_sens_adj[0]; // chip X
 891         }
 892 #endif
 893         break;
 894     case PIOS_MPU9250_TOP_270DEG:
 895 #ifdef PIOS_MPU9250_ACCEL
 896         queue_data->sample[0].y = GET_SENSOR_DATA(mpu9250_data, Accel_Y); // chip Y
 897         queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu9250_data, Accel_X)); // chip X
 898 #endif
 899         queue_data->sample[1].y = GET_SENSOR_DATA(mpu9250_data, Gyro_Y); // chip Y
 900         queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu9250_data, Gyro_X)); // chip X
 901 #ifdef PIOS_MPU9250_MAG
 902         if (mag_valid) {
 903             mag_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu9250_data, Mag_X)) * dev->mag_sens_adj[0]; // chip X
 904             mag_data->sample[0].x = GET_SENSOR_DATA(mpu9250_data, Mag_Y) * dev->mag_sens_adj[1]; // chip Y
 905         }
 906 #endif
 907         break;
 908     }
 909 #ifdef PIOS_MPU9250_ACCEL
 910     queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu9250_data, Accel_Z));
 911 #endif
 912     queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu9250_data, Gyro_Z));
 913     const int16_t temp = GET_SENSOR_DATA(mpu9250_data, Temperature);
 914     queue_data->temperature = 2100 + ((float)(temp - PIOS_MPU9250_TEMP_OFFSET)) * (100.0f / PIOS_MPU9250_TEMP_SENSITIVITY);
 915     queue_data->timestamp   = gyro_read_timestamp;
 916     mag_data->temperature   = queue_data->temperature;
 917 #ifdef PIOS_MPU9250_MAG
 918     if (mag_valid) {
 919         mag_data->sample[0].z = GET_SENSOR_DATA(mpu9250_data, Mag_Z) * dev->mag_sens_adj[2]; // chip Z
 920         mag_ready = true;
 921     }
 922 #endif
 923
 924     BaseType_t higherPriorityTaskWoken;
 925     xQueueSendToBackFromISR(dev->queue, queue_data, &higherPriorityTaskWoken);
 926     return higherPriorityTaskWoken == pdTRUE;
 927 }
 928
 929 static bool PIOS_MPU9250_ReadSensor(bool *woken)
 930 {
 931     const uint8_t mpu9250_send_buf[1 + PIOS_MPU9250_SAMPLES_BYTES] = { PIOS_MPU9250_SENSOR_FIRST_REG | 0x80 };
 932
 933     if (PIOS_MPU9250_ClaimBusISR(woken, true) != 0) {
 934         return false;
 935     }
 936     if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu9250_send_buf[0], &mpu9250_data.buffer[0], sizeof(mpu9250_data_t), NULL) < 0) {
 937         PIOS_MPU9250_ReleaseBusISR(woken);
 938         return false;
 939     }
 940     PIOS_MPU9250_ReleaseBusISR(woken);
 941     return true;
 942 }
 943
 944 // Sensor driver implementation
 945 bool PIOS_MPU9250_Main_driver_Test(__attribute__((unused)) uintptr_t context)
 946 {
 947     return !PIOS_MPU9250_Test();
 948 }
 949
 950 void PIOS_MPU9250_Main_driver_Reset(__attribute__((unused)) uintptr_t context)
 951 {
 952     PIOS_MPU9250_GetReg(PIOS_MPU9250_INT_STATUS_REG);
 953 }
 954
 955 void PIOS_MPU9250_Main_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t contet)
 956 {
 957     PIOS_Assert(size >= 2);
 958     scales[0] = PIOS_MPU9250_GetAccelScale();
 959     scales[1] = PIOS_MPU9250_GetScale();
 960 }
 961
 962 QueueHandle_t PIOS_MPU9250_Main_driver_get_queue(__attribute__((unused)) uintptr_t context)
 963 {
 964     return dev->queue;
 965 }
 966
 967
 968 /* PIOS sensor driver implementation */
 969 bool PIOS_MPU9250_Mag_driver_Test(__attribute__((unused)) uintptr_t context)
 970 {
 971     return !PIOS_MPU9250_Test();
 972 }
 973
 974 void PIOS_MPU9250_Mag_driver_Reset(__attribute__((unused)) uintptr_t context) {}
 975
 976 void PIOS_MPU9250_Mag_driver_get_scale(float *scales, uint8_t size, __attribute__((unused))  uintptr_t context)
 977 {
 978     PIOS_Assert(size > 0);
 979     scales[0] = 1;
 980 }
 981
 982 void PIOS_MPU9250_Mag_driver_fetch(void *data, uint8_t size, __attribute__((unused))  uintptr_t context)
 983 {
 984     mag_ready = false;
 985     PIOS_Assert(size > 0);
 986     memcpy(data, mag_data, MAG_SENSOR_DATA_SIZE);
 987 }
 988
 989 bool PIOS_MPU9250_Mag_driver_poll(__attribute__((unused)) uintptr_t context)
 990 {
 991     return mag_ready;
 992 }
 993
 994 #endif /* PIOS_INCLUDE_MPU9250 */
 995
 996 /**
 997  * @}
 998  * @}
 999  */