CC-8: Move the queue registration into the ADC PIOS driver to allow other

functions to use it easily

Conflicts:

	flight/Modules/Attitude/attitude.c

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2707 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/CopterControl/Makefile

@@ -57,7 +57,7 @@ FLASH_TOOL = OPENOCD
 USE_THUMB_MODE = YES
 
 # List of modules to include
-MODULES = Telemetry Attitude Stabilization Actuator ManualControl #FirmwareIAP
+MODULES = Telemetry Attitude Stabilization #Actuator ManualControl #FirmwareIAP
 
 # MCU name, submodel and board
 # - MCU used for compiler-option (-mcpu)

flight/Modules/Attitude/attitude.c

@@ -74,8 +74,6 @@ static xTaskHandle taskHandle;
 // Private functions
 static void AttitudeTask(void *parameters);
 
-void adc_callback(float * data);
-
 static float gyro_correct_int[3] = {0,0,0};
 static xQueueHandle gyro_queue;
 
@@ -99,9 +97,10 @@ int32_t AttitudeInitialize(void)
 	AttitudeActualSet(&attitude);
 	
 	// Create queue for passing gyro data, allow 2 back samples in case
-	gyro_queue = xQueueCreate(2, sizeof(float) * 3);
+	gyro_queue = xQueueCreate(2, sizeof(float) * 4);
 	if(gyro_queue == NULL) 
 		return -1;
+	PIOS_ADC_SetQueue(gyro_queue);
 	
 	// Start main task
 	xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
@@ -119,7 +118,6 @@ static void AttitudeTask(void *parameters)
 	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
 
 	PIOS_ADC_Config(PIOS_ADC_RATE / (1000 / UPDATE_RATE));
-	PIOS_ADC_SetCallback(adc_callback);
 
 	// Keep flash CS pin high while talking accel
 	PIOS_FLASH_DISABLE;		
@@ -161,7 +159,7 @@ static void updateSensors()
 	AttitudeSettingsGet(&settings);
 	
 	struct pios_adxl345_data accel_data;
-	float gyro[3];
+	float gyro[4];
 	
 	// Only wait the time for two nominal updates before setting an alarm
 	if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) {
@@ -169,9 +167,11 @@ static void updateSensors()
 		return;
 	}
 	
-	attitudeRaw.gyros[ATTITUDERAW_GYROS_X] = -(gyro[0] - GYRO_NEUTRAL) * settings.GyroGain;
+	
-	attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] = (gyro[1] - GYRO_NEUTRAL) * settings.GyroGain;
+	// First sample is temperature
-	attitudeRaw.gyros[ATTITUDERAW_GYROS_Z] = -(gyro[2] - GYRO_NEUTRAL) * settings.GyroGain;
+	attitudeRaw.gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * settings.GyroGain;
+	attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] = (gyro[2] - GYRO_NEUTRAL) * settings.GyroGain;
+	attitudeRaw.gyros[ATTITUDERAW_GYROS_Z] = -(gyro[3] - GYRO_NEUTRAL) * settings.GyroGain;
 	
 	// Applying integral component here so it can be seen on the gyros and correct bias
 	attitudeRaw.gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0];
@@ -285,14 +285,6 @@ static void updateAttitude()
 
 	AttitudeActualSet(&attitudeActual);
 }
-
-void adc_callback(float * data) 
-{
-	static portBASE_TYPE xHigherPriorityTaskWoken;
-	xQueueSendFromISR(gyro_queue, data, &xHigherPriorityTaskWoken);
-	portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
-}
-
 /**
   * @}
   * @}

flight/PiOS/STM32F10x/pios_adc.c

@@ -49,6 +49,10 @@ void PIOS_ADC_Init()
 {	
 	pios_adc_devs[0].callback_function = NULL;
 	
+#if defined(PIOS_INCLUDE_FREERTOS)
+	pios_adc_devs[0].data_queue = NULL;
+#endif
+	
 	/* Setup analog pins */
 	GPIO_InitTypeDef GPIO_InitStructure;
 	GPIO_StructInit(&GPIO_InitStructure);
@@ -191,6 +195,16 @@ void PIOS_ADC_SetCallback(ADCCallback new_function)
 	pios_adc_devs[0].callback_function = new_function;
 }
 
+#if defined(PIOS_INCLUDE_FREERTOS)
+/**
+ * @brief Register a queue to add data to when downsampled 
+ */
+void PIOS_ADC_SetQueue(xQueueHandle data_queue) 
+{
+	pios_adc_devs[0].data_queue = data_queue;
+}
+#endif
+
 /**
  * @brief Return the address of the downsampled data buffer 
  */
@@ -247,6 +261,13 @@ void PIOS_ADC_downsample_data()
 		downsampled_buffer[chan] = (float) sum / pios_adc_devs[0].fir_coeffs[pios_adc_devs[0].adc_oversample];
 	}
 	
+#if defined(PIOS_INCLUDE_FREERTOS)
+	if(pios_adc_devs[0].data_queue) {
+		static portBASE_TYPE xHigherPriorityTaskWoken;
+		xQueueSendFromISR(pios_adc_devs[0].data_queue, pios_adc_devs[0].downsampled_buffer, &xHigherPriorityTaskWoken);
+		portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);		
+	}
+#endif
 	if(pios_adc_devs[0].callback_function)
 		pios_adc_devs[0].callback_function(pios_adc_devs[0].downsampled_buffer);
 }

flight/PiOS/inc/pios_adc.h

@@ -44,6 +44,9 @@ int32_t PIOS_ADC_PinGet(uint32_t pin);
 int16_t * PIOS_ADC_GetRawBuffer(void);
 uint8_t PIOS_ADC_GetOverSampling(void);
 void PIOS_ADC_SetCallback(ADCCallback new_function);
+#if defined(PIOS_INCLUDE_FREERTOS)
+void PIOS_ADC_SetQueue(xQueueHandle data_queue);
+#endif
 extern void PIOS_ADC_DMA_Handler(void);
 
 #endif /* PIOS_ADC_H */

flight/PiOS/inc/pios_adc_priv.h

@@ -45,6 +45,9 @@ struct pios_adc_cfg {
 struct pios_adc_dev {
 	const struct pios_adc_cfg *const cfg;	
 	ADCCallback callback_function;
+#if defined(PIOS_INCLUDE_FREERTOS)
+	xQueueHandle data_queue;
+#endif
 	volatile int16_t *valid_data_buffer;
 	volatile uint8_t adc_oversample;
 	uint8_t dma_block_size;