/**
* Disables an interrupt in ADXL345 device
*
* @param Pointer to sensor structure
* @param which interrupt(s) to disable
*
* @return 0 on success, non-zero on failure
*/
static int
disable_interrupt(struct sensor * sensor, uint8_t ints_to_disable)
{
struct adxl345_private_driver_data *pdd;
struct adxl345 *adxl345;
struct sensor_itf *itf;
if (ints_to_disable == 0) {
return SYS_EINVAL;
}
adxl345 = (struct adxl345 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &adxl345->pdd;
/* update which interrupts are enabled */
pdd->int_enable &= ~ints_to_disable;
/* if no interrupts are now in use disable int pin */
if (pdd->int_enable == 0) {
hal_gpio_irq_disable(adxl345->sensor.s_itf.si_ints[pdd->int_num].host_pin);
}
/* update interrupt setup in device */
return adxl345_setup_interrupts(itf, pdd->int_enable, pdd->int_route);
}
static int
lis2dw12_sensor_handle_interrupt(struct sensor * sensor)
{
struct lis2dw12 * lis2dw12;
struct lis2dw12_private_driver_data *pdd;
struct sensor_itf *itf;
uint8_t int_src;
int rc;
lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &lis2dw12->pdd;
rc = lis2dw12_clear_int(itf, &int_src);
if (rc) {
LIS2DW12_ERR("Cound not read int status err=0x%02x\n", rc);
return rc;
}
if ((pdd->registered_mask & LIS2DW12_NOTIFY_MASK) &&
((int_src & LIS2DW12_INT_SRC_STAP) ||
(int_src & LIS2DW12_INT_SRC_DTAP))) {
sensor_mgr_put_notify_evt(&pdd->notify_ctx);
}
return 0;
}
static int
drv2605_shell_cmd_load_rom(int argc, char **argv, struct drv2605 *drv2605)
{
int rc;
uint8_t waveform[8] = {0};
uint8_t len = argc-2;
uint8_t i;
struct sensor_itf *itf;
if (argc > 10) {
return drv2605_shell_err_too_many_args(argv[1]);
} else if (argc < 3) {
return drv2605_shell_err_too_few_args(argv[1]);
}
for (i = 0; i<len;i++) {
waveform[i] = parse_ll_bounds(argv[i+2], 0, 255, &rc);
if (rc != 0) {
goto err;
}
}
itf = SENSOR_GET_ITF(&(drv2605->sensor));
rc = drv2605_load_rom(itf, waveform, len);
if (rc) {
console_printf("load failed %d\n", rc);
}else{
console_printf("load succeeded\n");
}
return 0;
err:
return rc;
}
static int
drv2605_shell_cmd_peek(int argc, char **argv, struct drv2605 *drv2605)
{
int rc;
uint8_t value;
uint8_t reg;
struct sensor_itf *itf;
if (argc > 3) {
return drv2605_shell_err_too_many_args(argv[1]);
} else if (argc < 3) {
return drv2605_shell_err_too_few_args(argv[1]);
}
reg = parse_ll_bounds(argv[2], 0, 34, &rc);
if (rc != 0) {
return drv2605_shell_err_invalid_arg(argv[2]);
}
itf = SENSOR_GET_ITF(&(drv2605->sensor));
rc = drv2605_read8(itf, reg, &value);
if (rc) {
console_printf("peek failed %d\n", rc);
}else{
console_printf("value: 0x%02X\n", value);
}
return 0;
}
static int
drv2605_shell_cmd_get_chip_id(int argc, char **argv, struct drv2605 *drv2605)
{
uint8_t id;
int rc;
struct sensor_itf *itf;
if (argc > 3) {
return drv2605_shell_err_too_many_args(argv[1]);
}
itf = SENSOR_GET_ITF(&(drv2605->sensor));
/* Display the chip id */
if (argc == 2) {
rc = drv2605_get_chip_id(itf, &id);
if (rc) {
console_printf("chipid failed %d\n", rc);
}else{
console_printf("0x%02X\n", id);
}
}
return 0;
}
static int
drv2605_shell_cmd_poke(int argc, char **argv, struct drv2605 *drv2605)
{
int rc;
uint8_t reg;
uint8_t value;
struct sensor_itf *itf;
if (argc > 4) {
return drv2605_shell_err_too_many_args(argv[1]);
} else if (argc < 4) {
return drv2605_shell_err_too_few_args(argv[1]);
}
reg = parse_ll_bounds(argv[2], 0, 34, &rc);
if (rc != 0) {
return drv2605_shell_err_invalid_arg(argv[2]);
}
value = parse_ll_bounds(argv[3], 0, 255, &rc);
if (rc != 0) {
return drv2605_shell_err_invalid_arg(argv[3]);
}
itf = SENSOR_GET_ITF(&(drv2605->sensor));
rc = drv2605_write8(itf, reg, value);
if (rc) {
console_printf("poke failed %d\n", rc);
}else{
console_printf("wrote: 0x%02X to 0x%02X\n", value, reg);
}
return 0;
}
static int
disable_interrupt(struct sensor * sensor, uint8_t int_to_disable)
{
struct lis2dw12 *lis2dw12;
struct lis2dw12_private_driver_data *pdd;
struct sensor_itf *itf;
int rc;
if (int_to_disable == 0) {
return SYS_EINVAL;
}
lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &lis2dw12->pdd;
pdd->int_enable &= ~int_to_disable;
/* disable int pin */
if (pdd->int_enable == 0) {
hal_gpio_irq_disable(itf->si_ints[pdd->int_num].host_pin);
rc = lis2dw12_set_int_enable(itf, 0);
if (rc) {
return rc;
}
}
/* update interrupt setup in device */
return lis2dw12_set_int1_pin_cfg(itf, pdd->int_enable);
}
static int
drv2605_shell_cmd_dump_all(int argc, char **argv, struct drv2605 *drv2605)
{
int rc;
uint8_t value;
int i;
struct sensor_itf *itf;
if (argc > 3) {
return drv2605_shell_err_too_many_args(argv[1]);
}
itf = SENSOR_GET_ITF(&(drv2605->sensor));
for (i=0; i<=34; i++){
rc = drv2605_read8(itf, i, &value);
if (rc) {
console_printf("dump failed %d\n", rc);
goto err;
}else{
console_printf("reg 0x:%02X = 0x%02X\n", i, value);
}
}
return 0;
err:
return rc;
}
static int
drv2605_shell_cmd_power_mode(int argc, char **argv, struct drv2605 *drv2605)
{
int rc;
enum drv2605_power_mode mode;
struct sensor_itf *itf;
if(strcmp(argv[2],"off") == 0) {
mode = DRV2605_POWER_OFF;
}else if(strcmp(argv[2],"standby") == 0) {
mode = DRV2605_POWER_STANDBY;
}else if(strcmp(argv[2],"active") == 0) {
mode = DRV2605_POWER_ACTIVE;
}else {
return drv2605_shell_err_unknown_arg(argv[2]);
}
itf = SENSOR_GET_ITF(&(drv2605->sensor));
rc = drv2605_set_power_mode(itf, mode);
if (rc) {
console_printf("power_mode failed %d\n", rc);
goto err;
}else{
console_printf("power_mode succeeded\n");
}
return 0;
err:
return rc;
}
/**
* Read Accelerometer data from ADXL345 sensor
*
* @param Pointer to sensor structure
* @param Type of sensor data to read
* @param Pointer to function to call to output data
* @param Pointer to data to pass to data_func
* @param timeout value
*
* @return 0 on success, non-zero on failure
*/
static int
adxl345_sensor_read(struct sensor *sensor, sensor_type_t type,
sensor_data_func_t data_func, void *data_arg, uint32_t timeout)
{
(void)timeout;
int rc;
struct sensor_itf *itf;
struct sensor_accel_data sad;
/* If the read isn't looking for accel don't do anything. */
if (!(type & SENSOR_TYPE_ACCELEROMETER)) {
return SYS_EINVAL;
}
itf = SENSOR_GET_ITF(sensor);
rc = adxl345_get_accel_data(itf, &sad);
if (rc) {
return rc;
}
/* output data using data_func */
rc = data_func(sensor, data_arg, &sad, SENSOR_TYPE_ACCELEROMETER);
if (rc) {
return rc;
}
return 0;
}
static int
tcs34725_sensor_read(struct sensor *sensor, sensor_type_t type,
sensor_data_func_t data_func, void *data_arg, uint32_t timeout)
{
struct tcs34725 *tcs34725;
struct sensor_color_data scd;
struct sensor_itf *itf;
uint16_t r;
uint16_t g;
uint16_t b;
uint16_t c;
int rc;
/* If the read isn't looking for accel or mag data, don't do anything. */
if (!(type & SENSOR_TYPE_COLOR)) {
rc = SYS_EINVAL;
goto err;
}
itf = SENSOR_GET_ITF(sensor);
tcs34725 = (struct tcs34725 *) SENSOR_GET_DEVICE(sensor);
/* Get a new accelerometer sample */
if (type & SENSOR_TYPE_COLOR) {
r = g = b = c = 0;
rc = tcs34725_get_rawdata(itf, &r, &g, &b, &c, tcs34725);
if (rc) {
goto err;
}
scd.scd_r = r;
scd.scd_g = g;
scd.scd_b = b;
scd.scd_c = c;
scd.scd_r_is_valid = 1;
scd.scd_g_is_valid = 1;
scd.scd_b_is_valid = 1;
scd.scd_c_is_valid = 1;
rc = tcs34725_calc_colortemp_lux(itf, &scd, tcs34725);
if (rc) {
goto err;
}
/* Call data function */
rc = data_func(sensor, data_arg, &scd, SENSOR_TYPE_COLOR);
if (rc != 0) {
goto err;
}
}
return 0;
err:
return rc;
}
/**
* Configure the sensor
*
* @param ptr to the sensor
* @param ptr to sensor config
* @return 0 on success, non-zero on failure
*/
int
tcs34725_config(struct tcs34725 *tcs34725, struct tcs34725_cfg *cfg)
{
int rc;
uint8_t id;
struct sensor_itf *itf;
itf = SENSOR_GET_ITF(&(tcs34725->sensor));
rc = tcs34725_get_chip_id(itf, &id);
if (id != TCS34725_ID || rc != 0) {
rc = SYS_EINVAL;
goto err;
}
rc = tcs34725_enable(itf, 1);
if (rc) {
goto err;
}
rc = tcs34725_set_integration_time(itf, cfg->integration_time);
if (rc) {
goto err;
}
tcs34725->cfg.integration_time = cfg->integration_time;
rc = tcs34725_set_gain(itf, cfg->gain);
if (rc) {
goto err;
}
tcs34725->cfg.gain = cfg->gain;
rc = tcs34725_enable_interrupt(itf, cfg->int_enable);
if (rc) {
goto err;
}
tcs34725->cfg.int_enable = cfg->int_enable;
rc = sensor_set_type_mask(&(tcs34725->sensor), cfg->mask);
if (rc) {
goto err;
}
tcs34725->cfg.mask = cfg->mask;
return 0;
err:
return (rc);
}
static int
lis2dw12_sensor_read(struct sensor *sensor, sensor_type_t type,
sensor_data_func_t data_func, void *data_arg, uint32_t timeout)
{
int rc;
const struct lis2dw12_cfg *cfg;
struct lis2dw12 *lis2dw12;
struct sensor_itf *itf;
/* If the read isn't looking for accel data, don't do anything. */
if (!(type & SENSOR_TYPE_ACCELEROMETER)) {
rc = SYS_EINVAL;
goto err;
}
itf = SENSOR_GET_ITF(sensor);
if (itf->si_type == SENSOR_ITF_SPI) {
rc = hal_spi_disable(sensor->s_itf.si_num);
if (rc) {
goto err;
}
rc = hal_spi_config(sensor->s_itf.si_num, &spi_lis2dw12_settings);
if (rc == EINVAL) {
/* If spi is already enabled, for nrf52, it returns -1, We should not
* fail if the spi is already enabled
*/
goto err;
}
rc = hal_spi_enable(sensor->s_itf.si_num);
if (rc) {
goto err;
}
}
lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
cfg = &lis2dw12->cfg;
if (cfg->read_mode == LIS2DW12_READ_M_POLL) {
rc = lis2dw12_poll_read(sensor, type, data_func, data_arg, timeout);
} else {
rc = lis2dw12_stream_read(sensor, type, data_func, data_arg, timeout);
}
err:
return rc;
}
static int
tsl2561_sensor_read(struct sensor *sensor, sensor_type_t type,
sensor_data_func_t data_func, void *data_arg, uint32_t timeout)
{
struct tsl2561 *tsl2561;
struct sensor_light_data sld;
struct sensor_itf *itf;
uint16_t full;
uint16_t ir;
uint32_t lux;
int rc;
/* If the read isn't looking for accel or mag data, don't do anything. */
if (!(type & SENSOR_TYPE_LIGHT)) {
rc = SYS_EINVAL;
goto err;
}
itf = SENSOR_GET_ITF(sensor);
tsl2561 = (struct tsl2561 *)SENSOR_GET_DEVICE(sensor);
/* Get a new accelerometer sample */
if (type & SENSOR_TYPE_LIGHT) {
full = ir = 0;
rc = tsl2561_get_data(itf, &full, &ir);
if (rc) {
goto err;
}
lux = tsl2561_calculate_lux(full, ir, &(tsl2561->cfg));
sld.sld_full = full;
sld.sld_ir = ir;
sld.sld_lux = lux;
sld.sld_full_is_valid = 1;
sld.sld_ir_is_valid = 1;
sld.sld_lux_is_valid = 1;
/* Call data function */
rc = data_func(sensor, data_arg, &sld, SENSOR_TYPE_LIGHT);
if (rc != 0) {
goto err;
}
}
return 0;
err:
return rc;
}
/**
* Handles and interrupt, firing correct events
*
* @param Pointer to sensor structure
*
* @return 0 on success, non-zero on failure
*/
static int
adxl345_sensor_handle_interrupt(struct sensor * sensor)
{
#if MYNEWT_VAL(ADXL345_INT_ENABLE)
struct adxl345 * adxl345;
struct adxl345_private_driver_data *pdd;
struct sensor_itf *itf;
uint8_t int_status;
int rc;
adxl345 = (struct adxl345 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &adxl345->pdd;
rc = adxl345_clear_interrupts(itf, &int_status);
if (rc != 0) {
ADXL345_LOG(ERROR, "Cound not read int status err=0x%02x\n", rc);
return rc;
}
if (pdd->registered_mask & ADXL345_NOTIFY_MASK) {
if (int_status & ADXL345_INT_SINGLE_TAP_BIT) {
sensor_mgr_put_notify_evt(&pdd->notify_ctx, SENSOR_EVENT_TYPE_SINGLE_TAP);
}
if (int_status & ADXL345_INT_DOUBLE_TAP_BIT) {
sensor_mgr_put_notify_evt(&pdd->notify_ctx, SENSOR_EVENT_TYPE_DOUBLE_TAP);
}
}
if ((pdd->registered_mask & ADXL345_READ_MASK) &&
((int_status & ADXL345_INT_ACTIVITY_BIT) ||
(int_status & ADXL345_INT_INACTIVITY_BIT))) {
ADXL345_LOG(ERROR, "READ EVT 0x%02x\n", int_status);
sensor_mgr_put_read_evt(&pdd->read_ctx);
}
return 0;
#else
return SYS_ENODEV;
#endif
}
static int lis2dw12_do_read(struct sensor *sensor, sensor_data_func_t data_func,
void * data_arg)
{
struct sensor_accel_data sad;
struct sensor_itf *itf;
int16_t x, y ,z;
float fx, fy ,fz;
int rc;
itf = SENSOR_GET_ITF(sensor);
x = y = z = 0;
rc = lis2dw12_get_data(itf, &x, &y, &z);
if (rc) {
goto err;
}
/* converting values from mg to ms^2 */
lis2dw12_calc_acc_ms2(x, &fx);
lis2dw12_calc_acc_ms2(y, &fy);
lis2dw12_calc_acc_ms2(z, &fz);
sad.sad_x = fx;
sad.sad_y = fy;
sad.sad_z = fz;
sad.sad_x_is_valid = 1;
sad.sad_y_is_valid = 1;
sad.sad_z_is_valid = 1;
/* Call data function */
rc = data_func(sensor, data_arg, &sad, SENSOR_TYPE_ACCELEROMETER);
if (rc != 0) {
goto err;
}
return 0;
err:
return rc;
}
static int
drv2605_shell_cmd_trigger_rom(int argc, char **argv, struct drv2605 *drv2605)
{
int rc;
struct sensor_itf *itf;
if (argc > 3) {
return drv2605_shell_err_too_many_args(argv[1]);
}
itf = SENSOR_GET_ITF(&(drv2605->sensor));
rc = drv2605_trigger_rom(itf);
if (rc) {
console_printf("trigger failed %d\n", rc);
}else{
console_printf("trigger succeeded\n");
}
return rc;
}
static int
enable_interrupt(struct sensor * sensor, uint8_t int_to_enable)
{
struct lis2dw12 *lis2dw12;
struct lis2dw12_private_driver_data *pdd;
struct sensor_itf *itf;
uint8_t reg;
int rc;
lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &lis2dw12->pdd;
rc = lis2dw12_clear_int(itf, ®);
if (rc) {
return rc;
}
/* if no interrupts are currently in use enable int pin */
if (pdd->int_enable == 0) {
hal_gpio_irq_enable(itf->si_ints[pdd->int_num].host_pin);
rc = lis2dw12_set_int_enable(itf, 1);
if (rc) {
return rc;
}
}
/*update which interrupts are enabled */
pdd->int_enable |= int_to_enable;
/* enable interrupt in device */
rc = lis2dw12_set_int1_pin_cfg(itf, pdd->int_enable);
if (rc) {
return rc;
}
return 0;
}
/**
* Configure the sensor
*
* @param ptr to sensor driver
* @param ptr to sensor driver config
*/
int
tsl2561_config(struct tsl2561 *tsl2561, struct tsl2561_cfg *cfg)
{
int rc;
struct sensor_itf *itf;
itf = SENSOR_GET_ITF(&(tsl2561->sensor));
rc = tsl2561_enable(itf, 1);
if (rc) {
goto err;
}
rc = tsl2561_set_integration_time(itf, cfg->integration_time);
if (rc) {
goto err;
}
tsl2561->cfg.integration_time = cfg->integration_time;
rc = tsl2561_set_gain(itf, cfg->gain);
if (rc) {
goto err;
}
tsl2561->cfg.gain = cfg->gain;
rc = sensor_set_type_mask(&(tsl2561->sensor), cfg->mask);
if (rc) {
goto err;
}
tsl2561->cfg.mask = cfg->mask;
return 0;
err:
return rc;
}
static int
drv2605_shell_cmd_dump_cal(int argc, char **argv, struct drv2605 *drv2605)
{
int rc;
uint8_t tmp[3];
struct sensor_itf *itf;
if (argc > 3) {
return drv2605_shell_err_too_many_args(argv[1]);
}
itf = SENSOR_GET_ITF(&(drv2605->sensor));
rc = drv2605_readlen(itf, DRV2605_AUTO_CALIBRATION_COMPENSATION_RESULT_ADDR, &tmp[0], sizeof(tmp));
if (rc) {
console_printf("dump failed %d\n", rc);
goto err;
}
console_printf("\nDRV2605_CALIBRATED_COMP: 0x%02X\nDRV2605_CALIBRATED_BEMF: 0x%02X\nDRV2605_CALIBRATED_BEMF_GAIN: %0d\n", tmp[0], tmp[1], tmp[2] & 0x03);
return 0;
err:
return rc;
}
/**
* Configure the sensor
*
* @param ptr to sensor driver
* @param ptr to sensor driver config
*/
int
lis2dw12_config(struct lis2dw12 *lis2dw12, struct lis2dw12_cfg *cfg)
{
int rc;
struct sensor_itf *itf;
uint8_t chip_id;
struct sensor *sensor;
itf = SENSOR_GET_ITF(&(lis2dw12->sensor));
sensor = &(lis2dw12->sensor);
if (itf->si_type == SENSOR_ITF_SPI) {
rc = hal_spi_disable(sensor->s_itf.si_num);
if (rc) {
goto err;
}
rc = hal_spi_config(sensor->s_itf.si_num, &spi_lis2dw12_settings);
if (rc == EINVAL) {
/* If spi is already enabled, for nrf52, it returns -1, We should not
* fail if the spi is already enabled
*/
goto err;
}
rc = hal_spi_enable(sensor->s_itf.si_num);
if (rc) {
goto err;
}
}
rc = lis2dw12_get_chip_id(itf, &chip_id);
if (rc) {
goto err;
}
if (chip_id != LIS2DW12_ID) {
rc = SYS_EINVAL;
goto err;
}
rc = lis2dw12_reset(itf);
if (rc) {
goto err;
}
rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG3, LIS2DW12_CTRL_REG3_LIR);
if (rc) {
goto err;
}
rc = lis2dw12_set_offsets(itf, cfg->offset_x, cfg->offset_y, cfg->offset_z,
cfg->offset_weight);
if (rc) {
goto err;
}
lis2dw12->cfg.offset_x = cfg->offset_x;
lis2dw12->cfg.offset_y = cfg->offset_y;
lis2dw12->cfg.offset_z = cfg->offset_z;
lis2dw12->cfg.offset_weight = cfg->offset_weight;
rc = lis2dw12_set_offset_enable(itf, cfg->offset_en);
if (rc) {
goto err;
}
lis2dw12->cfg.offset_en = cfg->offset_en;
rc = lis2dw12_set_filter_cfg(itf, LIS2DW12_FILTER_BW_ODR_DIV_2, 0);
if (rc) {
goto err;
}
rc = lis2dw12_set_full_scale(itf, cfg->fs);
if (rc) {
goto err;
}
lis2dw12->cfg.fs = cfg->fs;
rc = lis2dw12_set_rate(itf, cfg->rate);
if (rc) {
goto err;
}
lis2dw12->cfg.rate = cfg->rate;
rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_DISABLE);
if (rc) {
goto err;
}
rc = lis2dw12_set_power_mode(itf, LIS2DW12_PM_HIGH_PERF);
if (rc) {
goto err;
}
rc = lis2dw12_set_low_noise(itf, 1);
if (rc) {
goto err;
}
rc = lis2dw12_set_fifo_cfg(itf, cfg->fifo_mode, cfg->fifo_threshold);
if (rc) {
goto err;
}
lis2dw12->cfg.fifo_mode = cfg->fifo_mode;
lis2dw12->cfg.fifo_threshold = cfg->fifo_threshold;
rc = lis2dw12_set_int_enable(itf, cfg->int_enable);
if (rc) {
goto err;
}
lis2dw12->cfg.int_enable = cfg->int_enable;
rc = lis2dw12_set_int1_pin_cfg(itf, cfg->int1_pin_cfg);
if (rc) {
goto err;
}
lis2dw12->cfg.int1_pin_cfg = cfg->int1_pin_cfg;
rc = lis2dw12_set_int2_pin_cfg(itf, cfg->int2_pin_cfg);
if (rc) {
goto err;
}
lis2dw12->cfg.int2_pin_cfg = cfg->int2_pin_cfg;
rc = lis2dw12_set_tap_cfg(itf, &cfg->tap_cfg);
if (rc) {
goto err;
}
lis2dw12->cfg.tap_cfg = cfg->tap_cfg;
rc = sensor_set_type_mask(&(lis2dw12->sensor), cfg->mask);
if (rc) {
goto err;
}
lis2dw12->cfg.stream_read_interrupt = cfg->stream_read_interrupt;
lis2dw12->cfg.read_mode = cfg->read_mode;
lis2dw12->cfg.mask = cfg->mask;
return 0;
err:
return rc;
}
int
lis2dw12_stream_read(struct sensor *sensor,
sensor_type_t sensor_type,
sensor_data_func_t read_func,
void *read_arg,
uint32_t time_ms)
{
struct lis2dw12 *lis2dw12;
struct sensor_itf *itf;
int rc;
os_time_t time_ticks;
os_time_t stop_ticks = 0;
struct lis2dw12_private_driver_data *pdd;
uint8_t fifo_samples;
/* If the read isn't looking for accel data, don't do anything. */
if (!(sensor_type & SENSOR_TYPE_ACCELEROMETER)) {
return SYS_EINVAL;
}
lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &lis2dw12->pdd;
undo_interrupt(&lis2dw12->intr);
if (pdd->interrupt) {
return SYS_EBUSY;
}
/* enable interrupt */
pdd->interrupt = &lis2dw12->intr;
rc = enable_interrupt(sensor, lis2dw12->cfg.stream_read_interrupt);
if (rc) {
goto done;
}
if (time_ms != 0) {
rc = os_time_ms_to_ticks(time_ms, &time_ticks);
if (rc) {
goto done;
}
stop_ticks = os_time_get() + time_ticks;
}
for (;;) {
wait_interrupt(&lis2dw12->intr, pdd->int_num);
fifo_samples = 1;
while(fifo_samples > 0) {
rc = lis2dw12_do_read(sensor, read_func, read_arg);
if (rc) {
goto done;
}
rc = lis2dw12_get_fifo_samples(itf, &fifo_samples);
if (rc) {
goto done;
}
}
if (time_ms != 0 && OS_TIME_TICK_GT(os_time_get(), stop_ticks)) {
break;
}
}
done:
/* disable interrupt */
pdd->interrupt = NULL;
rc = disable_interrupt(sensor, lis2dw12->cfg.stream_read_interrupt);
return rc;
}
int
mpu6050_config(struct mpu6050 *mpu, struct mpu6050_cfg *cfg)
{
int rc;
struct sensor_itf *itf;
itf = SENSOR_GET_ITF(&(mpu->sensor));
/* Wake up */
rc = mpu6050_sleep(itf, 0);
if (rc) {
return rc;
}
rc = mpu6050_set_clock_source(itf, cfg->clock_source);
if (rc) {
return rc;
}
mpu->cfg.clock_source = cfg->clock_source;
uint8_t val;
rc = mpu6050_read8(itf, MPU6050_WHO_AM_I, &val);
if (rc) {
return rc;
}
if (val != MPU6050_WHO_AM_I_VAL) {
return SYS_EINVAL;
}
rc = mpu6050_set_lpf(itf, cfg->lpf_cfg);
if (rc) {
return rc;
}
mpu->cfg.lpf_cfg = cfg->lpf_cfg;
rc = mpu6050_set_sample_rate(itf, cfg->sample_rate_div);
if (rc) {
return rc;
}
mpu->cfg.sample_rate_div = cfg->sample_rate_div;
rc = mpu6050_set_gyro_range(itf, cfg->gyro_range);
if (rc) {
return rc;
}
mpu->cfg.gyro_range = cfg->gyro_range;
rc = mpu6050_set_accel_range(itf, cfg->accel_range);
if (rc) {
return rc;
}
mpu->cfg.accel_range = cfg->accel_range;
rc = mpu6050_config_interrupt(itf, cfg->int_cfg);
if (rc) {
return rc;
}
mpu->cfg.int_cfg = cfg->int_cfg;
/* Enable/disable interrupt */
rc = mpu6050_enable_interrupt(itf, cfg->int_enable);
if (rc) {
return rc;
}
mpu->cfg.int_enable = cfg->int_enable;
rc = sensor_set_type_mask(&(mpu->sensor), cfg->mask);
if (rc) {
return rc;
}
mpu->cfg.mask = cfg->mask;
return 0;
}
/**
* Sets up trigger thresholds and enables interrupts
*
* @param Pointer to sensor structure
* @param type of sensor
* @param threshold settings to configure
*
* @return 0 on success, non-zero on failure
*/
static int
adxl345_sensor_set_trigger_thresh(struct sensor * sensor,
sensor_type_t sensor_type,
struct sensor_type_traits * stt)
{
#if MYNEWT_VAL(ADXL345_INT_ENABLE)
struct adxl345 * adxl345;
struct sensor_itf *itf;
int rc;
const struct sensor_accel_data * low_thresh;
const struct sensor_accel_data * high_thresh;
uint8_t ints_to_enable = 0;
float thresh;
struct adxl345_private_driver_data *pdd;
struct adxl345_act_inact_enables axis_enables = { 0 };
if (sensor_type != SENSOR_TYPE_ACCELEROMETER) {
return SYS_EINVAL;
}
adxl345 = (struct adxl345 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &adxl345->pdd;
low_thresh = stt->stt_low_thresh.sad;
high_thresh = stt->stt_high_thresh.sad;
if (low_thresh->sad_x_is_valid |
low_thresh->sad_y_is_valid |
low_thresh->sad_z_is_valid) {
thresh = INFINITY;
if (low_thresh->sad_x_is_valid) {
axis_enables.inact_x = 1;
if (thresh > low_thresh->sad_x) {
thresh = low_thresh->sad_x;
}
}
if (low_thresh->sad_y_is_valid) {
axis_enables.inact_y = 1;
if (thresh > low_thresh->sad_y) {
thresh = low_thresh->sad_y;
}
}
if (low_thresh->sad_z_is_valid) {
axis_enables.inact_z = 1;
if (thresh > low_thresh->sad_z) {
thresh = low_thresh->sad_z;
}
}
rc = adxl345_set_inactive_settings(itf,
adxl345_convert_ms2_to_reg(thresh, 62.5), 2);
if (rc) {
return rc;
}
ints_to_enable |= ADXL345_INT_INACTIVITY_BIT;
}
if (high_thresh->sad_x_is_valid |
high_thresh->sad_y_is_valid |
high_thresh->sad_z_is_valid) {
thresh = 0.0;
if (high_thresh->sad_x_is_valid) {
axis_enables.act_x = 1;
if (thresh < high_thresh->sad_x) {
thresh = high_thresh->sad_x;
}
}
if (high_thresh->sad_y_is_valid) {
axis_enables.act_y = 1;
if (thresh < high_thresh->sad_y) {
thresh = high_thresh->sad_y;
}
}
if (high_thresh->sad_z_is_valid) {
axis_enables.act_z = 1;
if (thresh < high_thresh->sad_z) {
thresh = high_thresh->sad_z;
}
}
rc = adxl345_set_active_threshold(itf,
adxl345_convert_ms2_to_reg(thresh, 62.5));
if (rc) {
return rc;
}
ints_to_enable |= ADXL345_INT_ACTIVITY_BIT;
}
rc = enable_interrupt(sensor, ints_to_enable);
if (rc) {
return rc;
}
rc = adxl345_set_act_inact_enables(itf, axis_enables);
if (rc) {
return rc;
}
pdd->read_ctx.srec_type |= sensor_type;
pdd->registered_mask |= ADXL345_READ_MASK;
return 0;
#else
return SYS_ENODEV;
#endif
}
static int
mpu6050_sensor_read(struct sensor *sensor, sensor_type_t type,
sensor_data_func_t data_func, void *data_arg, uint32_t timeout)
{
(void)timeout;
int rc;
int16_t x, y, z;
uint8_t payload[6];
float lsb;
struct sensor_itf *itf;
struct mpu6050 *mpu;
union {
struct sensor_accel_data sad;
struct sensor_gyro_data sgd;
} databuf;
/* If the read isn't looking for accel or gyro, don't do anything. */
if (!(type & SENSOR_TYPE_ACCELEROMETER) &&
(!(type & SENSOR_TYPE_GYROSCOPE))) {
return SYS_EINVAL;
}
itf = SENSOR_GET_ITF(sensor);
mpu = (struct mpu6050 *) SENSOR_GET_DEVICE(sensor);
/* Get a new accelerometer sample */
if (type & SENSOR_TYPE_ACCELEROMETER) {
rc = mpu6050_read48(itf, MPU6050_ACCEL_XOUT_H, payload);
if (rc) {
return rc;
}
x = (((int16_t)payload[0]) << 8) | payload[1];
y = (((int16_t)payload[2]) << 8) | payload[3];
z = (((int16_t)payload[4]) << 8) | payload[5];
switch (mpu->cfg.accel_range) {
case MPU6050_ACCEL_RANGE_2: /* +/- 2g - 16384 LSB/g */
/* Falls through */
default:
lsb = 16384.0F;
break;
case MPU6050_ACCEL_RANGE_4: /* +/- 4g - 8192 LSB/g */
lsb = 8192.0F;
break;
case MPU6050_ACCEL_RANGE_8: /* +/- 8g - 4096 LSB/g */
lsb = 4096.0F;
break;
case MPU6050_ACCEL_RANGE_16: /* +/- 16g - 2048 LSB/g */
lsb = 2048.0F;
break;
}
databuf.sad.sad_x = (x / lsb) * STANDARD_ACCEL_GRAVITY;
databuf.sad.sad_x_is_valid = 1;
databuf.sad.sad_y = (y / lsb) * STANDARD_ACCEL_GRAVITY;
databuf.sad.sad_y_is_valid = 1;
databuf.sad.sad_z = (z / lsb) * STANDARD_ACCEL_GRAVITY;
databuf.sad.sad_z_is_valid = 1;
rc = data_func(sensor, data_arg, &databuf.sad,
SENSOR_TYPE_ACCELEROMETER);
if (rc) {
return rc;
}
}
/* Get a new gyroscope sample */
if (type & SENSOR_TYPE_GYROSCOPE) {
rc = mpu6050_read48(itf, MPU6050_GYRO_XOUT_H, payload);
if (rc) {
return rc;
}
x = (((int16_t)payload[0]) << 8) | payload[1];
y = (((int16_t)payload[2]) << 8) | payload[3];
z = (((int16_t)payload[4]) << 8) | payload[5];
switch (mpu->cfg.gyro_range) {
case MPU6050_GYRO_RANGE_250: /* +/- 250 Deg/s - 131 LSB/Deg/s */
/* Falls through */
default:
lsb = 131.0F;
break;
case MPU6050_GYRO_RANGE_500: /* +/- 500 Deg/s - 65.5 LSB/Deg/s */
lsb = 65.5F;
break;
case MPU6050_GYRO_RANGE_1000: /* +/- 1000 Deg/s - 32.8 LSB/Deg/s */
lsb = 32.8F;
break;
case MPU6050_GYRO_RANGE_2000: /* +/- 2000 Deg/s - 16.4 LSB/Deg/s */
lsb = 16.4F;
break;
}
databuf.sgd.sgd_x = x / lsb;
databuf.sgd.sgd_x_is_valid = 1;
databuf.sgd.sgd_y = y / lsb;
databuf.sgd.sgd_y_is_valid = 1;
databuf.sgd.sgd_z = z / lsb;
databuf.sgd.sgd_z_is_valid = 1;
rc = data_func(sensor, data_arg, &databuf.sgd, SENSOR_TYPE_GYROSCOPE);
if (rc) {
return rc;
}
}
return 0;
}
/**
* Configure ADXL345 sensor
*
* @param Sensor device ADXL345 structure
* @param Sensor device ADXL345 config
*
* @return 0 on success, non-zero on failure
*/
int
adxl345_config(struct adxl345 *dev, struct adxl345_cfg *cfg)
{
int rc;
struct sensor_itf *itf;
itf = SENSOR_GET_ITF(&(dev->sensor));
/* Check device id is correct */
uint8_t val = 0;
rc = adxl345_read8(itf, ADXL345_DEVID, &val);
if (rc) {
return rc;
}
if (val != ADXL345_DEVID_VAL) {
return SYS_EINVAL;
}
rc = adxl345_read8(itf, ADXL345_DATA_FORMAT, &val);
if (rc) {
return rc;
}
/* Set accelerometer into full resolution */
val |= 0x08;
/* Set interupt pin polarity to match local pin setting */
if (dev->sensor.s_itf.si_ints[dev->pdd.int_num].active) {
val &= ~0x20;
} else {
val |= 0x20;
}
rc = adxl345_write8(itf, ADXL345_DATA_FORMAT, val);
if (rc) {
return rc;
}
/* Setup range as per config */
rc = adxl345_set_accel_range(itf, cfg->accel_range);
if (rc) {
return rc;
}
dev->cfg.accel_range = cfg->accel_range;
/* setup sample rate */
rc = adxl345_set_sample_rate(itf, cfg->sample_rate);
if (rc) {
return rc;
}
dev->cfg.sample_rate = cfg->sample_rate;
/* setup data offsets */
rc = adxl345_set_offsets(itf, cfg->offset_x, cfg->offset_y, cfg->offset_z);
if (rc) {
return rc;
}
dev->cfg.offset_x = cfg->offset_x;
dev->cfg.offset_y = cfg->offset_y;
dev->cfg.offset_z = cfg->offset_z;
/* setup tap detection settings */
rc = adxl345_set_tap_settings(itf, cfg->tap_cfg);
if (rc) {
return rc;
}
dev->cfg.tap_cfg = cfg->tap_cfg;
/* setup activity detection settings */
rc = adxl345_set_active_threshold(itf, 0xFF);
if (rc) {
return rc;
}
rc = adxl345_set_inactive_settings(itf, 0, 0);
if (rc) {
return rc;
}
/* setup freefall settings */
rc = adxl345_set_freefall_settings(itf, cfg->freefall_threshold, cfg->freefall_time);
if (rc) {
return rc;
}
dev->cfg.freefall_threshold = cfg->freefall_threshold;
dev->cfg.freefall_time = cfg->freefall_time;
/* setup low power mode */
rc = adxl345_set_low_power_enable(itf, cfg->low_power_enable);
if (rc) {
return rc;
}
dev->cfg.low_power_enable = cfg->low_power_enable;
/* setup default interrupt config */
rc = adxl345_setup_interrupts(itf, 0, 0);
if (rc) {
return rc;
}
rc = adxl345_clear_interrupts(itf, &val);
if (rc) {
return rc;
}
/* Setup current power mode */
rc = adxl345_set_power_mode(itf, cfg->power_mode);
if (rc) {
return rc;
}
dev->cfg.power_mode = cfg->power_mode;
rc = sensor_set_type_mask(&(dev->sensor), cfg->mask);
if (rc) {
return rc;
}
dev->cfg.mask = cfg->mask;
return 0;
}
