/**
* @brief Set L3GD20 Interrupt configuration
* @param L3GD20_InterruptConfig_TypeDef: pointer to a L3GD20_InterruptConfig_TypeDef
* structure that contains the configuration setting for the L3GD20 Interrupt.
* @retval None
*/
void L3GD20_INT1InterruptConfig(L3GD20_InterruptConfigTypeDef *L3GD20_IntConfigStruct)
{
uint8_t ctrl_cfr = 0x00, ctrl3 = 0x00;
/* Read INT1_CFG register */
L3GD20_Read(&ctrl_cfr, L3GD20_INT1_CFG_ADDR, 1);
/* Read CTRL_REG3 register */
L3GD20_Read(&ctrl3, L3GD20_CTRL_REG3_ADDR, 1);
ctrl_cfr &= 0x80;
ctrl3 &= 0xDF;
/* Configure latch Interrupt request and axe interrupts */
ctrl_cfr |= (uint8_t)(L3GD20_IntConfigStruct->Latch_Request| \
L3GD20_IntConfigStruct->Interrupt_Axes);
ctrl3 |= (uint8_t)(L3GD20_IntConfigStruct->Interrupt_ActiveEdge);
/* Write value to MEMS INT1_CFG register */
L3GD20_Write(&ctrl_cfr, L3GD20_INT1_CFG_ADDR, 1);
/* Write value to MEMS CTRL_REG3 register */
L3GD20_Write(&ctrl3, L3GD20_CTRL_REG3_ADDR, 1);
}
void Gyroscope::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;
}
L3GD20_Read(&ctrl5, L3GD20_CTRL_REG5_ADDR, 1);
ctrl5 = (ctrl5 & (~0x40)) | fifoEn;
L3GD20_Write(&ctrl5, L3GD20_CTRL_REG5_ADDR, 1);
L3GD20_Read(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1);
fifoCtrl = (fifoCtrl & (~0xE0)) | fifoMode;
L3GD20_Write(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1);
// Clear FIFO from previously stored data
if(fifoEn == FIFO_ENABLED)
clearFifo();
}
/**
* @brief Set L3GD20 Initialization.
* @param L3GD20_InitStruct: pointer to a L3GD20_InitTypeDef structure
* that contains the configuration setting for the L3GD20.
* @retval None
*/
void L3GD20_Init(L3GD20_InitTypeDef *L3GD20_InitStruct) {
uint8_t ctrl1 = 0x00, ctrl4 = 0x00, fifoCTRL = 0x00;
/* Configure the low level interface ---------------------------------------*/
L3GD20_LowLevel_Init();
/* Configure MEMS: data rate, power mode, full scale and axes */
ctrl1 |= (uint8_t)(
L3GD20_InitStruct->Power_Mode | L3GD20_InitStruct->Output_DataRate
| L3GD20_InitStruct->Axes_Enable
| L3GD20_InitStruct->Band_Width);
ctrl4 |= (uint8_t)(
L3GD20_InitStruct->BlockData_Update | L3GD20_InitStruct->Endianness
| L3GD20_InitStruct->Full_Scale);
/* Write value to MEMS CTRL_REG1 regsister */
L3GD20_Write(&ctrl1, L3GD20_CTRL_REG1_ADDR, 1);
/* Write value to MEMS CTRL_REG4 regsister */
L3GD20_Write(&ctrl4, L3GD20_CTRL_REG4_ADDR, 1);
// configure fifo
fifoCTRL |= (uint8_t)(
L3GD20_InitStruct->FifoMode
| (L3GD20_InitStruct->FifoThreshold & 0x0F));
L3GD20_Write(&fifoCTRL, L3GD20_FIFO_CTRL_REG_ADDR, 1);
// Enable fifo is necessary
if (L3GD20_InitStruct->FifoMode != L3GD20_FIFO_BYPASS_MODE ) {
uint8_t enableFifo = 0x40;
L3GD20_Write(&enableFifo, L3GD20_CTRL_REG5_ADDR, 1);
}
}
void Gyroscope::resetFifo()
{
uint8_t fifoCtrl;
L3GD20_Read(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1);
// Set to bypass mode to restart data collection
fifoCtrl = (fifoCtrl & (~0xE0));
L3GD20_Write(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1);
sleep(CHANGE_DELAY);
// Change back to FIFO mode
fifoCtrl = fifoCtrl | FIFO_MODE;
L3GD20_Write(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1);
sleep(CHANGE_DELAY);
}
void Gyroscope::changeScale(uint8_t scale)
{
uint8_t ctrl4, fifoCtrl;
scale &= 0x30;
L3GD20_Read(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1);
bool fifoMode = (fifoCtrl & 0x70) != 0;
/* Retrieve stored values before changing scale */
retrieveValues();
/* Read current value from CTRL_REG4 register */
L3GD20_Read(&ctrl4, L3GD20_CTRL_REG4_ADDR, 1);
/* Change scale */
ctrl4 = (ctrl4 & ~0x30) | scale;
/* Write new value to CTRL_REG4 regsister */
L3GD20_Write(&ctrl4, L3GD20_CTRL_REG4_ADDR, 1);
/* 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 L3GD20 Initialization.
* @param L3GD20_InitStruct: pointer to a L3GD20_InitTypeDef structure
* that contains the configuration setting for the L3GD20.
* @retval None
*/
void L3GD20_Init(L3GD20_InitTypeDef *L3GD20_InitStruct)
{
uint8_t ctrl1 = 0x00, ctrl4 = 0x00;
/* Configure the low level interface ---------------------------------------*/
L3GD20_LowLevel_Init();
/* Configure MEMS: data rate, power mode, full scale and axes */
ctrl1 |= (uint8_t) (L3GD20_InitStruct->Power_Mode | L3GD20_InitStruct->Output_DataRate | \
L3GD20_InitStruct->Axes_Enable | L3GD20_InitStruct->Band_Width);
ctrl4 |= (uint8_t) (L3GD20_InitStruct->BlockData_Update | L3GD20_InitStruct->Endianness | \
L3GD20_InitStruct->Full_Scale);
/* Write value to MEMS CTRL_REG1 regsister */
L3GD20_Write(&ctrl1, L3GD20_CTRL_REG1_ADDR, 1);
/* Write value to MEMS CTRL_REG4 regsister */
L3GD20_Write(&ctrl4, L3GD20_CTRL_REG4_ADDR, 1);
}
/**
* @brief Enable or disable INT2 interrupt
* @param InterruptState: State of INT1 Interrupt
* This parameter can be:
* @arg L3GD20_INT2INTERRUPT_DISABLE
* @arg L3GD20_INT2INTERRUPT_ENABLE
* @retval None
*/
void L3GD20_INT2InterruptCmd(uint8_t InterruptState) {
uint8_t tmpreg;
/* Read CTRL_REG3 register */
L3GD20_Read(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1);
tmpreg &= 0xF7;
tmpreg |= InterruptState;
/* Write value to MEMS CTRL_REG3 regsister */
L3GD20_Write(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1);
}
/**
* @brief Reboot memory content of L3GD20
* @param None
* @retval None
*/
void L3GD20_RebootCmd(void) {
uint8_t tmpreg;
/* Read CTRL_REG5 register */
L3GD20_Read(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1);
/* Enable or Disable the reboot memory */
tmpreg |= L3GD20_BOOT_REBOOTMEMORY;
/* Write value to MEMS CTRL_REG5 regsister */
L3GD20_Write(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1);
}
/**
* @brief Enable or Disable High Pass Filter
* @param HighPassFilterState: new state of the High Pass Filter feature.
* This parameter can be:
* @arg: L3GD20_HIGHPASSFILTER_DISABLE
* @arg: L3GD20_HIGHPASSFILTER_ENABLE
* @retval None
*/
void L3GD20_FilterCmd(uint8_t HighPassFilterState) {
uint8_t tmpreg;
/* Read CTRL_REG5 register */
L3GD20_Read(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1);
tmpreg &= 0xEF;
tmpreg |= HighPassFilterState;
/* Write value to MEMS CTRL_REG5 regsister */
L3GD20_Write(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1);
}
/**
* @brief Set High Pass Filter Modality
* @param L3GD20_FilterStruct: pointer to a L3GD20_FilterConfigTypeDef structure
* that contains the configuration setting for the L3GD20.
* @retval None
*/
void L3GD20_FilterConfig(L3GD20_FilterConfigTypeDef *L3GD20_FilterStruct)
{
uint8_t tmpreg;
/* Read CTRL_REG2 register */
L3GD20_Read(&tmpreg, L3GD20_CTRL_REG2_ADDR, 1);
tmpreg &= 0xC0;
/* Configure MEMS: mode and cutoff frquency */
tmpreg |= (uint8_t) (L3GD20_FilterStruct->HighPassFilter_Mode_Selection |\
L3GD20_FilterStruct->HighPassFilter_CutOff_Frequency);
/* Write value to MEMS CTRL_REG2 regsister */
L3GD20_Write(&tmpreg, L3GD20_CTRL_REG2_ADDR, 1);
}