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:
- 4:77faf76e3cd8
- Parent:
- 3:4274d9103f1d
diff -r 4274d9103f1d -r 77faf76e3cd8 LSM6DSOSensor.cpp
--- a/LSM6DSOSensor.cpp Wed Jul 24 14:19:35 2019 +0000
+++ b/LSM6DSOSensor.cpp Thu Oct 29 12:50:52 2020 +0000
@@ -274,8 +274,8 @@
*odr = 0.0f;
break;
- case LSM6DSO_XL_ODR_6Hz5:
- *odr = 6.5f;
+ case LSM6DSO_XL_ODR_1Hz6:
+ *odr = 1.6f;
break;
case LSM6DSO_XL_ODR_12Hz5:
@@ -333,12 +333,200 @@
*/
int LSM6DSOSensor::set_x_odr(float odr)
{
+ return set_x_odr_with_mode(odr, LSM6DSO_ACC_HIGH_PERFORMANCE_MODE);
+}
+
+/**
+ * @brief Set the LSM6DSO accelerometer sensor output data rate with operating mode
+ * @param odr the output data rate value to be set
+ * @param mode the accelerometer operating mode
+ * @note This function switches off the gyroscope if Ultra Low Power Mode is set
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::set_x_odr_with_mode(float odr, LSM6DSO_ACC_Operating_Mode_t mode)
+{
+ int ret = 0;
+
+ switch (mode)
+ {
+ case LSM6DSO_ACC_HIGH_PERFORMANCE_MODE:
+ {
+ /* We must uncheck Low Power and Ultra Low Power bits if they are enabled */
+ lsm6dso_ctrl5_c_t val1;
+ lsm6dso_ctrl6_c_t val2;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_CTRL5_C, (uint8_t *)&val1, 1) != 0)
+ {
+ return 1;
+ }
+
+ if (val1.xl_ulp_en)
+ {
+ /* Power off the accelerometer */
+ if (_x_is_enabled == 1U)
+ {
+ if (lsm6dso_xl_data_rate_set(&_reg_ctx, LSM6DSO_XL_ODR_OFF) != 0)
+ {
+ return 1;
+ }
+ }
+
+ val1.xl_ulp_en = 0;
+ if (lsm6dso_write_reg(&_reg_ctx, LSM6DSO_CTRL5_C, (uint8_t *)&val1, 1) != 0)
+ {
+ return 1;
+ }
+ }
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_CTRL6_C, (uint8_t *)&val2, 1) != 0)
+ {
+ return 1;
+ }
+
+ if (val2.xl_hm_mode)
+ {
+ val2.xl_hm_mode = 0;
+ if (lsm6dso_write_reg(&_reg_ctx, LSM6DSO_CTRL6_C, (uint8_t *)&val2, 1) != 0)
+ {
+ return 1;
+ }
+ }
+
+ /* ODR should be at least 12.5Hz */
+ if (odr < 12.5f)
+ {
+ odr = 12.5f;
+ }
+ break;
+ }
+ case LSM6DSO_ACC_LOW_POWER_NORMAL_MODE:
+ {
+ /* We must uncheck Ultra Low Power bit if it is enabled */
+ /* and check the Low Power bit if it is unchecked */
+ lsm6dso_ctrl5_c_t val1;
+ lsm6dso_ctrl6_c_t val2;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_CTRL5_C, (uint8_t *)&val1, 1) != 0)
+ {
+ return 1;
+ }
+
+ if (val1.xl_ulp_en)
+ {
+ /* Power off the accelerometer */
+ if (_x_is_enabled == 1U)
+ {
+ if (lsm6dso_xl_data_rate_set(&_reg_ctx, LSM6DSO_XL_ODR_OFF) != 0)
+ {
+ return 1;
+ }
+ }
+
+ val1.xl_ulp_en = 0;
+ if (lsm6dso_write_reg(&_reg_ctx, LSM6DSO_CTRL5_C, (uint8_t *)&val1, 1) != 0)
+ {
+ return 1;
+ }
+ }
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_CTRL6_C, (uint8_t *)&val2, 1) != 0)
+ {
+ return 1;
+ }
+
+ if (!val2.xl_hm_mode)
+ {
+ val2.xl_hm_mode = 1U;
+ if (lsm6dso_write_reg(&_reg_ctx, LSM6DSO_CTRL6_C, (uint8_t *)&val2, 1) != 0)
+ {
+ return 1;
+ }
+ }
+
+ /* Now we need to limit the ODR to 208 Hz if it is higher */
+ if (odr > 208.0f)
+ {
+ odr = 208.0f;
+ }
+ break;
+ }
+ case LSM6DSO_ACC_ULTRA_LOW_POWER_MODE:
+ {
+ /* We must uncheck Low Power bit if it is enabled */
+ /* and check the Ultra Low Power bit if it is unchecked */
+ /* We must switch off gyro otherwise Ultra Low Power does not work */
+ lsm6dso_ctrl5_c_t val1;
+ lsm6dso_ctrl6_c_t val2;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_CTRL6_C, (uint8_t *)&val2, 1) != 0)
+ {
+ return 1;
+ }
+
+ if (val2.xl_hm_mode)
+ {
+ val2.xl_hm_mode = 0;
+ if (lsm6dso_write_reg(&_reg_ctx, LSM6DSO_CTRL6_C, (uint8_t *)&val2, 1) != 0)
+ {
+ return 1;
+ }
+ }
+
+ /* Disable Gyro */
+ if (_g_is_enabled == 1U)
+ {
+ if (disable_g() != 0)
+ {
+ return 1;
+ }
+ }
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_CTRL5_C, (uint8_t *)&val1, 1) != 0)
+ {
+ return 1;
+ }
+
+ if (!val1.xl_ulp_en)
+ {
+ /* Power off the accelerometer */
+ if (_x_is_enabled == 1U)
+ {
+ if (lsm6dso_xl_data_rate_set(&_reg_ctx, LSM6DSO_XL_ODR_OFF) != 0)
+ {
+ return 1;
+ }
+ }
+
+ val1.xl_ulp_en = 1U;
+ if (lsm6dso_write_reg(&_reg_ctx, LSM6DSO_CTRL5_C, (uint8_t *)&val1, 1) != 0)
+ {
+ return 1;
+ }
+ }
+
+ /* Now we need to limit the ODR to 208 Hz if it is higher */
+ if (odr > 208.0f)
+ {
+ odr = 208.0f;
+ }
+ break;
+ }
+ default:
+ ret = 1;
+ break;
+ }
+
/* 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);
+ if (_x_is_enabled == 1U)
+ {
+ ret = set_x_odr_when_enabled(odr);
}
+ else
+ {
+ ret = set_x_odr_when_disabled(odr);
+ }
+
+ return ret;
}
/**
@@ -350,16 +538,17 @@
{
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;
+ new_odr = (odr <= 1.6f) ? LSM6DSO_XL_ODR_1Hz6
+ : (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) {
@@ -376,16 +565,17 @@
*/
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;
+ _x_last_odr = (odr <= 1.6f) ? LSM6DSO_XL_ODR_1Hz6
+ : (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;
}
@@ -443,9 +633,9 @@
/* 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;
+ : (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;
@@ -674,12 +864,83 @@
*/
int LSM6DSOSensor::set_g_odr(float odr)
{
+ return set_g_odr_with_mode(odr, LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE);
+}
+
+/**
+ * @brief Set the LSM6DSO gyroscope sensor output data rate with operating mode
+ * @param odr the output data rate value to be set
+ * @param mode the gyroscope operating mode
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::set_g_odr_with_mode(float odr, LSM6DSO_GYRO_Operating_Mode_t mode)
+{
+ int ret = 0;
+
+ switch (mode)
+ {
+ case LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE:
+ {
+ /* We must uncheck Low Power bit if it is enabled */
+ lsm6dso_ctrl7_g_t val1;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_CTRL7_G, (uint8_t *)&val1, 1) != 0)
+ {
+ return 1;
+ }
+
+ if (val1.g_hm_mode)
+ {
+ val1.g_hm_mode = 0;
+ if (lsm6dso_write_reg(&_reg_ctx, LSM6DSO_CTRL7_G, (uint8_t *)&val1, 1) != 0)
+ {
+ return 1;
+ }
+ }
+ break;
+ }
+ case LSM6DSO_GYRO_LOW_POWER_NORMAL_MODE:
+ {
+ /* We must check the Low Power bit if it is unchecked */
+ lsm6dso_ctrl7_g_t val1;
+
+ if (lsm6dso_read_reg(&_reg_ctx, LSM6DSO_CTRL7_G, (uint8_t *)&val1, 1) != 0)
+ {
+ return 1;
+ }
+
+ if (!val1.g_hm_mode)
+ {
+ val1.g_hm_mode = 1U;
+ if (lsm6dso_write_reg(&_reg_ctx, LSM6DSO_CTRL7_G, (uint8_t *)&val1, 1) != 0)
+ {
+ return 1;
+ }
+ }
+
+ /* Now we need to limit the ODR to 208 Hz if it is higher */
+ if (odr > 208.0f)
+ {
+ odr = 208.0f;
+ }
+ break;
+ }
+ default:
+ ret = 1;
+ break;
+ }
+
/* 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);
+ if (_g_is_enabled == 1U)
+ {
+ ret = set_g_odr_when_enabled(odr);
}
+ else
+ {
+ ret = set_g_odr_when_disabled(odr);
+ }
+
+ return ret;
}
/**
@@ -692,15 +953,15 @@
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;
+ : (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) {
@@ -718,15 +979,15 @@
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;
+ : (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;
}
@@ -786,10 +1047,10 @@
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;
+ : (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;
@@ -947,21 +1208,21 @@
return 1;
}
- val1.md1_cfg.int1_ff = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.free_fall = 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) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.md2_cfg.int2_ff = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.free_fall = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
break;
@@ -988,19 +1249,19 @@
return 1;
}
- val1.md1_cfg.int1_ff = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.free_fall = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, val1) != 0) {
return 1;
}
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.md2_cfg.int2_ff = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.free_fall = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
@@ -1054,6 +1315,7 @@
int LSM6DSOSensor::enable_pedometer()
{
lsm6dso_pin_int1_route_t val;
+ lsm6dso_emb_sens_t emb_sens;
/* Output Data Rate selection */
if (set_x_odr(26.0f) != 0) {
@@ -1065,8 +1327,32 @@
return 1;
}
+ /* Save current embedded features */
+ if (lsm6dso_embedded_sens_get(&_reg_ctx, &emb_sens) != 0)
+ {
+ return 1;
+ }
+
+ /* Turn off embedded features */
+ if (lsm6dso_embedded_sens_off(&_reg_ctx) != 0)
+ {
+ return 1;
+ }
+
+ /* Wait for 10 ms */
+ ThisThread::sleep_for(10);
+
/* Enable pedometer algorithm. */
- if (lsm6dso_pedo_sens_set(&_reg_ctx, LSM6DSO_PEDO_BASE_MODE) != 0) {
+ emb_sens.step = PROPERTY_ENABLE;
+
+ if (lsm6dso_pedo_sens_set(&_reg_ctx, LSM6DSO_PEDO_BASE_MODE) != 0)
+ {
+ return 1;
+ }
+
+ /* Turn on embedded features */
+ if (lsm6dso_embedded_sens_set(&_reg_ctx, &emb_sens) != 0)
+ {
return 1;
}
@@ -1075,9 +1361,9 @@
return 1;
}
- val.emb_func_int1.int1_step_detector = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val) != 0) {
+ val.step_detector = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, val) != 0) {
return 1;
}
@@ -1091,20 +1377,30 @@
int LSM6DSOSensor::disable_pedometer()
{
lsm6dso_pin_int1_route_t val1;
+ lsm6dso_emb_sens_t emb_sens;
/* 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) {
+ val1.step_detector = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, val1) != 0) {
+ return 1;
+ }
+
+ /* Save current embedded features */
+ if (lsm6dso_embedded_sens_get(&_reg_ctx, &emb_sens) != 0)
+ {
return 1;
}
/* Disable pedometer algorithm. */
- if (lsm6dso_pedo_sens_set(&_reg_ctx, LSM6DSO_PEDO_DISABLE) != 0) {
+ emb_sens.step = PROPERTY_DISABLE;
+
+ if (lsm6dso_embedded_sens_set(&_reg_ctx, &emb_sens) != 0)
+ {
return 1;
}
@@ -1148,6 +1444,7 @@
int ret = 0;
lsm6dso_pin_int1_route_t val1;
lsm6dso_pin_int2_route_t val2;
+ lsm6dso_emb_sens_t emb_sens;
/* Output Data Rate selection */
if (set_x_odr(26.0f) != 0) {
@@ -1159,8 +1456,27 @@
return 1;
}
- /* Enable tilt calculation. */
- if (lsm6dso_tilt_sens_set(&_reg_ctx, PROPERTY_ENABLE) != 0) {
+ /* Save current embedded features */
+ if (lsm6dso_embedded_sens_get(&_reg_ctx, &emb_sens) != 0)
+ {
+ return 1;
+ }
+
+ /* Turn off embedded features */
+ if (lsm6dso_embedded_sens_off(&_reg_ctx) != 0)
+ {
+ return 1;
+ }
+
+ /* Wait for 10 ms */
+ ThisThread::sleep_for(10);
+
+ /* Enable tilt algorithm. */
+ emb_sens.tilt = PROPERTY_ENABLE;
+
+ /* Turn on embedded features */
+ if (lsm6dso_embedded_sens_set(&_reg_ctx, &emb_sens) != 0)
+ {
return 1;
}
@@ -1171,21 +1487,21 @@
return 1;
}
- val1.emb_func_int1.int1_tilt = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.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) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.emb_func_int2.int2_tilt = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.tilt = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
break;
@@ -1206,30 +1522,40 @@
{
lsm6dso_pin_int1_route_t val1;
lsm6dso_pin_int2_route_t val2;
+ lsm6dso_emb_sens_t emb_sens;
/* 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) {
+ val1.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) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.emb_func_int2.int2_tilt = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.tilt = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
- /* Disable tilt calculation. */
- if (lsm6dso_tilt_sens_set(&_reg_ctx, PROPERTY_DISABLE) != 0) {
+ /* Save current embedded features */
+ if (lsm6dso_embedded_sens_get(&_reg_ctx, &emb_sens) != 0)
+ {
+ return 1;
+ }
+
+ /* Disable tilt algorithm. */
+ emb_sens.tilt = PROPERTY_DISABLE;
+
+ if (lsm6dso_embedded_sens_set(&_reg_ctx, &emb_sens) != 0)
+ {
return 1;
}
@@ -1274,21 +1600,21 @@
return 1;
}
- val1.md1_cfg.int1_wu = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.wake_up = 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) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.md2_cfg.int2_wu = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.wake_up = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
break;
@@ -1316,19 +1642,19 @@
return 1;
}
- val1.md1_cfg.int1_wu = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.wake_up = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, val1) != 0) {
return 1;
}
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.md2_cfg.int2_wu = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.wake_up = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
@@ -1437,21 +1763,21 @@
return 1;
}
- val1.md1_cfg.int1_single_tap = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.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) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.md2_cfg.int2_single_tap = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.single_tap = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
break;
@@ -1478,19 +1804,19 @@
return 1;
}
- val1.md1_cfg.int1_single_tap = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.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) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.md2_cfg.int2_single_tap = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.single_tap = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
@@ -1595,21 +1921,21 @@
return 1;
}
- val1.md1_cfg.int1_double_tap = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.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) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.md2_cfg.int2_double_tap = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.double_tap = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
break;
@@ -1636,19 +1962,19 @@
return 1;
}
- val1.md1_cfg.int1_double_tap = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.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) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.md2_cfg.int2_double_tap = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.double_tap = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
@@ -1788,21 +2114,21 @@
return 1;
}
- val1.md1_cfg.int1_6d = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.six_d = 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) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.md2_cfg.int2_6d = PROPERTY_ENABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.six_d = PROPERTY_ENABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}
break;
@@ -1829,19 +2155,19 @@
return 1;
}
- val1.md1_cfg.int1_6d = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int1_route_set(&_reg_ctx, &val1) != 0) {
+ val1.six_d = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int1_route_set(&_reg_ctx, val1) != 0) {
return 1;
}
- if (lsm6dso_pin_int2_route_get(&_reg_ctx, &val2) != 0) {
+ if (lsm6dso_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) {
return 1;
}
- val2.md2_cfg.int2_6d = PROPERTY_DISABLE;
-
- if (lsm6dso_pin_int2_route_set(&_reg_ctx, &val2) != 0) {
+ val2.six_d = PROPERTY_DISABLE;
+
+ if (lsm6dso_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) {
return 1;
}