/**
* @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 Sets the maximum range of the L3GD20
* @returns 0 for success, -1 for invalid device, -2 if unable to set register
*/
int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range)
{
if(PIOS_L3GD20_Validate(dev) != 0)
return -1;
dev->range = range;
if(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, dev->range) != 0)
return -2;
return 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;
}
/**
* @brief Set the sample rate, 760 or 380
* @param[in] enum pios_l3gd20_rate
* @return 0 if successful, -1 for invalid device, -2 if unable to get register, -3 if unable to set register
*/
int32_t PIOS_L3GD20_SetSampleRate(enum pios_l3gd20_rate rate)
{
if (PIOS_L3GD20_Validate(pios_l3gd20_dev) != 0)
return -1;
int32_t l3gd20_reg1 = PIOS_L3GD20_GetReg(PIOS_L3GD20_CTRL_REG1) & 0x0F;
if (l3gd20_reg1 == -1)
return -2;
if (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, rate | l3gd20_reg1) != 0)
return -3;
return 0;
}
/**
* @brief Sets the maximum range of the L3GD20
* @returns 0 for success, -1 for invalid device, -2 if unable to set register
*/
int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range)
{
if (PIOS_L3GD20_Validate(pios_l3gd20_dev) != 0)
return -1;
pios_l3gd20_dev->range = range;
if (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, pios_l3gd20_dev->range) != 0)
return -2;
switch(range) {
case PIOS_L3GD20_SCALE_250_DEG:
PIOS_SENSORS_SetMaxGyro(250);
break;
case PIOS_L3GD20_SCALE_500_DEG:
PIOS_SENSORS_SetMaxGyro(500);
break;
case PIOS_L3GD20_SCALE_2000_DEG:
PIOS_SENSORS_SetMaxGyro(2000);
break;
}
return 0;
}
