/* * Driver for LIS3MDL 3-axes digital magnetometer connected to I2C or SPI. * * This driver is for the usage with the ESP8266 and FreeRTOS (esp-open-rtos) * [https://github.com/SuperHouse/esp-open-rtos]. It is also working with ESP32 * and ESP-IDF [https://github.com/espressif/esp-idf.git] as well as Linux * based systems using a wrapper library for ESP8266 functions. * * --------------------------------------------------------------------------- * * The BSD License (3-clause license) * * Copyright (c) 2017 Gunar Schorcht (https://github.com/gschorcht) * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * 3. Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from this * software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * The information provided is believed to be accurate and reliable. The * copyright holder assumes no responsibility for the consequences of use * of such information nor for any infringement of patents or other rights * of third parties which may result from its use. No license is granted by * implication or otherwise under any patent or patent rights of the copyright * holder. */ #include #include #include "lis3mdl.h" #if defined(LIS3MDL_DEBUG_LEVEL_2) #define debug(s, f, ...) printf("%s %s: " s "\n", "LIS3MDL", f, ## __VA_ARGS__) #define debug_dev(s, f, d, ...) printf("%s %s: bus %d, addr %02x - " s "\n", "LIS3MDL", f, d->bus, d->addr, ## __VA_ARGS__) #else #define debug(s, f, ...) #define debug_dev(s, f, d, ...) #endif #if defined(LIS3MDL_DEBUG_LEVEL_1) || defined(LIS3MDL_DEBUG_LEVEL_2) #define error(s, f, ...) printf("%s %s: " s "\n", "LIS3MDL", f, ## __VA_ARGS__) #define error_dev(s, f, d, ...) printf("%s %s: bus %d, addr %02x - " s "\n", "LIS3MDL", f, d->bus, d->addr, ## __VA_ARGS__) #else #define error(s, f, ...) #define error_dev(s, f, d, ...) #endif // register addresses #define LIS3MDL_REG_WHO_AM_I 0x0f #define LIS3MDL_REG_CTRL1 0x20 #define LIS3MDL_REG_CTRL2 0x21 #define LIS3MDL_REG_CTRL3 0x22 #define LIS3MDL_REG_CTRL4 0x23 #define LIS3MDL_REG_CTRL5 0x24 #define LIS3MDL_REG_STATUS 0x27 #define LIS3MDL_REG_OUT_X_L 0x28 #define LIS3MDL_REG_OUT_X_H 0x29 #define LIS3MDL_REG_OUT_Y_L 0x2a #define LIS3MDL_REG_OUT_Y_H 0x2b #define LIS3MDL_REG_OUT_Z_L 0x2c #define LIS3MDL_REG_OUT_Z_H 0x2d #define LIS3MDL_REG_TEMP_OUT_L 0x2e #define LIS3MDL_REG_TEMP_OUT_H 0x2f #define LIS3MDL_REG_INT_CFG 0x30 #define LIS3MDL_REG_INT_SRC 0x31 #define LIS3MDL_REG_INT_THS_L 0x32 #define LIS3MDL_REG_INT_THS_H 0x33 // register structure definitions struct lis3mdl_reg_status { uint8_t XDA :1; // STATUS<0> X axis new data available uint8_t YDA :1; // STATUS<1> Y axis new data available uint8_t ZDA :1; // STATUS<2> Z axis new data available uint8_t ZYXDA :1; // STATUS<3> X, Y and Z axis new data available uint8_t XOR :1; // STATUS<4> X axis data overrun uint8_t YOR :1; // STATUS<5> Y axis data overrun uint8_t ZOR :1; // STATUS<6> Z axis data overrun uint8_t ZYXOR :1; // STATUS<7> X, Y and Z axis data overrun }; #define LIS3MDL_ANY_DATA_READY 0x0f // LIS3MDL_REG_STATUS<3:0> struct lis3mdl_reg_ctrl1 { uint8_t ST :1; // CTRL1<0> Self-test enable uint8_t FAST_ODR :1; // CTRL1<1> Data rates higher 80 Hz enabled uint8_t DO :3; // CTRL1<4:2> Output data rate uint8_t OM :2; // CTRL1<6:5> X and Y axes operative mode uint8_t TEMP_EN :1; // CTRL1<7> Temperature sensor enabled }; struct lis3mdl_reg_ctrl2 { uint8_t unused1 :2; // CTRL2<1:0> unused uint8_t SOFT_RST :1; // CTRL2<2> configuration and user regs reset uint8_t REBOOT :1; // CTRL2<3> Reboot memory content uint8_t unused2 :1; // CTRL2<4> unused uint8_t FS :2; // CTRL2<6:5> uint8_t unused3 :1; // CTRL2<7> unused }; struct lis3mdl_reg_ctrl3 { uint8_t MD :2; // CTRL3<1:0> Operation mode selection uint8_t SIM :1; // CTRL3<2> SPI serial interface mode selection uint8_t unused1 :2; // CTRL3<4:3> unused uint8_t LP :1; // CTRL3<5> Low power mode configuration uint8_t unused2 :2; // CTRL3<7:6> unused }; struct lis3mdl_reg_ctrl4 { uint8_t unused1 :1; // CTRL4<0> unused uint8_t BLE :1; // CTRL4<1> Big/litle endian data selection uint8_t OMZ :2; // CTRL4<3:2> Z axis operative mode uint8_t unused2 :4; // CTRL4<7:4> unused }; struct lis3mdl_reg_ctrl5 { uint8_t unused :6; // CTRL5<5:0> unused uint8_t BDU :1; // CTRL5<6> Block data update uint8_t FAST_READ:1; // CTRL5<7> Fast read enabled }; struct lis3mdl_reg_int_cfg { uint8_t IEN :1; // INT_CFG<0> Interrupt enabled uint8_t LIR :1; // INT_CFG<1> Latch interrupt request uint8_t IEA :1; // INT_CFG<2> Interrupt active uint8_t unused :2; // INT_CFG<4:3> unused uint8_t ZIEN :1; // INT_CFG<5> Z axis threshold interrupt enabled uint8_t YIEN :1; // INT_CFG<6> Y axis threshold interrupt enabled uint8_t XIEN :1; // INT_CFG<7> X axis threshold interrupt enabled }; struct lis3mdl_reg_int_src { uint8_t PTH_X :1; // INT_SRC<0> X exceeds threshold on positive side uint8_t PTH_Y :1; // INT_SRC<1> Y exceeds threshold on positive side uint8_t PTH_Z :1; // INT_SRC<2> Z exceeds threshold on positive side uint8_t NTH_X :1; // INT_SRC<3> X exceeds threshold on negative side uint8_t NTH_Y :1; // INT_SRC<4> Y exceeds threshold on negative side uint8_t NTH_Z :1; // INT_SRC<5> Z exceeds threshold on negative side uint8_t MROI :1; // INT_SRC<6> Internal measurement range overflow uint8_t INT :1; // INT_SRC<7> Interrupt event occurs }; /** Forward declaration of functions for internal use */ static bool lis3mdl_reset (lis3mdl_sensor_t* dev); static bool lis3mdl_is_available(lis3mdl_sensor_t* dev); static bool lis3mdl_i2c_read (lis3mdl_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len); static bool lis3mdl_i2c_write (lis3mdl_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len); static bool lis3mdl_spi_read (lis3mdl_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len); static bool lis3mdl_spi_write (lis3mdl_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len); #define msb_lsb_to_type(t,b,o) (t)(((t)b[o] << 8) | b[o+1]) #define lsb_msb_to_type(t,b,o) (t)(((t)b[o+1] << 8) | b[o]) #define lsb_to_type(t,b,o) (t)(b[o]) #define lis3mdl_update_reg(dev,addr,type,elem,value) \ { \ struct type __reg; \ if (!lis3mdl_reg_read (dev, (addr), (uint8_t*)&__reg, 1)) \ return false; \ __reg.elem = (value); \ if (!lis3mdl_reg_write (dev, (addr), (uint8_t*)&__reg, 1)) \ return false; \ } lis3mdl_sensor_t* lis3mdl_init_sensor (uint8_t bus, uint8_t addr, uint8_t cs) { lis3mdl_sensor_t* dev; if ((dev = malloc (sizeof(lis3mdl_sensor_t))) == NULL) return NULL; // init sensor data structure dev->bus = bus; dev->addr = addr; dev->cs = cs; dev->error_code = LIS3MDL_OK; dev->scale = lis3mdl_scale_4_Gs; // if addr==0 then SPI is used and has to be initialized if (!addr && !spi_device_init (bus, cs)) { error_dev ("Could not initialize SPI interface.", __FUNCTION__, dev); free (dev); return NULL; } // check availability of the sensor if (!lis3mdl_is_available (dev)) { error_dev ("Sensor is not available.", __FUNCTION__, dev); free (dev); return NULL; } // reset the sensor if (!lis3mdl_reset(dev)) { error_dev ("Could not reset the sensor device.", __FUNCTION__, dev); free (dev); return NULL; } lis3mdl_update_reg (dev, LIS3MDL_REG_CTRL2, lis3mdl_reg_ctrl2, FS, lis3mdl_scale_4_Gs); lis3mdl_update_reg (dev, LIS3MDL_REG_CTRL5, lis3mdl_reg_ctrl5, BDU, 1); return dev; } // switching times // LP 0.90 // MP 1.65 // HP 3.23 // UHP 6.40 bool lis3mdl_set_mode (lis3mdl_sensor_t* dev, lis3mdl_mode_t mode) { if (!dev) return false; dev->error_code = LIS3MDL_OK; struct lis3mdl_reg_ctrl1 ctrl1; struct lis3mdl_reg_ctrl3 ctrl3; struct lis3mdl_reg_ctrl4 ctrl4; // read current register values if (!lis3mdl_reg_read (dev, LIS3MDL_REG_CTRL1, (uint8_t*)&ctrl1, 1) || !lis3mdl_reg_read (dev, LIS3MDL_REG_CTRL3, (uint8_t*)&ctrl3, 1) || !lis3mdl_reg_read (dev, LIS3MDL_REG_CTRL4, (uint8_t*)&ctrl4, 1)) return false; if (mode < lis3mdl_lpm_1000) { ctrl1.FAST_ODR = 0; ctrl3.MD = 0; // continuous measurement ctrl3.LP = 0; ctrl1.DO = mode; ctrl1.OM = 0; ctrl4.OMZ = ctrl1.OM; } else if (mode < lis3mdl_low_power) { ctrl1.FAST_ODR = 1; ctrl3.MD = 0; // continuous measurement ctrl3.LP = 0; ctrl1.DO = 0; ctrl1.OM = mode - lis3mdl_lpm_1000; ctrl4.OMZ = ctrl1.OM; } else if (mode == lis3mdl_low_power) { ctrl1.FAST_ODR = 0; ctrl3.MD = 0; // continuous measurement ctrl3.LP = 1; // at lowest data rate 0.625 Hz ctrl1.DO = 0; ctrl1.OM = 0; ctrl4.OMZ = ctrl1.OM; } else // lis3mdl_power_down { ctrl3.MD = 3; } if (!lis3mdl_reg_write (dev, LIS3MDL_REG_CTRL1, (uint8_t*)&ctrl1, 1) || !lis3mdl_reg_write (dev, LIS3MDL_REG_CTRL3, (uint8_t*)&ctrl3, 1) || !lis3mdl_reg_write (dev, LIS3MDL_REG_CTRL4, (uint8_t*)&ctrl4, 1)) return false; // wait until mode switch happened vTaskDelay (50/portTICK_PERIOD_MS); // dummy read last data register set lis3mdl_raw_data_t raw; lis3mdl_get_raw_data (dev, &raw); return false; } bool lis3mdl_set_scale (lis3mdl_sensor_t* dev, lis3mdl_scale_t scale) { if (!dev) return false; dev->error_code = LIS3MDL_OK; dev->scale = scale; // read CTRL2 register and write scale lis3mdl_update_reg (dev, LIS3MDL_REG_CTRL2, lis3mdl_reg_ctrl2, FS, scale); return true; } bool lis3mdl_new_data (lis3mdl_sensor_t* dev) { if (!dev) return false; dev->error_code = LIS3MDL_OK; struct lis3mdl_reg_status status; if (!lis3mdl_reg_read (dev, LIS3MDL_REG_STATUS, (uint8_t*)&status, 1)) { error_dev ("Could not get sensor status", __FUNCTION__, dev); return false; } return status.ZYXDA; } /** * Scaling factors for the conversion of raw sensor data to floating point _Gs * values. Scaling factors are from mechanical characteristics in datasheet. * * scale/sensitivity resolution sensitivity * +-4 gauss 6842 LSB/gauss 1.461561e-4 * +-8 gauss 3421 LSB/gauss 2,923122e-4 * +-12 gauss 2281 LSB/gauss 4,384042e-4 * +-16 gauss 1711 LSB/gauss 5,844535e-4 */ const static double LIS3MDL_SCALES[4] = { 1.0/6842, 1.0/3421, 1.0/2281, 1.0/1711 }; bool lis3mdl_get_float_data (lis3mdl_sensor_t* dev, lis3mdl_float_data_t* data) { if (!dev || !data) return false; lis3mdl_raw_data_t raw; if (!lis3mdl_get_raw_data (dev, &raw)) return false; data->mx = LIS3MDL_SCALES[dev->scale] * raw.mx; data->my = LIS3MDL_SCALES[dev->scale] * raw.my; data->mz = LIS3MDL_SCALES[dev->scale] * raw.mz; return true; } bool lis3mdl_get_raw_data (lis3mdl_sensor_t* dev, lis3mdl_raw_data_t* raw) { if (!dev || !raw) return false; dev->error_code = LIS3MDL_OK; uint8_t regs[6]; // read raw data sample if (!lis3mdl_reg_read (dev, LIS3MDL_REG_OUT_X_L, regs, 6)) { error_dev ("Could not get raw data sample", __FUNCTION__, dev); dev->error_code |= LIS3MDL_GET_RAW_DATA_FAILED; return false; } raw->mx = ((uint16_t)regs[1] << 8) | regs[0]; raw->my = ((uint16_t)regs[3] << 8) | regs[2]; raw->mz = ((uint16_t)regs[5] << 8) | regs[4]; return true; } bool lis3mdl_set_int_config (lis3mdl_sensor_t* dev, lis3mdl_int_config_t* cfg) { if (!dev || !cfg) return false; dev->error_code = LIS3MDL_OK; struct lis3mdl_reg_int_cfg int_cfg; int_cfg.unused = 0; int_cfg.XIEN = cfg->x_enabled; int_cfg.YIEN = cfg->y_enabled; int_cfg.ZIEN = cfg->z_enabled; int_cfg.LIR = cfg->latch; int_cfg.IEA = cfg->signal_level; int_cfg.IEN = cfg->x_enabled | cfg->y_enabled | cfg->z_enabled; if (// write the threshold to registers INT_THS_* !lis3mdl_reg_write (dev, LIS3MDL_REG_INT_THS_L, (uint8_t*)&cfg->threshold, 2) || // write configuration to INT_CFG !lis3mdl_reg_write (dev, LIS3MDL_REG_INT_CFG, (uint8_t*)&int_cfg, 1)) { error_dev ("Could not configure interrupt INT", __FUNCTION__, dev); dev->error_code |= LIS3MDL_CONFIG_INT_FAILED; return false; } return true; } bool lis3mdl_get_int_config (lis3mdl_sensor_t* dev, lis3mdl_int_config_t* cfg) { if (!dev || !cfg) return false; dev->error_code = LIS3MDL_OK; struct lis3mdl_reg_int_cfg int_cfg; if (!lis3mdl_reg_read (dev, LIS3MDL_REG_INT_THS_L, (uint8_t*)&cfg->threshold, 2) || !lis3mdl_reg_read (dev, LIS3MDL_REG_INT_CFG , (uint8_t*)&int_cfg, 1)) { error_dev ("Could not read configuration of interrupt INT from sensor", __FUNCTION__, dev); dev->error_code |= LIS3MDL_CONFIG_INT_FAILED; return false; } cfg->x_enabled = int_cfg.XIEN; cfg->y_enabled = int_cfg.YIEN; cfg->z_enabled = int_cfg.ZIEN; cfg->latch = int_cfg.LIR; cfg->signal_level = int_cfg.IEA; return true; } bool lis3mdl_get_int_source (lis3mdl_sensor_t* dev, lis3mdl_int_source_t* src) { if (!dev || !src) return false; dev->error_code = LIS3MDL_OK; struct lis3mdl_reg_int_src int_src; struct lis3mdl_reg_int_cfg int_cfg; if (!lis3mdl_reg_read (dev, LIS3MDL_REG_INT_SRC, (uint8_t*)&int_src, 1) || !lis3mdl_reg_read (dev, LIS3MDL_REG_INT_CFG, (uint8_t*)&int_cfg, 1)) { error_dev ("Could not read source of interrupt INT from sensor", __FUNCTION__, dev); dev->error_code |= LIS3MDL_INT_SOURCE_FAILED; return false; } src->active = int_src.INT; src->x_pos = int_src.PTH_X & int_cfg.XIEN; src->x_neg = int_src.NTH_X & int_cfg.XIEN; src->y_pos = int_src.PTH_Y & int_cfg.YIEN; src->y_neg = int_src.NTH_Y & int_cfg.YIEN; src->z_pos = int_src.PTH_Z & int_cfg.ZIEN; src->z_neg = int_src.NTH_Z & int_cfg.ZIEN; return true; } bool lis3mdl_enable_temperature (lis3mdl_sensor_t* dev, bool enable) { lis3mdl_update_reg (dev, LIS3MDL_REG_CTRL1, lis3mdl_reg_ctrl1, TEMP_EN, enable); return true; } float lis3mdl_get_temperature (lis3mdl_sensor_t* dev) { uint8_t regs[2]; // read raw data sample if (!lis3mdl_reg_read (dev, LIS3MDL_REG_TEMP_OUT_L, regs, 2)) { error_dev ("Could not get temperature data sample", __FUNCTION__, dev); dev->error_code |= LIS3MDL_GET_RAW_DATA_FAILED; return false; } return (((int16_t)((regs[1] << 8) | regs[0])) >> 3) + 25.0; } /** Functions for internal use only */ /** * @brief Check the chip ID to test whether sensor is available */ static bool lis3mdl_is_available (lis3mdl_sensor_t* dev) { uint8_t chip_id; if (!dev) return false; dev->error_code = LIS3MDL_OK; if (!lis3mdl_reg_read (dev, LIS3MDL_REG_WHO_AM_I, &chip_id, 1)) return false; if (chip_id != LIS3MDL_CHIP_ID) { error_dev ("Chip id %02x is wrong, should be %02x.", __FUNCTION__, dev, chip_id, LIS3MDL_CHIP_ID); dev->error_code = LIS3MDL_WRONG_CHIP_ID; return false; } return true; } static bool lis3mdl_reset (lis3mdl_sensor_t* dev) { if (!dev) return false; dev->error_code = LIS3MDL_OK; uint8_t ctrl_regs[5] = { 0x10, 0x00, 0x03, 0x00, 0x00 }; uint8_t int_cfg = 0x00; // initialize sensor completely including setting in power down mode lis3mdl_reg_write (dev, LIS3MDL_REG_CTRL1 , ctrl_regs, 5); lis3mdl_reg_write (dev, LIS3MDL_REG_INT_CFG, &int_cfg , 1); return true; } bool lis3mdl_reg_read(lis3mdl_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len) { if (!dev || !data) return false; return (dev->addr) ? lis3mdl_i2c_read (dev, reg, data, len) : lis3mdl_spi_read (dev, reg, data, len); } bool lis3mdl_reg_write(lis3mdl_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len) { if (!dev || !data) return false; return (dev->addr) ? lis3mdl_i2c_write (dev, reg, data, len) : lis3mdl_spi_write (dev, reg, data, len); } #define LIS3MDL_SPI_BUF_SIZE 64 // SPI register data buffer size of ESP866 #define LIS3MDL_SPI_READ_FLAG 0x80 #define LIS3MDL_SPI_WRITE_FLAG 0x00 #define LIS3MDL_SPI_AUTO_INC_FLAG 0x40 static bool lis3mdl_spi_read(lis3mdl_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len) { if (!dev || !data) return false; if (len >= LIS3MDL_SPI_BUF_SIZE) { dev->error_code |= LIS3MDL_SPI_BUFFER_OVERFLOW; error_dev ("Error on read from SPI slave on bus 1. Tried to transfer " "more than %d byte in one read operation.", __FUNCTION__, dev, LIS3MDL_SPI_BUF_SIZE); return false; } uint8_t addr = (reg & 0x3f) | LIS3MDL_SPI_READ_FLAG | LIS3MDL_SPI_AUTO_INC_FLAG; static uint8_t mosi[LIS3MDL_SPI_BUF_SIZE]; static uint8_t miso[LIS3MDL_SPI_BUF_SIZE]; memset (mosi, 0xff, LIS3MDL_SPI_BUF_SIZE); memset (miso, 0xff, LIS3MDL_SPI_BUF_SIZE); mosi[0] = addr; if (!spi_transfer_pf (dev->bus, dev->cs, mosi, miso, len+1)) { error_dev ("Could not read data from SPI", __FUNCTION__, dev); dev->error_code |= LIS3MDL_SPI_READ_FAILED; return false; } // shift data one by left, first byte received while sending register address is invalid for (int i=0; i < len; i++) data[i] = miso[i+1]; #ifdef LIS3MDL_DEBUG_LEVEL_2 printf("LIS3MDL %s: read the following bytes from reg %02x: ", __FUNCTION__, reg); for (int i=0; i < len; i++) printf("%02x ", data[i]); printf("\n"); #endif return true; } static bool lis3mdl_spi_write(lis3mdl_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len) { if (!dev || !data) return false; uint8_t addr = (reg & 0x3f) | LIS3MDL_SPI_WRITE_FLAG | LIS3MDL_SPI_AUTO_INC_FLAG; static uint8_t mosi[LIS3MDL_SPI_BUF_SIZE]; if (len >= LIS3MDL_SPI_BUF_SIZE) { dev->error_code |= LIS3MDL_SPI_BUFFER_OVERFLOW; error_dev ("Error on write to SPI slave on bus 1. Tried to transfer more" "than %d byte in one write operation.", __FUNCTION__, dev, LIS3MDL_SPI_BUF_SIZE); return false; } reg &= 0x7f; // first byte in output is the register address mosi[0] = addr; // shift data one byte right, first byte in output is the register address for (int i = 0; i < len; i++) mosi[i+1] = data[i]; #ifdef LIS3MDL_DEBUG_LEVEL_2 printf("LIS3MDL %s: Write the following bytes to reg %02x: ", __FUNCTION__, reg); for (int i = 1; i < len+1; i++) printf("%02x ", mosi[i]); printf("\n"); #endif if (!spi_transfer_pf (dev->bus, dev->cs, mosi, NULL, len+1)) { error_dev ("Could not write data to SPI.", __FUNCTION__, dev); dev->error_code |= LIS3MDL_SPI_WRITE_FAILED; return false; } return true; } #define I2C_AUTO_INCREMENT (0x80) static bool lis3mdl_i2c_read(lis3mdl_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len) { if (!dev || !data) return false; debug_dev ("Read %d byte from i2c slave register %02x.", __FUNCTION__, dev, len, reg); if (len > 1) reg |= I2C_AUTO_INCREMENT; int result = i2c_slave_read(dev->bus, dev->addr, ®, data, len); if (result) { dev->error_code |= (result == -EBUSY) ? LIS3MDL_I2C_BUSY : LIS3MDL_I2C_READ_FAILED; error_dev ("Error %d on read %d byte from I2C slave register %02x.", __FUNCTION__, dev, result, len, reg); return false; } # ifdef LIS3MDL_DEBUG_LEVEL_2 printf("LIS3MDL %s: Read following bytes: ", __FUNCTION__); printf("%02x: ", reg & 0x7f); for (int i=0; i < len; i++) printf("%02x ", data[i]); printf("\n"); # endif return true; } static bool lis3mdl_i2c_write(lis3mdl_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len) { if (!dev || !data) return false; debug_dev ("Write %d byte to i2c slave register %02x.", __FUNCTION__, dev, len, reg); if (len > 1) reg |= I2C_AUTO_INCREMENT; int result = i2c_slave_write(dev->bus, dev->addr, ®, data, len); if (result) { dev->error_code |= (result == -EBUSY) ? LIS3MDL_I2C_BUSY : LIS3MDL_I2C_WRITE_FAILED; error_dev ("Error %d on write %d byte to i2c slave register %02x.", __FUNCTION__, dev, result, len, reg); return false; } # ifdef LIS3MDL_DEBUG_LEVEL_2 printf("LIS3MDL %s: Wrote the following bytes: ", __FUNCTION__); printf("%02x: ", reg & 0x7f); for (int i=0; i < len; i++) printf("%02x ", data[i]); printf("\n"); # endif return true; }