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