ST
iNEMO inertial module: 3D accelerometer and 3D gyroscope.
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Dependents: X_NUCLEO_IKS01A3 X_NUCLEO_IKS01A3
Diff: LSM6DSOSensor.cpp
- Revision:
- 3:4274d9103f1d
- Parent:
- 2:4d14e9edf37e
- Child:
- 4:77faf76e3cd8
diff -r 4d14e9edf37e -r 4274d9103f1d LSM6DSOSensor.cpp
--- a/LSM6DSOSensor.cpp Mon Jun 03 09:16:08 2019 +0000
+++ b/LSM6DSOSensor.cpp Wed Jul 24 14:19:35 2019 +0000
@@ -53,28 +53,26 @@
*/
LSM6DSOSensor::LSM6DSOSensor(SPI *spi, PinName cs_pin, PinName int1_pin, PinName int2_pin, SPI_type_t spi_type) : _dev_spi(spi), _cs_pin(cs_pin), _int1_irq(int1_pin), _int2_irq(int2_pin), _spi_type(spi_type)
{
- assert (spi);
- if (cs_pin == NC)
- {
- printf ("ERROR LSM6DSO CS MUST NOT BE NC\n\r");
- _dev_spi = NULL;
+ assert(spi);
+ if (cs_pin == NC) {
+ printf("ERROR LSM6DSO CS MUST NOT BE NC\n\r");
+ _dev_spi = NULL;
+ _dev_i2c = NULL;
+ return;
+ }
+
+ _reg_ctx.write_reg = LSM6DSO_io_write;
+ _reg_ctx.read_reg = LSM6DSO_io_read;
+ _reg_ctx.handle = (void *)this;
+ _cs_pin = 1;
_dev_i2c = NULL;
- return;
- }
-
- _reg_ctx.write_reg = LSM6DSO_io_write;
- _reg_ctx.read_reg = LSM6DSO_io_read;
- _reg_ctx.handle = (void *)this;
- _cs_pin = 1;
- _dev_i2c = NULL;
- _address = 0;
-
- if (_spi_type == SPI3W)
- {
- /* Enable SPI 3-Wires on the component */
- uint8_t data = 0x0C;
- lsm6dso_write_reg(&_reg_ctx, LSM6DSO_CTRL3_C, &data, 1);
- }
+ _address = 0;
+
+ if (_spi_type == SPI3W) {
+ /* Enable SPI 3-Wires on the component */
+ uint8_t data = 0x0C;
+ lsm6dso_write_reg(&_reg_ctx, LSM6DSO_CTRL3_C, &data, 1);
+ }
}
@@ -86,11 +84,11 @@
*/
LSM6DSOSensor::LSM6DSOSensor(DevI2C *i2c, uint8_t address, PinName int1_pin, PinName int2_pin) : _dev_i2c(i2c), _address(address), _cs_pin(NC), _int1_irq(int1_pin), _int2_irq(int2_pin)
{
- assert (i2c);
- _dev_spi = NULL;
- _reg_ctx.write_reg = LSM6DSO_io_write;
- _reg_ctx.read_reg = LSM6DSO_io_read;
- _reg_ctx.handle = (void *)this;
+ assert(i2c);
+ _dev_spi = NULL;
+ _reg_ctx.write_reg = LSM6DSO_io_write;
+ _reg_ctx.read_reg = LSM6DSO_io_read;
+ _reg_ctx.handle = (void *)this;
}
/**
@@ -100,66 +98,58 @@
*/
int LSM6DSOSensor::init(void *init)
{
- /* Disable I3C */
- if (lsm6dso_i3c_disable_set(&_reg_ctx, LSM6DSO_I3C_DISABLE) != 0)
- {
- return 1;
- }
-
- /* Enable register address automatically incremented during a multiple byte
- access with a serial interface. */
- if (lsm6dso_auto_increment_set(&_reg_ctx, PROPERTY_ENABLE) != 0)
- {
- return 1;
- }
-
- /* Enable BDU */
- if (lsm6dso_block_data_update_set(&_reg_ctx, PROPERTY_ENABLE) != 0)
- {
- return 1;
- }
-
- /* FIFO mode selection */
- if (lsm6dso_fifo_mode_set(&_reg_ctx, LSM6DSO_BYPASS_MODE) != 0)
- {
- return 1;
- }
-
- /* Output data rate selection - power down. */
- if (lsm6dso_xl_data_rate_set(&_reg_ctx, LSM6DSO_XL_ODR_OFF) != 0)
- {
- return 1;
- }
-
- /* Full scale selection. */
- if (lsm6dso_xl_full_scale_set(&_reg_ctx, LSM6DSO_2g) != 0)
- {
- return 1;
- }
-
- /* Output data rate selection - power down. */
- if (lsm6dso_gy_data_rate_set(&_reg_ctx, LSM6DSO_GY_ODR_OFF) != 0)
- {
- return 1;
- }
-
- /* Full scale selection. */
- if (lsm6dso_gy_full_scale_set(&_reg_ctx, LSM6DSO_2000dps) != 0)
- {
- return 1;
- }
-
- /* Select default output data rate. */
- _x_last_odr = LSM6DSO_XL_ODR_104Hz;
-
- /* Select default output data rate. */
- _g_last_odr = LSM6DSO_GY_ODR_104Hz;
-
- _x_is_enabled = 0;
-
- _g_is_enabled = 0;
-
- return 0;
+ /* Disable I3C */
+ if (lsm6dso_i3c_disable_set(&_reg_ctx, LSM6DSO_I3C_DISABLE) != 0) {
+ return 1;
+ }
+
+ /* Enable register address automatically incremented during a multiple byte
+ access with a serial interface. */
+ if (lsm6dso_auto_increment_set(&_reg_ctx, PROPERTY_ENABLE) != 0) {
+ return 1;
+ }
+
+ /* Enable BDU */
+ if (lsm6dso_block_data_update_set(&_reg_ctx, PROPERTY_ENABLE) != 0) {
+ return 1;
+ }
+
+ /* FIFO mode selection */
+ if (lsm6dso_fifo_mode_set(&_reg_ctx, LSM6DSO_BYPASS_MODE) != 0) {
+ return 1;
+ }
+
+ /* Output data rate selection - power down. */
+ if (lsm6dso_xl_data_rate_set(&_reg_ctx, LSM6DSO_XL_ODR_OFF) != 0) {
+ return 1;
+ }
+
+ /* Full scale selection. */
+ if (lsm6dso_xl_full_scale_set(&_reg_ctx, LSM6DSO_2g) != 0) {
+ return 1;
+ }
+
+ /* Output data rate selection - power down. */
+ if (lsm6dso_gy_data_rate_set(&_reg_ctx, LSM6DSO_GY_ODR_OFF) != 0) {
+ return 1;
+ }
+
+ /* Full scale selection. */
+ if (lsm6dso_gy_full_scale_set(&_reg_ctx, LSM6DSO_2000dps) != 0) {
+ return 1;
+ }
+
+ /* Select default output data rate. */
+ _x_last_odr = LSM6DSO_XL_ODR_104Hz;
+
+ /* Select default output data rate. */
+ _g_last_odr = LSM6DSO_GY_ODR_104Hz;
+
+ _x_is_enabled = 0;
+
+ _g_is_enabled = 0;
+
+ return 0;
}
/**
@@ -169,12 +159,11 @@
*/
int LSM6DSOSensor::read_id(uint8_t *id)
{
- if (lsm6dso_device_id_get(&_reg_ctx, id) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_device_id_get(&_reg_ctx, id) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -183,21 +172,19 @@
*/
int LSM6DSOSensor::enable_x()
{
- /* Check if the component is already enabled */
- if (_x_is_enabled == 1U)
- {
+ /* Check if the component is already enabled */
+ if (_x_is_enabled == 1U) {
+ return 0;
+ }
+
+ /* Output data rate selection. */
+ if (lsm6dso_xl_data_rate_set(&_reg_ctx, _x_last_odr) != 0) {
+ return 1;
+ }
+
+ _x_is_enabled = 1;
+
return 0;
- }
-
- /* Output data rate selection. */
- if (lsm6dso_xl_data_rate_set(&_reg_ctx, _x_last_odr) != 0)
- {
- return 1;
- }
-
- _x_is_enabled = 1;
-
- return 0;
}
/**
@@ -206,27 +193,24 @@
*/
int LSM6DSOSensor::disable_x()
{
- /* Check if the component is already disabled */
- if (_x_is_enabled == 0U)
- {
+ /* Check if the component is already disabled */
+ if (_x_is_enabled == 0U) {
+ return 0;
+ }
+
+ /* Get current output data rate. */
+ if (lsm6dso_xl_data_rate_get(&_reg_ctx, &_x_last_odr) != 0) {
+ return 1;
+ }
+
+ /* Output data rate selection - power down. */
+ if (lsm6dso_xl_data_rate_set(&_reg_ctx, LSM6DSO_XL_ODR_OFF) != 0) {
+ return 1;
+ }
+
+ _x_is_enabled = 0;
+
return 0;
- }
-
- /* Get current output data rate. */
- if (lsm6dso_xl_data_rate_get(&_reg_ctx, &_x_last_odr) != 0)
- {
- return 1;
- }
-
- /* Output data rate selection - power down. */
- if (lsm6dso_xl_data_rate_set(&_reg_ctx, LSM6DSO_XL_ODR_OFF) != 0)
- {
- return 1;
- }
-
- _x_is_enabled = 0;
-
- return 0;
}
/**
@@ -236,40 +220,38 @@
*/
int LSM6DSOSensor::get_x_sensitivity(float *sensitivity)
{
- int ret = 0;
- lsm6dso_fs_xl_t full_scale;
-
- /* Read actual full scale selection from sensor. */
- if (lsm6dso_xl_full_scale_get(&_reg_ctx, &full_scale) != 0)
- {
- return 1;
- }
-
- /* Store the sensitivity based on actual full scale. */
- switch (full_scale)
- {
- case LSM6DSO_2g:
- *sensitivity = LSM6DSO_ACC_SENSITIVITY_FS_2G;
- break;
-
- case LSM6DSO_4g:
- *sensitivity = LSM6DSO_ACC_SENSITIVITY_FS_4G;
- break;
-
- case LSM6DSO_8g:
- *sensitivity = LSM6DSO_ACC_SENSITIVITY_FS_8G;
- break;
-
- case LSM6DSO_16g:
- *sensitivity = LSM6DSO_ACC_SENSITIVITY_FS_16G;
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ int ret = 0;
+ lsm6dso_fs_xl_t full_scale;
+
+ /* Read actual full scale selection from sensor. */
+ if (lsm6dso_xl_full_scale_get(&_reg_ctx, &full_scale) != 0) {
+ return 1;
+ }
+
+ /* Store the sensitivity based on actual full scale. */
+ switch (full_scale) {
+ case LSM6DSO_2g:
+ *sensitivity = LSM6DSO_ACC_SENSITIVITY_FS_2G;
+ break;
+
+ case LSM6DSO_4g:
+ *sensitivity = LSM6DSO_ACC_SENSITIVITY_FS_4G;
+ break;
+
+ case LSM6DSO_8g:
+ *sensitivity = LSM6DSO_ACC_SENSITIVITY_FS_8G;
+ break;
+
+ case LSM6DSO_16g:
+ *sensitivity = LSM6DSO_ACC_SENSITIVITY_FS_16G;
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -279,71 +261,69 @@
*/
int LSM6DSOSensor::get_x_odr(float *odr)
{
- int ret = 0;
- lsm6dso_odr_xl_t odr_low_level;
-
- /* Get current output data rate. */
- if (lsm6dso_xl_data_rate_get(&_reg_ctx, &odr_low_level) != 0)
- {
- return 1;
- }
-
- switch (odr_low_level)
- {
- case LSM6DSO_XL_ODR_OFF:
- *odr = 0.0f;
- break;
-
- case LSM6DSO_XL_ODR_6Hz5:
- *odr = 6.5f;
- break;
-
- case LSM6DSO_XL_ODR_12Hz5:
- *odr = 12.5f;
- break;
-
- case LSM6DSO_XL_ODR_26Hz:
- *odr = 26.0f;
- break;
-
- case LSM6DSO_XL_ODR_52Hz:
- *odr = 52.0f;
- break;
-
- case LSM6DSO_XL_ODR_104Hz:
- *odr = 104.0f;
- break;
-
- case LSM6DSO_XL_ODR_208Hz:
- *odr = 208.0f;
- break;
-
- case LSM6DSO_XL_ODR_417Hz:
- *odr = 417.0f;
- break;
-
- case LSM6DSO_XL_ODR_833Hz:
- *odr = 833.0f;
- break;
-
- case LSM6DSO_XL_ODR_1667Hz:
- *odr = 1667.0f;
- break;
-
- case LSM6DSO_XL_ODR_3333Hz:
- *odr = 3333.0f;
- break;
-
- case LSM6DSO_XL_ODR_6667Hz:
- *odr = 6667.0f;
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ int ret = 0;
+ lsm6dso_odr_xl_t odr_low_level;
+
+ /* Get current output data rate. */
+ if (lsm6dso_xl_data_rate_get(&_reg_ctx, &odr_low_level) != 0) {
+ return 1;
+ }
+
+ switch (odr_low_level) {
+ case LSM6DSO_XL_ODR_OFF:
+ *odr = 0.0f;
+ break;
+
+ case LSM6DSO_XL_ODR_6Hz5:
+ *odr = 6.5f;
+ break;
+
+ case LSM6DSO_XL_ODR_12Hz5:
+ *odr = 12.5f;
+ break;
+
+ case LSM6DSO_XL_ODR_26Hz:
+ *odr = 26.0f;
+ break;
+
+ case LSM6DSO_XL_ODR_52Hz:
+ *odr = 52.0f;
+ break;
+
+ case LSM6DSO_XL_ODR_104Hz:
+ *odr = 104.0f;
+ break;
+
+ case LSM6DSO_XL_ODR_208Hz:
+ *odr = 208.0f;
+ break;
+
+ case LSM6DSO_XL_ODR_417Hz:
+ *odr = 417.0f;
+ break;
+
+ case LSM6DSO_XL_ODR_833Hz:
+ *odr = 833.0f;
+ break;
+
+ case LSM6DSO_XL_ODR_1667Hz:
+ *odr = 1667.0f;
+ break;
+
+ case LSM6DSO_XL_ODR_3333Hz:
+ *odr = 3333.0f;
+ break;
+
+ case LSM6DSO_XL_ODR_6667Hz:
+ *odr = 6667.0f;
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -353,15 +333,12 @@
*/
int LSM6DSOSensor::set_x_odr(float odr)
{
- /* Check if the component is enabled */
- if (_x_is_enabled == 1U)
- {
- return set_x_odr_when_enabled(odr);
- }
- else
- {
- return set_x_odr_when_disabled(odr);
- }
+ /* Check if the component is enabled */
+ if (_x_is_enabled == 1U) {
+ return set_x_odr_when_enabled(odr);
+ } else {
+ return set_x_odr_when_disabled(odr);
+ }
}
/**
@@ -371,26 +348,25 @@
*/
int LSM6DSOSensor::set_x_odr_when_enabled(float odr)
{
- lsm6dso_odr_xl_t new_odr;
-
- new_odr = (odr <= 12.5f) ? LSM6DSO_XL_ODR_12Hz5
- : (odr <= 26.0f) ? LSM6DSO_XL_ODR_26Hz
- : (odr <= 52.0f) ? LSM6DSO_XL_ODR_52Hz
- : (odr <= 104.0f) ? LSM6DSO_XL_ODR_104Hz
- : (odr <= 208.0f) ? LSM6DSO_XL_ODR_208Hz
- : (odr <= 417.0f) ? LSM6DSO_XL_ODR_417Hz
- : (odr <= 833.0f) ? LSM6DSO_XL_ODR_833Hz
- : (odr <= 1667.0f) ? LSM6DSO_XL_ODR_1667Hz
- : (odr <= 3333.0f) ? LSM6DSO_XL_ODR_3333Hz
- : LSM6DSO_XL_ODR_6667Hz;
-
- /* Output data rate selection. */
- if (lsm6dso_xl_data_rate_set(&_reg_ctx, new_odr) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_odr_xl_t new_odr;
+
+ new_odr = (odr <= 12.5f) ? LSM6DSO_XL_ODR_12Hz5
+ : (odr <= 26.0f) ? LSM6DSO_XL_ODR_26Hz
+ : (odr <= 52.0f) ? LSM6DSO_XL_ODR_52Hz
+ : (odr <= 104.0f) ? LSM6DSO_XL_ODR_104Hz
+ : (odr <= 208.0f) ? LSM6DSO_XL_ODR_208Hz
+ : (odr <= 417.0f) ? LSM6DSO_XL_ODR_417Hz
+ : (odr <= 833.0f) ? LSM6DSO_XL_ODR_833Hz
+ : (odr <= 1667.0f) ? LSM6DSO_XL_ODR_1667Hz
+ : (odr <= 3333.0f) ? LSM6DSO_XL_ODR_3333Hz
+ : LSM6DSO_XL_ODR_6667Hz;
+
+ /* Output data rate selection. */
+ if (lsm6dso_xl_data_rate_set(&_reg_ctx, new_odr) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -400,18 +376,18 @@
*/
int LSM6DSOSensor::set_x_odr_when_disabled(float odr)
{
- _x_last_odr = (odr <= 12.5f) ? LSM6DSO_XL_ODR_12Hz5
- : (odr <= 26.0f) ? LSM6DSO_XL_ODR_26Hz
- : (odr <= 52.0f) ? LSM6DSO_XL_ODR_52Hz
- : (odr <= 104.0f) ? LSM6DSO_XL_ODR_104Hz
- : (odr <= 208.0f) ? LSM6DSO_XL_ODR_208Hz
- : (odr <= 417.0f) ? LSM6DSO_XL_ODR_417Hz
- : (odr <= 833.0f) ? LSM6DSO_XL_ODR_833Hz
- : (odr <= 1667.0f) ? LSM6DSO_XL_ODR_1667Hz
- : (odr <= 3333.0f) ? LSM6DSO_XL_ODR_3333Hz
- : LSM6DSO_XL_ODR_6667Hz;
-
- return 0;
+ _x_last_odr = (odr <= 12.5f) ? LSM6DSO_XL_ODR_12Hz5
+ : (odr <= 26.0f) ? LSM6DSO_XL_ODR_26Hz
+ : (odr <= 52.0f) ? LSM6DSO_XL_ODR_52Hz
+ : (odr <= 104.0f) ? LSM6DSO_XL_ODR_104Hz
+ : (odr <= 208.0f) ? LSM6DSO_XL_ODR_208Hz
+ : (odr <= 417.0f) ? LSM6DSO_XL_ODR_417Hz
+ : (odr <= 833.0f) ? LSM6DSO_XL_ODR_833Hz
+ : (odr <= 1667.0f) ? LSM6DSO_XL_ODR_1667Hz
+ : (odr <= 3333.0f) ? LSM6DSO_XL_ODR_3333Hz
+ : LSM6DSO_XL_ODR_6667Hz;
+
+ return 0;
}
@@ -422,39 +398,37 @@
*/
int LSM6DSOSensor::get_x_fs(float *full_scale)
{
- int ret = 0;
- lsm6dso_fs_xl_t fs_low_level;
-
- /* Read actual full scale selection from sensor. */
- if (lsm6dso_xl_full_scale_get(&_reg_ctx, &fs_low_level) != 0)
- {
- return 1;
- }
-
- switch (fs_low_level)
- {
- case LSM6DSO_2g:
- *full_scale = 2.0f;
- break;
-
- case LSM6DSO_4g:
- *full_scale = 4.0f;
- break;
-
- case LSM6DSO_8g:
- *full_scale = 8.0f;
- break;
-
- case LSM6DSO_16g:
- *full_scale = 16.0f;
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ int ret = 0;
+ lsm6dso_fs_xl_t fs_low_level;
+
+ /* Read actual full scale selection from sensor. */
+ if (lsm6dso_xl_full_scale_get(&_reg_ctx, &fs_low_level) != 0) {
+ return 1;
+ }
+
+ switch (fs_low_level) {
+ case LSM6DSO_2g:
+ *full_scale = 2.0f;
+ break;
+
+ case LSM6DSO_4g:
+ *full_scale = 4.0f;
+ break;
+
+ case LSM6DSO_8g:
+ *full_scale = 8.0f;
+ break;
+
+ case LSM6DSO_16g:
+ *full_scale = 16.0f;
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -464,21 +438,20 @@
*/
int LSM6DSOSensor::set_x_fs(float full_scale)
{
- lsm6dso_fs_xl_t new_fs;
-
- /* Seems like MISRA C-2012 rule 14.3a violation but only from single file statical analysis point of view because
- the parameter passed to the function is not known at the moment of analysis */
- new_fs = (full_scale <= 2.0f) ? LSM6DSO_2g
- : (full_scale <= 4.0f) ? LSM6DSO_4g
- : (full_scale <= 8.0f) ? LSM6DSO_8g
- : LSM6DSO_16g;
-
- if (lsm6dso_xl_full_scale_set(&_reg_ctx, new_fs) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_fs_xl_t new_fs;
+
+ /* Seems like MISRA C-2012 rule 14.3a violation but only from single file statical analysis point of view because
+ the parameter passed to the function is not known at the moment of analysis */
+ new_fs = (full_scale <= 2.0f) ? LSM6DSO_2g
+ : (full_scale <= 4.0f) ? LSM6DSO_4g
+ : (full_scale <= 8.0f) ? LSM6DSO_8g
+ : LSM6DSO_16g;
+
+ if (lsm6dso_xl_full_scale_set(&_reg_ctx, new_fs) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -488,20 +461,19 @@
*/
int LSM6DSOSensor::get_x_axes_raw(int16_t *value)
{
- axis3bit16_t data_raw;
-
- /* Read raw data values. */
- if (lsm6dso_acceleration_raw_get(&_reg_ctx, data_raw.u8bit) != 0)
- {
- return 1;
- }
-
- /* Format the data. */
- value[0] = data_raw.i16bit[0];
- value[1] = data_raw.i16bit[1];
- value[2] = data_raw.i16bit[2];
-
- return 0;
+ axis3bit16_t data_raw;
+
+ /* Read raw data values. */
+ if (lsm6dso_acceleration_raw_get(&_reg_ctx, data_raw.u8bit) != 0) {
+ return 1;
+ }
+
+ /* Format the data. */
+ value[0] = data_raw.i16bit[0];
+ value[1] = data_raw.i16bit[1];
+ value[2] = data_raw.i16bit[2];
+
+ return 0;
}
@@ -512,27 +484,25 @@
*/
int LSM6DSOSensor::get_x_axes(int32_t *acceleration)
{
- axis3bit16_t data_raw;
- float sensitivity = 0.0f;
-
- /* Read raw data values. */
- if (lsm6dso_acceleration_raw_get(&_reg_ctx, data_raw.u8bit) != 0)
- {
- return 1;
- }
-
- /* Get LSM6DSO actual sensitivity. */
- if (get_x_sensitivity(&sensitivity) != 0)
- {
- return 1;
- }
-
- /* Calculate the data. */
- acceleration[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity));
- acceleration[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity));
- acceleration[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity));
-
- return 0;
+ axis3bit16_t data_raw;
+ float sensitivity = 0.0f;
+
+ /* Read raw data values. */
+ if (lsm6dso_acceleration_raw_get(&_reg_ctx, data_raw.u8bit) != 0) {
+ return 1;
+ }
+
+ /* Get LSM6DSO actual sensitivity. */
+ if (get_x_sensitivity(&sensitivity) != 0) {
+ return 1;
+ }
+
+ /* Calculate the data. */
+ acceleration[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity));
+ acceleration[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity));
+ acceleration[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity));
+
+ return 0;
}
@@ -542,21 +512,19 @@
*/
int LSM6DSOSensor::enable_g()
{
- /* Check if the component is already enabled */
- if (_g_is_enabled == 1U)
- {
+ /* Check if the component is already enabled */
+ if (_g_is_enabled == 1U) {
+ return 0;
+ }
+
+ /* Output data rate selection. */
+ if (lsm6dso_gy_data_rate_set(&_reg_ctx, _g_last_odr) != 0) {
+ return 1;
+ }
+
+ _g_is_enabled = 1;
+
return 0;
- }
-
- /* Output data rate selection. */
- if (lsm6dso_gy_data_rate_set(&_reg_ctx, _g_last_odr) != 0)
- {
- return 1;
- }
-
- _g_is_enabled = 1;
-
- return 0;
}
@@ -566,27 +534,24 @@
*/
int LSM6DSOSensor::disable_g()
{
- /* Check if the component is already disabled */
- if (_g_is_enabled == 0U)
- {
+ /* Check if the component is already disabled */
+ if (_g_is_enabled == 0U) {
+ return 0;
+ }
+
+ /* Get current output data rate. */
+ if (lsm6dso_gy_data_rate_get(&_reg_ctx, &_g_last_odr) != 0) {
+ return 1;
+ }
+
+ /* Output data rate selection - power down. */
+ if (lsm6dso_gy_data_rate_set(&_reg_ctx, LSM6DSO_GY_ODR_OFF) != 0) {
+ return 1;
+ }
+
+ _g_is_enabled = 0;
+
return 0;
- }
-
- /* Get current output data rate. */
- if (lsm6dso_gy_data_rate_get(&_reg_ctx, &_g_last_odr) != 0)
- {
- return 1;
- }
-
- /* Output data rate selection - power down. */
- if (lsm6dso_gy_data_rate_set(&_reg_ctx, LSM6DSO_GY_ODR_OFF) != 0)
- {
- return 1;
- }
-
- _g_is_enabled = 0;
-
- return 0;
}
/**
@@ -596,44 +561,42 @@
*/
int LSM6DSOSensor::get_g_sensitivity(float *sensitivity)
{
- int ret = 0;
- lsm6dso_fs_g_t full_scale;
-
- /* Read actual full scale selection from sensor. */
- if (lsm6dso_gy_full_scale_get(&_reg_ctx, &full_scale) != 0)
- {
- return 1;
- }
-
- /* Store the sensitivity based on actual full scale. */
- switch (full_scale)
- {
- case LSM6DSO_125dps:
- *sensitivity = LSM6DSO_GYRO_SENSITIVITY_FS_125DPS;
- break;
-
- case LSM6DSO_250dps:
- *sensitivity = LSM6DSO_GYRO_SENSITIVITY_FS_250DPS;
- break;
-
- case LSM6DSO_500dps:
- *sensitivity = LSM6DSO_GYRO_SENSITIVITY_FS_500DPS;
- break;
-
- case LSM6DSO_1000dps:
- *sensitivity = LSM6DSO_GYRO_SENSITIVITY_FS_1000DPS;
- break;
-
- case LSM6DSO_2000dps:
- *sensitivity = LSM6DSO_GYRO_SENSITIVITY_FS_2000DPS;
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ int ret = 0;
+ lsm6dso_fs_g_t full_scale;
+
+ /* Read actual full scale selection from sensor. */
+ if (lsm6dso_gy_full_scale_get(&_reg_ctx, &full_scale) != 0) {
+ return 1;
+ }
+
+ /* Store the sensitivity based on actual full scale. */
+ switch (full_scale) {
+ case LSM6DSO_125dps:
+ *sensitivity = LSM6DSO_GYRO_SENSITIVITY_FS_125DPS;
+ break;
+
+ case LSM6DSO_250dps:
+ *sensitivity = LSM6DSO_GYRO_SENSITIVITY_FS_250DPS;
+ break;
+
+ case LSM6DSO_500dps:
+ *sensitivity = LSM6DSO_GYRO_SENSITIVITY_FS_500DPS;
+ break;
+
+ case LSM6DSO_1000dps:
+ *sensitivity = LSM6DSO_GYRO_SENSITIVITY_FS_1000DPS;
+ break;
+
+ case LSM6DSO_2000dps:
+ *sensitivity = LSM6DSO_GYRO_SENSITIVITY_FS_2000DPS;
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -643,67 +606,65 @@
*/
int LSM6DSOSensor::get_g_odr(float *odr)
{
- int ret = 0;
- lsm6dso_odr_g_t odr_low_level;
-
- /* Get current output data rate. */
- if (lsm6dso_gy_data_rate_get(&_reg_ctx, &odr_low_level) != 0)
- {
- return 1;
- }
-
- switch (odr_low_level)
- {
- case LSM6DSO_GY_ODR_OFF:
- *odr = 0.0f;
- break;
-
- case LSM6DSO_GY_ODR_12Hz5:
- *odr = 12.5f;
- break;
-
- case LSM6DSO_GY_ODR_26Hz:
- *odr = 26.0f;
- break;
-
- case LSM6DSO_GY_ODR_52Hz:
- *odr = 52.0f;
- break;
-
- case LSM6DSO_GY_ODR_104Hz:
- *odr = 104.0f;
- break;
-
- case LSM6DSO_GY_ODR_208Hz:
- *odr = 208.0f;
- break;
-
- case LSM6DSO_GY_ODR_417Hz:
- *odr = 417.0f;
- break;
-
- case LSM6DSO_GY_ODR_833Hz:
- *odr = 833.0f;
- break;
-
- case LSM6DSO_GY_ODR_1667Hz:
- *odr = 1667.0f;
- break;
-
- case LSM6DSO_GY_ODR_3333Hz:
- *odr = 3333.0f;
- break;
-
- case LSM6DSO_GY_ODR_6667Hz:
- *odr = 6667.0f;
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ int ret = 0;
+ lsm6dso_odr_g_t odr_low_level;
+
+ /* Get current output data rate. */
+ if (lsm6dso_gy_data_rate_get(&_reg_ctx, &odr_low_level) != 0) {
+ return 1;
+ }
+
+ switch (odr_low_level) {
+ case LSM6DSO_GY_ODR_OFF:
+ *odr = 0.0f;
+ break;
+
+ case LSM6DSO_GY_ODR_12Hz5:
+ *odr = 12.5f;
+ break;
+
+ case LSM6DSO_GY_ODR_26Hz:
+ *odr = 26.0f;
+ break;
+
+ case LSM6DSO_GY_ODR_52Hz:
+ *odr = 52.0f;
+ break;
+
+ case LSM6DSO_GY_ODR_104Hz:
+ *odr = 104.0f;
+ break;
+
+ case LSM6DSO_GY_ODR_208Hz:
+ *odr = 208.0f;
+ break;
+
+ case LSM6DSO_GY_ODR_417Hz:
+ *odr = 417.0f;
+ break;
+
+ case LSM6DSO_GY_ODR_833Hz:
+ *odr = 833.0f;
+ break;
+
+ case LSM6DSO_GY_ODR_1667Hz:
+ *odr = 1667.0f;
+ break;
+
+ case LSM6DSO_GY_ODR_3333Hz:
+ *odr = 3333.0f;
+ break;
+
+ case LSM6DSO_GY_ODR_6667Hz:
+ *odr = 6667.0f;
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -713,15 +674,12 @@
*/
int LSM6DSOSensor::set_g_odr(float odr)
{
- /* Check if the component is enabled */
- if (_g_is_enabled == 1U)
- {
- return set_g_odr_when_enabled(odr);
- }
- else
- {
- return set_g_odr_when_disabled(odr);
- }
+ /* Check if the component is enabled */
+ if (_g_is_enabled == 1U) {
+ return set_g_odr_when_enabled(odr);
+ } else {
+ return set_g_odr_when_disabled(odr);
+ }
}
/**
@@ -731,26 +689,25 @@
*/
int LSM6DSOSensor::set_g_odr_when_enabled(float odr)
{
- lsm6dso_odr_g_t new_odr;
-
- new_odr = (odr <= 12.5f) ? LSM6DSO_GY_ODR_12Hz5
- : (odr <= 26.0f) ? LSM6DSO_GY_ODR_26Hz
- : (odr <= 52.0f) ? LSM6DSO_GY_ODR_52Hz
- : (odr <= 104.0f) ? LSM6DSO_GY_ODR_104Hz
- : (odr <= 208.0f) ? LSM6DSO_GY_ODR_208Hz
- : (odr <= 417.0f) ? LSM6DSO_GY_ODR_417Hz
- : (odr <= 833.0f) ? LSM6DSO_GY_ODR_833Hz
- : (odr <= 1667.0f) ? LSM6DSO_GY_ODR_1667Hz
- : (odr <= 3333.0f) ? LSM6DSO_GY_ODR_3333Hz
- : LSM6DSO_GY_ODR_6667Hz;
-
- /* Output data rate selection. */
- if (lsm6dso_gy_data_rate_set(&_reg_ctx, new_odr) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_odr_g_t new_odr;
+
+ new_odr = (odr <= 12.5f) ? LSM6DSO_GY_ODR_12Hz5
+ : (odr <= 26.0f) ? LSM6DSO_GY_ODR_26Hz
+ : (odr <= 52.0f) ? LSM6DSO_GY_ODR_52Hz
+ : (odr <= 104.0f) ? LSM6DSO_GY_ODR_104Hz
+ : (odr <= 208.0f) ? LSM6DSO_GY_ODR_208Hz
+ : (odr <= 417.0f) ? LSM6DSO_GY_ODR_417Hz
+ : (odr <= 833.0f) ? LSM6DSO_GY_ODR_833Hz
+ : (odr <= 1667.0f) ? LSM6DSO_GY_ODR_1667Hz
+ : (odr <= 3333.0f) ? LSM6DSO_GY_ODR_3333Hz
+ : LSM6DSO_GY_ODR_6667Hz;
+
+ /* Output data rate selection. */
+ if (lsm6dso_gy_data_rate_set(&_reg_ctx, new_odr) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -760,18 +717,18 @@
*/
int LSM6DSOSensor::set_g_odr_when_disabled(float odr)
{
- _g_last_odr = (odr <= 12.5f) ? LSM6DSO_GY_ODR_12Hz5
- : (odr <= 26.0f) ? LSM6DSO_GY_ODR_26Hz
- : (odr <= 52.0f) ? LSM6DSO_GY_ODR_52Hz
- : (odr <= 104.0f) ? LSM6DSO_GY_ODR_104Hz
- : (odr <= 208.0f) ? LSM6DSO_GY_ODR_208Hz
- : (odr <= 417.0f) ? LSM6DSO_GY_ODR_417Hz
- : (odr <= 833.0f) ? LSM6DSO_GY_ODR_833Hz
- : (odr <= 1667.0f) ? LSM6DSO_GY_ODR_1667Hz
- : (odr <= 3333.0f) ? LSM6DSO_GY_ODR_3333Hz
- : LSM6DSO_GY_ODR_6667Hz;
-
- return 0;
+ _g_last_odr = (odr <= 12.5f) ? LSM6DSO_GY_ODR_12Hz5
+ : (odr <= 26.0f) ? LSM6DSO_GY_ODR_26Hz
+ : (odr <= 52.0f) ? LSM6DSO_GY_ODR_52Hz
+ : (odr <= 104.0f) ? LSM6DSO_GY_ODR_104Hz
+ : (odr <= 208.0f) ? LSM6DSO_GY_ODR_208Hz
+ : (odr <= 417.0f) ? LSM6DSO_GY_ODR_417Hz
+ : (odr <= 833.0f) ? LSM6DSO_GY_ODR_833Hz
+ : (odr <= 1667.0f) ? LSM6DSO_GY_ODR_1667Hz
+ : (odr <= 3333.0f) ? LSM6DSO_GY_ODR_3333Hz
+ : LSM6DSO_GY_ODR_6667Hz;
+
+ return 0;
}
@@ -782,43 +739,41 @@
*/
int LSM6DSOSensor::get_g_fs(float *full_scale)
{
- int ret = 0;
- lsm6dso_fs_g_t fs_low_level;
-
- /* Read actual full scale selection from sensor. */
- if (lsm6dso_gy_full_scale_get(&_reg_ctx, &fs_low_level) != 0)
- {
- return 1;
- }
-
- switch (fs_low_level)
- {
- case LSM6DSO_125dps:
- *full_scale = 125.0f;
- break;
-
- case LSM6DSO_250dps:
- *full_scale = 250.0f;
- break;
-
- case LSM6DSO_500dps:
- *full_scale = 500.0f;
- break;
-
- case LSM6DSO_1000dps:
- *full_scale = 1000.0f;
- break;
-
- case LSM6DSO_2000dps:
- *full_scale = 2000.0f;
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ int ret = 0;
+ lsm6dso_fs_g_t fs_low_level;
+
+ /* Read actual full scale selection from sensor. */
+ if (lsm6dso_gy_full_scale_get(&_reg_ctx, &fs_low_level) != 0) {
+ return 1;
+ }
+
+ switch (fs_low_level) {
+ case LSM6DSO_125dps:
+ *full_scale = 125.0f;
+ break;
+
+ case LSM6DSO_250dps:
+ *full_scale = 250.0f;
+ break;
+
+ case LSM6DSO_500dps:
+ *full_scale = 500.0f;
+ break;
+
+ case LSM6DSO_1000dps:
+ *full_scale = 1000.0f;
+ break;
+
+ case LSM6DSO_2000dps:
+ *full_scale = 2000.0f;
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -828,20 +783,19 @@
*/
int LSM6DSOSensor::set_g_fs(float full_scale)
{
- lsm6dso_fs_g_t new_fs;
-
- new_fs = (full_scale <= 125.0f) ? LSM6DSO_125dps
- : (full_scale <= 250.0f) ? LSM6DSO_250dps
- : (full_scale <= 500.0f) ? LSM6DSO_500dps
- : (full_scale <= 1000.0f) ? LSM6DSO_1000dps
- : LSM6DSO_2000dps;
-
- if (lsm6dso_gy_full_scale_set(&_reg_ctx, new_fs) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_fs_g_t new_fs;
+
+ new_fs = (full_scale <= 125.0f) ? LSM6DSO_125dps
+ : (full_scale <= 250.0f) ? LSM6DSO_250dps
+ : (full_scale <= 500.0f) ? LSM6DSO_500dps
+ : (full_scale <= 1000.0f) ? LSM6DSO_1000dps
+ : LSM6DSO_2000dps;
+
+ if (lsm6dso_gy_full_scale_set(&_reg_ctx, new_fs) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -851,20 +805,19 @@
*/
int LSM6DSOSensor::get_g_axes_raw(int16_t *value)
{
- axis3bit16_t data_raw;
-
- /* Read raw data values. */
- if (lsm6dso_angular_rate_raw_get(&_reg_ctx, data_raw.u8bit) != 0)
- {
- return 1;
- }
-
- /* Format the data. */
- value[0] = data_raw.i16bit[0];
- value[1] = data_raw.i16bit[1];
- value[2] = data_raw.i16bit[2];
-
- return 0;
+ axis3bit16_t data_raw;
+
+ /* Read raw data values. */
+ if (lsm6dso_angular_rate_raw_get(&_reg_ctx, data_raw.u8bit) != 0) {
+ return 1;
+ }
+
+ /* Format the data. */
+ value[0] = data_raw.i16bit[0];
+ value[1] = data_raw.i16bit[1];
+ value[2] = data_raw.i16bit[2];
+
+ return 0;
}
@@ -875,27 +828,25 @@
*/
int LSM6DSOSensor::get_g_axes(int32_t *angular_rate)
{
- axis3bit16_t data_raw;
- float sensitivity;
-
- /* Read raw data values. */
- if (lsm6dso_angular_rate_raw_get(&_reg_ctx, data_raw.u8bit) != 0)
- {
- return 1;
- }
-
- /* Get LSM6DSO actual sensitivity. */
- if (get_g_sensitivity(&sensitivity) != 0)
- {
- return 1;
- }
-
- /* Calculate the data. */
- angular_rate[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity));
- angular_rate[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity));
- angular_rate[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity));
-
- return 0;
+ axis3bit16_t data_raw;
+ float sensitivity;
+
+ /* Read raw data values. */
+ if (lsm6dso_angular_rate_raw_get(&_reg_ctx, data_raw.u8bit) != 0) {
+ return 1;
+ }
+
+ /* Get LSM6DSO actual sensitivity. */
+ if (get_g_sensitivity(&sensitivity) != 0) {
+ return 1;
+ }
+
+ /* Calculate the data. */
+ angular_rate[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity));
+ angular_rate[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity));
+ angular_rate[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity));
+
+ return 0;
}
@@ -907,12 +858,11 @@
*/
int LSM6DSOSensor::read_reg(uint8_t reg, uint8_t *data)
{
- if (lsm6dso_read_reg(&_reg_ctx, reg, data, 1) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_read_reg(&_reg_ctx, reg, data, 1) != 0) {
+ return 1;
+ }
+
+ return 0;
}
@@ -924,12 +874,11 @@
*/
int LSM6DSOSensor::write_reg(uint8_t reg, uint8_t data)
{
- if (lsm6dso_write_reg(&_reg_ctx, reg, &data, 1) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_write_reg(&_reg_ctx, reg, &data, 1) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -939,17 +888,15 @@
*/
int LSM6DSOSensor::set_interrupt_latch(uint8_t status)
{
- if (status > 1U)
- {
- return 1;
- }
-
- if (lsm6dso_int_notification_set(&_reg_ctx, (lsm6dso_lir_t)status) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (status > 1U) {
+ return 1;
+ }
+
+ if (lsm6dso_int_notification_set(&_reg_ctx, (lsm6dso_lir_t)status) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -959,83 +906,72 @@
*/
int LSM6DSOSensor::enable_free_fall_detection(LSM6DSO_Interrupt_Pin_t int_pin)
{
- int ret = 0;
- lsm6dso_pin_int1_route_t val1;
- lsm6dso_pin_int2_route_t val2;
-
- /* Output Data Rate selection */
- if (set_x_odr(416.0f) != 0)
- {
- return 1;
- }
-
- /* Full scale selection */
- if (set_x_fs(2.0f) != 0)
- {
- return 1;
- }
-
- /* FF_DUR setting */
- if (lsm6dso_ff_dur_set(&_reg_ctx, 0x06) != 0)
- {
- return 1;
- }
-
- /* WAKE_DUR setting */
- if (lsm6dso_wkup_dur_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* SLEEP_DUR setting */
- if (lsm6dso_act_sleep_dur_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* FF_THS setting */
- if (lsm6dso_ff_threshold_set(&_reg_ctx, LSM6DSO_FF_TSH_312mg) != 0)
- {
- return 1;
- }
-
- /* Enable free fall event on either INT1 or INT2 pin */
- switch (int_pin)
- {
- case LSM6DSO_INT1_PIN:
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
+ int ret = 0;
+ lsm6dso_pin_int1_route_t val1;
+ lsm6dso_pin_int2_route_t val2;
+
+ /* Output Data Rate selection */
+ if (set_x_odr(416.0f) != 0) {
+ return 1;
+ }
+
+ /* Full scale selection */
+ if (set_x_fs(2.0f) != 0) {
+ return 1;
+ }
+
+ /* FF_DUR setting */
+ if (lsm6dso_ff_dur_set(&_reg_ctx, 0x06) != 0) {
+ return 1;
+ }
+
+ /* WAKE_DUR setting */
+ if (lsm6dso_wkup_dur_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* SLEEP_DUR setting */
+ if (lsm6dso_act_sleep_dur_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* FF_THS setting */
+ if (lsm6dso_ff_threshold_set(&_reg_ctx, LSM6DSO_FF_TSH_312mg) != 0) {
return 1;
- }
-
- val1.md1_cfg.int1_ff = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
- break;
-
- case LSM6DSO_INT2_PIN:
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- val2.md2_cfg.int2_ff = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ }
+
+ /* Enable free fall event on either INT1 or INT2 pin */
+ switch (int_pin) {
+ case LSM6DSO_INT1_PIN:
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.md1_cfg.int1_ff = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+ break;
+
+ case LSM6DSO_INT2_PIN:
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.md2_cfg.int2_ff = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -1044,47 +980,41 @@
*/
int LSM6DSOSensor::disable_free_fall_detection()
{
- lsm6dso_pin_int1_route_t val1;
- lsm6dso_pin_int2_route_t val2;
-
- /* Disable free fall event on both INT1 and INT2 pins */
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- val1.md1_cfg.int1_ff = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- val2.md2_cfg.int2_ff = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- /* FF_DUR setting */
- if (lsm6dso_ff_dur_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* FF_THS setting */
- if (lsm6dso_ff_threshold_set(&_reg_ctx, LSM6DSO_FF_TSH_156mg) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_pin_int1_route_t val1;
+ lsm6dso_pin_int2_route_t val2;
+
+ /* Disable free fall event on both INT1 and INT2 pins */
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.md1_cfg.int1_ff = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.md2_cfg.int2_ff = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ /* FF_DUR setting */
+ if (lsm6dso_ff_dur_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* FF_THS setting */
+ if (lsm6dso_ff_threshold_set(&_reg_ctx, LSM6DSO_FF_TSH_156mg) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1094,12 +1024,11 @@
*/
int LSM6DSOSensor::set_free_fall_threshold(uint8_t thr)
{
- if (lsm6dso_ff_threshold_set(&_reg_ctx, (lsm6dso_ff_ths_t)thr) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_ff_threshold_set(&_reg_ctx, (lsm6dso_ff_ths_t)thr) != 0) {
+ return 1;
+ }
+
+ return 0;
}
@@ -1110,12 +1039,11 @@
*/
int LSM6DSOSensor::set_free_fall_duration(uint8_t dur)
{
- if (lsm6dso_ff_dur_set(&_reg_ctx, dur) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_ff_dur_set(&_reg_ctx, dur) != 0) {
+ return 1;
+ }
+
+ return 0;
}
@@ -1127,38 +1055,33 @@
{
lsm6dso_pin_int1_route_t val;
- /* Output Data Rate selection */
- if (set_x_odr(26.0f) != 0)
- {
- return 1;
- }
-
- /* Full scale selection */
- if (set_x_fs(2.0f) != 0)
- {
- return 1;
- }
-
- /* Enable pedometer algorithm. */
- if (lsm6dso_pedo_sens_set(&_reg_ctx, LSM6DSO_PEDO_BASE_MODE) != 0)
- {
- return 1;
- }
-
- /* Enable step detector on INT1 pin */
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val) != 0)
- {
- return 1;
- }
-
- val.emb_func_int1.int1_step_detector = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val) != 0)
- {
- return 1;
- }
-
- return 0;
+ /* Output Data Rate selection */
+ if (set_x_odr(26.0f) != 0) {
+ return 1;
+ }
+
+ /* Full scale selection */
+ if (set_x_fs(2.0f) != 0) {
+ return 1;
+ }
+
+ /* Enable pedometer algorithm. */
+ if (lsm6dso_pedo_sens_set(&_reg_ctx, LSM6DSO_PEDO_BASE_MODE) != 0) {
+ return 1;
+ }
+
+ /* Enable step detector on INT1 pin */
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val) != 0) {
+ return 1;
+ }
+
+ val.emb_func_int1.int1_step_detector = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1167,28 +1090,25 @@
*/
int LSM6DSOSensor::disable_pedometer()
{
- lsm6dso_pin_int1_route_t val1;
-
- /* Disable step detector on INT1 pin */
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- val1.emb_func_int1.int1_step_detector = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- /* Disable pedometer algorithm. */
- if (lsm6dso_pedo_sens_set(&_reg_ctx, LSM6DSO_PEDO_DISABLE) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_pin_int1_route_t val1;
+
+ /* Disable step detector on INT1 pin */
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.emb_func_int1.int1_step_detector = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ /* Disable pedometer algorithm. */
+ if (lsm6dso_pedo_sens_set(&_reg_ctx, LSM6DSO_PEDO_DISABLE) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1198,12 +1118,11 @@
*/
int LSM6DSOSensor::get_step_counter(uint16_t *step_count)
{
- if (lsm6dso_number_of_steps_get(&_reg_ctx, (uint8_t *)step_count) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_number_of_steps_get(&_reg_ctx, (uint8_t *)step_count) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1212,12 +1131,11 @@
*/
int LSM6DSOSensor::reset_step_counter()
{
- if (lsm6dso_steps_reset(&_reg_ctx) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_steps_reset(&_reg_ctx) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1227,65 +1145,57 @@
*/
int LSM6DSOSensor::enable_tilt_detection(LSM6DSO_Interrupt_Pin_t int_pin)
{
- int ret = 0;
- lsm6dso_pin_int1_route_t val1;
- lsm6dso_pin_int2_route_t val2;
-
- /* Output Data Rate selection */
- if (set_x_odr(26.0f) != 0)
- {
- return 1;
- }
-
- /* Full scale selection */
- if (set_x_fs(2.0f) != 0)
- {
- return 1;
- }
-
- /* Enable tilt calculation. */
- if (lsm6dso_tilt_sens_set(&_reg_ctx, PROPERTY_ENABLE) != 0)
- {
- return 1;
- }
-
- /* Enable tilt event on either INT1 or INT2 pin */
- switch (int_pin)
- {
- case LSM6DSO_INT1_PIN:
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
+ int ret = 0;
+ lsm6dso_pin_int1_route_t val1;
+ lsm6dso_pin_int2_route_t val2;
+
+ /* Output Data Rate selection */
+ if (set_x_odr(26.0f) != 0) {
+ return 1;
+ }
+
+ /* Full scale selection */
+ if (set_x_fs(2.0f) != 0) {
+ return 1;
+ }
+
+ /* Enable tilt calculation. */
+ if (lsm6dso_tilt_sens_set(&_reg_ctx, PROPERTY_ENABLE) != 0) {
return 1;
- }
-
- val1.emb_func_int1.int1_tilt = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
- break;
-
- case LSM6DSO_INT2_PIN:
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- val2.emb_func_int2.int2_tilt = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ }
+
+ /* Enable tilt event on either INT1 or INT2 pin */
+ switch (int_pin) {
+ case LSM6DSO_INT1_PIN:
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.emb_func_int1.int1_tilt = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+ break;
+
+ case LSM6DSO_INT2_PIN:
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.emb_func_int2.int2_tilt = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -1294,41 +1204,36 @@
*/
int LSM6DSOSensor::disable_tilt_detection()
{
- lsm6dso_pin_int1_route_t val1;
- lsm6dso_pin_int2_route_t val2;
-
- /* Disable tilt event on both INT1 and INT2 pins */
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- val1.emb_func_int1.int1_tilt = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- val2.emb_func_int2.int2_tilt = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- /* Disable tilt calculation. */
- if (lsm6dso_tilt_sens_set(&_reg_ctx, PROPERTY_DISABLE) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_pin_int1_route_t val1;
+ lsm6dso_pin_int2_route_t val2;
+
+ /* Disable tilt event on both INT1 and INT2 pins */
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.emb_func_int1.int1_tilt = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.emb_func_int2.int2_tilt = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ /* Disable tilt calculation. */
+ if (lsm6dso_tilt_sens_set(&_reg_ctx, PROPERTY_DISABLE) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1338,71 +1243,62 @@
*/
int LSM6DSOSensor::enable_wake_up_detection(LSM6DSO_Interrupt_Pin_t int_pin)
{
- int ret = 0;
- lsm6dso_pin_int1_route_t val1;
- lsm6dso_pin_int2_route_t val2;
-
- /* Output Data Rate selection */
- if (set_x_odr(416.0f) != 0)
- {
- return 1;
- }
-
- /* Full scale selection */
- if (set_x_fs(2.0f) != 0)
- {
- return 1;
- }
-
- /* WAKE_DUR setting */
- if (lsm6dso_wkup_dur_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* Set wake up threshold. */
- if (lsm6dso_wkup_threshold_set(&_reg_ctx, 0x02) != 0)
- {
- return 1;
- }
-
- /* Enable wake up event on either INT1 or INT2 pin */
- switch (int_pin)
- {
- case LSM6DSO_INT1_PIN:
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
+ int ret = 0;
+ lsm6dso_pin_int1_route_t val1;
+ lsm6dso_pin_int2_route_t val2;
+
+ /* Output Data Rate selection */
+ if (set_x_odr(416.0f) != 0) {
+ return 1;
+ }
+
+ /* Full scale selection */
+ if (set_x_fs(2.0f) != 0) {
+ return 1;
+ }
+
+ /* WAKE_DUR setting */
+ if (lsm6dso_wkup_dur_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* Set wake up threshold. */
+ if (lsm6dso_wkup_threshold_set(&_reg_ctx, 0x02) != 0) {
return 1;
- }
-
- val1.md1_cfg.int1_wu = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
- break;
-
- case LSM6DSO_INT2_PIN:
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- val2.md2_cfg.int2_wu = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ }
+
+ /* Enable wake up event on either INT1 or INT2 pin */
+ switch (int_pin) {
+ case LSM6DSO_INT1_PIN:
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.md1_cfg.int1_wu = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+ break;
+
+ case LSM6DSO_INT2_PIN:
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.md2_cfg.int2_wu = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
@@ -1415,44 +1311,38 @@
lsm6dso_pin_int1_route_t val1;
lsm6dso_pin_int2_route_t val2;
- /* Disable wake up event on both INT1 and INT2 pins */
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- val1.md1_cfg.int1_wu = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- val2.md2_cfg.int2_wu = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- /* Reset wake up threshold. */
- if (lsm6dso_wkup_threshold_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* WAKE_DUR setting */
- if (lsm6dso_wkup_dur_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- return 0;
+ /* Disable wake up event on both INT1 and INT2 pins */
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.md1_cfg.int1_wu = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.md2_cfg.int2_wu = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ /* Reset wake up threshold. */
+ if (lsm6dso_wkup_threshold_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* WAKE_DUR setting */
+ if (lsm6dso_wkup_dur_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1462,13 +1352,12 @@
*/
int LSM6DSOSensor::set_wake_up_threshold(uint8_t thr)
{
- /* Set wake up threshold. */
- if (lsm6dso_wkup_threshold_set(&_reg_ctx, thr) != 0)
- {
- return 1;
- }
-
- return 0;
+ /* Set wake up threshold. */
+ if (lsm6dso_wkup_threshold_set(&_reg_ctx, thr) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1478,13 +1367,12 @@
*/
int LSM6DSOSensor::set_wake_up_duration(uint8_t dur)
{
- /* Set wake up duration. */
- if (lsm6dso_wkup_dur_set(&_reg_ctx, dur) != 0)
- {
- return 1;
- }
-
- return 0;
+ /* Set wake up duration. */
+ if (lsm6dso_wkup_dur_set(&_reg_ctx, dur) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1494,99 +1382,86 @@
*/
int LSM6DSOSensor::enable_single_tap_detection(LSM6DSO_Interrupt_Pin_t int_pin)
{
- int ret = 0;
- lsm6dso_pin_int1_route_t val1;
- lsm6dso_pin_int2_route_t val2;
-
- /* Output Data Rate selection */
- if (set_x_odr(416.0f) != 0)
- {
- return 1;
- }
-
- /* Full scale selection */
- if (set_x_fs(2.0f) != 0)
- {
- return 1;
- }
-
- /* Enable X direction in tap recognition. */
- if (lsm6dso_tap_detection_on_x_set(&_reg_ctx, PROPERTY_ENABLE) != 0)
- {
- return 1;
- }
-
- /* Enable Y direction in tap recognition. */
- if (lsm6dso_tap_detection_on_y_set(&_reg_ctx, PROPERTY_ENABLE) != 0)
- {
- return 1;
- }
-
- /* Enable Z direction in tap recognition. */
- if (lsm6dso_tap_detection_on_z_set(&_reg_ctx, PROPERTY_ENABLE) != 0)
- {
- return 1;
- }
-
- /* Set tap threshold. */
- if (lsm6dso_tap_threshold_x_set(&_reg_ctx, 0x08) != 0)
- {
- return 1;
- }
-
- /* Set tap shock time window. */
- if (lsm6dso_tap_shock_set(&_reg_ctx, 0x02) != 0)
- {
- return 1;
- }
-
- /* Set tap quiet time window. */
- if (lsm6dso_tap_quiet_set(&_reg_ctx, 0x01) != 0)
- {
- return 1;
- }
-
- /* _NOTE_: Tap duration time window - don't care for single tap. */
-
- /* _NOTE_: Single/Double Tap event - don't care of this flag for single tap. */
-
- /* Enable single tap event on either INT1 or INT2 pin */
- switch (int_pin)
- {
- case LSM6DSO_INT1_PIN:
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
+ int ret = 0;
+ lsm6dso_pin_int1_route_t val1;
+ lsm6dso_pin_int2_route_t val2;
+
+ /* Output Data Rate selection */
+ if (set_x_odr(416.0f) != 0) {
+ return 1;
+ }
+
+ /* Full scale selection */
+ if (set_x_fs(2.0f) != 0) {
+ return 1;
+ }
+
+ /* Enable X direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_x_set(&_reg_ctx, PROPERTY_ENABLE) != 0) {
+ return 1;
+ }
+
+ /* Enable Y direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_y_set(&_reg_ctx, PROPERTY_ENABLE) != 0) {
+ return 1;
+ }
+
+ /* Enable Z direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_z_set(&_reg_ctx, PROPERTY_ENABLE) != 0) {
+ return 1;
+ }
+
+ /* Set tap threshold. */
+ if (lsm6dso_tap_threshold_x_set(&_reg_ctx, 0x08) != 0) {
+ return 1;
+ }
+
+ /* Set tap shock time window. */
+ if (lsm6dso_tap_shock_set(&_reg_ctx, 0x02) != 0) {
return 1;
- }
-
- val1.md1_cfg.int1_single_tap = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
- break;
-
- case LSM6DSO_INT2_PIN:
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
+ }
+
+ /* Set tap quiet time window. */
+ if (lsm6dso_tap_quiet_set(&_reg_ctx, 0x01) != 0) {
return 1;
- }
-
- val2.md2_cfg.int2_single_tap = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ }
+
+ /* _NOTE_: Tap duration time window - don't care for single tap. */
+
+ /* _NOTE_: Single/Double Tap event - don't care of this flag for single tap. */
+
+ /* Enable single tap event on either INT1 or INT2 pin */
+ switch (int_pin) {
+ case LSM6DSO_INT1_PIN:
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.md1_cfg.int1_single_tap = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+ break;
+
+ case LSM6DSO_INT2_PIN:
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.md2_cfg.int2_single_tap = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -1598,68 +1473,58 @@
lsm6dso_pin_int1_route_t val1;
lsm6dso_pin_int2_route_t val2;
- /* Disable single tap event on both INT1 and INT2 pins */
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- val1.md1_cfg.int1_single_tap = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- val2.md2_cfg.int2_single_tap = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- /* Reset tap quiet time window. */
- if (lsm6dso_tap_quiet_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* Reset tap shock time window. */
- if (lsm6dso_tap_shock_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* Reset tap threshold. */
- if (lsm6dso_tap_threshold_x_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* Disable Z direction in tap recognition. */
- if (lsm6dso_tap_detection_on_z_set(&_reg_ctx, PROPERTY_DISABLE) != 0)
- {
- return 1;
- }
-
- /* Disable Y direction in tap recognition. */
- if (lsm6dso_tap_detection_on_y_set(&_reg_ctx, PROPERTY_DISABLE) != 0)
- {
- return 1;
- }
-
- /* Disable X direction in tap recognition. */
- if (lsm6dso_tap_detection_on_x_set(&_reg_ctx, PROPERTY_DISABLE) != 0)
- {
- return 1;
- }
-
- return 0;
+ /* Disable single tap event on both INT1 and INT2 pins */
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.md1_cfg.int1_single_tap = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.md2_cfg.int2_single_tap = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ /* Reset tap quiet time window. */
+ if (lsm6dso_tap_quiet_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* Reset tap shock time window. */
+ if (lsm6dso_tap_shock_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* Reset tap threshold. */
+ if (lsm6dso_tap_threshold_x_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* Disable Z direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_z_set(&_reg_ctx, PROPERTY_DISABLE) != 0) {
+ return 1;
+ }
+
+ /* Disable Y direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_y_set(&_reg_ctx, PROPERTY_DISABLE) != 0) {
+ return 1;
+ }
+
+ /* Disable X direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_x_set(&_reg_ctx, PROPERTY_DISABLE) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1669,107 +1534,92 @@
*/
int LSM6DSOSensor::enable_double_tap_detection(LSM6DSO_Interrupt_Pin_t int_pin)
{
- int ret = 0;
- lsm6dso_pin_int1_route_t val1;
- lsm6dso_pin_int2_route_t val2;
-
- /* Output Data Rate selection */
- if (set_x_odr(416.0f) != 0)
- {
- return 1;
- }
-
- /* Full scale selection */
- if (set_x_fs(2.0f) != 0)
- {
- return 1;
- }
-
- /* Enable X direction in tap recognition. */
- if (lsm6dso_tap_detection_on_x_set(&_reg_ctx, PROPERTY_ENABLE) != 0)
- {
- return 1;
- }
-
- /* Enable Y direction in tap recognition. */
- if (lsm6dso_tap_detection_on_y_set(&_reg_ctx, PROPERTY_ENABLE) != 0)
- {
- return 1;
- }
-
- /* Enable Z direction in tap recognition. */
- if (lsm6dso_tap_detection_on_z_set(&_reg_ctx, PROPERTY_ENABLE) != 0)
- {
- return 1;
- }
-
- /* Set tap threshold. */
- if (lsm6dso_tap_threshold_x_set(&_reg_ctx, 0x08) != 0)
- {
- return 1;
- }
-
- /* Set tap shock time window. */
- if (lsm6dso_tap_shock_set(&_reg_ctx, 0x03) != 0)
- {
- return 1;
- }
-
- /* Set tap quiet time window. */
- if (lsm6dso_tap_quiet_set(&_reg_ctx, 0x03) != 0)
- {
- return 1;
- }
-
- /* Set tap duration time window. */
- if (lsm6dso_tap_dur_set(&_reg_ctx, 0x08) != 0)
- {
- return 1;
- }
-
- /* Single and double tap enabled. */
- if (lsm6dso_tap_mode_set(&_reg_ctx, LSM6DSO_BOTH_SINGLE_DOUBLE) != 0)
- {
- return 1;
- }
-
- /* Enable double tap event on either INT1 or INT2 pin */
- switch (int_pin)
- {
- case LSM6DSO_INT1_PIN:
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
+ int ret = 0;
+ lsm6dso_pin_int1_route_t val1;
+ lsm6dso_pin_int2_route_t val2;
+
+ /* Output Data Rate selection */
+ if (set_x_odr(416.0f) != 0) {
+ return 1;
+ }
+
+ /* Full scale selection */
+ if (set_x_fs(2.0f) != 0) {
+ return 1;
+ }
+
+ /* Enable X direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_x_set(&_reg_ctx, PROPERTY_ENABLE) != 0) {
+ return 1;
+ }
+
+ /* Enable Y direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_y_set(&_reg_ctx, PROPERTY_ENABLE) != 0) {
+ return 1;
+ }
+
+ /* Enable Z direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_z_set(&_reg_ctx, PROPERTY_ENABLE) != 0) {
+ return 1;
+ }
+
+ /* Set tap threshold. */
+ if (lsm6dso_tap_threshold_x_set(&_reg_ctx, 0x08) != 0) {
+ return 1;
+ }
+
+ /* Set tap shock time window. */
+ if (lsm6dso_tap_shock_set(&_reg_ctx, 0x03) != 0) {
+ return 1;
+ }
+
+ /* Set tap quiet time window. */
+ if (lsm6dso_tap_quiet_set(&_reg_ctx, 0x03) != 0) {
return 1;
- }
-
- val1.md1_cfg.int1_double_tap = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
+ }
+
+ /* Set tap duration time window. */
+ if (lsm6dso_tap_dur_set(&_reg_ctx, 0x08) != 0) {
return 1;
- }
- break;
-
- case LSM6DSO_INT2_PIN:
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
+ }
+
+ /* Single and double tap enabled. */
+ if (lsm6dso_tap_mode_set(&_reg_ctx, LSM6DSO_BOTH_SINGLE_DOUBLE) != 0) {
return 1;
- }
-
- val2.md2_cfg.int2_double_tap = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ }
+
+ /* Enable double tap event on either INT1 or INT2 pin */
+ switch (int_pin) {
+ case LSM6DSO_INT1_PIN:
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.md1_cfg.int1_double_tap = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+ break;
+
+ case LSM6DSO_INT2_PIN:
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.md2_cfg.int2_double_tap = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -1778,83 +1628,71 @@
*/
int LSM6DSOSensor::disable_double_tap_detection()
{
- lsm6dso_pin_int1_route_t val1;
- lsm6dso_pin_int2_route_t val2;
-
- /* Disable double tap event on both INT1 and INT2 pins */
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- val1.md1_cfg.int1_double_tap = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- val2.md2_cfg.int2_double_tap = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- /* Only single tap enabled. */
- if (lsm6dso_tap_mode_set(&_reg_ctx, LSM6DSO_ONLY_SINGLE) != 0)
- {
- return 1;
- }
-
- /* Reset tap duration time window. */
- if (lsm6dso_tap_dur_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* Reset tap quiet time window. */
- if (lsm6dso_tap_quiet_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* Reset tap shock time window. */
- if (lsm6dso_tap_shock_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* Reset tap threshold. */
- if (lsm6dso_tap_threshold_x_set(&_reg_ctx, 0x00) != 0)
- {
- return 1;
- }
-
- /* Disable Z direction in tap recognition. */
- if (lsm6dso_tap_detection_on_z_set(&_reg_ctx, PROPERTY_DISABLE) != 0)
- {
- return 1;
- }
-
- /* Disable Y direction in tap recognition. */
- if (lsm6dso_tap_detection_on_y_set(&_reg_ctx, PROPERTY_DISABLE) != 0)
- {
- return 1;
- }
-
- /* Disable X direction in tap recognition. */
- if (lsm6dso_tap_detection_on_x_set(&_reg_ctx, PROPERTY_DISABLE) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_pin_int1_route_t val1;
+ lsm6dso_pin_int2_route_t val2;
+
+ /* Disable double tap event on both INT1 and INT2 pins */
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.md1_cfg.int1_double_tap = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.md2_cfg.int2_double_tap = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ /* Only single tap enabled. */
+ if (lsm6dso_tap_mode_set(&_reg_ctx, LSM6DSO_ONLY_SINGLE) != 0) {
+ return 1;
+ }
+
+ /* Reset tap duration time window. */
+ if (lsm6dso_tap_dur_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* Reset tap quiet time window. */
+ if (lsm6dso_tap_quiet_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* Reset tap shock time window. */
+ if (lsm6dso_tap_shock_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* Reset tap threshold. */
+ if (lsm6dso_tap_threshold_x_set(&_reg_ctx, 0x00) != 0) {
+ return 1;
+ }
+
+ /* Disable Z direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_z_set(&_reg_ctx, PROPERTY_DISABLE) != 0) {
+ return 1;
+ }
+
+ /* Disable Y direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_y_set(&_reg_ctx, PROPERTY_DISABLE) != 0) {
+ return 1;
+ }
+
+ /* Disable X direction in tap recognition. */
+ if (lsm6dso_tap_detection_on_x_set(&_reg_ctx, PROPERTY_DISABLE) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1864,13 +1702,12 @@
*/
int LSM6DSOSensor::set_tap_threshold(uint8_t thr)
{
- /* Set tap threshold. */
- if (lsm6dso_tap_threshold_x_set(&_reg_ctx, thr) != 0)
- {
- return 1;
- }
-
- return 0;
+ /* Set tap threshold. */
+ if (lsm6dso_tap_threshold_x_set(&_reg_ctx, thr) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1880,13 +1717,12 @@
*/
int LSM6DSOSensor::set_tap_shock_time(uint8_t time)
{
- /* Set tap shock time window. */
- if (lsm6dso_tap_shock_set(&_reg_ctx, time) != 0)
- {
- return 1;
- }
-
- return 0;
+ /* Set tap shock time window. */
+ if (lsm6dso_tap_shock_set(&_reg_ctx, time) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1896,13 +1732,12 @@
*/
int LSM6DSOSensor::set_tap_quiet_time(uint8_t time)
{
- /* Set tap quiet time window. */
- if (lsm6dso_tap_quiet_set(&_reg_ctx, time) != 0)
- {
- return 1;
- }
-
- return 0;
+ /* Set tap quiet time window. */
+ if (lsm6dso_tap_quiet_set(&_reg_ctx, time) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1912,13 +1747,12 @@
*/
int LSM6DSOSensor::set_tap_duration_time(uint8_t time)
{
- /* Set tap duration time window. */
- if (lsm6dso_tap_dur_set(&_reg_ctx, time) != 0)
- {
- return 1;
- }
-
- return 0;
+ /* Set tap duration time window. */
+ if (lsm6dso_tap_dur_set(&_reg_ctx, time) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -1928,65 +1762,57 @@
*/
int LSM6DSOSensor::enable_6d_orientation(LSM6DSO_Interrupt_Pin_t int_pin)
{
- int ret = 0;
- lsm6dso_pin_int1_route_t val1;
- lsm6dso_pin_int2_route_t val2;
-
- /* Output Data Rate selection */
- if (set_x_odr(416.0f) != 0)
- {
- return 1;
- }
-
- /* Full scale selection */
- if (set_x_fs(2.0f) != 0)
- {
- return 1;
- }
-
- /* 6D orientation enabled. */
- if (lsm6dso_6d_threshold_set(&_reg_ctx, LSM6DSO_DEG_60) != 0)
- {
- return 1;
- }
-
- /* Enable 6D orientation event on either INT1 or INT2 pin */
- switch (int_pin)
- {
- case LSM6DSO_INT1_PIN:
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
+ int ret = 0;
+ lsm6dso_pin_int1_route_t val1;
+ lsm6dso_pin_int2_route_t val2;
+
+ /* Output Data Rate selection */
+ if (set_x_odr(416.0f) != 0) {
+ return 1;
+ }
+
+ /* Full scale selection */
+ if (set_x_fs(2.0f) != 0) {
+ return 1;
+ }
+
+ /* 6D orientation enabled. */
+ if (lsm6dso_6d_threshold_set(&_reg_ctx, LSM6DSO_DEG_60) != 0) {
return 1;
- }
-
- val1.md1_cfg.int1_6d = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
- break;
-
- case LSM6DSO_INT2_PIN:
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- val2.md2_cfg.int2_6d = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
- break;
-
- default:
- ret = 1;
- break;
- }
-
- return ret;
+ }
+
+ /* Enable 6D orientation event on either INT1 or INT2 pin */
+ switch (int_pin) {
+ case LSM6DSO_INT1_PIN:
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.md1_cfg.int1_6d = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+ break;
+
+ case LSM6DSO_INT2_PIN:
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.md2_cfg.int2_6d = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ return ret;
}
/**
@@ -1995,41 +1821,36 @@
*/
int LSM6DSOSensor::disable_6d_orientation()
{
- lsm6dso_pin_int1_route_t val1;
- lsm6dso_pin_int2_route_t val2;
-
- /* Disable 6D orientation event on both INT1 and INT2 pins */
- if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- val1.md1_cfg.int1_6d = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- val2.md2_cfg.int2_6d = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0)
- {
- return 1;
- }
-
- /* Reset 6D orientation. */
- if (lsm6dso_6d_threshold_set(&_reg_ctx, LSM6DSO_DEG_80) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_pin_int1_route_t val1;
+ lsm6dso_pin_int2_route_t val2;
+
+ /* Disable 6D orientation event on both INT1 and INT2 pins */
+ if (lsm6dso_pin_int1_route_get(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ val1.md1_cfg.int1_6d = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ val2.md2_cfg.int2_6d = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ return 1;
+ }
+
+ /* Reset 6D orientation. */
+ if (lsm6dso_6d_threshold_set(&_reg_ctx, LSM6DSO_DEG_80) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2039,12 +1860,11 @@
*/
int LSM6DSOSensor::set_6d_orientation_threshold(uint8_t thr)
{
- if (lsm6dso_6d_threshold_set(&_reg_ctx, (lsm6dso_sixd_ths_t)thr) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_6d_threshold_set(&_reg_ctx, (lsm6dso_sixd_ths_t)thr) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2054,16 +1874,15 @@
*/
int LSM6DSOSensor::get_6d_orientation_xl(uint8_t *xl)
{
- lsm6dso_d6d_src_t data;
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0)
- {
- return 1;
- }
-
- *xl = data.xl;
-
- return 0;
+ lsm6dso_d6d_src_t data;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0) {
+ return 1;
+ }
+
+ *xl = data.xl;
+
+ return 0;
}
/**
@@ -2073,16 +1892,15 @@
*/
int LSM6DSOSensor::get_6d_orientation_xh(uint8_t *xh)
{
- lsm6dso_d6d_src_t data;
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0)
- {
- return 1;
- }
-
- *xh = data.xh;
-
- return 0;
+ lsm6dso_d6d_src_t data;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0) {
+ return 1;
+ }
+
+ *xh = data.xh;
+
+ return 0;
}
/**
@@ -2092,16 +1910,15 @@
*/
int LSM6DSOSensor::get_6d_orientation_yl(uint8_t *yl)
{
- lsm6dso_d6d_src_t data;
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0)
- {
- return 1;
- }
-
- *yl = data.yl;
-
- return 0;
+ lsm6dso_d6d_src_t data;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0) {
+ return 1;
+ }
+
+ *yl = data.yl;
+
+ return 0;
}
/**
@@ -2111,16 +1928,15 @@
*/
int LSM6DSOSensor::get_6d_orientation_yh(uint8_t *yh)
{
- lsm6dso_d6d_src_t data;
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0)
- {
- return 1;
- }
-
- *yh = data.yh;
-
- return 0;
+ lsm6dso_d6d_src_t data;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0) {
+ return 1;
+ }
+
+ *yh = data.yh;
+
+ return 0;
}
/**
@@ -2130,16 +1946,15 @@
*/
int LSM6DSOSensor::get_6d_orientation_zl(uint8_t *zl)
{
- lsm6dso_d6d_src_t data;
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0)
- {
- return 1;
- }
-
- *zl = data.zl;
-
- return 0;
+ lsm6dso_d6d_src_t data;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0) {
+ return 1;
+ }
+
+ *zl = data.zl;
+
+ return 0;
}
/**
@@ -2149,16 +1964,15 @@
*/
int LSM6DSOSensor::get_6d_orientation_zh(uint8_t *zh)
{
- lsm6dso_d6d_src_t data;
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0)
- {
- return 1;
- }
-
- *zh = data.zh;
-
- return 0;
+ lsm6dso_d6d_src_t data;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&data, 1) != 0) {
+ return 1;
+ }
+
+ *zh = data.zh;
+
+ return 0;
}
/**
@@ -2168,12 +1982,11 @@
*/
int LSM6DSOSensor::get_x_drdy_status(uint8_t *status)
{
- if (lsm6dso_xl_flag_data_ready_get(&_reg_ctx, status) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_xl_flag_data_ready_get(&_reg_ctx, status) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2183,130 +1996,105 @@
*/
int LSM6DSOSensor::get_event_status(LSM6DSO_Event_Status_t *status)
{
- uint8_t tilt_ia = 0U;
- lsm6dso_wake_up_src_t wake_up_src;
- lsm6dso_tap_src_t tap_src;
- lsm6dso_d6d_src_t d6d_src;
- lsm6dso_emb_func_src_t func_src;
- lsm6dso_md1_cfg_t md1_cfg;
- lsm6dso_md2_cfg_t md2_cfg;
- lsm6dso_emb_func_int1_t int1_ctrl;
- lsm6dso_emb_func_int2_t int2_ctrl;
-
- (void)memset((void *)status, 0x0, sizeof(LSM6DSO_Event_Status_t));
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_WAKE_UP_SRC, (uint8_t *)&wake_up_src, 1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_TAP_SRC, (uint8_t *)&tap_src, 1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&d6d_src, 1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_mem_bank_set(&_reg_ctx, LSM6DSO_EMBEDDED_FUNC_BANK) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_EMB_FUNC_SRC, (uint8_t *)&func_src, 1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_EMB_FUNC_INT1, (uint8_t *)&int1_ctrl, 1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_EMB_FUNC_INT2, (uint8_t *)&int2_ctrl, 1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_mem_bank_set(&_reg_ctx, LSM6DSO_USER_BANK) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_MD1_CFG, (uint8_t *)&md1_cfg, 1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_MD2_CFG, (uint8_t *)&md2_cfg, 1) != 0)
- {
- return 1;
- }
-
- if (lsm6dso_tilt_flag_data_ready_get(&_reg_ctx, &tilt_ia) != 0)
- {
- return 1;
- }
-
- if ((md1_cfg.int1_ff == 1U) || (md2_cfg.int2_ff == 1U))
- {
- if (wake_up_src.ff_ia == 1U)
- {
- status->FreeFallStatus = 1;
+ uint8_t tilt_ia = 0U;
+ lsm6dso_wake_up_src_t wake_up_src;
+ lsm6dso_tap_src_t tap_src;
+ lsm6dso_d6d_src_t d6d_src;
+ lsm6dso_emb_func_src_t func_src;
+ lsm6dso_md1_cfg_t md1_cfg;
+ lsm6dso_md2_cfg_t md2_cfg;
+ lsm6dso_emb_func_int1_t int1_ctrl;
+ lsm6dso_emb_func_int2_t int2_ctrl;
+
+ (void)memset((void *)status, 0x0, sizeof(LSM6DSO_Event_Status_t));
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_WAKE_UP_SRC, (uint8_t *)&wake_up_src, 1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_TAP_SRC, (uint8_t *)&tap_src, 1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_D6D_SRC, (uint8_t *)&d6d_src, 1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_mem_bank_set(&_reg_ctx, LSM6DSO_EMBEDDED_FUNC_BANK) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_EMB_FUNC_SRC, (uint8_t *)&func_src, 1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_EMB_FUNC_INT1, (uint8_t *)&int1_ctrl, 1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_EMB_FUNC_INT2, (uint8_t *)&int2_ctrl, 1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_mem_bank_set(&_reg_ctx, LSM6DSO_USER_BANK) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_MD1_CFG, (uint8_t *)&md1_cfg, 1) != 0) {
+ return 1;
}
- }
-
- if ((md1_cfg.int1_wu == 1U) || (md2_cfg.int2_wu == 1U))
- {
- if (wake_up_src.wu_ia == 1U)
- {
- status->WakeUpStatus = 1;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_MD2_CFG, (uint8_t *)&md2_cfg, 1) != 0) {
+ return 1;
+ }
+
+ if (lsm6dso_tilt_flag_data_ready_get(&_reg_ctx, &tilt_ia) != 0) {
+ return 1;
}
- }
-
- if ((md1_cfg.int1_single_tap == 1U) || (md2_cfg.int2_single_tap == 1U))
- {
- if (tap_src.single_tap == 1U)
- {
- status->TapStatus = 1;
+
+ if ((md1_cfg.int1_ff == 1U) || (md2_cfg.int2_ff == 1U)) {
+ if (wake_up_src.ff_ia == 1U) {
+ status->FreeFallStatus = 1;
+ }
}
- }
-
- if ((md1_cfg.int1_double_tap == 1U) || (md2_cfg.int2_double_tap == 1U))
- {
- if (tap_src.double_tap == 1U)
- {
- status->DoubleTapStatus = 1;
+
+ if ((md1_cfg.int1_wu == 1U) || (md2_cfg.int2_wu == 1U)) {
+ if (wake_up_src.wu_ia == 1U) {
+ status->WakeUpStatus = 1;
+ }
+ }
+
+ if ((md1_cfg.int1_single_tap == 1U) || (md2_cfg.int2_single_tap == 1U)) {
+ if (tap_src.single_tap == 1U) {
+ status->TapStatus = 1;
+ }
}
- }
-
- if ((md1_cfg.int1_6d == 1U) || (md2_cfg.int2_6d == 1U))
- {
- if (d6d_src.d6d_ia == 1U)
- {
- status->D6DOrientationStatus = 1;
+
+ if ((md1_cfg.int1_double_tap == 1U) || (md2_cfg.int2_double_tap == 1U)) {
+ if (tap_src.double_tap == 1U) {
+ status->DoubleTapStatus = 1;
+ }
+ }
+
+ if ((md1_cfg.int1_6d == 1U) || (md2_cfg.int2_6d == 1U)) {
+ if (d6d_src.d6d_ia == 1U) {
+ status->D6DOrientationStatus = 1;
+ }
}
- }
-
- if (int1_ctrl.int1_step_detector == 1U)
- {
- if (func_src.step_detected == 1U)
- {
- status->StepStatus = 1;
+
+ if (int1_ctrl.int1_step_detector == 1U) {
+ if (func_src.step_detected == 1U) {
+ status->StepStatus = 1;
+ }
}
- }
-
- if ((int1_ctrl.int1_tilt == 1U) || (int2_ctrl.int2_tilt == 1U))
- {
- if (tilt_ia == 1U)
- {
- status->TiltStatus = 1;
+
+ if ((int1_ctrl.int1_tilt == 1U) || (int2_ctrl.int2_tilt == 1U)) {
+ if (tilt_ia == 1U) {
+ status->TiltStatus = 1;
+ }
}
- }
-
- return 0;
+
+ return 0;
}
/**
@@ -2316,19 +2104,18 @@
*/
int LSM6DSOSensor::set_x_self_test(uint8_t val)
{
- lsm6dso_st_xl_t reg;
-
- reg = (val == 0U) ? LSM6DSO_XL_ST_DISABLE
- : (val == 1U) ? LSM6DSO_XL_ST_POSITIVE
- : (val == 2U) ? LSM6DSO_XL_ST_NEGATIVE
- : LSM6DSO_XL_ST_DISABLE;
-
- if (lsm6dso_xl_self_test_set(&_reg_ctx, reg) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_st_xl_t reg;
+
+ reg = (val == 0U) ? LSM6DSO_XL_ST_DISABLE
+ : (val == 1U) ? LSM6DSO_XL_ST_POSITIVE
+ : (val == 2U) ? LSM6DSO_XL_ST_NEGATIVE
+ : LSM6DSO_XL_ST_DISABLE;
+
+ if (lsm6dso_xl_self_test_set(&_reg_ctx, reg) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2338,12 +2125,11 @@
*/
int LSM6DSOSensor::get_g_drdy_status(uint8_t *status)
{
- if (lsm6dso_gy_flag_data_ready_get(&_reg_ctx, status) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_gy_flag_data_ready_get(&_reg_ctx, status) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2353,20 +2139,19 @@
*/
int LSM6DSOSensor::set_g_self_test(uint8_t val)
{
- lsm6dso_st_g_t reg;
-
- reg = (val == 0U) ? LSM6DSO_GY_ST_DISABLE
- : (val == 1U) ? LSM6DSO_GY_ST_POSITIVE
- : (val == 2U) ? LSM6DSO_GY_ST_NEGATIVE
- : LSM6DSO_GY_ST_DISABLE;
-
-
- if (lsm6dso_gy_self_test_set(&_reg_ctx, reg) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_st_g_t reg;
+
+ reg = (val == 0U) ? LSM6DSO_GY_ST_DISABLE
+ : (val == 1U) ? LSM6DSO_GY_ST_POSITIVE
+ : (val == 2U) ? LSM6DSO_GY_ST_NEGATIVE
+ : LSM6DSO_GY_ST_DISABLE;
+
+
+ if (lsm6dso_gy_self_test_set(&_reg_ctx, reg) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2376,12 +2161,11 @@
*/
int LSM6DSOSensor::get_fifo_num_samples(uint16_t *num_samples)
{
- if (lsm6dso_fifo_data_level_get(&_reg_ctx, num_samples) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_fifo_data_level_get(&_reg_ctx, num_samples) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2391,16 +2175,15 @@
*/
int LSM6DSOSensor::get_fifo_full_status(uint8_t *status)
{
- lsm6dso_reg_t reg;
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_FIFO_STATUS2, ®.byte, 1) != 0)
- {
- return 1;
- }
-
- *status = reg.fifo_status2.fifo_full_ia;
-
- return 0;
+ lsm6dso_reg_t reg;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_FIFO_STATUS2, ®.byte, 1) != 0) {
+ return 1;
+ }
+
+ *status = reg.fifo_status2.fifo_full_ia;
+
+ return 0;
}
/**
@@ -2410,21 +2193,19 @@
*/
int LSM6DSOSensor::set_fifo_int1_fifo_full(uint8_t status)
{
- lsm6dso_reg_t reg;
-
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_INT1_CTRL, ®.byte, 1) != 0)
- {
- return 1;
- }
-
- reg.int1_ctrl.int1_fifo_full = status;
-
- if (lsm6dso_write_reg(&_reg_ctx, LSM6DSO_INT1_CTRL, ®.byte, 1) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_reg_t reg;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_INT1_CTRL, ®.byte, 1) != 0) {
+ return 1;
+ }
+
+ reg.int1_ctrl.int1_fifo_full = status;
+
+ if (lsm6dso_write_reg(&_reg_ctx, LSM6DSO_INT1_CTRL, ®.byte, 1) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2434,12 +2215,11 @@
*/
int LSM6DSOSensor::set_fifo_watermark_level(uint16_t watermark)
{
- if (lsm6dso_fifo_watermark_set(&_reg_ctx, watermark) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_fifo_watermark_set(&_reg_ctx, watermark) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2449,12 +2229,11 @@
*/
int LSM6DSOSensor::set_fifo_stop_on_fth(uint8_t status)
{
- if (lsm6dso_fifo_stop_on_wtm_set(&_reg_ctx, status) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_fifo_stop_on_wtm_set(&_reg_ctx, status) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2464,34 +2243,31 @@
*/
int LSM6DSOSensor::set_fifo_mode(uint8_t mode)
{
- int ret = 0;
-
- /* Verify that the passed parameter contains one of the valid values. */
- switch ((lsm6dso_fifo_mode_t)mode)
- {
- case LSM6DSO_BYPASS_MODE:
- case LSM6DSO_FIFO_MODE:
- case LSM6DSO_STREAM_TO_FIFO_MODE:
- case LSM6DSO_BYPASS_TO_STREAM_MODE:
- case LSM6DSO_STREAM_MODE:
- break;
-
- default:
- ret = 1;
- break;
- }
-
- if (ret == 1)
- {
+ int ret = 0;
+
+ /* Verify that the passed parameter contains one of the valid values. */
+ switch ((lsm6dso_fifo_mode_t)mode) {
+ case LSM6DSO_BYPASS_MODE:
+ case LSM6DSO_FIFO_MODE:
+ case LSM6DSO_STREAM_TO_FIFO_MODE:
+ case LSM6DSO_BYPASS_TO_STREAM_MODE:
+ case LSM6DSO_STREAM_MODE:
+ break;
+
+ default:
+ ret = 1;
+ break;
+ }
+
+ if (ret == 1) {
+ return ret;
+ }
+
+ if (lsm6dso_fifo_mode_set(&_reg_ctx, (lsm6dso_fifo_mode_t)mode) != 0) {
+ return 1;
+ }
+
return ret;
- }
-
- if (lsm6dso_fifo_mode_set(&_reg_ctx, (lsm6dso_fifo_mode_t)mode) != 0)
- {
- return 1;
- }
-
- return ret;
}
/**
@@ -2501,16 +2277,15 @@
*/
int LSM6DSOSensor::get_fifo_tag(uint8_t *tag)
{
- lsm6dso_fifo_tag_t tag_local;
-
- if (lsm6dso_fifo_sensor_tag_get(&_reg_ctx, &tag_local) != 0)
- {
- return 1;
- }
-
- *tag = (uint8_t)tag_local;
-
- return 0;
+ lsm6dso_fifo_tag_t tag_local;
+
+ if (lsm6dso_fifo_sensor_tag_get(&_reg_ctx, &tag_local) != 0) {
+ return 1;
+ }
+
+ *tag = (uint8_t)tag_local;
+
+ return 0;
}
/**
@@ -2520,12 +2295,11 @@
*/
int LSM6DSOSensor::get_fifo_data(uint8_t *data)
{
- if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_FIFO_DATA_OUT_X_L, data, 6) != 0)
- {
- return 1;
- }
-
- return 0;
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_FIFO_DATA_OUT_X_L, data, 6) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2535,34 +2309,32 @@
*/
int LSM6DSOSensor::get_fifo_x_axes(int32_t *acceleration)
{
- uint8_t data[6];
- int16_t data_raw[3];
- float sensitivity = 0.0f;
- float acceleration_float[3];
-
- if (get_fifo_data(data) != 0)
- {
- return 1;
- }
-
- data_raw[0] = ((int16_t)data[1] << 8) | data[0];
- data_raw[1] = ((int16_t)data[3] << 8) | data[2];
- data_raw[2] = ((int16_t)data[5] << 8) | data[4];
-
- if (get_x_sensitivity(&sensitivity) != 0)
- {
- return 1;
- }
-
- acceleration_float[0] = (float)data_raw[0] * sensitivity;
- acceleration_float[1] = (float)data_raw[1] * sensitivity;
- acceleration_float[2] = (float)data_raw[2] * sensitivity;
-
- acceleration[0] = (int32_t)acceleration_float[0];
- acceleration[1] = (int32_t)acceleration_float[1];
- acceleration[2] = (int32_t)acceleration_float[2];
-
- return 0;
+ uint8_t data[6];
+ int16_t data_raw[3];
+ float sensitivity = 0.0f;
+ float acceleration_float[3];
+
+ if (get_fifo_data(data) != 0) {
+ return 1;
+ }
+
+ data_raw[0] = ((int16_t)data[1] << 8) | data[0];
+ data_raw[1] = ((int16_t)data[3] << 8) | data[2];
+ data_raw[2] = ((int16_t)data[5] << 8) | data[4];
+
+ if (get_x_sensitivity(&sensitivity) != 0) {
+ return 1;
+ }
+
+ acceleration_float[0] = (float)data_raw[0] * sensitivity;
+ acceleration_float[1] = (float)data_raw[1] * sensitivity;
+ acceleration_float[2] = (float)data_raw[2] * sensitivity;
+
+ acceleration[0] = (int32_t)acceleration_float[0];
+ acceleration[1] = (int32_t)acceleration_float[1];
+ acceleration[2] = (int32_t)acceleration_float[2];
+
+ return 0;
}
/**
@@ -2572,26 +2344,25 @@
*/
int LSM6DSOSensor::set_fifo_x_bdr(float bdr)
{
- lsm6dso_bdr_xl_t new_bdr;
-
- new_bdr = (bdr <= 0.0f) ? LSM6DSO_XL_NOT_BATCHED
- : (bdr <= 12.5f) ? LSM6DSO_XL_BATCHED_AT_12Hz5
- : (bdr <= 26.0f) ? LSM6DSO_XL_BATCHED_AT_26Hz
- : (bdr <= 52.0f) ? LSM6DSO_XL_BATCHED_AT_52Hz
- : (bdr <= 104.0f) ? LSM6DSO_XL_BATCHED_AT_104Hz
- : (bdr <= 208.0f) ? LSM6DSO_XL_BATCHED_AT_208Hz
- : (bdr <= 416.0f) ? LSM6DSO_XL_BATCHED_AT_417Hz
- : (bdr <= 833.0f) ? LSM6DSO_XL_BATCHED_AT_833Hz
- : (bdr <= 1660.0f) ? LSM6DSO_XL_BATCHED_AT_1667Hz
- : (bdr <= 3330.0f) ? LSM6DSO_XL_BATCHED_AT_3333Hz
- : LSM6DSO_XL_BATCHED_AT_6667Hz;
-
- if (lsm6dso_fifo_xl_batch_set(&_reg_ctx, new_bdr) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_bdr_xl_t new_bdr;
+
+ new_bdr = (bdr <= 0.0f) ? LSM6DSO_XL_NOT_BATCHED
+ : (bdr <= 12.5f) ? LSM6DSO_XL_BATCHED_AT_12Hz5
+ : (bdr <= 26.0f) ? LSM6DSO_XL_BATCHED_AT_26Hz
+ : (bdr <= 52.0f) ? LSM6DSO_XL_BATCHED_AT_52Hz
+ : (bdr <= 104.0f) ? LSM6DSO_XL_BATCHED_AT_104Hz
+ : (bdr <= 208.0f) ? LSM6DSO_XL_BATCHED_AT_208Hz
+ : (bdr <= 416.0f) ? LSM6DSO_XL_BATCHED_AT_417Hz
+ : (bdr <= 833.0f) ? LSM6DSO_XL_BATCHED_AT_833Hz
+ : (bdr <= 1660.0f) ? LSM6DSO_XL_BATCHED_AT_1667Hz
+ : (bdr <= 3330.0f) ? LSM6DSO_XL_BATCHED_AT_3333Hz
+ : LSM6DSO_XL_BATCHED_AT_6667Hz;
+
+ if (lsm6dso_fifo_xl_batch_set(&_reg_ctx, new_bdr) != 0) {
+ return 1;
+ }
+
+ return 0;
}
/**
@@ -2601,34 +2372,32 @@
*/
int LSM6DSOSensor::get_fifo_g_axes(int32_t *angular_velocity)
{
- uint8_t data[6];
- int16_t data_raw[3];
- float sensitivity = 0.0f;
- float angular_velocity_float[3];
-
- if (get_fifo_data(data) != 0)
- {
- return 1;
- }
-
- data_raw[0] = ((int16_t)data[1] << 8) | data[0];
- data_raw[1] = ((int16_t)data[3] << 8) | data[2];
- data_raw[2] = ((int16_t)data[5] << 8) | data[4];
-
- if (get_g_sensitivity(&sensitivity) != 0)
- {
- return 1;
- }
-
- angular_velocity_float[0] = (float)data_raw[0] * sensitivity;
- angular_velocity_float[1] = (float)data_raw[1] * sensitivity;
- angular_velocity_float[2] = (float)data_raw[2] * sensitivity;
-
- angular_velocity[0] = (int32_t)angular_velocity_float[0];
- angular_velocity[1] = (int32_t)angular_velocity_float[1];
- angular_velocity[2] = (int32_t)angular_velocity_float[2];
-
- return 0;
+ uint8_t data[6];
+ int16_t data_raw[3];
+ float sensitivity = 0.0f;
+ float angular_velocity_float[3];
+
+ if (get_fifo_data(data) != 0) {
+ return 1;
+ }
+
+ data_raw[0] = ((int16_t)data[1] << 8) | data[0];
+ data_raw[1] = ((int16_t)data[3] << 8) | data[2];
+ data_raw[2] = ((int16_t)data[5] << 8) | data[4];
+
+ if (get_g_sensitivity(&sensitivity) != 0) {
+ return 1;
+ }
+
+ angular_velocity_float[0] = (float)data_raw[0] * sensitivity;
+ angular_velocity_float[1] = (float)data_raw[1] * sensitivity;
+ angular_velocity_float[2] = (float)data_raw[2] * sensitivity;
+
+ angular_velocity[0] = (int32_t)angular_velocity_float[0];
+ angular_velocity[1] = (int32_t)angular_velocity_float[1];
+ angular_velocity[2] = (int32_t)angular_velocity_float[2];
+
+ return 0;
}
/**
@@ -2638,36 +2407,35 @@
*/
int LSM6DSOSensor::set_fifo_g_bdr(float bdr)
{
- lsm6dso_bdr_gy_t new_bdr;
-
- new_bdr = (bdr <= 0.0f) ? LSM6DSO_GY_NOT_BATCHED
- : (bdr <= 12.5f) ? LSM6DSO_GY_BATCHED_AT_12Hz5
- : (bdr <= 26.0f) ? LSM6DSO_GY_BATCHED_AT_26Hz
- : (bdr <= 52.0f) ? LSM6DSO_GY_BATCHED_AT_52Hz
- : (bdr <= 104.0f) ? LSM6DSO_GY_BATCHED_AT_104Hz
- : (bdr <= 208.0f) ? LSM6DSO_GY_BATCHED_AT_208Hz
- : (bdr <= 416.0f) ? LSM6DSO_GY_BATCHED_AT_417Hz
- : (bdr <= 833.0f) ? LSM6DSO_GY_BATCHED_AT_833Hz
- : (bdr <= 1660.0f) ? LSM6DSO_GY_BATCHED_AT_1667Hz
- : (bdr <= 3330.0f) ? LSM6DSO_GY_BATCHED_AT_3333Hz
- : LSM6DSO_GY_BATCHED_AT_6667Hz;
-
- if (lsm6dso_fifo_gy_batch_set(&_reg_ctx, new_bdr) != 0)
- {
- return 1;
- }
-
- return 0;
+ lsm6dso_bdr_gy_t new_bdr;
+
+ new_bdr = (bdr <= 0.0f) ? LSM6DSO_GY_NOT_BATCHED
+ : (bdr <= 12.5f) ? LSM6DSO_GY_BATCHED_AT_12Hz5
+ : (bdr <= 26.0f) ? LSM6DSO_GY_BATCHED_AT_26Hz
+ : (bdr <= 52.0f) ? LSM6DSO_GY_BATCHED_AT_52Hz
+ : (bdr <= 104.0f) ? LSM6DSO_GY_BATCHED_AT_104Hz
+ : (bdr <= 208.0f) ? LSM6DSO_GY_BATCHED_AT_208Hz
+ : (bdr <= 416.0f) ? LSM6DSO_GY_BATCHED_AT_417Hz
+ : (bdr <= 833.0f) ? LSM6DSO_GY_BATCHED_AT_833Hz
+ : (bdr <= 1660.0f) ? LSM6DSO_GY_BATCHED_AT_1667Hz
+ : (bdr <= 3330.0f) ? LSM6DSO_GY_BATCHED_AT_3333Hz
+ : LSM6DSO_GY_BATCHED_AT_6667Hz;
+
+ if (lsm6dso_fifo_gy_batch_set(&_reg_ctx, new_bdr) != 0) {
+ return 1;
+ }
+
+ return 0;
}
int32_t LSM6DSO_io_write(void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite)
{
- return ((LSM6DSOSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite);
+ return ((LSM6DSOSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite);
}
int32_t LSM6DSO_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead)
{
- return ((LSM6DSOSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead);
+ return ((LSM6DSOSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead);
}