/**
* @brief Initialize the L3GD20 3-axis gyro sensor
* \return none
* \param[in] PIOS_L3GD20_ConfigTypeDef struct to be used to configure sensor.
*
*/
static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg)
{
// This register enables the channels and sets the bandwidth
while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, PIOS_L3GD20_CTRL1_FASTEST |
PIOS_L3GD20_CTRL1_PD | PIOS_L3GD20_CTRL1_ZEN |
PIOS_L3GD20_CTRL1_YEN | PIOS_L3GD20_CTRL1_XEN) != 0);
// Disable the high pass filters
while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG2, 0) != 0);
// Set int2 to go high on data ready
while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG3, 0x08) != 0);
// Select SPI interface, 500 deg/s, endianness?
while(PIOS_L3GD20_SetRange(cfg->range) != 0);
// Enable FIFO, disable HPF
while(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG5, 0x40) != 0);
// Fifo stream mode
while(PIOS_L3GD20_SetReg(PIOS_L3GD20_FIFO_CTRL_REG, 0x40) != 0);
}
/**
* @brief Initialize the L3GD20 3-axis gyro sensor
* \return 0 for successful configuration or -1 otherwise
* \param[in] PIOS_L3GD20_ConfigTypeDef struct to be used to configure sensor.
*
*/
static int32_t PIOS_L3GD20_Config(const struct pios_l3gd20_cfg *cfg)
{
// This register enables the channels
while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, PIOS_L3GD20_RATE_380HZ_100HZ |
PIOS_L3GD20_CTRL1_PD | PIOS_L3GD20_CTRL1_ZEN |
PIOS_L3GD20_CTRL1_YEN | PIOS_L3GD20_CTRL1_XEN) != 0);
// Disable the high pass filters
while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG2, 0) != 0);
// Set int2 to go high on data ready
while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG3, 0x08) != 0);
// Select SPI interface, 500 deg/s, endianness?
while (PIOS_L3GD20_SetRange(cfg->range) != 0);
// disable HPF
while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG5, 0x00) != 0);
pios_l3gd20_dev->configured = true;
return 0;
}
