diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index bd90a3ae8c5..25a027b29ef 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -55,10 +55,12 @@ #include "fc/runtime_config.h" #include "flight/failsafe.h" +#include "flight/gps_rescue.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" #include "flight/servos.h" +#include "flight/position.h" #include "io/beeper.h" #include "io/gimbal.h" @@ -277,6 +279,34 @@ STATIC_UNIT_TESTED void updateDimensions(void) } +enum ledBarIds { + LED_BAR_GPS, + LED_BAR_BATTERY, + LED_BAR_COUNT +}; +static uint8_t ledBarStates[LED_BAR_COUNT] = {0}; + +void updateLedBars(void) +{ + memset(ledBarStates, 0, sizeof(ledBarStates)); + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex]; + int fn = ledGetFunction(ledConfig); + switch (fn) { +#ifdef USE_GPS + case LED_FUNCTION_GPS_BAR: + ledBarStates[LED_BAR_GPS]++; + break; +#endif + case LED_FUNCTION_BATTERY_BAR: + ledBarStates[LED_BAR_BATTERY]++; + break; + default: + break; + } + } +} + STATIC_UNIT_TESTED void updateLedCount(void) { int count = 0, countRing = 0, countScanner= 0; @@ -308,6 +338,7 @@ void reevaluateLedConfig(void) updateDimensions(); updateLedRingCounts(); updateRequiredOverlay(); + updateLedBars(); } // get specialColor by index @@ -316,9 +347,35 @@ static const hsvColor_t* getSC(ledSpecialColorIds_e index) return &ledStripStatusModeConfig()->colors[ledStripStatusModeConfig()->specialColors.color[index]]; } -static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' }; -static const char baseFunctionCodes[LED_BASEFUNCTION_COUNT] = { 'C', 'F', 'A', 'L', 'S', 'G', 'R' }; -static const char overlayCodes[LED_OVERLAY_COUNT] = { 'T', 'Y', 'O', 'B', 'V', 'I', 'W' }; +static const char directionCodes[LED_DIRECTION_COUNT] = { + [LED_DIRECTION_NORTH] = 'N', + [LED_DIRECTION_EAST] = 'E', + [LED_DIRECTION_SOUTH] = 'S', + [LED_DIRECTION_WEST] = 'W', + [LED_DIRECTION_UP] = 'U', + [LED_DIRECTION_DOWN] = 'D' +}; +static const char baseFunctionCodes[LED_BASEFUNCTION_COUNT] = { + [LED_FUNCTION_COLOR] = 'C', + [LED_FUNCTION_FLIGHT_MODE] = 'F', + [LED_FUNCTION_ARM_STATE] = 'A', + [LED_FUNCTION_BATTERY] = 'L', + [LED_FUNCTION_RSSI] = 'S', + [LED_FUNCTION_GPS] = 'G', + [LED_FUNCTION_THRUST_RING] = 'R', + [LED_FUNCTION_GPS_BAR] = 'P', + [LED_FUNCTION_BATTERY_BAR] = 'E', + [LED_FUNCTION_ALTITUDE] = 'U' +}; +static const char overlayCodes[LED_OVERLAY_COUNT] = { + [LED_OVERLAY_THROTTLE] = 'T', + [LED_OVERLAY_RAINBOW] = 'Y', + [LED_OVERLAY_LARSON_SCANNER] = 'O', + [LED_OVERLAY_BLINK] = 'B', + [LED_OVERLAY_VTX] = 'V', + [LED_OVERLAY_INDICATOR] = 'I', + [LED_OVERLAY_WARNING] = 'W' +}; #define CHUNK_BUFFER_SIZE 11 bool parseLedStripConfig(int ledIndex, const char *config) @@ -496,6 +553,8 @@ static const struct { static void applyLedFixedLayers(void) { + uint8_t ledBarCounters[LED_BAR_COUNT] = {0}; + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex]; hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); @@ -543,10 +602,39 @@ static void applyLedFixedLayers(void) break; case LED_FUNCTION_BATTERY: + case LED_FUNCTION_BATTERY_BAR: color = HSV(RED); hOffset += MAX(scaleRange(calculateBatteryPercentageRemaining(), 0, 100, -30, 120), 0); break; +#ifdef USE_GPS + case LED_FUNCTION_GPS_BAR: + { + uint8_t minSats = 8; +#ifdef USE_GPS_RESCUE + minSats = gpsRescueConfig()->minSats; +#endif + if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) { + color = HSV(RED); + } else { + if (gpsSol.numSat >= minSats) { + color = HSV(GREEN); + } else { + color = HSV(RED); + hOffset += MAX(scaleRange(gpsSol.numSat, 0, minSats, -30, 120), 0); + } + } + break; + } +#endif + +#if defined(USE_BARO) || defined(USE_GPS) + case LED_FUNCTION_ALTITUDE: + color = ledStripStatusModeConfig()->colors[ledGetColor(ledConfig)]; + hOffset += MAX(scaleRange(getEstimatedAltitudeCm(), 0, 500, -30, 120), 0); + break; +#endif + case LED_FUNCTION_RSSI: color = HSV(RED); hOffset += MAX(scaleRange(getRssiPercent(), 0, 100, -30, 120), 0); @@ -560,9 +648,32 @@ static void applyLedFixedLayers(void) const int auxInput = rcData[ledStripStatusModeConfig()->ledstrip_aux_channel]; hOffset += scaleRange(auxInput, PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1); } - color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1); - setLedHsv(ledIndex, &color); + + switch (fn) { +#ifdef USE_GPS + case LED_FUNCTION_GPS_BAR: + if (ledBarCounters[LED_BAR_GPS] < gpsSol.numSat || ledBarCounters[LED_BAR_GPS] == 0) { + ledBarCounters[LED_BAR_GPS]++; + setLedHsv(ledIndex, &color); + } else { + setLedHsv(ledIndex, getSC(LED_SCOLOR_BACKGROUND)); + } + break; +#endif + case LED_FUNCTION_BATTERY_BAR: + if (ledBarCounters[LED_BAR_BATTERY] < (calculateBatteryPercentageRemaining() * ledBarStates[LED_BAR_BATTERY]) / 100 || ledBarCounters[LED_BAR_BATTERY] == 0) { + ledBarCounters[LED_BAR_BATTERY]++; + setLedHsv(ledIndex, &color); + } else { + setLedHsv(ledIndex, getSC(LED_SCOLOR_BACKGROUND)); + } + break; + + default: + setLedHsv(ledIndex, &color); + break; + } } } diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 308475132f0..fc6d467a6c0 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -33,7 +33,7 @@ #define LED_CONFIGURABLE_COLOR_COUNT 16 #define LED_MODE_COUNT 6 #define LED_DIRECTION_COUNT 6 -#define LED_BASEFUNCTION_COUNT 7 +#define LED_BASEFUNCTION_COUNT 10 #define LED_OVERLAY_COUNT 7 #define LED_SPECIAL_COLOR_COUNT 11 @@ -129,7 +129,10 @@ typedef enum { LED_FUNCTION_BATTERY, LED_FUNCTION_RSSI, LED_FUNCTION_GPS, - LED_FUNCTION_THRUST_RING + LED_FUNCTION_THRUST_RING, + LED_FUNCTION_GPS_BAR, + LED_FUNCTION_BATTERY_BAR, + LED_FUNCTION_ALTITUDE } ledBaseFunctionId_e; typedef enum { diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index c557b86e415..0179a25199b 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -391,6 +391,7 @@ bool rxIsReceivingSignal() { return true; } bool isBeeperOn() { return false; }; uint8_t calculateBatteryPercentageRemaining() { return 0; } +uint32_t getEstimatedAltitudeCm() { return 0; } bool sensors(uint32_t mask) {