From 49da1aca865a7ce4a1a1015d882b4f5612e0e687 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 15 Jun 2013 16:22:24 +0200 Subject: [PATCH 1/7] OP-1009 Redo gyro zero in Complementary after calibration parameters changes --- flight/modules/Attitude/revolution/attitude.c | 32 +++++++++++++++---- 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 452ba4ba2..ac4317466 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -75,10 +75,12 @@ #include "CoordinateConversions.h" // Private constants -#define STACK_SIZE_BYTES 2048 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -#define FAILSAFE_TIMEOUT_MS 10 +#define STACK_SIZE_BYTES 2048 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define FAILSAFE_TIMEOUT_MS 10 +#define CALIBRATION_DELAY 4000 +#define CALIBRATION_DURATION 6000 // low pass filter configuration to calculate offset // of barometric altitude sensor // reasoning: updates at: 10 Hz, tau= 300 s settle time @@ -319,6 +321,7 @@ static int32_t updateAttitudeComplementary(bool first_run) float dT; static uint8_t init = 0; static bool magCalibrated = true; + static uint32_t initStartupTime = 0; // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || @@ -389,10 +392,26 @@ static int32_t updateAttitudeComplementary(bool first_run) timeval = PIOS_DELAY_GetRaw(); + // wait calibration_delay only at powerup + if (xTaskGetTickCount() < 3000) { + initStartupTime = 0; + } else { + initStartupTime = xTaskGetTickCount() - CALIBRATION_DELAY; + } + + // Zero gyro bias + // This is really needed after updating calibration settings. + GyrosBiasData zeroGyrosBias; + GyrosBiasGet(&zeroGyrosBias); + zeroGyrosBias.x = 0; + zeroGyrosBias.y = 0; + zeroGyrosBias.z = 0; + GyrosBiasSet(&zeroGyrosBias); return 0; } - if ((xTaskGetTickCount() < 10000) && (xTaskGetTickCount() > 4000)) { + if ((xTaskGetTickCount() - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY) && + (xTaskGetTickCount() - initStartupTime > CALIBRATION_DELAY)) { // For first 7 seconds use accels to get gyro bias attitudeSettings.AccelKp = 1.0f; attitudeSettings.AccelKi = 0.0f; @@ -619,8 +638,9 @@ static int32_t updateAttitudeComplementary(bool first_run) } } - - if (variance_error) { + if (!init) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + } else if (variance_error) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); From 354fdf4bdfadecdf6ee1686cc7f1149f0a5556b3 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 15 Jun 2013 16:38:53 +0200 Subject: [PATCH 2/7] OP-1012 implement several messages for alarm led --- flight/modules/System/systemmod.c | 32 +++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index b218876be..c2f8b660a 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -167,7 +167,7 @@ static void systemTask(__attribute__((unused)) void *parameters) { /* start the delayed callback scheduler */ CallbackSchedulerStart(); - + static uint8_t cycleCount; /* create all modules thread */ MODULE_TASKCREATE_ALL; @@ -204,6 +204,7 @@ static void systemTask(__attribute__((unused)) void *parameters) while (1) { // Update the system statistics updateStats(); + cycleCount = cycleCount > 0 ? cycleCount - 1 : 3; // Update the system alarms updateSystemAlarms(); @@ -220,26 +221,33 @@ static void systemTask(__attribute__((unused)) void *parameters) // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); + uint8_t armingStatus; + FlightStatusArmedGet(&armingStatus); + if ((armingStatus == FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x1)) || + (armingStatus != FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x2))) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set #if defined(PIOS_LED_ALARM) - if (AlarmsHasWarnings()) { + if (AlarmsHasCritical()) { + PIOS_LED_On(PIOS_LED_ALARM); + } else if( (AlarmsHasErrors() && (cycleCount & 0x1)) || + (!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x2))){ PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); } #endif /* PIOS_LED_ALARM */ - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); UAVObjEvent ev; - int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? - SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) : - SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS; + int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ); if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { // If object persistence is updated call the callback @@ -468,14 +476,14 @@ static void updateStats() idleCounter = 0; } #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) - if(pios_uavo_settings_fs_id){ + if (pios_uavo_settings_fs_id) { PIOS_FLASHFS_GetStats(pios_uavo_settings_fs_id, &fsStats); - stats.SysSlotsFree = fsStats.num_free_slots; + stats.SysSlotsFree = fsStats.num_free_slots; stats.SysSlotsActive = fsStats.num_active_slots; } - if(pios_user_fs_id){ + if (pios_user_fs_id) { PIOS_FLASHFS_GetStats(pios_user_fs_id, &fsStats); - stats.UsrSlotsFree = fsStats.num_free_slots; + stats.UsrSlotsFree = fsStats.num_free_slots; stats.UsrSlotsActive = fsStats.num_active_slots; } #endif From f2431b2868feae569d8a9e910ab3f10da0b538c8 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 15 Jun 2013 16:39:29 +0200 Subject: [PATCH 3/7] GPS module is builtin so there is no way to shut the gps alarm when gps is not used. This change does remove any alarm when no port is configured for gps. Things depending on GPS will continue to raise their own alarm when no gps data are feed. --- flight/modules/GPS/GPS.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 4158a4b3b..caa61260f 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -112,8 +112,6 @@ int32_t GPSStart(void) PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle); return 0; } - - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); } return -1; } From 517c331dd4dcb68c74d458ee4eaecc175b00e907 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 15 Jun 2013 17:27:48 +0200 Subject: [PATCH 4/7] OP-1012 fixed speeds to make easy to distinguish flash rates --- flight/modules/System/systemmod.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index c2f8b660a..4873a7877 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -204,7 +204,7 @@ static void systemTask(__attribute__((unused)) void *parameters) while (1) { // Update the system statistics updateStats(); - cycleCount = cycleCount > 0 ? cycleCount - 1 : 3; + cycleCount = cycleCount > 0 ? cycleCount - 1 : 7; // Update the system alarms updateSystemAlarms(); @@ -224,7 +224,7 @@ static void systemTask(__attribute__((unused)) void *parameters) uint8_t armingStatus; FlightStatusArmedGet(&armingStatus); if ((armingStatus == FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x1)) || - (armingStatus != FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x2))) { + (armingStatus != FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x4))) { PIOS_LED_On(PIOS_LED_HEARTBEAT); } else { PIOS_LED_Off(PIOS_LED_HEARTBEAT); @@ -238,7 +238,7 @@ static void systemTask(__attribute__((unused)) void *parameters) if (AlarmsHasCritical()) { PIOS_LED_On(PIOS_LED_ALARM); } else if( (AlarmsHasErrors() && (cycleCount & 0x1)) || - (!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x2))){ + (!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x4))){ PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); @@ -247,7 +247,7 @@ static void systemTask(__attribute__((unused)) void *parameters) UAVObjEvent ev; - int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ); + int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2); if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { // If object persistence is updated call the callback From d5a77fb5258295c3319954340ff53cc7e2765dbe Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 15 Jun 2013 17:28:30 +0200 Subject: [PATCH 5/7] OP-1009 prevent gyro zeroing from raise an alarm when arming --- flight/modules/Attitude/revolution/attitude.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index ac4317466..f608ce442 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -638,7 +638,7 @@ static int32_t updateAttitudeComplementary(bool first_run) } } - if (!init) { + if (!init && flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); } else if (variance_error) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); From b012d55f268b3697a3cd526ee3ba7ec20cd553ee Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 17 Jun 2013 21:01:24 +0200 Subject: [PATCH 6/7] OP-1009 fix comments and fixes review +review OPReview-516 --- flight/modules/Attitude/revolution/attitude.c | 3 ++- flight/modules/System/systemmod.c | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index f608ce442..c955e25ed 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -412,7 +412,8 @@ static int32_t updateAttitudeComplementary(bool first_run) if ((xTaskGetTickCount() - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY) && (xTaskGetTickCount() - initStartupTime > CALIBRATION_DELAY)) { - // For first 7 seconds use accels to get gyro bias + // For first CALIBRATION_DURATION seconds after CALIBRATION_DELAY from startup + // Zero gyro bias assuming it is steady, smoothing the gyro input value applying rollPitchBiasRate. attitudeSettings.AccelKp = 1.0f; attitudeSettings.AccelKi = 0.0f; attitudeSettings.YawBiasRate = 0.23f; diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 4873a7877..42664128d 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -167,7 +167,7 @@ static void systemTask(__attribute__((unused)) void *parameters) { /* start the delayed callback scheduler */ CallbackSchedulerStart(); - static uint8_t cycleCount; + uint8_t cycleCount; /* create all modules thread */ MODULE_TASKCREATE_ALL; @@ -237,8 +237,8 @@ static void systemTask(__attribute__((unused)) void *parameters) #if defined(PIOS_LED_ALARM) if (AlarmsHasCritical()) { PIOS_LED_On(PIOS_LED_ALARM); - } else if( (AlarmsHasErrors() && (cycleCount & 0x1)) || - (!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x4))){ + } else if ((AlarmsHasErrors() && (cycleCount & 0x1)) || + (!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x4))) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); From 54af41765e6b8d960666ce4c0b4f62849b82df23 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 17 Jun 2013 21:41:54 +0200 Subject: [PATCH 7/7] OP-1009 missing initialization on a previously static variable. +review OPReview-516 --- flight/modules/System/systemmod.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 42664128d..9ff5dd7e0 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -167,7 +167,7 @@ static void systemTask(__attribute__((unused)) void *parameters) { /* start the delayed callback scheduler */ CallbackSchedulerStart(); - uint8_t cycleCount; + uint8_t cycleCount = 0; /* create all modules thread */ MODULE_TASKCREATE_ALL;