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

Update the MPU6000 FIFO code and also fix the temperature code

Browse Source
This commit is contained in:
James Cotton 2011-11-18 00:24:55 -06:00
parent 6d74e96c31
commit a7ef5601e0
2 changed files with 30 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -274,10 +274,10 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = -((float) gyro_accum[2]) * scaling;
// From data sheet 35 deg C corresponds to -13200, and 280 LSB per C
attitudeRaw->temperature[ATTITUDERAW_TEMPERATURE_GYRO] = gyro.temperature = 35.0f + ((float) gyro.temperature + 13200) / 280;
attitudeRaw->temperature[ATTITUDERAW_TEMPERATURE_GYRO] = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
// From the data sheet 25 deg C corresponds to 2 and 2 LSB per C
attitudeRaw->temperature[ATTITUDERAW_TEMPERATURE_ACCEL] = 25.0f + ((float) accel.temperature - 2) / 2;
attitudeRaw->temperature[ATTITUDERAW_TEMPERATURE_ACCEL] = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
if(bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias

29
flight/PiOS/STM32F4xx/pios_mpu6000.c
View File

@ -285,11 +285,34 @@ uint8_t PIOS_MPU6000_Test(void)
return 0;
}
/**
* @brief Run self-test operation.
* \return 0 if test succeeded
* \return non-zero value if test succeeded
*/
static int32_t PIOS_MPU6000_FifoDepth(void)
{
uint8_t mpu6000_send_buf[3] = {PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0};
uint8_t mpu6000_rec_buf[3];
if(PIOS_MPU6000_ClaimBus() != 0)
return -1;
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBus();
return -1;
}
PIOS_MPU6000_ReleaseBus();
return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2];
}
/**
* @brief IRQ Handler. Read all the data from onboard buffer
*/
uint32_t mpu6000_irq = 0;
uint32_t mpu6000_count;
int32_t mpu6000_count;
uint32_t mpu6000_fifo_full = 0;
uint8_t mpu6000_last_read_count = 0;
@ -308,6 +331,10 @@ void PIOS_MPU6000_IRQHandler(void)
if(!mpu6000_configured)
return;
mpu6000_count = PIOS_MPU6000_FifoDepth();
if(mpu6000_count < sizeof(struct pios_mpu6000_data))
return;
if(PIOS_MPU6000_ClaimBus() != 0)
return;