From 46ef59f1865717deb9b2f42dee6d81ec027f8e91 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 17 Sep 2014 01:24:01 +0200 Subject: [PATCH] OP-1379 - Rebuild and simplify the queuing mechanism --- flight/libraries/inc/optypes.h | 4 +- flight/libraries/lednotification.c | 110 +++++++++------------- flight/libraries/optypes.c | 1 + flight/modules/Notify/notify.c | 18 ---- flight/tests/lednotification/unittest.cpp | 90 ++++++++++++++---- 5 files changed, 123 insertions(+), 100 deletions(-) diff --git a/flight/libraries/inc/optypes.h b/flight/libraries/inc/optypes.h index 82a95e52e..6b9080db7 100644 --- a/flight/libraries/inc/optypes.h +++ b/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 } diff --git a/flight/libraries/lednotification.c b/flight/libraries/lednotification.c index 4b5950d17..e6d961747 100644 --- a/flight/libraries/lednotification.c +++ b/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]; - LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS]; - LedSequence_t background_sequence; + int8_t queued_priorities[MAX_BACKGROUND_NOTIFICATIONS]; // slot 0 is reserved for background + LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS]; // slot 0 is reserved for background 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. - // try to enqueue it - int8_t insert_point = MAX_BACKGROUND_NOTIFICATIONS - 1; - int8_t first_free = -1; - for (int8_t i = MAX_BACKGROUND_NOTIFICATIONS - 1; i >= 0; 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 + // slot 0 is reserved for Background sequence + for (int8_t i = 1; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { + if (status->queued_priorities[i] < lowest_priority) { + lowest_priority_index = i; + lowest_priority = status->queued_priorities[i]; } } - - // 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) { - // start the lower priority item + if (status->active_sequence_num > BACKGROUND_SEQUENCE) { + // 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 = - status->active_sequence_num == BACKGROUND_SEQUENCE ? - &status->background_sequence : &status->queued_sequences[status->active_sequence_num]; + LedSequence_t *activeSequence = &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 ? - &status->background_sequence : &status->queued_sequences[status->active_sequence_num]; + LedSequence_t *activeSequence = &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++) { diff --git a/flight/libraries/optypes.c b/flight/libraries/optypes.c index b67b81c3f..3b2c7b728 100644 --- a/flight/libraries/optypes.c +++ b/flight/libraries/optypes.c @@ -27,6 +27,7 @@ #include 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; diff --git a/flight/modules/Notify/notify.c b/flight/modules/Notify/notify.c index 8ccb3cdc4..aeb7526bf 100644 --- a/flight/modules/Notify/notify.c +++ b/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 #include diff --git a/flight/tests/lednotification/unittest.cpp b/flight/tests/lednotification/unittest.cpp index 3d289ffec..853d4b9f8 100644 --- a/flight/tests/lednotification/unittest.cpp +++ b/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[2]); - EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, status.queued_priorities[3]); - EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, status.queued_priorities[4]); + + EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, (status.queued_priorities[status.active_sequence_num])); + pop_queued_sequence(&status); + EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, (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, 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]); - EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[3]); - EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[2]); - EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[1]); - EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[0]); + set_highest_active(&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_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++) { - EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[i]); + set_highest_active(&status); + + + 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]); - 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[1]); - EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[0]); + set_highest_active(&status); + + 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_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])); }