flight/pios/common/pios_hmc5x83.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup PIOS_HMC5x83 HMC5x83 Functions
   6  * @brief Deals with the hardware interface to the magnetometers
   7  * @{
   8  * @file       pios_hmc5x83.c
   9  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  10  * @brief      HMC5x83 Magnetic Sensor Functions from AHRS
  11  * @see        The GNU Public License (GPL) Version 3
  12  *
  13  ******************************************************************************
  14  */
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include "pios.h"
  32 #include <pios_hmc5x83.h>
  33 #include <pios_mem.h>
  34
  35 #ifdef PIOS_INCLUDE_HMC5X83
  36
  37 #define PIOS_HMC5X83_MAGIC 0x4d783833
  38 /* Global Variables */
  39
  40 /* Local Types */
  41
  42 typedef struct {
  43     uint32_t magic;
  44     const struct pios_hmc5x83_cfg *cfg;
  45     uint32_t port_id;
  46     uint8_t  slave_num;
  47     uint8_t  CTRLB;
  48     volatile bool data_ready;
  49 } pios_hmc5x83_dev_data_t;
  50
  51 static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev);
  52
  53 // sensor driver interface
  54 bool PIOS_HMC5x83_driver_Test(uintptr_t context);
  55 void PIOS_HMC5x83_driver_Reset(uintptr_t context);
  56 void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
  57 void PIOS_HMC5x83_driver_fetch(void *, uint8_t size, uintptr_t context);
  58 bool PIOS_HMC5x83_driver_poll(uintptr_t context);
  59
  60 const PIOS_SENSORS_Driver PIOS_HMC5x83_Driver = {
  61     .test      = PIOS_HMC5x83_driver_Test,
  62     .poll      = PIOS_HMC5x83_driver_poll,
  63     .fetch     = PIOS_HMC5x83_driver_fetch,
  64     .reset     = PIOS_HMC5x83_driver_Reset,
  65     .get_queue = NULL,
  66     .get_scale = PIOS_HMC5x83_driver_get_scale,
  67     .is_polled = true,
  68 };
  69 /**
  70  * Allocate the device setting structure
  71  * @return pios_hmc5x83_dev_data_t pointer to newly created structure
  72  */
  73 pios_hmc5x83_dev_data_t *dev_alloc()
  74 {
  75     pios_hmc5x83_dev_data_t *dev = (pios_hmc5x83_dev_data_t *)pios_malloc(sizeof(pios_hmc5x83_dev_data_t));
  76
  77     PIOS_DEBUG_Assert(dev);
  78     memset(dev, 0x00, sizeof(pios_hmc5x83_dev_data_t));
  79     dev->magic = PIOS_HMC5X83_MAGIC;
  80     return dev;
  81 }
  82
  83 /**
  84  * Validate a pios_hmc5x83_dev_t handler and return the related pios_hmc5x83_dev_data_t pointer
  85  * @param dev device handler
  86  * @return the device data structure
  87  */
  88 pios_hmc5x83_dev_data_t *dev_validate(pios_hmc5x83_dev_t dev)
  89 {
  90     pios_hmc5x83_dev_data_t *dev_data = (pios_hmc5x83_dev_data_t *)dev;
  91
  92     PIOS_DEBUG_Assert(dev_data->magic == PIOS_HMC5X83_MAGIC);
  93     return dev_data;
  94 }
  95
  96 /**
  97  * @brief Initialize the HMC5x83 magnetometer sensor.
  98  * @return none
  99  */
 100 pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t slave_num)
 101 {
 102     pios_hmc5x83_dev_data_t *dev = dev_alloc();
 103
 104     dev->cfg       = cfg; // store config before enabling interrupt
 105     dev->port_id   = port_id;
 106     dev->slave_num = slave_num;
 107 #ifdef PIOS_HMC5X83_HAS_GPIOS
 108     PIOS_EXTI_Init(cfg->exti_cfg);
 109 #endif
 110
 111     int32_t val = PIOS_HMC5x83_Config(dev);
 112     PIOS_Assert(val == 0);
 113
 114     dev->data_ready = false;
 115     return (pios_hmc5x83_dev_t)dev;
 116 }
 117
 118 void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler)
 119 {
 120     PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, PIOS_SENSORS_TYPE_3AXIS_MAG, handler);
 121 }
 122
 123 /**
 124  * @brief Initialize the HMC5x83 magnetometer sensor
 125  * \return none
 126  * \param[in] pios_hmc5x83_dev_data_t device config to be used.
 127  * \param[in] PIOS_HMC5x83_ConfigTypeDef struct to be used to configure sensor.
 128  *
 129  * CTRL_REGA: Control Register A
 130  * Read Write
 131  * Default value: 0x10
 132  * 7:5  0   These bits must be cleared for correct operation.
 133  * 4:2 DO2-DO0: Data Output Rate Bits
 134  *             DO2 |  DO1 |  DO0 |   Minimum Data Output Rate (Hz)
 135  *            ------------------------------------------------------
 136  *              0  |  0   |  0   |            0.75
 137  *              0  |  0   |  1   |            1.5
 138  *              0  |  1   |  0   |            3
 139  *              0  |  1   |  1   |            7.5
 140  *              1  |  0   |  0   |           15 (default)
 141  *              1  |  0   |  1   |           30
 142  *              1  |  1   |  0   |           75
 143  *              1  |  1   |  1   |           Not Used
 144  * 1:0 MS1-MS0: Measurement Configuration Bits
 145  *             MS1 | MS0 |   MODE
 146  *            ------------------------------
 147  *              0  |  0   |  Normal
 148  *              0  |  1   |  Positive Bias
 149  *              1  |  0   |  Negative Bias
 150  *              1  |  1   |  Not Used
 151  *
 152  * CTRL_REGB: Control RegisterB
 153  * Read Write
 154  * Default value: 0x20
 155  * 7:5 GN2-GN0: Gain Configuration Bits.
 156  *             GN2 |  GN1 |  GN0 |   Mag Input   | Gain       | Output Range
 157  *                 |      |      |  Range[Ga]    | [LSB/mGa]  |
 158  *            ------------------------------------------------------
 159  *              0  |  0   |  0   |  ±0.88Ga      |   1370     | 0xF8000x07FF (-2048:2047)
 160  *              0  |  0   |  1   |  ±1.3Ga (def) |   1090     | 0xF8000x07FF (-2048:2047)
 161  *              0  |  1   |  0   |  ±1.9Ga       |   820      | 0xF8000x07FF (-2048:2047)
 162  *              0  |  1   |  1   |  ±2.5Ga       |   660      | 0xF8000x07FF (-2048:2047)
 163  *              1  |  0   |  0   |  ±4.0Ga       |   440      | 0xF8000x07FF (-2048:2047)
 164  *              1  |  0   |  1   |  ±4.7Ga       |   390      | 0xF8000x07FF (-2048:2047)
 165  *              1  |  1   |  0   |  ±5.6Ga       |   330      | 0xF8000x07FF (-2048:2047)
 166  *              1  |  1   |  1   |  ±8.1Ga       |   230      | 0xF8000x07FF (-2048:2047)
 167  *                               |Not recommended|
 168  *
 169  * 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
 170  *
 171  * _MODE_REG: Mode Register
 172  * Read Write
 173  * Default value: 0x02
 174  * 7:2  0   These bits must be cleared for correct operation.
 175  * 1:0 MD1-MD0: Mode Select Bits
 176  *             MS1 | MS0 |   MODE
 177  *            ------------------------------
 178  *              0  |  0   |  Continuous-Conversion Mode.
 179  *              0  |  1   |  Single-Conversion Mode
 180  *              1  |  0   |  Negative Bias
 181  *              1  |  1   |  Sleep Mode
 182  */
 183 static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev)
 184 {
 185     uint8_t CTRLA = 0x00;
 186     uint8_t MODE  = 0x00;
 187
 188     const struct pios_hmc5x83_cfg *cfg = dev->cfg;
 189
 190     dev->CTRLB = 0;
 191
 192     CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf);
 193     CTRLA |= cfg->TempCompensation ? PIOS_HMC5x83_CTRLA_TEMP : 0;
 194     dev->CTRLB |= (uint8_t)(cfg->Gain);
 195     MODE  |= (uint8_t)(cfg->Mode);
 196
 197     // CRTL_REGA
 198     if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_A, CTRLA) != 0) {
 199         return -1;
 200     }
 201
 202     // CRTL_REGB
 203     if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_B, dev->CTRLB) != 0) {
 204         return -1;
 205     }
 206
 207     // Mode register
 208     if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_MODE_REG, MODE) != 0) {
 209         return -1;
 210     }
 211
 212     return 0;
 213 }
 214
 215 /**
 216  * @brief Read current X, Z, Y values (in that order)
 217  * \param[in] dev device handler
 218  * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
 219  * \return 0 for success or -1 for failure
 220  */
 221 int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
 222 {
 223     pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
 224
 225     dev->data_ready = false;
 226     uint8_t buffer[6];
 227     int16_t temp[3];
 228     int32_t sensitivity;
 229
 230     if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) {
 231         return -1;
 232     }
 233
 234     switch (dev->CTRLB & 0xE0) {
 235     case 0x00:
 236         sensitivity = PIOS_HMC5x83_Sensitivity_0_88Ga;
 237         break;
 238     case 0x20:
 239         sensitivity = PIOS_HMC5x83_Sensitivity_1_3Ga;
 240         break;
 241     case 0x40:
 242         sensitivity = PIOS_HMC5x83_Sensitivity_1_9Ga;
 243         break;
 244     case 0x60:
 245         sensitivity = PIOS_HMC5x83_Sensitivity_2_5Ga;
 246         break;
 247     case 0x80:
 248         sensitivity = PIOS_HMC5x83_Sensitivity_4_0Ga;
 249         break;
 250     case 0xA0:
 251         sensitivity = PIOS_HMC5x83_Sensitivity_4_7Ga;
 252         break;
 253     case 0xC0:
 254         sensitivity = PIOS_HMC5x83_Sensitivity_5_6Ga;
 255         break;
 256     case 0xE0:
 257         sensitivity = PIOS_HMC5x83_Sensitivity_8_1Ga;
 258         break;
 259     default:
 260         PIOS_Assert(0);
 261     }
 262     for (int i = 0; i < 3; i++) {
 263         int16_t v = ((int16_t)((uint16_t)buffer[2 * i] << 8)
 264                      + buffer[2 * i + 1]) * 1000 / sensitivity;
 265         temp[i] = v;
 266     }
 267
 268     switch (dev->cfg->Orientation) {
 269     case PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP:
 270         out[0] = temp[2];
 271         out[1] = temp[0];
 272         out[2] = -temp[1];
 273         break;
 274     case PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP:
 275         out[0] = -temp[0];
 276         out[1] = temp[2];
 277         out[2] = -temp[1];
 278         break;
 279     case PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP:
 280         out[0] = -temp[2];
 281         out[1] = -temp[0];
 282         out[2] = -temp[1];
 283         break;
 284     case PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP:
 285         out[0] = temp[0];
 286         out[1] = -temp[2];
 287         out[2] = -temp[1];
 288         break;
 289     case PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN:
 290         out[0] = temp[2];
 291         out[1] = -temp[0];
 292         out[2] = temp[1];
 293         break;
 294     case PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN:
 295         out[0] = -temp[0];
 296         out[1] = -temp[2];
 297         out[2] = temp[1];
 298         break;
 299     case PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN:
 300         out[0] = -temp[2];
 301         out[1] = temp[0];
 302         out[2] = temp[1];
 303         break;
 304     case PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN:
 305         out[0] = temp[0];
 306         out[1] = temp[2];
 307         out[2] = temp[1];
 308         break;
 309     }
 310
 311     // This should not be necessary but for some reason it is coming out of continuous conversion mode
 312     dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS);
 313
 314     return 0;
 315 }
 316
 317
 318 /**
 319  * @brief Read the identification bytes from the HMC5x83 sensor
 320  * \param[out] uint8_t array of size 4 to store HMC5x83 ID.
 321  * \return 0 if successful, -1 if not
 322  */
 323 uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4])
 324 {
 325     pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
 326     uint8_t retval = dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_IDA_REG, out, 3);
 327
 328     out[3] = '\0';
 329     return retval;
 330 }
 331
 332 /**
 333  * @brief Tells whether new magnetometer readings are available
 334  * \return true if new data is available
 335  * \return false if new data is not available
 336  */
 337 bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler)
 338 {
 339     pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
 340
 341     return dev->data_ready;
 342 }
 343
 344 /**
 345  * @brief Run self-test operation.  Do not call this during operational use!!
 346  * \return 0 if success, -1 if test failed
 347  */
 348 int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler)
 349 {
 350     int32_t failed = 0;
 351     uint8_t registers[3] = { 0, 0, 0 };
 352     uint8_t status;
 353     uint8_t ctrl_a_read;
 354     uint8_t ctrl_b_read;
 355     uint8_t mode_read;
 356     int16_t values[3];
 357     pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
 358
 359     /* Verify that ID matches (HMC5x83 ID is null-terminated ASCII string "H43") */
 360     char id[4];
 361
 362     PIOS_HMC5x83_ReadID(handler, (uint8_t *)id);
 363     if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43
 364         return -1;
 365     }
 366
 367     /* Backup existing configuration */
 368     if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, registers, 3) != 0) {
 369         return -1;
 370     }
 371
 372     /* Stop the device and read out last value */
 373     PIOS_DELAY_WaitmS(10);
 374     if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_IDLE) != 0) {
 375         return -1;
 376     }
 377     if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1) != 0) {
 378         return -1;
 379     }
 380     if (PIOS_HMC5x83_ReadMag(handler, values) != 0) {
 381         return -1;
 382     }
 383
 384     /*
 385      * Put HMC5x83 into self test mode
 386      * This is done by placing measurement config into positive (0x01) or negative (0x10) bias
 387      * and then placing the mode register into single-measurement mode.  This causes the HMC5x83
 388      * to create an artificial magnetic field of ~1.1 Gauss.
 389      *
 390      * If gain were PIOS_HMC5x83_GAIN_2_5, for example, X and Y will read around +766 LSB
 391      * (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga)
 392      *
 393      * Changing measurement config back to PIOS_HMC5x83_MEASCONF_NORMAL will leave self-test mode.
 394      */
 395     PIOS_DELAY_WaitmS(10);
 396     if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, PIOS_HMC5x83_MEASCONF_BIAS_POS | PIOS_HMC5x83_ODR_15) != 0) {
 397         return -1;
 398     }
 399     PIOS_DELAY_WaitmS(10);
 400     if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, PIOS_HMC5x83_GAIN_8_1) != 0) {
 401         return -1;
 402     }
 403     PIOS_DELAY_WaitmS(10);
 404     if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_SINGLE) != 0) {
 405         return -1;
 406     }
 407
 408     /* Must wait for value to be updated */
 409     PIOS_DELAY_WaitmS(200);
 410
 411     if (PIOS_HMC5x83_ReadMag(handler, values) != 0) {
 412         return -1;
 413     }
 414
 415     dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, &ctrl_a_read, 1);
 416     dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_B, &ctrl_b_read, 1);
 417     dev->cfg->Driver->Read(handler, PIOS_HMC5x83_MODE_REG, &mode_read, 1);
 418     dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1);
 419
 420
 421     /* Restore backup configuration */
 422     PIOS_DELAY_WaitmS(10);
 423     if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, registers[0]) != 0) {
 424         return -1;
 425     }
 426     PIOS_DELAY_WaitmS(10);
 427     if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, registers[1]) != 0) {
 428         return -1;
 429     }
 430     PIOS_DELAY_WaitmS(10);
 431     if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, registers[2]) != 0) {
 432         return -1;
 433     }
 434
 435     return failed;
 436 }
 437
 438 /**
 439  * @brief IRQ Handler
 440  */
 441 bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler)
 442 {
 443     pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
 444
 445     dev->data_ready = true;
 446     return false;
 447 }
 448
 449 #ifdef PIOS_INCLUDE_SPI
 450 int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
 451 int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
 452
 453 const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER = {
 454     .Read  = PIOS_HMC5x83_SPI_Read,
 455     .Write = PIOS_HMC5x83_SPI_Write,
 456 };
 457
 458 static int32_t pios_hmc5x83_spi_claim_bus(pios_hmc5x83_dev_data_t *dev)
 459 {
 460     if (PIOS_SPI_ClaimBus(dev->port_id) < 0) {
 461         return -1;
 462     }
 463     PIOS_SPI_SetClockSpeed(dev->port_id, SPI_BaudRatePrescaler_16);
 464     PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 0);
 465     return 0;
 466 }
 467
 468 static void pios_hmc5x83_spi_release_bus(pios_hmc5x83_dev_data_t *dev)
 469 {
 470     PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 1);
 471     PIOS_SPI_ReleaseBus(dev->port_id);
 472 }
 473 /**
 474  * @brief Reads one or more bytes into a buffer
 475  * \param[in] address HMC5x83 register address (depends on size)
 476  * \param[out] buffer destination buffer
 477  * \param[in] len number of bytes which should be read
 478  * \return 0 if operation was successful
 479  * \return -1 if error during I2C transfer
 480  * \return -2 if unable to claim i2c device
 481  */
 482 int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len)
 483 {
 484     pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
 485
 486     if (pios_hmc5x83_spi_claim_bus(dev) < 0) {
 487         return -1;
 488     }
 489
 490     memset(buffer, 0xA5, len);
 491     PIOS_SPI_TransferByte(dev->port_id, address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG);
 492
 493     // buffer[0] = address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG;
 494     /* Copy the transfer data to the buffer */
 495     if (PIOS_SPI_TransferBlock(dev->port_id, NULL, buffer, len, NULL) < 0) {
 496         pios_hmc5x83_spi_release_bus(dev);
 497         return -3;
 498     }
 499     pios_hmc5x83_spi_release_bus(dev);
 500     return 0;
 501 }
 502
 503 /**
 504  * @brief Writes one or more bytes to the HMC5x83
 505  * \param[in] address Register address
 506  * \param[in] buffer source buffer
 507  * \return 0 if operation was successful
 508  * \return -1 if error during I2C transfer
 509  * \return -2 if unable to claim spi device
 510  */
 511 int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer)
 512 {
 513     pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
 514
 515     if (pios_hmc5x83_spi_claim_bus(dev) < 0) {
 516         return -1;
 517     }
 518     uint8_t data[] = {
 519         address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG,
 520         buffer,
 521     };
 522
 523     if (PIOS_SPI_TransferBlock(dev->port_id, data, NULL, sizeof(data), NULL) < 0) {
 524         pios_hmc5x83_spi_release_bus(dev);
 525         return -2;
 526     }
 527
 528     pios_hmc5x83_spi_release_bus(dev);
 529     return 0;
 530 }
 531 #endif /* PIOS_INCLUDE_SPI */
 532 #ifdef PIOS_INCLUDE_I2C
 533
 534 int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
 535 int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
 536
 537 const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER = {
 538     .Read  = PIOS_HMC5x83_I2C_Read,
 539     .Write = PIOS_HMC5x83_I2C_Write,
 540 };
 541
 542 /**
 543  * @brief Reads one or more bytes into a buffer
 544  * \param[in] address HMC5x83 register address (depends on size)
 545  * \param[out] buffer destination buffer
 546  * \param[in] len number of bytes which should be read
 547  * \return 0 if operation was successful
 548  * \return -1 if error during I2C transfer
 549  * \return -2 if unable to claim i2c device
 550  */
 551 int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len)
 552 {
 553     pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
 554     uint8_t addr_buffer[] = {
 555         address,
 556     };
 557
 558     const struct pios_i2c_txn txn_list[] = {
 559         {
 560             .info = __func__,
 561             .addr = PIOS_HMC5x83_I2C_ADDR,
 562             .rw   = PIOS_I2C_TXN_WRITE,
 563             .len  = sizeof(addr_buffer),
 564             .buf  = addr_buffer,
 565         }
 566         ,
 567         {
 568             .info = __func__,
 569             .addr = PIOS_HMC5x83_I2C_ADDR,
 570             .rw   = PIOS_I2C_TXN_READ,
 571             .len  = len,
 572             .buf  = buffer,
 573         }
 574     };
 575
 576     return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list));
 577 }
 578
 579 /**
 580  * @brief Writes one or more bytes to the HMC5x83
 581  * \param[in] address Register address
 582  * \param[in] buffer source buffer
 583  * \return 0 if operation was successful
 584  * \return -1 if error during I2C transfer
 585  * \return -2 if unable to claim i2c device
 586  */
 587 int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer)
 588 {
 589     pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
 590     uint8_t data[] = {
 591         address,
 592         buffer,
 593     };
 594
 595     const struct pios_i2c_txn txn_list[] = {
 596         {
 597             .info = __func__,
 598             .addr = PIOS_HMC5x83_I2C_ADDR,
 599             .rw   = PIOS_I2C_TXN_WRITE,
 600             .len  = sizeof(data),
 601             .buf  = data,
 602         }
 603         ,
 604     };
 605
 606     ;
 607     return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list));
 608 }
 609 #endif /* PIOS_INCLUDE_I2C */
 610
 611 /* PIOS sensor driver implementation */
 612 bool PIOS_HMC5x83_driver_Test(uintptr_t context)
 613 {
 614     return !PIOS_HMC5x83_Test((pios_hmc5x83_dev_t)context);
 615 }
 616
 617 void PIOS_HMC5x83_driver_Reset(__attribute__((unused)) uintptr_t context) {}
 618
 619 void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, __attribute__((unused))  uintptr_t context)
 620 {
 621     PIOS_Assert(size > 0);
 622     scales[0] = 1;
 623 }
 624
 625 void PIOS_HMC5x83_driver_fetch(void *data, uint8_t size, uintptr_t context)
 626 {
 627     PIOS_Assert(size > 0);
 628     int16_t mag[3];
 629     PIOS_HMC5x83_ReadMag((pios_hmc5x83_dev_t)context, mag);
 630     PIOS_SENSORS_3Axis_SensorsWithTemp *tmp = data;
 631     tmp->count = 1;
 632     tmp->sample[0].x = mag[0];
 633     tmp->sample[0].y = mag[1];
 634     tmp->sample[0].z = mag[2];
 635     tmp->temperature = 0;
 636 }
 637
 638 bool PIOS_HMC5x83_driver_poll(uintptr_t context)
 639 {
 640     return PIOS_HMC5x83_NewDataAvailable((pios_hmc5x83_dev_t)context);
 641 }
 642
 643 #endif /* PIOS_INCLUDE_HMC5x83 */
 644
 645 /**
 646  * @}
 647  * @}
 648  */