rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

CC-8 Whenever the gyro data is updated the callback now pushes the data onto a

Browse Source 
queue, which update sensors waits for.  This confirms one update per gyro
update.  All available accel data will be pulled off each time.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2705 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-02-03 02:42:33 +00:00 committed by peabody124
parent 90fd4551b6
commit c95a6447bf
1 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -75,9 +75,9 @@ static xTaskHandle taskHandle;
static void AttitudeTask(void *parameters);
void adc_callback(float * data);
float gyro[3] = {0, 0, 0};
static float gyro_correct_int[3] = {0,0,0};
static xQueueHandle gyro_queue;
static void initSensors();
static void updateSensors();
@ -98,7 +98,10 @@ int32_t AttitudeInitialize(void)
attitude.q4 = 0;
AttitudeActualSet(&attitude);
// Create queue for passing gyro data, allow 2 back samples in case
gyro_queue = xQueueCreate(2, sizeof(float) * 3);
if(gyro_queue == NULL)
return -1;
// Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
@ -106,7 +109,7 @@ int32_t AttitudeInitialize(void)
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
return 0;
}
static portTickType lastSysTime;
/**
* Module thread, should not return.
*/
@ -126,18 +129,10 @@ static void AttitudeTask(void *parameters)
// Main task loop
while (1) {
//
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
// TODO: register the adc callback, push the data onto a queue (safe for thread)
// with the queue ISR version
updateSensors();
updateAttitude();
/* Wait for the next update interval */
vTaskDelayUntil(&lastSysTime, UPDATE_RATE / portTICK_RATE_MS);
//vTaskDelay(UPDATE_RATE / portTICK_RATE_MS);
}
}
@ -167,6 +162,13 @@ static void updateSensors()
AttitudeSettingsGet(&settings);
struct pios_adxl345_data accel_data;
float gyro[3];
// Only wait the time for two nominal updates before setting an alarm
if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return;
}
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = -(gyro[0] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = (gyro[1] - GYRO_NEUTRAL) * settings.GyroGain;
@ -291,9 +293,9 @@ static void updateAttitude()
void adc_callback(float * data)
{
gyro[0] = data[1];
gyro[1] = data[2];
gyro[2] = data[3];
static portBASE_TYPE xHigherPriorityTaskWoken;
xQueueSendFromISR(gyro_queue, data, &xHigherPriorityTaskWoken);
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
}
/**