/**
* @brief Writes one byte to the L3GD20
* \param[in] reg Register address
* \param[in] data Byte to write
* \return 0 if operation was successful
* \return -1 if unable to claim SPI bus
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_L3GD20_SetReg(uint8_t reg, uint8_t data)
{
if (PIOS_L3GD20_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferByte(pios_l3gd20_dev->spi_id, 0x7f & reg);
PIOS_SPI_TransferByte(pios_l3gd20_dev->spi_id, data);
PIOS_L3GD20_ReleaseBus();
return 0;
}
/**
* @brief Read a register from L3GD20
* @returns The register value or -1 if failure to get bus
* @param reg[in] Register address to be read
*/
static int32_t PIOS_L3GD20_GetReg(uint8_t reg)
{
uint8_t data;
if(PIOS_L3GD20_ClaimBus() != 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_ReleaseBus();
return data;
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \returns The number of samples remaining in the fifo
*/
static int32_t PIOS_L3GD20_ReadGyros(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];
if (PIOS_L3GD20_ClaimBus() != 0)
return -1;
if (PIOS_SPI_TransferBlock(pios_l3gd20_dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBus();
data->gyro_x = 0;
data->gyro_y = 0;
data->gyro_z = 0;
data->temperature = 0;
return -2;
}
PIOS_L3GD20_ReleaseBus();
memcpy((uint8_t *)&data->gyro_x, &rec[1], 6);
data->temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP);
return 0;
}