src/lib/LED/devRGB.cpp

   1 #include "targets.h"
   2 #include "common.h"
   3 #include "device.h"
   4
   5 #if defined(PLATFORM_ESP32) && defined(GPIO_PIN_LED_WS2812) && (GPIO_PIN_LED_WS2812 != UNDEF_PIN)
   6 #include <NeoPixelBus.h>
   7 static constexpr uint16_t PixelCount = 2;
   8 #ifdef WS2812_IS_GRB
   9 static NeoPixelBus<NeoGrbFeature, Neo800KbpsMethod> strip(PixelCount, GPIO_PIN_LED_WS2812);
  10 #else
  11 static NeoPixelBus<NeoRgbFeature, Neo800KbpsMethod> strip(PixelCount, GPIO_PIN_LED_WS2812);
  12 #endif
  13
  14 void WS281Binit()
  15 {
  16     strip.Begin();
  17 }
  18
  19 void WS281BsetLED(uint32_t color)
  20 {
  21     strip.ClearTo(RgbColor(HtmlColor(color)));
  22     strip.Show();
  23 }
  24 #endif
  25
  26 #if defined(PLATFORM_STM32) && (GPIO_PIN_LED_WS2812 != UNDEF_PIN) && (GPIO_PIN_LED_WS2812_FAST != UNDEF_PIN)
  27 #include "STM32F3_WS2812B_LED.h"
  28 #endif
  29
  30
  31 #if defined(GPIO_PIN_LED_WS2812) && (GPIO_PIN_LED_WS2812 != UNDEF_PIN)
  32
  33 #include "logging.h"
  34 #include "crsf_protocol.h"
  35 #include "POWERMGNT.h"
  36
  37 #if defined(TARGET_RX)
  38 extern bool InBindingMode;
  39 extern bool connectionHasModelMatch;
  40 #endif
  41
  42 typedef struct {
  43   uint8_t h, s, v;
  44 } blinkyColor_t;
  45
  46 uint32_t HsvToRgb(blinkyColor_t &blinkyColor)
  47 {
  48     uint8_t region, remainder, p, q, t;
  49
  50     if (blinkyColor.s == 0)
  51     {
  52         return blinkyColor.v << 16 | blinkyColor.v << 8 | blinkyColor.v;
  53     }
  54
  55     region = blinkyColor.h / 43;
  56     remainder = (blinkyColor.h - (region * 43)) * 6;
  57
  58     p = (blinkyColor.v * (255 - blinkyColor.s)) >> 8;
  59     q = (blinkyColor.v * (255 - ((blinkyColor.s * remainder) >> 8))) >> 8;
  60     t = (blinkyColor.v * (255 - ((blinkyColor.s * (255 - remainder)) >> 8))) >> 8;
  61
  62     switch (region)
  63     {
  64         case 0:
  65             return blinkyColor.v << 16 | t << 8 | p;
  66         case 1:
  67             return q << 16 | blinkyColor.v << 8 | p;
  68         case 2:
  69             return p << 16 | blinkyColor.v << 8 | t;
  70         case 3:
  71             return p << 16 | q << 8 | blinkyColor.v;
  72         case 4:
  73             return t << 16 | p << 8 | blinkyColor.v;
  74         default:
  75             return blinkyColor.v << 16 | p << 8 | q;
  76     }
  77 }
  78
  79 void brightnessFadeLED(blinkyColor_t &blinkyColor, uint8_t start, uint8_t end)
  80 {
  81     static uint8_t lightness = 0;
  82     static int8_t dir = 1;
  83
  84     if (lightness <= start)
  85     {
  86         lightness = start;
  87         dir = 1;
  88     }
  89     else if (lightness >= end)
  90     {
  91         lightness = end;
  92         dir = -1;
  93     }
  94
  95     lightness += dir;
  96
  97     blinkyColor.v = lightness;
  98     WS281BsetLED(HsvToRgb(blinkyColor));
  99 }
 100
 101 void hueFadeLED(blinkyColor_t &blinkyColor, uint16_t start, uint16_t end, uint8_t lightness, uint8_t count)
 102 {
 103     static bool hueMode = true;
 104     static uint16_t hue = 0;
 105     static int8_t dir = 1;
 106     static uint8_t counter = 0;
 107
 108     if (!hueMode)
 109     {
 110         blinkyColor.v--;
 111         if (blinkyColor.v == 0)
 112         {
 113             hueMode = true;
 114         }
 115         WS281BsetLED(HsvToRgb(blinkyColor));
 116     }
 117     else
 118     {
 119         if (start < end)
 120         {
 121             if (hue <= start)
 122             {
 123                 hue = start;
 124                 dir = 1;
 125             }
 126             else if (hue >= end)
 127             {
 128                 hue = end;
 129                 dir = -1;
 130             }
 131         }
 132         else
 133         {
 134             if (hue >= start)
 135             {
 136                 hue = start;
 137                 dir = -1;
 138             }
 139             else if (hue <= end)
 140             {
 141                 hue = end;
 142                 dir = 1;
 143             }
 144         }
 145
 146         blinkyColor.h = hue % 256;
 147         blinkyColor.v = lightness;
 148         WS281BsetLED(HsvToRgb(blinkyColor));
 149         hue += dir;
 150         if (count != 0 && hue == start)
 151         {
 152             counter++;
 153             if (counter >= count)
 154             {
 155                 counter = 0;
 156                 hueMode = false;
 157             }
 158         }
 159     }
 160 }
 161
 162 uint16_t flashLED(blinkyColor_t &blinkyColor, uint8_t onLightness, uint8_t offLightness, const uint8_t durations[], uint8_t durationCounts)
 163 {
 164     static int counter = 0;
 165
 166     blinkyColor.v = counter % 2 == 0 ? onLightness : offLightness;
 167     WS281BsetLED(HsvToRgb(blinkyColor));
 168     if (counter >= durationCounts)
 169     {
 170         counter = 0;
 171     }
 172     return durations[counter++] * 10;
 173 }
 174
 175 static enum {
 176     STARTUP = 0,
 177     NORMAL = 1
 178 } blinkyState;
 179
 180 constexpr uint8_t LEDSEQ_RADIO_FAILED[] = {  10, 10 }; // 100ms on, 100ms off (fast blink)
 181 constexpr uint8_t LEDSEQ_DISCONNECTED[] = { 50, 50 };  // 500ms on, 500ms off
 182 constexpr uint8_t LEDSEQ_NO_CROSSFIRE[] = {  10, 100 }; // 1 blink, 1s pause (one blink/s)
 183 constexpr uint8_t LEDSEQ_BINDING[] = { 10, 10, 10, 100 };   // 2x 100ms blink, 1s pause
 184 constexpr uint8_t LEDSEQ_MODEL_MISMATCH[] = { 10, 10, 10, 10, 10, 100 };   // 3x 100ms blink, 1s pause
 185
 186 #define NORMAL_UPDATE_INTERVAL 50
 187
 188 constexpr uint8_t rate_hue[RATE_MAX] =
 189 {
 190     170,     // 500/200 hz  blue
 191     85,      // 250/100 hz  green
 192     21,      // 150/50 hz   orange
 193     0        // 50/25 hz    red
 194 };
 195
 196 static blinkyColor_t blinkyColor;
 197
 198 static int blinkyUpdate() {
 199     static constexpr uint8_t hueStepValue = 1;
 200     static constexpr uint8_t lightnessStep = 5;
 201
 202     WS281BsetLED(HsvToRgb(blinkyColor));
 203     if ((int)blinkyColor.h + hueStepValue > 255) {
 204         if ((int)blinkyColor.v - lightnessStep < 0) {
 205             blinkyState = NORMAL;
 206             return NORMAL_UPDATE_INTERVAL;
 207         }
 208         blinkyColor.v -= lightnessStep;
 209     } else {
 210         blinkyColor.h += hueStepValue;
 211     }
 212     return 3000/(256/hueStepValue);
 213 }
 214
 215 static void initialize()
 216 {
 217     WS281Binit();
 218     blinkyColor.h = 0;
 219     blinkyColor.s = 255;
 220     blinkyColor.v = 128;
 221 }
 222
 223 static int start()
 224 {
 225     blinkyState = STARTUP;
 226     #ifdef PLATFORM_ESP32
 227     // Only do the blinkies if it was NOT a software reboot
 228     if (esp_reset_reason() == ESP_RST_SW) {
 229         blinkyState = NORMAL;
 230         return NORMAL_UPDATE_INTERVAL;
 231     }
 232     #endif
 233     return DURATION_IMMEDIATELY;
 234 }
 235
 236 static int timeout()
 237 {
 238     if (blinkyState == STARTUP && connectionState < FAILURE_STATES)
 239     {
 240         return blinkyUpdate();
 241     }
 242     #if defined(TARGET_RX)
 243         if (InBindingMode)
 244         {
 245             blinkyColor.h = 10;
 246             return flashLED(blinkyColor, 192, 0, LEDSEQ_BINDING, sizeof(LEDSEQ_BINDING));
 247         }
 248     #endif
 249     switch (connectionState)
 250     {
 251     case connected:
 252         #if defined(TARGET_RX)
 253             if (!connectionHasModelMatch)
 254             {
 255                 blinkyColor.h = 10;
 256                 return flashLED(blinkyColor, 192, 0, LEDSEQ_MODEL_MISMATCH, sizeof(LEDSEQ_MODEL_MISMATCH));
 257             }
 258         #endif
 259         // Set the color and we're done!
 260         blinkyColor.h = rate_hue[ExpressLRS_currAirRate_Modparams->index];
 261         blinkyColor.v = fmap(POWERMGNT::currPower(), 0, PWR_COUNT-1, 10, 128);
 262         WS281BsetLED(HsvToRgb(blinkyColor));
 263         return DURATION_NEVER;
 264     case tentative:
 265         // Set the color and we're done!
 266         blinkyColor.h = rate_hue[ExpressLRS_currAirRate_Modparams->index];
 267         blinkyColor.v = fmap(POWERMGNT::currPower(), 0, PWR_COUNT-1, 10, 50);
 268         WS281BsetLED(HsvToRgb(blinkyColor));
 269         return DURATION_NEVER;
 270     case disconnected:
 271         #if defined(TARGET_RX)
 272             blinkyColor.h = 10;
 273             return flashLED(blinkyColor, 192, 0, LEDSEQ_DISCONNECTED, sizeof(LEDSEQ_DISCONNECTED));
 274         #endif
 275         #if defined(TARGET_TX)
 276             blinkyColor.h = rate_hue[ExpressLRS_currAirRate_Modparams->index];
 277             brightnessFadeLED(blinkyColor, 0, 64);
 278             return NORMAL_UPDATE_INTERVAL;
 279         #endif
 280     case wifiUpdate:
 281         hueFadeLED(blinkyColor, 85, 85-30, 128, 2);      // Yellow->Green cross-fade
 282         return 5;
 283     case bleJoystick:
 284         hueFadeLED(blinkyColor, 170, 170+30, 128, 2);    // Blue cross-fade
 285         return 5;
 286     case radioFailed:
 287         blinkyColor.h = 0;
 288         return flashLED(blinkyColor, 192, 0, LEDSEQ_RADIO_FAILED, sizeof(LEDSEQ_RADIO_FAILED));
 289     case noCrossfire:
 290         blinkyColor.h = 10;
 291         return flashLED(blinkyColor, 192, 0, LEDSEQ_NO_CROSSFIRE, sizeof(LEDSEQ_NO_CROSSFIRE));
 292     default:
 293         return DURATION_NEVER;
 294     }
 295 }
 296
 297 device_t RGB_device = {
 298     .initialize = initialize,
 299     .start = start,
 300     .event = timeout,
 301     .timeout = timeout
 302 };
 303
 304 #else
 305
 306 device_t RGB_device = {
 307     NULL
 308 };
 309
 310 #endif