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