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