From 354fdf4bdfadecdf6ee1686cc7f1149f0a5556b3 Mon Sep 17 00:00:00 2001
From: Alessio Morale <alessiomorale@gmail.com>
Date: Sat, 15 Jun 2013 16:38:53 +0200
Subject: [PATCH] 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