diff --git a/docs/LedStrip.md b/docs/LedStrip.md index 9f6900a435..449903a969 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -7,6 +7,7 @@ require that all the LEDs in the strip show the same color. Addressable LED strips can be used to show information from the flight controller system, the current implementation supports the following: +* Up to 32 LEDs. * Indicators showing pitch/roll stick positions. * Heading/Orientation lights. * Flight mode specific color schemes. @@ -17,12 +18,12 @@ The function and orientation configuration is fixed for now but later it should 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 28 LEDs is possible, it just requires additional development. +Likewise, support for more than 32 LEDs is possible, it just requires additional development. ## Supported hardware -Only strips of 28 WS2812 LEDs are supported currently. If the strip is longer than 28 leds it does not matter, -but only the first 28 are used. +Only strips of 32 WS2812 LEDs are supported currently. If the strip is longer than 32 leds it does not matter, +but only the first 32 are used. WS2812 LEDs require an 800khz signal and precise timings and thus requires the use of a dedicated hardware timer. @@ -42,24 +43,220 @@ uses. e.g. ESC1/BEC1 -> FC, ESC2/BEC2 -> LED strip. It's also possible to pow from another BEC. Just ensure that the GROUND is the same for all BEC outputs and LEDs. -| Target | Pin | Led Strip | -| --------------------- | --- | --------- | -| Naze/Olimexino | RC5 | Data In | -| ChebuzzF3/F3Discovery | PB8 | Data In | +| Target | Pin | Led Strip | Signal | +| --------------------- | --- | --------- | -------| +| Naze/Olimexino | RC5 | Data In | PA6 | +| CC3D | ??? | Data In | PB4 | +| ChebuzzF3/F3Discovery | PB8 | Data In | PB8 | Since RC5 is also used for SoftSerial on the Naze/Olimexino it means that you cannot use softserial and led strips at the same time. Additionally, since RC5 is also used for Parallel PWM RC input on both the Naze, Chebuzz and STM32F3Discovery targets, led strips can not be used at the same time at Parallel PWM. + +## Configuration + +Enable the `LED_STRIP` feature via the cli: + +``` +feature LED_STRIP +``` + +If you enable LED_STRIP feature and the feature is turned off again after a reboot then check your config does not conflict with other features, as above. + +Configure the LEDs using the `led` command. + +The `led` command takes either zero or two arguments - an zero-based led number and a pair of coordinates, direction flags and mode flags. + +If used with zero arguments it prints out the led configuration which can be copied for future reference. + +Each led is configured using the following template: `x,y:ddd:mmm` + +`x` and `y` are grid coordinates of a 0 based 16x16 grid, north west is 0,0, south east is 15,15 +`ddd` specifies the directions, since an led can face in any direction it can have multiple directions. Directions are: + + `N` - North + `E` - East + `S` - South + `W` - West + `U` - Up + `D` - Down + +For instance, an LED that faces South-east at a 45 degree downwards angle could be configured as `SED`. + +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: + +* `B` - `B`attery warning. +* `F` - `F`light mode & Orientation +* `I` - `I`ndicator. +* `A` - `A`rmed state. + +Example: + +``` +led 0 0,15:SD:IAB +led 1 15,0:ND:IA +led 2 0,0:ND:IAB +led 3 0,15:SD:IAB +``` + +to erase an led, and to mark the end of the chain, use `0,0::` as the second argument, like this: + +``` +led 4 0,0:: +``` + + +### Modes + +#### Battery Warning + +This mode simply flashes the LED RED when the battery is low if battery monitoring is enabled. + +#### Flight Mode & Orientation + +This mode shows the flight mode and orientation. + +When flight modes are active then the leds are updated to show different colors depending on the mode, placement on the grid and direction. + +Leds are set in a specific order: + * Leds that marked as facing up or down. + * Leds that marked as facing west or east AND are on the west or east side of the grid. + * Leds that marked as facing north or south AND are on the north or south side of the grid. + +That is, south facing leds have priority. + +#### Indicator + +This mode flashes LEDs that correspond to roll and pitch stick positions. i.e. they indicate the direction the craft is going to turn. + +#### Armed state + +This mode toggles LEDs between green and blue when disarmed and armed, respectively. + +Note: Armed State cannot be used with Flight Mode. + ## Positioning -Cut the strip into 5 sections as per diagram below. When the strips are cut ensure you reconnect each output to each input with cable where the break is made. +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. e.g. connect 5V out to 5V in, GND to GND and Data Out to Data In. Orientation is when viewed with the front of the aircraft facing away from you and viewed from above. -LED numbers and positions for a quad. +### Example 12 LED config + +The default configuration is as follows +``` +led 0 2,2:ES:IA +led 1 2,1:E:BF +led 2 2,0:NE:IA +led 3 1,0:N:BF +led 4 0,0:NW:IA +led 5 0,1:W:BF +led 6 0,2:SW:IA +led 7 1,2:S:BF +led 8 1,1:U:BF +led 9 1,1:U:BF +led 10 1,1:D:BF +led 11 1,1:D:BF +``` + +Which translates into the following positions: + +``` + 5 3 + \ / + \ 4 / + \ FRONT / + 6 | 9-12 | 2 + / BACK \ + / 8 \ + / \ + 7 1 +``` + +LEDs 1,3,5 and 7 should be placed underneath the quad, facing downwards. +LEDs 2, 4, 6 and 8 should be positioned so the face east/north/west/south, respectively. +LEDs 9-10 should be placed facing down, in the middle +LEDs 11-12 should be placed facing up, in the middle + +This is the default so that if you don't want to place LEDs top and bottom in the middle just connect the first 8 leds. + +### Example 16 LED config + +``` +15,15:SD:IA +8,8:E:FB +8,7:E:FB +15,0:ND:IA +7,7:N:FB +8,7:N:FB +0,0:ND:IA +7,7:W:FB +7,8:W:FB +0,15:SD:IA +7,8:S:FB +8,8:S:FB +7,7:D:FB +8,7:D:FB +7,7:U:FB +8,7:U:FB +``` + +Which translates into the following positions: + +``` + 7 4 + \ / + \ 6-5 / + 8 \ FRONT / 3 + | 13-16 | + 9 / BACK \ 2 + / 11-12 \ + / \ + 10 1 +``` + +LEDs 1,4,7 and 10 should be placed underneath the quad, facing downwards. +LEDs 2-3, 6-5, 8-9 and 11-12 should be positioned so the face east/north/west/south, respectively. +LEDs 13-14 should be placed facing down, in the middle +LEDs 15-16 should be placed facing up, in the middle + +### Exmple 28 LED config + +``` +9,9:S:FB +10,10:S:FB +11,11:S:IA +11,11:E:IA +10,10:E:F +9,9:E:F +10,5:S:F +11,4:S:F +12,3:S:IA +12,2:N:IA +11,1:N:F +10,0:N:F +7,0:N:FB +6,0:N:FB +5,0:N:FB +4,0:N:FB +2,0:N:F +1,1:N:F +0,2:N:IA +0,3:W:IA +1,4:W:F +2,5:W:F +2,9:W:F +1,10:W:F +0,11:W:IA +0,11:S:IA +1,10:S:FB +2,9:S:FB +``` ``` 17-19 10-12 @@ -72,15 +269,12 @@ LED numbers and positions for a quad. 26-28 1-3 ``` -## Configuration +All LEDs should face outwards from the chassis in this configuration. -Enable the `LED_STRIP` feature via the cli: +Note: +This configuration is specifically designed for the Alien Spider AQ50D PRO 250mm frame. -``` -feature LED_STRIP -``` - -If you enable LED_STRIP feature and the feature is turned off again after a reboot then check your config does not conflict with other features, as above. +http://www.goodluckbuy.com/alien-spider-aq50d-pro-250mm-mini-quadcopter-carbon-fiber-micro-multicopter-frame.html ## Troubleshooting diff --git a/src/main/config/config.c b/src/main/config/config.c index e7e6f15d86..7296af3c8b 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -50,6 +50,7 @@ #include "rx/rx.h" #include "io/rc_controls.h" #include "io/rc_curves.h" +#include "io/ledstrip.h" #include "io/gps.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -97,7 +98,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; // profile config struct -static const uint8_t EEPROM_CONF_VERSION = 76; +static const uint8_t EEPROM_CONF_VERSION = 77; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -374,6 +375,8 @@ static void resetConf(void) for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) masterConfig.customMixer[i].throttle = 0.0f; + applyDefaultLedStripConfig(masterConfig.ledConfigs); + // copy first profile into remaining profile for (i = 1; i < 3; i++) memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index ffbb77bf1c..413d54f7dc 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -74,6 +74,8 @@ typedef struct master_t { telemetryConfig_t telemetryConfig; + ledConfig_t ledConfigs[MAX_LED_STRIP_LENGTH]; + profile_t profile[3]; // 3 separate profiles uint8_t current_profile_index; // currently loaded profile diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 43d982c4e3..2e46c92ef3 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -17,11 +17,7 @@ #pragma once -#ifdef USE_ALTERNATE_LED_LAYOUT -#define WS2811_LED_STRIP_LENGTH 31 -#else -#define WS2811_LED_STRIP_LENGTH 28 -#endif +#define WS2811_LED_STRIP_LENGTH 32 #define WS2811_BITS_PER_LED 24 #define WS2811_DELAY_BUFFER_LENGTH 42 // for 50us delay diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 53eff943f9..a9d7560db8 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -38,7 +38,7 @@ void ws2811LedStripHardwareInit(void) uint16_t prescalerValue; - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE); + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); GPIO_PinAFConfig(GPIOB, GPIO_PinSource8, GPIO_AF_1); diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 7b818056a4..9da6157f00 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -44,6 +44,7 @@ #include "io/gimbal.h" #include "io/gps.h" #include "io/serial.h" +#include "io/ledstrip.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index b690402009..249101bf3c 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -18,14 +18,23 @@ #include #include #include +#include +#include #include +#include + #ifdef LED_STRIP -#include #include "drivers/light_ws2811strip.h" #include "drivers/system.h" +#include "drivers/serial.h" + +#include +#include +#include + #include "sensors/battery.h" @@ -36,142 +45,99 @@ #include "io/ledstrip.h" -#define LED_WHITE {255, 255, 255} -#define LED_BLACK {0, 0, 0 } -#define LED_RED {255, 0, 0 } -#define LED_GREEN {0, 255, 0 } -#define LED_BLUE {0, 0, 255} -#define LED_CYAN {0, 255, 255} -#define LED_YELLOW {255, 255, 0 } -#define LED_ORANGE {255, 128, 0 } -#define LED_PINK {255, 0, 128} -#define LED_PURPLE {192, 64, 255} +#if MAX_LED_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH +#error "Led strip length must match driver" +#endif + +//#define USE_LED_ANIMATION + +#define LED_WHITE {255, 255, 255} +#define LED_BLACK {0, 0, 0 } + +#define LED_RED {255, 0, 0 } +#define LED_ORANGE {255, 128, 0 } +#define LED_YELLOW {255, 255, 0 } +#define LED_LIME_GREEN {128, 255, 0 } +#define LED_CYAN {0, 255, 255} +#define LED_GREEN {0, 255, 0 } +#define LED_LIGHT_BLUE {0, 128, 255} +#define LED_BLUE {0, 0, 255} +#define LED_DARK_MAGENTA {128, 0, 128} +#define LED_PINK {255, 0, 255} +#define LED_DARK_VIOLET {128, 0, 255} +#define LED_DEEP_PINK {255, 0, 128} const rgbColor24bpp_t black = { LED_BLACK }; +const rgbColor24bpp_t white = { LED_WHITE }; + const rgbColor24bpp_t red = { LED_RED }; const rgbColor24bpp_t orange = { LED_ORANGE }; -const rgbColor24bpp_t white = { LED_WHITE }; const rgbColor24bpp_t green = { LED_GREEN }; const rgbColor24bpp_t blue = { LED_BLUE }; + +uint8_t ledGridWidth; +uint8_t ledGridHeight; +uint8_t ledCount; + +ledConfig_t *ledConfigs; + +const ledConfig_t defaultLedStripConfig[] = { + { CALCULATE_LED_XY( 2, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 2, 1), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 2), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, +}; + + /* - * 0..5 - rear right cluster, 0..2 rear 3..5 right - * 6..11 - front right cluster, 6..8 rear, 9..11 front - * 12..15 - front center cluster - * 16..21 - front left cluster, 16..18 front, 19..21 rear - * 22..27 - rear left cluster, 22..24 left, 25..27 rear + * 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 { - 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_BATTERY = (1 << 7), - LED_FUNCTION_MODE = (1 << 8), - LED_FUNCTION_ARM_STATE = (1 << 9) -} ledFlag_e; + X_COORDINATE, + Y_COORDINATE, + DIRECTIONS, + FUNCTIONS +} parseState_e; -#define LED_X_BIT_OFFSET 4 -#define LED_Y_BIT_OFFSET 0 +#define PARSE_STATE_COUNT 4 -#define LED_XY_MASK (0x0F) +static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':', '\0' }; -#define LED_X(ledConfig) ((ledConfig->xy >> LED_X_BIT_OFFSET) & LED_XY_MASK) -#define LED_Y(ledConfig) ((ledConfig->xy >> LED_Y_BIT_OFFSET) & LED_XY_MASK) - -#define LED_XY(x,y) (((x & LED_XY_MASK) << LED_X_BIT_OFFSET) | ((y & LED_XY_MASK) << LED_Y_BIT_OFFSET)) - -typedef struct ledConfig_s { - uint8_t xy; // see LED_X/Y_MASK defines - uint16_t flags; // see ledFlag_e -} ledConfig_t; - -static uint8_t ledGridWidth; -static uint8_t ledGridHeight; - -#ifdef USE_ALTERNATE_LED_LAYOUT -static const ledConfig_t ledConfigs[WS2811_LED_STRIP_LENGTH] = { - { LED_XY( 1, 14), LED_DIRECTION_SOUTH | LED_FUNCTION_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { LED_XY( 0, 13), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY( 0, 12), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { LED_XY( 0, 11), LED_DIRECTION_WEST | LED_FUNCTION_MODE }, - { LED_XY( 0, 10), LED_DIRECTION_WEST | LED_FUNCTION_MODE }, - { LED_XY( 0, 9), LED_DIRECTION_WEST | LED_FUNCTION_MODE }, - { LED_XY( 0, 8), LED_DIRECTION_WEST | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY( 0, 7), LED_DIRECTION_WEST | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY( 0, 6), LED_DIRECTION_WEST | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY( 0, 5), LED_DIRECTION_WEST | LED_FUNCTION_MODE }, - { LED_XY( 0, 4), LED_DIRECTION_WEST | LED_FUNCTION_MODE }, - { LED_XY( 0, 3), LED_DIRECTION_WEST | LED_FUNCTION_MODE }, - - { LED_XY( 0, 2), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_FUNCTION_MODE | LED_FUNCTION_ARM_STATE }, - { LED_XY( 3, 0), LED_DIRECTION_NORTH | LED_FUNCTION_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { LED_XY( 4, 1), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY( 4, 2), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { LED_XY( 4, 3), LED_DIRECTION_EAST | LED_FUNCTION_MODE }, - { LED_XY( 4, 4), LED_DIRECTION_EAST | LED_FUNCTION_MODE }, - { LED_XY( 4, 5), LED_DIRECTION_EAST | LED_FUNCTION_MODE }, - { LED_XY( 4, 6), LED_DIRECTION_EAST | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY( 4, 7), LED_DIRECTION_EAST | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY( 4, 8), LED_DIRECTION_EAST | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY( 4, 9), LED_DIRECTION_EAST | LED_FUNCTION_MODE }, - { LED_XY( 4, 10), LED_DIRECTION_EAST | LED_FUNCTION_MODE }, - { LED_XY( 4, 11), LED_DIRECTION_EAST | LED_FUNCTION_MODE }, - - { LED_XY( 4, 12), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY( 4, 13), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { LED_XY( 3, 14), LED_DIRECTION_SOUTH | LED_FUNCTION_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, +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 }; -#else -static const ledConfig_t ledConfigs[WS2811_LED_STRIP_LENGTH] = { - { LED_XY( 9, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY(10, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY(11, 11), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY(11, 11), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY(10, 10), LED_DIRECTION_EAST | LED_FUNCTION_MODE }, - { LED_XY( 9, 9), LED_DIRECTION_EAST | LED_FUNCTION_MODE }, - { LED_XY(10, 5), LED_DIRECTION_SOUTH | LED_FUNCTION_MODE }, - { LED_XY(11, 4), LED_DIRECTION_SOUTH | LED_FUNCTION_MODE }, - { LED_XY(12, 3), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY(12, 2), LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY(11, 1), LED_DIRECTION_NORTH | LED_FUNCTION_MODE }, - { LED_XY(10, 0), LED_DIRECTION_NORTH | LED_FUNCTION_MODE }, - - { LED_XY( 7, 0), LED_DIRECTION_NORTH | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY( 6, 0), LED_DIRECTION_NORTH | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY( 5, 0), LED_DIRECTION_NORTH | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY( 4, 0), LED_DIRECTION_NORTH | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - - { LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_FUNCTION_MODE }, - { LED_XY( 1, 1), LED_DIRECTION_NORTH | LED_FUNCTION_MODE }, - { LED_XY( 0, 2), LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY( 0, 3), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY( 1, 4), LED_DIRECTION_WEST | LED_FUNCTION_MODE }, - { LED_XY( 2, 5), LED_DIRECTION_WEST | LED_FUNCTION_MODE }, - - { LED_XY( 2, 9), LED_DIRECTION_WEST | LED_FUNCTION_MODE }, - { LED_XY( 1, 10), LED_DIRECTION_WEST | LED_FUNCTION_MODE }, - { LED_XY( 0, 11), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY( 0, 11), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - { LED_XY( 1, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY }, - { LED_XY( 2, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_MODE | LED_FUNCTION_BATTERY } +static const char functionCodes[] = { 'I', 'B', 'F', 'A' }; +#define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0])) +static const uint16_t functionMappings[FUNCTION_COUNT] = { + LED_FUNCTION_INDICATOR, + LED_FUNCTION_BATTERY, + LED_FUNCTION_FLIGHT_MODE, + LED_FUNCTION_ARM_STATE }; -#endif // grid offsets uint8_t highestYValueForNorth; @@ -179,6 +145,168 @@ uint8_t lowestYValueForSouth; uint8_t highestXValueForWest; uint8_t lowestXValueForEast; +void determineLedStripDimensions(void) +{ + ledGridWidth = 0; + ledGridHeight = 0; + + uint8_t ledIndex; + const ledConfig_t *ledConfig; + + 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; + } + } +} + +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; + highestXValueForWest = (ledGridWidth / 2) - 1; + lowestXValueForEast = (ledGridWidth / 2) + widthModifier; +} + +void updateLedCount(void) +{ + uint8_t ledIndex; + ledCount = 0; + for (ledIndex = 0; ledIndex < MAX_LED_STRIP_LENGTH; ledIndex++) { + if (ledConfigs[ledIndex].flags == 0 && ledConfigs[ledIndex].xy == 0) { + break; + } + ledCount++; + } +} + +static void reevalulateLedConfig(void) +{ + updateLedCount(); + determineLedStripDimensions(); + determineOrientationLimits(); +} + +#define CHUNK_BUFFER_SIZE 10 + +#define NEXT_PARSE_STATE(parseState) ((parseState + 1) % PARSE_STATE_COUNT) + + +bool parseLedStripConfig(uint8_t ledIndex, const char *config) +{ + char chunk[CHUNK_BUFFER_SIZE]; + uint8_t chunkIndex; + uint8_t val; + + uint8_t parseState = X_COORDINATE; + bool ok = true; + + if (ledIndex >= MAX_LED_STRIP_LENGTH) { + return !ok; + } + + ledConfig_t *ledConfig = &ledConfigs[ledIndex]; + memset(ledConfig, 0, sizeof(ledConfig_t)); + + while (ok) { + + char chunkSeparator = chunkSeparators[parseState]; + + memset(&chunk, 0, sizeof(chunk)); + chunkIndex = 0; + + while (*config && chunkIndex < CHUNK_BUFFER_SIZE && *config != chunkSeparator) { + chunk[chunkIndex++] = *config++; + } + + if (*config++ != chunkSeparator) { + ok = false; + break; + } + + switch((parseState_e)parseState) { + case X_COORDINATE: + val = atoi(chunk); + ledConfig->xy |= CALCULATE_LED_X(val); + break; + case Y_COORDINATE: + val = atoi(chunk); + ledConfig->xy |= CALCULATE_LED_Y(val); + 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]; + 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]; + break; + } + } + } + break; + } + + parseState++; + if (parseState >= PARSE_STATE_COUNT) { + break; + } + } + + if (!ok) { + memset(ledConfig, 0, sizeof(ledConfig_t)); + } + + reevalulateLedConfig(); + + return ok; +} + +void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize) +{ + char functions[FUNCTION_COUNT]; + char directions[DIRECTION_COUNT]; + uint8_t index; + uint8_t mappingIndex; + ledConfig_t *ledConfig = &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]; + } + } + + for (mappingIndex = 0, index = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { + if (ledConfig->flags & directionMappings[mappingIndex]) { + directions[index++] = directionCodes[mappingIndex]; + } + } + + sprintf(ledConfigBuffer, "%u,%u:%s:%s", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions); +} + // timers uint32_t nextAnimationUpdateAt = 0; uint32_t nextIndicatorFlashAt = 0; @@ -207,69 +335,97 @@ typedef union { static const modeColors_t orientationModeColors = { .raw = { {LED_WHITE}, - {LED_BLUE}, + {LED_DARK_VIOLET}, {LED_RED}, - {LED_GREEN}, - {LED_PURPLE}, - {LED_CYAN} + {LED_DEEP_PINK}, + {LED_BLUE}, + {LED_ORANGE} } }; static const modeColors_t headfreeModeColors = { .raw = { - {LED_PINK}, - {LED_BLACK}, + {LED_LIME_GREEN}, + {LED_DARK_VIOLET}, {LED_ORANGE}, - {LED_BLACK}, - {LED_BLACK}, - {LED_BLACK} + {LED_DEEP_PINK}, + {LED_BLUE}, + {LED_ORANGE} } }; static const modeColors_t horizonModeColors = { .raw = { {LED_BLUE}, - {LED_BLACK}, + {LED_DARK_VIOLET}, {LED_YELLOW}, - {LED_BLACK}, - {LED_BLACK}, - {LED_BLACK} + {LED_DEEP_PINK}, + {LED_BLUE}, + {LED_ORANGE} } }; static const modeColors_t angleModeColors = { .raw = { {LED_CYAN}, - {LED_BLACK}, + {LED_DARK_VIOLET}, {LED_YELLOW}, - {LED_BLACK}, - {LED_BLACK}, - {LED_BLACK} + {LED_DEEP_PINK}, + {LED_BLUE}, + {LED_ORANGE} } }; static const modeColors_t magModeColors = { .raw = { - {LED_PURPLE}, - {LED_BLACK}, + {LED_PINK}, + {LED_DARK_VIOLET}, {LED_ORANGE}, - {LED_BLACK}, - {LED_BLACK}, - {LED_BLACK} + {LED_DEEP_PINK}, + {LED_BLUE}, + {LED_ORANGE} + } +}; + +static const modeColors_t baroModeColors = { + .raw = { + {LED_LIGHT_BLUE}, + {LED_DARK_VIOLET}, + {LED_RED}, + {LED_DEEP_PINK}, + {LED_BLUE}, + {LED_ORANGE} } }; void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColors_t *modeColors) { - if (ledConfig->flags & LED_DIRECTION_NORTH && LED_Y(ledConfig) < highestYValueForNorth) { - setLedColor(ledIndex, &modeColors->colors.north); - return; + // apply up/down colors regardless of quadrant. + if ((ledConfig->flags & LED_DIRECTION_UP)) { + setLedColor(ledIndex, &modeColors->colors.up); } - if (ledConfig->flags & LED_DIRECTION_SOUTH && LED_Y(ledConfig) >= lowestYValueForSouth) { - setLedColor(ledIndex, &modeColors->colors.south); - return; + if ((ledConfig->flags & LED_DIRECTION_DOWN)) { + setLedColor(ledIndex, &modeColors->colors.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) { + setLedColor(ledIndex, &modeColors->colors.west); + } + + if ((ledConfig->flags & LED_DIRECTION_EAST) && GET_LED_X(ledConfig) >= lowestXValueForEast) { + setLedColor(ledIndex, &modeColors->colors.east); + } + + if ((ledConfig->flags & LED_DIRECTION_NORTH) && GET_LED_Y(ledConfig) <= highestYValueForNorth) { + setLedColor(ledIndex, &modeColors->colors.north); + } + + if ((ledConfig->flags & LED_DIRECTION_SOUTH) && GET_LED_Y(ledConfig) >= lowestYValueForSouth) { + setLedColor(ledIndex, &modeColors->colors.south); + } + } typedef enum { @@ -283,25 +439,25 @@ void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, co { switch (quadrant) { case QUADRANT_NORTH_EAST: - if (LED_Y(ledConfig) <= highestYValueForNorth && LED_X(ledConfig) >= lowestXValueForEast) { + if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) { setLedColor(ledIndex, color); } return; case QUADRANT_SOUTH_EAST: - if (LED_Y(ledConfig) >= lowestYValueForSouth && LED_X(ledConfig) >= lowestXValueForEast) { + if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) { setLedColor(ledIndex, color); } return; case QUADRANT_SOUTH_WEST: - if (LED_Y(ledConfig) >= lowestYValueForSouth && LED_X(ledConfig) <= highestXValueForWest) { + if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) { setLedColor(ledIndex, color); } return; case QUADRANT_NORTH_WEST: - if (LED_Y(ledConfig) <= highestYValueForNorth && LED_X(ledConfig) <= highestXValueForWest) { + if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) { setLedColor(ledIndex, color); } return; @@ -313,13 +469,13 @@ void applyLedModeLayer(void) const ledConfig_t *ledConfig; uint8_t ledIndex; - for (ledIndex = 0; ledIndex < WS2811_LED_STRIP_LENGTH; ledIndex++) { + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; setLedColor(ledIndex, &black); - if (!(ledConfig->flags & LED_FUNCTION_MODE)) { + if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) { if (ledConfig->flags & LED_FUNCTION_ARM_STATE) { if (!ARMING_FLAG(ARMED)) { setLedColor(ledIndex, &green); @@ -337,6 +493,10 @@ void applyLedModeLayer(void) #ifdef MAG } else if (FLIGHT_MODE(MAG_MODE)) { applyDirectionalModeColor(ledIndex, ledConfig, &magModeColors); +#endif +#ifdef BARO + } else if (FLIGHT_MODE(BARO_MODE)) { + applyDirectionalModeColor(ledIndex, ledConfig, &baroModeColors); #endif } else if (FLIGHT_MODE(HORIZON_MODE)) { applyDirectionalModeColor(ledIndex, ledConfig, &horizonModeColors); @@ -351,7 +511,7 @@ void applyLedLowBatteryLayer(uint8_t batteryFlashState) const ledConfig_t *ledConfig; uint8_t ledIndex; - for (ledIndex = 0; ledIndex < WS2811_LED_STRIP_LENGTH; ledIndex++) { + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; @@ -381,7 +541,7 @@ void applyLedIndicatorLayer(uint8_t indicatorFlashState) uint8_t ledIndex; - for (ledIndex = 0; ledIndex < WS2811_LED_STRIP_LENGTH; ledIndex++) { + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; @@ -428,6 +588,7 @@ static void updateLedAnimationState(void) frameCounter = (frameCounter + 1) % animationFrames; } +#ifdef USE_LED_ANIMATION static void applyLedAnimationLayer(void) { const ledConfig_t *ledConfig; @@ -437,21 +598,22 @@ static void applyLedAnimationLayer(void) } uint8_t ledIndex; - for (ledIndex = 0; ledIndex < WS2811_LED_STRIP_LENGTH; ledIndex++) { + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; - if (LED_Y(ledConfig) == previousRow) { + if (GET_LED_Y(ledConfig) == previousRow) { setLedColor(ledIndex, &white); setLedBrightness(ledIndex, 50); - } else if (LED_Y(ledConfig) == currentRow) { + } else if (GET_LED_Y(ledConfig) == currentRow) { setLedColor(ledIndex, &white); - } else if (LED_Y(ledConfig) == nextRow) { + } else if (GET_LED_Y(ledConfig) == nextRow) { setLedBrightness(ledIndex, 50); } } } +#endif void updateLedStrip(void) { @@ -519,57 +681,22 @@ void updateLedStrip(void) updateLedAnimationState(); } +#ifdef USE_LED_ANIMATION applyLedAnimationLayer(); - +#endif ws2811UpdateStrip(); } -void determineLedStripDimensions() +void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) { - ledGridWidth = 0; - ledGridHeight = 0; - - uint8_t ledIndex; - const ledConfig_t *ledConfig; - - for (ledIndex = 0; ledIndex < WS2811_LED_STRIP_LENGTH; ledIndex++) { - ledConfig = &ledConfigs[ledIndex]; - - if (LED_X(ledConfig) >= ledGridWidth) { - ledGridWidth = LED_X(ledConfig) + 1; - } - if (LED_Y(ledConfig) >= ledGridHeight) { - ledGridHeight = LED_Y(ledConfig) + 1; - } - } + memset(ledConfigs, 0, MAX_LED_STRIP_LENGTH * sizeof(ledConfig_t)); + memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); + reevalulateLedConfig(); } -void determineOrientationLimits(void) +void ledStripInit(ledConfig_t *ledConfigsToUse) { - highestYValueForNorth = (ledGridHeight / 2) - 1; - if (highestYValueForNorth > 1) { // support small grid (e.g. gridwidth 5) - highestYValueForNorth &= ~(1 << 0); // make even - } - - lowestYValueForSouth = (ledGridHeight / 2) - 1; - if (lowestYValueForSouth & 1) { - lowestYValueForSouth = min(lowestYValueForSouth + 1, ledGridHeight - 1); - } - - highestXValueForWest = (ledGridWidth / 2) - 1; - if (highestXValueForWest > 1) { // support small grid (e.g. gridwidth 5) - highestXValueForWest &= ~(1 << 0); // make even - } - - lowestXValueForEast = (ledGridWidth / 2) - 1; - if (lowestXValueForEast & 1) { - lowestXValueForEast = min(lowestXValueForEast + 1, ledGridWidth - 1); - } -} - -void ledStripInit(void) -{ - determineLedStripDimensions(); - determineOrientationLimits(); + ledConfigs = ledConfigsToUse; + reevalulateLedConfig(); } #endif diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index b107775ab5..9ce3e00823 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -17,4 +17,43 @@ #pragma once +#define MAX_LED_STRIP_LENGTH 32 + +#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)) + +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_BATTERY = (1 << 7), + LED_FUNCTION_FLIGHT_MODE = (1 << 8), + LED_FUNCTION_ARM_STATE = (1 << 9) +} ledFlag_e; + +typedef struct ledConfig_s { + uint8_t xy; // see LED_X/Y_MASK defines + uint16_t flags; // see ledFlag_e +} ledConfig_t; + +extern uint8_t ledCount; + +bool parseLedStripConfig(uint8_t ledIndex, const char *config); void updateLedStrip(void); +void applyDefaultLedStripConfig(ledConfig_t *ledConfig); +void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize); diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 55b999c0e8..a65e720318 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -17,7 +17,7 @@ #pragma once -enum { +typedef enum { BOXARM = 0, BOXANGLE, BOXHORIZON, diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c4a6b6efc9..fa20512917 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -47,6 +47,7 @@ #include "io/gimbal.h" #include "io/rc_controls.h" #include "io/serial.h" +#include "io/ledstrip.h" #include "sensors/battery.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" @@ -77,6 +78,7 @@ static void cliGpsPassthrough(char *cmdline); #endif static void cliHelp(char *cmdline); static void cliMap(char *cmdline); +static void cliLed(char *cmdline); static void cliMixer(char *cmdline); static void cliMotor(char *cmdline); static void cliProfile(char *cmdline); @@ -143,6 +145,7 @@ const clicmd_t cmdTable[] = { { "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough }, #endif { "help", "", cliHelp }, + { "led", "configure leds", cliLed }, { "map", "mapping of rc channel order", cliMap }, { "mixer", "mixer name or list", cliMixer }, { "motor", "get/set motor output value", cliMotor }, @@ -495,6 +498,33 @@ static void cliCMix(char *cmdline) } } +static void cliLed(char *cmdline) +{ + int i; + uint8_t len; + char *ptr; + char ledConfigBuffer[20]; + + len = strlen(cmdline); + if (len == 0) { + for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); + printf("led %u %s\r\n", i, ledConfigBuffer); + } + } else { + ptr = cmdline; + i = atoi(ptr); + if (i < MAX_LED_STRIP_LENGTH) { + ptr = strchr(cmdline, ' '); + if (!parseLedStripConfig(i, ++ptr)) { + printf("Parse error\r\n", MAX_LED_STRIP_LENGTH); + } + } else { + printf("Invalid led index: must be < %u\r\n", MAX_LED_STRIP_LENGTH); + } + } +} + static void dumpValues(uint8_t mask) { uint32_t i; @@ -592,6 +622,9 @@ static void cliDump(char *cmdline) buf[i] = '\0'; printf("map %s\r\n", buf); + printf("\r\n\r\n# led\r\n"); + cliLed(""); + printSectionBreak(); dumpValues(MASTER_VALUE); } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 089ee70f10..c8e11134d8 100755 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -47,6 +47,7 @@ #include "io/gps.h" #include "io/gimbal.h" #include "io/serial.h" +#include "io/ledstrip.h" #include "telemetry/telemetry.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" diff --git a/src/main/main.c b/src/main/main.c index d2318eba8e..b3f87890ea 100755 --- a/src/main/main.c +++ b/src/main/main.c @@ -50,6 +50,7 @@ #include "io/escservo.h" #include "io/rc_controls.h" #include "io/gimbal.h" +#include "io/ledstrip.h" #include "sensors/sensors.h" #include "sensors/sonar.h" #include "sensors/barometer.h" @@ -88,7 +89,7 @@ void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig); void imuInit(void); -void ledStripInit(void); +void ledStripInit(ledConfig_t *ledConfigs); void loop(void); @@ -237,7 +238,7 @@ void init(void) #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { ws2811LedStripInit(); - ledStripInit(); + ledStripInit(masterConfig.ledConfigs); } #endif diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 293274d68c..4090fea3e1 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -19,6 +19,9 @@ #include #include "platform.h" + +#include "build_config.h" + #include "common/axis.h" #include "drivers/accgyro.h" @@ -163,11 +166,16 @@ extern acc_t acc; #ifdef USE_FAKE_GYRO static void fakeGyroInit(void) {} -static void fakeGyroRead(int16_t *gyroData) {} -static void fakeGyroReadTemp(int16_t *tempData) {} +static void fakeGyroRead(int16_t *gyroData) { + UNUSED(gyroData); +} +static void fakeGyroReadTemp(int16_t *tempData) { + UNUSED(tempData); +} bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf) { + UNUSED(lpf); gyro->init = fakeGyroInit; gyro->read = fakeGyroRead; gyro->temperature = fakeGyroReadTemp; @@ -177,7 +185,9 @@ bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf) #ifdef USE_FAKE_ACC static void fakeAccInit(void) {} -static void fakeAccRead(int16_t *accData) {} +static void fakeAccRead(int16_t *accData) { + UNUSED(accData); +} bool fakeAccDetect(acc_t *acc) { diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 7d6d6846a5..9cc2167d5f 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -65,8 +65,6 @@ #define GPS #define LED_STRIP -//#define USE_ALTERNATE_LED_LAYOUT - #define TELEMETRY #define SOFT_SERIAL #define SERIAL_RX diff --git a/src/test/Makefile b/src/test/Makefile index 2b12dd4908..f5845ac357 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -33,7 +33,13 @@ CXXFLAGS += -g -Wall -Wextra -pthread -ggdb -O0 # All tests produced by this Makefile. Remember to add new tests you # created to the list. -TESTS = battery_unittest flight_imu_unittest gps_conversion_unittest telemetry_hott_unittest rc_controls_unittest +TESTS = \ + battery_unittest \ + flight_imu_unittest \ + gps_conversion_unittest \ + telemetry_hott_unittest \ + rc_controls_unittest \ + ledstrip_unittest # All Google Test headers. Usually you shouldn't change this # definition. @@ -153,3 +159,16 @@ rc_controls_unittest :$(OBJECT_DIR)/io/rc_controls.o $(OBJECT_DIR)/rc_controls_u $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/io/ledstrip.o : $(USER_DIR)/io/ledstrip.c $(USER_DIR)/io/ledstrip.h $(GTEST_HEADERS) + @mkdir -p $(dir $@) + $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/ledstrip.c -o $@ + +$(OBJECT_DIR)/ledstrip_unittest.o : $(TEST_DIR)/ledstrip_unittest.cc \ + $(USER_DIR)/io/ledstrip.h $(GTEST_HEADERS) + @mkdir -p $(dir $@) + $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ledstrip_unittest.cc -o $@ + +ledstrip_unittest :$(OBJECT_DIR)/io/ledstrip.o $(OBJECT_DIR)/ledstrip_unittest.o $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + + diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc new file mode 100644 index 0000000000..9d711cfd01 --- /dev/null +++ b/src/test/unit/ledstrip_unittest.cc @@ -0,0 +1,322 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ +#include + +#include + +#include "common/axis.h" +#include "flight/flight.h" + +#include "sensors/battery.h" +#include "config/runtime_config.h" +#include "config/config.h" + +#include "drivers/light_ws2811strip.h" +#include "io/ledstrip.h" + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +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; + +void determineLedStripDimensions(void); +void determineOrientationLimits(void); + +ledConfig_t systemLedConfigs[MAX_LED_STRIP_LENGTH]; + +TEST(LedStripTest, parseLedStripConfig) +{ + /* + * 0..5 - rear right cluster, 0..2 rear 3..5 right + * 6..11 - front right cluster, 6..8 rear, 9..11 front + * 12..15 - front center cluster + * 16..21 - front left cluster, 16..18 front, 19..21 rear + * 22..27 - rear left cluster, 22..24 left, 25..27 rear + */ + + // given + static const ledConfig_t expectedLedStripConfig[WS2811_LED_STRIP_LENGTH] = { + { CALCULATE_LED_XY( 9, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY(10, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY(11, 11), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(11, 11), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(10, 10), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 9, 9), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + + { CALCULATE_LED_XY(10, 5), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(11, 4), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(12, 3), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(12, 2), LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY(11, 1), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY(10, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + + { CALCULATE_LED_XY( 7, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 6, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 5, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 4, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + + { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_NORTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 3), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 4), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 2, 5), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + + { CALCULATE_LED_XY( 2, 9), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 10), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 11), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 11), LED_DIRECTION_SOUTH | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 2, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + + { 0, 0 }, + { 0, 0 }, + { 0, 0 }, + { 0, 0 }, + }; + + // and + const char *ledStripConfigCommands[] = { + // Spider quad + + // right rear cluster + "9,9:S:FB", + "10,10:S:FB", + "11,11:S:IA", + "11,11:E:IA", + "10,10:E:F", + "9,9:E:F", + + // right front cluster + "10,5:S:F", + "11,4:S:F", + "12,3:S:IA", + "12,2:N:IA", + "11,1:N:F", + "10,0:N:F", + + // center front cluster + "7,0:N:FB", + "6,0:N:FB", + "5,0:N:FB", + "4,0:N:FB", + + // left front cluster + "2,0:N:F", + "1,1:N:F", + "0,2:N:IA", + "0,3:W:IA", + "1,4:W:F", + "2,5:W:F", + + // left rear cluster + "2,9:W:F", + "1,10:W:F", + "0,11:W:IA", + "0,11:S:IA", + "1,10:S:FB", + "2,9:S:FB" + }; + // and + memset(&systemLedConfigs, 0, sizeof(systemLedConfigs)); + ledConfigs = systemLedConfigs; + + // and + bool ok = false; + + // when + for (uint8_t index = 0; index < (sizeof(ledStripConfigCommands) / sizeof(ledStripConfigCommands[0])); index++) { + ok |= parseLedStripConfig(index, ledStripConfigCommands[index]); + } + + // then + EXPECT_EQ(true, ok); + EXPECT_EQ(28, ledCount); + + + // and + for (uint8_t index = 0; index < WS2811_LED_STRIP_LENGTH; index++) { + printf("iteration: %d\n", index); + + EXPECT_EQ(expectedLedStripConfig[index].xy, ledConfigs[index].xy); + EXPECT_EQ(expectedLedStripConfig[index].flags, ledConfigs[index].flags); + } + + // then + EXPECT_EQ(13, ledGridWidth); + EXPECT_EQ(12, ledGridHeight); + + // then + EXPECT_EQ(5, highestXValueForWest); + EXPECT_EQ(7, lowestXValueForEast); + EXPECT_EQ(5, highestYValueForNorth); + EXPECT_EQ(6, lowestYValueForSouth); +} + +TEST(LedStripTest, smallestGridWithCenter) +{ + // given + memset(&systemLedConfigs, 0, sizeof(systemLedConfigs)); + ledConfigs = systemLedConfigs; + + // and + static const ledConfig_t testLedConfigs[] = { + { CALCULATE_LED_XY( 2, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 2, 1), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 1, 2), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY } + }; + memcpy(&systemLedConfigs, &testLedConfigs, sizeof(testLedConfigs)); + + // when + determineLedStripDimensions(); + + // then + EXPECT_EQ(3, ledGridWidth); + EXPECT_EQ(3, ledGridHeight); + + // when + determineOrientationLimits(); + + // then + EXPECT_EQ(0, highestXValueForWest); + EXPECT_EQ(2, lowestXValueForEast); + EXPECT_EQ(0, highestYValueForNorth); + EXPECT_EQ(2, lowestYValueForSouth); +} + +TEST(LedStripTest, smallestGrid) +{ + // given + memset(&systemLedConfigs, 0, sizeof(systemLedConfigs)); + ledConfigs = systemLedConfigs; + + // and + static const ledConfig_t testLedConfigs[] = { + { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + }; + memcpy(&systemLedConfigs, &testLedConfigs, sizeof(testLedConfigs)); + + // when + determineLedStripDimensions(); + + // then + EXPECT_EQ(2, ledGridWidth); + EXPECT_EQ(2, ledGridHeight); + + // when + determineOrientationLimits(); + + // then + EXPECT_EQ(0, highestXValueForWest); + EXPECT_EQ(1, lowestXValueForEast); + EXPECT_EQ(0, highestYValueForNorth); + EXPECT_EQ(1, lowestYValueForSouth); +} + +/* + { CALCULATE_LED_XY( 1, 14), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_FLIGHT_MODE }, + + { CALCULATE_LED_XY( 0, 13), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 12), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + + { CALCULATE_LED_XY( 0, 11), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 10), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 9), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 8), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 0, 7), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 0, 6), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 0, 5), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 4), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 0, 3), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE }, + + { CALCULATE_LED_XY( 0, 2), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + + { CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 3, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + + { CALCULATE_LED_XY( 4, 1), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 4, 2), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + + { CALCULATE_LED_XY( 4, 3), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 4), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 5), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 6), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 4, 7), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 4, 8), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_BATTERY }, + { CALCULATE_LED_XY( 4, 9), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 10), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + { CALCULATE_LED_XY( 4, 11), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE }, + + { CALCULATE_LED_XY( 4, 12), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + { CALCULATE_LED_XY( 4, 13), LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + + { CALCULATE_LED_XY( 3, 14), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, + + */ + +uint8_t armingFlags = 0; +uint16_t flightModeFlags = 0; +int16_t rcCommand[4]; + +void ws2811UpdateStrip(void) {} +void setLedColor(uint16_t index, const rgbColor24bpp_t *color) { + UNUSED(index); + UNUSED(color); +} +void setLedBrightness(uint16_t index, const uint8_t scalePercent) { + UNUSED(index); + UNUSED(scalePercent); +} +void setStripColor(const rgbColor24bpp_t *color) { + UNUSED(color); +} +void setStripColors(const rgbColor24bpp_t *colors) { + UNUSED(colors); +} + +bool isWS2811LedStripReady(void) { return false; } + +void delay(uint32_t ms) +{ + UNUSED(ms); + return; +} + +uint32_t micros(void) { return 0; } +bool shouldSoundBatteryAlarm(void) { return false; } +bool feature(uint32_t mask) { + UNUSED(mask); + return false; +} + +void tfp_sprintf(char *, char*, ...) { }; diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 2a8ac9771b..cf2441f30f 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -20,6 +20,7 @@ #define BARO #define GPS #define TELEMETRY +#define LED_STRIP #define SERIAL_PORT_COUNT 4