From a7ef5601e0311a26707d6dadb3fb5bfda907f3ed Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Fri, 18 Nov 2011 00:24:55 -0600
Subject: [PATCH] Update the MPU6000 FIFO code and also fix the temperature
 code

---
 flight/Modules/Attitude/revolution/attitude.c |  4 +--
 flight/PiOS/STM32F4xx/pios_mpu6000.c          | 29 ++++++++++++++++++-
 2 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c
index edb50ad68..c0da755d6 100644
--- a/flight/Modules/Attitude/revolution/attitude.c
+++ b/flight/Modules/Attitude/revolution/attitude.c
@@ -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
diff --git a/flight/PiOS/STM32F4xx/pios_mpu6000.c b/flight/PiOS/STM32F4xx/pios_mpu6000.c
index 01b50063c..59b6a6548 100644
--- a/flight/PiOS/STM32F4xx/pios_mpu6000.c
+++ b/flight/PiOS/STM32F4xx/pios_mpu6000.c
@@ -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;