rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Get the MPU6050 fifo running and read by the attitude module

Browse Source
This commit is contained in:
James Cotton 2011-11-14 11:23:14 -06:00
parent 59507249e1
commit aeb42332d6
2 changed files with 13 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/Modules/Attitude/revolution/attitude.c
View File

@ -257,7 +257,7 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = accel_accum[2] * scaling;
/*
// Make sure we get one sample
count = 0;
while((read_good = PIOS_MPU6050_ReadFifo(&gyro)) != 0);
@ -271,13 +271,6 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
read_good = PIOS_MPU6050_ReadFifo(&gyro);
}
gyro_samples = count;
*/
PIOS_MPU6050_ReadGyros(&gyro);
gyro_accum[0] = gyro.gyro_x;
gyro_accum[1] = gyro.gyro_y;
gyro_accum[2] = gyro.gyro_z;
gyro_samples = 1;
scaling = PIOS_MPU6050_GetScale() / gyro_samples;
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -((float) gyro_accum[1]) * scaling;

57
flight/PiOS/STM32F4xx/pios_mpu6050.c
View File

@ -82,8 +82,6 @@ void PIOS_MPU6050_Init(const struct pios_mpu6050_cfg * new_cfg)
* \param[in] PIOS_MPU6050_ConfigTypeDef struct to be used to configure sensor.
*
*/
uint8_t reg_val;
static void PIOS_MPU6050_Config(struct pios_mpu6050_cfg const * cfg)
{
mpu6050_first_read = true;
@ -117,8 +115,6 @@ static void PIOS_MPU6050_Config(struct pios_mpu6050_cfg const * cfg)
// Interrupt configuration
while (PIOS_MPU6050_Write(PIOS_MPU6050_INT_EN_REG, cfg->interrupt_en) != 0) ;
PIOS_MPU6050_Read(PIOS_MPU6050_INT_CFG_REG, &reg_val, 1);
mpu6050_configured = true;
}
@ -299,28 +295,18 @@ uint8_t PIOS_MPU6050_Test(void)
return 0;
}
static uint8_t mpu6050_read_buffer[sizeof(struct pios_mpu6050_data) + 2]; // Right now using ,Y,Z,fifo_footer
static uint8_t mpu6050_read_buffer[sizeof(struct pios_mpu6050_data)]; // Right now using ,Y,Z,fifo_footer
static void MPU6050_callback()
{
struct pios_mpu6050_data data;
if(fifoBuf_getFree(&pios_mpu6050_fifo) < sizeof(data))
goto out;
if(mpu6050_first_read) {
data.temperature = mpu6050_read_buffer[0] << 8 | mpu6050_read_buffer[1];
data.gyro_x = mpu6050_read_buffer[2] << 8 | mpu6050_read_buffer[3];
data.gyro_y = mpu6050_read_buffer[4] << 8 | mpu6050_read_buffer[5];
data.gyro_z = mpu6050_read_buffer[6] << 8 | mpu6050_read_buffer[7];
mpu6050_first_read = false;
} else {
// First two bytes are left over fifo from last call
data.temperature = mpu6050_read_buffer[2] << 8 | mpu6050_read_buffer[3];
data.gyro_x = mpu6050_read_buffer[4] << 8 | mpu6050_read_buffer[5];
data.gyro_y = mpu6050_read_buffer[6] << 8 | mpu6050_read_buffer[7];
data.gyro_z = mpu6050_read_buffer[8] << 8 | mpu6050_read_buffer[9];
}
data.temperature = mpu6050_read_buffer[0] << 8 | mpu6050_read_buffer[1];
data.gyro_x = mpu6050_read_buffer[2] << 8 | mpu6050_read_buffer[3];
data.gyro_y = mpu6050_read_buffer[4] << 8 | mpu6050_read_buffer[5];
data.gyro_z = mpu6050_read_buffer[6] << 8 | mpu6050_read_buffer[7];
fifoBuf_putData(&pios_mpu6050_fifo, (uint8_t *) &data, sizeof(data));
@ -344,7 +330,6 @@ void PIOS_MPU6050_IRQHandler(void)
if(!mpu6050_configured)
return;
return;
//PIOS_Assert(MPU6050_cb_ready);
if(!mpu6050_cb_ready) {
PIOS_LED_Toggle(LED2);
@ -352,36 +337,18 @@ void PIOS_MPU6050_IRQHandler(void)
return;
}
// If at least one read doesnt succeed then the irq not reset and we will stall
while(PIOS_MPU6050_Read(PIOS_MPU6050_FIFO_CNT_MSB, (uint8_t *) &fifo_level_data, sizeof(fifo_level_data)) != 0)
PIOS_DELAY_WaituS(10);
/*// If at least one read doesnt succeed then the irq not reset and we will stall
if(PIOS_MPU6050_Read(PIOS_MPU6050_FIFO_CNT_MSB, (uint8_t *) &fifo_level_data, sizeof(fifo_level_data)) != 0)
return;
fifo_level = (fifo_level_data[0] << 8) + fifo_level_data[1];
PIOS_DELAY_WaituS(10);
*/
if(mpu6050_first_read) {
// Stupid system for MPU6050. If first read from buffer then we will read 4 sensors without fifo
// footer. After this we will read out a fifo footer
if(fifo_level < sizeof(mpu6050_read_buffer))
return;
mpu6050_cb_ready = false;
// Leave footer in buffer
PIOS_MPU6050_Read_Callback(PIOS_MPU6050_FIFO_REG, mpu6050_read_buffer, sizeof(mpu6050_read_buffer) - 2, MPU6050_callback);
} else {
// Stupid system for MPU6050. Ensure something is left in buffer
if(fifo_level < (sizeof(mpu6050_read_buffer) + 2))
return;
mpu6050_cb_ready = false;
// Leave footer in buffer
PIOS_MPU6050_Read_Callback(PIOS_MPU6050_FIFO_REG, mpu6050_read_buffer, sizeof(mpu6050_read_buffer), MPU6050_callback);
}
// Leave footer in buffer
PIOS_MPU6050_Read_Callback(PIOS_MPU6050_FIFO_REG, mpu6050_read_buffer, sizeof(mpu6050_read_buffer), MPU6050_callback);
}
/**