From 53e872cee8ad33e5b98968ebdc6d15b7d2516b23 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20James?= Date: Sun, 24 Jul 2016 09:49:37 +0200 Subject: [PATCH] LED strip rework (#362) * Implement new LED strip functions from CF and BF. * Warnings: blink to black instead of function color --- docs/LedStrip.md | 170 +- src/main/common/utils.h | 11 + src/main/config/config.c | 5 +- src/main/config/config_master.h | 8 +- src/main/drivers/light_ws2811strip.h | 3 - .../drivers/light_ws2811strip_stm32f10x.c | 2 + .../drivers/light_ws2811strip_stm32f30x.c | 2 + src/main/io/ledstrip.c | 1608 ++++++++++------- src/main/io/ledstrip.h | 207 ++- src/main/io/msp_protocol.h | 2 +- src/main/io/serial_cli.c | 61 +- src/main/io/serial_msp.c | 70 +- src/main/main.c | 4 +- src/main/sensors/battery.c | 2 +- src/test/unit/ledstrip_unittest.cc | 249 +-- 15 files changed, 1453 insertions(+), 951 deletions(-) diff --git a/docs/LedStrip.md b/docs/LedStrip.md index 57805fa5a8..81775f6bb0 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -12,14 +12,12 @@ supports the following: * Heading/Orientation lights. * Flight mode specific color schemes. * Low battery warning. -* AUX operated on/off switch +* AUX operated on/off switch. +* GPS state. +* RSSI level. +* Battery level. -The function and orientation configuration is fixed for now but later it should be able to be set via the UI or CLI.. - -In the future, if someone codes it, they could be used to show GPS navigation status, thrust levels, RSSI, etc. -Lots of scope for ideas and improvements. - -Likewise, support for more than 32 LEDs is possible, it just requires additional development. +Support for more than 32 LEDs is possible, it just requires additional development. ## Supported hardware @@ -71,12 +69,12 @@ The datasheet can be found here: http://www.adafruit.com/datasheets/WS2812.pdf ## Configuration -The led strip feature can be configured via the GUI +The led strip feature can be configured via the GUI. GUI: Enable the Led Strip feature via the GUI under setup. -Configure the leds from the Led Strip tab in the cleanflight GUI. +Configure the leds from the Led Strip tab in the INAV GUI. First setup how the led's are laid out so that you can visualize it later as you configure and so the flight controller knows how many led's there are available. There is a step by step guide on how to use the GUI to configure the Led Strip feature using the GUI http://blog.oscarliang.net/setup-rgb-led-cleanflight/ which was published early 2015 by Oscar Liang which may or may not be up-to-date by the time you read this. @@ -112,27 +110,39 @@ For instance, an LED that faces South-east at a 45 degree downwards angle could Note: It is perfectly possible to configure an LED to have all directions `NESWUD` but probably doesn't make sense. -`mmm` specifies the modes that should be applied an LED. Modes are: +`mmm` specifies the modes that should be applied an LED. + +Each LED has one base function: + +* `C` - `C`olor. +* `F` - `F`light mode & Orientation +* `A` - `A`rmed state. +* `R` - `R`ing thrust state. +* `G` - `G`PS state. +* `S` - R`S`SSI level. +* `L` - Battery `L`evel. + +And each LED has overlays: * `W` - `W`warnings. -* `F` - `F`light mode & Orientation * `I` - `I`ndicator. -* `A` - `A`rmed state. * `T` - `T`hrust state. -* `R` - `R`ing thrust state. -* `C` - `C`olor. +* `B` - `B`link (flash twice) mode. +* `O` - Lars`O`n Scanner (Cylon Effect). +* `N` - Blink on la`N`ding (throttle < 50%). `cc` specifies the color number (0 based index). Example: ``` -led 0 0,15:SD:IAW:0 -led 1 15,0:ND:IAW:0 -led 2 0,0:ND:IAW:0 -led 3 0,15:SD:IAW:0 +led 0 0,15:SD:AWI:0 +led 1 15,0:ND:AWI:0 +led 2 0,0:ND:AWI:0 +led 3 0,15:SD:AWI:0 led 4 7,7::C:1 led 5 8,8::C:2 +led 6 8,9::B:1 ``` To erase an led, and to mark the end of the chain, use `0,0::` as the second argument, like this: @@ -157,6 +167,61 @@ This mode simply uses the LEDs to flash when warnings occur. Flash patterns appear in order, so that it's clear which warnings are enabled. +#### GPS state + +This mode shows the GPS state and satellite count. + +No fix = red LED +3D fix = green LED + +The LEDs will blink as many times as the satellite count, then pause and start again. + +#### RSSI level + +This mode binds the LED color to RSSI level. + +| Color | RSSI | +| ---------- | ---------| +| Green | 100% | +| Lime green | 80% | +| Yellow | 60% | +| Orange | 40% | +| Red | 20% | +| Deep pink | 0% | + +When RSSI is below 50% is reached, LEDs will blink slowly, and they will blink fast when under 20%. + + +#### Battery level + +This mode binds the LED color to remaining battery capacity. + +| Color | Capacity | +| ---------- | ---------| +| Green | 100% | +| Lime green | 80% | +| Yellow | 60% | +| Orange | 40% | +| Red | 20% | +| Deep pink | 0% | + +When Warning or Critial voltage is reached, LEDs will blink slowly or fast. +Note: this mode requires a current sensor. If you don't have the actual device you can set up a virtual current sensor (see [Battery](Battery.md)). + +#### Blink + +This mode blinks the current LED, alternatively from black to the current active color. + +#### Blink on landing + +This mode blinks the current LED, alternatively from black to the current active color, when throttle is below 50% and the craft is armed. + +#### Larson Scanner (Cylon Effect) + +The Larson Scanner replicates the scanning "eye" effect seen on the mechanical Cylons and on Kitt from Knight Rider. + +This overlay merely varies the brightness of each LED's current color. + #### Flight Mode & Orientation This mode shows the flight mode and orientation. @@ -234,7 +299,7 @@ the same time. Thrust should normally be combined with Color or Mode/Orientatio #### Thrust ring state -This mode is allows you to use a 12, 16 or 24 leds ring (e.g. NeoPixel ring) for an afterburner effect. When armed the leds use the following sequences: 2 On, 4 Off, 2 On, 4 Off, and so on. The light pattern rotates clockwise as throttle increases. +This mode is allows you to use one or multiple led rings (e.g. NeoPixel ring) for an afterburner effect. The light pattern rotates clockwise as throttle increases. A better effect is acheived when LEDs configured for thrust ring have no other functions. @@ -242,7 +307,7 @@ LED direction and X/Y positions are irrelevant for thrust ring LED state. The o Each LED of the ring can be a different color. The color can be selected between the 16 colors availables. -For example, led 0 is set as a `R`ing thrust state led in color 13 as follow. +For example, led 0 is set as a `R`ing thrust state led in color 13 as follow. ``` led 0 2,2::R:13 @@ -258,7 +323,7 @@ x,y position and directions are ignored when using this mode. Other modes will override or combine with the color mode. -For example, to set led 0 to always use color 10 you would issue this command. +For example, to set led 0 to always use color 10 you would issue this command. ``` led 0 0,0::C:10 @@ -314,6 +379,57 @@ color 14 0,0,0 color 15 0,0,0 ``` +### Mode Colors Assignement + +Mode Colors can be configured using the cli `mode_color` command. + +- No arguments: lists all mode colors +- arguments: mode, function, color + +First 6 groups of ModeIndexes are : + +| mode | name | +|------|-------------| +| 0 | orientation | +| 1 | headfree | +| 2 | horizon | +| 3 | angle | +| 4 | mag | +| 5 | baro | +| 6 | special | + +Modes 0 to 5 functions: + +| function | name | +|----------|-------| +| 0 | north | +| 1 | east | +| 2 | south | +| 3 | west | +| 4 | up | +| 5 | down | + +Mode 6 use these functions: + +| function | name | +|----------|--------------------| +| 0 | disarmed | +| 1 | armed | +| 2 | animation | +| 3 | background | +| 4 | blink background | +| 5 | gps: no satellites | +| 6 | gps: no fix | +| 7 | gps: 3D fix | + +The ColorIndex is picked from the colors array ("palette"). + +Examples (using the default colors): + +- set armed color to red: ```mode_color 6 1 2``` +- set disarmed color to yellow: ```mode_color 6 0 4``` +- set Headfree mode 'south' to Cyan: ```mode_color 1 2 8``` + ## Positioning Cut the strip into sections as per diagrams below. When the strips are cut ensure you reconnect each output to each input with cable where the break is made. @@ -407,13 +523,13 @@ Which translates into the following positions: ``` 6 3 - \ / - \ 5-4 / + \ / + \ 5-4 / 7 \ FRONT / 2 - | 12-15 | + | 12-15 | 8 / BACK \ 1 / 10-11 \ - / \ + / \ 9 0 ``` @@ -421,7 +537,7 @@ LEDs 0,3,6 and 9 should be placed underneath the quad, facing downwards. LEDs 1-2, 4-5, 7-8 and 10-11 should be positioned so the face east/north/west/south, respectively. LEDs 12-13 should be placed facing down, in the middle LEDs 14-15 should be placed facing up, in the middle - + ### Exmple 28 LED config ``` @@ -463,7 +579,7 @@ led 27 2,9:S:FWT:0 ``` 16-18 9-11 19-21 \ / 6-8 - \ 12-15 / + \ 12-15 / \ FRONT / / BACK \ / \ diff --git a/src/main/common/utils.h b/src/main/common/utils.h index db9e8c0c52..34b2df2d61 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -71,4 +71,15 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html static inline int16_t cmp16(uint16_t a, uint16_t b) { return a-b; } static inline int32_t cmp32(uint32_t a, uint32_t b) { return a-b; } +// using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions +// than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions) +#ifdef UNIT_TEST +// Call memcpy when building unittest - this is easier that asm symbol name mangling (symbols start with _underscore on win32) +#include +static inline void memcpy_fn ( void * destination, const void * source, size_t num ) { memcpy(destination, source, num); }; +#else +void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("memcpy"); +#endif + + #endif diff --git a/src/main/config/config.c b/src/main/config/config.c index 5aaa90abbf..145d672fa5 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -616,8 +616,11 @@ static void resetConf(void) masterConfig.customMotorMixer[i].throttle = 0.0f; #ifdef LED_STRIP - applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT); + applyDefaultColors(masterConfig.colors); applyDefaultLedStripConfig(masterConfig.ledConfigs); + applyDefaultModeColors(masterConfig.modeColors); + applyDefaultSpecialColors(&(masterConfig.specialColors)); + masterConfig.ledstrip_visual_beeper = 0; #endif #ifdef BLACKBOX diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 259383d7be..3f4aece685 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -94,9 +94,13 @@ typedef struct master_t { telemetryConfig_t telemetryConfig; + #ifdef LED_STRIP - ledConfig_t ledConfigs[MAX_LED_STRIP_LENGTH]; - hsvColor_t colors[CONFIGURABLE_COLOR_COUNT]; + ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH]; + hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT]; + modeColorIndexes_t modeColors[LED_MODE_COUNT]; + specialColorIndexes_t specialColors; + uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on #endif profile_t profile[MAX_PROFILE_COUNT]; diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 88282ca593..83cb36db13 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -48,6 +48,3 @@ bool isWS2811LedStripReady(void); extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; extern volatile uint8_t ws2811LedDataTransferInProgress; - -extern const hsvColor_t hsv_white; -extern const hsvColor_t hsv_black; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 2b0de69bdb..a5dede77ba 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -100,6 +100,8 @@ void ws2811LedStripHardwareInit(void) DMA_ITConfig(DMA1_Channel6, DMA_IT_TC, ENABLE); + + const hsvColor_t hsv_white = { 0, 255, 255}; NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel6_IRQn; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 7404f8e539..f72876de40 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -112,6 +112,8 @@ void ws2811LedStripHardwareInit(void) DMA_ITConfig(WS2811_DMA_CHANNEL, DMA_IT_TC, ENABLE); + + const hsvColor_t hsv_white = { 0, 255, 255}; NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel = WS2811_IRQ; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 4ea94febec..decdcdc876 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -34,20 +34,53 @@ #include "drivers/light_ws2811strip.h" #include "drivers/system.h" #include "drivers/serial.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/gpio.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" #include - -#include "sensors/battery.h" +#include "common/axis.h" +#include "common/utils.h" #include "io/rc_controls.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" +#include "sensors/boardalignment.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" + #include "io/ledstrip.h" +#include "io/beeper.h" +#include "io/escservo.h" +#include "io/gimbal.h" +#include "io/serial.h" +#include "io/gps.h" #include "rx/rx.h" #include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation_rewrite.h" + +#include "telemetry/telemetry.h" #include "config/runtime_config.h" #include "config/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +/* +PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); +PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0); +PG_REGISTER_ARR_WITH_RESET_FN(modeColorIndexes_t, LED_MODE_COUNT, modeColors, PG_MODE_COLOR_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(specialColorIndexes_t, specialColors, PG_SPECIAL_COLOR_CONFIG, 0); +*/ static bool ledStripInitialised = false; static bool ledStripEnabled = true; @@ -57,73 +90,13 @@ static void ledStripDisable(void); //#define USE_LED_ANIMATION //#define USE_LED_RING_DEFAULT_CONFIG -// timers -#ifdef USE_LED_ANIMATION -static uint32_t nextAnimationUpdateAt = 0; +#define LED_STRIP_HZ(hz) ((int32_t)((1000 * 1000) / (hz))) +#define LED_STRIP_MS(ms) ((int32_t)(1000 * (ms))) + +#if LED_MAX_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH +# error "Led strip length must match driver" #endif -static uint32_t nextIndicatorFlashAt = 0; -static uint32_t nextWarningFlashAt = 0; -static uint32_t nextRotationUpdateAt = 0; - -#define LED_STRIP_20HZ ((1000 * 1000) / 20) -#define LED_STRIP_10HZ ((1000 * 1000) / 10) -#define LED_STRIP_5HZ ((1000 * 1000) / 5) - -#if MAX_LED_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH -#error "Led strip length must match driver" -#endif - -// H S V -#define LED_BLACK { 0, 0, 0} -#define LED_WHITE { 0, 255, 255} -#define LED_RED { 0, 0, 255} -#define LED_ORANGE { 30, 0, 255} -#define LED_YELLOW { 60, 0, 255} -#define LED_LIME_GREEN { 90, 0, 255} -#define LED_GREEN {120, 0, 255} -#define LED_MINT_GREEN {150, 0, 255} -#define LED_CYAN {180, 0, 255} -#define LED_LIGHT_BLUE {210, 0, 255} -#define LED_BLUE {240, 0, 255} -#define LED_DARK_VIOLET {270, 0, 255} -#define LED_MAGENTA {300, 0, 255} -#define LED_DEEP_PINK {330, 0, 255} - -const hsvColor_t hsv_black = LED_BLACK; -const hsvColor_t hsv_white = LED_WHITE; -const hsvColor_t hsv_red = LED_RED; -const hsvColor_t hsv_orange = LED_ORANGE; -const hsvColor_t hsv_yellow = LED_YELLOW; -const hsvColor_t hsv_limeGreen = LED_LIME_GREEN; -const hsvColor_t hsv_green = LED_GREEN; -const hsvColor_t hsv_mintGreen = LED_MINT_GREEN; -const hsvColor_t hsv_cyan = LED_CYAN; -const hsvColor_t hsv_lightBlue = LED_LIGHT_BLUE; -const hsvColor_t hsv_blue = LED_BLUE; -const hsvColor_t hsv_darkViolet = LED_DARK_VIOLET; -const hsvColor_t hsv_magenta = LED_MAGENTA; -const hsvColor_t hsv_deepPink = LED_DEEP_PINK; - -#define LED_DIRECTION_COUNT 6 - -const hsvColor_t * const defaultColors[] = { - &hsv_black, - &hsv_white, - &hsv_red, - &hsv_orange, - &hsv_yellow, - &hsv_limeGreen, - &hsv_green, - &hsv_mintGreen, - &hsv_cyan, - &hsv_lightBlue, - &hsv_blue, - &hsv_darkViolet, - &hsv_magenta, - &hsv_deepPink -}; - typedef enum { COLOR_BLACK = 0, COLOR_WHITE, @@ -139,98 +112,134 @@ typedef enum { COLOR_DARK_VIOLET, COLOR_MAGENTA, COLOR_DEEP_PINK, -} colorIds; +} colorId_e; -typedef enum { - DIRECTION_NORTH = 0, - DIRECTION_EAST, - DIRECTION_SOUTH, - DIRECTION_WEST, - DIRECTION_UP, - DIRECTION_DOWN -} directionId_e; - -typedef struct modeColorIndexes_s { - uint8_t north; - uint8_t east; - uint8_t south; - uint8_t west; - uint8_t up; - uint8_t down; -} modeColorIndexes_t; - - -// Note, the color index used for the mode colors below refer to the default colors. -// if the colors are reconfigured the index is still valid but the displayed color might -// be different. -// See colors[] and defaultColors[] and applyDefaultColors[] - -static const modeColorIndexes_t orientationModeColors = { - COLOR_WHITE, - COLOR_DARK_VIOLET, - COLOR_RED, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +const hsvColor_t hsv[] = { + // H S V + [COLOR_BLACK] = { 0, 0, 0}, + [COLOR_WHITE] = { 0, 255, 255}, + [COLOR_RED] = { 0, 0, 255}, + [COLOR_ORANGE] = { 30, 0, 255}, + [COLOR_YELLOW] = { 60, 0, 255}, + [COLOR_LIME_GREEN] = { 90, 0, 255}, + [COLOR_GREEN] = {120, 0, 255}, + [COLOR_MINT_GREEN] = {150, 0, 255}, + [COLOR_CYAN] = {180, 0, 255}, + [COLOR_LIGHT_BLUE] = {210, 0, 255}, + [COLOR_BLUE] = {240, 0, 255}, + [COLOR_DARK_VIOLET] = {270, 0, 255}, + [COLOR_MAGENTA] = {300, 0, 255}, + [COLOR_DEEP_PINK] = {330, 0, 255}, }; +// macro to save typing on default colors +#define HSV(color) (hsv[COLOR_ ## color]) -static const modeColorIndexes_t headfreeModeColors = { - COLOR_LIME_GREEN, - COLOR_DARK_VIOLET, - COLOR_ORANGE, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +STATIC_UNIT_TESTED uint8_t ledGridWidth; +STATIC_UNIT_TESTED uint8_t ledGridHeight; +// grid offsets +STATIC_UNIT_TESTED uint8_t highestYValueForNorth; +STATIC_UNIT_TESTED uint8_t lowestYValueForSouth; +STATIC_UNIT_TESTED uint8_t highestXValueForWest; +STATIC_UNIT_TESTED uint8_t lowestXValueForEast; + +STATIC_UNIT_TESTED ledCounts_t ledCounts; + +// macro for initializer +#define LF(name) LED_FUNCTION_ ## name +#define LO(name) LED_FLAG_OVERLAY(LED_OVERLAY_ ## name) +#define LD(name) LED_FLAG_DIRECTION(LED_DIRECTION_ ## name) + +#ifdef USE_LED_RING_DEFAULT_CONFIG +static const ledConfig_t defaultLedStripConfig[] = { + DEFINE_LED( 2, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 2, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 2, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), }; +#else +static const ledConfig_t defaultLedStripConfig[] = { + DEFINE_LED(15, 15, 0, LD(SOUTH) | LD(EAST), LF(ARM_STATE), LO(INDICATOR), 0), -static const modeColorIndexes_t horizonModeColors = { - COLOR_BLUE, - COLOR_DARK_VIOLET, - COLOR_YELLOW, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE -}; + DEFINE_LED(15, 8, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED(15, 7, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), -static const modeColorIndexes_t angleModeColors = { - COLOR_CYAN, - COLOR_DARK_VIOLET, - COLOR_YELLOW, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE -}; + DEFINE_LED(15, 0, 0, LD(NORTH) | LD(EAST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 8, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), + DEFINE_LED( 7, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), + + DEFINE_LED( 0, 0, 0, LD(NORTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 0, 7, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 0, 8, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 0, 15, 0, LD(SOUTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 7, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 7, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 7, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 8, 9, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 9, 10, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED(10, 11, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED(10, 12, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 9, 13, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 8, 14, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 7, 14, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 6, 13, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 5, 12, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 5, 11, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 6, 10, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 7, 9, 3, 0, LF(THRUST_RING), 0, 0), -#ifdef MAG -static const modeColorIndexes_t magModeColors = { - COLOR_MINT_GREEN, - COLOR_DARK_VIOLET, - COLOR_ORANGE, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE }; #endif -static const modeColorIndexes_t navigationModeColors = { - COLOR_LIGHT_BLUE, - COLOR_DARK_VIOLET, - COLOR_RED, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +#undef LD +#undef LF +#undef LO + +static const modeColorIndexes_t defaultModeColors[] = { + // NORTH EAST SOUTH WEST UP DOWN + [LED_MODE_ORIENTATION] = {{ COLOR_WHITE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_HEADFREE] = {{ COLOR_LIME_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_HORIZON] = {{ COLOR_BLUE, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_ANGLE] = {{ COLOR_CYAN, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_MAG] = {{ COLOR_MINT_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_BARO] = {{ COLOR_LIGHT_BLUE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, +}; + +static const specialColorIndexes_t defaultSpecialColors[] = { + {{ [LED_SCOLOR_DISARMED] = COLOR_GREEN, + [LED_SCOLOR_ARMED] = COLOR_BLUE, + [LED_SCOLOR_ANIMATION] = COLOR_WHITE, + [LED_SCOLOR_BACKGROUND] = COLOR_BLACK, + [LED_SCOLOR_BLINKBACKGROUND] = COLOR_BLACK, + [LED_SCOLOR_GPSNOSATS] = COLOR_RED, + [LED_SCOLOR_GPSNOLOCK] = COLOR_ORANGE, + [LED_SCOLOR_GPSLOCKED] = COLOR_GREEN, + }} }; -uint8_t ledGridWidth; -uint8_t ledGridHeight; -uint8_t ledCount; -uint8_t ledsInRingCount; - -ledConfig_t *ledConfigs; -hsvColor_t *colors; +static int scaledThrottle; + + +/* #ifdef USE_LED_RING_DEFAULT_CONFIG const ledConfig_t defaultLedStripConfig[] = { { CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING}, @@ -246,6 +255,19 @@ const ledConfig_t defaultLedStripConfig[] = { { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, }; +#elif defined(USE_COLIBTI_RACE_LED_DEFAULT_CONFIG) +const ledConfig_t defaultLedStripConfig[] = { + { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, +}; #else const ledConfig_t defaultLedStripConfig[] = { { CALCULATE_LED_XY(15, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, @@ -288,594 +310,774 @@ const ledConfig_t defaultLedStripConfig[] = { }; #endif +*/ -/* - * 6 coords @nn,nn - * 4 direction @## - * 6 modes @#### - * = 16 bytes per led - * 16 * 32 leds = 512 bytes storage needed worst case. - * = not efficient to store led configs as strings in flash. - * = becomes a problem to send all the data via cli due to serial/cli buffers - */ -typedef enum { - X_COORDINATE, - Y_COORDINATE, - DIRECTIONS, - FUNCTIONS, - RING_COLORS -} parseState_e; +static void updateLedRingCounts(void); -#define PARSE_STATE_COUNT 5 - -static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0' }; - -static const char directionCodes[] = { 'N', 'E', 'S', 'W', 'U', 'D' }; -#define DIRECTION_COUNT (sizeof(directionCodes) / sizeof(directionCodes[0])) -static const uint8_t directionMappings[DIRECTION_COUNT] = { - LED_DIRECTION_NORTH, - LED_DIRECTION_EAST, - LED_DIRECTION_SOUTH, - LED_DIRECTION_WEST, - LED_DIRECTION_UP, - LED_DIRECTION_DOWN -}; - -static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T', 'R', 'C' }; -#define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0])) -static const uint16_t functionMappings[FUNCTION_COUNT] = { - LED_FUNCTION_INDICATOR, - LED_FUNCTION_WARNING, - LED_FUNCTION_FLIGHT_MODE, - LED_FUNCTION_ARM_STATE, - LED_FUNCTION_THROTTLE, - LED_FUNCTION_THRUST_RING, - LED_FUNCTION_COLOR -}; - -// grid offsets -uint8_t highestYValueForNorth; -uint8_t lowestYValueForSouth; -uint8_t highestXValueForWest; -uint8_t lowestXValueForEast; - -void determineLedStripDimensions(void) +STATIC_UNIT_TESTED void determineLedStripDimensions(void) { - ledGridWidth = 0; - ledGridHeight = 0; + int maxX = 0; + int maxY = 0; - uint8_t ledIndex; - const ledConfig_t *ledConfig; + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - ledConfig = &ledConfigs[ledIndex]; - - if (GET_LED_X(ledConfig) >= ledGridWidth) { - ledGridWidth = GET_LED_X(ledConfig) + 1; - } - if (GET_LED_Y(ledConfig) >= ledGridHeight) { - ledGridHeight = GET_LED_Y(ledConfig) + 1; - } + maxX = MAX(ledGetX(ledConfig), maxX); + maxY = MAX(ledGetY(ledConfig), maxY); } + ledGridWidth = maxX + 1; + ledGridHeight = maxY + 1; } -void determineOrientationLimits(void) +STATIC_UNIT_TESTED void determineOrientationLimits(void) { - bool isOddHeight = (ledGridHeight & 1); - bool isOddWidth = (ledGridWidth & 1); - uint8_t heightModifier = isOddHeight ? 1 : 0; - uint8_t widthModifier = isOddWidth ? 1 : 0; - highestYValueForNorth = (ledGridHeight / 2) - 1; - lowestYValueForSouth = (ledGridHeight / 2) + heightModifier; + lowestYValueForSouth = ((ledGridHeight + 1) / 2); highestXValueForWest = (ledGridWidth / 2) - 1; - lowestXValueForEast = (ledGridWidth / 2) + widthModifier; + lowestXValueForEast = ((ledGridWidth + 1) / 2); } -void updateLedCount(void) +STATIC_UNIT_TESTED void updateLedCount(void) { - const ledConfig_t *ledConfig; - uint8_t ledIndex; - ledCount = 0; - ledsInRingCount = 0; + int count = 0, countRing = 0, countScanner= 0; - if( ledConfigs == 0 ){ - return; - } + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - for (ledIndex = 0; ledIndex < MAX_LED_STRIP_LENGTH; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (ledConfig->flags == 0 && ledConfig->xy == 0) { + if (!(*ledConfig)) break; - } - ledCount++; + count++; - if ((ledConfig->flags & LED_FUNCTION_THRUST_RING)) { - ledsInRingCount++; - } + if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) + countRing++; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) + countScanner++; } + + ledCounts.count = count; + ledCounts.ring = countRing; + ledCounts.larson = countScanner; } -void reevalulateLedConfig(void) +void reevaluateLedConfig(void) { updateLedCount(); determineLedStripDimensions(); determineOrientationLimits(); + updateLedRingCounts(); } -#define CHUNK_BUFFER_SIZE 10 - -#define NEXT_PARSE_STATE(parseState) ((parseState + 1) % PARSE_STATE_COUNT) - - -bool parseLedStripConfig(uint8_t ledIndex, const char *config) +// get specialColor by index +static hsvColor_t* getSC(ledSpecialColorIds_e index) { - char chunk[CHUNK_BUFFER_SIZE]; - uint8_t chunkIndex; - uint8_t val; + return &masterConfig.colors[masterConfig.specialColors.color[index]]; +} - uint8_t parseState = X_COORDINATE; - bool ok = true; +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', 'O', 'B', 'N', 'I', 'W' }; - if (ledIndex >= MAX_LED_STRIP_LENGTH) { - return !ok; - } +#define CHUNK_BUFFER_SIZE 11 - ledConfig_t *ledConfig = &ledConfigs[ledIndex]; +bool parseLedStripConfig(int ledIndex, const char *config) +{ + if (ledIndex >= LED_MAX_STRIP_LENGTH) + return false; + + enum parseState_e { + X_COORDINATE, + Y_COORDINATE, + DIRECTIONS, + FUNCTIONS, + RING_COLORS, + PARSE_STATE_COUNT + }; + static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0'}; + + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; memset(ledConfig, 0, sizeof(ledConfig_t)); - while (ok) { + int x = 0, y = 0, color = 0; // initialize to prevent warnings + int baseFunction = 0; + int overlay_flags = 0; + int direction_flags = 0; - char chunkSeparator = chunkSeparators[parseState]; - - memset(&chunk, 0, sizeof(chunk)); - chunkIndex = 0; - - while (*config && chunkIndex < CHUNK_BUFFER_SIZE && *config != chunkSeparator) { - chunk[chunkIndex++] = *config++; + for (enum parseState_e parseState = 0; parseState < PARSE_STATE_COUNT; parseState++) { + char chunk[CHUNK_BUFFER_SIZE]; + { + char chunkSeparator = chunkSeparators[parseState]; + int chunkIndex = 0; + while (*config && *config != chunkSeparator && chunkIndex < (CHUNK_BUFFER_SIZE - 1)) { + chunk[chunkIndex++] = *config++; + } + chunk[chunkIndex++] = 0; // zero-terminate chunk + if (*config != chunkSeparator) { + return false; + } + config++; // skip separator } - - if (*config++ != chunkSeparator) { - ok = false; - break; - } - - switch((parseState_e)parseState) { + switch (parseState) { case X_COORDINATE: - val = atoi(chunk); - ledConfig->xy |= CALCULATE_LED_X(val); + x = atoi(chunk); break; case Y_COORDINATE: - val = atoi(chunk); - ledConfig->xy |= CALCULATE_LED_Y(val); + y = atoi(chunk); break; case DIRECTIONS: - for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) { - for (uint8_t mappingIndex = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { - if (directionCodes[mappingIndex] == chunk[chunkIndex]) { - ledConfig->flags |= directionMappings[mappingIndex]; + for (char *ch = chunk; *ch; ch++) { + for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) { + if (directionCodes[dir] == *ch) { + direction_flags |= LED_FLAG_DIRECTION(dir); break; } } } break; case FUNCTIONS: - for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) { - for (uint8_t mappingIndex = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { - if (functionCodes[mappingIndex] == chunk[chunkIndex]) { - ledConfig->flags |= functionMappings[mappingIndex]; + for (char *ch = chunk; *ch; ch++) { + for (ledBaseFunctionId_e fn = 0; fn < LED_BASEFUNCTION_COUNT; fn++) { + if (baseFunctionCodes[fn] == *ch) { + baseFunction = fn; + break; + } + } + + for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) { + if (overlayCodes[ol] == *ch) { + overlay_flags |= LED_FLAG_OVERLAY(ol); break; } } } break; case RING_COLORS: - if (atoi(chunk) < CONFIGURABLE_COLOR_COUNT) { - ledConfig->color = atoi(chunk); - } else { - ledConfig->color = 0; - } + color = atoi(chunk); + if (color >= LED_CONFIGURABLE_COLOR_COUNT) + color = 0; break; - default : - break; - } - - parseState++; - if (parseState >= PARSE_STATE_COUNT) { - break; + case PARSE_STATE_COUNT:; // prevent warning } } - if (!ok) { - memset(ledConfig, 0, sizeof(ledConfig_t)); - } + *ledConfig = DEFINE_LED(x, y, color, direction_flags, baseFunction, overlay_flags, 0); - reevalulateLedConfig(); + reevaluateLedConfig(); - return ok; + return true; } -void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize) +void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize) { - char functions[FUNCTION_COUNT]; - char directions[DIRECTION_COUNT]; - uint8_t index; - uint8_t mappingIndex; + char directions[LED_DIRECTION_COUNT + 1]; + char baseFunctionOverlays[LED_OVERLAY_COUNT + 2]; - ledConfig_t *ledConfig = &ledConfigs[ledIndex]; + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; memset(ledConfigBuffer, 0, bufferSize); - memset(&functions, 0, sizeof(functions)); - memset(&directions, 0, sizeof(directions)); - for (mappingIndex = 0, index = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { - if (ledConfig->flags & functionMappings[mappingIndex]) { - functions[index++] = functionCodes[mappingIndex]; + char *dptr = directions; + for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) { + if (ledGetDirectionBit(ledConfig, dir)) { + *dptr++ = directionCodes[dir]; } } + *dptr = 0; - for (mappingIndex = 0, index = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { - if (ledConfig->flags & directionMappings[mappingIndex]) { - directions[index++] = directionCodes[mappingIndex]; + char *fptr = baseFunctionOverlays; + *fptr++ = baseFunctionCodes[ledGetFunction(ledConfig)]; + + for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) { + if (ledGetOverlayBit(ledConfig, ol)) { + *fptr++ = overlayCodes[ol]; } } + *fptr = 0; - sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions, ledConfig->color); + // TODO - check buffer length + sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig)); } -void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors) -{ - // apply up/down colors regardless of quadrant. - if ((ledConfig->flags & LED_DIRECTION_UP)) { - setLedHsv(ledIndex, &colors[modeColors->up]); - } - - if ((ledConfig->flags & LED_DIRECTION_DOWN)) { - setLedHsv(ledIndex, &colors[modeColors->down]); - } - - // override with n/e/s/w colors to each n/s e/w half - bail at first match. - if ((ledConfig->flags & LED_DIRECTION_WEST) && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, &colors[modeColors->west]); - } - - if ((ledConfig->flags & LED_DIRECTION_EAST) && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, &colors[modeColors->east]); - } - - if ((ledConfig->flags & LED_DIRECTION_NORTH) && GET_LED_Y(ledConfig) <= highestYValueForNorth) { - setLedHsv(ledIndex, &colors[modeColors->north]); - } - - if ((ledConfig->flags & LED_DIRECTION_SOUTH) && GET_LED_Y(ledConfig) >= lowestYValueForSouth) { - setLedHsv(ledIndex, &colors[modeColors->south]); - } - -} typedef enum { - QUADRANT_NORTH_EAST = 1, - QUADRANT_SOUTH_EAST, - QUADRANT_SOUTH_WEST, - QUADRANT_NORTH_WEST + // the ordering is important, see below how NSEW is mapped to NE/SE/NW/SW + QUADRANT_NORTH = 1 << 0, + QUADRANT_SOUTH = 1 << 1, + QUADRANT_EAST = 1 << 2, + QUADRANT_WEST = 1 << 3, + QUADRANT_NORTH_EAST = 1 << 4, + QUADRANT_SOUTH_EAST = 1 << 5, + QUADRANT_NORTH_WEST = 1 << 6, + QUADRANT_SOUTH_WEST = 1 << 7, + QUADRANT_NONE = 1 << 8, + QUADRANT_NOTDIAG = 1 << 9, // not in NE/SE/NW/SW + // values for test + QUADRANT_ANY = QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST | QUADRANT_NONE, } quadrant_e; -void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const quadrant_e quadrant, const hsvColor_t *color) +static quadrant_e getLedQuadrant(const int ledIndex) { - switch (quadrant) { - case QUADRANT_NORTH_EAST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - case QUADRANT_SOUTH_EAST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; + int x = ledGetX(ledConfig); + int y = ledGetY(ledConfig); - case QUADRANT_SOUTH_WEST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; + int quad = 0; + if (y <= highestYValueForNorth) + quad |= QUADRANT_NORTH; + else if (y >= lowestYValueForSouth) + quad |= QUADRANT_SOUTH; + if (x >= lowestXValueForEast) + quad |= QUADRANT_EAST; + else if (x <= highestXValueForWest) + quad |= QUADRANT_WEST; + + if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH)) + && (quad & (QUADRANT_EAST | QUADRANT_WEST)) ) { // is led in one of NE/SE/NW/SW? + quad |= 1 << (4 + ((quad & QUADRANT_SOUTH) ? 1 : 0) + ((quad & QUADRANT_WEST) ? 2 : 0)); + } else { + quad |= QUADRANT_NOTDIAG; + } + + if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST)) == 0) + quad |= QUADRANT_NONE; + + return quad; +} + +static const struct { + uint8_t dir; // ledDirectionId_e + uint16_t quadrantMask; // quadrant_e +} directionQuadrantMap[] = { + {LED_DIRECTION_SOUTH, QUADRANT_SOUTH}, + {LED_DIRECTION_NORTH, QUADRANT_NORTH}, + {LED_DIRECTION_EAST, QUADRANT_EAST}, + {LED_DIRECTION_WEST, QUADRANT_WEST}, + {LED_DIRECTION_DOWN, QUADRANT_ANY}, + {LED_DIRECTION_UP, QUADRANT_ANY}, +}; + +static hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIndexes_t *modeColors) +{ + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + quadrant_e quad = getLedQuadrant(ledIndex); + for (unsigned i = 0; i < ARRAYLEN(directionQuadrantMap); i++) { + ledDirectionId_e dir = directionQuadrantMap[i].dir; + quadrant_e quadMask = directionQuadrantMap[i].quadrantMask; + + if (ledGetDirectionBit(ledConfig, dir) && (quad & quadMask)) + return &masterConfig.colors[modeColors->color[dir]]; + } + return NULL; +} + + +// map flight mode to led mode, in order of priority +// flightMode == 0 is always active +static const struct { + uint16_t flightMode; + uint8_t ledMode; +} flightModeToLed[] = { + {HEADFREE_MODE, LED_MODE_HEADFREE}, +#ifdef MAG + {MAG_MODE, LED_MODE_MAG}, +#endif +#ifdef BARO + {NAV_ALTHOLD_MODE, LED_MODE_BARO}, +#endif + {HORIZON_MODE, LED_MODE_HORIZON}, + {ANGLE_MODE, LED_MODE_ANGLE}, + {0, LED_MODE_ORIENTATION}, +}; + +static void applyLedFixedLayers() +{ + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); + + int fn = ledGetFunction(ledConfig); + int hOffset = HSV_HUE_MAX; + + switch (fn) { + case LED_FUNCTION_COLOR: + color = masterConfig.colors[ledGetColor(ledConfig)]; + break; + + case LED_FUNCTION_FLIGHT_MODE: + for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) + if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { + color = *getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); + break; // stop on first match + } + break; + + case LED_FUNCTION_ARM_STATE: + color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED); + break; + + case LED_FUNCTION_BATTERY: + color = HSV(RED); + hOffset += scaleRange(calculateBatteryCapacityRemainingPercentage(), 0, 100, -30, 120); + break; + + case LED_FUNCTION_RSSI: + color = HSV(RED); + hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120); + break; + + default: + break; + } + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { + hOffset += ((scaledThrottle - 10) * 4) / 3; + } + + color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1); + + setLedHsv(ledIndex, &color); - case QUADRANT_NORTH_WEST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; } } -void applyLedModeLayer(void) +static void applyLedHsv(uint32_t mask, const hsvColor_t *color) { - const ledConfig_t *ledConfig; - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) { - if (ledConfig->flags & LED_FUNCTION_COLOR) { - setLedHsv(ledIndex, &colors[ledConfig->color]); - } else { - setLedHsv(ledIndex, &hsv_black); - } - } - - if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) { - if (ledConfig->flags & LED_FUNCTION_ARM_STATE) { - if (!ARMING_FLAG(ARMED)) { - setLedHsv(ledIndex, &hsv_green); - } else { - setLedHsv(ledIndex, &hsv_blue); - } - } - continue; - } - - applyDirectionalModeColor(ledIndex, ledConfig, &orientationModeColors); - - if (FLIGHT_MODE(HEADFREE_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &headfreeModeColors); -#ifdef MAG - } else if (FLIGHT_MODE(MAG_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &magModeColors); -#endif -#ifdef BARO - } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE | NAV_RTH_MODE | NAV_WP_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &navigationModeColors); -#endif - } else if (FLIGHT_MODE(HORIZON_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &horizonModeColors); - } else if (FLIGHT_MODE(ANGLE_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &angleModeColors); - } + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if ((*ledConfig & mask) == mask) + setLedHsv(ledIndex, color); } } typedef enum { - WARNING_FLAG_NONE = 0, - WARNING_FLAG_LOW_BATTERY = (1 << 0), - WARNING_FLAG_FAILSAFE = (1 << 1), - WARNING_FLAG_ARMING_DISABLED = (1 << 2) + WARNING_ARMING_DISABLED, + WARNING_LOW_BATTERY, + WARNING_FAILSAFE, } warningFlags_e; -static uint8_t warningFlags = WARNING_FLAG_NONE; -void applyLedWarningLayer(uint8_t updateNow) +static void applyLedWarningLayer(bool updateNow, uint32_t *timer) { - const ledConfig_t *ledConfig; - uint8_t ledIndex; static uint8_t warningFlashCounter = 0; + static uint8_t warningFlags = 0; // non-zero during blinks - if (updateNow && warningFlashCounter == 0) { - warningFlags = WARNING_FLAG_NONE; - if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) { - warningFlags |= WARNING_FLAG_LOW_BATTERY; - } - if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) { - warningFlags |= WARNING_FLAG_FAILSAFE; - } - if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) { - warningFlags |= WARNING_FLAG_ARMING_DISABLED; - } - } - - if (warningFlags || warningFlashCounter > 0) { - const hsvColor_t *warningColor = &hsv_black; - - if ((warningFlashCounter & 1) == 0) { - if (warningFlashCounter < 4 && (warningFlags & WARNING_FLAG_ARMING_DISABLED)) { - warningColor = &hsv_green; - } - if (warningFlashCounter >= 4 && warningFlashCounter < 12 && (warningFlags & WARNING_FLAG_LOW_BATTERY)) { - warningColor = &hsv_red; - } - if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { - warningColor = &hsv_yellow; - } - } else { - if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { - warningColor = &hsv_blue; - } - } - - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_WARNING)) { - continue; - } - setLedHsv(ledIndex, warningColor); - } - } - - if (updateNow && (warningFlags || warningFlashCounter)) { + if (updateNow) { + // keep counter running, so it stays in sync with blink warningFlashCounter++; - if (warningFlashCounter == 20) { - warningFlashCounter = 0; + warningFlashCounter &= 0xF; + + if (warningFlashCounter == 0) { // update when old flags was processed + warningFlags = 0; + if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) + warningFlags |= 1 << WARNING_LOW_BATTERY; + if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) + warningFlags |= 1 << WARNING_FAILSAFE; + if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) + warningFlags |= 1 << WARNING_ARMING_DISABLED; } + *timer += LED_STRIP_HZ(10); + } + + if (warningFlags) { + const hsvColor_t *warningColor = NULL; + + bool colorOn = (warningFlashCounter % 2) == 0; // w_w_ + warningFlags_e warningId = warningFlashCounter / 4; + if (warningFlags & (1 << warningId)) { + switch (warningId) { + case WARNING_ARMING_DISABLED: + warningColor = colorOn ? &HSV(GREEN) : &HSV(BLACK); + break; + case WARNING_LOW_BATTERY: + warningColor = colorOn ? &HSV(RED) : &HSV(BLACK); + break; + case WARNING_FAILSAFE: + warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE); + break; + default:; + } + } + if (warningColor) + applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor); } } +static void applyLedBatteryLayer(bool updateNow, uint32_t *timer) +{ + static bool flash = false; + + int state; + int timeOffset = 1; + + if (updateNow) { + state = getBatteryState(); + + switch (state) { + case BATTERY_OK: + flash = false; + timeOffset = 1; + break; + case BATTERY_WARNING: + timeOffset = 2; + break; + default: + timeOffset = 8; + break; + } + flash = !flash; + } + + *timer += LED_STRIP_HZ(timeOffset); + + if (!flash) { + hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc); + } +} + +static void applyLedRssiLayer(bool updateNow, uint32_t *timer) +{ + static bool flash = false; + + int state; + int timeOffset = 0; + + if (updateNow) { + state = (rssi * 100) / 1023; + + if (state > 50) { + flash = false; + timeOffset = 1; + } else if (state > 20) { + timeOffset = 2; + } else { + timeOffset = 8; + } + flash = !flash; + } + + + *timer += LED_STRIP_HZ(timeOffset); + + if (!flash) { + hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc); + } +} + +#ifdef GPS +static void applyLedGpsLayer(bool updateNow, uint32_t *timer) +{ + static uint8_t gpsFlashCounter = 0; + static uint8_t gpsPauseCounter = 0; + const uint8_t blinkPauseLength = 4; + + if (updateNow) { + if (gpsPauseCounter > 0) { + gpsPauseCounter--; + } else if (gpsFlashCounter >= gpsSol.numSat) { + gpsFlashCounter = 0; + gpsPauseCounter = blinkPauseLength; + } else { + gpsFlashCounter++; + gpsPauseCounter = 1; + } + *timer += LED_STRIP_HZ(2.5); + } + + const hsvColor_t *gpsColor; + + if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) { + gpsColor = getSC(LED_SCOLOR_GPSNOSATS); + } else { + bool colorOn = gpsPauseCounter == 0; // each interval starts with pause + if (STATE(GPS_FIX)) { + gpsColor = colorOn ? getSC(LED_SCOLOR_GPSLOCKED) : getSC(LED_SCOLOR_BACKGROUND); + } else { + gpsColor = colorOn ? getSC(LED_SCOLOR_GPSNOLOCK) : getSC(LED_SCOLOR_GPSNOSATS); + } + } + + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_GPS), gpsColor); +} + +#endif + + #define INDICATOR_DEADBAND 25 -void applyLedIndicatorLayer(uint8_t indicatorFlashState) +static void applyLedIndicatorLayer(bool updateNow, uint32_t *timer) { - const ledConfig_t *ledConfig; - static const hsvColor_t *flashColor; + static bool flash = 0; - if (!rxIsReceivingSignal()) { + if (updateNow) { + if (rxIsReceivingSignal()) { + // calculate update frequency + int scale = MAX(ABS(rcCommand[ROLL]), ABS(rcCommand[PITCH])); // 0 - 500 + scale += (50 - INDICATOR_DEADBAND); // start increasing frequency right after deadband + *timer += LED_STRIP_HZ(5) * 50 / MAX(50, scale); // 5 - 50Hz update, 2.5 - 25Hz blink + + flash = !flash; + } else { + *timer += LED_STRIP_HZ(5); // try again soon + } + } + + if (!flash) return; + + const hsvColor_t *flashColor = &HSV(ORANGE); // TODO - use user color? + + quadrant_e quadrants = 0; + if (rcCommand[ROLL] > INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_EAST | QUADRANT_SOUTH_EAST; + } else if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_WEST | QUADRANT_SOUTH_WEST; + } + if (rcCommand[PITCH] > INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_EAST | QUADRANT_NORTH_WEST; + } else if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { + quadrants |= QUADRANT_SOUTH_EAST | QUADRANT_SOUTH_WEST; } - if (indicatorFlashState == 0) { - flashColor = &hsv_orange; - } else { - flashColor = &hsv_black; - } - - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_INDICATOR)) { - continue; + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) { + if (getLedQuadrant(ledIndex) & quadrants) + setLedHsv(ledIndex, flashColor); } - - if (rcCommand[ROLL] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); - } - - if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); - } - - if (rcCommand[PITCH] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); - } - - if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); - } - } -} - -void applyLedThrottleLayer() -{ - const ledConfig_t *ledConfig; - hsvColor_t color; - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - ledConfig = &ledConfigs[ledIndex]; - if (!(ledConfig->flags & LED_FUNCTION_THROTTLE)) { - continue; - } - - getLedHsv(ledIndex, &color); - - int scaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, -60, +60); - scaled += HSV_HUE_MAX; - color.h = (color.h + scaled) % HSV_HUE_MAX; - setLedHsv(ledIndex, &color); } } #define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off -#define ROTATION_SEQUENCE_LED_WIDTH 2 +#define ROTATION_SEQUENCE_LED_WIDTH 2 // 2 on -void applyLedThrustRingLayer(void) +static void updateLedRingCounts(void) { - uint8_t ledIndex; - static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; - bool nextLedOn = false; - hsvColor_t ringColor; - const ledConfig_t *ledConfig; - - uint8_t ledRingIndex = 0; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if ((ledConfig->flags & LED_FUNCTION_THRUST_RING) == 0) { - continue; + int seqLen; + // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds + if ((ledCounts.ring % ROTATION_SEQUENCE_LED_COUNT) == 0) { + seqLen = ROTATION_SEQUENCE_LED_COUNT; + } else { + seqLen = ledCounts.ring; + // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds + // TODO - improve partitioning (15 leds -> 3x5) + while ((seqLen > ROTATION_SEQUENCE_LED_COUNT) && ((seqLen % 2) == 0)) { + seqLen /= 2; } + } + ledCounts.ringSeqLen = seqLen; +} - bool applyColor = false; - if (ARMING_FLAG(ARMED)) { - if ((ledRingIndex + rotationPhase) % ROTATION_SEQUENCE_LED_COUNT < ROTATION_SEQUENCE_LED_WIDTH) { - applyColor = true; - } - } else { - if (nextLedOn == false) { - applyColor = true; - } - nextLedOn = !nextLedOn; - } +static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer) +{ + static uint8_t rotationPhase; + int ledRingIndex = 0; - if (applyColor) { - ringColor = colors[ledConfig->color]; - } else { - ringColor = hsv_black; - } + if (updateNow) { + rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1; - setLedHsv(ledIndex, &ringColor); - - ledRingIndex++; + int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + *timer += LED_STRIP_HZ(5) * 10 / scale; // 5 - 50Hz update rate } - rotationPhase--; - if (rotationPhase == 0) { - rotationPhase = ROTATION_SEQUENCE_LED_COUNT; + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) { + + bool applyColor; + if (ARMING_FLAG(ARMED)) { + applyColor = (ledRingIndex + rotationPhase) % ledCounts.ringSeqLen < ROTATION_SEQUENCE_LED_WIDTH; + } else { + applyColor = !(ledRingIndex % 2); // alternating pattern + } + + if (applyColor) { + const hsvColor_t *ringColor = &masterConfig.colors[ledGetColor(ledConfig)]; + setLedHsv(ledIndex, ringColor); + } + + ledRingIndex++; + } } } +typedef struct larsonParameters_s { + uint8_t currentBrightness; + int8_t currentIndex; + int8_t direction; +} larsonParameters_t; + +static int brightnessForLarsonIndex(larsonParameters_t *larsonParameters, uint8_t larsonIndex) +{ + int offset = larsonIndex - larsonParameters->currentIndex; + static const int larsonLowValue = 8; + + if (ABS(offset) > 1) + return (larsonLowValue); + + if (offset == 0) + return (larsonParameters->currentBrightness); + + if (larsonParameters->direction == offset) { + return (larsonParameters->currentBrightness - 127); + } + + return (255 - larsonParameters->currentBrightness); + +} + +static void larsonScannerNextStep(larsonParameters_t *larsonParameters, int delta) +{ + if (larsonParameters->currentBrightness > (255 - delta)) { + larsonParameters->currentBrightness = 127; + if (larsonParameters->currentIndex >= ledCounts.larson || larsonParameters->currentIndex < 0) { + larsonParameters->direction = -larsonParameters->direction; + } + larsonParameters->currentIndex += larsonParameters->direction; + } else { + larsonParameters->currentBrightness += delta; + } +} + +static void applyLarsonScannerLayer(bool updateNow, uint32_t *timer) +{ + static larsonParameters_t larsonParameters = { 0, 0, 1 }; + + if (updateNow) { + larsonScannerNextStep(&larsonParameters, 15); + *timer += LED_STRIP_HZ(60); + } + + int scannerLedIndex = 0; + for (unsigned i = 0; i < ledCounts.count; i++) { + + const ledConfig_t *ledConfig = &ledConfigs[i]; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) { + hsvColor_t ledColor; + getLedHsv(i, &ledColor); + ledColor.v = brightnessForLarsonIndex(&larsonParameters, scannerLedIndex); + setLedHsv(i, &ledColor); + scannerLedIndex++; + } + } +} + +// blink twice, then wait ; either always or just when landing +static void applyLedBlinkLayer(bool updateNow, uint32_t *timer) +{ + const uint16_t blinkPattern = 0x8005; // 0b1000000000000101; + static uint16_t blinkMask; + + if (updateNow) { + blinkMask = blinkMask >> 1; + if (blinkMask <= 1) + blinkMask = blinkPattern; + + *timer += LED_STRIP_HZ(10); + } + + bool ledOn = (blinkMask & 1); // b_b_____... + if (!ledOn) { + for (int i = 0; i < ledCounts.count; ++i) { + const ledConfig_t *ledConfig = &ledConfigs[i]; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_BLINK) || + (ledGetOverlayBit(ledConfig, LED_OVERLAY_LANDING_FLASH) && scaledThrottle < 55 && scaledThrottle > 10)) { + setLedHsv(i, getSC(LED_SCOLOR_BLINKBACKGROUND)); + } + } + } +} + + #ifdef USE_LED_ANIMATION -static uint8_t previousRow; -static uint8_t currentRow; -static uint8_t nextRow; -void updateLedAnimationState(void) +static void applyLedAnimationLayer(bool updateNow, uint32_t *timer) { static uint8_t frameCounter = 0; - - uint8_t animationFrames = ledGridHeight; - - previousRow = (frameCounter + animationFrames - 1) % animationFrames; - currentRow = frameCounter; - nextRow = (frameCounter + 1) % animationFrames; - - frameCounter = (frameCounter + 1) % animationFrames; -} - -static void applyLedAnimationLayer(void) -{ - const ledConfig_t *ledConfig; - - if (ARMING_FLAG(ARMED)) { - return; + const int animationFrames = ledGridHeight; + if(updateNow) { + frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; + *timer += LED_STRIP_HZ(20); } - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { + if (ARMING_FLAG(ARMED)) + return; - ledConfig = &ledConfigs[ledIndex]; + int previousRow = frameCounter > 0 ? frameCounter - 1 : animationFrames - 1; + int currentRow = frameCounter; + int nextRow = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; - if (GET_LED_Y(ledConfig) == previousRow) { - setLedHsv(ledIndex, &hsv_white); + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + if (ledGetY(ledConfig) == previousRow) { + setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); scaleLedValue(ledIndex, 50); - - } else if (GET_LED_Y(ledConfig) == currentRow) { - setLedHsv(ledIndex, &hsv_white); - } else if (GET_LED_Y(ledConfig) == nextRow) { + } else if (ledGetY(ledConfig) == currentRow) { + setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); + } else if (ledGetY(ledConfig) == nextRow) { scaleLedValue(ledIndex, 50); } } } #endif +typedef enum { + timBlink, + timLarson, + timBattery, + timRssi, +#ifdef GPS + timGps, +#endif + timWarning, + timIndicator, +#ifdef USE_LED_ANIMATION + timAnimation, +#endif + timRing, + timTimerCount +} timId_e; + +static uint32_t timerVal[timTimerCount]; + + +// function to apply layer. +// function must replan self using timer pointer +// when updateNow is true (timer triggered), state must be updated first, +// before calculating led state. Otherwise update started by different trigger +// may modify LED state. +typedef void applyLayerFn_timed(bool updateNow, uint32_t *timer); + + +static applyLayerFn_timed* layerTable[] = { + [timBlink] = &applyLedBlinkLayer, + [timLarson] = &applyLarsonScannerLayer, + [timBattery] = &applyLedBatteryLayer, + [timRssi] = &applyLedRssiLayer, +#ifdef GPS + [timGps] = &applyLedGpsLayer, +#endif + [timWarning] = &applyLedWarningLayer, + [timIndicator] = &applyLedIndicatorLayer, +#ifdef USE_LED_ANIMATION + [timAnimation] = &applyLedAnimationLayer, +#endif + [timRing] = &applyLedThrustRingLayer +}; + void updateLedStrip(void) { - - if (!(ledStripInitialised && isWS2811LedStripReady())) { + if (!(ledStripInitialised && isWS2811LedStripReady())) { return; } @@ -884,167 +1086,179 @@ void updateLedStrip(void) ledStripDisable(); ledStripEnabled = false; } - } else { - ledStripEnabled = true; - } - - if (!ledStripEnabled){ return; } - + ledStripEnabled = true; uint32_t now = micros(); - bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; - bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; - bool rotationUpdateNow = (int32_t)(now - nextRotationUpdateAt) >= 0L; -#ifdef USE_LED_ANIMATION - bool animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L; -#endif - if (!( - indicatorFlashNow || - rotationUpdateNow || - warningFlashNow -#ifdef USE_LED_ANIMATION - || animationUpdateNow -#endif - )) { - return; - } - - static uint8_t indicatorFlashState = 0; - - // LAYER 1 - applyLedModeLayer(); - applyLedThrottleLayer(); - - // LAYER 2 - - if (warningFlashNow) { - nextWarningFlashAt = now + LED_STRIP_10HZ; - } - applyLedWarningLayer(warningFlashNow); - - // LAYER 3 - - if (indicatorFlashNow) { - - uint8_t rollScale = ABS(rcCommand[ROLL]) / 50; - uint8_t pitchScale = ABS(rcCommand[PITCH]) / 50; - uint8_t scale = MAX(rollScale, pitchScale); - nextIndicatorFlashAt = now + (LED_STRIP_5HZ / MAX(1, scale)); - - if (indicatorFlashState == 0) { - indicatorFlashState = 1; - } else { - indicatorFlashState = 0; + // test all led timers, setting corresponding bits + uint32_t timActive = 0; + for (timId_e timId = 0; timId < timTimerCount; timId++) { + // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. + // max delay is limited to 5s + int32_t delta = cmp32(now, timerVal[timId]); + if (delta < 0 && delta > -LED_STRIP_MS(5000)) + continue; // not ready yet + timActive |= 1 << timId; + if (delta >= LED_STRIP_MS(100) || delta < 0) { + timerVal[timId] = now; } } - applyLedIndicatorLayer(indicatorFlashState); + if (!timActive) + return; // no change this update, keep old state -#ifdef USE_LED_ANIMATION - if (animationUpdateNow) { - nextAnimationUpdateAt = now + LED_STRIP_20HZ; - updateLedAnimationState(); + // apply all layers; triggered timed functions has to update timers + + scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + + applyLedFixedLayers(); + + for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) { + uint32_t *timer = &timerVal[timId]; + bool updateNow = timActive & (1 << timId); + (*layerTable[timId])(updateNow, timer); } - applyLedAnimationLayer(); -#endif - - if (rotationUpdateNow) { - - applyLedThrustRingLayer(); - - uint8_t animationSpeedScale = 1; - - if (ARMING_FLAG(ARMED)) { - animationSpeedScale = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10); - } - - nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScale; - } - ws2811UpdateStrip(); } -bool parseColor(uint8_t index, const char *colorConfig) +bool parseColor(int index, const char *colorConfig) { const char *remainingCharacters = colorConfig; - hsvColor_t *color = &colors[index]; + hsvColor_t *color = &masterConfig.colors[index]; - bool ok = true; - - uint8_t componentIndex; - for (componentIndex = 0; ok && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { - uint16_t val = atoi(remainingCharacters); + bool result = true; + static const uint16_t hsv_limit[HSV_COLOR_COMPONENT_COUNT] = { + [HSV_HUE] = HSV_HUE_MAX, + [HSV_SATURATION] = HSV_SATURATION_MAX, + [HSV_VALUE] = HSV_VALUE_MAX, + }; + for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { + int val = atoi(remainingCharacters); + if(val > hsv_limit[componentIndex]) { + result = false; + break; + } switch (componentIndex) { case HSV_HUE: - if (val > HSV_HUE_MAX) { - ok = false; - continue; - - } - colors[index].h = val; + color->h = val; break; case HSV_SATURATION: - if (val > HSV_SATURATION_MAX) { - ok = false; - continue; - } - colors[index].s = (uint8_t)val; + color->s = val; break; case HSV_VALUE: - if (val > HSV_VALUE_MAX) { - ok = false; - continue; - } - colors[index].v = (uint8_t)val; + color->v = val; break; } - remainingCharacters = strstr(remainingCharacters, ","); + remainingCharacters = strchr(remainingCharacters, ','); if (remainingCharacters) { - remainingCharacters++; + remainingCharacters++; // skip separator } else { - if (componentIndex < 2) { - ok = false; + if (componentIndex < HSV_COLOR_COMPONENT_COUNT - 1) { + result = false; } } } - if (!ok) { - memset(color, 0, sizeof(hsvColor_t)); + if (!result) { + memset(color, 0, sizeof(*color)); } - return ok; + return result; } -void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount) +/* + * Redefine a color in a mode. + * */ +bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) { - memset(colors, 0, colorCount * sizeof(hsvColor_t)); - for (uint8_t colorIndex = 0; colorIndex < colorCount && colorIndex < (sizeof(defaultColors) / sizeof(defaultColors[0])); colorIndex++) { - *colors++ = *defaultColors[colorIndex]; + // check color + if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT) + return false; + if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough + if(modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT) + return false; + masterConfig.modeColors[modeIndex].color[modeColorIndex] = colorIndex; + } else if (modeIndex == LED_SPECIAL) { + if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT) + return false; + masterConfig.specialColors.color[modeColorIndex] = colorIndex; + } else { + return false; + } + return true; +} + +/* +void pgResetFn_ledConfigs(ledConfig_t *instance) +{ + memcpy_fn(instance, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); +} + +void pgResetFn_colors(hsvColor_t *instance) +{ + // copy hsv colors as default + BUILD_BUG_ON(ARRAYLEN(*colors_arr()) < ARRAYLEN(hsv)); + + for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { + *instance++ = hsv[colorIndex]; } } +void pgResetFn_modeColors(modeColorIndexes_t *instance) +{ + memcpy_fn(instance, &defaultModeColors, sizeof(defaultModeColors)); +} + +void pgResetFn_specialColors(specialColorIndexes_t *instance) +{ + memcpy_fn(instance, &defaultSpecialColors, sizeof(defaultSpecialColors)); +} +*/ + void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) { - memset(ledConfigs, 0, MAX_LED_STRIP_LENGTH * sizeof(ledConfig_t)); + memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); - reevalulateLedConfig(); + reevaluateLedConfig(); } -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse) +void applyDefaultColors(hsvColor_t *colors) +{ + // copy hsv colors as default + memset(colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t)); + for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { + *colors++ = hsv[colorIndex]; + } +} + +void applyDefaultModeColors(modeColorIndexes_t *modeColors) +{ + memcpy_fn(modeColors, &defaultModeColors, sizeof(defaultModeColors)); +} + +void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) +{ + memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); +} + + + +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse) { ledConfigs = ledConfigsToUse; colors = colorsToUse; + modeColors = modeColorsToUse; + specialColors = *specialColorsToUse; ledStripInitialised = false; } void ledStripEnable(void) { - reevalulateLedConfig(); + reevaluateLedConfig(); ledStripInitialised = true; ws2811LedStripInit(); @@ -1052,7 +1266,7 @@ void ledStripEnable(void) static void ledStripDisable(void) { - setStripColor(&hsv_black); + setStripColor(&HSV(BLACK)); ws2811UpdateStrip(); } diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 99d0397646..39b3cad6d2 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -17,82 +17,163 @@ #pragma once -#define MAX_LED_STRIP_LENGTH 32 -#define CONFIGURABLE_COLOR_COUNT 16 +#define LED_MAX_STRIP_LENGTH 32 +#define LED_CONFIGURABLE_COLOR_COUNT 16 +#define LED_MODE_COUNT 6 +#define LED_DIRECTION_COUNT 6 +#define LED_BASEFUNCTION_COUNT 7 +#define LED_OVERLAY_COUNT 6 +#define LED_SPECIAL_COLOR_COUNT 11 + +#define LED_POS_OFFSET 0 +#define LED_FUNCTION_OFFSET 8 +#define LED_OVERLAY_OFFSET 12 +#define LED_COLOR_OFFSET 18 +#define LED_DIRECTION_OFFSET 22 +#define LED_PARAMS_OFFSET 28 + +#define LED_POS_BITCNT 8 +#define LED_FUNCTION_BITCNT 4 +#define LED_OVERLAY_BITCNT 6 +#define LED_COLOR_BITCNT 4 +#define LED_DIRECTION_BITCNT 6 +#define LED_PARAMS_BITCNT 4 + +#define LED_FLAG_OVERLAY_MASK ((1 << LED_OVERLAY_BITCNT) - 1) +#define LED_FLAG_DIRECTION_MASK ((1 << LED_DIRECTION_BITCNT) - 1) + +#define LED_MOV_POS(pos) ((pos) << LED_POS_OFFSET) +#define LED_MOV_FUNCTION(func) ((func) << LED_FUNCTION_OFFSET) +#define LED_MOV_OVERLAY(overlay) ((overlay) << LED_OVERLAY_OFFSET) +#define LED_MOV_COLOR(colorId) ((colorId) << LED_COLOR_OFFSET) +#define LED_MOV_DIRECTION(direction) ((direction) << LED_DIRECTION_OFFSET) +#define LED_MOV_PARAMS(param) ((param) << LED_PARAMS_OFFSET) + +#define LED_BIT_MASK(len) ((1 << (len)) - 1) + +#define LED_POS_MASK LED_MOV_POS(((1 << LED_POS_BITCNT) - 1)) +#define LED_FUNCTION_MASK LED_MOV_FUNCTION(((1 << LED_FUNCTION_BITCNT) - 1)) +#define LED_OVERLAY_MASK LED_MOV_OVERLAY(LED_FLAG_OVERLAY_MASK) +#define LED_COLOR_MASK LED_MOV_COLOR(((1 << LED_COLOR_BITCNT) - 1)) +#define LED_DIRECTION_MASK LED_MOV_DIRECTION(LED_FLAG_DIRECTION_MASK) +#define LED_PARAMS_MASK LED_MOV_PARAMS(((1 << LED_PARAMS_BITCNT) - 1)) + +#define LED_FLAG_OVERLAY(id) (1 << (id)) +#define LED_FLAG_DIRECTION(id) (1 << (id)) #define LED_X_BIT_OFFSET 4 #define LED_Y_BIT_OFFSET 0 - -#define LED_XY_MASK (0x0F) - -#define GET_LED_X(ledConfig) ((ledConfig->xy >> LED_X_BIT_OFFSET) & LED_XY_MASK) -#define GET_LED_Y(ledConfig) ((ledConfig->xy >> LED_Y_BIT_OFFSET) & LED_XY_MASK) - -#define CALCULATE_LED_X(x) ((x & LED_XY_MASK) << LED_X_BIT_OFFSET) -#define CALCULATE_LED_Y(y) ((y & LED_XY_MASK) << LED_Y_BIT_OFFSET) - - -#define CALCULATE_LED_XY(x,y) (CALCULATE_LED_X(x) | CALCULATE_LED_Y(y)) +#define LED_XY_MASK 0x0F +#define CALCULATE_LED_XY(x, y) ((((x) & LED_XY_MASK) << LED_X_BIT_OFFSET) | (((y) & LED_XY_MASK) << LED_Y_BIT_OFFSET)) typedef enum { - LED_DISABLED = 0, - LED_DIRECTION_NORTH = (1 << 0), - LED_DIRECTION_EAST = (1 << 1), - LED_DIRECTION_SOUTH = (1 << 2), - LED_DIRECTION_WEST = (1 << 3), - LED_DIRECTION_UP = (1 << 4), - LED_DIRECTION_DOWN = (1 << 5), - LED_FUNCTION_INDICATOR = (1 << 6), - LED_FUNCTION_WARNING = (1 << 7), - LED_FUNCTION_FLIGHT_MODE = (1 << 8), - LED_FUNCTION_ARM_STATE = (1 << 9), - LED_FUNCTION_THROTTLE = (1 << 10), - LED_FUNCTION_THRUST_RING = (1 << 11), - LED_FUNCTION_COLOR = (1 << 12), -} ledFlag_e; + LED_MODE_ORIENTATION = 0, + LED_MODE_HEADFREE, + LED_MODE_HORIZON, + LED_MODE_ANGLE, + LED_MODE_MAG, + LED_MODE_BARO, + LED_SPECIAL +} ledModeIndex_e; -#define LED_DIRECTION_BIT_OFFSET 0 -#define LED_DIRECTION_MASK ( \ - LED_DIRECTION_NORTH | \ - LED_DIRECTION_EAST | \ - LED_DIRECTION_SOUTH | \ - LED_DIRECTION_WEST | \ - LED_DIRECTION_UP | \ - LED_DIRECTION_DOWN \ -) -#define LED_FUNCTION_BIT_OFFSET 6 -#define LED_FUNCTION_MASK ( \ - LED_FUNCTION_INDICATOR | \ - LED_FUNCTION_WARNING | \ - LED_FUNCTION_FLIGHT_MODE | \ - LED_FUNCTION_ARM_STATE | \ - LED_FUNCTION_THROTTLE | \ - LED_FUNCTION_THRUST_RING | \ - LED_FUNCTION_COLOR \ -) +typedef enum { + LED_SCOLOR_DISARMED = 0, + LED_SCOLOR_ARMED, + LED_SCOLOR_ANIMATION, + LED_SCOLOR_BACKGROUND, + LED_SCOLOR_BLINKBACKGROUND, + LED_SCOLOR_GPSNOSATS, + LED_SCOLOR_GPSNOLOCK, + LED_SCOLOR_GPSLOCKED +} ledSpecialColorIds_e; + +typedef enum { + LED_DIRECTION_NORTH = 0, + LED_DIRECTION_EAST, + LED_DIRECTION_SOUTH, + LED_DIRECTION_WEST, + LED_DIRECTION_UP, + LED_DIRECTION_DOWN +} ledDirectionId_e; + +typedef enum { + LED_FUNCTION_COLOR, + LED_FUNCTION_FLIGHT_MODE, + LED_FUNCTION_ARM_STATE, + LED_FUNCTION_BATTERY, + LED_FUNCTION_RSSI, + LED_FUNCTION_GPS, + LED_FUNCTION_THRUST_RING, +} ledBaseFunctionId_e; + +typedef enum { + LED_OVERLAY_THROTTLE, + LED_OVERLAY_LARSON_SCANNER, + LED_OVERLAY_BLINK, + LED_OVERLAY_LANDING_FLASH, + LED_OVERLAY_INDICATOR, + LED_OVERLAY_WARNING, +} ledOverlayId_e; + +typedef struct modeColorIndexes_s { + uint8_t color[LED_DIRECTION_COUNT]; +} modeColorIndexes_t; + +typedef struct specialColorIndexes_s { + uint8_t color[LED_SPECIAL_COLOR_COUNT]; +} specialColorIndexes_t; + +typedef uint32_t ledConfig_t; + +typedef struct ledCounts_s { + uint8_t count; + uint8_t ring; + uint8_t larson; + uint8_t ringSeqLen; +} ledCounts_t; -typedef struct ledConfig_s { - uint8_t xy; // see LED_X/Y_MASK defines - uint8_t color; // see colors (config_master) - uint16_t flags; // see ledFlag_e -} ledConfig_t; +ledConfig_t *ledConfigs; +hsvColor_t *colors; +modeColorIndexes_t *modeColors; +specialColorIndexes_t specialColors; -extern uint8_t ledCount; -extern uint8_t ledsInRingCount; +#define DEFINE_LED(x, y, col, dir, func, ol, params) (LED_MOV_POS(CALCULATE_LED_XY(x, y)) | LED_MOV_COLOR(col) | LED_MOV_DIRECTION(dir) | LED_MOV_FUNCTION(func) | LED_MOV_OVERLAY(ol) | LED_MOV_PARAMS(params)) +static inline uint8_t ledGetXY(const ledConfig_t *lcfg) { return ((*lcfg >> LED_POS_OFFSET) & LED_BIT_MASK(LED_POS_BITCNT)); } +static inline uint8_t ledGetX(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_X_BIT_OFFSET)) & LED_XY_MASK); } +static inline uint8_t ledGetY(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_Y_BIT_OFFSET)) & LED_XY_MASK); } +static inline uint8_t ledGetFunction(const ledConfig_t *lcfg) { return ((*lcfg >> LED_FUNCTION_OFFSET) & LED_BIT_MASK(LED_FUNCTION_BITCNT)); } +static inline uint8_t ledGetOverlay(const ledConfig_t *lcfg) { return ((*lcfg >> LED_OVERLAY_OFFSET) & LED_BIT_MASK(LED_OVERLAY_BITCNT)); } +static inline uint8_t ledGetColor(const ledConfig_t *lcfg) { return ((*lcfg >> LED_COLOR_OFFSET) & LED_BIT_MASK(LED_COLOR_BITCNT)); } +static inline uint8_t ledGetDirection(const ledConfig_t *lcfg) { return ((*lcfg >> LED_DIRECTION_OFFSET) & LED_BIT_MASK(LED_DIRECTION_BITCNT)); } +static inline uint8_t ledGetParams(const ledConfig_t *lcfg) { return ((*lcfg >> LED_PARAMS_OFFSET) & LED_BIT_MASK(LED_PARAMS_BITCNT)); } +static inline bool ledGetOverlayBit(const ledConfig_t *lcfg, int id) { return ((ledGetOverlay(lcfg) >> id) & 1); } +static inline bool ledGetDirectionBit(const ledConfig_t *lcfg, int id) { return ((ledGetDirection(lcfg) >> id) & 1); } +/* +PG_DECLARE_ARR(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs); +PG_DECLARE_ARR(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors); +PG_DECLARE_ARR(modeColorIndexes_t, LED_MODE_COUNT, modeColors); +PG_DECLARE(specialColorIndexes_t, specialColors); +*/ +bool parseColor(int index, const char *colorConfig); -bool parseLedStripConfig(uint8_t ledIndex, const char *config); +bool parseLedStripConfig(int ledIndex, const char *config); +void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize); +void reevaluateLedConfig(void); + +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); +void ledStripEnable(void); void updateLedStrip(void); -void updateLedRing(void); + +bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex); + +extern uint16_t rssi; // FIXME dependency on mw.c + void applyDefaultLedStripConfig(ledConfig_t *ledConfig); -void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize); - -bool parseColor(uint8_t index, const char *colorConfig); -void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); - -void ledStripEnable(void); -void reevalulateLedConfig(void); +void applyDefaultColors(hsvColor_t *colors); +void applyDefaultModeColors(modeColorIndexes_t *modeColors); +void applyDefaultSpecialColors(specialColorIndexes_t *specialColors); diff --git a/src/main/io/msp_protocol.h b/src/main/io/msp_protocol.h index 36b768af89..02054d1bf3 100644 --- a/src/main/io/msp_protocol.h +++ b/src/main/io/msp_protocol.h @@ -60,7 +60,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 17 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 20 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1803d87c2d..c48c7424ec 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -144,6 +144,7 @@ static void cliMap(char *cmdline); #ifdef LED_STRIP static void cliLed(char *cmdline); static void cliColor(char *cmdline); +static void cliModeColor(char *cmdline); #endif #ifndef USE_QUAD_MIXER_ONLY @@ -249,6 +250,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux), #ifdef LED_STRIP CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), + CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor), #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dump", "dump configuration", @@ -796,6 +798,9 @@ const clivalue_t valueTable[] = { { "acczero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.accZero.raw[X], .config.minmax = { -32768, 32767 }, 0 }, { "acczero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.accZero.raw[Y], .config.minmax = { -32768, 32767 }, 0 }, { "acczero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.accZero.raw[Z], .config.minmax = { -32768, 32767 }, 0 }, +#ifdef LED_STRIP + { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, +#endif { "accgain_x", VAR_INT16 | MASTER_VALUE, &masterConfig.accGain.raw[X], .config.minmax = { 1, 8192 }, 0 }, { "accgain_y", VAR_INT16 | MASTER_VALUE, &masterConfig.accGain.raw[Y], .config.minmax = { 1, 8192 }, 0 }, @@ -1299,20 +1304,20 @@ static void cliLed(char *cmdline) char ledConfigBuffer[20]; if (isEmpty(cmdline)) { - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); cliPrintf("led %u %s\r\n", i, ledConfigBuffer); } } else { ptr = cmdline; i = atoi(ptr); - if (i < MAX_LED_STRIP_LENGTH) { + if (i < LED_MAX_STRIP_LENGTH) { ptr = strchr(cmdline, ' '); if (!parseLedStripConfig(i, ++ptr)) { cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, MAX_LED_STRIP_LENGTH - 1); + cliShowArgumentRangeError("index", 0, LED_MAX_STRIP_LENGTH - 1); } } } @@ -1323,7 +1328,7 @@ static void cliColor(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { cliPrintf("color %u %d,%u,%u\r\n", i, masterConfig.colors[i].h, @@ -1334,16 +1339,57 @@ static void cliColor(char *cmdline) } else { ptr = cmdline; i = atoi(ptr); - if (i < CONFIGURABLE_COLOR_COUNT) { + if (i < LED_CONFIGURABLE_COLOR_COUNT) { ptr = strchr(cmdline, ' '); if (!parseColor(i, ++ptr)) { cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, CONFIGURABLE_COLOR_COUNT - 1); + cliShowArgumentRangeError("index", 0, LED_CONFIGURABLE_COLOR_COUNT - 1); } } } + +static void cliModeColor(char *cmdline) +{ + if (isEmpty(cmdline)) { + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + int colorIndex = modeColors[i].color[j]; + cliPrintf("mode_color %u %u %u\r\n", i, j, colorIndex); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + int colorIndex = specialColors.color[j]; + cliPrintf("mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); + } + } else { + enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; + int args[ARGS_COUNT]; + int argNo = 0; + char* ptr = strtok(cmdline, " "); + while (ptr && argNo < ARGS_COUNT) { + args[argNo++] = atoi(ptr); + ptr = strtok(NULL, " "); + } + + if (ptr != NULL || argNo != ARGS_COUNT) { + cliShowParseError(); + return; + } + + int modeIdx = args[MODE]; + int funIdx = args[FUNCTION]; + int color = args[COLOR]; + if(!setModeColor(modeIdx, funIdx, color)) { + cliShowParseError(); + return; + } + // values are validated + cliPrintf("mode_color %u %u %u\r\n", modeIdx, funIdx, color); + } +} #endif #ifdef USE_SERVOS @@ -1814,6 +1860,9 @@ static void cliDump(char *cmdline) cliPrint("\r\n\r\n# color\r\n"); cliColor(""); + + cliPrint("\r\n\r\n# mode_color\r\n"); + cliModeColor(""); #endif printSectionBreak(); dumpValues(MASTER_VALUE); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 7ab7c33960..7289fb8cfa 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -48,6 +48,7 @@ #include "drivers/buf_writer.h" #include "rx/rx.h" #include "rx/msp.h" +#include "blackbox/blackbox.h" #include "io/escservo.h" #include "io/rc_controls.h" @@ -1047,8 +1048,8 @@ static bool processOutCommand(uint8_t cmdMSP) #ifdef LED_STRIP case MSP_LED_COLORS: - headSerialReply(CONFIGURABLE_COLOR_COUNT * 4); - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + headSerialReply(LED_CONFIGURABLE_COLOR_COUNT * 4); + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; serialize16(color->h); serialize8(color->s); @@ -1057,14 +1058,27 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_LED_STRIP_CONFIG: - headSerialReply(MAX_LED_STRIP_LENGTH * 7); - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + headSerialReply(LED_MAX_STRIP_LENGTH * 4); + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); - serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); - serialize8(GET_LED_X(ledConfig)); - serialize8(GET_LED_Y(ledConfig)); - serialize8(ledConfig->color); + serialize32(*ledConfig); + } + break; + + case MSP_LED_STRIP_MODECOLOR: + headSerialReply(((LED_MODE_COUNT * LED_DIRECTION_COUNT) + LED_SPECIAL_COLOR_COUNT) * 3); + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + serialize8(i); + serialize8(j); + serialize8(masterConfig.modeColors[i].color[j]); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + serialize8(LED_MODE_COUNT); + serialize8(j); + serialize8(masterConfig.specialColors.color[j]); } break; #endif @@ -1092,18 +1106,18 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_3D: - headSerialReply(2 * 4); + headSerialReply(2 * 3); serialize16(masterConfig.flight3DConfig.deadband3d_low); serialize16(masterConfig.flight3DConfig.deadband3d_high); serialize16(masterConfig.flight3DConfig.neutral3d); - serialize16(masterConfig.flight3DConfig.deadband3d_throttle); break; case MSP_RC_DEADBAND: - headSerialReply(3); + headSerialReply(5); serialize8(currentProfile->rcControlsConfig.deadband); serialize8(currentProfile->rcControlsConfig.yaw_deadband); serialize8(currentProfile->rcControlsConfig.alt_hold_deadband); + serialize16(masterConfig.flight3DConfig.deadband3d_throttle); break; case MSP_SENSOR_ALIGNMENT: headSerialReply(3); @@ -1553,7 +1567,7 @@ static bool processInCommand(void) #ifdef LED_STRIP case MSP_SET_LED_COLORS: - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; color->h = read16(); color->s = read8(); @@ -1564,29 +1578,24 @@ static bool processInCommand(void) case MSP_SET_LED_STRIP_CONFIG: { i = read8(); - if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) { + if (i >= LED_MAX_STRIP_LENGTH || currentPort->dataSize != (1 + 4)) { headSerialError(0); break; } ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - uint16_t mask; - // currently we're storing directions and functions in a uint16 (flags) - // the msp uses 2 x uint16_t to cater for future expansion - mask = read16(); - ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; + *ledConfig = read32(); + reevaluateLedConfig(); + } + break; - mask = read16(); - ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; + case MSP_SET_LED_STRIP_MODECOLOR: + { + ledModeIndex_e modeIdx = read8(); + int funIdx = read8(); + int color = read8(); - mask = read8(); - ledConfig->xy = CALCULATE_LED_X(mask); - - mask = read8(); - ledConfig->xy |= CALCULATE_LED_Y(mask); - - ledConfig->color = read8(); - - reevalulateLedConfig(); + if (!setModeColor(modeIdx, funIdx, color)) + return false; } break; #endif @@ -1596,6 +1605,7 @@ static bool processInCommand(void) #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE case MSP_SET_4WAY_IF: + // get channel number // switch all motor lines HI // reply the count of ESC found headSerialReply(1); diff --git a/src/main/main.c b/src/main/main.c index 66905700ac..4bb6c3194a 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -118,7 +118,7 @@ void gpsPreInit(gpsConfig_t *initialGpsConfig); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); void spektrumBind(rxConfig_t *rxConfig); const sonarHcsr04Hardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor); @@ -468,7 +468,7 @@ void init(void) #endif #ifdef LED_STRIP - ledStripInit(masterConfig.ledConfigs, masterConfig.colors); + ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index bb7eb60aff..cc43e8d7da 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -208,7 +208,7 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea uint8_t calculateBatteryPercentage(void) { - return (((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount); + return constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); } uint8_t calculateBatteryCapacityRemainingPercentage(void) diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 4e518163c3..1dfb5bcd7d 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -26,6 +26,7 @@ extern "C" { #include "common/color.h" #include "common/axis.h" + #include "common/utils.h" #include "sensors/battery.h" #include "config/runtime_config.h" @@ -43,62 +44,66 @@ extern "C" { #include "gtest/gtest.h" extern "C" { - extern ledConfig_t *ledConfigs; extern uint8_t highestYValueForNorth; extern uint8_t lowestYValueForSouth; extern uint8_t highestXValueForWest; extern uint8_t lowestXValueForEast; extern uint8_t ledGridWidth; extern uint8_t ledGridHeight; + extern ledCounts_t ledCounts; + void updateLedCount(void); void determineLedStripDimensions(void); void determineOrientationLimits(void); - - ledConfig_t systemLedConfigs[MAX_LED_STRIP_LENGTH]; } +// utility macros +#define LF(name) LED_FUNCTION_ ## name +#define LO(name) LED_FLAG_OVERLAY(LED_OVERLAY_ ## name) +#define LD(name) LED_FLAG_DIRECTION(LED_DIRECTION_ ## name) + TEST(LedStripTest, parseLedStripConfig) { // given static const ledConfig_t expectedLedStripConfig[WS2811_LED_STRIP_LENGTH] = { - { CALCULATE_LED_XY( 9, 9), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY(10, 10), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY(11, 11), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(11, 11), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(10, 10), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + DEFINE_LED( 9, 9, 0, LD(SOUTH) , LF(FLIGHT_MODE) , LO(WARNING) , 0), + DEFINE_LED(10, 10, 0, LD(SOUTH) , LF(FLIGHT_MODE) , LO(WARNING) , 0), + DEFINE_LED(11, 11, 0, LD(SOUTH) , LF(ARM_STATE) , LO(INDICATOR) , 0), + DEFINE_LED(11, 11, 0, LD(EAST) , LF(ARM_STATE) , LO(INDICATOR) , 0), + DEFINE_LED(10, 10, 0, LD(EAST) , LF(FLIGHT_MODE) , 0, 0), - { CALCULATE_LED_XY(10, 5), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(11, 4), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(12, 3), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(12, 2), 0, LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY(11, 1), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY(10, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + DEFINE_LED(10, 5, 0, LD(SOUTH) , LF(FLIGHT_MODE) , 0, 0), + DEFINE_LED(11, 4, 0, LD(SOUTH) , LF(FLIGHT_MODE) , 0, 0), + DEFINE_LED(12, 3, 0, LD(SOUTH) , LF(ARM_STATE) , LO(INDICATOR) , 0), + DEFINE_LED(12, 2, 0, LD(NORTH) , LF(ARM_STATE) , LO(INDICATOR) , 0), + DEFINE_LED(11, 1, 0, LD(NORTH) , LF(FLIGHT_MODE) , 0, 0), + DEFINE_LED(10, 0, 0, LD(NORTH) , LF(FLIGHT_MODE) , 0, 0), - { CALCULATE_LED_XY( 7, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 6, 0), 1, LED_DIRECTION_NORTH | LED_FUNCTION_COLOR | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 5, 0), 1, LED_DIRECTION_NORTH | LED_FUNCTION_COLOR | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + DEFINE_LED( 7, 0, 0, LD(NORTH) , LF(FLIGHT_MODE) , LO(WARNING) , 0), + DEFINE_LED( 6, 0, 1, LD(NORTH) , LF(COLOR) , LO(WARNING) , 0), + DEFINE_LED( 5, 0, 1, LD(NORTH) , LF(COLOR) , LO(WARNING) , 0), + DEFINE_LED( 4, 0, 0, LD(NORTH) , LF(FLIGHT_MODE) , LO(WARNING) , 0), - { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 3), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 4), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 2, 5), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + DEFINE_LED( 2, 0, 0, LD(NORTH) , LF(FLIGHT_MODE) , 0, 0), + DEFINE_LED( 1, 1, 0, LD(NORTH) , LF(FLIGHT_MODE) , 0, 0), + DEFINE_LED( 0, 2, 0, LD(NORTH) , LF(ARM_STATE) , LO(INDICATOR) , 0), + DEFINE_LED( 0, 3, 0, LD(WEST) , LF(ARM_STATE) , LO(INDICATOR) , 0), + DEFINE_LED( 1, 4, 0, LD(WEST) , LF(FLIGHT_MODE) , 0, 0), + DEFINE_LED( 2, 5, 0, LD(WEST) , LF(FLIGHT_MODE) , 0, 0), - { CALCULATE_LED_XY( 1, 10), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 10), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 9), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + DEFINE_LED( 1, 10, 0, LD(WEST) , LF(FLIGHT_MODE) , 0, 0), + DEFINE_LED( 0, 11, 0, LD(WEST) , LF(ARM_STATE) , LO(INDICATOR) , 0), + DEFINE_LED( 0, 11, 0, LD(SOUTH) , LF(ARM_STATE) , LO(INDICATOR) , 0), + DEFINE_LED( 1, 10, 0, LD(SOUTH) , LF(FLIGHT_MODE) , LO(WARNING) , 0), + DEFINE_LED( 2, 9, 0, LD(SOUTH) , LF(FLIGHT_MODE) , LO(WARNING) , 0), - { CALCULATE_LED_XY( 7, 7), 14, LED_FUNCTION_THRUST_RING }, - { CALCULATE_LED_XY( 8, 7), 15, LED_FUNCTION_THRUST_RING }, - { CALCULATE_LED_XY( 8, 8), 14, LED_FUNCTION_THRUST_RING }, - { CALCULATE_LED_XY( 7, 8), 15, LED_FUNCTION_THRUST_RING }, + DEFINE_LED( 7, 7, 14, 0, LF(THRUST_RING) , 0 , 0), + DEFINE_LED( 8, 7, 15, 0, LF(THRUST_RING) , 0 , 0), + DEFINE_LED( 8, 8, 14, 0, LF(THRUST_RING) , 0 , 0), + DEFINE_LED( 7, 8, 15, 0, LF(THRUST_RING) , 0 , 0), - { 0, 0, 0 }, - { 0, 0, 0 }, + DEFINE_LED( 0, 0, 0, 0 , 0 , 0 , 0), + DEFINE_LED( 0, 0, 0, 0 , 0 , 0 , 0), }; // and @@ -108,15 +113,15 @@ TEST(LedStripTest, parseLedStripConfig) // right rear cluster "9,9:S:FW:0", "10,10:S:FW:0", - "11,11:S:IA:0", - "11,11:E:IA:0", + "11,11:S:AI:0", + "11,11:E:AI:0", "10,10:E:F:0", // right front cluster "10,5:S:F:0", "11,4:S:F:0", - "12,3:S:IA:0", - "12,2:N:IA:0", + "12,3:S:AI:0", + "12,2:N:AI:0", "11,1:N:F:0", "10,0:N:F:0", @@ -129,15 +134,15 @@ TEST(LedStripTest, parseLedStripConfig) // left front cluster "2,0:N:F:0", "1,1:N:F:0", - "0,2:N:IA:0", - "0,3:W:IA:0", + "0,2:N:AI:0", + "0,3:W:AI:0", "1,4:W:F:0", "2,5:W:F:0", // left rear cluster "1,10:W:F:0", - "0,11:W:IA:0", - "0,11:S:IA:0", + "0,11:W:AI:0", + "0,11:S:AI:0", "1,10:S:FW:0", "2,9:S:FW:0", @@ -148,31 +153,33 @@ TEST(LedStripTest, parseLedStripConfig) "7,8::R:15" }; // and - memset(&systemLedConfigs, 0, sizeof(systemLedConfigs)); - ledConfigs = systemLedConfigs; + memset(ledConfigs_arr(), 0, sizeof(*ledConfigs_arr())); // and - bool ok = false; + bool result = true; // when - for (uint8_t index = 0; index < (sizeof(ledStripConfigCommands) / sizeof(ledStripConfigCommands[0])); index++) { - ok |= parseLedStripConfig(index, ledStripConfigCommands[index]); + for (unsigned index = 0; index < ARRAYLEN(ledStripConfigCommands); index++) { + result = result && parseLedStripConfig(index, ledStripConfigCommands[index]); } // then - EXPECT_EQ(true, ok); - EXPECT_EQ(30, ledCount); - EXPECT_EQ(4, ledsInRingCount); + EXPECT_EQ(true, result); + EXPECT_EQ(30, ledCounts.count); + EXPECT_EQ(4, ledCounts.ring); // and - for (uint8_t index = 0; index < WS2811_LED_STRIP_LENGTH; index++) { + for (int index = 0; index < WS2811_LED_STRIP_LENGTH; index++) { #ifdef DEBUG_LEDSTRIP printf("iteration: %d\n", index); #endif - EXPECT_EQ(expectedLedStripConfig[index].xy, ledConfigs[index].xy); - EXPECT_EQ(expectedLedStripConfig[index].flags, ledConfigs[index].flags); - EXPECT_EQ(expectedLedStripConfig[index].color, ledConfigs[index].color); + EXPECT_EQ(ledGetXY(&expectedLedStripConfig[index]), ledGetXY(ledConfigs(index))); + EXPECT_EQ(ledGetFunction(&expectedLedStripConfig[index]), ledGetFunction(ledConfigs(index))); + EXPECT_EQ(ledGetOverlay(&expectedLedStripConfig[index]), ledGetOverlay(ledConfigs(index))); + EXPECT_EQ(ledGetColor(&expectedLedStripConfig[index]), ledGetColor(ledConfigs(index))); + EXPECT_EQ(ledGetDirection(&expectedLedStripConfig[index]), ledGetDirection(ledConfigs(index))); + EXPECT_EQ(ledGetParams(&expectedLedStripConfig[index]), ledGetParams(ledConfigs(index))); } // then @@ -189,26 +196,27 @@ TEST(LedStripTest, parseLedStripConfig) TEST(LedStripTest, smallestGridWithCenter) { // given - memset(&systemLedConfigs, 0, sizeof(systemLedConfigs)); - ledConfigs = systemLedConfigs; + memset(ledConfigs_arr(), 0, sizeof(*ledConfigs_arr())); // and static const ledConfig_t testLedConfigs[] = { - { CALCULATE_LED_XY( 2, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 2, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 1, 2), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING } + DEFINE_LED( 2, 2, 0, LD(SOUTH) | LD(EAST) , LF(ARM_STATE) , LO(INDICATOR), 0), + DEFINE_LED( 2, 1, 0, LD(EAST) , LF(FLIGHT_MODE) , LO(WARNING) , 0), + DEFINE_LED( 2, 0, 0, LD(NORTH) | LD(EAST) , LF(ARM_STATE) , LO(INDICATOR), 0), + DEFINE_LED( 1, 0, 0, LD(NORTH) , LF(FLIGHT_MODE) , LO(WARNING) , 0), + DEFINE_LED( 0, 0, 0, LD(NORTH) | LD(WEST) , LF(ARM_STATE) , LO(INDICATOR), 0), + DEFINE_LED( 0, 1, 0, LD(WEST) , LF(FLIGHT_MODE) , LO(WARNING) , 0), + DEFINE_LED( 0, 2, 0, LD(SOUTH) | LD(WEST) , LF(ARM_STATE) , LO(INDICATOR), 0), + DEFINE_LED( 1, 2, 0, LD(SOUTH) , LF(FLIGHT_MODE) , LO(WARNING) , 0), }; - memcpy(&systemLedConfigs, &testLedConfigs, sizeof(testLedConfigs)); + memcpy(ledConfigs_arr(), &testLedConfigs, sizeof(testLedConfigs)); // when + updateLedCount(); determineLedStripDimensions(); // then + EXPECT_EQ(8, ledCounts.count); EXPECT_EQ(3, ledGridWidth); EXPECT_EQ(3, ledGridHeight); @@ -225,22 +233,23 @@ TEST(LedStripTest, smallestGridWithCenter) TEST(LedStripTest, smallestGrid) { // given - memset(&systemLedConfigs, 0, sizeof(systemLedConfigs)); - ledConfigs = systemLedConfigs; + memset(ledConfigs_arr(), 0, sizeof(*ledConfigs_arr())); // and static const ledConfig_t testLedConfigs[] = { - { CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + DEFINE_LED( 1, 1, 0, LD(SOUTH) | LD(EAST) , LF(FLIGHT_MODE) , LO(INDICATOR), 0), + DEFINE_LED( 1, 0, 0, LD(NORTH) | LD(EAST) , LF(FLIGHT_MODE) , LO(INDICATOR), 0), + DEFINE_LED( 0, 0, 0, LD(NORTH) | LD(WEST) , LF(FLIGHT_MODE) , LO(INDICATOR), 0), + DEFINE_LED( 0, 1, 0, LD(SOUTH) | LD(WEST) , LF(FLIGHT_MODE) , LO(INDICATOR), 0), }; - memcpy(&systemLedConfigs, &testLedConfigs, sizeof(testLedConfigs)); + memcpy(ledConfigs_arr(), &testLedConfigs, sizeof(testLedConfigs)); // when + updateLedCount(); determineLedStripDimensions(); // then + EXPECT_EQ(4, ledCounts.count); EXPECT_EQ(2, ledGridWidth); EXPECT_EQ(2, ledGridHeight); @@ -255,59 +264,56 @@ TEST(LedStripTest, smallestGrid) } /* - { CALCULATE_LED_XY( 1, 14), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + LED( 1, 14, 0, LD(SOUTH) , LF(FLIGHT_MODE) , LO(INDICATOR) , 0 ), - { CALCULATE_LED_XY( 0, 13), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 12), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + LED( 0, 13, 0, LD(WEST) , LF(ARM_STATE) , LO(INDICATOR) , 0 ), + LED( 0, 12, 0, LD(WEST) , LF(ARM_STATE) , LO(INDICATOR) , 0 ), - { CALCULATE_LED_XY( 0, 11), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 10), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 9), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 8), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 7), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 6), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 5), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 4), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 0, 3), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + LED( 0, 11, 0, LD(WEST) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 0, 10, 0, LD(WEST) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 0, 9, 0, LD(WEST) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 0, 8, 0, LD(WEST) , LF(FLIGHT_MODE) , LO(WARNING) , 0 ), + LED( 0, 7, 0, LD(WEST) , LF(FLIGHT_MODE) , LO(WARNING) , 0 ), + LED( 0, 6, 0, LD(WEST) , LF(FLIGHT_MODE) , LO(WARNING) , 0 ), + LED( 0, 5, 0, LD(WEST) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 0, 4, 0, LD(WEST) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 0, 3, 0, LD(WEST) , LF(FLIGHT_MODE) , 0 , 0 ), - { CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + LED( 0, 2, 0, LD(WEST) , LF(ARM_STATE) , LO(INDICATOR) , 0 ), + LED( 0, 1, 0, LD(WEST) , LF(ARM_STATE) , LO(INDICATOR) , 0 ), - { CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 3, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + LED( 1, 0, 0, LD(NORTH) , LF(FLIGHT_MODE) , LO(INDICATOR) , 0 ), + LED( 2, 0, 0, LD(NORTH) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 3, 0, 0, LD(NORTH) , LF(FLIGHT_MODE) , LO(INDICATOR) , 0 ), - { CALCULATE_LED_XY( 4, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 4, 2), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + LED( 4, 1, 0, LD(EAST) , LF(ARM_STATE) , LO(INDICATOR) , 0 ), + LED( 4, 2, 0, LD(EAST) , LF(ARM_STATE) , LO(INDICATOR) , 0 ), - { CALCULATE_LED_XY( 4, 3), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 4), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 5), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 6), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 7), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 8), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 4, 9), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 10), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 4, 11), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + LED( 4, 3, 0, LD(EAST) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 4, 4, 0, LD(EAST) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 4, 5, 0, LD(EAST) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 4, 6, 0, LD(EAST) , LF(FLIGHT_MODE) , LO(WARNING) , 0 ), + LED( 4, 7, 0, LD(EAST) , LF(FLIGHT_MODE) , LO(WARNING) , 0 ), + LED( 4, 8, 0, LD(EAST) , LF(FLIGHT_MODE) , LO(WARNING) , 0 ), + LED( 4, 9, 0, LD(EAST) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 4, 10, 0, LD(EAST) , LF(FLIGHT_MODE) , 0 , 0 ), + LED( 4, 11, 0, LD(EAST) , LF(FLIGHT_MODE) , 0 , 0 ), - { CALCULATE_LED_XY( 4, 12), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { CALCULATE_LED_XY( 4, 13), 0, LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + LED( 4, 12, 0, LD(EAST) , LF(ARM_STATE) , LO(INDICATOR) , 0 ), + LED( 4, 13, 0, LD(EAST) , LF(ARM_STATE) , LO(INDICATOR) , 0 ), - { CALCULATE_LED_XY( 3, 14), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + LED( 3, 14, 0, LD(SOUTH) , LF(FLIGHT_MODE) , LO(INDICATOR) , 0 ), */ -hsvColor_t testColors[CONFIGURABLE_COLOR_COUNT]; - -extern hsvColor_t *colors; +hsvColor_t testColors[LED_CONFIGURABLE_COLOR_COUNT]; #define TEST_COLOR_COUNT 4 TEST(ColorTest, parseColor) { // given - colors = testColors; - memset(colors, 0, sizeof(colors) * CONFIGURABLE_COLOR_COUNT); + memset(colors_arr(), 0, sizeof(*colors_arr())); // and const hsvColor_t expectedColors[TEST_COLOR_COUNT] = { @@ -326,7 +332,7 @@ TEST(ColorTest, parseColor) }; // when - for (uint8_t index = 0; index < TEST_COLOR_COUNT; index++) { + for (int index = 0; index < TEST_COLOR_COUNT; index++) { #ifdef DEBUG_LEDSTRIP printf("parse iteration: %d\n", index); #endif @@ -336,14 +342,14 @@ TEST(ColorTest, parseColor) // then - for (uint8_t index = 0; index < TEST_COLOR_COUNT; index++) { + for (int index = 0; index < TEST_COLOR_COUNT; index++) { #ifdef DEBUG_LEDSTRIP printf("iteration: %d\n", index); #endif - EXPECT_EQ(expectedColors[index].h, colors[index].h); - EXPECT_EQ(expectedColors[index].s, colors[index].s); - EXPECT_EQ(expectedColors[index].v, colors[index].v); + EXPECT_EQ(expectedColors[index].h, colors(index)->h); + EXPECT_EQ(expectedColors[index].s, colors(index)->s); + EXPECT_EQ(expectedColors[index].v, colors(index)->v); } } @@ -362,23 +368,23 @@ batteryState_e getBatteryState(void) { void ws2811LedStripInit(void) {} void ws2811UpdateStrip(void) {} -void setLedValue(uint16_t index, const uint8_t value) { +void setLedValue(int index, const uint8_t value) { UNUSED(index); UNUSED(value); } -void setLedHsv(uint16_t index, const hsvColor_t *color) { +void setLedHsv(int index, const hsvColor_t *color) { UNUSED(index); UNUSED(color); } -void getLedHsv(uint16_t index, hsvColor_t *color) { +void getLedHsv(int index, hsvColor_t *color) { UNUSED(index); UNUSED(color); } -void scaleLedValue(uint16_t index, const uint8_t scalePercent) { +void scaleLedValue(int index, const uint8_t scalePercent) { UNUSED(index); UNUSED(scalePercent); } @@ -418,7 +424,14 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) { return 0; } +bool rcModeIsActive(boxId_e modeId) { return rcModeActivationMask & (1 << modeId); } bool failsafeIsActive() { return false; } bool rxIsReceivingSignal() { return true; } +bool sensors(uint32_t mask) { UNUSED(mask); return true; } +uint8_t calculateBatteryCapacityRemainingPercentage(void) { return 40; } + +uint8_t GPS_numSat; +uint8_t stateFlags; +uint16_t rssi; }