in Drivers/BSP/Components/lsm6dso/lsm6dso_reg.c [10625:11037]
int32_t lsm6dso_mode_set(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx,
lsm6dso_md_t *val)
{
lsm6dso_func_cfg_access_t func_cfg_access;
lsm6dso_ctrl1_ois_t ctrl1_ois;
lsm6dso_ctrl2_ois_t ctrl2_ois;
lsm6dso_ctrl3_ois_t ctrl3_ois;
lsm6dso_ctrl1_xl_t ctrl1_xl;
lsm6dso_ctrl8_xl_t ctrl8_xl;
lsm6dso_ctrl2_g_t ctrl2_g;
lsm6dso_ctrl3_c_t ctrl3_c;
lsm6dso_ctrl4_c_t ctrl4_c;
lsm6dso_ctrl5_c_t ctrl5_c;
lsm6dso_ctrl6_c_t ctrl6_c;
lsm6dso_ctrl7_g_t ctrl7_g;
uint8_t xl_hm_mode;
uint8_t g_hm_mode;
uint8_t xl_ulp_en;
uint8_t odr_gy;
uint8_t odr_xl;
uint8_t reg[8];
int32_t ret;
ret = 0;
/* FIXME: Remove warnings with STM32CubeIDE */
ctrl3_c.not_used_01 = 0;
ctrl4_c.not_used_01 = 0;
/* reading input configuration */
xl_hm_mode = ( (uint8_t)val->ui.xl.odr & 0x10U ) >> 4;
xl_ulp_en = ( (uint8_t)val->ui.xl.odr & 0x20U ) >> 5;
odr_xl = (uint8_t)val->ui.xl.odr & 0x0FU;
/* if enable xl ultra low power mode disable gy and OIS chain */
if (xl_ulp_en == PROPERTY_ENABLE) {
val->ois.xl.odr = LSM6DSO_XL_OIS_OFF;
val->ois.gy.odr = LSM6DSO_GY_OIS_OFF;
val->ui.gy.odr = LSM6DSO_GY_UI_OFF;
}
/* if OIS xl is enabled also gyro OIS is enabled */
if (val->ois.xl.odr == LSM6DSO_XL_OIS_6667Hz_HP) {
val->ois.gy.odr = LSM6DSO_GY_OIS_6667Hz_HP;
}
g_hm_mode = ( (uint8_t)val->ui.gy.odr & 0x10U ) >> 4;
odr_gy = (uint8_t)val->ui.gy.odr & 0x0FU;
/* reading registers to be configured */
if ( ctx != NULL ) {
ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_XL, reg, 8);
bytecpy(( uint8_t *)&ctrl1_xl, ®[0]);
bytecpy(( uint8_t *)&ctrl2_g, ®[1]);
bytecpy(( uint8_t *)&ctrl3_c, ®[2]);
bytecpy(( uint8_t *)&ctrl4_c, ®[3]);
bytecpy(( uint8_t *)&ctrl5_c, ®[4]);
bytecpy(( uint8_t *)&ctrl6_c, ®[5]);
bytecpy(( uint8_t *)&ctrl7_g, ®[6]);
bytecpy(( uint8_t *)&ctrl8_xl, ®[7]);
if ( ret == 0 ) {
ret = lsm6dso_read_reg(ctx, LSM6DSO_FUNC_CFG_ACCESS,
(uint8_t *)&func_cfg_access, 1);
}
/* if toggle xl ultra low power mode, turn off xl before reconfigure */
if (ctrl5_c.xl_ulp_en != xl_ulp_en) {
ctrl1_xl.odr_xl = (uint8_t) 0x00U;
ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_XL,
(uint8_t *)&ctrl1_xl, 1);
}
}
/* reading OIS registers to be configured */
if ( aux_ctx != NULL ) {
if (ret == 0) {
ret = lsm6dso_read_reg(aux_ctx, LSM6DSO_CTRL1_OIS, reg, 3);
}
bytecpy(( uint8_t *)&ctrl1_ois, ®[0]);
bytecpy(( uint8_t *)&ctrl2_ois, ®[1]);
bytecpy(( uint8_t *)&ctrl3_ois, ®[2]);
}
else {
if ( ctx != NULL ) {
if (ret == 0) {
ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, reg, 3);
}
bytecpy(( uint8_t *)&ctrl1_ois, ®[0]);
bytecpy(( uint8_t *)&ctrl2_ois, ®[1]);
bytecpy(( uint8_t *)&ctrl3_ois, ®[2]);
}
}
/* Check the Finite State Machine data rate constraints */
if (val->fsm.sens != LSM6DSO_FSM_DISABLE) {
switch (val->fsm.odr) {
case LSM6DSO_FSM_12Hz5:
if ( (val->fsm.sens != LSM6DSO_FSM_GY) && (odr_xl == 0x00U) ) {
odr_xl = 0x01U;
}
if ( (val->fsm.sens != LSM6DSO_FSM_XL) && (odr_gy == 0x00U) ) {
xl_ulp_en = PROPERTY_DISABLE;
odr_gy = 0x01U;
}
break;
case LSM6DSO_FSM_26Hz:
if ( (val->fsm.sens != LSM6DSO_FSM_GY) && (odr_xl < 0x02U) ) {
odr_xl = 0x02U;
}
if ( (val->fsm.sens != LSM6DSO_FSM_XL) && (odr_gy < 0x02U) ) {
xl_ulp_en = PROPERTY_DISABLE;
odr_gy = 0x02U;
}
break;
case LSM6DSO_FSM_52Hz:
if ( (val->fsm.sens != LSM6DSO_FSM_GY) && (odr_xl < 0x03U) ) {
odr_xl = 0x03U;
}
if ( (val->fsm.sens != LSM6DSO_FSM_XL) && (odr_gy < 0x03U) ) {
xl_ulp_en = PROPERTY_DISABLE;
odr_gy = 0x03U;
}
break;
case LSM6DSO_FSM_104Hz:
if ( (val->fsm.sens != LSM6DSO_FSM_GY) && (odr_xl < 0x04U) ) {
odr_xl = 0x04U;
}
if ( (val->fsm.sens != LSM6DSO_FSM_XL) && (odr_gy < 0x04U) ) {
xl_ulp_en = PROPERTY_DISABLE;
odr_gy = 0x04U;
}
break;
default:
odr_xl = 0x00U;
odr_gy = 0x00U;
break;
}
}
/* Updating the accelerometer data rate configuration */
switch ( ( ctrl5_c.xl_ulp_en << 5 ) | ( ctrl6_c.xl_hm_mode << 4 ) |
ctrl1_xl.odr_xl ) {
case LSM6DSO_XL_UI_OFF:
val->ui.xl.odr = LSM6DSO_XL_UI_OFF;
break;
case LSM6DSO_XL_UI_12Hz5_HP:
val->ui.xl.odr = LSM6DSO_XL_UI_12Hz5_HP;
break;
case LSM6DSO_XL_UI_26Hz_HP:
val->ui.xl.odr = LSM6DSO_XL_UI_26Hz_HP;
break;
case LSM6DSO_XL_UI_52Hz_HP:
val->ui.xl.odr = LSM6DSO_XL_UI_52Hz_HP;
break;
case LSM6DSO_XL_UI_104Hz_HP:
val->ui.xl.odr = LSM6DSO_XL_UI_104Hz_HP;
break;
case LSM6DSO_XL_UI_208Hz_HP:
val->ui.xl.odr = LSM6DSO_XL_UI_208Hz_HP;
break;
case LSM6DSO_XL_UI_416Hz_HP:
val->ui.xl.odr = LSM6DSO_XL_UI_416Hz_HP;
break;
case LSM6DSO_XL_UI_833Hz_HP:
val->ui.xl.odr = LSM6DSO_XL_UI_833Hz_HP;
break;
case LSM6DSO_XL_UI_1667Hz_HP:
val->ui.xl.odr = LSM6DSO_XL_UI_1667Hz_HP;
break;
case LSM6DSO_XL_UI_3333Hz_HP:
val->ui.xl.odr = LSM6DSO_XL_UI_3333Hz_HP;
break;
case LSM6DSO_XL_UI_6667Hz_HP:
val->ui.xl.odr = LSM6DSO_XL_UI_6667Hz_HP;
break;
case LSM6DSO_XL_UI_1Hz6_LP:
val->ui.xl.odr = LSM6DSO_XL_UI_1Hz6_LP;
break;
case LSM6DSO_XL_UI_12Hz5_LP:
val->ui.xl.odr = LSM6DSO_XL_UI_12Hz5_LP;
break;
case LSM6DSO_XL_UI_26Hz_LP:
val->ui.xl.odr = LSM6DSO_XL_UI_26Hz_LP;
break;
case LSM6DSO_XL_UI_52Hz_LP:
val->ui.xl.odr = LSM6DSO_XL_UI_52Hz_LP;
break;
case LSM6DSO_XL_UI_104Hz_NM:
val->ui.xl.odr = LSM6DSO_XL_UI_104Hz_NM;
break;
case LSM6DSO_XL_UI_208Hz_NM:
val->ui.xl.odr = LSM6DSO_XL_UI_208Hz_NM;
break;
case LSM6DSO_XL_UI_1Hz6_ULP:
val->ui.xl.odr = LSM6DSO_XL_UI_1Hz6_ULP;
break;
case LSM6DSO_XL_UI_12Hz5_ULP:
val->ui.xl.odr = LSM6DSO_XL_UI_12Hz5_ULP;
break;
case LSM6DSO_XL_UI_26Hz_ULP:
val->ui.xl.odr = LSM6DSO_XL_UI_26Hz_ULP;
break;
case LSM6DSO_XL_UI_52Hz_ULP:
val->ui.xl.odr = LSM6DSO_XL_UI_52Hz_ULP;
break;
case LSM6DSO_XL_UI_104Hz_ULP:
val->ui.xl.odr = LSM6DSO_XL_UI_104Hz_ULP;
break;
case LSM6DSO_XL_UI_208Hz_ULP:
val->ui.xl.odr = LSM6DSO_XL_UI_208Hz_ULP;
break;
default:
val->ui.xl.odr = LSM6DSO_XL_UI_OFF;
break;
}
/* Updating the accelerometer data rate configuration */
switch ( (ctrl7_g.g_hm_mode << 4) | ctrl2_g.odr_g) {
case LSM6DSO_GY_UI_OFF:
val->ui.gy.odr = LSM6DSO_GY_UI_OFF;
break;
case LSM6DSO_GY_UI_12Hz5_LP:
val->ui.gy.odr = LSM6DSO_GY_UI_12Hz5_LP;
break;
case LSM6DSO_GY_UI_12Hz5_HP:
val->ui.gy.odr = LSM6DSO_GY_UI_12Hz5_HP;
break;
case LSM6DSO_GY_UI_26Hz_LP:
val->ui.gy.odr = LSM6DSO_GY_UI_26Hz_LP;
break;
case LSM6DSO_GY_UI_26Hz_HP:
val->ui.gy.odr = LSM6DSO_GY_UI_26Hz_HP;
break;
case LSM6DSO_GY_UI_52Hz_LP:
val->ui.gy.odr = LSM6DSO_GY_UI_52Hz_LP;
break;
case LSM6DSO_GY_UI_52Hz_HP:
val->ui.gy.odr = LSM6DSO_GY_UI_52Hz_HP;
break;
case LSM6DSO_GY_UI_104Hz_NM:
val->ui.gy.odr = LSM6DSO_GY_UI_104Hz_NM;
break;
case LSM6DSO_GY_UI_104Hz_HP:
val->ui.gy.odr = LSM6DSO_GY_UI_104Hz_HP;
break;
case LSM6DSO_GY_UI_208Hz_NM:
val->ui.gy.odr = LSM6DSO_GY_UI_208Hz_NM;
break;
case LSM6DSO_GY_UI_208Hz_HP:
val->ui.gy.odr = LSM6DSO_GY_UI_208Hz_HP;
break;
case LSM6DSO_GY_UI_416Hz_HP:
val->ui.gy.odr = LSM6DSO_GY_UI_416Hz_HP;
break;
case LSM6DSO_GY_UI_833Hz_HP:
val->ui.gy.odr = LSM6DSO_GY_UI_833Hz_HP;
break;
case LSM6DSO_GY_UI_1667Hz_HP:
val->ui.gy.odr = LSM6DSO_GY_UI_1667Hz_HP;
break;
case LSM6DSO_GY_UI_3333Hz_HP:
val->ui.gy.odr = LSM6DSO_GY_UI_3333Hz_HP;
break;
case LSM6DSO_GY_UI_6667Hz_HP:
val->ui.gy.odr = LSM6DSO_GY_UI_6667Hz_HP;
break;
default:
val->ui.gy.odr = LSM6DSO_GY_UI_OFF;
break;
}
/* Check accelerometer full scale constraints */
/* Full scale of 16g must be the same for UI and OIS */
if ( (val->ui.xl.fs == LSM6DSO_XL_UI_16g) ||
(val->ois.xl.fs == LSM6DSO_XL_OIS_16g) ) {
val->ui.xl.fs = LSM6DSO_XL_UI_16g;
val->ois.xl.fs = LSM6DSO_XL_OIS_16g;
}
/* prapare new configuration */
/* Full scale of 16g must be the same for UI and OIS */
if (val->ui.xl.fs == LSM6DSO_XL_UI_16g) {
ctrl8_xl.xl_fs_mode = PROPERTY_DISABLE;
}
else {
ctrl8_xl.xl_fs_mode = PROPERTY_ENABLE;
}
/* OIS new configuration */
ctrl7_g.ois_on_en = val->ois.ctrl_md & 0x01U;
switch (val->ois.ctrl_md) {
case LSM6DSO_OIS_ONLY_AUX:
ctrl1_ois.fs_g_ois = (uint8_t)val->ois.gy.fs;
ctrl1_ois.ois_en_spi2 = (uint8_t)val->ois.gy.odr |
(uint8_t)val->ois.xl.odr;
ctrl1_ois.mode4_en = (uint8_t) val->ois.xl.odr;
ctrl3_ois.fs_xl_ois = (uint8_t)val->ois.xl.fs;
break;
case LSM6DSO_OIS_MIXED:
ctrl1_ois.fs_g_ois = (uint8_t)val->ois.gy.fs;
ctrl7_g.ois_on = (uint8_t)val->ois.gy.odr | (uint8_t)val->ois.xl.odr;
ctrl1_ois.mode4_en = (uint8_t) val->ois.xl.odr;
ctrl3_ois.fs_xl_ois = (uint8_t)val->ois.xl.fs;
break;
default:
ctrl1_ois.fs_g_ois = (uint8_t)val->ois.gy.fs;
ctrl1_ois.ois_en_spi2 = (uint8_t)val->ois.gy.odr |
(uint8_t)val->ois.xl.odr;
ctrl1_ois.mode4_en = (uint8_t) val->ois.xl.odr;
ctrl3_ois.fs_xl_ois = (uint8_t)val->ois.xl.fs;
break;
}
/* UI new configuration */
ctrl1_xl.odr_xl = odr_xl;
ctrl1_xl.fs_xl = (uint8_t)val->ui.xl.fs;
ctrl5_c.xl_ulp_en = xl_ulp_en;
ctrl6_c.xl_hm_mode = xl_hm_mode;
ctrl7_g.g_hm_mode = g_hm_mode;
ctrl2_g.odr_g = odr_gy;
ctrl2_g.fs_g = (uint8_t) val->ui.gy.fs;
/* writing checked configuration */
if ( ctx != NULL ) {
bytecpy(®[0], ( uint8_t *)&ctrl1_xl);
bytecpy(®[1], ( uint8_t *)&ctrl2_g);
bytecpy(®[2], ( uint8_t *)&ctrl3_c);
bytecpy(®[3], ( uint8_t *)&ctrl4_c);
bytecpy(®[4], ( uint8_t *)&ctrl5_c);
bytecpy(®[5], ( uint8_t *)&ctrl6_c);
bytecpy(®[6], ( uint8_t *)&ctrl7_g);
bytecpy(®[7], ( uint8_t *)&ctrl8_xl);
if ( ret == 0 ) {
ret = lsm6dso_write_reg(ctx, LSM6DSO_CTRL1_XL, (uint8_t *)®, 8);
}
if ( ret == 0 ) {
ret = lsm6dso_write_reg(ctx, LSM6DSO_FUNC_CFG_ACCESS,
(uint8_t *)&func_cfg_access, 1);
}
}
/* writing OIS checked configuration */
if ( aux_ctx != NULL ) {
bytecpy(®[0], ( uint8_t *)&ctrl1_ois);
bytecpy(®[1], ( uint8_t *)&ctrl2_ois);
bytecpy(®[2], ( uint8_t *)&ctrl3_ois);
if (ret == 0) {
ret = lsm6dso_write_reg(aux_ctx, LSM6DSO_CTRL1_OIS, reg, 3);
}
}
return ret;
}