OP-1379 - Rebuild and simplify the queuing mechanism

flight/libraries/inc/optypes.h

@@ -33,6 +33,7 @@ typedef struct {
 } Color_t;
 
 extern const Color_t Color_Off;
+extern const Color_t Color_Black;
 extern const Color_t Color_Red;
 extern const Color_t Color_Lime;
 extern const Color_t Color_Blue;
@@ -46,7 +47,8 @@ extern const Color_t Color_Teal;
 extern const Color_t Color_Orange;
 extern const Color_t Color_White;
 
-#define COLOR_OFF     { .R = 0x00, .G = 0x00, .B = 0x00 }
+#define COLOR_BLACK   { .R = 0x00, .G = 0x00, .B = 0x00 }
+#define COLOR_OFF     COLOR_BLACK
 #define COLOR_RED     { .R = 0xFF, .G = 0x00, .B = 0x00 }
 #define COLOR_LIME    { .R = 0x00, .G = 0xFF, .B = 0x00 }
 #define COLOR_BLUE    { .R = 0x00, .G = 0x00, .B = 0xFF }

flight/libraries/lednotification.c

@@ -35,19 +35,18 @@
 // Private defines
 
 // Maximum number of notifications enqueued when a higher priority notification is added
-#define MAX_BACKGROUND_NOTIFICATIONS 5
+#define MAX_BACKGROUND_NOTIFICATIONS 6
 #define MAX_HANDLED_LED              1
 
-#define BACKGROUND_SEQUENCE          -1
+#define BACKGROUND_SEQUENCE          0
 #define RESET_STEP                   -1
 #define GET_CURRENT_MILLIS           (xTaskGetTickCount() * portTICK_RATE_MS)
 
 // Private data types definition
 // this is the status for a single notification led set
 typedef struct {
-    int8_t   queued_priorities[MAX_BACKGROUND_NOTIFICATIONS];
+    int8_t   queued_priorities[MAX_BACKGROUND_NOTIFICATIONS]; // slot 0 is reserved for background
-    LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS];
+    LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS]; // slot 0 is reserved for background
-    LedSequence_t background_sequence;
     uint32_t next_run_time;
     uint32_t sequence_starting_time;
 
@@ -100,74 +99,62 @@ static void restart_sequence(NotifierLedStatus_t *status, bool immediate)
  */
 static void push_queued_sequence(ExtLedNotification_t *new_notification, NotifierLedStatus_t *status)
 {
-    int8_t updated_sequence = BACKGROUND_SEQUENCE;
+    int8_t updated_sequence;
+
+    int8_t lowest_priority_index = -1;
+    int8_t lowest_priority = new_notification->priority;
 
     if (new_notification->priority == NOTIFY_PRIORITY_BACKGROUND) {
-        status->background_sequence = new_notification->sequence;
+        lowest_priority_index = BACKGROUND_SEQUENCE;
     } else {
-        // a notification with priority higher than background.
+        // slot 0 is reserved for Background sequence
-        // try to enqueue it
+        for (int8_t i = 1; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
-        int8_t insert_point = MAX_BACKGROUND_NOTIFICATIONS - 1;
+            if (status->queued_priorities[i] < lowest_priority) {
-        int8_t first_free   = -1;
+                lowest_priority_index = i;
-        for (int8_t i = MAX_BACKGROUND_NOTIFICATIONS - 1; i >= 0; i--) {
+                lowest_priority = status->queued_priorities[i];
-            const int8_t priority_i = status->queued_priorities[i];
-            if (priority_i == NOTIFY_PRIORITY_BACKGROUND) {
-                first_free   = i;
-                insert_point = i;
-                continue;
-            }
-            if (priority_i > new_notification->priority) {
-                insert_point = ((i > 0) || (first_free > -1)) ? i : -1; // check whether priority is no bigger than lowest queued priority
             }
         }
-
-        // no space left on queue for this new notification, ignore.
-        if (insert_point < 0) {
-            return;
-        }
-        if (insert_point != first_free) {
-            // there is a free slot, move everything up one place
-            if (first_free != -1) {
-                for (uint8_t i = MAX_BACKGROUND_NOTIFICATIONS - 1; i > insert_point; i--) {
-                    status->queued_priorities[i] = status->queued_priorities[i - 1];
-                    status->queued_sequences[i]  = status->queued_sequences[i - 1];
-                }
-                if (status->active_sequence_num >= insert_point) {
-                    status->active_sequence_num++;
-                }
-            } else {
-                // no free space, discard lowest priority notification and move everything down
-                for (uint8_t i = 0; i < insert_point; i++) {
-                    status->queued_priorities[i] = status->queued_priorities[i + 1];
-                    status->queued_sequences[i]  = status->queued_sequences[i + 1];
-                }
-                if (status->active_sequence_num <= insert_point) {
-                    status->active_sequence_num--;
-                }
-            }
-        }
-
-        status->queued_priorities[insert_point] = new_notification->priority;
-        status->queued_sequences[insert_point]  = new_notification->sequence;
-        updated_sequence = insert_point;
     }
 
-    if (status->active_sequence_num < updated_sequence) {
+    // no items with priority lower than the one we are trying to enqueue. skip
+    if (lowest_priority_index < 0) {
+        return;
+    }
+
+    status->queued_priorities[lowest_priority_index] = new_notification->priority;
+    status->queued_sequences[lowest_priority_index]  = new_notification->sequence;
+    updated_sequence = lowest_priority_index;;
+
+
+    // check whether we should preempt the current notification and play this new one
+    if (status->queued_priorities[status->active_sequence_num] < new_notification->priority) {
         status->active_sequence_num = updated_sequence;
-        restart_sequence(status, true);
     }
-    if (updated_sequence == BACKGROUND_SEQUENCE) {
+
-        restart_sequence(status, false);
+    if (status->active_sequence_num == updated_sequence) {
+        restart_sequence(status, true);
     }
 }
 
 static bool pop_queued_sequence(NotifierLedStatus_t *status)
 {
-    if (status->active_sequence_num != BACKGROUND_SEQUENCE) {
+    if (status->active_sequence_num > BACKGROUND_SEQUENCE) {
-        // start the lower priority item
+        // set the last active slot as empty
         status->queued_priorities[status->active_sequence_num] = NOTIFY_PRIORITY_BACKGROUND;
-        status->active_sequence_num--;
+
-        return true;
+        // search the highest priority item
+        int8_t highest_priority_index = BACKGROUND_SEQUENCE;
+        int8_t highest_priority = NOTIFY_PRIORITY_BACKGROUND;
+
+        for (int8_t i = 1; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
+            if (status->queued_priorities[i] > highest_priority) {
+                highest_priority_index = i;
+                highest_priority = status->queued_priorities[i];
+            }
+        }
+        // set the next sequence to activate (or BACKGROUND_SEQUENCE when all slots are empty)
+        status->active_sequence_num = highest_priority_index;
+        return highest_priority_index != BACKGROUND_SEQUENCE;
     }
     // background sequence was completed
     return false;
@@ -178,9 +165,7 @@ static bool pop_queued_sequence(NotifierLedStatus_t *status)
  */
 static void advance_sequence(NotifierLedStatus_t *status)
 {
-    LedSequence_t *activeSequence =
+    LedSequence_t *activeSequence = &status->queued_sequences[status->active_sequence_num];
-        status->active_sequence_num == BACKGROUND_SEQUENCE ?
-        &status->background_sequence : &status->queued_sequences[status->active_sequence_num];
 
     uint8_t step = status->next_sequence_step;
     LedStep_t *currentStep = &activeSequence->steps[step];
@@ -244,8 +229,7 @@ static void run_led(NotifierLedStatus_t *status)
     status->next_run_time = currentTime;
     uint8_t step = status->next_sequence_step;
 
-    LedSequence_t *activeSequence = status->active_sequence_num == BACKGROUND_SEQUENCE ?
+    LedSequence_t *activeSequence = &status->queued_sequences[status->active_sequence_num];
-                                    &status->background_sequence : &status->queued_sequences[status->active_sequence_num];
     const Color_t color = status->step_phase_on ? activeSequence->steps[step].color : Color_Off;
 
     for (uint8_t i = status->led_set_start; i <= status->led_set_end; i++) {

flight/libraries/optypes.c

@@ -27,6 +27,7 @@
 #include <optypes.h>
 
 const Color_t Color_Off     = COLOR_OFF;
+const Color_t Color_Black   = COLOR_BLACK;
 const Color_t Color_Red     = COLOR_RED;
 const Color_t Color_Lime    = COLOR_LIME;
 const Color_t Color_Blue    = COLOR_BLUE;

flight/modules/Notify/notify.c

@@ -24,24 +24,6 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-/**
- * Input objects: ExampleObject1, ExampleSettings
- * Output object: ExampleObject2
- *
- * This module executes in response to ExampleObject1 updates. When the
- * module is triggered it will update the data of ExampleObject2.
- *
- * No threads are used in this example.
- *
- * UAVObjects are automatically generated by the UAVObjectGenerator from
- * the object definition XML file.
- *
- * Modules have no API, all communication to other modules is done through UAVObjects.
- * However modules may use the API exposed by shared libraries.
- * See the OpenPilot wiki for more details.
- * http://www.openpilot.org/OpenPilot_Application_Architecture
- *
- */
 #include "openpilot.h"
 #include <flightstatus.h>
 #include <systemalarms.h>

flight/tests/lednotification/unittest.cpp

@@ -16,6 +16,32 @@ void PIOS_WS2811_Update() {}
 
 class LedNotificationTest : public testing::Test {};
 
+int compare_int8(const void *a, const void *b)
+{
+    const int8_t *da = (const int8_t *)a;
+    const int8_t *db = (const int8_t *)b;
+
+    return (*da > *db) - (*da < *db);
+}
+
+
+void sort_priorities(int8_t *queued_priorities)
+{
+    qsort(queued_priorities, MAX_BACKGROUND_NOTIFICATIONS, sizeof(int8_t), compare_int8);
+}
+
+void set_highest_active(NotifierLedStatus_t *status)
+{
+    int8_t highest = NOTIFY_PRIORITY_BACKGROUND;
+
+    for (int i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
+        if (status->queued_priorities[i] > highest) {
+            status->active_sequence_num = i;
+            highest = status->queued_priorities[i];
+        }
+    }
+}
+
 void insert(NotifierLedStatus_t *status, pios_notify_priority priority)
 {
     ExtLedNotification_t notification;
@@ -41,12 +67,18 @@ TEST_F(LedNotificationTest, TestQueueOrder1) {
     insert(&status, NOTIFY_PRIORITY_LOW);
     insert(&status, NOTIFY_PRIORITY_CRITICAL);
 
+    set_highest_active(&status);
 
-    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[0]);
+
-    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[1]);
+    EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, (status.queued_priorities[status.active_sequence_num]));
-    EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, status.queued_priorities[2]);
+    pop_queued_sequence(&status);
-    EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, status.queued_priorities[3]);
+    EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, (status.queued_priorities[status.active_sequence_num]));
-    EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, status.queued_priorities[4]);
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, (status.queued_priorities[status.active_sequence_num]));
 }
 
 TEST_F(LedNotificationTest, TestQueueOrder2) {
@@ -58,26 +90,40 @@ TEST_F(LedNotificationTest, TestQueueOrder2) {
 // 148            updated_sequence = insert_point;
 
     init(&status, NOTIFY_PRIORITY_LOW);
+    status.queued_priorities[0] = NOTIFY_PRIORITY_BACKGROUND;
 
     insert(&status, NOTIFY_PRIORITY_REGULAR);
 
-    EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[4]);
+    set_highest_active(&status);
-    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[3]);
+
-    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[2]);
+    EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, (status.queued_priorities[status.active_sequence_num]));
-    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[1]);
+    pop_queued_sequence(&status);
-    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[0]);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, (status.queued_priorities[status.active_sequence_num]));
 }
 
 TEST_F(LedNotificationTest, TestQueueOrder3) {
     NotifierLedStatus_t status;
 
-    // Fails because queued_priorities[0] _LOW and not _REGULAR. I _think_ this is a bug.
+    // Fails because queued_priorities[0] _LOW and not _REGULAR. I _thibnk_ this is a bug.
     init(&status, NOTIFY_PRIORITY_REGULAR);
+    status.queued_priorities[0] = NOTIFY_PRIORITY_BACKGROUND;
 
     insert(&status, NOTIFY_PRIORITY_LOW);
 
-    for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
+    set_highest_active(&status);
-        EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[i]);
+
+
+    for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS - 1; i++) {
+        EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, (status.queued_priorities[status.active_sequence_num]));
+        pop_queued_sequence(&status);
     }
 }
 
@@ -91,9 +137,17 @@ TEST_F(LedNotificationTest, TestQueueOrder4) {
     insert(&status, NOTIFY_PRIORITY_REGULAR);
     insert(&status, NOTIFY_PRIORITY_LOW);
 
-    EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, status.queued_priorities[4]);
+    set_highest_active(&status);
-    EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[3]);
+
-    EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[2]);
+    EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, (status.queued_priorities[status.active_sequence_num]));
-    EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[1]);
+    pop_queued_sequence(&status);
-    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[0]);
+    EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, (status.queued_priorities[status.active_sequence_num]));
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, (status.queued_priorities[status.active_sequence_num]));
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, (status.queued_priorities[status.active_sequence_num]));
+    pop_queued_sequence(&status);
+    EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, (status.queued_priorities[status.active_sequence_num]));
 }