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

For L3GD20 sensor task run at the sensor speed

Browse Source
This commit is contained in:
James Cotton 2012-01-28 09:41:03 -06:00
parent 13447ac907
commit a778c45d6f
1 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
flight/Modules/Sensors/sensors.c
View File

@ -143,6 +143,8 @@ int32_t mag_test;
* 2. MPU6000 gyro and accel * 2. MPU6000 gyro and accel
* 3. BMA180 accel and L3GD20 gyro * 3. BMA180 accel and L3GD20 gyro
*/ */
uint32_t sensor_dt_us;
static void SensorsTask(void *parameters) static void SensorsTask(void *parameters)
{ {
uint8_t init = 0; uint8_t init = 0;
@ -153,6 +155,7 @@ static void SensorsTask(void *parameters)
int32_t gyro_accum[3] = {0,0,0}; int32_t gyro_accum[3] = {0,0,0};
float gyro_scaling; float gyro_scaling;
float accel_scaling; float accel_scaling;
static int32_t timeval;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
@ -191,6 +194,8 @@ static void SensorsTask(void *parameters)
bool error = false; bool error = false;
while (1) { while (1) {
// TODO: add timeouts to the sensor reads and set an error if the fail // TODO: add timeouts to the sensor reads and set an error if the fail
sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (error) { if (error) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
@ -278,12 +283,16 @@ static void SensorsTask(void *parameters)
gyro_samples = 0; gyro_samples = 0;
xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
while(xQueueReceive(gyro_queue, (void *) &gyro, 0) != errQUEUE_EMPTY) { if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) {
gyro_samples++; error = true;
gyro_accum[0] += gyro.gyro_x; continue;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
} }
gyro_samples = 1;
gyro_accum[0] += gyro.gyro_x;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
gyro_scaling = PIOS_L3GD20_GetScale(); gyro_scaling = PIOS_L3GD20_GetScale();
#else #else
@ -398,6 +407,8 @@ static void SensorsTask(void *parameters)
// For L3GD20 which runs at 760 then one cycle per sample // For L3GD20 which runs at 760 then one cycle per sample
#if defined(PIOS_INCLUDE_MPU6000) && !defined(PIOS_INCLUDE_L3GD20) #if defined(PIOS_INCLUDE_MPU6000) && !defined(PIOS_INCLUDE_L3GD20)
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
#else
lastSysTime = xTaskGetTickCount();
#endif #endif
} }
} }