int32_t lsm6dso_mode

int32_t lsm6dso_mode_get()

in Drivers/BSP/Components/lsm6dso/lsm6dso_reg.c [11049:11568]
403 lines of code
93 McCabe index (conditional complexity)

int32_t lsm6dso_mode_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx,
                         lsm6dso_md_t *val)
{
  lsm6dso_emb_func_odr_cfg_b_t emb_func_odr_cfg_b;
  lsm6dso_func_cfg_access_t func_cfg_access;
  lsm6dso_emb_func_en_b_t emb_func_en_b;
  lsm6dso_fsm_enable_a_t fsm_enable_a;
  lsm6dso_fsm_enable_b_t fsm_enable_b;
  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_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 reg[8];
  int32_t ret;
  ret = 0;

  /* reading the registers of the device */
  if ( ctx != NULL ) {
    ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_XL, reg, 7);
    bytecpy(( uint8_t *)&ctrl1_xl, &reg[0]);
    bytecpy(( uint8_t *)&ctrl2_g,  &reg[1]);
    bytecpy(( uint8_t *)&ctrl3_c,  &reg[2]);
    bytecpy(( uint8_t *)&ctrl4_c,  &reg[3]);
    bytecpy(( uint8_t *)&ctrl5_c,  &reg[4]);
    bytecpy(( uint8_t *)&ctrl6_c,  &reg[5]);
    bytecpy(( uint8_t *)&ctrl7_g,  &reg[6]);

    if ( ret == 0 ) {
      ret = lsm6dso_read_reg(ctx, LSM6DSO_FUNC_CFG_ACCESS,
                             (uint8_t *)&func_cfg_access, 1);
    }

    if (ret == 0) {
      ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_EMBEDDED_FUNC_BANK);
    }

    if (ret == 0) {
      ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_ODR_CFG_B, reg, 1);
      bytecpy(( uint8_t *)&emb_func_odr_cfg_b, &reg[0]);
    }

    if (ret == 0) {
      ret = lsm6dso_read_reg(ctx, LSM6DSO_EMB_FUNC_EN_B,
                             (uint8_t *)&emb_func_en_b, 1);
    }

    if (ret == 0) {
      ret = lsm6dso_read_reg(ctx, LSM6DSO_FSM_ENABLE_A, reg, 2);
      bytecpy(( uint8_t *)&fsm_enable_a, &reg[0]);
      bytecpy(( uint8_t *)&fsm_enable_b, &reg[1]);
    }

    if (ret == 0) {
      ret = lsm6dso_mem_bank_set(ctx, LSM6DSO_USER_BANK);
    }
  }

  if ( aux_ctx != NULL ) {
    if (ret == 0) {
      ret = lsm6dso_read_reg(aux_ctx, LSM6DSO_CTRL1_OIS, reg, 3);
    }

    bytecpy(( uint8_t *)&ctrl1_ois, &reg[0]);
    bytecpy(( uint8_t *)&ctrl2_ois, &reg[1]);
    bytecpy(( uint8_t *)&ctrl3_ois, &reg[2]);
  }

  else {
    if ( ctx != NULL ) {
      if (ret == 0) {
        ret = lsm6dso_read_reg(ctx, LSM6DSO_CTRL1_OIS, reg, 3);
      }

      bytecpy(( uint8_t *)&ctrl1_ois, &reg[0]);
      bytecpy(( uint8_t *)&ctrl2_ois, &reg[1]);
      bytecpy(( uint8_t *)&ctrl3_ois, &reg[2]);
    }
  }

  /* fill the input structure */

  /* get accelerometer 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;
  }

  switch ( ctrl1_xl.fs_xl ) {
    case LSM6DSO_XL_UI_2g:
      val->ui.xl.fs = LSM6DSO_XL_UI_2g;
      break;

    case LSM6DSO_XL_UI_4g:
      val->ui.xl.fs = LSM6DSO_XL_UI_4g;
      break;

    case LSM6DSO_XL_UI_8g:
      val->ui.xl.fs = LSM6DSO_XL_UI_8g;
      break;

    case LSM6DSO_XL_UI_16g:
      val->ui.xl.fs = LSM6DSO_XL_UI_16g;
      break;

    default:
      val->ui.xl.fs = LSM6DSO_XL_UI_2g;
      break;
  }

  /* get gyroscope 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;
  }

  switch (ctrl2_g.fs_g) {
    case LSM6DSO_GY_UI_125dps:
      val->ui.gy.fs = LSM6DSO_GY_UI_125dps;
      break;

    case LSM6DSO_GY_UI_250dps:
      val->ui.gy.fs = LSM6DSO_GY_UI_250dps;
      break;

    case LSM6DSO_GY_UI_500dps:
      val->ui.gy.fs = LSM6DSO_GY_UI_500dps;
      break;

    case LSM6DSO_GY_UI_1000dps:
      val->ui.gy.fs = LSM6DSO_GY_UI_1000dps;
      break;

    case LSM6DSO_GY_UI_2000dps:
      val->ui.gy.fs = LSM6DSO_GY_UI_2000dps;
      break;

    default:
      val->ui.gy.fs = LSM6DSO_GY_UI_125dps;
      break;
  }

  /* get finite state machine configuration */
  if ( (fsm_enable_a.fsm1_en | fsm_enable_a.fsm2_en |
        fsm_enable_a.fsm3_en |
        fsm_enable_a.fsm4_en | fsm_enable_a.fsm5_en | fsm_enable_a.fsm6_en |
        fsm_enable_a.fsm7_en | fsm_enable_a.fsm8_en | fsm_enable_b.fsm9_en |
        fsm_enable_b.fsm10_en | fsm_enable_b.fsm11_en |
        fsm_enable_b.fsm12_en | fsm_enable_b.fsm13_en |
        fsm_enable_b.fsm14_en | fsm_enable_b.fsm15_en |
        fsm_enable_b.fsm16_en) == PROPERTY_ENABLE ) {
    switch (emb_func_odr_cfg_b.fsm_odr) {
      case LSM6DSO_FSM_12Hz5:
        val->fsm.odr = LSM6DSO_FSM_12Hz5;
        break;

      case LSM6DSO_FSM_26Hz:
        val->fsm.odr = LSM6DSO_FSM_26Hz;
        break;

      case LSM6DSO_FSM_52Hz:
        val->fsm.odr = LSM6DSO_FSM_52Hz;
        break;

      case LSM6DSO_FSM_104Hz:
        val->fsm.odr = LSM6DSO_FSM_104Hz;
        break;

      default:
        val->fsm.odr = LSM6DSO_FSM_12Hz5;
        break;
    }

    val->fsm.sens = LSM6DSO_FSM_XL_GY;

    if (val->ui.gy.odr == LSM6DSO_GY_UI_OFF) {
      val->fsm.sens = LSM6DSO_FSM_XL;
    }

    if (val->ui.xl.odr == LSM6DSO_XL_UI_OFF) {
      val->fsm.sens = LSM6DSO_FSM_GY;
    }
  }

  else {
    val->fsm.sens = LSM6DSO_FSM_DISABLE;
  }

  /* get ois configuration */

  /* OIS configuration mode */
  switch ( ctrl7_g.ois_on_en ) {
    case LSM6DSO_OIS_ONLY_AUX:
      switch ( ctrl3_ois.fs_xl_ois ) {
        case LSM6DSO_XL_OIS_2g:
          val->ois.xl.fs = LSM6DSO_XL_OIS_2g;
          break;

        case LSM6DSO_XL_OIS_4g:
          val->ois.xl.fs = LSM6DSO_XL_OIS_4g;
          break;

        case LSM6DSO_XL_OIS_8g:
          val->ois.xl.fs = LSM6DSO_XL_OIS_8g;
          break;

        case LSM6DSO_XL_OIS_16g:
          val->ois.xl.fs = LSM6DSO_XL_OIS_16g;
          break;

        default:
          val->ois.xl.fs = LSM6DSO_XL_OIS_2g;
          break;
      }

      switch ( ctrl1_ois.mode4_en ) {
        case LSM6DSO_XL_OIS_OFF:
          val->ois.xl.odr = LSM6DSO_XL_OIS_OFF;
          break;

        case LSM6DSO_XL_OIS_6667Hz_HP:
          val->ois.xl.odr = LSM6DSO_XL_OIS_6667Hz_HP;
          break;

        default:
          val->ois.xl.odr = LSM6DSO_XL_OIS_OFF;
          break;
      }

      switch ( ctrl1_ois.fs_g_ois ) {
        case LSM6DSO_GY_OIS_250dps:
          val->ois.gy.fs = LSM6DSO_GY_OIS_250dps;
          break;

        case LSM6DSO_GY_OIS_500dps:
          val->ois.gy.fs = LSM6DSO_GY_OIS_500dps;
          break;

        case LSM6DSO_GY_OIS_1000dps:
          val->ois.gy.fs = LSM6DSO_GY_OIS_1000dps;
          break;

        case LSM6DSO_GY_OIS_2000dps:
          val->ois.gy.fs = LSM6DSO_GY_OIS_2000dps;
          break;

        default:
          val->ois.gy.fs = LSM6DSO_GY_OIS_250dps;
          break;
      }

      switch ( ctrl1_ois.ois_en_spi2 ) {
        case LSM6DSO_GY_OIS_OFF:
          val->ois.gy.odr = LSM6DSO_GY_OIS_OFF;
          break;

        case LSM6DSO_GY_OIS_6667Hz_HP:
          val->ois.gy.odr = LSM6DSO_GY_OIS_6667Hz_HP;
          break;

        default:
          val->ois.gy.odr = LSM6DSO_GY_OIS_OFF;
          break;
      }

      val->ois.ctrl_md = LSM6DSO_OIS_ONLY_AUX;
      break;

    case LSM6DSO_OIS_MIXED:
      switch ( ctrl3_ois.fs_xl_ois ) {
        case LSM6DSO_XL_OIS_2g:
          val->ois.xl.fs = LSM6DSO_XL_OIS_2g;
          break;

        case LSM6DSO_XL_OIS_4g:
          val->ois.xl.fs = LSM6DSO_XL_OIS_4g;
          break;

        case LSM6DSO_XL_OIS_8g:
          val->ois.xl.fs = LSM6DSO_XL_OIS_8g;
          break;

        case LSM6DSO_XL_OIS_16g:
          val->ois.xl.fs = LSM6DSO_XL_OIS_16g;
          break;

        default:
          val->ois.xl.fs = LSM6DSO_XL_OIS_2g;
          break;
      }

      switch ( ctrl1_ois.mode4_en ) {
        case LSM6DSO_XL_OIS_OFF:
          val->ois.xl.odr = LSM6DSO_XL_OIS_OFF;
          break;

        case LSM6DSO_XL_OIS_6667Hz_HP:
          val->ois.xl.odr = LSM6DSO_XL_OIS_6667Hz_HP;
          break;

        default:
          val->ois.xl.odr = LSM6DSO_XL_OIS_OFF;
          break;
      }

      switch ( ctrl1_ois.fs_g_ois ) {
        case LSM6DSO_GY_OIS_250dps:
          val->ois.gy.fs = LSM6DSO_GY_OIS_250dps;
          break;

        case LSM6DSO_GY_OIS_500dps:
          val->ois.gy.fs = LSM6DSO_GY_OIS_500dps;
          break;

        case LSM6DSO_GY_OIS_1000dps:
          val->ois.gy.fs = LSM6DSO_GY_OIS_1000dps;
          break;

        case LSM6DSO_GY_OIS_2000dps:
          val->ois.gy.fs = LSM6DSO_GY_OIS_2000dps;
          break;

        default:
          val->ois.gy.fs = LSM6DSO_GY_OIS_250dps;
          break;
      }

      switch ( ctrl1_ois.ois_en_spi2 ) {
        case LSM6DSO_GY_OIS_OFF:
          val->ois.gy.odr = LSM6DSO_GY_OIS_OFF;
          break;

        case LSM6DSO_GY_OIS_6667Hz_HP:
          val->ois.gy.odr = LSM6DSO_GY_OIS_6667Hz_HP;
          break;

        default:
          val->ois.gy.odr = LSM6DSO_GY_OIS_OFF;
          break;
      }

      val->ois.ctrl_md = LSM6DSO_OIS_MIXED;
      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;
      val->ois.ctrl_md = LSM6DSO_OIS_ONLY_AUX;
      break;
  }

  return ret;
}