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