flight/libraries/lednotification.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       lednotification.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
   6  * @brief      led notification library.
   7  *             --
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26 #include "inc/lednotification.h"
  27 #include <stdbool.h>
  28 #include <stdint.h>
  29 #include <string.h>
  30 #include <FreeRTOS.h>
  31 #include <pios.h>
  32 #include <pios_notify.h>
  33 #include <pios_ws2811.h>
  34
  35 // Private defines
  36
  37 // Maximum number of notifications enqueued when a higher priority notification is added
  38 #define MAX_BACKGROUND_NOTIFICATIONS 6
  39 #define MAX_HANDLED_LED              1
  40
  41 #define BACKGROUND_SEQUENCE          0
  42 #define RESET_STEP                   -1
  43 #define GET_CURRENT_MILLIS           (xTaskGetTickCount() * portTICK_RATE_MS)
  44
  45 // Private data types definition
  46 // this is the status for a single notification led set
  47 typedef struct {
  48     int8_t   queued_priorities[MAX_BACKGROUND_NOTIFICATIONS]; // slot 0 is reserved for background
  49     LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS]; // slot 0 is reserved for background
  50     uint32_t next_run_time;
  51     uint32_t sequence_starting_time;
  52
  53     int8_t   active_sequence_num; // active queued sequence or BACKGROUND_SEQUENCE
  54     bool     running; // is this led running?
  55     bool     step_phase_on; // true = step on phase, false = step off phase
  56     uint8_t  next_sequence_step; // (step number to be executed) << 1 || (0x00 = on phase, 0x01 = off phase)
  57     uint8_t  next_step_rep; // next repetition number for next step (valid if step.repeats >1)
  58     uint8_t  next_sequence_rep; // next sequence repetition counter (valid if sequence.repeats > 1)
  59     uint8_t  led_set_start; // first target led for this set
  60     uint8_t  led_set_end; // last target led for this set
  61 } NotifierLedStatus_t;
  62
  63 static bool led_status_initialized = false;
  64
  65 NotifierLedStatus_t led_status[MAX_HANDLED_LED];
  66
  67 static void InitExtLed()
  68 {
  69     memset(led_status, 0, sizeof(NotifierLedStatus_t) * MAX_HANDLED_LED);
  70     const uint32_t now = GET_CURRENT_MILLIS;
  71     for (uint8_t l = 0; l < MAX_HANDLED_LED; l++) {
  72         led_status[l].led_set_start = 0;
  73         led_status[l].led_set_end   = PIOS_WS2811_NUMLEDS - 1;
  74         led_status[l].next_run_time = now + 500; // start within half a second
  75         for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
  76             led_status[l].queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND;
  77         }
  78     }
  79 }
  80
  81 /**
  82  * restart current sequence
  83  */
  84 static void restart_sequence(NotifierLedStatus_t *status, bool immediate)
  85 {
  86     status->next_sequence_step = 0;
  87     status->next_step_rep = 0;
  88     status->step_phase_on = true;
  89     status->running = true;
  90     if (immediate) {
  91         uint32_t currentTime = GET_CURRENT_MILLIS;
  92         status->next_run_time = currentTime;
  93     }
  94     status->sequence_starting_time = status->next_run_time;
  95 }
  96
  97 /**
  98  * modify background sequence or enqueue a new sequence to play
  99  */
 100 static void push_queued_sequence(ExtLedNotification_t *new_notification, NotifierLedStatus_t *status)
 101 {
 102     int8_t updated_sequence;
 103
 104     int8_t lowest_priority_index = -1;
 105     int8_t lowest_priority = new_notification->priority;
 106
 107     if (new_notification->priority == NOTIFY_PRIORITY_BACKGROUND) {
 108         lowest_priority_index = BACKGROUND_SEQUENCE;
 109     } else {
 110         // slot 0 is reserved for Background sequence
 111         for (int8_t i = 1; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
 112             if (status->queued_priorities[i] < lowest_priority) {
 113                 lowest_priority_index = i;
 114                 lowest_priority = status->queued_priorities[i];
 115             }
 116         }
 117     }
 118
 119     // no items with priority lower than the one we are trying to enqueue. skip
 120     if (lowest_priority_index < 0) {
 121         return;
 122     }
 123
 124     status->queued_priorities[lowest_priority_index] = new_notification->priority;
 125     status->queued_sequences[lowest_priority_index]  = new_notification->sequence;
 126     updated_sequence = lowest_priority_index;;
 127
 128
 129     // check whether we should preempt the current notification and play this new one
 130     if (status->queued_priorities[status->active_sequence_num] < new_notification->priority) {
 131         status->active_sequence_num = updated_sequence;
 132     }
 133
 134     if (status->active_sequence_num == updated_sequence) {
 135         restart_sequence(status, true);
 136     }
 137 }
 138
 139 static bool pop_queued_sequence(NotifierLedStatus_t *status)
 140 {
 141     if (status->active_sequence_num > BACKGROUND_SEQUENCE) {
 142         // set the last active slot as empty
 143         status->queued_priorities[status->active_sequence_num] = NOTIFY_PRIORITY_BACKGROUND;
 144
 145         // search the highest priority item
 146         int8_t highest_priority_index = BACKGROUND_SEQUENCE;
 147         int8_t highest_priority = NOTIFY_PRIORITY_BACKGROUND;
 148
 149         for (int8_t i = 1; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
 150             if (status->queued_priorities[i] > highest_priority) {
 151                 highest_priority_index = i;
 152                 highest_priority = status->queued_priorities[i];
 153             }
 154         }
 155         // set the next sequence to activate (or BACKGROUND_SEQUENCE when all slots are empty)
 156         status->active_sequence_num = highest_priority_index;
 157         return highest_priority_index != BACKGROUND_SEQUENCE;
 158     }
 159     // background sequence was completed
 160     return false;
 161 }
 162
 163 /**
 164  * advance current sequence pointers for next step
 165  */
 166 static void advance_sequence(NotifierLedStatus_t *status)
 167 {
 168     LedSequence_t *activeSequence = &status->queued_sequences[status->active_sequence_num];
 169
 170     uint8_t step = status->next_sequence_step;
 171     LedStep_t *currentStep = &activeSequence->steps[step];
 172
 173     // Next step will be the OFF phase, so just update the time and
 174     if (status->step_phase_on) {
 175         // next will be the off phase
 176         status->next_run_time += currentStep->time_on;
 177         status->step_phase_on  = false;
 178         // check if off phase should be skipped
 179         if (currentStep->time_off != 0) {
 180             return;
 181         }
 182     }
 183
 184     // next step is ON phase. check whether to repeat current step or move to next one
 185     status->next_run_time += currentStep->time_off;
 186     status->step_phase_on  = true;
 187
 188     if (status->next_step_rep + 1 < currentStep->repeats) {
 189         // setup next repetition
 190         status->next_step_rep++;
 191         return;
 192     }
 193
 194     // move to next step
 195     LedStep_t *nextStep = (step + 1 < NOTIFY_SEQUENCE_MAX_STEPS) ? &activeSequence->steps[step + 1] : 0;
 196
 197     // next step is null, check whether sequence must be repeated or it must move to lower priority queued or background sequences
 198     if (NOTIFY_IS_NULL_STEP(nextStep)) {
 199         if (activeSequence->repeats == -1 || status->next_sequence_rep + 1 < activeSequence->repeats) {
 200             status->next_sequence_rep++;
 201             // restart the sequence
 202             restart_sequence(status, false);
 203             return;
 204         }
 205         if (status->active_sequence_num != BACKGROUND_SEQUENCE) {
 206             // no repeat, pop enqueued or background sequences
 207             pop_queued_sequence(status);
 208             restart_sequence(status, false);
 209             status->next_sequence_rep = 0;
 210         } else {
 211             status->running = false;
 212         }
 213     } else {
 214         status->next_step_rep = 0;
 215         status->next_sequence_step++;
 216     }
 217 }
 218
 219 /**
 220  * run a led set
 221  */
 222 static void run_led(NotifierLedStatus_t *status)
 223 {
 224     const uint32_t currentTime = GET_CURRENT_MILLIS;
 225
 226     if (!status->running || currentTime < status->next_run_time) {
 227         return;
 228     }
 229     status->next_run_time = currentTime;
 230     uint8_t step = status->next_sequence_step;
 231
 232     LedSequence_t *activeSequence = &status->queued_sequences[status->active_sequence_num];
 233     const Color_t color = status->step_phase_on ? activeSequence->steps[step].color : Color_Off;
 234
 235     for (uint8_t i = status->led_set_start; i <= status->led_set_end; i++) {
 236         PIOS_WS2811_setColorRGB(color, i, false);
 237     }
 238     PIOS_WS2811_Update();
 239     advance_sequence(status);
 240 }
 241
 242 void LedNotificationExtLedsRun()
 243 {
 244     // handle incoming sequences
 245     if (!led_status_initialized) {
 246         InitExtLed();
 247         led_status_initialized = true;
 248     }
 249     static ExtLedNotification_t *newNotification;
 250     newNotification = PIOS_NOTIFY_GetNewExtLedSequence(true);
 251     if (newNotification) {
 252         push_queued_sequence(newNotification, &led_status[0]);
 253     }
 254
 255     // Run Leds
 256     for (uint8_t i = 0; i < MAX_HANDLED_LED; i++) {
 257         run_led(&led_status[i]);
 258     }
 259 }