/**
* @brief Read a register from L3GD20 from ISR context
* @returns The register value or -1 if failure to get bus
* @param reg[in] Register address to be read
* \param[in] task woken
*/
static int32_t PIOS_L3GD20_GetRegIsr(uint8_t reg, bool *woken)
{
uint8_t data;
if(PIOS_L3GD20_ClaimBusIsr(woken) != 0)
return -1;
PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte
data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response
PIOS_L3GD20_ReleaseBusIsr(woken);
return data;
}
/**
* @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 IRQ Handler. Read all the data from onboard buffer
*/
bool PIOS_L3GD20_IRQHandler(void)
{
l3gd20_irq++;
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(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);
portBASE_TYPE xHigherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *) &normalized_data, &xHigherPriorityTaskWoken);
return woken || (xHigherPriorityTaskWoken == pdTRUE);
}
