/** * @brief Disable free fall detection * @param None * @retval none */ void LSM6DS3_Disable_Free_Fall_Detection( void ) { uint8_t tmp1 = 0x00; LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1); /* INT1_FF setting */ tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK); tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_DISABLE; LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1); LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1); /* FF_DUR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK); tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_DEFAULT; /* FF_THS setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK); tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_156MG; LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1); }
imu_status_t imu_sensor_read_fifo_status(void) { uint8_t fifo_1_number = 0,fifo_2_number = 0; uint32_t p_data_rate = 0; uint16_t fifo_data_number = 0; if(LSM6DS3_IO_Read(&fifo_1_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS1, 1) != imu_status_ok) { return imu_status_fail; } if(LSM6DS3_IO_Read(&fifo_2_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS2, 1) != imu_status_ok) { return imu_status_fail; } printf("fifo status: %x\n",fifo_2_number); fifo_data_number |= (fifo_2_number & 0x0F); fifo_data_number = (fifo_data_number << 8) | fifo_1_number; printf("number of unread words: %d\n",fifo_data_number); return imu_status_ok; }
/** * @brief Read raw data from LSM6DS3 Gyroscope output register * @param pData the pointer where the gyroscope raw data are stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_G_GetAxesRaw( int16_t *pData ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg[2] = {0, 0}; if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_X_L_G, 2) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]); if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_Y_L_G, 2) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]); if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_Z_L_G, 2) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]); return IMU_6AXES_OK; }
static imu_status_t imu_sensor_clear_fifo(void) { uint8_t tempReg[2] = {0, 0}; uint8_t fifo_1_number = 0,fifo_2_number = 0; uint32_t p_data_rate = 10; uint16_t fifo_data_number = 0; if(LSM6DS3_IO_Read(&fifo_1_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS1, 1) != imu_status_ok) { return imu_status_fail; } if(LSM6DS3_IO_Read(&fifo_2_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS2, 1) != imu_status_ok) { return imu_status_fail; } fifo_data_number |= (fifo_2_number & 0x0F); fifo_data_number = (fifo_data_number << 8) | fifo_1_number; imu_sensor_set_data_rate(&p_data_rate, LSM6DS3_XG_FIFO_MODE_BYPASS); printf("set data rate to 0x00\n"); uint16_t i; for(i = 0; i < fifo_data_number; i++){ if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_DATA_OUT_L, 2) != imu_status_ok) { printf("read sensor error\n"); return imu_status_fail; } } printf("clear fifo over \n"); return imu_status_ok; }
/** * @brief Write Gyro Full Scale * @param fullScale the gyroscope full scale to be set * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_G_Set_FS( float fullScale ) { uint8_t new_fs = 0x00; uint8_t tempReg = 0x00; if(fullScale <= 125.0f) { new_fs = LSM6DS3_G_FS_125_ENABLE; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_G_FS_125_MASK); tempReg |= new_fs; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } } else { /* Disable G FS 125dpp */ if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_G_FS_125_MASK); tempReg |= LSM6DS3_G_FS_125_DISABLE; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } new_fs = ( fullScale <= 245.0f ) ? LSM6DS3_G_FS_245 : ( fullScale <= 500.0f ) ? LSM6DS3_G_FS_500 : ( fullScale <= 1000.0f ) ? LSM6DS3_G_FS_1000 : LSM6DS3_G_FS_2000; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_G_FS_MASK); tempReg |= new_fs; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } } return IMU_6AXES_OK; }
/** * @brief Enable free fall detection * @param None * @retval none */ void LSM6DS3_Enable_Free_Fall_Detection( void ) { uint8_t tmp1 = 0x00; LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1); /* Output Data Rate selection */ tmp1 &= ~(LSM6DS3_XL_ODR_MASK); tmp1 |= LSM6DS3_XL_ODR_416HZ; /* Full scale selection */ tmp1 &= ~(LSM6DS3_XL_FS_MASK); tmp1 |= LSM6DS3_XL_FS_2G; LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1); LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_UP_DUR, 1); /* FF_DUR5 setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_MASK); tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_DEFAULT; /* WAKE_DUR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_MASK); tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_DEFAULT; /* TIMER_HR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_MASK); tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_DEFAULT; /* SLEEP_DUR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_MASK); tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_DEFAULT; LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_UP_DUR, 1); LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1); /* FF_DUR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK); tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_TYPICAL; /* FF_THS setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK); tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_312MG; LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1); LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1); /* INT1_FF setting */ tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK); tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_ENABLE; LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1); }
/*fifo threshold level setting*/ static imu_status_t imu_sensor_fifo_threshold_level(uint16_t fifo_level) { uint8_t tmp1; { if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL1, 1) != imu_status_ok) { return imu_status_fail; } /*set water mark low 8bit*/ tmp1 &= ~(0xFF); tmp1 |= (fifo_level & 0xFF); if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL1, 1) != imu_status_ok) { return imu_status_fail; } } { if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL2, 1) != imu_status_ok) { return imu_status_fail; } /*set water mark high 4bit*/ tmp1 &= ~(LSM6DS3_XG_FIFO_THRESHOLD_MASK); tmp1 |= (fifo_level >> 8) & 0x0F; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL2, 1) != imu_status_ok) { return imu_status_fail; } } { if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL3, 1) != imu_status_ok) { return imu_status_fail; } /*set */ tmp1 &= ~(0x3F); tmp1 |= 0x09; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL3, 1) != imu_status_ok) { return imu_status_fail; } } return imu_status_ok; }
/** * @brief Write Accelero Full Scale * @param fullScale the accelerometer full scale to be set * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_X_Set_FS( float fullScale ) { uint8_t new_fs = 0x00; uint8_t tempReg = 0x00; new_fs = ( fullScale <= 2.0f ) ? LSM6DS3_XL_FS_2G : ( fullScale <= 4.0f ) ? LSM6DS3_XL_FS_4G : ( fullScale <= 8.0f ) ? LSM6DS3_XL_FS_8G : LSM6DS3_XL_FS_16G; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_XL_FS_MASK); tempReg |= new_fs; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; }
/** * @brief Read Accelero Full Scale * @param fullScale the pointer where the accelerometer full scale is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_X_Get_FS( float *fullScale ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_XL_FS_MASK; switch( tempReg ) { case LSM6DS3_XL_FS_2G: *fullScale = 2.0f; break; case LSM6DS3_XL_FS_4G: *fullScale = 4.0f; break; case LSM6DS3_XL_FS_8G: *fullScale = 8.0f; break; case LSM6DS3_XL_FS_16G: *fullScale = 16.0f; break; default: break; } return IMU_6AXES_OK; }
/** * @brief Read Accelero Sensitivity * @param pfData the pointer where the accelerometer sensitivity is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_X_GetSensitivity( float *pfData ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_XL_FS_MASK; switch( tempReg ) { case LSM6DS3_XL_FS_2G: *pfData = 0.061f; break; case LSM6DS3_XL_FS_4G: *pfData = 0.122f; break; case LSM6DS3_XL_FS_8G: *pfData = 0.244f; break; case LSM6DS3_XL_FS_16G: *pfData = 0.488f; break; default: break; } return IMU_6AXES_OK; }
/** * @brief Set the status of the axes for gyroscope * @param enableX the status of the x axis to be set * @param enableY the status of the y axis to be set * @param enableZ the status of the z axis to be set * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_G_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ) { uint8_t tmp1 = 0x00; uint8_t eX = 0x00; uint8_t eY = 0x00; uint8_t eZ = 0x00; eX = ( enableX == 0 ) ? LSM6DS3_G_XEN_DISABLE : LSM6DS3_G_XEN_ENABLE; eY = ( enableY == 0 ) ? LSM6DS3_G_YEN_DISABLE : LSM6DS3_G_YEN_ENABLE; eZ = ( enableZ == 0 ) ? LSM6DS3_G_ZEN_DISABLE : LSM6DS3_G_ZEN_ENABLE; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL10_C, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* Enable X axis selection */ tmp1 &= ~(LSM6DS3_G_XEN_MASK); tmp1 |= eX; /* Enable Y axis selection */ tmp1 &= ~(LSM6DS3_G_YEN_MASK); tmp1 |= eY; /* Enable Z axis selection */ tmp1 &= ~(LSM6DS3_G_ZEN_MASK); tmp1 |= eZ; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL10_C, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; }
/** * @brief Write Gyro Output Data Rate * @param odr the gyroscope output data rate to be set * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_G_Set_ODR( float odr ) { uint8_t new_odr = 0x00; uint8_t tempReg = 0x00; new_odr = ( odr <= 0.0f ) ? LSM6DS3_G_ODR_PD /* Power Down */ : ( odr <= 13.0f ) ? LSM6DS3_G_ODR_13HZ : ( odr <= 26.0f ) ? LSM6DS3_G_ODR_26HZ : ( odr <= 52.0f ) ? LSM6DS3_G_ODR_52HZ : ( odr <= 104.0f ) ? LSM6DS3_G_ODR_104HZ : ( odr <= 208.0f ) ? LSM6DS3_G_ODR_208HZ : ( odr <= 416.0f ) ? LSM6DS3_G_ODR_416HZ : ( odr <= 833.0f ) ? LSM6DS3_G_ODR_833HZ : LSM6DS3_G_ODR_1660HZ; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_G_ODR_MASK); tempReg |= new_odr; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; }
/*fifo config*/ static imu_status_t lsm6ds3_fifo_sensor_enable(void) { uint8_t tmp1 = 0x00; /*lsm6ds3*/ { if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != imu_status_ok) { return imu_status_fail; } /* Enable register address automatically incremented during a multiple byte access with a serial interface (I2C or SPI) */ tmp1 &= ~(LSM6DS3_XG_IF_INC_MASK); tmp1 |= LSM6DS3_XG_IF_INC; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != imu_status_ok) { return imu_status_fail; } } return imu_status_ok; }
/*active gyro*/ static imu_status_t imu_sensor_config_gyro(uint8_t data_rate, uint8_t scale) { uint8_t tempReg = 0x00; /*gyro rate and scale*/ if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != imu_status_ok) { return imu_status_fail; } tempReg &= ~(LSM6DS3_G_ODR_MASK); tempReg |= data_rate; /* Full scale selection */ tempReg &= ~(LSM6DS3_G_FS_125_MASK); tempReg |= scale; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1) != imu_status_ok) { return imu_status_fail; } /*get sensitivity*/ if(imu_sensor_gyro_get_sensitivity( &sensor_data_sensitivity.gyro_sensitivity ) != imu_status_ok) { return imu_status_fail; } printf("gyro_sensitivity : %f\n", sensor_data_sensitivity.gyro_sensitivity); return imu_status_ok; }
/*fifo read*/ void imu_sensor_read_data_from_fifo(void* arg) { uint16_t fifo_sample_number = 0; /*fifo sample number*/ if(imu_sensor_fifo_data_number(&fifo_sample_number) != imu_status_ok) { printf("imu_sensor_fifo_data_number error \n"); return ; } if(fifo_sample_number < 6) { if(imu_sensor_bypass_clear_fifo(NULL) != imu_status_ok) { printf("imu_sensor_bypass_clear_fifo error\n"); return; } } /*fifo pattern*/ number_tmp = imu_sensor_read_fifo_pattern(); if(number_tmp != 0){ number_tmp = 12 - (number_tmp*2); } /*read from dma*/ if(LSM6DS3_IO_Read(tempReg_sensor, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_DATA_OUT_L, DMA_READ_6AXIS_DEPTH) != imu_status_ok) { printf("LSM6DS3_IO_Read_DMA error \n"); return; } }
/** * @brief Read Accelero Sensitivity * @param pfData the pointer where the accelerometer sensitivity is stored * @retval none */ void LSM6DS3_X_GetSensitivity( float *pfData ) { /*Here we could check if the parameters are valid*/ uint8_t tempReg = 0x00; LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1 ); tempReg &= LSM6DS3_XL_FS_MASK; switch( tempReg ) { case LSM6DS3_XL_FS_2G: *pfData = 0.061; break; case LSM6DS3_XL_FS_4G: *pfData = 0.122; break; case LSM6DS3_XL_FS_8G: *pfData = 0.244; break; case LSM6DS3_XL_FS_16G: *pfData = 0.488; break; default: break; } }
/** * @brief Read Gyro Sensitivity * @param pfData the pointer where the gyroscope sensitivity is stored * @retval none */ void LSM6DS3_G_GetSensitivity( float *pfData ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ); tempReg &= LSM6DS3_G_FS_MASK; switch( tempReg ) { case LSM6DS3_G_FS_125: *pfData = 4.375; break; case LSM6DS3_G_FS_245: *pfData = 8.75; break; case LSM6DS3_G_FS_500: *pfData = 17.50; break; case LSM6DS3_G_FS_1000: *pfData = 35; break; case LSM6DS3_G_FS_2000: *pfData = 70; break; default: break; } }
void LSM6DS3_G_GetAxesRaw( int16_t *pData ) { /*Here we could check if the parameters are valid*/ uint8_t tempReg[2] = {0,0}; LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_X_L_G, 2); pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_Y_L_G, 2); pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_OUT_Z_L_G, 2); pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); }
void LSM6DS3_X_GetAxes( int32_t *pData ) { /*Here we could check if the parameters are valid*/ uint8_t tempReg = 0x00; int16_t pDataRaw[3]; float sensitivity = 0; LSM6DS3_X_GetAxesRaw(pDataRaw); LSM6DS3_IO_Read(&tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1); tempReg &= LSM6DS3_XL_FS_MASK; switch(tempReg) { case LSM6DS3_XL_FS_2G: sensitivity = 0.061; break; case LSM6DS3_XL_FS_4G: sensitivity = 0.122; break; case LSM6DS3_XL_FS_8G: sensitivity = 0.244; break; case LSM6DS3_XL_FS_16G: sensitivity = 0.488; break; } pData[0] = (int32_t)(pDataRaw[0] * sensitivity); pData[1] = (int32_t)(pDataRaw[1] * sensitivity); pData[2] = (int32_t)(pDataRaw[2] * sensitivity); }
static imu_status_t imu_sensor_acc_get_sensitivity( float *pfData ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1 ) != imu_status_ok) { return imu_status_fail; } tempReg &= LSM6DS3_XL_FS_MASK; switch( tempReg ) { case LSM6DS3_XL_FS_2G: *pfData = 0.061f; break; case LSM6DS3_XL_FS_4G: *pfData = 0.122f; break; case LSM6DS3_XL_FS_8G: *pfData = 0.244f; break; case LSM6DS3_XL_FS_16G: *pfData = 0.488f; break; default: break; } return imu_status_ok; }
static imu_status_t imu_sensor_bypass_clear_fifo(void* arg) { uint8_t tmp1; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != imu_status_ok) { return imu_status_fail; } /* FIFO mode selection */ tmp1 &= ~(LSM6DS3_XG_FIFO_MODE_MASK); /*bypass mode*/ tmp1 |= LSM6DS3_XG_FIFO_MODE_BYPASS; /*BYPASS MODE*/ if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != imu_status_ok) { return imu_status_fail; } /* FIFO mode selection */ tmp1 &= ~(LSM6DS3_XG_FIFO_MODE_MASK); /*bypass mode*/ tmp1 |= LSM6DS3_XG_FIFO_MODE_CONTINUOUS_OVERWRITE; /*CONTINUES MODE*/ if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != imu_status_ok) { return imu_status_fail; } return imu_status_ok; }
/** * @brief Read Gyro Full Scale * @param fullScale the pointer where the gyroscope full scale is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_G_Get_FS( float *fullScale ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_G_FS_125_MASK; if(tempReg == LSM6DS3_G_FS_125_ENABLE) { *fullScale = 125.0f; } else { if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_G_FS_MASK; switch( tempReg ) { case LSM6DS3_G_FS_245: *fullScale = 245.0f; break; case LSM6DS3_G_FS_500: *fullScale = 500.0f; break; case LSM6DS3_G_FS_1000: *fullScale = 1000.0f; break; case LSM6DS3_G_FS_2000: *fullScale = 2000.0f; break; default: break; } } return IMU_6AXES_OK; }
static imu_status_t imu_sensor_gyro_get_sensitivity( float *pfData ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != imu_status_ok) { return imu_status_fail; } tempReg &= LSM6DS3_G_FS_125_MASK; if(tempReg == LSM6DS3_G_FS_125_ENABLE) { *pfData = 4.375f; } else { if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != imu_status_ok) { return imu_status_fail; } tempReg &= LSM6DS3_G_FS_MASK; switch( tempReg ) { case LSM6DS3_G_FS_245: *pfData = 8.75f; break; case LSM6DS3_G_FS_500: *pfData = 17.50f; break; case LSM6DS3_G_FS_1000: *pfData = 35.0f; break; case LSM6DS3_G_FS_2000: *pfData = 70.0f; break; default: break; } } return imu_status_ok; }
/** * @brief Read Gyro Sensitivity * @param pfData the pointer where the gyroscope sensitivity is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_G_GetSensitivity( float *pfData ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_G_FS_125_MASK; if(tempReg == LSM6DS3_G_FS_125_ENABLE) { *pfData = 4.375f; } else { if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_G_FS_MASK; switch( tempReg ) { case LSM6DS3_G_FS_245: *pfData = 8.75f; break; case LSM6DS3_G_FS_500: *pfData = 17.50f; break; case LSM6DS3_G_FS_1000: *pfData = 35.0f; break; case LSM6DS3_G_FS_2000: *pfData = 70.0f; break; default: break; } } return IMU_6AXES_OK; }
/** * @brief Read ID of LSM6DS3 Accelerometer and Gyroscope * @param xg_id the pointer where the ID of the device is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_Read_XG_ID( uint8_t *xg_id) { if(!xg_id) { return IMU_6AXES_ERROR; } return LSM6DS3_IO_Read(xg_id, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WHO_AM_I_ADDR, 1); }
static imu_status_t imu_sensor_fifo_data_number(uint16_t* number) { uint8_t fifo_1_number = 0,fifo_2_number = 0; if(LSM6DS3_IO_Read(&fifo_1_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS1, 1) != imu_status_ok) { return imu_status_fail; } if(LSM6DS3_IO_Read(&fifo_2_number, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS2, 1) != imu_status_ok) { return imu_status_fail; } *number |= (fifo_2_number & 0x0F); *number = (*number << 8) | fifo_1_number; return imu_status_ok; }
/*read fifo pattern*/ static uint8_t imu_sensor_read_fifo_pattern(void) { uint8_t tmp1 = 0x00; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_STATUS3, 1) != imu_status_ok) { return 0xff; } return tmp1; }
/** * @brief Read Accelero Output Data Rate * @param odr the pointer where the accelerometer output data rate is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_X_Get_ODR( float *odr ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_XL_ODR_MASK; switch( tempReg ) { case LSM6DS3_XL_ODR_PD: *odr = 0.0f; break; case LSM6DS3_XL_ODR_13HZ: *odr = 13.0f; break; case LSM6DS3_XL_ODR_26HZ: *odr = 26.0f; break; case LSM6DS3_XL_ODR_52HZ: *odr = 52.0f; break; case LSM6DS3_XL_ODR_104HZ: *odr = 104.0f; break; case LSM6DS3_XL_ODR_208HZ: *odr = 208.0f; break; case LSM6DS3_XL_ODR_416HZ: *odr = 416.0f; break; case LSM6DS3_XL_ODR_833HZ: *odr = 833.0f; break; case LSM6DS3_XL_ODR_1660HZ: *odr = 1660.0f; break; case LSM6DS3_XL_ODR_3330HZ: *odr = 3330.0f; break; case LSM6DS3_XL_ODR_6660HZ: *odr = 6660.0f; break; default: break; } return IMU_6AXES_OK; }
/** * @brief Set LSM6DS3 common initialization * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_Common_Sensor_Enable(void) { uint8_t tmp1 = 0x00; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* Enable register address automatically incremented during a multiple byte access with a serial interface (I2C or SPI) */ tmp1 &= ~(LSM6DS3_XG_IF_INC_MASK); tmp1 |= LSM6DS3_XG_IF_INC; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_CTRL3_C, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* FIFO ODR selection */ tmp1 &= ~(LSM6DS3_XG_FIFO_ODR_MASK); tmp1 |= LSM6DS3_XG_FIFO_ODR_NA; /* FIFO mode selection */ tmp1 &= ~(LSM6DS3_XG_FIFO_MODE_MASK); tmp1 |= LSM6DS3_XG_FIFO_MODE_BYPASS; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; }
/** * @brief Disable free fall detection * @retval IMU_6AXES_OK in case of success, an error code otherwise */ static IMU_6AXES_StatusTypeDef LSM6DS3_Disable_Free_Fall_Detection( void ) { uint8_t tmp1 = 0x00; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* INT1_FF setting */ tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK); tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_DISABLE; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* FF_DUR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK); tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_DEFAULT; /* FF_THS setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK); tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_156MG; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; }