rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

OP-1379 - Rebuild and simplify the queuing mechanism

Browse Source
This commit is contained in:
Alessio Morale 2014-09-17 01:24:01 +02:00
parent cf9329b8ee
commit 46ef59f186
5 changed files with 123 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/libraries/inc/optypes.h
View File

@ -33,6 +33,7 @@ typedef struct {
} Color_t; } Color_t;
extern const Color_t Color_Off; extern const Color_t Color_Off;
extern const Color_t Color_Black;
extern const Color_t Color_Red; extern const Color_t Color_Red;
extern const Color_t Color_Lime; extern const Color_t Color_Lime;
extern const Color_t Color_Blue; 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_Orange;
extern const Color_t Color_White; 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_RED { .R = 0xFF, .G = 0x00, .B = 0x00 }
#define COLOR_LIME { .R = 0x00, .G = 0xFF, .B = 0x00 } #define COLOR_LIME { .R = 0x00, .G = 0xFF, .B = 0x00 }
#define COLOR_BLUE { .R = 0x00, .G = 0x00, .B = 0xFF } #define COLOR_BLUE { .R = 0x00, .G = 0x00, .B = 0xFF }

102
flight/libraries/lednotification.c
View File

@ -35,19 +35,18 @@
// Private defines // Private defines
// Maximum number of notifications enqueued when a higher priority notification is added // 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 MAX_HANDLED_LED 1
#define BACKGROUND_SEQUENCE -1 #define BACKGROUND_SEQUENCE 0
#define RESET_STEP -1 #define RESET_STEP -1
#define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS) #define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS)
// Private data types definition // Private data types definition
// this is the status for a single notification led set // this is the status for a single notification led set
typedef struct { 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 next_run_time;
uint32_t sequence_starting_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) 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) { if (new_notification->priority == NOTIFY_PRIORITY_BACKGROUND) {
status->background_sequence = new_notification->sequence; lowest_priority_index = BACKGROUND_SEQUENCE;
} else { } 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. // no items with priority lower than the one we are trying to enqueue. skip
if (insert_point < 0) { if (lowest_priority_index < 0) {
return; 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_priorities[lowest_priority_index] = new_notification->priority;
status->queued_sequences[insert_point] = new_notification->sequence; status->queued_sequences[lowest_priority_index] = new_notification->sequence;
updated_sequence = insert_point; updated_sequence = lowest_priority_index;;
}
if (status->active_sequence_num < updated_sequence) {
// 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; 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) 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->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 // background sequence was completed
return false; return false;
@ -178,9 +165,7 @@ static bool pop_queued_sequence(NotifierLedStatus_t *status)
*/ */
static void advance_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; uint8_t step = status->next_sequence_step;
LedStep_t *currentStep = &activeSequence->steps[step]; LedStep_t *currentStep = &activeSequence->steps[step];
@ -244,8 +229,7 @@ static void run_led(NotifierLedStatus_t *status)
status->next_run_time = currentTime; status->next_run_time = currentTime;
uint8_t step = status->next_sequence_step; 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; 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++) { for (uint8_t i = status->led_set_start; i <= status->led_set_end; i++) {

1
flight/libraries/optypes.c
View File

@ -27,6 +27,7 @@
#include <optypes.h> #include <optypes.h>
const Color_t Color_Off = COLOR_OFF; 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_Red = COLOR_RED;
const Color_t Color_Lime = COLOR_LIME; const Color_t Color_Lime = COLOR_LIME;
const Color_t Color_Blue = COLOR_BLUE; const Color_t Color_Blue = COLOR_BLUE;

18
flight/modules/Notify/notify.c
View File

@ -24,24 +24,6 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 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 "openpilot.h"
#include <flightstatus.h> #include <flightstatus.h>
#include <systemalarms.h> #include <systemalarms.h>

90
flight/tests/lednotification/unittest.cpp
View File

@ -16,6 +16,32 @@ void PIOS_WS2811_Update() {}
class LedNotificationTest : public testing::Test {}; 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) void insert(NotifierLedStatus_t *status, pios_notify_priority priority)
{ {
ExtLedNotification_t notification; ExtLedNotification_t notification;
@ -41,12 +67,18 @@ TEST_F(LedNotificationTest, TestQueueOrder1) {
insert(&status, NOTIFY_PRIORITY_LOW); insert(&status, NOTIFY_PRIORITY_LOW);
insert(&status, NOTIFY_PRIORITY_CRITICAL); 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) { TEST_F(LedNotificationTest, TestQueueOrder2) {
@ -58,26 +90,40 @@ TEST_F(LedNotificationTest, TestQueueOrder2) {
// 148 updated_sequence = insert_point; // 148 updated_sequence = insert_point;
init(&status, NOTIFY_PRIORITY_LOW); init(&status, NOTIFY_PRIORITY_LOW);
status.queued_priorities[0] = NOTIFY_PRIORITY_BACKGROUND;
insert(&status, NOTIFY_PRIORITY_REGULAR); 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) { TEST_F(LedNotificationTest, TestQueueOrder3) {
NotifierLedStatus_t status; 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); init(&status, NOTIFY_PRIORITY_REGULAR);
status.queued_priorities[0] = NOTIFY_PRIORITY_BACKGROUND;
insert(&status, NOTIFY_PRIORITY_LOW); 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_REGULAR);
insert(&status, NOTIFY_PRIORITY_LOW); 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]));
} }