/**
* @brief Release the SPI bus for the accel communications and end the transaction
* @return 0 if successful, -1 for invalid device
*/
static int32_t PIOS_L3GD20_ReleaseBus()
{
if(PIOS_L3GD20_Validate(dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1);
return PIOS_SPI_ReleaseBus(dev->spi_id);
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction
* \param[in] pointer which receives if a task has been woken
* @return 0 if successful, -1 for invalid device
*/
static int32_t PIOS_L3GD20_ReleaseBusIsr(bool *woken)
{
if (PIOS_L3GD20_Validate(pios_l3gd20_dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(pios_l3gd20_dev->spi_id, pios_l3gd20_dev->slave_num, 1);
return PIOS_SPI_ReleaseBusISR(pios_l3gd20_dev->spi_id, woken);
}
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* \param[in] pointer which receives if a task has been woken
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
*/
static int32_t PIOS_L3GD20_ClaimBusIsr(bool *woken)
{
if(PIOS_L3GD20_Validate(dev) != 0)
return -1;
if(PIOS_SPI_ClaimBusISR(dev->spi_id, woken) < 0)
return -2;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
return 0;
}
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
*/
static int32_t PIOS_L3GD20_ClaimBus()
{
if(PIOS_L3GD20_Validate(dev) != 0)
return -1;
if(PIOS_SPI_ClaimBus(dev->spi_id) != 0)
return -2;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
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(dev) != 0)
return -1;
dev->range = range;
if(PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, dev->range) != 0)
return -2;
return 0;
}
/**
* @brief IRQ Handler. Read all the data from onboard buffer
*/
bool PIOS_L3GD20_IRQHandler(void)
{
if (PIOS_L3GD20_Validate(pios_l3gd20_dev) != 0)
return false;
bool woken = false;
PIOS_Semaphore_Give_FromISR(pios_l3gd20_dev->data_ready_sema, &woken);
return woken;
}
/**
* @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;
}
static float PIOS_L3GD20_GetScale()
{
if(PIOS_L3GD20_Validate(dev) != 0)
return -1;
switch (dev->range) {
case PIOS_L3GD20_SCALE_250_DEG:
return 0.00875f;
case PIOS_L3GD20_SCALE_500_DEG:
return 0.01750f;
case PIOS_L3GD20_SCALE_2000_DEG:
return 0.070f;
}
return 0;
}
/**
* @brief IRQ Handler. Read all the data from onboard buffer
*/
bool PIOS_L3GD20_IRQHandler(void)
{
if (PIOS_L3GD20_Validate(pios_l3gd20_dev) != 0 || pios_l3gd20_dev->configured == false)
return false;
struct pios_l3gd20_data data;
uint8_t buf[7] = { PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0 };
uint8_t rec[7];
/* This code duplicates ReadGyros above but uses ClaimBusIsr */
bool woken = false;
if (PIOS_L3GD20_ClaimBusIsr(&woken) != 0)
return woken;
if (PIOS_SPI_TransferBlock(pios_l3gd20_dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBusIsr(&woken);
return woken;
}
PIOS_L3GD20_ReleaseBusIsr(&woken);
memcpy((uint8_t *)&data.gyro_x, &rec[1], 6);
// TODO: This reordering is specific to the FlyingF3 chip placement. Whenever
// this code is used on another board add an orientation mapping to the configuration
struct pios_sensor_gyro_data normalized_data;
float scale = PIOS_L3GD20_GetScale();
normalized_data.y = data.gyro_x * scale;
normalized_data.x = data.gyro_y * scale;
normalized_data.z = -data.gyro_z * scale;
normalized_data.temperature = PIOS_L3GD20_GetRegIsr(PIOS_L3GD20_OUT_TEMP, &woken);
PIOS_Queue_Send_FromISR(pios_l3gd20_dev->queue, &normalized_data, &woken);
return woken;
}
/**
* @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;
}
