/**
* @brief Writes one byte to the MPU6000
* \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_MPU6000_SetReg(uint8_t reg, uint8_t data)
{
if (PIOS_MPU6000_ClaimBus() != 0)
return -1;
if (PIOS_SPI_TransferByte(pios_mpu6000_dev->spi_id, 0x7f & reg) != 0) {
PIOS_MPU6000_ReleaseBus();
return -2;
}
if (PIOS_SPI_TransferByte(pios_mpu6000_dev->spi_id, data) != 0) {
PIOS_MPU6000_ReleaseBus();
return -3;
}
PIOS_MPU6000_ReleaseBus();
return 0;
}
/**
* @brief Read a register from MPU6000
* @returns The register value or -1 if failure to get bus
* @param reg[in] Register address to be read
*/
static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
{
uint8_t data;
if (PIOS_MPU6000_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferByte(pios_mpu6000_dev->spi_id, (0x80 | reg)); // request byte
data = PIOS_SPI_TransferByte(pios_mpu6000_dev->spi_id, 0); // receive response
PIOS_MPU6000_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 0 if succesful
*/
int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
{
// THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION
uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 };
uint8_t rec[7];
if (PIOS_MPU6000_ClaimBus() != 0) {
return -1;
}
if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
return -2;
}
PIOS_MPU6000_ReleaseBus();
data->gyro_x = rec[1] << 8 | rec[2];
data->gyro_y = rec[3] << 8 | rec[4];
data->gyro_z = rec[5] << 8 | rec[6];
return 0;
}
static void PIOS_MPU6000_Task(void *parameters)
{
while (1) {
//Wait for data ready interrupt
if (PIOS_Semaphore_Take(pios_mpu6000_dev->data_ready_sema, PIOS_SEMAPHORE_TIMEOUT_MAX) != true)
continue;
enum {
IDX_SPI_DUMMY_BYTE = 0,
IDX_ACCEL_XOUT_H,
IDX_ACCEL_XOUT_L,
IDX_ACCEL_YOUT_H,
IDX_ACCEL_YOUT_L,
IDX_ACCEL_ZOUT_H,
IDX_ACCEL_ZOUT_L,
IDX_TEMP_OUT_H,
IDX_TEMP_OUT_L,
IDX_GYRO_XOUT_H,
IDX_GYRO_XOUT_L,
IDX_GYRO_YOUT_H,
IDX_GYRO_YOUT_L,
IDX_GYRO_ZOUT_H,
IDX_GYRO_ZOUT_L,
BUFFER_SIZE,
};
uint8_t mpu6000_send_buf[BUFFER_SIZE] = { PIOS_MPU60X0_ACCEL_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
uint8_t mpu6000_rec_buf[BUFFER_SIZE];
if (PIOS_MPU6000_ClaimBus() != 0)
continue;
if (PIOS_SPI_TransferBlock(pios_mpu6000_dev->spi_id, mpu6000_send_buf, mpu6000_rec_buf, sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBus();
continue;
}
PIOS_MPU6000_ReleaseBus();
// Rotate the sensor to OP convention. The datasheet defines X as towards the right
// and Y as forward. OP convention transposes this. Also the Z is defined negatively
// to our convention
#if defined(PIOS_MPU6000_ACCEL)
// Currently we only support rotations on top so switch X/Y accordingly
struct pios_sensor_accel_data accel_data;
struct pios_sensor_gyro_data gyro_data;
switch (pios_mpu6000_dev->cfg->orientation) {
case PIOS_MPU60X0_TOP_0DEG:
accel_data.y = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]);
accel_data.x = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]);
gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
gyro_data.z = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]);
accel_data.z = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]);
break;
case PIOS_MPU60X0_TOP_90DEG:
accel_data.y = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]);
accel_data.x = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]);
gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
gyro_data.z = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]);
accel_data.z = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]);
break;
case PIOS_MPU60X0_TOP_180DEG:
accel_data.y = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]);
accel_data.x = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]);
gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
gyro_data.z = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]);
accel_data.z = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]);
break;
case PIOS_MPU60X0_TOP_270DEG:
accel_data.y = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]);
accel_data.x = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]);
gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
gyro_data.z = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]);
accel_data.z = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]);
break;
case PIOS_MPU60X0_BOTTOM_0DEG:
accel_data.y = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]);
accel_data.x = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]);
gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
gyro_data.z = (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]);
accel_data.z = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]);
break;
case PIOS_MPU60X0_BOTTOM_90DEG:
accel_data.y = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]);
accel_data.x = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]);
gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
gyro_data.z = (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]);
accel_data.z = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]);
break;
case PIOS_MPU60X0_BOTTOM_180DEG:
accel_data.y = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]);
accel_data.x = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]);
gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
gyro_data.z = (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]);
accel_data.z = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]);
break;
case PIOS_MPU60X0_BOTTOM_270DEG:
accel_data.y = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]);
accel_data.x = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]);
gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
gyro_data.z = (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]);
accel_data.z = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]);
break;
}
int16_t raw_temp = (int16_t)(mpu6000_rec_buf[IDX_TEMP_OUT_H] << 8 | mpu6000_rec_buf[IDX_TEMP_OUT_L]);
float temperature = 35.0f + ((float)raw_temp + 512.0f) / 340.0f;
// Apply sensor scaling
float accel_scale = PIOS_MPU6000_GetAccelScale();
accel_data.x *= accel_scale;
accel_data.y *= accel_scale;
accel_data.z *= accel_scale;
accel_data.temperature = temperature;
float gyro_scale = PIOS_MPU6000_GetGyroScale();
gyro_data.x *= gyro_scale;
gyro_data.y *= gyro_scale;
gyro_data.z *= gyro_scale;
gyro_data.temperature = temperature;
PIOS_Queue_Send(pios_mpu6000_dev->accel_queue, &accel_data, 0);
PIOS_Queue_Send(pios_mpu6000_dev->gyro_queue, &gyro_data, 0);
#else
struct pios_sensor_gyro_data gyro_data;
switch (pios_mpu6000_dev->cfg->orientation) {
case PIOS_MPU60X0_TOP_0DEG:
gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
break;
case PIOS_MPU60X0_TOP_90DEG:
gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
break;
case PIOS_MPU60X0_TOP_180DEG:
gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
break;
case PIOS_MPU60X0_TOP_270DEG:
gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]);
gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]);
break;
}
gyro_data.z = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]);
int32_t raw_temp = (int16_t)(mpu6000_rec_buf[IDX_TEMP_OUT_H] << 8 | mpu6000_rec_buf[IDX_TEMP_OUT_L]);
float temperature = 35.0f + ((float)raw_temp + 512.0f) / 340.0f;
// Apply sensor scaling
float gyro_scale = PIOS_MPU6000_GetGyroScale();
gyro_data.x *= gyro_scale;
gyro_data.y *= gyro_scale;
gyro_data.z *= gyro_scale;
gyro_data.temperature = temperature;
PIOS_Queue_Send(pios_mpu6000_dev->gyro_queue, &gyro_data, 0);
#endif /* PIOS_MPU6000_ACCEL */
}
}
/**
* @brief Initialize the MPU6000 3-axis gyro sensor
* \return none
* \param[in] PIOS_MPU6000_ConfigTypeDef struct to be used to configure sensor.
*
*/
static void PIOS_MPU6000_Config(const struct pios_mpu60x0_cfg *cfg)
{
#if defined(PIOS_MPU6000_SIMPLE_INIT_SEQUENCE)
// Reset chip registers
PIOS_MPU6000_SetReg(PIOS_MPU60X0_PWR_MGMT_REG, PIOS_MPU60X0_PWRMGMT_IMU_RST);
// Reset sensors signal path
PIOS_MPU6000_SetReg(PIOS_MPU60X0_USER_CTRL_REG, PIOS_MPU60X0_USERCTL_GYRO_RST);
// Give chip some time to initialize
PIOS_DELAY_WaitmS(10);
//Power management configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_PWR_MGMT_REG, cfg->Pwr_mgmt_clk);
// User control
PIOS_MPU6000_SetReg(PIOS_MPU60X0_USER_CTRL_REG, cfg->User_ctl);
// Digital low-pass filter and scale
// set this before sample rate else sample rate calculation will fail
PIOS_MPU6000_SetLPF(cfg->default_filter);
// Sample rate
PIOS_MPU6000_SetSampleRate(cfg->default_samplerate);
// Set the gyro scale
PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG);
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel scale
PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G);
#endif /* PIOS_MPU6000_ACCEL */
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_CFG_REG, cfg->interrupt_cfg);
// Interrupt enable
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_EN_REG, cfg->interrupt_en);
#else /* PIOS_MPU6000_SIMPLE_INIT_SEQUENCE */
/* This init sequence should really be dropped in favor of something
* less redundant but it seems to be hard to get it running well
* on all different targets.
*/
PIOS_MPU6000_ClaimBus();
PIOS_DELAY_WaitmS(1);
PIOS_MPU6000_ReleaseBus();
PIOS_DELAY_WaitmS(10);
// Reset chip
PIOS_MPU6000_SetReg(PIOS_MPU60X0_PWR_MGMT_REG, 0x80 | cfg->Pwr_mgmt_clk);
do {
PIOS_DELAY_WaitmS(5);
} while (PIOS_MPU6000_GetReg(PIOS_MPU60X0_PWR_MGMT_REG) & 0x80);
PIOS_DELAY_WaitmS(25);
// Reset chip and fifo
PIOS_MPU6000_SetReg(PIOS_MPU60X0_USER_CTRL_REG, 0x80 | 0x01 | 0x02 | 0x04);
do {
PIOS_DELAY_WaitmS(5);
} while (PIOS_MPU6000_GetReg(PIOS_MPU60X0_USER_CTRL_REG) & 0x07);
PIOS_DELAY_WaitmS(25);
//Power management configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_PWR_MGMT_REG, cfg->Pwr_mgmt_clk);
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_CFG_REG, cfg->interrupt_cfg);
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_EN_REG, cfg->interrupt_en);
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel scale
PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G);
#endif
// Digital low-pass filter and scale
// set this before sample rate else sample rate calculation will fail
PIOS_MPU6000_SetLPF(cfg->default_filter);
// Sample rate
PIOS_MPU6000_SetSampleRate(cfg->default_samplerate);
// Set the gyro scale
PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG);
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_USER_CTRL_REG, cfg->User_ctl);
//Power management configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_PWR_MGMT_REG, cfg->Pwr_mgmt_clk);
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_CFG_REG, cfg->interrupt_cfg);
// Interrupt configuration
PIOS_MPU6000_SetReg(PIOS_MPU60X0_INT_EN_REG, cfg->interrupt_en);
#endif /* PIOS_MPU6000_SIMPLE_INIT_SEQUENCE */
pios_mpu6000_dev->configured = true;
}
