/**
* @brief Read the identification bytes from the MPU6050 sensor
* \return ID read from MPU6050 or -1 if failure
*/
int32_t PIOS_L3GD20_ReadID()
{
int32_t l3gd20_id = PIOS_L3GD20_GetReg(PIOS_L3GD20_WHOAMI);
if(l3gd20_id < 0)
return -1;
return l3gd20_id;
}
/**
* @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 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;
}
