From e39c3f897e9129855c47e62cdfc7da503d36bc73 Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Mon, 26 Aug 2013 19:14:47 +0200 Subject: [PATCH] Use xTaskGetTickCount() to time long intervals rather than PIOS_DELAY_GetRaw(), because of wrap-around issues. --- flight/modules/StateEstimation/filtercf.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 224cf1953..3c3ba136f 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -47,6 +47,9 @@ #define STACK_REQUIRED 512 +#define CALIBRATION_DELAY_MS 4000 +#define CALIBRATION_DURATION_MS 6000 + // Private types struct data { AttitudeSettingsData attitudeSettings; @@ -274,18 +277,18 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel this->grot_filtered[0] = 0.0f; this->grot_filtered[1] = 0.0f; this->grot_filtered[2] = 0.0f; - this->timeval = PIOS_DELAY_GetRaw(); - this->starttime = this->timeval; + this->timeval = PIOS_DELAY_GetRaw(); // Cycle counter used for precise timing + this->starttime = xTaskGetTickCount(); // Tick counter used for long time intervals return 0; // must return zero on initial initialization, so attitude will init with a valid quaternion } - if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 4000000) { + if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) { // wait 4 seconds for the user to get his hands off in case the board was just powered this->timeval = PIOS_DELAY_GetRaw(); return 1; - } else if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 10000000) { - // For first 7 seconds use accels to get gyro bias + } else if (this->init == 0 && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) { + // For first 6 seconds use accels to get gyro bias this->attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKi = 0.0f; this->attitudeSettings.YawBiasRate = 0.23f;