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