From f556a5e335cb3bdb47b183468536e5012a980993 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 13 Aug 2014 15:14:55 +0200 Subject: [PATCH] OP-1379 - Split simple notification and external lednotifications libraries --- flight/libraries/inc/lednotification.h | 33 +++ flight/libraries/inc/notification.h | 1 - flight/libraries/lednotification.c | 268 ++++++++++++++++++ flight/libraries/notification.c | 244 +--------------- flight/modules/System/systemmod.c | 10 +- .../boards/discoveryf4bare/firmware/Makefile | 1 + .../boards/revolution/firmware/Makefile | 1 + 7 files changed, 314 insertions(+), 244 deletions(-) create mode 100644 flight/libraries/inc/lednotification.h create mode 100644 flight/libraries/lednotification.c diff --git a/flight/libraries/inc/lednotification.h b/flight/libraries/inc/lednotification.h new file mode 100644 index 000000000..fadd100f0 --- /dev/null +++ b/flight/libraries/inc/lednotification.h @@ -0,0 +1,33 @@ +/** + ****************************************************************************** + * + * @file lednotification.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief led 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 LEDNOTIFICATION_H_ +#define LEDNOTIFICATION_H_ +#include + +void LedNotificationExtLedsRun(); + + +#endif /* LEDNOTIFICATION_H_ */ diff --git a/flight/libraries/inc/notification.h b/flight/libraries/inc/notification.h index bcf85015c..2053a9609 100644 --- a/flight/libraries/inc/notification.h +++ b/flight/libraries/inc/notification.h @@ -25,7 +25,6 @@ */ #ifndef NOTIFICATION_H #define NOTIFICATION_H -#include // period of each blink phase #define LED_BLINK_PERIOD_MS 200 diff --git a/flight/libraries/lednotification.c b/flight/libraries/lednotification.c new file mode 100644 index 000000000..b2ca13597 --- /dev/null +++ b/flight/libraries/lednotification.c @@ -0,0 +1,268 @@ +/** + ****************************************************************************** + * + * @file lednotification.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief led 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/lednotification.h" +#include +#include +#include +#include +#include +#include +#include + +// Private defines + +// Maximum number of notifications enqueued when a higher priority notification is added +#define MAX_BACKGROUND_NOTIFICATIONS 5 +#define MAX_HANDLED_LED 1 + +#define BACKGROUND_SEQUENCE -1 +#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; + uint32_t next_run_time; + uint32_t sequence_starting_time; + + int8_t active_sequence_num; // active queued sequence or BACKGROUND_SEQUENCE + bool running; // is this led running? + bool step_phase_on; // true = step on phase, false = step off phase + uint8_t next_sequence_step; // (step number to be executed) << 1 || (0x00 = on phase, 0x01 = off phase) + uint8_t next_step_rep; // next repetition number for next step (valid if step.repeats >1) + uint8_t next_sequence_rep; // next sequence repetition counter (valid if sequence.repeats > 1) + uint8_t led_set; // target led set +} NotifierLedStatus_t; + +static bool led_status_initialized = false; + +NotifierLedStatus_t led_status[MAX_HANDLED_LED]; + +static void InitExtLed() +{ + memset(led_status, 0, sizeof(NotifierLedStatus_t) * MAX_HANDLED_LED); + const uint32_t now = GET_CURRENT_MILLIS; + for (uint8_t l = 0; l < MAX_HANDLED_LED; l++) { + led_status[l].led_set = 0; + led_status[l].next_run_time = now + 500; // start within half a second + for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { + led_status[l].queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND; + } + } +} + +/** + * restart current sequence + */ +static void restart_sequence(NotifierLedStatus_t *status, bool immediate) +{ + status->next_sequence_step = 0; + status->next_step_rep = 0; + status->step_phase_on = true; + status->running = true; + if (immediate) { + uint32_t currentTime = GET_CURRENT_MILLIS; + status->next_run_time = currentTime; + } + status->sequence_starting_time = status->next_run_time; +} + +/** + * modify background sequence or enqueue a new sequence to play + */ +static void push_queued_sequence(ExtLedNotification_t *new_notification, NotifierLedStatus_t *status) +{ + int8_t updated_sequence = BACKGROUND_SEQUENCE; + + if (new_notification->priority == NOTIFY_PRIORITY_BACKGROUND) { + status->background_sequence = new_notification->sequence; + } else { + // a notification with priority higher than background. + // try to enqueue it + int8_t insert_point = -1; + int8_t first_free = -1; + for (int8_t i = MAX_BACKGROUND_NOTIFICATIONS; i > -1; 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; + } + } + + 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) { + status->active_sequence_num = updated_sequence; + restart_sequence(status, true); + } + if (updated_sequence == BACKGROUND_SEQUENCE) { + restart_sequence(status, false); + } +} + +static bool pop_queued_sequence(NotifierLedStatus_t *status) +{ + if (status->active_sequence_num != BACKGROUND_SEQUENCE) { + // start the lower priority item + status->queued_priorities[status->active_sequence_num] = NOTIFY_PRIORITY_BACKGROUND; + status->active_sequence_num--; + return true; + } + // background sequence was completed + return false; +} + +/** + * advance current sequence pointers for next step + */ +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]; + + uint8_t step = status->next_sequence_step; + LedStep_t *currentStep = &activeSequence->steps[step]; + + // Next step will be the OFF phase, so just update the time and + if (status->step_phase_on) { + // next will be the off phase + status->next_run_time += currentStep->time_on; + status->step_phase_on = false; + // check if off phase should be skipped + if (currentStep->time_off != 0) { + return; + } + } + + // next step is ON phase. check whether to repeat current step or move to next one + status->next_run_time += currentStep->time_off; + status->step_phase_on = true; + + if (status->next_step_rep + 1 < currentStep->repeats) { + // setup next repetition + status->next_step_rep++; + return; + } + + // move to next step + LedStep_t *nextStep = (step + 1 < NOTIFY_SEQUENCE_MAX_STEPS) ? &activeSequence->steps[step + 1] : 0; + + // next step is null, check whether sequence must be repeated or it must move to lower priority queued or background sequences + if (NOTIFY_IS_NULL_STEP(nextStep)) { + if (activeSequence->repeats == -1 || status->next_sequence_rep + 1 < activeSequence->repeats) { + status->next_sequence_rep++; + // restart the sequence + restart_sequence(status, false); + return; + } + if (status->active_sequence_num != BACKGROUND_SEQUENCE) { + // no repeat, pop enqueued or background sequences + pop_queued_sequence(status); + restart_sequence(status, false); + status->next_sequence_rep = 0; + } else { + status->running = false; + } + } else { + status->next_step_rep = 0; + status->next_sequence_step++; + } +} + +/** + * run a led set + */ +static void run_led(NotifierLedStatus_t *status) +{ + uint32_t currentTime = GET_CURRENT_MILLIS; + + if (!status->running || currentTime < status->next_run_time) { + return; + } + 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]; + if (status->step_phase_on) { + PIOS_WS2811_setColorRGB(activeSequence->steps[step].color, status->led_set, true); + } else { + PIOS_WS2811_setColorRGB(Color_Off, status->led_set, true); + } + advance_sequence(status); +} + +void LedNotificationExtLedsRun() +{ + // handle incoming sequences + if (!led_status_initialized) { + InitExtLed(); + led_status_initialized = true; + } + static ExtLedNotification_t *newNotification; + newNotification = PIOS_NOTIFY_GetNewExtLedSequence(true); + if (newNotification) { + push_queued_sequence(newNotification, &led_status[0]); + } + + // Run Leds + for (uint8_t i = 0; i < MAX_HANDLED_LED; i++) { + run_led(&led_status[i]); + } +} diff --git a/flight/libraries/notification.c b/flight/libraries/notification.c index c02152e02..cf704d4d5 100644 --- a/flight/libraries/notification.c +++ b/flight/libraries/notification.c @@ -31,40 +31,10 @@ #include #include -#ifdef PIOS_INCLUDE_WS2811 -#include -#endif -// Private defines -// Maximum number of notifications enqueued when a higher priority notification is added -#define MAX_BACKGROUND_NOTIFICATIONS 3 -#define MAX_HANDLED_LED 1 -#define BACKGROUND_SEQUENCE -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 -// this is the status for a single LED notification led set -typedef struct { - int8_t queued_priorities[MAX_BACKGROUND_NOTIFICATIONS]; - LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS]; - LedSequence_t background_sequence; - uint32_t next_run_time; - uint32_t sequence_starting_time; - - int8_t active_sequence_num; // active queued sequence or BACKGROUND_SEQUENCE - bool running; // is this led running? - bool step_phase_on; // true = step on phase, false = step off phase - uint8_t next_sequence_step; // (step number to be executed) << 1 || (0x00 = on phase, 0x01 = off phase) - uint8_t next_step_rep; // next repetition number for next step (valid if step.repeats >1) - uint8_t next_sequence_rep; // next sequence repetition counter (valid if sequence.repeats > 1) - uint8_t led_set; // target led set -} NotifierLedStatus_t; - -#ifdef PIOS_INCLUDE_WS2811 -static bool led_status_initialized = false; -#endif - #ifdef PIOS_LED_ALARM #define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM) #define ALARM_LED_OFF() PIOS_LED_Off(PIOS_LED_ALARM) @@ -173,7 +143,6 @@ static bool handleNotifications(pios_notify_notification runningNotification, ui static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern); static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern); -static void HandleExtLeds(); void NotificationUpdateStatus() { @@ -202,8 +171,8 @@ void NotificationOnboardLedsRun() STATUS_LENGHT } status; - HandleExtLeds(); const uint32_t current_timestamp = GET_CURRENT_MILLIS; + if (!started || (current_timestamp - lastRunTimestamp) < LED_BLINK_PERIOD_MS) { return; } @@ -334,212 +303,3 @@ static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern) *b_pattern = BLINK_B_HEARTBEAT_PATTERN; *r_pattern = BLINK_R_HEARTBEAT_PATTERN; } - -#ifdef PIOS_INCLUDE_WS2811 - -NotifierLedStatus_t led_status[MAX_HANDLED_LED]; - -static void InitExtLed() -{ - memset(led_status, 0, sizeof(NotifierLedStatus_t) * MAX_HANDLED_LED); - const uint32_t now = GET_CURRENT_MILLIS; - for (uint8_t l = 0; l < MAX_HANDLED_LED; l++) { - led_status[l].led_set = l; - led_status[l].next_run_time = now + 500; // start within half a second - for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { - led_status[l].queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND; - } - } -} - -/** - * restart current sequence - */ -static void restart_sequence(NotifierLedStatus_t *status, bool immediate) -{ - status->next_sequence_step = 0; - status->next_step_rep = 0; - status->step_phase_on = true; - status->running = true; - if (immediate) { - uint32_t currentTime = GET_CURRENT_MILLIS; - status->next_run_time = currentTime; - } - status->sequence_starting_time = status->next_run_time; -} - -/** - * modify background sequence or enqueue a new sequence to play - */ -static void push_queued_sequence(ExtLedNotification_t *new_notification, NotifierLedStatus_t *status) -{ - int8_t updated_sequence = BACKGROUND_SEQUENCE; - - if (new_notification->priority == NOTIFY_PRIORITY_BACKGROUND) { - status->background_sequence = new_notification->sequence; - } else { - // a notification with priority higher than background. - // try to enqueue it - int8_t insert_point = -1; - int8_t first_free = -1; - for (int8_t i = MAX_BACKGROUND_NOTIFICATIONS; i > -1; 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; - } - } - - 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) { - status->active_sequence_num = updated_sequence; - restart_sequence(status, true); - } - if (updated_sequence == BACKGROUND_SEQUENCE) { - restart_sequence(status, false); - } -} - -static bool pop_queued_sequence(NotifierLedStatus_t *status) -{ - if (status->active_sequence_num != BACKGROUND_SEQUENCE) { - // start the lower priority item - status->queued_priorities[status->active_sequence_num] = NOTIFY_PRIORITY_BACKGROUND; - status->active_sequence_num--; - return true; - } - // background sequence was completed - return false; -} - -/** - * advance current sequence pointers for next step - */ -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]; - - uint8_t step = status->next_sequence_step; - LedStep_t *currentStep = &activeSequence->steps[step]; - - // Next step will be the OFF phase, so just update the time and - if (status->step_phase_on) { - // next will be the off phase - status->next_run_time += currentStep->time_on; - status->step_phase_on = false; - // check if off phase should be skipped - if (currentStep->time_off != 0) { - return; - } - } - - // next step is ON phase. check whether to repeat current step or move to next one - status->next_run_time += currentStep->time_off; - status->step_phase_on = true; - - if (status->next_step_rep + 1 < currentStep->repeats) { - // setup next repetition - status->next_step_rep++; - return; - } - - // move to next step - LedStep_t *nextStep = (step + 1 < NOTIFY_SEQUENCE_MAX_STEPS) ? &activeSequence->steps[step + 1] : 0; - - // next step is null, check whether sequence must be repeated or it must move to lower priority queued or background sequences - if (NOTIFY_IS_NULL_STEP(nextStep)) { - if (activeSequence->repeats == -1 || status->next_sequence_rep + 1 < activeSequence->repeats) { - status->next_sequence_rep++; - // restart the sequence - restart_sequence(status, false); - return; - } - if (status->active_sequence_num != BACKGROUND_SEQUENCE) { - // no repeat, pop enqueued or background sequences - pop_queued_sequence(status); - restart_sequence(status, false); - } else { - status->running = false; - } - } else { - status->next_step_rep = 0; - status->next_sequence_step++; - } -} - -/** - * run a led set - */ -static void run_led(NotifierLedStatus_t *status) -{ - uint32_t currentTime = GET_CURRENT_MILLIS; - - if (!status->running || currentTime < status->next_run_time) { - return; - } - 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]; - if (status->step_phase_on) { - PIOS_WS2811_setColorRGB(activeSequence->steps[step].color, status->led_set, true); - } else { - PIOS_WS2811_setColorRGB(Color_Off, status->led_set, true); - } - advance_sequence(status); -} - -void HandleExtLeds() -{ - // handle incoming sequences - if (!led_status_initialized) { - InitExtLed(); - led_status_initialized = true; - } - static ExtLedNotification_t *newNotification; - newNotification = PIOS_NOTIFY_GetNewExtLedSequence(true); - if (newNotification) { - push_queued_sequence(newNotification, &led_status[0]); - } - - // Run Leds - for (uint8_t i = 0; i < MAX_HANDLED_LED; i++) { - run_led(&led_status[i]); - } -} -#else /* ifdef PIOS_INCLUDE_WS2811 */ -void HandleExtLeds() {} -#endif /* ifdef PIOS_INCLUDE_WS2811 */ diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index aa6a8f54d..e37168061 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -42,7 +42,11 @@ #include // private includes #include "inc/systemmod.h" -#include "notification.h" + +#include +#ifdef PIOS_INCLUDE_WS2811 +#include +#endif // UAVOs #include @@ -55,6 +59,7 @@ #include #include #include +#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include @@ -633,6 +638,9 @@ static void updateSystemAlarms() void vApplicationIdleHook(void) { NotificationOnboardLedsRun(); +#ifdef PIOS_INCLUDE_WS2811 + LedNotificationExtLedsRun(); +#endif } /** * Called by the RTOS when a stack overflow is detected. diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 608d2b6cf..29cf57378 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -87,6 +87,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/lednotification.c ## UAVObjects include ./UAVObjects.inc diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index cb99372f1..e4220ade0 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -88,6 +88,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/lednotification.c ## UAVObjects include ./UAVObjects.inc