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