j3d1/esp-open-rtos
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
esp-open-rtos/extras/lis3mdl/lis3mdl.c

739 lines
23 KiB
C
Raw Normal View History

LIS3MDL 3-axes magnetometer driver added (#547)
2018-01-20 12:00:35 +00:00
/*
* 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 <string.h>
#include <stdlib.h>
#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, &reg, 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, &reg, 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;
}
Reference in a new issue Copy permalink