src/main/io/ledstrip.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdlib.h>
  23 #include <stdint.h>
  24 #include <string.h>
  25 #include <stdarg.h>
  26 #include <math.h>
  27
  28 #include "platform.h"
  29
  30 #ifdef USE_LED_STRIP
  31
  32 #include "build/build_config.h"
  33
  34 #include "common/axis.h"
  35 #include "common/color.h"
  36 #include "common/maths.h"
  37 #include "common/printf.h"
  38 #include "common/typeconversion.h"
  39 #include "common/utils.h"
  40
  41 #include "config/feature.h"
  42 #include "pg/pg.h"
  43 #include "pg/pg_ids.h"
  44 #include "pg/rx.h"
  45
  46 #include "drivers/light_ws2811strip.h"
  47 #include "drivers/serial.h"
  48 #include "drivers/time.h"
  49 #include "drivers/vtx_common.h"
  50
  51 #include "config/config.h"
  52 #include "fc/core.h"
  53 #include "fc/rc_controls.h"
  54 #include "fc/rc_modes.h"
  55 #include "fc/runtime_config.h"
  56
  57 #include "flight/failsafe.h"
  58 #include "flight/imu.h"
  59 #include "flight/mixer.h"
  60 #include "flight/pid.h"
  61 #include "flight/servos.h"
  62
  63 #include "io/beeper.h"
  64 #include "io/gimbal.h"
  65 #include "io/gps.h"
  66 #include "io/ledstrip.h"
  67 #include "io/serial.h"
  68
  69 #include "rx/rx.h"
  70
  71 #include "scheduler/scheduler.h"
  72
  73 #include "sensors/acceleration.h"
  74 #include "sensors/barometer.h"
  75 #include "sensors/battery.h"
  76 #include "sensors/boardalignment.h"
  77 #include "sensors/gyro.h"
  78 #include "sensors/sensors.h"
  79
  80 #include "telemetry/telemetry.h"
  81
  82 #define COLOR_UNDEFINED 255
  83
  84 static bool ledStripEnabled = false;
  85 static uint8_t previousProfileColorIndex = COLOR_UNDEFINED;
  86
  87 #define HZ_TO_US(hz) ((int32_t)((1000 * 1000) / (hz)))
  88
  89 #define MAX_TIMER_DELAY (5 * 1000 * 1000)
  90
  91 #define PROFILE_COLOR_UPDATE_INTERVAL_US 1e6  // normally updates when color changes but this is a 1 second forced update
  92
  93 #define VISUAL_BEEPER_COLOR COLOR_WHITE
  94
  95 #define BEACON_FAILSAFE_PERIOD_US 250      // 2Hz
  96 #define BEACON_FAILSAFE_ON_PERCENT 50      // 50% duty cycle
  97
  98 const hsvColor_t hsv[] = {
  99     //                        H    S    V
 100     [COLOR_BLACK] =        {  0,   0,   0},
 101     [COLOR_WHITE] =        {  0, 255, 255},
 102     [COLOR_RED] =          {  0,   0, 255},
 103     [COLOR_ORANGE] =       { 30,   0, 255},
 104     [COLOR_YELLOW] =       { 60,   0, 255},
 105     [COLOR_LIME_GREEN] =   { 90,   0, 255},
 106     [COLOR_GREEN] =        {120,   0, 255},
 107     [COLOR_MINT_GREEN] =   {150,   0, 255},
 108     [COLOR_CYAN] =         {180,   0, 255},
 109     [COLOR_LIGHT_BLUE] =   {210,   0, 255},
 110     [COLOR_BLUE] =         {240,   0, 255},
 111     [COLOR_DARK_VIOLET] =  {270,   0, 255},
 112     [COLOR_MAGENTA] =      {300,   0, 255},
 113     [COLOR_DEEP_PINK] =    {330,   0, 255},
 114 };
 115 // macro to save typing on default colors
 116 #define HSV(color) (hsv[COLOR_ ## color])
 117
 118 PG_REGISTER_WITH_RESET_FN(ledStripConfig_t, ledStripConfig, PG_LED_STRIP_CONFIG, 2);
 119
 120 void pgResetFn_ledStripConfig(ledStripConfig_t *ledStripConfig)
 121 {
 122     ledStripConfig->ledstrip_visual_beeper = 0;
 123 #ifdef USE_LED_STRIP_STATUS_MODE
 124     ledStripConfig->ledstrip_profile = LED_PROFILE_STATUS;
 125 #else
 126     ledStripConfig->ledstrip_profile = LED_PROFILE_RACE;
 127 #endif
 128     ledStripConfig->ledstrip_race_color = COLOR_ORANGE;
 129     ledStripConfig->ledstrip_beacon_color = COLOR_WHITE;
 130     ledStripConfig->ledstrip_beacon_period_ms = 500;    // 0.5 second (2hz)
 131     ledStripConfig->ledstrip_beacon_percent = 50;       // 50% duty cycle
 132     ledStripConfig->ledstrip_beacon_armed_only = false; // blink always
 133     ledStripConfig->ledstrip_visual_beeper_color = VISUAL_BEEPER_COLOR;
 134     ledStripConfig->ledstrip_brightness = 100;
 135 #ifndef UNIT_TEST
 136     ledStripConfig->ioTag = timerioTagGetByUsage(TIM_USE_LED, 0);
 137 #endif
 138 }
 139
 140 #ifdef USE_LED_STRIP_STATUS_MODE
 141
 142 #if LED_MAX_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH
 143 # error "Led strip length must match driver"
 144 #endif
 145
 146 const hsvColor_t *colors;
 147 const modeColorIndexes_t *modeColors;
 148 specialColorIndexes_t specialColors;
 149
 150 STATIC_UNIT_TESTED uint8_t ledGridRows;
 151 // grid offsets
 152 STATIC_UNIT_TESTED int8_t highestYValueForNorth;
 153 STATIC_UNIT_TESTED int8_t lowestYValueForSouth;
 154 STATIC_UNIT_TESTED int8_t highestXValueForWest;
 155 STATIC_UNIT_TESTED int8_t lowestXValueForEast;
 156
 157 STATIC_UNIT_TESTED ledCounts_t ledCounts;
 158
 159 static const modeColorIndexes_t defaultModeColors[] = {
 160     //                          NORTH             EAST               SOUTH            WEST             UP          DOWN
 161     [LED_MODE_ORIENTATION] = {{ COLOR_WHITE,      COLOR_DARK_VIOLET, COLOR_RED,       COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 162     [LED_MODE_HEADFREE]    = {{ COLOR_LIME_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE,    COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 163     [LED_MODE_HORIZON]     = {{ COLOR_BLUE,       COLOR_DARK_VIOLET, COLOR_YELLOW,    COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 164     [LED_MODE_ANGLE]       = {{ COLOR_CYAN,       COLOR_DARK_VIOLET, COLOR_YELLOW,    COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 165     [LED_MODE_MAG]         = {{ COLOR_MINT_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE,    COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 166 };
 167
 168 static const specialColorIndexes_t defaultSpecialColors[] = {
 169     {{ [LED_SCOLOR_DISARMED]        = COLOR_GREEN,
 170        [LED_SCOLOR_ARMED]           = COLOR_BLUE,
 171        [LED_SCOLOR_ANIMATION]       = COLOR_WHITE,
 172        [LED_SCOLOR_BACKGROUND]      = COLOR_BLACK,
 173        [LED_SCOLOR_BLINKBACKGROUND] = COLOR_BLACK,
 174        [LED_SCOLOR_GPSNOSATS]       = COLOR_RED,
 175        [LED_SCOLOR_GPSNOLOCK]       = COLOR_ORANGE,
 176        [LED_SCOLOR_GPSLOCKED]       = COLOR_GREEN,
 177     }}
 178 };
 179
 180 PG_REGISTER_WITH_RESET_FN(ledStripStatusModeConfig_t, ledStripStatusModeConfig, PG_LED_STRIP_STATUS_MODE_CONFIG, 0);
 181
 182 void pgResetFn_ledStripStatusModeConfig(ledStripStatusModeConfig_t *ledStripStatusModeConfig)
 183 {
 184     memset(ledStripStatusModeConfig->ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t));
 185     // copy hsv colors as default
 186     memset(ledStripStatusModeConfig->colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t));
 187     STATIC_ASSERT(LED_CONFIGURABLE_COLOR_COUNT >= ARRAYLEN(hsv), LED_CONFIGURABLE_COLOR_COUNT_invalid);
 188     for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) {
 189         ledStripStatusModeConfig->colors[colorIndex] = hsv[colorIndex];
 190     }
 191     memcpy_fn(&ledStripStatusModeConfig->modeColors, &defaultModeColors, sizeof(defaultModeColors));
 192     memcpy_fn(&ledStripStatusModeConfig->specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors));
 193     ledStripStatusModeConfig->ledstrip_aux_channel = THROTTLE;
 194 }
 195
 196 #define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off
 197 #define ROTATION_SEQUENCE_LED_WIDTH 2 // 2 on
 198
 199 static void updateLedRingCounts(void)
 200 {
 201     int seqLen;
 202     // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds
 203     if ((ledCounts.ring % ROTATION_SEQUENCE_LED_COUNT) == 0) {
 204         seqLen = ROTATION_SEQUENCE_LED_COUNT;
 205     } else {
 206         seqLen = ledCounts.ring;
 207         // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds
 208         // TODO - improve partitioning (15 leds -> 3x5)
 209         while ((seqLen > ROTATION_SEQUENCE_LED_COUNT) && ((seqLen % 2) == 0)) {
 210             seqLen /= 2;
 211         }
 212     }
 213     ledCounts.ringSeqLen = seqLen;
 214 }
 215
 216 STATIC_UNIT_TESTED void updateDimensions(void)
 217 {
 218     int maxX = 0;
 219     int minX = LED_XY_MASK;
 220     int maxY = 0;
 221     int minY = LED_XY_MASK;
 222
 223     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 224         const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
 225
 226         int ledX = ledGetX(ledConfig);
 227         maxX = MAX(ledX, maxX);
 228         minX = MIN(ledX, minX);
 229         int ledY = ledGetY(ledConfig);
 230         maxY = MAX(ledY, maxY);
 231         minY = MIN(ledY, minY);
 232     }
 233
 234     ledGridRows = maxY - minY + 1;
 235
 236     if (minX < maxX) {
 237         lowestXValueForEast = (minX + maxX) / 2 + 1;
 238         highestXValueForWest = (minX + maxX - 1) / 2;
 239     } else {
 240         lowestXValueForEast = LED_XY_MASK / 2;
 241         highestXValueForWest = lowestXValueForEast - 1;
 242     }
 243     if (minY < maxY) {
 244         lowestYValueForSouth = (minY + maxY) / 2 + 1;
 245         highestYValueForNorth = (minY + maxY - 1) / 2;
 246     } else {
 247         lowestYValueForSouth = LED_XY_MASK / 2;
 248         highestYValueForNorth = lowestYValueForSouth - 1;
 249     }
 250
 251 }
 252
 253 STATIC_UNIT_TESTED void updateLedCount(void)
 254 {
 255     int count = 0, countRing = 0, countScanner= 0;
 256
 257     for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) {
 258         const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
 259
 260         if (!(*ledConfig))
 261             break;
 262
 263         count++;
 264
 265         if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING)
 266             countRing++;
 267
 268         if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER))
 269             countScanner++;
 270     }
 271
 272     ledCounts.count = count;
 273     ledCounts.ring = countRing;
 274     ledCounts.larson = countScanner;
 275     setUsedLedCount(ledCounts.count);
 276 }
 277
 278 void reevaluateLedConfig(void)
 279 {
 280     updateLedCount();
 281     updateDimensions();
 282     updateLedRingCounts();
 283     updateRequiredOverlay();
 284 }
 285
 286 // get specialColor by index
 287 static const hsvColor_t* getSC(ledSpecialColorIds_e index)
 288 {
 289     return &ledStripStatusModeConfig()->colors[ledStripStatusModeConfig()->specialColors.color[index]];
 290 }
 291
 292 static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' };
 293 static const char baseFunctionCodes[LED_BASEFUNCTION_COUNT]   = { 'C', 'F', 'A', 'L', 'S', 'G', 'R' };
 294 static const char overlayCodes[LED_OVERLAY_COUNT]   = { 'T', 'O', 'B', 'V', 'I', 'W' };
 295
 296 #define CHUNK_BUFFER_SIZE 11
 297 bool parseLedStripConfig(int ledIndex, const char *config)
 298 {
 299     if (ledIndex >= LED_MAX_STRIP_LENGTH)
 300         return false;
 301
 302     enum parseState_e {
 303         X_COORDINATE,
 304         Y_COORDINATE,
 305         DIRECTIONS,
 306         FUNCTIONS,
 307         RING_COLORS,
 308         PARSE_STATE_COUNT
 309     };
 310     static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':', ':', '\0'};
 311
 312     ledConfig_t *ledConfig = &ledStripStatusModeConfigMutable()->ledConfigs[ledIndex];
 313     memset(ledConfig, 0, sizeof(ledConfig_t));
 314
 315     int x = 0, y = 0, color = 0;   // initialize to prevent warnings
 316     int baseFunction = 0;
 317     int overlay_flags = 0;
 318     int direction_flags = 0;
 319
 320     for (enum parseState_e parseState = 0; parseState < PARSE_STATE_COUNT; parseState++) {
 321         char chunk[CHUNK_BUFFER_SIZE];
 322         {
 323             char chunkSeparator = chunkSeparators[parseState];
 324             int chunkIndex = 0;
 325             while (*config  && *config != chunkSeparator && chunkIndex < (CHUNK_BUFFER_SIZE - 1)) {
 326                 chunk[chunkIndex++] = *config++;
 327             }
 328             chunk[chunkIndex++] = 0; // zero-terminate chunk
 329             if (*config != chunkSeparator) {
 330                 return false;
 331             }
 332             config++;   // skip separator
 333         }
 334         switch (parseState) {
 335             case X_COORDINATE:
 336                 x = atoi(chunk);
 337                 break;
 338             case Y_COORDINATE:
 339                 y = atoi(chunk);
 340                 break;
 341             case DIRECTIONS:
 342                 for (char *ch = chunk; *ch; ch++) {
 343                     for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) {
 344                         if (directionCodes[dir] == *ch) {
 345                             direction_flags |= LED_FLAG_DIRECTION(dir);
 346                             break;
 347                         }
 348                     }
 349                 }
 350                 break;
 351             case FUNCTIONS:
 352                 for (char *ch = chunk; *ch; ch++) {
 353                     for (ledBaseFunctionId_e fn = 0; fn < LED_BASEFUNCTION_COUNT; fn++) {
 354                         if (baseFunctionCodes[fn] == *ch) {
 355                             baseFunction = fn;
 356                             break;
 357                         }
 358                     }
 359
 360                     for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) {
 361                         if (overlayCodes[ol] == *ch) {
 362                             overlay_flags |= LED_FLAG_OVERLAY(ol);
 363                             break;
 364                         }
 365                     }
 366                 }
 367                 break;
 368             case RING_COLORS:
 369                 color = atoi(chunk);
 370                 if (color >= LED_CONFIGURABLE_COLOR_COUNT)
 371                     color = 0;
 372                 break;
 373             case PARSE_STATE_COUNT:; // prevent warning
 374         }
 375     }
 376
 377     *ledConfig = DEFINE_LED(x, y, color, direction_flags, baseFunction, overlay_flags, 0);
 378
 379     reevaluateLedConfig();
 380
 381     return true;
 382 }
 383
 384 void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t bufferSize)
 385 {
 386     char directions[LED_DIRECTION_COUNT + 1];
 387     char baseFunctionOverlays[LED_OVERLAY_COUNT + 2];
 388
 389     memset(ledConfigBuffer, 0, bufferSize);
 390
 391     char *dptr = directions;
 392     for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) {
 393         if (ledGetDirectionBit(ledConfig, dir)) {
 394             *dptr++ = directionCodes[dir];
 395         }
 396     }
 397     *dptr = 0;
 398
 399     char *fptr = baseFunctionOverlays;
 400     *fptr++ = baseFunctionCodes[ledGetFunction(ledConfig)];
 401
 402     for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) {
 403         if (ledGetOverlayBit(ledConfig, ol)) {
 404             *fptr++ = overlayCodes[ol];
 405         }
 406     }
 407     *fptr = 0;
 408
 409     // TODO - check buffer length
 410     tfp_sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig));
 411 }
 412
 413 typedef enum {
 414     // the ordering is important, see below how NSEW is mapped to  NE/SE/NW/SW
 415     QUADRANT_NORTH      = 1 << 0,
 416     QUADRANT_SOUTH      = 1 << 1,
 417     QUADRANT_EAST       = 1 << 2,
 418     QUADRANT_WEST       = 1 << 3,
 419 } quadrant_e;
 420
 421 static quadrant_e getLedQuadrant(const int ledIndex)
 422 {
 423     const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
 424
 425     int x = ledGetX(ledConfig);
 426     int y = ledGetY(ledConfig);
 427
 428     int quad = 0;
 429     if (y <= highestYValueForNorth)
 430         quad |= QUADRANT_NORTH;
 431     else if (y >= lowestYValueForSouth)
 432         quad |= QUADRANT_SOUTH;
 433     if (x >= lowestXValueForEast)
 434         quad |= QUADRANT_EAST;
 435     else if (x <= highestXValueForWest)
 436         quad |= QUADRANT_WEST;
 437
 438     return quad;
 439 }
 440
 441 static const hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIndexes_t *modeColors)
 442 {
 443     const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
 444     const int ledDirection = ledGetDirection(ledConfig);
 445
 446     for (unsigned i = 0; i < LED_DIRECTION_COUNT; i++) {
 447         if (ledDirection & (1 << i)) {
 448             return &ledStripStatusModeConfig()->colors[modeColors->color[i]];
 449         }
 450     }
 451
 452     return NULL;
 453 }
 454
 455 // map flight mode to led mode, in order of priority
 456 // flightMode == 0 is always active
 457 static const struct {
 458     uint16_t flightMode;
 459     uint8_t ledMode;
 460 } flightModeToLed[] = {
 461     {HEADFREE_MODE, LED_MODE_HEADFREE},
 462 #ifdef USE_MAG
 463     {MAG_MODE,      LED_MODE_MAG},
 464 #endif
 465     {HORIZON_MODE,  LED_MODE_HORIZON},
 466     {ANGLE_MODE,    LED_MODE_ANGLE},
 467     {0,             LED_MODE_ORIENTATION},
 468 };
 469
 470 static void applyLedFixedLayers(void)
 471 {
 472     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 473         const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
 474         hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND);
 475
 476         int fn = ledGetFunction(ledConfig);
 477         int hOffset = HSV_HUE_MAX + 1;
 478
 479         switch (fn) {
 480         case LED_FUNCTION_COLOR:
 481             color = ledStripStatusModeConfig()->colors[ledGetColor(ledConfig)];
 482
 483             hsvColor_t nextColor = ledStripStatusModeConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT];
 484             hsvColor_t previousColor = ledStripStatusModeConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT];
 485
 486             if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) {   //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor
 487                 const int auxInput = rcData[ledStripStatusModeConfig()->ledstrip_aux_channel];
 488                 int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2;
 489                 if (auxInput < centerPWM) {
 490                     color.h = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.h, color.h);
 491                     color.s = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.s, color.s);
 492                     color.v = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.v, color.v);
 493                 } else {
 494                     color.h = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.h, nextColor.h);
 495                     color.s = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.s, nextColor.s);
 496                     color.v = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.v, nextColor.v);
 497                 }
 498             }
 499
 500             break;
 501
 502         case LED_FUNCTION_FLIGHT_MODE:
 503             for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++)
 504                 if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) {
 505                     const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripStatusModeConfig()->modeColors[flightModeToLed[i].ledMode]);
 506                     if (directionalColor) {
 507                         color = *directionalColor;
 508                     }
 509
 510                     break; // stop on first match
 511                 }
 512             break;
 513
 514         case LED_FUNCTION_ARM_STATE:
 515             color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED);
 516             break;
 517
 518         case LED_FUNCTION_BATTERY:
 519             color = HSV(RED);
 520             hOffset += MAX(scaleRange(calculateBatteryPercentageRemaining(), 0, 100, -30, 120), 0);
 521             break;
 522
 523         case LED_FUNCTION_RSSI:
 524             color = HSV(RED);
 525             hOffset += MAX(scaleRange(getRssiPercent(), 0, 100, -30, 120), 0);
 526             break;
 527
 528         default:
 529             break;
 530         }
 531
 532         if ((fn != LED_FUNCTION_COLOR) && ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) {
 533             const int auxInput = rcData[ledStripStatusModeConfig()->ledstrip_aux_channel];
 534             hOffset += scaleRange(auxInput, PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1);
 535         }
 536
 537         color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1);
 538         setLedHsv(ledIndex, &color);
 539     }
 540 }
 541
 542 static void applyLedHsv(uint32_t mask, const hsvColor_t *color)
 543 {
 544     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 545         const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
 546         if ((*ledConfig & mask) == mask)
 547             setLedHsv(ledIndex, color);
 548     }
 549 }
 550
 551 typedef enum {
 552     WARNING_ARMING_DISABLED,
 553     WARNING_LOW_BATTERY,
 554     WARNING_FAILSAFE,
 555     WARNING_CRASH_FLIP_ACTIVE,
 556 } warningFlags_e;
 557
 558 static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
 559 {
 560     static uint8_t warningFlashCounter = 0;
 561     static uint8_t warningFlags = 0;          // non-zero during blinks
 562
 563     if (updateNow) {
 564         // keep counter running, so it stays in sync with blink
 565         warningFlashCounter++;
 566         warningFlashCounter &= 0xF;
 567
 568         if (warningFlashCounter == 0) {      // update when old flags was processed
 569             warningFlags = 0;
 570             if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryState() != BATTERY_OK) {
 571                 warningFlags |= 1 << WARNING_LOW_BATTERY;
 572             }
 573             if (failsafeIsActive()) {
 574                 warningFlags |= 1 << WARNING_FAILSAFE;
 575             }
 576             if (!ARMING_FLAG(ARMED) && isArmingDisabled()) {
 577                 warningFlags |= 1 << WARNING_ARMING_DISABLED;
 578             }
 579             if (isFlipOverAfterCrashActive()) {
 580                 warningFlags |= 1 << WARNING_CRASH_FLIP_ACTIVE;
 581             }
 582         }
 583         *timer += HZ_TO_US(10);
 584     }
 585
 586     const hsvColor_t *warningColor = NULL;
 587
 588     if (warningFlags) {
 589         bool colorOn = (warningFlashCounter % 2) == 0;   // w_w_
 590         warningFlags_e warningId = warningFlashCounter / 4;
 591         if (warningFlags & (1 << warningId)) {
 592             switch (warningId) {
 593                 case WARNING_ARMING_DISABLED:
 594                     warningColor = colorOn ? &HSV(GREEN) : &HSV(BLACK);
 595                     break;
 596                 case WARNING_CRASH_FLIP_ACTIVE:
 597                     warningColor = colorOn ? &HSV(MAGENTA) : &HSV(BLACK);
 598                     break;
 599                 case WARNING_LOW_BATTERY:
 600                     warningColor = colorOn ? &HSV(RED) : &HSV(BLACK);
 601                     break;
 602                 case WARNING_FAILSAFE:
 603                     warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE);
 604                     break;
 605                 default:;
 606             }
 607         }
 608     } else {
 609         if (isBeeperOn()) {
 610             warningColor = &hsv[ledStripConfig()->ledstrip_visual_beeper_color];
 611         }
 612     }
 613
 614     if (warningColor) {
 615         applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor);
 616     }
 617 }
 618
 619 #ifdef USE_VTX_COMMON
 620 static void applyLedVtxLayer(bool updateNow, timeUs_t *timer)
 621 {
 622     static uint16_t frequency = 0;
 623     static uint8_t power = 255;
 624     static unsigned vtxStatus = UINT32_MAX;
 625     static uint8_t showSettings = false;
 626     static uint16_t lastCheck = 0;
 627     static bool blink = false;
 628
 629     const vtxDevice_t *vtxDevice = vtxCommonDevice();
 630     if (!vtxDevice) {
 631         return;
 632     }
 633
 634     uint8_t band = 255, channel = 255;
 635     uint16_t check = 0;
 636
 637     if (updateNow) {
 638         // keep counter running, so it stays in sync with vtx
 639         vtxCommonGetBandAndChannel(vtxDevice, &band, &channel);
 640         vtxCommonGetPowerIndex(vtxDevice, &power);
 641         vtxCommonGetStatus(vtxDevice, &vtxStatus);
 642
 643         frequency = vtxCommonLookupFrequency(vtxDevice, band, channel);
 644
 645         // check if last vtx values have changed.
 646         check = ((vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0) + (power << 1) + (band << 4) + (channel << 8);
 647         if (!showSettings && check != lastCheck) {
 648             // display settings for 3 seconds.
 649             showSettings = 15;
 650         }
 651         lastCheck = check; // quick way to check if any settings changed.
 652
 653         if (showSettings) {
 654             showSettings--;
 655         }
 656         blink = !blink;
 657         *timer += HZ_TO_US(5); // check 5 times a second
 658     }
 659
 660     hsvColor_t color = {0, 0, 0};
 661     if (showSettings) { // show settings
 662         uint8_t vtxLedCount = 0;
 663         for (int i = 0; i < ledCounts.count && vtxLedCount < 6; ++i) {
 664             const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i];
 665             if (ledGetOverlayBit(ledConfig, LED_OVERLAY_VTX)) {
 666                 if (vtxLedCount == 0) {
 667                     color.h = HSV(GREEN).h;
 668                     color.s = HSV(GREEN).s;
 669                     color.v = blink ? 15 : 0; // blink received settings
 670                 }
 671                 else if (vtxLedCount > 0 && power >= vtxLedCount && !(vtxStatus & VTX_STATUS_PIT_MODE)) { // show power
 672                     color.h = HSV(ORANGE).h;
 673                     color.s = HSV(ORANGE).s;
 674                     color.v = blink ? 15 : 0; // blink received settings
 675                 }
 676                 else { // turn rest off
 677                     color.h = HSV(BLACK).h;
 678                     color.s = HSV(BLACK).s;
 679                     color.v = HSV(BLACK).v;
 680                 }
 681                 setLedHsv(i, &color);
 682                 ++vtxLedCount;
 683             }
 684         }
 685     }
 686     else { // show frequency
 687         // calculate the VTX color based on frequency
 688         int colorIndex = 0;
 689         if (frequency <= 5672) {
 690             colorIndex = COLOR_WHITE;
 691         } else if (frequency <= 5711) {
 692             colorIndex = COLOR_RED;
 693         } else if (frequency <= 5750) {
 694             colorIndex = COLOR_ORANGE;
 695         } else if (frequency <= 5789) {
 696             colorIndex = COLOR_YELLOW;
 697         } else if (frequency <= 5829) {
 698             colorIndex = COLOR_GREEN;
 699         } else if (frequency <= 5867) {
 700             colorIndex = COLOR_BLUE;
 701         } else if (frequency <= 5906) {
 702             colorIndex = COLOR_DARK_VIOLET;
 703         } else {
 704             colorIndex = COLOR_DEEP_PINK;
 705         }
 706         hsvColor_t color = ledStripStatusModeConfig()->colors[colorIndex];
 707         color.v = (vtxStatus & VTX_STATUS_PIT_MODE) ? (blink ? 15 : 0) : 255; // blink when in pit mode
 708         applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_VTX)), &color);
 709     }
 710 }
 711 #endif
 712
 713 static void applyLedBatteryLayer(bool updateNow, timeUs_t *timer)
 714 {
 715     static bool flash = false;
 716
 717     int timerDelayUs = HZ_TO_US(1);
 718
 719     if (updateNow) {
 720
 721         switch (getBatteryState()) {
 722             case BATTERY_OK:
 723                 flash = true;
 724                 timerDelayUs = HZ_TO_US(1);
 725
 726                 break;
 727             case BATTERY_WARNING:
 728                 flash = !flash;
 729                 timerDelayUs = HZ_TO_US(2);
 730
 731                 break;
 732             default:
 733                 flash = !flash;
 734                 timerDelayUs = HZ_TO_US(8);
 735
 736                 break;
 737         }
 738     }
 739
 740     *timer += timerDelayUs;
 741
 742     if (!flash) {
 743        const hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND);
 744        applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc);
 745     }
 746 }
 747
 748 static void applyLedRssiLayer(bool updateNow, timeUs_t *timer)
 749 {
 750     static bool flash = false;
 751
 752     int timerDelay = HZ_TO_US(1);
 753
 754     if (updateNow) {
 755         int state = getRssiPercent();
 756
 757         if (state > 50) {
 758             flash = true;
 759             timerDelay = HZ_TO_US(1);
 760         } else if (state > 20) {
 761             flash = !flash;
 762             timerDelay = HZ_TO_US(2);
 763         } else {
 764             flash = !flash;
 765             timerDelay = HZ_TO_US(8);
 766         }
 767     }
 768
 769     *timer += timerDelay;
 770
 771     if (!flash) {
 772         const hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND);
 773         applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc);
 774     }
 775 }
 776
 777 #ifdef USE_GPS
 778 static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
 779 {
 780
 781     static uint8_t gpsPauseCounter = 0;
 782     const uint8_t blinkPauseLength = 4;
 783
 784     if (updateNow) {
 785         static uint8_t gpsFlashCounter = 0;
 786         if (gpsPauseCounter > 0) {
 787             gpsPauseCounter--;
 788         } else if (gpsFlashCounter >= gpsSol.numSat) {
 789             gpsFlashCounter = 0;
 790             gpsPauseCounter = blinkPauseLength;
 791         } else {
 792             gpsFlashCounter++;
 793             gpsPauseCounter = 1;
 794         }
 795         *timer += HZ_TO_US(2.5f);
 796     }
 797
 798     const hsvColor_t *gpsColor;
 799
 800     if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) {
 801         gpsColor = getSC(LED_SCOLOR_GPSNOSATS);
 802     } else {
 803         bool colorOn = gpsPauseCounter == 0;  // each interval starts with pause
 804         if (STATE(GPS_FIX)) {
 805             gpsColor = colorOn ? getSC(LED_SCOLOR_GPSLOCKED) : getSC(LED_SCOLOR_BACKGROUND);
 806         } else {
 807             gpsColor = colorOn ? getSC(LED_SCOLOR_GPSNOLOCK) : getSC(LED_SCOLOR_GPSNOSATS);
 808         }
 809     }
 810
 811     applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_GPS), gpsColor);
 812 }
 813 #endif
 814
 815 #define INDICATOR_DEADBAND 25
 816
 817 static void applyLedIndicatorLayer(bool updateNow, timeUs_t *timer)
 818 {
 819     static bool flash = 0;
 820
 821     if (updateNow) {
 822         if (rxIsReceivingSignal()) {
 823             // calculate update frequency
 824             int scale = MAX(fabsf(rcCommand[ROLL]), fabsf(rcCommand[PITCH]));  // 0 - 500
 825             scale = scale - INDICATOR_DEADBAND;  // start increasing frequency right after deadband
 826             *timer += HZ_TO_US(5 + (45 * scale) / (500 - INDICATOR_DEADBAND));   // 5 - 50Hz update, 2.5 - 25Hz blink
 827
 828             flash = !flash;
 829         } else {
 830             *timer += HZ_TO_US(5);
 831         }
 832     }
 833
 834     if (!flash)
 835         return;
 836
 837     const hsvColor_t *flashColor = &HSV(ORANGE); // TODO - use user color?
 838
 839     quadrant_e quadrants = 0;
 840     if (rcCommand[ROLL] > INDICATOR_DEADBAND) {
 841         quadrants |= QUADRANT_EAST;
 842     } else if (rcCommand[ROLL] < -INDICATOR_DEADBAND) {
 843         quadrants |= QUADRANT_WEST;
 844     }
 845     if (rcCommand[PITCH] > INDICATOR_DEADBAND) {
 846         quadrants |= QUADRANT_NORTH;
 847     } else if (rcCommand[PITCH] < -INDICATOR_DEADBAND) {
 848         quadrants |= QUADRANT_SOUTH;
 849     }
 850
 851     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 852         const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
 853         if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) {
 854             if (getLedQuadrant(ledIndex) & quadrants)
 855                 setLedHsv(ledIndex, flashColor);
 856         }
 857     }
 858 }
 859
 860 static void applyLedThrustRingLayer(bool updateNow, timeUs_t *timer)
 861 {
 862     static uint8_t rotationPhase;
 863     int ledRingIndex = 0;
 864
 865     if (updateNow) {
 866         rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1;
 867
 868         const int scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100) : 0;
 869         *timer += HZ_TO_US(5 + (45 * scaledThrottle) / 100);  // 5 - 50Hz update rate
 870     }
 871
 872     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 873         const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
 874         if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) {
 875
 876             bool applyColor;
 877             if (ARMING_FLAG(ARMED)) {
 878                 applyColor = (ledRingIndex + rotationPhase) % ledCounts.ringSeqLen < ROTATION_SEQUENCE_LED_WIDTH;
 879             } else {
 880                 applyColor = !(ledRingIndex % 2); // alternating pattern
 881             }
 882
 883             if (applyColor) {
 884                 const hsvColor_t *ringColor = &ledStripStatusModeConfig()->colors[ledGetColor(ledConfig)];
 885                 setLedHsv(ledIndex, ringColor);
 886             }
 887
 888             ledRingIndex++;
 889         }
 890     }
 891 }
 892
 893 typedef struct larsonParameters_s {
 894     uint8_t currentBrightness;
 895     int8_t currentIndex;
 896     int8_t direction;
 897 } larsonParameters_t;
 898
 899 static int brightnessForLarsonIndex(larsonParameters_t *larsonParameters, uint8_t larsonIndex)
 900 {
 901     int offset = larsonIndex - larsonParameters->currentIndex;
 902     static const int larsonLowValue = 8;
 903
 904     if (ABS(offset) > 1)
 905         return (larsonLowValue);
 906
 907     if (offset == 0)
 908         return (larsonParameters->currentBrightness);
 909
 910     if (larsonParameters->direction == offset) {
 911         return (larsonParameters->currentBrightness - 127);
 912     }
 913
 914     return (255 - larsonParameters->currentBrightness);
 915
 916 }
 917
 918 static void larsonScannerNextStep(larsonParameters_t *larsonParameters, int delta)
 919 {
 920     if (larsonParameters->currentBrightness > (255 - delta)) {
 921         larsonParameters->currentBrightness = 127;
 922         if (larsonParameters->currentIndex >= ledCounts.larson || larsonParameters->currentIndex < 0) {
 923             larsonParameters->direction = -larsonParameters->direction;
 924         }
 925         larsonParameters->currentIndex += larsonParameters->direction;
 926     } else {
 927         larsonParameters->currentBrightness += delta;
 928     }
 929 }
 930
 931 static void applyLarsonScannerLayer(bool updateNow, timeUs_t *timer)
 932 {
 933     static larsonParameters_t larsonParameters = { 0, 0, 1 };
 934
 935     if (updateNow) {
 936         larsonScannerNextStep(&larsonParameters, 15);
 937         *timer += HZ_TO_US(60);
 938     }
 939
 940     int scannerLedIndex = 0;
 941     for (unsigned i = 0; i < ledCounts.count; i++) {
 942
 943         const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i];
 944
 945         if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) {
 946             hsvColor_t ledColor;
 947             getLedHsv(i, &ledColor);
 948             ledColor.v = brightnessForLarsonIndex(&larsonParameters, scannerLedIndex);
 949             setLedHsv(i, &ledColor);
 950             scannerLedIndex++;
 951         }
 952     }
 953 }
 954
 955 // blink twice, then wait ; either always or just when landing
 956 static void applyLedBlinkLayer(bool updateNow, timeUs_t *timer)
 957 {
 958     const uint16_t blinkPattern = 0x8005; // 0b1000000000000101;
 959     static uint16_t blinkMask;
 960
 961     if (updateNow) {
 962         blinkMask = blinkMask >> 1;
 963         if (blinkMask <= 1)
 964             blinkMask = blinkPattern;
 965
 966         *timer += HZ_TO_US(10);
 967     }
 968
 969     bool ledOn = (blinkMask & 1);  // b_b_____...
 970     if (!ledOn) {
 971         for (int i = 0; i < ledCounts.count; ++i) {
 972             const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i];
 973
 974             if (ledGetOverlayBit(ledConfig, LED_OVERLAY_BLINK)) {
 975                 setLedHsv(i, getSC(LED_SCOLOR_BLINKBACKGROUND));
 976             }
 977         }
 978     }
 979 }
 980
 981 // In reverse order of priority
 982 typedef enum {
 983     timBlink,
 984     timLarson,
 985     timRing,
 986     timIndicator,
 987 #ifdef USE_VTX_COMMON
 988     timVtx,
 989 #endif
 990 #ifdef USE_GPS
 991     timGps,
 992 #endif
 993     timBattery,
 994     timRssi,
 995     timWarning,
 996     timTimerCount
 997 } timId_e;
 998
 999 static timeUs_t timerVal[timTimerCount];
1000 static uint16_t disabledTimerMask;
1001
1002 STATIC_ASSERT(timTimerCount <= sizeof(disabledTimerMask) * 8, disabledTimerMask_too_small);
1003
1004 // function to apply layer.
1005 // function must replan self using timer pointer
1006 // when updateNow is true (timer triggered), state must be updated first,
1007 //  before calculating led state. Otherwise update started by different trigger
1008 //  may modify LED state.
1009 typedef void applyLayerFn_timed(bool updateNow, timeUs_t *timer);
1010
1011 static applyLayerFn_timed* layerTable[] = {
1012     [timBlink] = &applyLedBlinkLayer,
1013     [timLarson] = &applyLarsonScannerLayer,
1014     [timBattery] = &applyLedBatteryLayer,
1015     [timRssi] = &applyLedRssiLayer,
1016 #ifdef USE_GPS
1017     [timGps] = &applyLedGpsLayer,
1018 #endif
1019     [timWarning] = &applyLedWarningLayer,
1020 #ifdef USE_VTX_COMMON
1021     [timVtx] = &applyLedVtxLayer,
1022 #endif
1023     [timIndicator] = &applyLedIndicatorLayer,
1024     [timRing] = &applyLedThrustRingLayer
1025 };
1026
1027 bool isOverlayTypeUsed(ledOverlayId_e overlayType)
1028 {
1029     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
1030         const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
1031         if (ledGetOverlayBit(ledConfig, overlayType)) {
1032             return true;
1033         }
1034     }
1035     return false;
1036 }
1037
1038 void updateRequiredOverlay(void)
1039 {
1040     disabledTimerMask = 0;
1041     disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_BLINK) << timBlink;
1042     disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_LARSON_SCANNER) << timLarson;
1043     disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_WARNING) << timWarning;
1044 #ifdef USE_VTX_COMMON
1045     disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_VTX) << timVtx;
1046 #endif
1047     disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_INDICATOR) << timIndicator;
1048 }
1049
1050 static void applyStatusProfile(timeUs_t now) {
1051
1052     // apply all layers; triggered timed functions has to update timers
1053     // test all led timers, setting corresponding bits
1054     uint32_t timActive = 0;
1055     for (timId_e timId = 0; timId < timTimerCount; timId++) {
1056         if (!(disabledTimerMask & (1 << timId))) {
1057             // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
1058             const timeDelta_t delta = cmpTimeUs(now, timerVal[timId]);
1059             // max delay is limited to 5s
1060             if (delta < 0 && delta > -MAX_TIMER_DELAY)
1061                 continue;  // not ready yet
1062             timActive |= 1 << timId;
1063             if (delta >= 100 * 1000 || delta < 0) {
1064                 timerVal[timId] = now;
1065             }
1066         }
1067     }
1068
1069     if (!timActive) {
1070         // Call schedulerIgnoreTaskExecTime() unless data is being processed
1071         schedulerIgnoreTaskExecTime();
1072         return;          // no change this update, keep old state
1073     }
1074
1075     applyLedFixedLayers();
1076     for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) {
1077         uint32_t *timer = &timerVal[timId];
1078         bool updateNow = timActive & (1 << timId);
1079         (*layerTable[timId])(updateNow, timer);
1080     }
1081     ws2811UpdateStrip((ledStripFormatRGB_e) ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness);
1082 }
1083
1084 bool parseColor(int index, const char *colorConfig)
1085 {
1086     const char *remainingCharacters = colorConfig;
1087
1088     hsvColor_t *color = &ledStripStatusModeConfigMutable()->colors[index];
1089
1090     bool result = true;
1091     static const uint16_t hsv_limit[HSV_COLOR_COMPONENT_COUNT] = {
1092         [HSV_HUE] = HSV_HUE_MAX,
1093         [HSV_SATURATION] = HSV_SATURATION_MAX,
1094         [HSV_VALUE] = HSV_VALUE_MAX,
1095     };
1096     for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) {
1097         int val = atoi(remainingCharacters);
1098         if (val > hsv_limit[componentIndex]) {
1099             result = false;
1100             break;
1101         }
1102         switch (componentIndex) {
1103             case HSV_HUE:
1104                 color->h = val;
1105                 break;
1106             case HSV_SATURATION:
1107                 color->s = val;
1108                 break;
1109             case HSV_VALUE:
1110                 color->v = val;
1111                 break;
1112         }
1113         remainingCharacters = strchr(remainingCharacters, ',');
1114         if (remainingCharacters) {
1115             remainingCharacters++;  // skip separator
1116         } else {
1117             if (componentIndex < HSV_COLOR_COMPONENT_COUNT - 1) {
1118                 result = false;
1119             }
1120         }
1121     }
1122
1123     if (!result) {
1124         memset(color, 0, sizeof(*color));
1125     }
1126
1127     return result;
1128 }
1129
1130 /*
1131  * Redefine a color in a mode.
1132  * */
1133 bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex)
1134 {
1135     // check color
1136     if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT)
1137         return false;
1138     if (modeIndex < LED_MODE_COUNT) {  // modeIndex_e is unsigned, so one-sided test is enough
1139         if (modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT)
1140             return false;
1141         ledStripStatusModeConfigMutable()->modeColors[modeIndex].color[modeColorIndex] = colorIndex;
1142     } else if (modeIndex == LED_SPECIAL) {
1143         if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT)
1144             return false;
1145         ledStripStatusModeConfigMutable()->specialColors.color[modeColorIndex] = colorIndex;
1146     } else if (modeIndex == LED_AUX_CHANNEL) {
1147         if (modeColorIndex < 0 || modeColorIndex >= 1)
1148             return false;
1149         ledStripStatusModeConfigMutable()->ledstrip_aux_channel = colorIndex;
1150     } else {
1151         return false;
1152     }
1153     return true;
1154 }
1155 #endif
1156
1157 void ledStripEnable(void)
1158 {
1159     ws2811LedStripEnable();
1160
1161     ledStripEnabled = true;
1162 }
1163
1164 void ledStripDisable(void)
1165 {
1166     ledStripEnabled = false;
1167     previousProfileColorIndex = COLOR_UNDEFINED;
1168
1169     setStripColor(&HSV(BLACK));
1170     ws2811UpdateStrip((ledStripFormatRGB_e)ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness);
1171 }
1172
1173 void ledStripInit(void)
1174 {
1175 #if defined(USE_LED_STRIP_STATUS_MODE)
1176     colors = ledStripStatusModeConfig()->colors;
1177     modeColors = ledStripStatusModeConfig()->modeColors;
1178     specialColors = ledStripStatusModeConfig()->specialColors;
1179
1180     reevaluateLedConfig();
1181 #endif
1182
1183     ws2811LedStripInit(ledStripConfig()->ioTag);
1184 }
1185
1186 static uint8_t selectVisualBeeperColor(uint8_t colorIndex)
1187 {
1188     if (ledStripConfig()->ledstrip_visual_beeper && isBeeperOn()) {
1189         return ledStripConfig()->ledstrip_visual_beeper_color;
1190     } else {
1191         return colorIndex;
1192     }
1193 }
1194
1195 static void applySimpleProfile(timeUs_t currentTimeUs)
1196 {
1197     static timeUs_t colorUpdateTimeUs = 0;
1198     uint8_t colorIndex = COLOR_BLACK;
1199     bool blinkLed = false;
1200     bool visualBeeperOverride = true;
1201     unsigned flashPeriod;
1202     unsigned onPercent;
1203
1204     if (IS_RC_MODE_ACTIVE(BOXBEEPERON) || failsafeIsActive()) {
1205         // RX_SET or failsafe - force the beacon on and override the profile settings
1206         blinkLed = true;
1207         visualBeeperOverride = false; // prevent the visual beeper from interfering
1208         flashPeriod = BEACON_FAILSAFE_PERIOD_US;
1209         onPercent = BEACON_FAILSAFE_ON_PERCENT;
1210         colorIndex = ledStripConfig()->ledstrip_visual_beeper_color;
1211     } else {
1212         switch (ledStripConfig()->ledstrip_profile) {
1213             case LED_PROFILE_RACE:
1214                 colorIndex = ledStripConfig()->ledstrip_race_color;
1215                 break;
1216
1217             case LED_PROFILE_BEACON: {
1218                 if (!ledStripConfig()->ledstrip_beacon_armed_only || ARMING_FLAG(ARMED)) {
1219                     flashPeriod = ledStripConfig()->ledstrip_beacon_period_ms;
1220                     onPercent = ledStripConfig()->ledstrip_beacon_percent;
1221                     colorIndex = ledStripConfig()->ledstrip_beacon_color;
1222                     blinkLed = true;
1223                 }
1224                 break;
1225             }
1226
1227             default:
1228                 break;
1229         }
1230     }
1231
1232     if (blinkLed) {
1233         const unsigned onPeriod = flashPeriod * onPercent / 100;
1234         const bool beaconState = (millis() % flashPeriod) < onPeriod;
1235         colorIndex = (beaconState) ? colorIndex : COLOR_BLACK;
1236     }
1237
1238     if (visualBeeperOverride) {
1239         colorIndex = selectVisualBeeperColor(colorIndex);
1240     }
1241
1242     if ((colorIndex != previousProfileColorIndex) || (currentTimeUs >= colorUpdateTimeUs)) {
1243         setStripColor(&hsv[colorIndex]);
1244         ws2811UpdateStrip((ledStripFormatRGB_e)ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness);
1245         previousProfileColorIndex = colorIndex;
1246         colorUpdateTimeUs = currentTimeUs + PROFILE_COLOR_UPDATE_INTERVAL_US;
1247     }
1248 }
1249
1250 void ledStripUpdate(timeUs_t currentTimeUs)
1251 {
1252     UNUSED(currentTimeUs);
1253
1254     if (!isWS2811LedStripReady()) {
1255         // Call schedulerIgnoreTaskExecTime() unless data is being processed
1256         schedulerIgnoreTaskExecTime();
1257         return;
1258     }
1259
1260     if (ledStripEnabled && IS_RC_MODE_ACTIVE(BOXLEDLOW)) {
1261         ledStripDisable();
1262     } else if (!IS_RC_MODE_ACTIVE(BOXLEDLOW)) {
1263         ledStripEnable();
1264     }
1265
1266     if (ledStripEnabled) {
1267         switch (ledStripConfig()->ledstrip_profile) {
1268 #ifdef USE_LED_STRIP_STATUS_MODE
1269             case LED_PROFILE_STATUS: {
1270                 applyStatusProfile(currentTimeUs);
1271                 break;
1272             }
1273 #endif
1274             case LED_PROFILE_RACE:
1275             case LED_PROFILE_BEACON: {
1276                 applySimpleProfile(currentTimeUs);
1277                 break;
1278             }
1279
1280             default:
1281                 break;
1282         }
1283     } else {
1284         // Call schedulerIgnoreTaskExecTime() unless data is being processed
1285         schedulerIgnoreTaskExecTime();
1286     }
1287 }
1288
1289 uint8_t getLedProfile(void)
1290 {
1291     return ledStripConfig()->ledstrip_profile;
1292 }
1293
1294 void setLedProfile(uint8_t profile)
1295 {
1296     if (profile < LED_PROFILE_COUNT) {
1297         ledStripConfigMutable()->ledstrip_profile = profile;
1298     }
1299 }
1300 #endif