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