Make revolution work with the FreeRTOS queue based MPU6000 driver

flight/Modules/Sensors/sensors.c

@@ -158,8 +158,8 @@ uint32_t sensor_dt_us;
 static void SensorsTask(void *parameters)
 {
 	portTickType lastSysTime;
-	uint32_t accel_samples;
+	uint32_t accel_samples = 0;
-	uint32_t gyro_samples;
+	uint32_t gyro_samples = 0;
 	int32_t accel_accum[3] = {0, 0, 0};
 	int32_t gyro_accum[3] = {0,0,0};
 	float gyro_scaling = 0;
@@ -301,35 +301,28 @@ static void SensorsTask(void *parameters)
 			case 0x02:  // MPU6000 board
 #if defined(PIOS_INCLUDE_MPU6000)
 			{
-				struct pios_mpu6000_data gyro;
+				struct pios_mpu6000_data mpu6000_data;
+				xQueueHandle queue = PIOS_MPU6000_GetQueue();
 				
-				count = 0;
+				while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY)
-				while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0 && !error)
+				{
-					error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
+					gyro_accum[0] += mpu6000_data.gyro_x;
-				if (error)
+					gyro_accum[1] += mpu6000_data.gyro_y;
-					continue;
+					gyro_accum[2] += mpu6000_data.gyro_z;
-				while(read_good == 0) {
-					count++;
 
-					gyro_accum[0] += gyro.gyro_x;
+					accel_accum[0] += mpu6000_data.accel_x;
-					gyro_accum[1] += gyro.gyro_y;
+					accel_accum[1] += mpu6000_data.accel_y;
-					gyro_accum[2] += gyro.gyro_z;
+					accel_accum[2] += mpu6000_data.accel_z;
 
-					accel_accum[0] += gyro.accel_x;
+					gyro_samples ++;
-					accel_accum[1] += gyro.accel_y;
+					accel_samples ++;
-					accel_accum[2] += gyro.accel_z;
-					
-					read_good = PIOS_MPU6000_ReadFifo(&gyro);
 				}
-				gyro_samples = count;
-				gyro_scaling = PIOS_MPU6000_GetScale();
 
-				accel_samples = count;
+				gyro_scaling = PIOS_MPU6000_GetScale();
 				accel_scaling = PIOS_MPU6000_GetAccelScale();
 
-				// Get temp from last reading
+				gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
-				gyrosData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
+				accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
-				accelsData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
 			}
 #endif /* PIOS_INCLUDE_MPU6000 */
 				break;

flight/PiOS/Common/pios_mpu6000.c

@@ -154,7 +154,7 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
 
 	// FIFO storage
 #if defined(PIOS_MPU6000_ACCEL)
-	// Set the accel to 8g mode
+	// Set the accel scale
 	while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, cfg->accel_range) != 0);
 	
 	while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0);

flight/PiOS/STM32F4xx/pios_spi.c

@@ -199,7 +199,17 @@ out_fail:
  * (Re-)initialises SPI peripheral clock rate
  *
  * \param[in] spi SPI number (0 or 1)
-* \param[in] spi_prescaler configures the SPI speed
+ * \param[in] spi_prescaler configures the SPI speed:
+ * <UL>
+ *   <LI>PIOS_SPI_PRESCALER_2: sets clock rate 27.7~ nS @ 72 MHz (36 MBit/s) (only supported for spi==0, spi1 uses 4 instead)
+ *   <LI>PIOS_SPI_PRESCALER_4: sets clock rate 55.5~ nS @ 72 MHz (18 MBit/s)
+ *   <LI>PIOS_SPI_PRESCALER_8: sets clock rate 111.1~ nS @ 72 MHz (9 MBit/s)
+ *   <LI>PIOS_SPI_PRESCALER_16: sets clock rate 222.2~ nS @ 72 MHz (4.5 MBit/s)
+ *   <LI>PIOS_SPI_PRESCALER_32: sets clock rate 444.4~ nS @ 72 MHz (2.25 MBit/s)
+ *   <LI>PIOS_SPI_PRESCALER_64: sets clock rate 888.8~ nS @ 72 MHz (1.125 MBit/s)
+ *   <LI>PIOS_SPI_PRESCALER_128: sets clock rate 1.7~ nS @ 72 MHz (0.562 MBit/s)
+ *   <LI>PIOS_SPI_PRESCALER_256: sets clock rate 3.5~ nS @ 72 MHz (0.281 MBit/s)
+ * </UL>
  * \return 0 if no error
  * \return -1 if disabled SPI port selected
  * \return -3 if invalid spi_prescaler selected
@@ -222,13 +232,11 @@ int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescale
 	SPI_InitStructure = spi_dev->cfg->init;
 	
 	/* Adjust the prescaler for the peripheral's clock */
-	/* XXX is this correct? */
 	SPI_InitStructure.SPI_BaudRatePrescaler = ((uint16_t) spi_prescaler & 7) << 3;
 	
 	/* Write back the new configuration */
 	SPI_Init(spi_dev->cfg->regs, &SPI_InitStructure);
 	
-	/* XXX what gets selected by this? */
 	PIOS_SPI_TransferByte(spi_id, 0xFF);
 	return 0;
 }
@@ -620,17 +628,6 @@ int32_t PIOS_SPI_Busy(uint32_t spi_id)
 	return(0);
 }
 
-void PIOS_SPI_SetPrescalar(uint32_t spi_id, uint32_t prescaler)
-{
-	struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;
-	
-	bool valid = PIOS_SPI_validate(spi_dev);
-	PIOS_Assert(valid);
-	PIOS_Assert(IS_SPI_BAUDRATE_PRESCALER(prescaler));
-	
-	spi_dev->cfg->regs->CR1 = (spi_dev->cfg->regs->CR1 & ~0x0038) | prescaler;
-}
-
 void PIOS_SPI_IRQ_Handler(uint32_t spi_id)
 {
 	struct pios_spi_dev * spi_dev = (struct pios_spi_dev *)spi_id;

flight/Revolution/System/pios_board.c

@@ -179,6 +179,7 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
 	.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
 	.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN,
 	.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
+	.accel_range = PIOS_MPU6000_ACCEL_8G,
 	.gyro_range = PIOS_MPU6000_SCALE_500_DEG,
 	.filter = PIOS_MPU6000_LOWPASS_256_HZ
 };
@@ -815,8 +816,7 @@ void PIOS_Board_Init(void) {
 			break;
 		case 0x02:
 #if defined(PIOS_INCLUDE_MPU6000)
-			PIOS_MPU6000_Attach(pios_spi_gyro_id);
+			PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg);
-			PIOS_MPU6000_Init(&pios_mpu6000_cfg);
 #endif
 			break;
 		default: