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