src/main/io/ledstrip.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdlib.h>
  20 #include <stdint.h>
  21 #include <string.h>
  22 #include <stdarg.h>
  23
  24 #include <platform.h>
  25
  26 #include <build_config.h>
  27
  28 #ifdef LED_STRIP
  29
  30 #include <common/color.h>
  31
  32 #include "drivers/light_ws2811strip.h"
  33 #include "drivers/system.h"
  34 #include "drivers/serial.h"
  35
  36 #include <common/maths.h>
  37 #include <common/printf.h>
  38 #include <common/typeconversion.h>
  39
  40 #include "sensors/battery.h"
  41
  42 #include "config/runtime_config.h"
  43 #include "config/config.h"
  44 #include "rx/rx.h"
  45 #include "io/rc_controls.h"
  46 #include "flight/failsafe.h"
  47
  48 #include "io/ledstrip.h"
  49
  50 static bool ledStripInitialised = false;
  51 static failsafe_t* failsafe;
  52
  53 #if MAX_LED_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH
  54 #error "Led strip length must match driver"
  55 #endif
  56
  57 hsvColor_t *colors;
  58
  59 //#define USE_LED_ANIMATION
  60
  61 //                          H    S    V
  62 #define LED_BLACK        {  0,   0,   0}
  63 #define LED_WHITE        {  0, 255, 255}
  64 #define LED_RED          {  0,   0, 255}
  65 #define LED_ORANGE       { 30,   0, 255}
  66 #define LED_YELLOW       { 60,   0, 255}
  67 #define LED_LIME_GREEN   { 90,   0, 255}
  68 #define LED_GREEN        {120,   0, 255}
  69 #define LED_MINT_GREEN   {150,   0, 255}
  70 #define LED_CYAN         {180,   0, 255}
  71 #define LED_LIGHT_BLUE   {210,   0, 255}
  72 #define LED_BLUE         {240,   0, 255}
  73 #define LED_DARK_VIOLET  {270,   0, 255}
  74 #define LED_MAGENTA      {300,   0, 255}
  75 #define LED_DEEP_PINK    {330,   0, 255}
  76
  77 const hsvColor_t hsv_black       = LED_BLACK;
  78 const hsvColor_t hsv_white       = LED_WHITE;
  79 const hsvColor_t hsv_red         = LED_RED;
  80 const hsvColor_t hsv_orange      = LED_ORANGE;
  81 const hsvColor_t hsv_yellow      = LED_YELLOW;
  82 const hsvColor_t hsv_limeGreen   = LED_LIME_GREEN;
  83 const hsvColor_t hsv_green       = LED_GREEN;
  84 const hsvColor_t hsv_mintGreen   = LED_MINT_GREEN;
  85 const hsvColor_t hsv_cyan        = LED_CYAN;
  86 const hsvColor_t hsv_lightBlue   = LED_LIGHT_BLUE;
  87 const hsvColor_t hsv_blue        = LED_BLUE;
  88 const hsvColor_t hsv_darkViolet  = LED_DARK_VIOLET;
  89 const hsvColor_t hsv_magenta     = LED_MAGENTA;
  90 const hsvColor_t hsv_deepPink    = LED_DEEP_PINK;
  91
  92 #define LED_DIRECTION_COUNT 6
  93
  94 const hsvColor_t * const defaultColors[] = {
  95         &hsv_black,
  96         &hsv_white,
  97         &hsv_red,
  98         &hsv_orange,
  99         &hsv_yellow,
 100         &hsv_limeGreen,
 101         &hsv_green,
 102         &hsv_mintGreen,
 103         &hsv_cyan,
 104         &hsv_lightBlue,
 105         &hsv_blue,
 106         &hsv_darkViolet,
 107         &hsv_magenta,
 108         &hsv_deepPink
 109 };
 110
 111 typedef enum {
 112     COLOR_BLACK = 0,
 113     COLOR_WHITE,
 114     COLOR_RED,
 115     COLOR_ORANGE,
 116     COLOR_YELLOW,
 117     COLOR_LIME_GREEN,
 118     COLOR_GREEN,
 119     COLOR_MINT_GREEN,
 120     COLOR_CYAN,
 121     COLOR_LIGHT_BLUE,
 122     COLOR_BLUE,
 123     COLOR_DARK_VIOLET,
 124     COLOR_MAGENTA,
 125     COLOR_DEEP_PINK,
 126 } colorIds;
 127
 128 typedef enum {
 129     DIRECTION_NORTH = 0,
 130     DIRECTION_EAST,
 131     DIRECTION_SOUTH,
 132     DIRECTION_WEST,
 133     DIRECTION_UP,
 134     DIRECTION_DOWN
 135 } directionId_e;
 136
 137 typedef struct modeColorIndexes_s {
 138     uint8_t north;
 139     uint8_t east;
 140     uint8_t south;
 141     uint8_t west;
 142     uint8_t up;
 143     uint8_t down;
 144 } modeColorIndexes_t;
 145
 146
 147 // Note, the color index used for the mode colors below refer to the default colors.
 148 // if the colors are reconfigured the index is still valid but the displayed color might
 149 // be different.
 150 // See colors[] and defaultColors[] and applyDefaultColors[]
 151
 152 static const modeColorIndexes_t orientationModeColors = {
 153         COLOR_WHITE,
 154         COLOR_DARK_VIOLET,
 155         COLOR_RED,
 156         COLOR_DEEP_PINK,
 157         COLOR_BLUE,
 158         COLOR_ORANGE
 159 };
 160
 161 static const modeColorIndexes_t headfreeModeColors = {
 162         COLOR_LIME_GREEN,
 163         COLOR_DARK_VIOLET,
 164         COLOR_ORANGE,
 165         COLOR_DEEP_PINK,
 166         COLOR_BLUE,
 167         COLOR_ORANGE
 168 };
 169
 170 static const modeColorIndexes_t horizonModeColors = {
 171         COLOR_BLUE,
 172         COLOR_DARK_VIOLET,
 173         COLOR_YELLOW,
 174         COLOR_DEEP_PINK,
 175         COLOR_BLUE,
 176         COLOR_ORANGE
 177 };
 178
 179 static const modeColorIndexes_t angleModeColors = {
 180         COLOR_CYAN,
 181         COLOR_DARK_VIOLET,
 182         COLOR_YELLOW,
 183         COLOR_DEEP_PINK,
 184         COLOR_BLUE,
 185         COLOR_ORANGE
 186 };
 187
 188 static const modeColorIndexes_t magModeColors = {
 189         COLOR_MINT_GREEN,
 190         COLOR_DARK_VIOLET,
 191         COLOR_ORANGE,
 192         COLOR_DEEP_PINK,
 193         COLOR_BLUE,
 194         COLOR_ORANGE
 195 };
 196
 197 static const modeColorIndexes_t baroModeColors = {
 198         COLOR_LIGHT_BLUE,
 199         COLOR_DARK_VIOLET,
 200         COLOR_RED,
 201         COLOR_DEEP_PINK,
 202         COLOR_BLUE,
 203         COLOR_ORANGE
 204 };
 205
 206
 207 uint8_t ledGridWidth;
 208 uint8_t ledGridHeight;
 209 uint8_t ledCount;
 210
 211 ledConfig_t *ledConfigs;
 212
 213 const ledConfig_t defaultLedStripConfig[] = {
 214     { CALCULATE_LED_XY( 2,  2), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
 215     { CALCULATE_LED_XY( 2,  1), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
 216     { CALCULATE_LED_XY( 2,  0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
 217     { CALCULATE_LED_XY( 1,  0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
 218     { CALCULATE_LED_XY( 0,  0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
 219     { CALCULATE_LED_XY( 0,  1), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
 220     { CALCULATE_LED_XY( 0,  2), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
 221     { CALCULATE_LED_XY( 1,  2), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
 222     { CALCULATE_LED_XY( 1,  1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
 223     { CALCULATE_LED_XY( 1,  1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
 224     { CALCULATE_LED_XY( 1,  1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
 225     { CALCULATE_LED_XY( 1,  1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
 226 };
 227
 228
 229 /*
 230  * 6 coords @nn,nn
 231  * 4 direction @##
 232  * 6 modes @####
 233  * = 16 bytes per led
 234  * 16 * 32 leds = 512 bytes storage needed worst case.
 235  * = not efficient to store led configs as strings in flash.
 236  * = becomes a problem to send all the data via cli due to serial/cli buffers
 237  */
 238
 239 typedef enum {
 240     X_COORDINATE,
 241     Y_COORDINATE,
 242     DIRECTIONS,
 243     FUNCTIONS
 244 } parseState_e;
 245
 246 #define PARSE_STATE_COUNT 4
 247
 248 static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':', '\0' };
 249
 250 static const char directionCodes[] = { 'N', 'E', 'S', 'W', 'U', 'D' };
 251 #define DIRECTION_COUNT (sizeof(directionCodes) / sizeof(directionCodes[0]))
 252 static const uint8_t directionMappings[DIRECTION_COUNT] = {
 253     LED_DIRECTION_NORTH,
 254     LED_DIRECTION_EAST,
 255     LED_DIRECTION_SOUTH,
 256     LED_DIRECTION_WEST,
 257     LED_DIRECTION_UP,
 258     LED_DIRECTION_DOWN
 259 };
 260
 261 static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T' };
 262 #define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0]))
 263 static const uint16_t functionMappings[FUNCTION_COUNT] = {
 264     LED_FUNCTION_INDICATOR,
 265     LED_FUNCTION_WARNING,
 266     LED_FUNCTION_FLIGHT_MODE,
 267     LED_FUNCTION_ARM_STATE,
 268     LED_FUNCTION_THROTTLE
 269 };
 270
 271 // grid offsets
 272 uint8_t highestYValueForNorth;
 273 uint8_t lowestYValueForSouth;
 274 uint8_t highestXValueForWest;
 275 uint8_t lowestXValueForEast;
 276
 277 void determineLedStripDimensions(void)
 278 {
 279     ledGridWidth = 0;
 280     ledGridHeight = 0;
 281
 282     uint8_t ledIndex;
 283     const ledConfig_t *ledConfig;
 284
 285     for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {
 286         ledConfig = &ledConfigs[ledIndex];
 287
 288         if (GET_LED_X(ledConfig) >= ledGridWidth) {
 289             ledGridWidth = GET_LED_X(ledConfig) + 1;
 290         }
 291         if (GET_LED_Y(ledConfig) >= ledGridHeight) {
 292             ledGridHeight = GET_LED_Y(ledConfig) + 1;
 293         }
 294     }
 295 }
 296
 297 void determineOrientationLimits(void)
 298 {
 299     bool isOddHeight = (ledGridHeight & 1);
 300     bool isOddWidth = (ledGridWidth & 1);
 301     uint8_t heightModifier = isOddHeight ? 1 : 0;
 302     uint8_t widthModifier = isOddWidth ? 1 : 0;
 303
 304     highestYValueForNorth = (ledGridHeight / 2) - 1;
 305     lowestYValueForSouth = (ledGridHeight / 2) + heightModifier;
 306     highestXValueForWest = (ledGridWidth / 2) - 1;
 307     lowestXValueForEast = (ledGridWidth / 2) + widthModifier;
 308 }
 309
 310 void updateLedCount(void)
 311 {
 312     uint8_t ledIndex;
 313     ledCount = 0;
 314     for (ledIndex = 0; ledIndex < MAX_LED_STRIP_LENGTH; ledIndex++) {
 315         if (ledConfigs[ledIndex].flags == 0 && ledConfigs[ledIndex].xy == 0) {
 316             break;
 317         }
 318         ledCount++;
 319     }
 320 }
 321
 322 static void reevalulateLedConfig(void)
 323 {
 324     updateLedCount();
 325     determineLedStripDimensions();
 326     determineOrientationLimits();
 327 }
 328
 329 #define CHUNK_BUFFER_SIZE 10
 330
 331 #define NEXT_PARSE_STATE(parseState) ((parseState + 1) % PARSE_STATE_COUNT)
 332
 333
 334 bool parseLedStripConfig(uint8_t ledIndex, const char *config)
 335 {
 336     char chunk[CHUNK_BUFFER_SIZE];
 337     uint8_t chunkIndex;
 338     uint8_t val;
 339
 340     uint8_t parseState = X_COORDINATE;
 341     bool ok = true;
 342
 343     if (ledIndex >= MAX_LED_STRIP_LENGTH) {
 344         return !ok;
 345     }
 346
 347     ledConfig_t *ledConfig = &ledConfigs[ledIndex];
 348     memset(ledConfig, 0, sizeof(ledConfig_t));
 349
 350     while (ok) {
 351
 352         char chunkSeparator = chunkSeparators[parseState];
 353
 354         memset(&chunk, 0, sizeof(chunk));
 355         chunkIndex = 0;
 356
 357         while (*config && chunkIndex < CHUNK_BUFFER_SIZE && *config != chunkSeparator) {
 358             chunk[chunkIndex++] = *config++;
 359         }
 360
 361         if (*config++ != chunkSeparator) {
 362             ok = false;
 363             break;
 364         }
 365
 366         switch((parseState_e)parseState) {
 367             case X_COORDINATE:
 368                 val = atoi(chunk);
 369                 ledConfig->xy |= CALCULATE_LED_X(val);
 370                 break;
 371             case Y_COORDINATE:
 372                 val = atoi(chunk);
 373                 ledConfig->xy |= CALCULATE_LED_Y(val);
 374                 break;
 375             case DIRECTIONS:
 376                 for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) {
 377                     for (uint8_t mappingIndex = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) {
 378                         if (directionCodes[mappingIndex] == chunk[chunkIndex]) {
 379                             ledConfig->flags |= directionMappings[mappingIndex];
 380                             break;
 381                         }
 382                     }
 383                 }
 384                 break;
 385             case FUNCTIONS:
 386                 for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) {
 387                     for (uint8_t mappingIndex = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) {
 388                         if (functionCodes[mappingIndex] == chunk[chunkIndex]) {
 389                             ledConfig->flags |= functionMappings[mappingIndex];
 390                             break;
 391                         }
 392                     }
 393                 }
 394                 break;
 395         }
 396
 397         parseState++;
 398         if (parseState >= PARSE_STATE_COUNT) {
 399             break;
 400         }
 401     }
 402
 403     if (!ok) {
 404         memset(ledConfig, 0, sizeof(ledConfig_t));
 405     }
 406
 407     reevalulateLedConfig();
 408
 409     return ok;
 410 }
 411
 412 void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize)
 413 {
 414     char functions[FUNCTION_COUNT];
 415     char directions[DIRECTION_COUNT];
 416     uint8_t index;
 417     uint8_t mappingIndex;
 418     ledConfig_t *ledConfig = &ledConfigs[ledIndex];
 419
 420     memset(ledConfigBuffer, 0, bufferSize);
 421     memset(&functions, 0, sizeof(functions));
 422     memset(&directions, 0, sizeof(directions));
 423
 424     for (mappingIndex = 0, index = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) {
 425         if (ledConfig->flags & functionMappings[mappingIndex]) {
 426             functions[index++] = functionCodes[mappingIndex];
 427         }
 428     }
 429
 430     for (mappingIndex = 0, index = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) {
 431         if (ledConfig->flags & directionMappings[mappingIndex]) {
 432             directions[index++] = directionCodes[mappingIndex];
 433         }
 434     }
 435
 436     sprintf(ledConfigBuffer, "%u,%u:%s:%s", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions);
 437 }
 438
 439 // timers
 440 uint32_t nextAnimationUpdateAt = 0;
 441 uint32_t nextIndicatorFlashAt = 0;
 442 uint32_t nextWarningFlashAt = 0;
 443
 444 #define LED_STRIP_20HZ ((1000 * 1000) / 20)
 445 #define LED_STRIP_10HZ ((1000 * 1000) / 10)
 446 #define LED_STRIP_5HZ ((1000 * 1000) / 5)
 447
 448 void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors)
 449 {
 450     // apply up/down colors regardless of quadrant.
 451     if ((ledConfig->flags & LED_DIRECTION_UP)) {
 452         setLedHsv(ledIndex, &colors[modeColors->up]);
 453     }
 454
 455     if ((ledConfig->flags & LED_DIRECTION_DOWN)) {
 456         setLedHsv(ledIndex, &colors[modeColors->down]);
 457     }
 458
 459     // override with n/e/s/w colors to each n/s e/w half - bail at first match.
 460     if ((ledConfig->flags & LED_DIRECTION_WEST) && GET_LED_X(ledConfig) <= highestXValueForWest) {
 461         setLedHsv(ledIndex, &colors[modeColors->west]);
 462     }
 463
 464     if ((ledConfig->flags & LED_DIRECTION_EAST) && GET_LED_X(ledConfig) >= lowestXValueForEast) {
 465         setLedHsv(ledIndex, &colors[modeColors->east]);
 466     }
 467
 468     if ((ledConfig->flags & LED_DIRECTION_NORTH) && GET_LED_Y(ledConfig) <= highestYValueForNorth) {
 469         setLedHsv(ledIndex, &colors[modeColors->north]);
 470     }
 471
 472     if ((ledConfig->flags & LED_DIRECTION_SOUTH) && GET_LED_Y(ledConfig) >= lowestYValueForSouth) {
 473         setLedHsv(ledIndex, &colors[modeColors->south]);
 474     }
 475
 476 }
 477
 478 typedef enum {
 479     QUADRANT_NORTH_EAST = 1,
 480     QUADRANT_SOUTH_EAST,
 481     QUADRANT_SOUTH_WEST,
 482     QUADRANT_NORTH_WEST
 483 } quadrant_e;
 484
 485 void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const quadrant_e quadrant, const hsvColor_t *color)
 486 {
 487     switch (quadrant) {
 488         case QUADRANT_NORTH_EAST:
 489             if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) {
 490                 setLedHsv(ledIndex, color);
 491             }
 492             return;
 493
 494         case QUADRANT_SOUTH_EAST:
 495             if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) {
 496                 setLedHsv(ledIndex, color);
 497             }
 498             return;
 499
 500         case QUADRANT_SOUTH_WEST:
 501             if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) {
 502                 setLedHsv(ledIndex, color);
 503             }
 504             return;
 505
 506         case QUADRANT_NORTH_WEST:
 507             if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) {
 508                 setLedHsv(ledIndex, color);
 509             }
 510             return;
 511     }
 512 }
 513
 514 void applyLedModeLayer(void)
 515 {
 516     const ledConfig_t *ledConfig;
 517
 518     uint8_t ledIndex;
 519     for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {
 520
 521         ledConfig = &ledConfigs[ledIndex];
 522
 523         setLedHsv(ledIndex, &hsv_black);
 524
 525         if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) {
 526             if (ledConfig->flags & LED_FUNCTION_ARM_STATE) {
 527                 if (!ARMING_FLAG(ARMED)) {
 528                     setLedHsv(ledIndex, &hsv_green);
 529                 } else {
 530                     setLedHsv(ledIndex, &hsv_blue);
 531                 }
 532             }
 533             continue;
 534         }
 535
 536         applyDirectionalModeColor(ledIndex, ledConfig, &orientationModeColors);
 537
 538         if (FLIGHT_MODE(HEADFREE_MODE)) {
 539             applyDirectionalModeColor(ledIndex, ledConfig, &headfreeModeColors);
 540 #ifdef MAG
 541         } else if (FLIGHT_MODE(MAG_MODE)) {
 542             applyDirectionalModeColor(ledIndex, ledConfig, &magModeColors);
 543 #endif
 544 #ifdef BARO
 545         } else if (FLIGHT_MODE(BARO_MODE)) {
 546             applyDirectionalModeColor(ledIndex, ledConfig, &baroModeColors);
 547 #endif
 548         } else if (FLIGHT_MODE(HORIZON_MODE)) {
 549             applyDirectionalModeColor(ledIndex, ledConfig, &horizonModeColors);
 550         } else if (FLIGHT_MODE(ANGLE_MODE)) {
 551             applyDirectionalModeColor(ledIndex, ledConfig, &angleModeColors);
 552         }
 553     }
 554 }
 555
 556 typedef enum {
 557     WARNING_FLAG_NONE = 0,
 558     WARNING_FLAG_LOW_BATTERY = (1 << 0),
 559     WARNING_FLAG_FAILSAFE = (1 << 1),
 560     WARNING_FLAG_ARMING_DISABLED = (1 << 2)
 561 } warningFlags_e;
 562
 563 static uint8_t warningFlags = WARNING_FLAG_NONE;
 564
 565 void applyLedWarningLayer(uint8_t updateNow)
 566 {
 567     const ledConfig_t *ledConfig;
 568     uint8_t ledIndex;
 569     static uint8_t warningFlashCounter = 0;
 570
 571     if (updateNow && warningFlashCounter == 0) {
 572         warningFlags = WARNING_FLAG_NONE;
 573         if (feature(FEATURE_VBAT) && shouldSoundBatteryAlarm()) {
 574             warningFlags |= WARNING_FLAG_LOW_BATTERY;
 575         }
 576         if (failsafe->vTable->hasTimerElapsed()) {
 577             warningFlags |= WARNING_FLAG_FAILSAFE;
 578         }
 579         if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) {
 580             warningFlags |= WARNING_FLAG_ARMING_DISABLED;
 581         }
 582     }
 583
 584     if (warningFlags || warningFlashCounter > 0) {
 585         const hsvColor_t *warningColor = &hsv_black;
 586
 587         if ((warningFlashCounter & 1) == 0) {
 588             if (warningFlashCounter < 4 && (warningFlags & WARNING_FLAG_ARMING_DISABLED)) {
 589                 warningColor = &hsv_green;
 590             }
 591             if (warningFlashCounter >= 4 && warningFlashCounter < 12 && (warningFlags & WARNING_FLAG_LOW_BATTERY)) {
 592                 warningColor = &hsv_red;
 593             }
 594             if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) {
 595                 warningColor = &hsv_yellow;
 596             }
 597         }  else {
 598             if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) {
 599                 warningColor = &hsv_blue;
 600             }
 601         }
 602
 603         for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {
 604
 605             ledConfig = &ledConfigs[ledIndex];
 606
 607             if (!(ledConfig->flags & LED_FUNCTION_WARNING)) {
 608                 continue;
 609             }
 610             setLedHsv(ledIndex, warningColor);
 611         }
 612     }
 613
 614     if (updateNow && (warningFlags || warningFlashCounter)) {
 615         warningFlashCounter++;
 616         if (warningFlashCounter == 20) {
 617             warningFlashCounter = 0;
 618         }
 619     }
 620 }
 621
 622 #define INDICATOR_DEADBAND 25
 623
 624 void applyLedIndicatorLayer(uint8_t indicatorFlashState)
 625 {
 626     const ledConfig_t *ledConfig;
 627     static const hsvColor_t *flashColor;
 628
 629
 630     if (indicatorFlashState == 0) {
 631         flashColor = &hsv_orange;
 632     } else {
 633         flashColor = &hsv_black;
 634     }
 635
 636
 637     uint8_t ledIndex;
 638     for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {
 639
 640         ledConfig = &ledConfigs[ledIndex];
 641
 642         if (!(ledConfig->flags & LED_FUNCTION_INDICATOR)) {
 643             continue;
 644         }
 645
 646         if (rcCommand[ROLL] > INDICATOR_DEADBAND) {
 647             applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor);
 648             applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor);
 649         }
 650
 651         if (rcCommand[ROLL] < -INDICATOR_DEADBAND) {
 652             applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor);
 653             applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor);
 654         }
 655
 656         if (rcCommand[PITCH] > INDICATOR_DEADBAND) {
 657             applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor);
 658             applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor);
 659         }
 660
 661         if (rcCommand[PITCH] < -INDICATOR_DEADBAND) {
 662             applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor);
 663             applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor);
 664         }
 665     }
 666 }
 667
 668 void applyLedThrottleLayer()
 669 {
 670     const ledConfig_t *ledConfig;
 671     hsvColor_t color;
 672
 673     uint8_t ledIndex;
 674     for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {
 675         ledConfig = &ledConfigs[ledIndex];
 676         if(!(ledConfig->flags & LED_FUNCTION_THROTTLE)) {
 677             continue;
 678         }
 679
 680         getLedHsv(ledIndex, &color);
 681
 682         int scaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, -60, +60);
 683         scaled += HSV_HUE_MAX;
 684         color.h = scaled % HSV_HUE_MAX;
 685         setLedHsv(ledIndex, &color);
 686     }
 687 }
 688
 689 static uint8_t frameCounter = 0;
 690
 691 static uint8_t previousRow;
 692 static uint8_t currentRow;
 693 static uint8_t nextRow;
 694
 695 static void updateLedAnimationState(void)
 696 {
 697     uint8_t animationFrames = ledGridHeight;
 698
 699     previousRow = (frameCounter + animationFrames - 1) % animationFrames;
 700     currentRow = frameCounter;
 701     nextRow = (frameCounter + 1) % animationFrames;
 702
 703     frameCounter = (frameCounter + 1) % animationFrames;
 704 }
 705
 706 #ifdef USE_LED_ANIMATION
 707 static void applyLedAnimationLayer(void)
 708 {
 709     const ledConfig_t *ledConfig;
 710
 711     if (ARMING_FLAG(ARMED)) {
 712         return;
 713     }
 714
 715     uint8_t ledIndex;
 716     for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {
 717
 718         ledConfig = &ledConfigs[ledIndex];
 719
 720         if (GET_LED_Y(ledConfig) == previousRow) {
 721             setLedHsv(ledIndex, &white);
 722             setLedBrightness(ledIndex, 50);
 723
 724         } else if (GET_LED_Y(ledConfig) == currentRow) {
 725             setLedHsv(ledIndex, &white);
 726         } else if (GET_LED_Y(ledConfig) == nextRow) {
 727             setLedBrightness(ledIndex, 50);
 728         }
 729     }
 730 }
 731 #endif
 732
 733 void updateLedStrip(void)
 734 {
 735     if (!(ledStripInitialised && isWS2811LedStripReady())) {
 736         return;
 737     }
 738
 739     uint32_t now = micros();
 740
 741     bool animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L;
 742     bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L;
 743     bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L;
 744
 745     if (!(warningFlashNow || indicatorFlashNow || animationUpdateNow)) {
 746         return;
 747     }
 748
 749     static uint8_t indicatorFlashState = 0;
 750
 751     // LAYER 1
 752     applyLedModeLayer();
 753     applyLedThrottleLayer();
 754
 755     // LAYER 2
 756
 757     if (warningFlashNow) {
 758         nextWarningFlashAt = now + LED_STRIP_10HZ;
 759     }
 760     applyLedWarningLayer(warningFlashNow);
 761
 762     // LAYER 3
 763
 764     if (indicatorFlashNow) {
 765
 766         uint8_t rollScale = abs(rcCommand[ROLL]) / 50;
 767         uint8_t pitchScale = abs(rcCommand[PITCH]) / 50;
 768         uint8_t scale = max(rollScale, pitchScale);
 769         nextIndicatorFlashAt = now + (LED_STRIP_5HZ / max(1, scale));
 770
 771         if (indicatorFlashState == 0) {
 772             indicatorFlashState = 1;
 773         } else {
 774             indicatorFlashState = 0;
 775         }
 776     }
 777
 778     applyLedIndicatorLayer(indicatorFlashState);
 779
 780     if (animationUpdateNow) {
 781         nextAnimationUpdateAt = now + LED_STRIP_20HZ;
 782         updateLedAnimationState();
 783     }
 784
 785 #ifdef USE_LED_ANIMATION
 786     applyLedAnimationLayer();
 787 #endif
 788     ws2811UpdateStrip();
 789 }
 790
 791 bool parseColor(uint8_t index, char *colorConfig)
 792 {
 793     char *remainingCharacters = colorConfig;
 794
 795     hsvColor_t *color = &colors[index];
 796
 797     bool ok = true;
 798
 799     uint8_t componentIndex;
 800     for (componentIndex = 0; ok && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) {
 801         uint16_t val = atoi(remainingCharacters);
 802         switch (componentIndex) {
 803             case HSV_HUE:
 804                 if (val > HSV_HUE_MAX) {
 805                     ok = false;
 806                     continue;
 807                 }
 808                 colors[index].h = val;
 809                 break;
 810             case HSV_SATURATION:
 811                 if (val > HSV_SATURATION_MAX) {
 812                     ok = false;
 813                     continue;
 814                 }
 815                 colors[index].s = (uint8_t)val;
 816                 break;
 817             case HSV_VALUE:
 818                 if (val > HSV_VALUE_MAX) {
 819                     ok = false;
 820                     continue;
 821                 }
 822                 colors[index].v = (uint8_t)val;
 823                 break;
 824         }
 825         remainingCharacters = strstr(remainingCharacters, ",");
 826         if (remainingCharacters) {
 827             remainingCharacters++;
 828         } else {
 829             if (componentIndex < 2) {
 830                 ok = false;
 831             }
 832         }
 833     }
 834
 835     if (!ok) {
 836         memset(color, 0, sizeof(hsvColor_t));
 837     }
 838
 839     return ok;
 840 }
 841
 842 void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount)
 843 {
 844     memset(colors, 0, colorCount * sizeof(colors));
 845     for (uint8_t colorIndex = 0; colorIndex < colorCount && colorIndex < (sizeof(defaultColors) / sizeof(defaultColors[0])); colorIndex++) {
 846         *colors++ = *defaultColors[colorIndex];
 847     }
 848 }
 849
 850 void applyDefaultLedStripConfig(ledConfig_t *ledConfigs)
 851 {
 852     memset(ledConfigs, 0, MAX_LED_STRIP_LENGTH * sizeof(ledConfig_t));
 853     memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig));
 854
 855     reevalulateLedConfig();
 856 }
 857
 858 void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse)
 859 {
 860     ledConfigs = ledConfigsToUse;
 861     colors = colorsToUse;
 862     failsafe = failsafeToUse;
 863     ledStripInitialised = false;
 864 }
 865
 866 void ledStripEnable(void)
 867 {
 868     reevalulateLedConfig();
 869     ledStripInitialised = true;
 870
 871     ws2811LedStripInit();
 872 }
 873 #endif