rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

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

Browse Source 
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
This commit is contained in:
peabody124 2011-02-03 02:42:43 +00:00 committed by peabody124
parent 810fa70856
commit fedadb1275
5 changed files with 37 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/CopterControl/Makefile
View File

@ -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)

26
flight/Modules/Attitude/attitude.c
View File

@ -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,10 +97,11 @@ 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);
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, 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;
attitudeRaw.gyros[ATTITUDERAW_GYROS_Z] = -(gyro[2] - GYRO_NEUTRAL) * settings.GyroGain;
// First sample is temperature
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);
}
/**
* @}
* @}

21
flight/PiOS/STM32F10x/pios_adc.c
View File

@ -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);
}

3
flight/PiOS/inc/pios_adc.h
View File

@ -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 */

3
flight/PiOS/inc/pios_adc_priv.h
View File

@ -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;