Merge branch 'amorale/OP-1302_updated_led_behaviour' into next

flight/libraries/alarms.c

@@ -254,6 +254,33 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
     xSemaphoreGiveRecursive(lock);
     return 0;
 }
+/**
+ * Get the highest alarm severity
+ * @return
+ */
+SystemAlarmsAlarmOptions AlarmsGetHighestSeverity()
+{
+    SystemAlarmsAlarmData alarmsData;
+    SystemAlarmsAlarmOptions highest = SYSTEMALARMS_ALARM_UNINITIALISED;
+
+    // Lock
+    xSemaphoreTakeRecursive(lock, portMAX_DELAY);
+
+    // Read alarms
+    SystemAlarmsAlarmGet(&alarmsData);
+
+    // Go through alarms and find the highest severity
+    uint32_t n = 0;
+    while (n < SYSTEMALARMS_ALARM_NUMELEM && highest != SYSTEMALARMS_ALARM_CRITICAL) {
+        if (cast_struct_to_array(alarmsData, alarmsData.Actuator)[n] > highest) {
+            highest = cast_struct_to_array(alarmsData, alarmsData.Actuator)[n];
+        }
+        n++;
+    }
+
+    xSemaphoreGiveRecursive(lock);
+    return highest;
+}
 
 /**
  * @}

flight/libraries/inc/alarms.h

@@ -42,9 +42,11 @@ int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm);
 void AlarmsDefaultAll();
 int32_t AlarmsClear(SystemAlarmsAlarmElem alarm);
 void AlarmsClearAll();
+
 int32_t AlarmsHasWarnings();
 int32_t AlarmsHasErrors();
 int32_t AlarmsHasCritical();
+SystemAlarmsAlarmOptions AlarmsGetHighestSeverity();
 
 #endif // ALARMS_H
 

flight/libraries/inc/notification.h

@@ -0,0 +1,38 @@
+/**
+ ******************************************************************************
+ *
+ * @file       notification.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      notification library
+ *             --
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef NOTIFICATION_H
+#define NOTIFICATION_H
+
+// period of each blink phase
+#define LED_BLINK_PERIOD_MS 200
+
+// update the status snapshot used by notifcations
+void NotificationUpdateStatus();
+
+// run the led notifications
+void NotificationOnboardLedsRun();
+
+#endif /* NOTIFICATION_H */

flight/libraries/notification.c

@@ -0,0 +1,296 @@
+/**
+ ******************************************************************************
+ *
+ * @file       notification.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      notification library.
+ *             --
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#include "inc/notification.h"
+#include <openpilot.h>
+#include <pios_struct_helper.h>
+#include <systemalarms.h>
+#include <flightstatus.h>
+#include <pios_notify.h>
+
+#ifdef PIOS_LED_ALARM
+#define ALARM_LED_ON()  PIOS_LED_On(PIOS_LED_ALARM)
+#define ALARM_LED_OFF() PIOS_LED_Off(PIOS_LED_ALARM)
+#else
+#define ALARM_LED_ON()
+#define ALARM_LED_OFF()
+#endif
+
+#ifdef PIOS_LED_HEARTBEAT
+#define HEARTBEAT_LED_ON()  PIOS_LED_On(PIOS_LED_HEARTBEAT)
+#define HEARTBEAT_LED_OFF() PIOS_LED_Off(PIOS_LED_HEARTBEAT)
+#else
+#define HEARTBEAT_LED_ON()
+#define HEARTBEAT_LED_OFF()
+#endif
+
+#define BLINK_R_ALARM_PATTERN(x) \
+    (x == SYSTEMALARMS_ALARM_OK ? 0 : \
+     x == SYSTEMALARMS_ALARM_WARNING ? 0b0000000001000000 : \
+     x == SYSTEMALARMS_ALARM_ERROR ? 0b0000001000100000 : \
+     x == SYSTEMALARMS_ALARM_CRITICAL ? 0b0111111111111110 : 0)
+#define BLINK_B_ALARM_PATTERN(x) \
+    (x == SYSTEMALARMS_ALARM_OK ? 0 : \
+     x == SYSTEMALARMS_ALARM_WARNING ? 0 : \
+     x == SYSTEMALARMS_ALARM_ERROR ? 0 : \
+     x == SYSTEMALARMS_ALARM_CRITICAL ? 0 : 0)
+
+
+#define BLINK_B_FM_ARMED_PATTERN(x) \
+    (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000100001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000010000100001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000100001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000010000100001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000100001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100010001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100010001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000100001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000100001 : 0b0000000000000001)
+
+#define BLINK_R_FM_ARMED_PATTERN(x) \
+    (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000001 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000000000 : 0b0000010000000000)
+
+#define BLINK_B_FM_DISARMED_PATTERN(x) \
+    (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000001100011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000110001100011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000001100011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000110001100011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110001100011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100110011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100110011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110001100011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110001100011 : 0b0000000000000011)
+
+#define BLINK_R_FM_DISARMED_PATTERN(x) \
+    (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000011 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110000000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110000000000 : \
+     x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110000000000 : 0b0000110000000000)
+
+#define BLINK_B_HEARTBEAT_PATTERN 0b0001111111111111
+#define BLINK_R_HEARTBEAT_PATTERN 0
+
+#define BLINK_B_NOTIFY_PATTERN(x) \
+    (x == NOTIFY_NONE ? 0 : \
+     x == NOTIFY_OK ? 0b0000100100111111 : \
+     x == NOTIFY_NOK ? 0b0000000000111111 : \
+     x == NOTIFY_DRAW_ATTENTION ? 0b0101010101010101 : 0b0101010101010101)
+
+#define BLINK_R_NOTIFY_PATTERN(x) \
+    (x == NOTIFY_NONE ? 0 : \
+     x == NOTIFY_OK ? 0b0000000000001111 : \
+     x == NOTIFY_NOK ? 0b0011000011001111 : \
+     x == NOTIFY_DRAW_ATTENTION ? 0b1010101010101010 : 0b1010101010101010)
+
+// led notification handling
+static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK;
+static volatile FlightStatusData currentFlightStatus;
+static volatile bool started = false;
+static volatile pios_notify_notification nextNotification = NOTIFY_NONE;
+
+#ifdef PIOS_LED_ALARM
+static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern);
+#endif // PIOS_LED_ALARM
+static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern);
+static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern);
+static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern);
+void NotificationUpdateStatus()
+{
+    started = true;
+    // get values to be used for led handling
+    FlightStatusGet((FlightStatusData *)&currentFlightStatus);
+    currentAlarmLevel = AlarmsGetHighestSeverity();
+    if (nextNotification == NOTIFY_NONE) {
+        nextNotification = PIOS_NOTIFY_GetActiveNotification(true);
+    }
+}
+
+void NotificationOnboardLedsRun()
+{
+    static portTickType lastRunTimestamp;
+    static uint16_t r_pattern;
+    static uint16_t b_pattern;
+    static uint8_t cycleCount; // count the number of cycles
+    static uint8_t lastFlightMode   = -1;
+    static bool forceShowFlightMode = false;
+    static pios_notify_notification runningNotification = NOTIFY_NONE;
+    static enum {
+        STATUS_NOTIFY,
+        STATUS_ALARM,
+        STATUS_FLIGHTMODE, // flightMode/HeartBeat
+        STATUS_LENGHT
+    } status;
+
+    if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 4)) {
+        return;
+    }
+
+    lastRunTimestamp = xTaskGetTickCount();
+    // the led will show various status information, subdivided in three phases
+    // - Notification
+    // - Alarm
+    // - Flight status
+    // they are shown using the above priority
+    // a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end
+
+    cycleCount++;
+    // Notifications are "modal" to other statuses so they takes precedence
+    if (status != STATUS_NOTIFY && nextNotification != NOTIFY_NONE) {
+        // read next notification to show
+        runningNotification = nextNotification;
+        nextNotification    = NOTIFY_NONE;
+        // Force a notification status
+        status     = STATUS_NOTIFY;
+        cycleCount = 0; // instantly start a notify cycle
+    } else {
+        if (lastFlightMode != currentFlightStatus.FlightMode) {
+            status         = STATUS_FLIGHTMODE;
+            lastFlightMode = currentFlightStatus.FlightMode;
+            cycleCount     = 0; // instantly start a flightMode cycle
+            forceShowFlightMode = true;
+        }
+    }
+
+    // check if a phase has just finished
+    if (cycleCount & 0x10) {
+        HEARTBEAT_LED_OFF();
+        ALARM_LED_OFF();
+        cycleCount = 0x0;
+        forceShowFlightMode = false;
+        // Notification has been just shown, cleanup
+        if (status == STATUS_NOTIFY) {
+            runningNotification = NOTIFY_NONE;
+        }
+        status = (status + 1) % STATUS_LENGHT;
+    }
+
+    if (status == STATUS_NOTIFY) {
+        if (!cycleCount && !handleNotifications(runningNotification, &r_pattern, &b_pattern)) {
+            // no notifications, advance
+            status++;
+        }
+    }
+
+    // Handles Alarm display
+    if (status == STATUS_ALARM) {
+#ifdef PIOS_LED_ALARM
+        if (!cycleCount && !handleAlarms(&r_pattern, &b_pattern)) {
+            // no alarms, advance
+            status++;
+        }
+#else
+        // no alarms leds, advance
+        status++;
+#endif // PIOS_LED_ALARM
+    }
+
+    // **** Handles flightmode display
+    if (status == STATUS_FLIGHTMODE && !cycleCount) {
+        if (forceShowFlightMode || currentFlightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
+            handleFlightMode(&r_pattern, &b_pattern);
+        } else {
+            handleHeartbeat(&r_pattern, &b_pattern);
+        }
+    }
+
+    // led output
+    if (b_pattern & 0x1) {
+        HEARTBEAT_LED_ON();
+    } else {
+        HEARTBEAT_LED_OFF();
+    }
+    if (r_pattern & 0x1) {
+        ALARM_LED_ON();
+    } else {
+        ALARM_LED_OFF();
+    }
+    r_pattern >>= 1;
+    b_pattern >>= 1;
+}
+
+#if defined(PIOS_LED_ALARM)
+static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern)
+{
+    if (currentAlarmLevel == SYSTEMALARMS_ALARM_OK) {
+        return false;
+    }
+    *b_pattern = BLINK_B_ALARM_PATTERN(currentAlarmLevel);
+    *r_pattern = BLINK_R_ALARM_PATTERN(currentAlarmLevel);
+    return true;
+}
+#endif /* PIOS_LED_ALARM */
+
+
+static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern)
+{
+    if (runningNotification == NOTIFY_NONE) {
+        return false;
+    }
+    *b_pattern = BLINK_B_NOTIFY_PATTERN(runningNotification);
+    *r_pattern = BLINK_R_NOTIFY_PATTERN(runningNotification);
+    return true;
+}
+
+static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern)
+{
+    // Flash the heartbeat LED
+    uint8_t flightmode = currentFlightStatus.FlightMode;
+
+    if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
+        *b_pattern = BLINK_B_FM_DISARMED_PATTERN(flightmode);
+        *r_pattern = BLINK_R_FM_DISARMED_PATTERN(flightmode);
+    } else {
+        *b_pattern = BLINK_B_FM_ARMED_PATTERN(flightmode);
+        *r_pattern = BLINK_R_FM_ARMED_PATTERN(flightmode);
+    }
+}
+
+static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern)
+{
+    // Flash the heartbeat LED
+    *b_pattern = BLINK_B_HEARTBEAT_PATTERN;
+    *r_pattern = BLINK_R_HEARTBEAT_PATTERN;
+}

flight/modules/Attitude/attitude.c

@@ -63,7 +63,7 @@
 #include "taskinfo.h"
 
 #include "CoordinateConversions.h"
-
+#include <pios_notify.h>
 
 // Private constants
 #define STACK_SIZE_BYTES 540
@@ -252,6 +252,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
             rollPitchBiasRate    = 0.01f;
             accel_filter_enabled = false;
             init = 0;
+            PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
         } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
             accelKp     = 1.0f;
             accelKi     = 0.0f;
@@ -259,6 +260,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
             rollPitchBiasRate    = 0.01f;
             accel_filter_enabled = false;
             init = 0;
+            PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
         } else if (init == 0) {
             // Reload settings (all the rates)
             AttitudeSettingsAccelKiGet(&accelKi);

flight/modules/StateEstimation/filtercf.c

@@ -42,7 +42,7 @@
 #include <revocalibration.h>
 
 #include <CoordinateConversions.h>
-
+#include <pios_notify.h>
 // Private constants
 
 #define STACK_REQUIRED          512
@@ -295,6 +295,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
         this->accel_filter_enabled   = false;
         this->rollPitchBiasRate      = 0.01f;
         this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
+        PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
     } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
         this->attitudeSettings.AccelKp     = 1.0f;
         this->attitudeSettings.AccelKi     = 0.0f;
@@ -303,6 +304,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
         this->rollPitchBiasRate      = 0.01f;
         this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
         this->init = 0;
+        PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
     } else if (this->init == 0) {
         // Reload settings (all the rates)
         AttitudeSettingsGet(&this->attitudeSettings);

flight/modules/System/systemmod.c

@@ -42,7 +42,7 @@
 #include <pios_struct_helper.h>
 // private includes
 #include "inc/systemmod.h"
-
+#include "notification.h"
 
 // UAVOs
 #include <objectpersistence.h>
@@ -72,22 +72,15 @@
 #endif
 
 // Private constants
-#define SYSTEM_UPDATE_PERIOD_MS        1000
-#define LED_BLINK_RATE_HZ              5
-
-#ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD
-#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c
-// must be updated if the FreeRTOS or compiler
-// optimisation options are changed.
-#endif
+#define SYSTEM_UPDATE_PERIOD_MS 250
 
 #if defined(PIOS_SYSTEM_STACK_SIZE)
-#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE
+#define STACK_SIZE_BYTES        PIOS_SYSTEM_STACK_SIZE
 #else
-#define STACK_SIZE_BYTES 1024
+#define STACK_SIZE_BYTES        1024
 #endif
 
-#define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)
+#define TASK_PRIORITY           (tskIDLE_PRIORITY + 1)
 
 // Private types
 
@@ -98,6 +91,7 @@ static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_C
 static bool mallocFailed;
 static HwSettingsData bootHwSettings;
 static struct PIOS_FLASHFS_Stats fsStats;
+
 // Private functions
 static void objectUpdatedCb(UAVObjEvent *ev);
 static void hwSettingsUpdatedCb(UAVObjEvent *ev);
@@ -170,8 +164,6 @@ MODULE_INITCALL(SystemModInitialize, 0);
  */
 static void systemTask(__attribute__((unused)) void *parameters)
 {
-    uint8_t cycleCount = 0;
-
     /* create all modules thread */
     MODULE_TASKCREATE_ALL;
 
@@ -189,9 +181,6 @@ static void systemTask(__attribute__((unused)) void *parameters)
     /* Record a successful boot */
     PIOS_IAP_WriteBootCount(0);
 #endif
-
-    // Initialize vars
-
     // Listen for SettingPersistance object updates, connect a callback function
     ObjectPersistenceConnectQueue(objectPersistenceQueue);
 
@@ -204,13 +193,10 @@ static void systemTask(__attribute__((unused)) void *parameters)
     TaskInfoData taskInfoData;
     CallbackInfoData callbackInfoData;
 #endif
-
     // Main system loop
     while (1) {
+        NotificationUpdateStatus();
         // Update the system statistics
-
-        cycleCount = cycleCount > 0 ? cycleCount - 1 : 7;
-// if(cycleCount == 1){
         updateStats();
         // Update the system alarms
         updateSystemAlarms();
@@ -230,35 +216,10 @@ static void systemTask(__attribute__((unused)) void *parameters)
 // }
 #endif
 // }
-        // Flash the heartbeat LED
-#if defined(PIOS_LED_HEARTBEAT)
-        uint8_t armingStatus;
-        FlightStatusArmedGet(&armingStatus);
-        if ((armingStatus == FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x1)) ||
-            (armingStatus != FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x4))) {
-            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 (AlarmsHasCritical()) {
-            PIOS_LED_On(PIOS_LED_ALARM);
-        } else if ((AlarmsHasErrors() && (cycleCount & 0x1)) ||
-                   (!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x4))) {
-            PIOS_LED_On(PIOS_LED_ALARM);
-        } else {
-            PIOS_LED_Off(PIOS_LED_ALARM);
-        }
-#endif /* PIOS_LED_ALARM */
 
 
         UAVObjEvent ev;
-        int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2);
+        int delayTime = SYSTEM_UPDATE_PERIOD_MS;
 
 #if defined(PIOS_INCLUDE_RFM22B)
 
@@ -649,11 +610,12 @@ static void updateSystemAlarms()
 }
 
 /**
- * Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
+ * Called by the RTOS when the CPU is idle,
  */
 void vApplicationIdleHook(void)
-{}
-
+{
+    NotificationOnboardLedsRun();
+}
 /**
  * Called by the RTOS when a stack overflow is detected.
  */

flight/modules/Telemetry/telemetry.c

@@ -661,11 +661,10 @@ static void updateTelemetryStats()
         flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
     }
 
-    // Update the telemetry alarm
+    // TODO: check whether is there any error condition worth raising an alarm
+    // Disconnection is actually a normal (non)working status so it is not raising alarms anymore.
     if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
         AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
-    } else {
-        AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
     }
 
     // Update object

flight/pios/common/pios_notify.c

@@ -0,0 +1,49 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_notify.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Handles user notifications.
+ *             --
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "pios_notify.h"
+
+static volatile pios_notify_notification currentNotification = NOTIFY_NONE;
+static volatile pios_notify_priority currentPriority;
+
+
+void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority)
+{
+    if (currentNotification == NOTIFY_NONE || currentPriority < priority) {
+        currentPriority     = priority;
+        currentNotification = notification;
+    }
+}
+
+pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear)
+{
+    pios_notify_notification ret = currentNotification;
+
+    if (clear && ret != NOTIFY_NONE) {
+        currentNotification = NOTIFY_NONE;
+    }
+    return ret;
+}

flight/pios/inc/pios_notify.h

@@ -0,0 +1,58 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_notify.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      Handles user notifications.
+ *             --
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef PIOS_NOTIFY_H_
+#define PIOS_NOTIFY_H_
+#include <stdbool.h>
+
+typedef enum {
+    NOTIFY_NONE = 0,
+    NOTIFY_OK,
+    NOTIFY_NOK,
+    NOTIFY_DRAW_ATTENTION
+} pios_notify_notification;
+
+typedef enum {
+    NOTIFY_PRIORITY_CRITICAL = 2,
+    NOTIFY_PRIORITY_REGULAR  = 1,
+    NOTIFY_PRIORITY_LOW = 0,
+} pios_notify_priority;
+
+/**
+ * start a new notification. If a notification is active it will be overwritten if its priority is lower than the new one.
+ * The new will be discarded otherwise
+ * @param notification kind of notification
+ * @param priority priority of the new notification
+ */
+void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority);
+
+/**
+ * retrieve any active notification
+ * @param clear
+ * @return
+ */
+pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear);
+
+#endif /* PIOS_NOTIFY_H_ */

flight/targets/boards/coptercontrol/firmware/Makefile

@@ -48,9 +48,13 @@ OPTMODULES += Osd/osdoutput
 #OPTMODULES += Altitude
 #OPTMODULES += Fault
 
+SRC += $(FLIGHTLIB)/notification.c
+
 # Include all camera options
 CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF
 
+
+
 # Erase flash firmware should be buildable from command line
 ifeq ($(ERASE_FLASH), YES)
     CDEFS += -DERASE_FLASH

flight/targets/boards/discoveryf4bare/firmware/Makefile

@@ -52,6 +52,8 @@ MODULES += Telemetry
 
 OPTMODULES += ComUsbBridge
 
+SRC += $(FLIGHTLIB)/notification.c
+
 # Include all camera options
 CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF
 

flight/targets/boards/osd/firmware/Makefile

@@ -37,6 +37,8 @@ MODULES += Telemetry
 
 OPTMODULES =
 
+SRC += $(FLIGHTLIB)/notification.c
+
 # Some diagnostics
 CDEFS += -DDIAG_TASKS
 

flight/targets/boards/revolution/firmware/Makefile

@@ -53,6 +53,8 @@ MODULES += Telemetry
 
 OPTMODULES += ComUsbBridge
 
+SRC += $(FLIGHTLIB)/notification.c
+
 # Include all camera options
 CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF
 
@@ -96,7 +98,9 @@ endif
 
 # Optional component libraries
 include $(FLIGHTLIB)/rscode/library.mk
+
 #include $(FLIGHTLIB)/PyMite/pymite.mk
 
+
 include $(ROOT_DIR)/make/apps-defs.mk
 include $(ROOT_DIR)/make/common-defs.mk

flight/targets/boards/revoproto/firmware/Makefile

@@ -50,6 +50,8 @@ MODULES += Osd/osdoutout
 MODULES += Logging
 MODULES += Telemetry
 
+SRC += $(FLIGHTLIB)/notification.c
+
 OPTMODULES += ComUsbBridge
 
 # Include all camera options

flight/targets/boards/simposix/firmware/Makefile

@@ -45,6 +45,8 @@ MODULES += Airspeed
 #MODULES += AltitudeHold # now integrated in Stabilization
 #MODULES += OveroSync
 
+SRC += $(FLIGHTLIB)/notification.c
+
 # Paths
 OPSYSTEM = .
 BOARDINC = ..
@@ -103,6 +105,8 @@ SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c
 SRC += $(PIOSCORECOMMON)/pios_debuglog.c
 SRC += $(PIOSCORECOMMON)/pios_callbackscheduler.c
 SRC += $(PIOSCORECOMMON)/pios_deltatime.c
+SRC += $(PIOSCORECOMMON)/pios_notify.c
+
 
 ## PIOS Hardware
 include $(PIOS)/posix/library.mk

make/apps-defs.mk

@@ -99,7 +99,7 @@ SRC += $(PIOSCOMMON)/pios_usb_util.c
 ## PIOS system code
 SRC += $(PIOSCOMMON)/pios_task_monitor.c
 SRC += $(PIOSCOMMON)/pios_callbackscheduler.c
-
+SRC += $(PIOSCOMMON)/pios_notify.c
 ## Misc library functions
 SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/sanitycheck.c