Browse Source

drivers/adxl345: cleanup/enhancements

Signed-off-by: dylad <dylan.laduranty@mesotic.com>
master
dylad 5 years ago
parent
commit
530a2ef277
  1. 53
      drivers/adxl345/adxl345.c
  2. 34
      drivers/adxl345/include/adxl345_params.h
  3. 80
      drivers/include/adxl345.h
  4. 9
      tests/driver_adxl345/main.c

53
drivers/adxl345/adxl345.c

@ -31,17 +31,20 @@
#define I2C_SPEED I2C_SPEED_NORMAL
#define BUS (dev->params.i2c)
#define ADDR (dev->params.addr)
#define BUS (dev->i2c)
#define ADDR (dev->addr)
int adxl345_init(adxl345_t *dev, const adxl345_params_t* params)
int adxl345_init(adxl345_t *dev, adxl345_params_t* params)
{
uint8_t reg;
assert(dev && params);
/* get device descriptor */
dev->params= *params;
dev->params = (adxl345_params_t*)params;
/* get scale_factor from full_res and range parameters */
dev->scale_factor = (dev->params->full_res ? 3.9 : (dev->params->range * 3.9));
/* Acquire exclusive access */
i2c_acquire(BUS);
@ -61,11 +64,11 @@ int adxl345_init(adxl345_t *dev, const adxl345_params_t* params)
return ADXL345_NODEV;
}
/* configure the user offset */
i2c_write_regs(BUS, ADDR, ACCEL_ADXL345_OFFSET_X, dev->params.offset, 3);
i2c_write_regs(BUS, ADDR, ACCEL_ADXL345_OFFSET_X, dev->params->offset, 3);
/* Basic device setup */
reg = (dev->params.full_res | dev->params.range);
reg = (dev->params->full_res | dev->params->range);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_DATA_FORMAT, reg);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_BW_RATE, dev->params.rate);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_BW_RATE, dev->params->rate);
/* Put device in measure mode */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_POWER_CTL, MEASURE_BIT);
@ -79,7 +82,7 @@ int adxl345_init(adxl345_t *dev, const adxl345_params_t* params)
void adxl345_read(adxl345_t *dev, adxl345_data_t *data)
{
uint8_t result[6];
int8_t result[6];
assert(dev && data);
@ -87,9 +90,9 @@ void adxl345_read(adxl345_t *dev, adxl345_data_t *data)
i2c_read_regs(BUS, ADDR, ACCEL_ADXL345_DATA_X0, result, 6);
i2c_release(BUS);
data->x = (((result[1] << 8)+result[0]) * dev->params.scale_factor);
data->y = (((result[3] << 8)+result[2]) * dev->params.scale_factor);
data->z = (((result[5] << 8)+result[4]) * dev->params.scale_factor);
data->x = (((result[1] << 8)+result[0]) * dev->scale_factor);
data->y = (((result[3] << 8)+result[2]) * dev->scale_factor);
data->z = (((result[5] << 8)+result[4]) * dev->scale_factor);
}
void adxl345_set_interrupt(adxl345_t *dev)
@ -100,33 +103,33 @@ void adxl345_set_interrupt(adxl345_t *dev)
i2c_acquire(BUS);
/* Set threshold */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_THRESH_TAP, dev->params.interrupt.thres_tap);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_THRESH_TAP, dev->interrupt.thres_tap);
/* Set Map */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_INT_MAP, dev->params.interrupt.map);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_INT_MAP, dev->interrupt.map);
/* Set Duration */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TAP_DUR, dev->params.interrupt.thres_dur);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TAP_DUR, dev->interrupt.thres_dur);
/* Enable axes */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TAP_AXES, dev->params.interrupt.tap_axes);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TAP_AXES, dev->interrupt.tap_axes);
/* Set source */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_INT_SOURCE, dev->params.interrupt.source);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_INT_SOURCE, dev->interrupt.source);
/* Set latent threshold */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TAP_LAT, dev->params.interrupt.thres_latent);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TAP_LAT, dev->interrupt.thres_latent);
/* Set window threshold */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TAP_WIN, dev->params.interrupt.thres_window);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TAP_WIN, dev->interrupt.thres_window);
/* Set activity threshold */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_THRESH_ACT, dev->params.interrupt.thres_act);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_THRESH_ACT, dev->interrupt.thres_act);
/* Set inactivity threshold */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_THRESH_INACT, dev->params.interrupt.thres_inact);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_THRESH_INACT, dev->interrupt.thres_inact);
/* Set inactivity time */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TIME_INACT, dev->params.interrupt.time_inact);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TIME_INACT, dev->interrupt.time_inact);
/* Set free-fall threshold */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_THRESH_FF, dev->params.interrupt.thres_ff);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_THRESH_FF, dev->interrupt.thres_ff);
/* Set free-fall time */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TIME_FF, dev->params.interrupt.time_ff);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_TIME_FF, dev->interrupt.time_ff);
/* Set axis control */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_ACT_INACT_CTL, dev->params.interrupt.act_inact);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_ACT_INACT_CTL, dev->interrupt.act_inact);
/* Enable interrupt */
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_INT_ENABLE, dev->params.interrupt.enable);
i2c_write_reg(BUS, ADDR, ACCEL_ADXL345_INT_ENABLE, dev->interrupt.enable);
/* Release the bus */
i2c_release(BUS);

34
drivers/adxl345/include/adxl345_params.h

@ -32,39 +32,35 @@ extern "C" {
* @{
*/
#ifndef ADXL345_PARAM_I2C
#define ADXL345_PARAM_I2C (I2C_DEV(0))
#define ADXL345_PARAM_I2C (I2C_DEV(0))
#endif
#ifndef ADXL345_PARAM_ADDR
#define ADXL345_PARAM_ADDR (ADXL345_ADDR_53)
#define ADXL345_PARAM_ADDR (ADXL345_ADDR_53)
#endif
#ifndef ADXL345_PARAM_RATE
#define ADXL345_PARAM_RATE (ADXL345_RATE_200HZ)
#define ADXL345_PARAM_RATE (ADXL345_RATE_200HZ)
#endif
#ifndef ADXL345_PARAM_RANGE
#define ADXL345_PARAM_RANGE (ADXL345_RANGE_16G)
#endif
#ifndef ADXL345_PARAM_OFFSET
#define ADXL345_PARAM_OFFSET { 0, 0, 0 }
#define ADXL345_PARAM_RANGE (ADXL345_RANGE_16G)
#endif
#ifndef ADXL345_PARAM_INTERRUPT
#define ADXL345_PARAM_INTERRUPT {0x0F, 0xBF, 0x40, 0xF0, 0xFF, 0x00, 0x00, \
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F}
#define ADXL345_PARAM_INTERRUPT {0x0F, 0xBF, 0x40, 0xF0, 0xFF, 0x00, 0x00, \
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F}
#endif
#ifndef ADXL345_PARAM_FULL_RES
#define ADXL345_PARAM_FULL_RES (1)
#define ADXL345_PARAM_FULL_RES (1)
#endif
#ifndef ADXL345_PARAM_OFFSET
#define ADXL345_PARAM_OFFSET { 0, 0, 0 }
#endif
#ifndef ADXL345_PARAM_SCALE_FACTOR
#define ADXL345_PARAM_SCALE_FACTOR (3.9)
#define ADXL345_PARAM_SCALE_FACTOR (3.9)
#endif
#ifndef ADXL345_PARAMS
#define ADXL345_PARAMS { .i2c = ADXL345_PARAM_I2C, \
.addr = ADXL345_PARAM_ADDR, \
.rate = ADXL345_PARAM_RATE, \
.range = ADXL345_PARAM_RANGE, \
.offset = ADXL345_PARAM_OFFSET, \
.interrupt = ADXL345_PARAM_INTERRUPT, \
.full_res = ADXL345_PARAM_FULL_RES, \
.scale_factor = ADXL345_PARAM_SCALE_FACTOR}
#define ADXL345_PARAMS { .offset = ADXL345_PARAM_OFFSET, \
.range = ADXL345_PARAM_RANGE, \
.rate = ADXL345_PARAM_RATE, \
.full_res = ADXL345_PARAM_FULL_RES }
#endif
/**@}*/

80
drivers/include/adxl345.h

@ -26,45 +26,56 @@ extern "C" {
#endif
#include "periph/i2c.h"
#include "periph/gpio.h"
/**
* @brief Possible ADXL345 hardware addresses (wiring specific)
*/
enum {
ADXL345_ADDR_1D = 0x1D, /**< I2C device address if SDO / Alt address pin is high */
ADXL345_ADDR_53 = 0x53, /**< I2C device address if SDO / Alt address pin is low */
ADXL345_ADDR_1D = 0x1D, /**< I2C device address if Alt addr pin is high */
ADXL345_ADDR_53 = 0x53, /**< I2C device address if Alt addr pin is low */
};
/**
* @brief List ADXL345 power mode
*/
enum {
ADXL345_MEASURE_MODE,
ADXL345_STANDBY_MODE,
ADXL345_SLEEP_MODE,
ADXL345_AUTOSLEEP_MODE,
};
/**
* @brief Define ADXL345 sensitivity
*/
enum {
ADXL345_RANGE_2G, /**< +/- 2 g Full Scale Rang */
ADXL345_RANGE_4G, /**< +/- 4 g Full Scale Rang */
ADXL345_RANGE_8G, /**< +/- 8 g Full Scale Rang */
ADXL345_RANGE_16G /**< +/- 16 g Full Scale Rang */
ADXL345_RANGE_2G = 1, /**< +/- 2 g Full Scale Rang */
ADXL345_RANGE_4G = 2, /**< +/- 4 g Full Scale Rang */
ADXL345_RANGE_8G = 4, /**< +/- 8 g Full Scale Rang */
ADXL345_RANGE_16G = 8 /**< +/- 16 g Full Scale Rang */
};
/**
* @brief List bandwidth rate
*/
enum {
ADXL345_RATE_0HZ1 = 0, /**< 0.1 Hz Output Data Rate */
ADXL345_RATE_0HZ2 = 1, /**< 0.2 Hz Output Data Rate */
ADXL345_RATE_0HZ39 = 2, /**< 0.39 Hz Output Data Rate */
ADXL345_RATE_0HZ78 = 3, /**< 0.78 Hz Output Data Rate */
ADXL345_RATE_1HZ56 = 4, /**< 1.56 Hz Output Data Rate */
ADXL345_RATE_3HZ13 = 5, /**< 3.13 Hz Output Data Rate */
ADXL345_RATE_6HZ25 = 6, /**< 6.25 Hz Output Data Rate */
ADXL345_RATE_12HZ50 = 7, /**< 12.5 Hz Output Data Rate */
ADXL345_RATE_25HZ = 8, /**< 25 Hz Output Data Rate */
ADXL345_RATE_50HZ = 9, /**< 50 Hz Output Data Rate */
ADXL345_RATE_100HZ = 10, /**< 100 Hz Output Data Rate */
ADXL345_RATE_200HZ = 11, /**< 200 Hz Output Data Rate */
ADXL345_RATE_400HZ = 12, /**< 400 Hz Output Data Rate */
ADXL345_RATE_800HZ = 13, /**< 800 Hz Output Data Rate */
ADXL345_RATE_1600HZ = 14, /**< 1600 Hz Output Data Rate */
ADXL345_RATE_3200HZ = 15 /**< 3200 Hz Output Data Rate */
ADXL345_RATE_0HZ1 = 0, /**< 0.1 Hz Output Data Rate */
ADXL345_RATE_0HZ2 = 1, /**< 0.2 Hz Output Data Rate */
ADXL345_RATE_0HZ39 = 2, /**< 0.39 Hz Output Data Rate */
ADXL345_RATE_0HZ78 = 3, /**< 0.78 Hz Output Data Rate */
ADXL345_RATE_1HZ56 = 4, /**< 1.56 Hz Output Data Rate */
ADXL345_RATE_3HZ13 = 5, /**< 3.13 Hz Output Data Rate */
ADXL345_RATE_6HZ25 = 6, /**< 6.25 Hz Output Data Rate */
ADXL345_RATE_12HZ50 = 7, /**< 12.5 Hz Output Data Rate */
ADXL345_RATE_25HZ = 8, /**< 25 Hz Output Data Rate */
ADXL345_RATE_50HZ = 9, /**< 50 Hz Output Data Rate */
ADXL345_RATE_100HZ = 10, /**< 100 Hz Output Data Rate */
ADXL345_RATE_200HZ = 11, /**< 200 Hz Output Data Rate */
ADXL345_RATE_400HZ = 12, /**< 400 Hz Output Data Rate */
ADXL345_RATE_800HZ = 13, /**< 800 Hz Output Data Rate */
ADXL345_RATE_1600HZ = 14, /**< 1600 Hz Output Data Rate */
ADXL345_RATE_3200HZ = 15 /**< 3200 Hz Output Data Rate */
};
/**
@ -120,7 +131,7 @@ typedef struct {
uint8_t time_inact; /**< Inactivity time */
uint8_t thres_ff; /**< Free-fall threshold */
uint8_t time_ff; /**< Time threshold */
uint8_t act_inact; /**< Axis enable control for activity and inactivity detection */
uint8_t act_inact; /**< Enable ctrl for activity/inactivity detection */
uint8_t tap_axes; /**< Axis control for single tap/double tap */
} adxl345_interrupt_t;
@ -128,21 +139,23 @@ typedef struct {
* @brief Configuration struct for the ADXL345 sensor
*/
typedef struct {
i2c_t i2c; /**< I2C device which is used */
uint8_t addr; /**< I2C address */
adxl345_interrupt_t interrupt; /**< Interrupts configuration */
uint8_t offset[3]; /**< offset axis */
uint8_t range; /**< Sensitivity configuration */
uint8_t rate; /**< Configured sample rate for accel */
uint8_t full_res; /**< Resolution bit */
uint8_t scale_factor; /**< Scale factor for converting value to mg */
gpio_t int1; /**< accelerometer int1 pin */
gpio_t int2; /**< accelerometer int2 pin */
uint8_t offset[3]; /**< offset axis */
uint8_t range; /**< Sensitivity configuration */
uint8_t rate; /**< Configured sample rate for accel */
uint8_t full_res; /**< Resolution bit */
} adxl345_params_t;
/**
* @brief Device descriptor for the ADXL345 sensor
*/
typedef struct {
adxl345_params_t params; /**< Device configuration */
i2c_t i2c; /**< I2C device which is used */
uint8_t addr; /**< I2C address */
adxl345_params_t *params; /**< Device configuration */
adxl345_interrupt_t interrupt; /**< Interrupts configuration */
float scale_factor; /**< Scale factor for converting value to mg */
} adxl345_t;
/**
@ -155,8 +168,7 @@ typedef struct {
* @return ADXL345_NOI2C if initialization of I2C bus failed
* @return ADXL345_NODEV if accelerometer test failed
*/
int adxl345_init(adxl345_t *dev, const adxl345_params_t* params);
int adxl345_init(adxl345_t *dev, adxl345_params_t* params);
/**
* @brief Read accelerometer's data
*

9
tests/driver_adxl345/main.c

@ -31,11 +31,14 @@ int main(void)
adxl345_t dev;
adxl345_data_t data;
dev.i2c = ADXL345_PARAM_I2C;
dev.addr = ADXL345_PARAM_ADDR;
puts("ADXL345 test application");
printf("Initializing ADXL345 accelerometer at I2C_DEV(%i)... ",
adxl345_params->i2c);
dev.i2c);
if (adxl345_init(&dev, adxl345_params) == ADXL345_OK) {
if (adxl345_init(&dev, (adxl345_params_t*)adxl345_params) == ADXL345_OK) {
puts("[OK]\n");
}
else {
@ -46,7 +49,7 @@ int main(void)
while(1) {
adxl345_read(&dev, &data);
printf("Acceleration [in mg]: X axis:%d Y axis:%d Z axis:%d\n",
data.x, data.y, data.z);
(int)data.x, (int)data.y, (int)data.z);
xtimer_usleep(SLEEP_DELAY);
}
return 0;

Loading…
Cancel
Save