mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
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
This commit is contained in:
parent
810fa70856
commit
fedadb1275
@ -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)
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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 */
|
||||
|
@ -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;
|
||||
|
Loading…
x
Reference in New Issue
Block a user