/**
* Expects to be called back through os_dev_create().
*
* @param The device object associated with this color sensor
* @param Argument passed to OS device init, unused
*
* @return 0 on success, non-zero error on failure.
*/
int
tcs34725_init(struct os_dev *dev, void *arg)
{
struct tcs34725 *tcs34725;
struct sensor *sensor;
int rc;
if (!arg || !dev) {
rc = SYS_ENODEV;
goto err;
}
tcs34725 = (struct tcs34725 *) dev;
tcs34725->cfg.mask = SENSOR_TYPE_ALL;
sensor = &tcs34725->sensor;
/* Initialise the stats entry */
rc = stats_init(
STATS_HDR(g_tcs34725stats),
STATS_SIZE_INIT_PARMS(g_tcs34725stats, STATS_SIZE_32),
STATS_NAME_INIT_PARMS(tcs34725_stat_section));
SYSINIT_PANIC_ASSERT(rc == 0);
/* Register the entry with the stats registry */
rc = stats_register("tcs34725", STATS_HDR(g_tcs34725stats));
SYSINIT_PANIC_ASSERT(rc == 0);
rc = sensor_init(sensor, dev);
if (rc != 0) {
goto err;
}
/* Add the color sensor driver */
rc = sensor_set_driver(sensor, SENSOR_TYPE_COLOR,
(struct sensor_driver *) &g_tcs34725_sensor_driver);
if (rc != 0) {
goto err;
}
/* Set the interface */
rc = sensor_set_interface(sensor, arg);
if (rc) {
goto err;
}
rc = sensor_mgr_register(sensor);
if (rc != 0) {
goto err;
}
rc = sensor_set_type_mask(sensor, tcs34725->cfg.mask);
if (rc) {
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);
}
/**
* 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;
}
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;
}
/**
* 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;
}
/**
* 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;
}
