rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Add timeout in the sensor task for reading sensors to stop the watchdog firing.

Browse Source 
All sensors should later be converted to use FreeRTOS queues.
This commit is contained in:
James Cotton 2012-01-21 08:32:41 -06:00
parent 7f284596f2
commit 6d72678086
1 changed files with 25 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -63,6 +63,7 @@
// Private constants
#define STACK_SIZE_BYTES 1540
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
#define SENSOR_PERIOD 2
#define F_PI 3.14159265358979323846f
#define PI_MOD(x) (fmod(x + F_PI, F_PI * 2) - F_PI)
@ -182,9 +183,20 @@ static void SensorsTask(void *parameters)
// Main task loop
lastSysTime = xTaskGetTickCount();
bool error = false;
while (1) {
// TODO: add timeouts to the sensor reads and set an error if the fail
if (error) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
lastSysTime = xTaskGetTickCount();
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
error = false;
} else {
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
}
int32_t read_good;
int32_t count;
@ -200,7 +212,10 @@ static void SensorsTask(void *parameters)
struct pios_bma180_data accel;
count = 0;
while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0);
while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error)
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
if (error)
continue;
while(read_good == 0) {
count++;
@ -219,7 +234,10 @@ static void SensorsTask(void *parameters)
struct pios_mpu6000_data gyro;
count = 0;
while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0);
while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0 && !error)
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
if (error)
continue;
while(read_good == 0) {
count++;
@ -247,7 +265,10 @@ static void SensorsTask(void *parameters)
#elif defined(PIOS_INCLUDE_L3GD20)
struct pios_l3gd20_data gyro;
count = 0;
while((read_good = PIOS_L3GD20_ReadFifo(&gyro)) != 0);
while((read_good = PIOS_L3GD20_ReadFifo(&gyro)) != 0 && !error)
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
if (error)
continue;
while(read_good == 0) {
count++;
@ -364,7 +385,7 @@ static void SensorsTask(void *parameters)
}
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
vTaskDelayUntil(&lastSysTime, 2 / portTICK_RATE_MS);
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
}
}