src/lib/LED/devRGB.cpp

   1 #include "targets.h"
   2 #include "common.h"
   3 #include "devLED.h"
   4
   5 #if defined(TARGET_TX)
   6 #include "config.h"
   7 #endif
   8
   9 #include "crsf_protocol.h"
  10 #include "POWERMGNT.h"
  11
  12 static uint8_t pixelCount;
  13 static uint8_t *statusLEDs;
  14 static uint8_t statusLEDcount;
  15 static uint8_t *vtxStatusLEDs;
  16 static uint8_t vtxLEDcount;
  17 static uint8_t *bootLEDs;
  18 static uint8_t bootLEDcount;
  19
  20 #if defined(PLATFORM_ESP32)
  21 #include "esp32rgb.h"
  22 static ESP32S3LedDriverGRB *stripgrb;
  23 static ESP32S3LedDriverRGB *striprgb;
  24 #else
  25 #include <NeoPixelBus.h>
  26 #define METHOD NeoEsp8266Uart1800KbpsMethod
  27 static NeoPixelBus<NeoGrbFeature, METHOD> *stripgrb;
  28 static NeoPixelBus<NeoRgbFeature, METHOD> *striprgb;
  29 #endif
  30
  31 void WS281Binit()
  32 {
  33     if (OPT_WS2812_IS_GRB)
  34     {
  35 #if defined(PLATFORM_ESP32)
  36         stripgrb = new ESP32S3LedDriverGRB(pixelCount, GPIO_PIN_LED_WS2812);
  37 #else
  38         stripgrb = new NeoPixelBus<NeoGrbFeature, METHOD>(pixelCount, GPIO_PIN_LED_WS2812);
  39 #endif
  40         stripgrb->Begin();
  41         stripgrb->ClearTo(RgbColor(0), 0, pixelCount-1);
  42         stripgrb->Show();
  43     }
  44     else
  45     {
  46 #if defined(PLATFORM_ESP32)
  47         striprgb = new ESP32S3LedDriverRGB(pixelCount, GPIO_PIN_LED_WS2812);
  48 #else
  49         striprgb = new NeoPixelBus<NeoRgbFeature, METHOD> (pixelCount, GPIO_PIN_LED_WS2812);
  50 #endif
  51         striprgb->Begin();
  52         striprgb->ClearTo(RgbColor(0), 0, pixelCount-1);
  53         striprgb->Show();
  54     }
  55 }
  56
  57 void WS281BsetLED(int index, uint32_t color)
  58 {
  59     if (OPT_WS2812_IS_GRB)
  60     {
  61         stripgrb->SetPixelColor(index, RgbColor(color >> 16, color >> 8, color));
  62     }
  63     else
  64     {
  65         striprgb->SetPixelColor(index, RgbColor(color >> 16, color >> 8, color));
  66     }
  67 }
  68
  69 void WS281BsetLED(uint32_t color)
  70 {
  71     for (int i=0 ; i<statusLEDcount ; i++)
  72     {
  73         if (OPT_WS2812_IS_GRB)
  74         {
  75             stripgrb->SetPixelColor(statusLEDs[i], RgbColor(color >> 16, color >> 8, color));
  76         }
  77         else
  78         {
  79             striprgb->SetPixelColor(statusLEDs[i], RgbColor(color >> 16, color >> 8, color));
  80         }
  81     }
  82     if (OPT_WS2812_IS_GRB)
  83     {
  84         stripgrb->Show();
  85     }
  86     else
  87     {
  88         striprgb->Show();
  89     }
  90 }
  91
  92 typedef struct {
  93     uint8_t h, s, v;
  94 } blinkyColor_t;
  95
  96 uint32_t HsvToRgb(const blinkyColor_t &blinkyColor)
  97 {
  98     uint8_t region, remainder, p, q, t;
  99
 100     if (blinkyColor.s == 0)
 101     {
 102         return blinkyColor.v << 16 | blinkyColor.v << 8 | blinkyColor.v;
 103     }
 104
 105     region = blinkyColor.h / 43;
 106     remainder = (blinkyColor.h - (region * 43)) * 6;
 107
 108     p = (blinkyColor.v * (255 - blinkyColor.s)) >> 8;
 109     q = (blinkyColor.v * (255 - ((blinkyColor.s * remainder) >> 8))) >> 8;
 110     t = (blinkyColor.v * (255 - ((blinkyColor.s * (255 - remainder)) >> 8))) >> 8;
 111
 112     switch (region)
 113     {
 114         case 0:
 115             return blinkyColor.v << 16 | t << 8 | p;
 116         case 1:
 117             return q << 16 | blinkyColor.v << 8 | p;
 118         case 2:
 119             return p << 16 | blinkyColor.v << 8 | t;
 120         case 3:
 121             return p << 16 | q << 8 | blinkyColor.v;
 122         case 4:
 123             return t << 16 | p << 8 | blinkyColor.v;
 124         default:
 125             return blinkyColor.v << 16 | p << 8 | q;
 126     }
 127 }
 128
 129 void brightnessFadeLED(blinkyColor_t &blinkyColor, uint8_t start, uint8_t end)
 130 {
 131     static uint8_t lightness = 0;
 132     static int8_t dir = 1;
 133
 134     if (lightness <= start)
 135     {
 136         lightness = start;
 137         dir = 1;
 138     }
 139     else if (lightness >= end)
 140     {
 141         lightness = end;
 142         dir = -1;
 143     }
 144
 145     lightness += dir;
 146
 147     blinkyColor.v = lightness;
 148     WS281BsetLED(HsvToRgb(blinkyColor));
 149 }
 150
 151 void hueFadeLED(blinkyColor_t &blinkyColor, uint16_t start, uint16_t end, uint8_t lightness, uint8_t count)
 152 {
 153     static bool hueMode = true;
 154
 155     if (!hueMode)
 156     {
 157         blinkyColor.v--;
 158         if (blinkyColor.v == 0)
 159         {
 160             hueMode = true;
 161         }
 162         WS281BsetLED(HsvToRgb(blinkyColor));
 163     }
 164     else
 165     {
 166         static uint16_t hue = 0;
 167         static int8_t dir = 1;
 168         if (start < end)
 169         {
 170             if (hue <= start)
 171             {
 172                 hue = start;
 173                 dir = 1;
 174             }
 175             else if (hue >= end)
 176             {
 177                 hue = end;
 178                 dir = -1;
 179             }
 180         }
 181         else
 182         {
 183             if (hue >= start)
 184             {
 185                 hue = start;
 186                 dir = -1;
 187             }
 188             else if (hue <= end)
 189             {
 190                 hue = end;
 191                 dir = 1;
 192             }
 193         }
 194
 195         blinkyColor.h = hue % 256;
 196         blinkyColor.v = lightness;
 197         WS281BsetLED(HsvToRgb(blinkyColor));
 198         hue += dir;
 199         if (count != 0 && hue == start)
 200         {
 201             static uint8_t counter = 0;
 202             counter++;
 203             if (counter >= count)
 204             {
 205                 counter = 0;
 206                 hueMode = false;
 207             }
 208         }
 209     }
 210 }
 211
 212 uint16_t flashLED(blinkyColor_t &blinkyColor, uint8_t onLightness, uint8_t offLightness, const uint8_t durations[], uint8_t durationCounts)
 213 {
 214     static int counter = 0;
 215
 216     blinkyColor.v = counter % 2 == 0 ? onLightness : offLightness;
 217     WS281BsetLED(HsvToRgb(blinkyColor));
 218     if (counter >= durationCounts)
 219     {
 220         counter = 0;
 221     }
 222     return durations[counter++] * 10;
 223 }
 224
 225 uint32_t toRGB(uint8_t c)
 226 {
 227   uint32_t r = c & 0xE0 ;
 228   r = ((r << 16) + (r << 13) + (r << 10)) & 0xFF0000;
 229   uint32_t g = c & 0x1C;
 230   g = ((g<< 11) + (g << 8) + (g << 5)) & 0xFF00;
 231   uint32_t b = ((c & 0x3) << 1) + ((c & 0x3) >> 1);
 232   b = (b << 5) + (b << 2) + (b >> 1);
 233   return r+g+b;
 234 }
 235
 236 void setButtonColors(uint8_t b1, uint8_t b2)
 237 {
 238     #if defined(PLATFORM_ESP32) && defined(TARGET_TX)
 239     if (USER_BUTTON_LED != -1)
 240     {
 241         WS281BsetLED(USER_BUTTON_LED, toRGB(b1));
 242     }
 243     if (USER_BUTTON2_LED != -1)
 244     {
 245         WS281BsetLED(USER_BUTTON2_LED, toRGB(b2));
 246     }
 247     #endif
 248 }
 249
 250 static enum {
 251     STARTUP = 0,
 252     NORMAL = 1
 253 } blinkyState;
 254
 255 constexpr uint8_t LEDSEQ_RADIO_FAILED[] = {  10, 10 }; // 100ms on, 100ms off (fast blink)
 256 constexpr uint8_t LEDSEQ_DISCONNECTED[] = { 50, 50 };  // 500ms on, 500ms off
 257 constexpr uint8_t LEDSEQ_NO_CROSSFIRE[] = {  10, 100 }; // 1 blink, 1s pause (one blink/s)
 258 constexpr uint8_t LEDSEQ_BINDING[] = { 10, 10, 10, 100 };   // 2x 100ms blink, 1s pause
 259 constexpr uint8_t LEDSEQ_MODEL_MISMATCH[] = { 10, 10, 10, 10, 10, 100 };   // 3x 100ms blink, 1s pause
 260 constexpr uint8_t LEDSEQ_UPDATE[] = { 20, 5, 5, 5, 5, 40 };   // 200ms on, 2x 50ms off/on, 400ms off
 261
 262 #define NORMAL_UPDATE_INTERVAL 50
 263
 264 static blinkyColor_t blinkyColor;
 265
 266 static int blinkyUpdate() {
 267     static constexpr uint8_t hueStepValue = 1;
 268     static constexpr uint8_t lightnessStep = 5;
 269
 270     if (pixelCount == 1)
 271     {
 272         WS281BsetLED(HsvToRgb(blinkyColor));
 273     }
 274     else
 275     {
 276         blinkyColor_t c = blinkyColor;
 277         for (int i=0 ; i<bootLEDcount ; i++)
 278         {
 279             c.h += 16;
 280             auto color = HsvToRgb(c);
 281             if (OPT_WS2812_IS_GRB)
 282             {
 283                 stripgrb->SetPixelColor(bootLEDs[i], RgbColor(color >> 16, color >> 8, color));
 284             }
 285             else
 286             {
 287                 striprgb->SetPixelColor(bootLEDs[i], RgbColor(color >> 16, color >> 8, color));
 288             }
 289         }
 290         if (OPT_WS2812_IS_GRB)
 291         {
 292             stripgrb->Show();
 293         }
 294         else
 295         {
 296             striprgb->Show();
 297         }
 298     }
 299     if ((int)blinkyColor.h + hueStepValue > 255) {
 300         if ((int)blinkyColor.v - lightnessStep < 0) {
 301             blinkyState = NORMAL;
 302             #if defined(PLATFORM_ESP32) || defined(PLATFORM_ESP8266)
 303             if (pixelCount != 1)
 304             {
 305                 if (OPT_WS2812_IS_GRB)
 306                 {
 307                     stripgrb->ClearTo(RgbColor(0), 0, pixelCount-1);
 308                 }
 309                 else
 310                 {
 311                     striprgb->ClearTo(RgbColor(0), 0, pixelCount-1);
 312                 }
 313             }
 314             #if defined(TARGET_TX)
 315             setButtonColors(config.GetButtonActions(0)->val.color, config.GetButtonActions(1)->val.color);
 316             #endif
 317             if (OPT_WS2812_IS_GRB)
 318             {
 319                 stripgrb->Show();
 320             }
 321             else
 322             {
 323                 striprgb->Show();
 324             }
 325             #endif
 326             return NORMAL_UPDATE_INTERVAL;
 327         }
 328         blinkyColor.v -= lightnessStep;
 329     } else {
 330         blinkyColor.h += hueStepValue;
 331     }
 332     return 3000/(256/hueStepValue);
 333 }
 334
 335 static void initialize()
 336 {
 337     if (GPIO_PIN_LED_WS2812 != UNDEF_PIN)
 338     {
 339         pixelCount = 1;
 340         statusLEDcount = WS2812_STATUS_LEDS_COUNT;
 341         if (statusLEDcount == 0)
 342         {
 343             statusLEDs = new uint8_t[1];
 344             statusLEDs[0] = 0;
 345             statusLEDcount = 1;
 346         }
 347         else
 348         {
 349             statusLEDs = new uint8_t[statusLEDcount];
 350             for (int i=0 ; i<statusLEDcount ; i++)
 351             {
 352                 statusLEDs[i] = WS2812_STATUS_LEDS[i];
 353                 pixelCount = max((int)pixelCount, statusLEDs[i]+1);
 354             }
 355         }
 356
 357         vtxLEDcount = WS2812_VTX_STATUS_LEDS_COUNT;
 358         vtxStatusLEDs = new uint8_t[vtxLEDcount];
 359         for (int i=0 ; i<vtxLEDcount ; i++)
 360         {
 361             vtxStatusLEDs[i] = WS2812_VTX_STATUS_LEDS[i];
 362             pixelCount = max((int)pixelCount, vtxStatusLEDs[i]+1);
 363         }
 364
 365         bootLEDcount = WS2812_BOOT_LEDS_COUNT;
 366         if (bootLEDcount == 0)
 367         {
 368             bootLEDs = statusLEDs;
 369             bootLEDcount = statusLEDcount;
 370         }
 371         else
 372         {
 373             bootLEDs = new uint8_t[bootLEDcount];
 374             for (int i=0 ; i<bootLEDcount ; i++)
 375             {
 376                 bootLEDs[i] = WS2812_BOOT_LEDS[i];
 377                 pixelCount = max((int)pixelCount, bootLEDs[i]+1);
 378             }
 379         }
 380         WS281Binit();
 381         blinkyColor.h = 0;
 382         blinkyColor.s = 255;
 383         blinkyColor.v = 128;
 384     }
 385 }
 386
 387 static int start()
 388 {
 389     if (GPIO_PIN_LED_WS2812 == UNDEF_PIN)
 390     {
 391         return DURATION_NEVER;
 392     }
 393     blinkyState = STARTUP;
 394     #if defined(PLATFORM_ESP32)
 395     // Only do the blinkies if it was NOT a software reboot
 396     if (esp_reset_reason() == ESP_RST_SW) {
 397         blinkyState = NORMAL;
 398         #if defined(TARGET_TX)
 399         setButtonColors(config.GetButtonActions(0)->val.color, config.GetButtonActions(1)->val.color);
 400         #endif
 401         return NORMAL_UPDATE_INTERVAL;
 402     }
 403     #endif
 404     return DURATION_IMMEDIATELY;
 405 }
 406
 407 static int timeout()
 408 {
 409     if (GPIO_PIN_LED_WS2812 == UNDEF_PIN)
 410     {
 411         return DURATION_NEVER;
 412     }
 413     if (blinkyState == STARTUP && connectionState < FAILURE_STATES)
 414     {
 415         return blinkyUpdate();
 416     }
 417 #if defined(TARGET_RX)
 418     if (InBindingMode)
 419     {
 420         blinkyColor.h = 10;
 421         return flashLED(blinkyColor, 192, 0, LEDSEQ_BINDING, sizeof(LEDSEQ_BINDING));
 422     }
 423 #endif
 424     switch (connectionState)
 425     {
 426     case connected:
 427 #if defined(TARGET_RX)
 428         if (!connectionHasModelMatch || !teamraceHasModelMatch)
 429         {
 430             blinkyColor.h = 10;
 431             return flashLED(blinkyColor, 192, 0, LEDSEQ_MODEL_MISMATCH, sizeof(LEDSEQ_MODEL_MISMATCH));
 432         }
 433 #endif
 434         // Set the color and we're done!
 435         blinkyColor.h = ExpressLRS_currAirRate_Modparams->index * 256 / RATE_MAX;
 436         blinkyColor.v = fmap(POWERMGNT::currPower(), 0, PWR_COUNT-1, 10, 128);
 437         WS281BsetLED(HsvToRgb(blinkyColor));
 438         return DURATION_NEVER;
 439     case tentative:
 440         // Set the color and we're done!
 441         blinkyColor.h = ExpressLRS_currAirRate_Modparams->index * 256 / RATE_MAX;
 442         blinkyColor.v = fmap(POWERMGNT::currPower(), 0, PWR_COUNT-1, 10, 50);
 443         WS281BsetLED(HsvToRgb(blinkyColor));
 444         return DURATION_NEVER;
 445     case disconnected:
 446 #if defined(TARGET_RX)
 447         blinkyColor.h = 10;
 448         return flashLED(blinkyColor, 192, 0, LEDSEQ_DISCONNECTED, sizeof(LEDSEQ_DISCONNECTED));
 449 #else
 450         blinkyColor.h = ExpressLRS_currAirRate_Modparams->index * 256 / RATE_MAX;
 451         brightnessFadeLED(blinkyColor, 0, 64);
 452         return NORMAL_UPDATE_INTERVAL;
 453 #endif
 454     case wifiUpdate:
 455         hueFadeLED(blinkyColor, 85, 85-30, 128, 2);      // Yellow->Green cross-fade
 456         return 5;
 457     case serialUpdate:
 458         blinkyColor.h = 172;
 459         return flashLED(blinkyColor, 192, 0, LEDSEQ_UPDATE, sizeof(LEDSEQ_UPDATE));
 460     case bleJoystick:
 461         hueFadeLED(blinkyColor, 170, 170+30, 128, 2);    // Blue cross-fade
 462         return 5;
 463     case radioFailed:
 464         blinkyColor.h = 0;
 465         return flashLED(blinkyColor, 192, 0, LEDSEQ_RADIO_FAILED, sizeof(LEDSEQ_RADIO_FAILED));
 466     case noCrossfire:
 467         blinkyColor.h = 10;
 468         return flashLED(blinkyColor, 192, 0, LEDSEQ_NO_CROSSFIRE, sizeof(LEDSEQ_NO_CROSSFIRE));
 469     default:
 470         return DURATION_NEVER;
 471     }
 472 }
 473
 474 device_t RGB_device = {
 475     .initialize = initialize,
 476     .start = start,
 477     .event = timeout,
 478     .timeout = timeout
 479 };