src/main/io/ledstrip.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdlib.h>
  20 #include <stdint.h>
  21 #include <string.h>
  22 #include <stdarg.h>
  23
  24 #include <platform.h>
  25
  26 #ifdef USE_LED_STRIP
  27
  28 #include "build/build_config.h"
  29
  30 #include "common/axis.h"
  31 #include "common/maths.h"
  32 #include "common/printf.h"
  33 #include "common/typeconversion.h"
  34 #include "common/string_light.h"
  35 #include "common/utils.h"
  36
  37 #include "config/feature.h"
  38 #include "config/parameter_group.h"
  39 #include "config/parameter_group_ids.h"
  40
  41 #include "drivers/light_ws2811strip.h"
  42 #include "drivers/serial.h"
  43 #include "drivers/time.h"
  44
  45 #include "fc/config.h"
  46 #include "fc/rc_controls.h"
  47 #include "fc/rc_modes.h"
  48 #include "fc/runtime_config.h"
  49
  50 #include "flight/failsafe.h"
  51 #include "flight/mixer.h"
  52 #include "flight/servos.h"
  53 #include "flight/pid.h"
  54 #include "flight/imu.h"
  55
  56 #include "io/ledstrip.h"
  57 #include "io/beeper.h"
  58 #include "io/serial.h"
  59 #include "io/gps.h"
  60
  61 #include "navigation/navigation.h"
  62
  63 #include "rx/rx.h"
  64 #include "sensors/acceleration.h"
  65 #include "sensors/diagnostics.h"
  66 #include "sensors/barometer.h"
  67 #include "sensors/battery.h"
  68 #include "sensors/boardalignment.h"
  69 #include "sensors/gyro.h"
  70 #include "sensors/sensors.h"
  71 #include "sensors/pitotmeter.h"
  72
  73 #include "telemetry/telemetry.h"
  74
  75
  76 PG_REGISTER_WITH_RESET_FN(ledStripConfig_t, ledStripConfig, PG_LED_STRIP_CONFIG, 2);
  77
  78 static bool ledStripInitialised = false;
  79 static bool ledStripEnabled = true;
  80
  81 static void ledStripDisable(void);
  82
  83 //#define USE_LED_ANIMATION
  84
  85 #define LED_STRIP_HZ(hz) ((int32_t)((1000 * 1000) / (hz)))
  86 #define LED_STRIP_MS(ms) ((int32_t)(1000 * (ms)))
  87
  88 #if LED_MAX_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH
  89 # error "Led strip length must match driver"
  90 #endif
  91
  92 const hsvColor_t hsv[] = {
  93     //                        H    S    V
  94     [COLOR_BLACK] =        {  0,   0,   0},
  95     [COLOR_WHITE] =        {  0, 255, 255},
  96     [COLOR_RED] =          {  0,   0, 255},
  97     [COLOR_ORANGE] =       { 30,   0, 255},
  98     [COLOR_YELLOW] =       { 60,   0, 255},
  99     [COLOR_LIME_GREEN] =   { 90,   0, 255},
 100     [COLOR_GREEN] =        {120,   0, 255},
 101     [COLOR_MINT_GREEN] =   {150,   0, 255},
 102     [COLOR_CYAN] =         {180,   0, 255},
 103     [COLOR_LIGHT_BLUE] =   {210,   0, 255},
 104     [COLOR_BLUE] =         {240,   0, 255},
 105     [COLOR_DARK_VIOLET] =  {270,   0, 255},
 106     [COLOR_MAGENTA] =      {300,   0, 255},
 107     [COLOR_DEEP_PINK] =    {330,   0, 255},
 108 };
 109 // macro to save typing on default colors
 110 #define HSV(color) (hsv[COLOR_ ## color])
 111
 112 STATIC_UNIT_TESTED uint8_t ledGridWidth;
 113 STATIC_UNIT_TESTED uint8_t ledGridHeight;
 114 // grid offsets
 115 STATIC_UNIT_TESTED uint8_t highestYValueForNorth;
 116 STATIC_UNIT_TESTED uint8_t lowestYValueForSouth;
 117 STATIC_UNIT_TESTED uint8_t highestXValueForWest;
 118 STATIC_UNIT_TESTED uint8_t lowestXValueForEast;
 119
 120 STATIC_UNIT_TESTED ledCounts_t ledCounts;
 121
 122 static const modeColorIndexes_t defaultModeColors[] = {
 123     //                          NORTH             EAST               SOUTH            WEST             UP          DOWN
 124     [LED_MODE_ORIENTATION] = {{ COLOR_WHITE,      COLOR_DARK_VIOLET, COLOR_RED,       COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 125     [LED_MODE_HEADFREE]    = {{ COLOR_LIME_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE,    COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 126     [LED_MODE_HORIZON]     = {{ COLOR_BLUE,       COLOR_DARK_VIOLET, COLOR_YELLOW,    COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 127     [LED_MODE_ANGLE]       = {{ COLOR_CYAN,       COLOR_DARK_VIOLET, COLOR_YELLOW,    COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 128     [LED_MODE_MAG]         = {{ COLOR_MINT_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE,    COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 129     [LED_MODE_BARO]        = {{ COLOR_LIGHT_BLUE, COLOR_DARK_VIOLET, COLOR_RED,       COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
 130 };
 131
 132 static const specialColorIndexes_t defaultSpecialColors[] = {
 133     {{ [LED_SCOLOR_DISARMED]        = COLOR_GREEN,
 134        [LED_SCOLOR_ARMED]           = COLOR_BLUE,
 135        [LED_SCOLOR_ANIMATION]       = COLOR_WHITE,
 136        [LED_SCOLOR_BACKGROUND]      = COLOR_BLACK,
 137        [LED_SCOLOR_BLINKBACKGROUND] = COLOR_BLACK,
 138        [LED_SCOLOR_GPSNOSATS]       = COLOR_RED,
 139        [LED_SCOLOR_GPSNOLOCK]       = COLOR_ORANGE,
 140        [LED_SCOLOR_GPSLOCKED]       = COLOR_GREEN,
 141        [LED_SCOLOR_STROBE]          = COLOR_WHITE,
 142     }}
 143 };
 144
 145 void pgResetFn_ledStripConfig(ledStripConfig_t *instance)
 146 {
 147     memset(instance->ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t));
 148     // copy hsv colors as default
 149     memset(instance->colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t));
 150     BUILD_BUG_ON(LED_CONFIGURABLE_COLOR_COUNT < ARRAYLEN(hsv));
 151     for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) {
 152         instance->colors[colorIndex] = hsv[colorIndex];
 153     }
 154     memcpy_fn(&instance->modeColors, &defaultModeColors, sizeof(defaultModeColors));
 155     memcpy_fn(&instance->specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors));
 156 }
 157
 158 static int scaledThrottle;
 159
 160 static void updateLedRingCounts(void);
 161
 162 STATIC_UNIT_TESTED void determineLedStripDimensions(void)
 163 {
 164     int maxX = 0;
 165     int maxY = 0;
 166
 167     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 168         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 169
 170         maxX = MAX(ledGetX(ledConfig), maxX);
 171         maxY = MAX(ledGetY(ledConfig), maxY);
 172     }
 173     ledGridWidth = maxX + 1;
 174     ledGridHeight = maxY + 1;
 175 }
 176
 177 STATIC_UNIT_TESTED void determineOrientationLimits(void)
 178 {
 179     highestYValueForNorth = MIN((ledGridHeight / 2) - 1, 0);
 180     lowestYValueForSouth = (ledGridHeight + 1) / 2;
 181     highestXValueForWest = MIN((ledGridWidth / 2) - 1, 0);
 182     lowestXValueForEast = (ledGridWidth + 1) / 2;
 183 }
 184
 185 STATIC_UNIT_TESTED void updateLedCount(void)
 186 {
 187     int count = 0, countRing = 0, countScanner= 0;
 188
 189     const ledConfig_t configNotSet = {};
 190     for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) {
 191         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 192
 193         if (!memcmp(ledConfig, &configNotSet, sizeof(ledConfig_t)))
 194             break;
 195
 196         count++;
 197
 198         if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING)
 199             countRing++;
 200
 201         if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER))
 202             countScanner++;
 203     }
 204
 205     ledCounts.count = count;
 206     ledCounts.ring = countRing;
 207     ledCounts.larson = countScanner;
 208 }
 209
 210 void reevaluateLedConfig(void)
 211 {
 212     updateLedCount();
 213     determineLedStripDimensions();
 214     determineOrientationLimits();
 215     updateLedRingCounts();
 216 }
 217
 218 // get specialColor by index
 219 static const hsvColor_t* getSC(ledSpecialColorIds_e index)
 220 {
 221     return &ledStripConfig()->colors[ledStripConfig()->specialColors.color[index]];
 222 }
 223
 224 static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' };
 225 static const char baseFunctionCodes[LED_BASEFUNCTION_COUNT]   = { 'C', 'F', 'A', 'L', 'S', 'G', 'R', 'H' };
 226 static const char overlayCodes[LED_OVERLAY_COUNT]   = { 'T', 'O', 'B', 'N', 'I', 'W', 'E' };
 227
 228 #define CHUNK_BUFFER_SIZE 11
 229
 230 bool parseLedStripConfig(int ledIndex, const char *config)
 231 {
 232     if (ledIndex >= LED_MAX_STRIP_LENGTH)
 233         return false;
 234
 235     enum parseState_e {
 236         X_COORDINATE,
 237         Y_COORDINATE,
 238         DIRECTIONS,
 239         FUNCTIONS,
 240         RING_COLORS,
 241         PARSE_STATE_COUNT
 242     };
 243     static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0'};
 244
 245     ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[ledIndex];
 246     memset(ledConfig, 0, sizeof(ledConfig_t));
 247
 248     int x = 0, y = 0, color = 0;   // initialize to prevent warnings
 249     int baseFunction = 0;
 250     int overlay_flags = 0;
 251     int direction_flags = 0;
 252
 253     for (enum parseState_e parseState = 0; parseState < PARSE_STATE_COUNT; parseState++) {
 254         char chunk[CHUNK_BUFFER_SIZE];
 255         {
 256             char chunkSeparator = chunkSeparators[parseState];
 257             int chunkIndex = 0;
 258             while (*config  && *config != chunkSeparator && chunkIndex < (CHUNK_BUFFER_SIZE - 1)) {
 259                 chunk[chunkIndex++] = *config++;
 260             }
 261             chunk[chunkIndex++] = 0; // zero-terminate chunk
 262             if (*config != chunkSeparator) {
 263                 return false;
 264             }
 265             config++;   // skip separator
 266         }
 267         switch (parseState) {
 268             case X_COORDINATE:
 269                 x = fastA2I(chunk);
 270                 break;
 271             case Y_COORDINATE:
 272                 y = fastA2I(chunk);
 273                 break;
 274             case DIRECTIONS:
 275                 for (char *ch = chunk; *ch; ch++) {
 276                     for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) {
 277                         if (directionCodes[dir] == *ch) {
 278                             direction_flags |= LED_FLAG_DIRECTION(dir);
 279                             break;
 280                         }
 281                     }
 282                 }
 283                 break;
 284             case FUNCTIONS:
 285                 for (char *ch = chunk; *ch; ch++) {
 286                     for (ledBaseFunctionId_e fn = 0; fn < LED_BASEFUNCTION_COUNT; fn++) {
 287                         if (baseFunctionCodes[fn] == *ch) {
 288                             baseFunction = fn;
 289                             break;
 290                         }
 291                     }
 292
 293                     for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) {
 294                         if (overlayCodes[ol] == *ch) {
 295                             overlay_flags |= LED_FLAG_OVERLAY(ol);
 296                             break;
 297                         }
 298                     }
 299                 }
 300                 break;
 301             case RING_COLORS:
 302                 color = fastA2I(chunk);
 303                 if (color >= LED_CONFIGURABLE_COLOR_COUNT)
 304                     color = 0;
 305                 break;
 306             case PARSE_STATE_COUNT:; // prevent warning
 307         }
 308     }
 309
 310     DEFINE_LED(ledConfig, x, y, color, direction_flags, baseFunction, overlay_flags, 0);
 311
 312     reevaluateLedConfig();
 313
 314     return true;
 315 }
 316
 317 void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t bufferSize)
 318 {
 319     char directions[LED_DIRECTION_COUNT + 1];
 320     char baseFunctionOverlays[LED_OVERLAY_COUNT + 2];
 321
 322     memset(ledConfigBuffer, 0, bufferSize);
 323
 324     char *dptr = directions;
 325     for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) {
 326         if (ledGetDirectionBit(ledConfig, dir)) {
 327             *dptr++ = directionCodes[dir];
 328         }
 329     }
 330     *dptr = 0;
 331
 332     char *fptr = baseFunctionOverlays;
 333     *fptr++ = baseFunctionCodes[ledGetFunction(ledConfig)];
 334
 335     for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) {
 336         if (ledGetOverlayBit(ledConfig, ol)) {
 337             *fptr++ = overlayCodes[ol];
 338         }
 339     }
 340     *fptr = 0;
 341
 342     // TODO - check buffer length
 343     tfp_sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig));
 344 }
 345
 346 typedef enum {
 347     // the ordering is important, see below how NSEW is mapped to  NE/SE/NW/SW
 348     QUADRANT_NORTH      = 1 << 0,
 349     QUADRANT_SOUTH      = 1 << 1,
 350     QUADRANT_EAST       = 1 << 2,
 351     QUADRANT_WEST       = 1 << 3,
 352     QUADRANT_NORTH_EAST = 1 << 4,
 353     QUADRANT_SOUTH_EAST = 1 << 5,
 354     QUADRANT_NORTH_WEST = 1 << 6,
 355     QUADRANT_SOUTH_WEST = 1 << 7,
 356     QUADRANT_NONE       = 1 << 8,
 357     QUADRANT_NOTDIAG    = 1 << 9,  // not in NE/SE/NW/SW
 358     // values for test
 359     QUADRANT_ANY        = QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST | QUADRANT_NONE,
 360 } quadrant_e;
 361
 362 static quadrant_e getLedQuadrant(const int ledIndex)
 363 {
 364     const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 365
 366     int x = ledGetX(ledConfig);
 367     int y = ledGetY(ledConfig);
 368
 369     int quad = 0;
 370     if (y <= highestYValueForNorth)
 371         quad |= QUADRANT_NORTH;
 372     else if (y >= lowestYValueForSouth)
 373         quad |= QUADRANT_SOUTH;
 374     if (x >= lowestXValueForEast)
 375         quad |= QUADRANT_EAST;
 376     else if (x <= highestXValueForWest)
 377         quad |= QUADRANT_WEST;
 378
 379     if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH))
 380        && (quad & (QUADRANT_EAST | QUADRANT_WEST)) ) { // is led  in one of NE/SE/NW/SW?
 381         quad |= 1 << (4 + ((quad & QUADRANT_SOUTH) ? 1 : 0) + ((quad & QUADRANT_WEST) ? 2 : 0));
 382     } else {
 383         quad |= QUADRANT_NOTDIAG;
 384     }
 385
 386     if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST)) == 0)
 387         quad |= QUADRANT_NONE;
 388
 389     return quad;
 390 }
 391
 392 static const struct {
 393     uint8_t dir;             // ledDirectionId_e
 394     uint16_t quadrantMask;   // quadrant_e
 395 } directionQuadrantMap[] = {
 396     {LED_DIRECTION_SOUTH, QUADRANT_SOUTH},
 397     {LED_DIRECTION_NORTH, QUADRANT_NORTH},
 398     {LED_DIRECTION_EAST,  QUADRANT_EAST},
 399     {LED_DIRECTION_WEST,  QUADRANT_WEST},
 400     {LED_DIRECTION_DOWN,  QUADRANT_ANY},
 401     {LED_DIRECTION_UP,    QUADRANT_ANY},
 402 };
 403
 404 static const hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIndexes_t *modeColors)
 405 {
 406     const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 407
 408     quadrant_e quad = getLedQuadrant(ledIndex);
 409     for (unsigned i = 0; i < ARRAYLEN(directionQuadrantMap); i++) {
 410         ledDirectionId_e dir = directionQuadrantMap[i].dir;
 411         quadrant_e quadMask = directionQuadrantMap[i].quadrantMask;
 412
 413         if (ledGetDirectionBit(ledConfig, dir) && (quad & quadMask))
 414             return &ledStripConfig()->colors[modeColors->color[dir]];
 415     }
 416     return NULL;
 417 }
 418
 419 // map flight mode to led mode, in order of priority
 420 // flightMode == 0 is always active
 421 static const struct {
 422     uint16_t flightMode;
 423     uint8_t ledMode;
 424 } flightModeToLed[] = {
 425     {HEADFREE_MODE, LED_MODE_HEADFREE},
 426     {HEADING_MODE,  LED_MODE_MAG},
 427 #ifdef USE_BARO
 428     {NAV_ALTHOLD_MODE, LED_MODE_BARO},
 429 #endif
 430     {HORIZON_MODE,  LED_MODE_HORIZON},
 431     {ANGLE_MODE,    LED_MODE_ANGLE},
 432     {0,             LED_MODE_ORIENTATION},
 433 };
 434
 435 static void applyLedFixedLayers(void)
 436 {
 437     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 438         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 439         hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND);
 440
 441         int fn = ledGetFunction(ledConfig);
 442         int hOffset = HSV_HUE_MAX;
 443         uint8_t channel = 0;
 444
 445         switch (fn) {
 446             case LED_FUNCTION_COLOR:
 447                 color = ledStripConfig()->colors[ledGetColor(ledConfig)];
 448                 break;
 449
 450             case LED_FUNCTION_FLIGHT_MODE:
 451                 for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++)
 452                     if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) {
 453                         const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]);
 454                         if (directionalColor) {
 455                             color = *directionalColor;
 456                         }
 457
 458                         break; // stop on first match
 459                     }
 460                 break;
 461
 462             case LED_FUNCTION_ARM_STATE:
 463                 color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED);
 464                 break;
 465
 466             case LED_FUNCTION_BATTERY:
 467                 color = HSV(RED);
 468                 hOffset += scaleRange(calculateBatteryPercentage(), 0, 100, -30, 120);
 469                 break;
 470
 471             case LED_FUNCTION_RSSI:
 472                 color = HSV(RED);
 473                 hOffset += scaleRange(getRSSI() * 100, 0, 1023, -30, 120);
 474                 break;
 475
 476             case LED_FUNCTION_CHANNEL:
 477                 channel = ledGetColor(ledConfig) - 1;
 478                 color = HSV(RED);
 479                 hOffset = scaleRange(rxGetChannelValue(channel), PWM_RANGE_MIN, PWM_RANGE_MAX, -1, 360);
 480                 // add black and white to range of colors
 481                 if (hOffset < 0) {
 482                     color = HSV(BLACK);
 483                 } else if (hOffset > HSV_HUE_MAX) {
 484                     color = HSV(WHITE);
 485                 }
 486                 break;
 487
 488             default:
 489                 break;
 490         }
 491
 492         if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) {
 493             hOffset += ((scaledThrottle - 10) * 4) / 3;
 494         }
 495
 496         color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1);
 497
 498         setLedHsv(ledIndex, &color);
 499
 500     }
 501 }
 502
 503 static void applyLedHsv(uint32_t mask, uint32_t ledOperation, const hsvColor_t *color)
 504 {
 505     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 506         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 507         if ((*((uint32_t *)ledConfig) & mask) == ledOperation)
 508             setLedHsv(ledIndex, color);
 509     }
 510 }
 511
 512 typedef enum {
 513     WARNING_ARMING_DISABLED,
 514     WARNING_LOW_BATTERY,
 515     WARNING_FAILSAFE,
 516     WARNING_HW_ERROR,
 517 } warningFlags_e;
 518
 519 static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
 520 {
 521     static uint8_t warningFlashCounter = 0;
 522     static uint8_t warningFlags = 0;          // non-zero during blinks
 523
 524     if (updateNow) {
 525         // keep counter running, so it stays in sync with blink
 526         warningFlashCounter++;
 527         warningFlashCounter &= 0xF;
 528
 529         if (warningFlashCounter == 0) {      // update when old flags was processed
 530             warningFlags = 0;
 531             if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK)
 532                 warningFlags |= 1 << WARNING_LOW_BATTERY;
 533             if (failsafeIsActive())
 534                 warningFlags |= 1 << WARNING_FAILSAFE;
 535             if (!ARMING_FLAG(ARMED) && isArmingDisabled())
 536                 warningFlags |= 1 << WARNING_ARMING_DISABLED;
 537             if (!isHardwareHealthy())
 538                 warningFlags |= 1 << WARNING_HW_ERROR;
 539         }
 540         *timer += LED_STRIP_HZ(10);
 541     }
 542
 543     if (warningFlags) {
 544         const hsvColor_t *warningColor = NULL;
 545
 546         bool colorOn = (warningFlashCounter % 2) == 0;   // w_w_
 547         warningFlags_e warningId = warningFlashCounter / 4;
 548         if (warningFlags & (1 << warningId)) {
 549             switch (warningId) {
 550                 case WARNING_ARMING_DISABLED:
 551                     warningColor = colorOn ? &HSV(GREEN)  : &HSV(BLACK);
 552                     break;
 553                 case WARNING_LOW_BATTERY:
 554                     warningColor = colorOn ? &HSV(RED)    : &HSV(BLACK);
 555                     break;
 556                 case WARNING_FAILSAFE:
 557                     warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE);
 558                     break;
 559                 case WARNING_HW_ERROR:
 560                     warningColor = colorOn ? &HSV(BLUE) : &HSV(BLACK);
 561                     break;
 562                 default:;
 563             }
 564         }
 565         if (warningColor)
 566             applyLedHsv(LED_OVERLAY_MASK, LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor);
 567     }
 568 }
 569
 570 static void applyLedBatteryLayer(bool updateNow, timeUs_t *timer)
 571 {
 572     static bool flash = false;
 573
 574     int state;
 575     int timeOffset = 1;
 576
 577     if (updateNow) {
 578        state = getBatteryState();
 579
 580        switch (state) {
 581            case BATTERY_OK:
 582                flash = false;
 583                timeOffset = 1;
 584                break;
 585            case BATTERY_WARNING:
 586                timeOffset = 2;
 587                break;
 588            default:
 589                timeOffset = 8;
 590                break;
 591        }
 592        flash = !flash;
 593
 594         *timer += LED_STRIP_HZ(timeOffset);
 595     }
 596
 597
 598     if (!flash) {
 599        const hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND);
 600        applyLedHsv(LED_FUNCTION_MASK, LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc);
 601     }
 602 }
 603
 604 static void applyLedRssiLayer(bool updateNow, timeUs_t *timer)
 605 {
 606     static bool flash = false;
 607
 608     int state;
 609     int timeOffset = 1;
 610
 611     if (updateNow) {
 612        state = (getRSSI() * 100) / 1023;
 613
 614        if (state > 50) {
 615            flash = false;
 616            timeOffset = 1;
 617        } else if (state > 20) {
 618            timeOffset = 2;
 619        } else {
 620            timeOffset = 8;
 621        }
 622        flash = !flash;
 623
 624        *timer += LED_STRIP_HZ(timeOffset);
 625     }
 626
 627
 628     if (!flash) {
 629        const hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND);
 630        applyLedHsv(LED_FUNCTION_MASK, LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc);
 631     }
 632 }
 633
 634 #ifdef USE_GPS
 635 static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
 636 {
 637     static uint8_t gpsFlashCounter = 0;
 638     static uint8_t gpsPauseCounter = 0;
 639     const uint8_t blinkPauseLength = 4;
 640
 641     if (updateNow) {
 642         if (gpsPauseCounter > 0) {
 643             gpsPauseCounter--;
 644         } else if (gpsFlashCounter >= (gpsSol.numSat - 1)) {
 645             gpsFlashCounter = 0;
 646             gpsPauseCounter = blinkPauseLength;
 647         } else {
 648             gpsFlashCounter++;
 649             gpsPauseCounter = 1;
 650         }
 651         *timer += LED_STRIP_HZ(2.5);
 652     }
 653
 654     const hsvColor_t *gpsColor;
 655
 656     if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) {
 657         gpsColor = getSC(LED_SCOLOR_GPSNOSATS);
 658     } else {
 659         bool colorOn = gpsPauseCounter == 0;  // each interval starts with pause
 660         if (STATE(GPS_FIX)) {
 661             gpsColor = colorOn ? getSC(LED_SCOLOR_GPSLOCKED) : getSC(LED_SCOLOR_BACKGROUND);
 662         } else {
 663             gpsColor = colorOn ? getSC(LED_SCOLOR_GPSNOLOCK) : getSC(LED_SCOLOR_GPSNOSATS);
 664         }
 665     }
 666
 667     applyLedHsv(LED_FUNCTION_MASK, LED_MOV_FUNCTION(LED_FUNCTION_GPS), gpsColor);
 668 }
 669
 670 #endif
 671
 672 #define INDICATOR_DEADBAND 25
 673
 674 static void applyLedIndicatorLayer(bool updateNow, timeUs_t *timer)
 675 {
 676     static bool flash = 0;
 677
 678     if (updateNow) {
 679         if (rxIsReceivingSignal()) {
 680             // calculate update frequency
 681             int scale = (STATE(AIRPLANE) || STATE(ROVER)) ? ABS(rcCommand[ROLL]) : MAX(ABS(rcCommand[ROLL]), ABS(rcCommand[PITCH]));  // 0 - 500
 682             scale += (50 - INDICATOR_DEADBAND);  // start increasing frequency right after deadband
 683             *timer += LED_STRIP_HZ(5) * 50 / MAX(50, scale);   // 5 - 50Hz update, 2.5 - 25Hz blink
 684
 685             flash = !flash;
 686         } else {
 687             *timer += LED_STRIP_HZ(5);  // try again soon
 688         }
 689     }
 690
 691     if (!flash)
 692         return;
 693
 694     const hsvColor_t *flashColor = &HSV(ORANGE); // TODO - use user color?
 695
 696     if (STATE(AIRPLANE) || STATE(ROVER)) {
 697         for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 698             const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 699             if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) {
 700                 if (((rcCommand[ROLL] > INDICATOR_DEADBAND) && (ledGetX(ledConfig) >= 8)) || ((rcCommand[ROLL] < -INDICATOR_DEADBAND) && (ledGetX(ledConfig) < 8)))
 701                     setLedHsv(ledIndex, flashColor);
 702             }
 703         }
 704     } else {
 705         quadrant_e quadrants = 0;
 706         if (rcCommand[ROLL] > INDICATOR_DEADBAND) {
 707             quadrants |= QUADRANT_NORTH_EAST | QUADRANT_SOUTH_EAST;
 708         } else if (rcCommand[ROLL] < -INDICATOR_DEADBAND) {
 709             quadrants |= QUADRANT_NORTH_WEST | QUADRANT_SOUTH_WEST;
 710         }
 711         if (rcCommand[PITCH] > INDICATOR_DEADBAND) {
 712             quadrants |= QUADRANT_NORTH_EAST | QUADRANT_NORTH_WEST;
 713         } else if (rcCommand[PITCH] < -INDICATOR_DEADBAND) {
 714             quadrants |= QUADRANT_SOUTH_EAST | QUADRANT_SOUTH_WEST;
 715         }
 716
 717         for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 718             const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 719             if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) {
 720                 if (getLedQuadrant(ledIndex) & quadrants)
 721                     setLedHsv(ledIndex, flashColor);
 722             }
 723         }
 724     }
 725 }
 726
 727 #define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off
 728 #define ROTATION_SEQUENCE_LED_WIDTH 2 // 2 on
 729
 730 static void updateLedRingCounts(void)
 731 {
 732     int seqLen;
 733     // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds
 734     if ((ledCounts.ring % ROTATION_SEQUENCE_LED_COUNT) == 0) {
 735         seqLen = ROTATION_SEQUENCE_LED_COUNT;
 736     } else {
 737         seqLen = ledCounts.ring;
 738         // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds
 739         // TODO - improve partitioning (15 leds -> 3x5)
 740         while ((seqLen > ROTATION_SEQUENCE_LED_COUNT) && ((seqLen % 2) == 0)) {
 741             seqLen /= 2;
 742         }
 743     }
 744     ledCounts.ringSeqLen = seqLen;
 745 }
 746
 747 static void applyLedThrustRingLayer(bool updateNow, timeUs_t *timer)
 748 {
 749     static uint8_t rotationPhase;
 750     int ledRingIndex = 0;
 751
 752     if (updateNow) {
 753         rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1;
 754
 755         int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rxGetChannelValue(THROTTLE), PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10;
 756         *timer += LED_STRIP_HZ(5) * 10 / scale;  // 5 - 50Hz update rate
 757     }
 758
 759     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 760         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 761         if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) {
 762
 763             bool applyColor;
 764             if (ARMING_FLAG(ARMED)) {
 765                 applyColor = (ledRingIndex + rotationPhase) % ledCounts.ringSeqLen < ROTATION_SEQUENCE_LED_WIDTH;
 766             } else {
 767                 applyColor = !(ledRingIndex % 2); // alternating pattern
 768             }
 769
 770             if (applyColor) {
 771                 const hsvColor_t *ringColor = &ledStripConfig()->colors[ledGetColor(ledConfig)];
 772                 setLedHsv(ledIndex, ringColor);
 773             }
 774
 775             ledRingIndex++;
 776         }
 777     }
 778 }
 779
 780 typedef struct larsonParameters_s {
 781     uint8_t currentBrightness;
 782     int8_t currentIndex;
 783     int8_t direction;
 784 } larsonParameters_t;
 785
 786 static int brightnessForLarsonIndex(larsonParameters_t *larsonParameters, uint8_t larsonIndex)
 787 {
 788     int offset = larsonIndex - larsonParameters->currentIndex;
 789     static const int larsonLowValue = 8;
 790
 791     if (ABS(offset) > 1)
 792         return (larsonLowValue);
 793
 794     if (offset == 0)
 795         return (larsonParameters->currentBrightness);
 796
 797     if (larsonParameters->direction == offset) {
 798         return (larsonParameters->currentBrightness - 127);
 799     }
 800
 801     return (255 - larsonParameters->currentBrightness);
 802
 803 }
 804
 805 static void larsonScannerNextStep(larsonParameters_t *larsonParameters, int delta)
 806 {
 807     if (larsonParameters->currentBrightness > (255 - delta)) {
 808         larsonParameters->currentBrightness = 127;
 809         if (larsonParameters->currentIndex >= ledCounts.larson || larsonParameters->currentIndex < 0) {
 810             larsonParameters->direction = -larsonParameters->direction;
 811         }
 812         larsonParameters->currentIndex += larsonParameters->direction;
 813     } else {
 814         larsonParameters->currentBrightness += delta;
 815     }
 816 }
 817
 818 static void applyLarsonScannerLayer(bool updateNow, timeUs_t *timer)
 819 {
 820     static larsonParameters_t larsonParameters = { 0, 0, 1 };
 821
 822     if (updateNow) {
 823         larsonScannerNextStep(&larsonParameters, 15);
 824         *timer += LED_STRIP_HZ(60);
 825     }
 826
 827     int scannerLedIndex = 0;
 828     for (unsigned i = 0; i < ledCounts.count; i++) {
 829
 830         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
 831
 832         if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) {
 833             hsvColor_t ledColor;
 834             getLedHsv(i, &ledColor);
 835             ledColor.v = brightnessForLarsonIndex(&larsonParameters, scannerLedIndex);
 836             setLedHsv(i, &ledColor);
 837             scannerLedIndex++;
 838         }
 839     }
 840 }
 841
 842 // blink twice, then wait ; either always or just when landing
 843 static void applyLedBlinkLayer(bool updateNow, timeUs_t *timer)
 844 {
 845     const uint16_t blinkPattern = 0x8005; // 0b1000000000000101;
 846     static uint16_t blinkMask;
 847
 848     if (updateNow) {
 849         blinkMask = blinkMask >> 1;
 850         if (blinkMask <= 1)
 851             blinkMask = blinkPattern;
 852
 853         *timer += LED_STRIP_HZ(10);
 854     }
 855
 856     bool ledOn = (blinkMask & 1);  // b_b_____...
 857     for (int i = 0; i < ledCounts.count; ++i) {
 858         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
 859
 860         if (ledOn) {
 861             if (ledGetOverlayBit(ledConfig, LED_OVERLAY_STROBE)) {
 862                 setLedHsv(i, getSC(LED_SCOLOR_STROBE));
 863             }
 864         } else {
 865             if (ledGetOverlayBit(ledConfig, LED_OVERLAY_BLINK) ||
 866                     (ledGetOverlayBit(ledConfig, LED_OVERLAY_LANDING_FLASH) && scaledThrottle < 55 && scaledThrottle > 10)) {
 867                 setLedHsv(i, getSC(LED_SCOLOR_BLINKBACKGROUND));
 868             }
 869         }
 870     }
 871 }
 872
 873 #ifdef USE_LED_ANIMATION
 874 static void applyLedAnimationLayer(bool updateNow, timeUs_t *timer)
 875 {
 876     static uint8_t frameCounter = 0;
 877     const int animationFrames = ledGridHeight;
 878     if (updateNow) {
 879         frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0;
 880         *timer += LED_STRIP_HZ(20);
 881     }
 882
 883     if (ARMING_FLAG(ARMED))
 884         return;
 885
 886     int previousRow = frameCounter > 0 ? frameCounter - 1 : animationFrames - 1;
 887     int currentRow = frameCounter;
 888     int nextRow = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0;
 889
 890     for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
 891         const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex];
 892
 893         if (ledGetY(ledConfig) == previousRow) {
 894             setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION));
 895             scaleLedValue(ledIndex, 50);
 896         } else if (ledGetY(ledConfig) == currentRow) {
 897             setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION));
 898         } else if (ledGetY(ledConfig) == nextRow) {
 899             scaleLedValue(ledIndex, 50);
 900         }
 901     }
 902 }
 903 #endif
 904
 905 typedef enum {
 906     timBlink = 0,
 907     timLarson,
 908     timBattery,
 909     timRssi,
 910 #ifdef USE_GPS
 911     timGps,
 912 #endif
 913     timWarning,
 914     timIndicator,
 915 #ifdef USE_LED_ANIMATION
 916     timAnimation,
 917 #endif
 918     timRing,
 919     timTimerCount
 920 } timId_e;
 921
 922 static timeUs_t timerVal[timTimerCount];
 923
 924 // function to apply layer.
 925 // function must replan self using timer pointer
 926 // when updateNow is true (timer triggered), state must be updated first,
 927 //  before calculating led state. Otherwise update started by different trigger
 928 //  may modify LED state.
 929 typedef void applyLayerFn_timed(bool updateNow, timeUs_t *timer);
 930
 931 static applyLayerFn_timed* layerTable[timTimerCount] = {
 932     [timBlink] = &applyLedBlinkLayer,
 933     [timLarson] = &applyLarsonScannerLayer,
 934     [timBattery] = &applyLedBatteryLayer,
 935     [timRssi] = &applyLedRssiLayer,
 936 #ifdef USE_GPS
 937     [timGps] = &applyLedGpsLayer,
 938 #endif
 939     [timWarning] = &applyLedWarningLayer,
 940     [timIndicator] = &applyLedIndicatorLayer,
 941 #ifdef USE_LED_ANIMATION
 942     [timAnimation] = &applyLedAnimationLayer,
 943 #endif
 944     [timRing] = &applyLedThrustRingLayer
 945 };
 946
 947 void ledStripUpdate(timeUs_t currentTimeUs)
 948 {
 949     if (!(ledStripInitialised && isWS2811LedStripReady())) {
 950         return;
 951     }
 952
 953     if (IS_RC_MODE_ACTIVE(BOXLEDLOW)) {
 954         if (ledStripEnabled) {
 955             ledStripDisable();
 956             ledStripEnabled = false;
 957         }
 958         return;
 959     }
 960     ledStripEnabled = true;
 961
 962     // test all led timers, setting corresponding bits
 963     uint32_t timActive = 0;
 964     for (timId_e timId = 0; timId < timTimerCount; timId++) {
 965         // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
 966         // max delay is limited to 5s
 967         timeDelta_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]);
 968         if (delta < 0 && delta > -LED_STRIP_MS(5000))
 969             continue;  // not ready yet
 970         timActive |= 1 << timId;
 971         if (delta >= LED_STRIP_MS(100) || delta < 0) {
 972             timerVal[timId] = currentTimeUs;
 973         }
 974     }
 975
 976     if (!timActive)
 977         return;          // no change this update, keep old state
 978
 979     // apply all layers; triggered timed functions has to update timers
 980
 981     scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rxGetChannelValue(THROTTLE), PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10;
 982
 983     applyLedFixedLayers();
 984
 985     for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) {
 986         timeUs_t *timer = &timerVal[timId];
 987         bool updateNow = timActive & (1 << timId);
 988         (*layerTable[timId])(updateNow, timer);
 989     }
 990     ws2811UpdateStrip();
 991 }
 992
 993 bool parseColor(int index, const char *colorConfig)
 994 {
 995     const char *remainingCharacters = colorConfig;
 996
 997     hsvColor_t *color = &ledStripConfigMutable()->colors[index];
 998
 999     bool result = true;
1000     static const uint16_t hsv_limit[HSV_COLOR_COMPONENT_COUNT] = {
1001         [HSV_HUE] = HSV_HUE_MAX,
1002         [HSV_SATURATION] = HSV_SATURATION_MAX,
1003         [HSV_VALUE] = HSV_VALUE_MAX,
1004     };
1005     for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) {
1006         int val = fastA2I(remainingCharacters);
1007         if (val > hsv_limit[componentIndex]) {
1008             result = false;
1009             break;
1010         }
1011         switch (componentIndex) {
1012             case HSV_HUE:
1013                 color->h = val;
1014                 break;
1015             case HSV_SATURATION:
1016                 color->s = val;
1017                 break;
1018             case HSV_VALUE:
1019                 color->v = val;
1020                 break;
1021         }
1022         remainingCharacters = strchr(remainingCharacters, ',');
1023         if (remainingCharacters) {
1024             remainingCharacters++;  // skip separator
1025         } else {
1026             if (componentIndex < HSV_COLOR_COMPONENT_COUNT - 1) {
1027                 result = false;
1028             }
1029         }
1030     }
1031
1032     if (!result) {
1033         memset(color, 0, sizeof(*color));
1034     }
1035
1036     return result;
1037 }
1038
1039 /*
1040  * Redefine a color in a mode.
1041  * */
1042 bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex)
1043 {
1044     // check color
1045     if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT)
1046         return false;
1047     if (modeIndex < LED_MODE_COUNT) {  // modeIndex_e is unsigned, so one-sided test is enough
1048         if (modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT)
1049             return false;
1050         ledStripConfigMutable()->modeColors[modeIndex].color[modeColorIndex] = colorIndex;
1051     } else if (modeIndex == LED_SPECIAL) {
1052         if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT)
1053             return false;
1054         ledStripConfigMutable()->specialColors.color[modeColorIndex] = colorIndex;
1055     } else {
1056         return false;
1057     }
1058     return true;
1059 }
1060
1061 void ledStripInit(void)
1062 {
1063     ledStripInitialised = false;
1064 }
1065
1066 void ledStripEnable(void)
1067 {
1068     reevaluateLedConfig();
1069     ledStripInitialised = true;
1070
1071     ws2811LedStripInit();
1072 }
1073
1074 static void ledStripDisable(void)
1075 {
1076     setStripColor(&HSV(BLACK));
1077
1078     ws2811UpdateStrip();
1079 }
1080 #endif