/**
* @brief Set LSM303DLHC Mag Initialization.
* @param LSM303DLHC_InitStruct: pointer to a LSM303DLHC_MagInitTypeDef structure
* that contains the configuration setting for the LSM303DLHC.
* @retval None
*/
void LSM303DLHC_MagInit(LSM303DLHCMag_InitTypeDef *LSM303DLHC_InitStruct)
{
uint8_t cra_regm = 0x00, crb_regm = 0x00, mr_regm = 0x00;
/* Configure the low level interface ---------------------------------------*/
LSM303DLHC_LowLevel_Init();
/* Configure MEMS: temp and Data rate */
cra_regm |= (uint8_t) (LSM303DLHC_InitStruct->Temperature_Sensor | LSM303DLHC_InitStruct->MagOutput_DataRate);
/* Configure MEMS: full Scale */
crb_regm |= (uint8_t) (LSM303DLHC_InitStruct->MagFull_Scale);
/* Configure MEMS: working mode */
mr_regm |= (uint8_t) (LSM303DLHC_InitStruct->Working_Mode);
/* Write value to Mag MEMS CRA_REG regsister */
LSM303DLHC_Write(MAG_I2C_ADDRESS, LSM303DLHC_CRA_REG_M, &cra_regm);
/* Write value to Mag MEMS CRB_REG regsister */
LSM303DLHC_Write(MAG_I2C_ADDRESS, LSM303DLHC_CRB_REG_M, &crb_regm);
/* Write value to Mag MEMS MR_REG regsister */
LSM303DLHC_Write(MAG_I2C_ADDRESS, LSM303DLHC_MR_REG_M, &mr_regm);
}
void Accelerometer::selectMode(Mode mode)
{
uint8_t ctrl5, fifoCtrl;
uint8_t fifoEn, fifoMode;
switch(mode)
{
case BypassMode:
fifoEn = FIFO_DISABLED;
fifoMode = BYPASS_MODE;
break;
case FifoMode:
fifoEn = FIFO_ENABLED;
fifoMode = FIFO_MODE;
break;
case StreamMode:
fifoEn = FIFO_ENABLED;
fifoMode = STREAM_MODE;
break;
default:
return;
}
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG5_A, &ctrl5, 1);
ctrl5 = (ctrl5 & (~FIFO_ENABLED)) | fifoEn;
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG5_A, &ctrl5);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_CTRL_REG_A, &fifoCtrl, 1);
fifoCtrl = (fifoCtrl & (~MODE_BITS)) | fifoMode;
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_CTRL_REG_A, &fifoCtrl);
// Clear FIFO from previously stored data
if(fifoEn == FIFO_ENABLED)
clearFifo();
}
void Accelerometer::resetFifo()
{
uint8_t fifoCtrl;
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_CTRL_REG_A, &fifoCtrl, 1);
// Set to bypass mode to restart data collection
fifoCtrl = (fifoCtrl & (~MODE_BITS));
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_CTRL_REG_A, &fifoCtrl);
sleep(CHANGE_DELAY);
// Change back to FIFO mode
fifoCtrl = fifoCtrl | FIFO_MODE;
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_CTRL_REG_A, &fifoCtrl);
sleep(CHANGE_DELAY);
}
void Accelerometer::changeScale(uint8_t scale)
{
uint8_t ctrl4, fifoCtrl;
scale &= SCALE_BITS;
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_CTRL_REG_A, &fifoCtrl, 1);
bool fifoMode = (fifoCtrl & IS_FIFO) != 0;
/* Retrieve stored values before changing scale */
retrieveValues();
/* Read current value from CTRL_REG4 register */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, &ctrl4, 1);
/* Change scale */
ctrl4 = (ctrl4 & ~SCALE_BITS) | scale;
/* Write new value to CTRL_REG4 regsister */
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, &ctrl4);
/* Let L3GD20 perform changes */
sleep(CHANGE_DELAY);
/* Discard values recorded during scale change */
if(fifoMode)
clearFifo();
if (_scaleBuffer.back().second > 0)
_scaleBuffer.push_back(std::make_pair(scale, 0));
else
_scaleBuffer.back().first = scale;
}
/**
* @brief Set LSM303DLHC Initialization.
* @param LSM303DLHC_InitStruct: pointer to a LSM303DLHC_InitTypeDef structure
* that contains the configuration setting for the LSM303DLHC.
* @retval None
*/
void LSM303DLHC_AccInit(LSM303DLHCAcc_InitTypeDef *LSM303DLHC_InitStruct)
{
uint8_t ctrl1 = 0x00, ctrl4 = 0x00;
/* Configure the low level interface ---------------------------------------*/
LSM303DLHC_LowLevel_Init();
/* Configure MEMS: data rate, power mode, full scale and axes */
ctrl1 |= (uint8_t) (LSM303DLHC_InitStruct->Power_Mode | LSM303DLHC_InitStruct->AccOutput_DataRate | \
LSM303DLHC_InitStruct->Axes_Enable);
ctrl4 |= (uint8_t) (LSM303DLHC_InitStruct->BlockData_Update | LSM303DLHC_InitStruct->Endianness | \
LSM303DLHC_InitStruct->AccFull_Scale|LSM303DLHC_InitStruct->High_Resolution);
/* Write value to ACC MEMS CTRL_REG1 regsister */
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG1_A, &ctrl1);
/* Write value to ACC MEMS CTRL_REG4 regsister */
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, &ctrl4);
}
/**
* @brief Reboot memory content of LSM303DLHC
* @param None
* @retval None
*/
void LSM303DLHC_AccRebootCmd(void) {
uint8_t tmpreg;
/* Read CTRL_REG5 register */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG5_A, &tmpreg, 1);
/* Enable or Disable the reboot memory */
tmpreg |= LSM303DLHC_BOOT_REBOOTMEMORY;
/* Write value to ACC MEMS CTRL_REG5 regsister */
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG5_A, &tmpreg);
}
/**
* @brief Enable or Disable High Pass Filter on CLick
* @param HighPassFilterState: new state of the High Pass Filter feature.
* This parameter can be:
* @arg: LSM303DLHC_HighPassFilter_DISABLE
* @arg: LSM303DLHC_HighPassFilter_ENABLE
* @retval None
*/
void LSM303DLHC_AccFilterClickCmd(uint8_t HighPassFilterClickState) {
uint8_t tmpreg;
/* Read CTRL_REG2 register */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG2_A, &tmpreg, 1);
tmpreg &= 0xFB;
tmpreg |= HighPassFilterClickState;
/* Write value to ACC MEMS CTRL_REG2 regsister */
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG2_A, &tmpreg);
}
/**
* @brief INT1 interrupt config
* @param ITCombination: Or or And combination
* ITAxes: axes to be enabled
* NewState: Enable or Disable
* @retval None
*/
void LSM303DLHC_AccClickITConfig(uint8_t ITClick, FunctionalState NewState) {
uint8_t tmpval;
/* Read CLICK_CFR register */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CLICK_CFG_A, &tmpval, 1);
if (NewState != DISABLE) {
tmpval |= ITClick;
} else {
/* Disable the selected interrupt */
tmpval = ~ITClick;
}
/* Write value to MEMS CLICK_CFR register */
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_CLICK_CFG_A, &tmpval);
}
/**
* @brief INT1 interrupt config
* @param ITCombination: Or or And combination
* ITAxes: axes to be enabled
* NewState: Enable or Disable
* @retval None
*/
void LSM303DLHC_AccINT2InterruptConfig(uint8_t ITCombination, uint8_t ITAxes,
FunctionalState NewState) {
uint8_t tmpval = ITCombination;
/* Read INT2_CFR register */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_INT2_CFG_A, &tmpval, 1);
if (NewState != DISABLE) {
tmpval |= ITAxes;
} else {
/* Disable the selected interrupt */
tmpval = (~ITAxes) | ITCombination;
}
/* Write value to MEMS INT2_CFR register */
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_INT2_CFG_A, &tmpval);
}
/**
* @brief Set High Pass Filter Modality
* @param LSM303DLHC_FilterStruct: pointer to a LSM303DLHC_FilterConfigTypeDef structure
* that contains the configuration setting for the LSM303DLHC.
* @retval None
*/
void LSM303DLHC_AccFilterConfig(LSM303DLHCAcc_FilterConfigTypeDef *LSM303DLHC_FilterStruct)
{
uint8_t tmpreg;
/* Read CTRL_REG2 register */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG2_A, &tmpreg, 1);
tmpreg &= 0x0C;
/* Configure MEMS: mode, cutoff frquency, Filter status, Click, AOI1 and AOI2 */
tmpreg |= (uint8_t) (LSM303DLHC_FilterStruct->HighPassFilter_Mode_Selection |\
LSM303DLHC_FilterStruct->HighPassFilter_CutOff_Frequency|\
LSM303DLHC_FilterStruct->HighPassFilter_AOI1|\
LSM303DLHC_FilterStruct->HighPassFilter_AOI2);
/* Write value to ACC MEMS CTRL_REG2 regsister */
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG2_A, &tmpreg);
}
/**
* @brief Set LSM303DLHC Interrupt2 configuration
* @param LSM303DLHC_IT: specifies the LSM303DLHC interrupt source to be enabled.
* This parameter can be any combination of the following values:
* @arg LSM303DLHC_IT2_CLICK2
* @arg LSM303DLHC_IT2_INT1
* @arg LSM303DLHC_IT2_INT2
* @arg LSM303DLHC_IT2_BOOT
* @arg LSM303DLHC_IT2_ACT
* @arg LSM303DLHC_IT2_HLACTIVE
* @param NewState: new state of the selected LSM303DLHC interrupts.
* This parameter can be: ENABLE or DISABLE.
* @retval None
*/
void LSM303DLHC_AccIT2Config(uint8_t LSM303DLHC_IT, FunctionalState NewState) {
uint8_t tmpval = 0x00;
/* Read CTRL_REG3 register */
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG6_A, &tmpval, 1);
tmpval &= ~LSM303DLHC_IT;
if (NewState != DISABLE) {
tmpval |= LSM303DLHC_IT;
} else {
/* Disable the selected interrupt */
tmpval = ~LSM303DLHC_IT;
}
/* Write value to MEMS CTRL_REG3 register */
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG6_A, &tmpval);
}
void accelerometerCompassInit()
{
LSM303DLHCMag_InitTypeDef LSM303DLHC_InitStructure;
LSM303DLHCAcc_InitTypeDef LSM303DLHCAcc_InitStructure;
LSM303DLHCAcc_FilterConfigTypeDef LSM303DLHCFilter_InitStructure;
uint8_t threshold;
/* Configure MEMS magnetometer main parameters: temp, working mode, full Scale and Data rate */
LSM303DLHC_InitStructure.Temperature_Sensor = LSM303DLHC_TEMPSENSOR_DISABLE;
LSM303DLHC_InitStructure.MagOutput_DataRate =LSM303DLHC_ODR_30_HZ ;
LSM303DLHC_InitStructure.MagFull_Scale = LSM303DLHC_FS_8_1_GA;
LSM303DLHC_InitStructure.Working_Mode = LSM303DLHC_CONTINUOS_CONVERSION;
LSM303DLHC_MagInit(&LSM303DLHC_InitStructure);
/* Fill the accelerometer structure */
LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_NORMAL_MODE;
LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_50_HZ;
LSM303DLHCAcc_InitStructure.Axes_Enable= LSM303DLHC_AXES_ENABLE;
LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB;
LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE;
/* Configure the accelerometer main parameters */
LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
/* Fill the accelerometer LPF structure */
LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection =LSM303DLHC_HPM_NORMAL_MODE;
LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency = LSM303DLHC_HPFCF_16;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1 = LSM303DLHC_HPF_AOI1_DISABLE;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2 = LSM303DLHC_HPF_AOI2_DISABLE;
/* Configure the accelerometer LPF main parameters */
LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
LSM303DLHC_AccIT1Config(LSM303DLHC_IT1_AOI1, ENABLE);
threshold = 126;
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_INT1_THS_A, 1, &threshold);
LSM303DLHC_AccINT1InterruptConfig(LSM303DLHC_OR_COMBINATION,
(LSM303DLHC_X_HIGH |
LSM303DLHC_Y_HIGH |
LSM303DLHC_Z_HIGH), ENABLE);
}
