diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000000..dfc48f92fe --- /dev/null +++ b/.gitattributes @@ -0,0 +1,13 @@ +*.md text +*.c text +*.h text +*.cc text +*.S text +*.s text +*.hex -text +*.elf -text +*.ld text +Makefile text +*.bat eol=crlf +*.txt text + diff --git a/docs/LedStrip.md b/docs/LedStrip.md index 9f6900a435..7bea700ade 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -1,96 +1,293 @@ -# Led Strip - -Cleanflight supports the use of addressable LED strips. Addressable LED strips allow each LED in the strip to -be programmed with a unique and independant color. This is far more advanced than the normal RGB strips which -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: - -* Indicators showing pitch/roll stick positions. -* Heading/Orientation lights. -* Flight mode specific color schemes. -* Low battery warning. - -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 28 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. - -WS2812 LEDs require an 800khz signal and precise timings and thus requires the use of a dedicated hardware timer. - -Note: The initial code may work with WS2801 + External LEDs since the protocol is the same, WS2811/WS2812B should also work but -may require very simple timing adjustments to be made in the source. -Not all WS2812 ICs use the same timings, some batches use different timings. - -It could be possible to be able to specify the timings required via CLI if users request it. - -## Connections - -WS2812 LED strips generally require a single data line, 5V and GND. - -WS2812 LEDs on full brightness can consume quite a bit of current. It is recommended to verify the current draw and ensure your -supply can cope with the load. On a multirotor that uses multiple BEC ESC's you can try use a different BEC to the one the FC -uses. e.g. ESC1/BEC1 -> FC, ESC2/BEC2 -> LED strip. It's also possible to power one half of the strip from one BEC and the other half -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 | - - -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. - -## 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. -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. - -``` - 17-19 10-12 -20-22 \ / 7-9 - \ 13-16 / - \ FRONT / - / BACK \ - / \ -23-25 / \ 4-6 - 26-28 1-3 -``` - -## 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. - -## Troubleshooting - -On initial power up the LEDs on the strip will be set to WHITE. This means you can attach a current meter to verify -the current draw if your measurement equipment is fast enough. This also means that you can make sure that each R,G and B LED -in each LED module on the strip is also functioning. - -After a short delay the LEDs will show the unarmed color sequence and or low-battery warning sequence. - -If the LEDs flash intermittently or do not show the correct colors verify all connections and check the specifications of the -LEDs you have against the supported timings (for now, you'll have to look in the source). - -Also check that the feature `LED_STRIP` was correctly enabled and that it does not conflict with other features, as above. +# Led Strip + +Cleanflight supports the use of addressable LED strips. Addressable LED strips allow each LED in the strip to +be programmed with a unique and independant color. This is far more advanced than the normal RGB strips which +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. +* Low battery warning. + +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. + +## Supported hardware + +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. + +Note: The initial code may work with WS2801 + External LEDs since the protocol is the same, WS2811/WS2812B should also work but +may require very simple timing adjustments to be made in the source. +Not all WS2812 ICs use the same timings, some batches use different timings. + +It could be possible to be able to specify the timings required via CLI if users request it. + +## Connections + +WS2812 LED strips generally require a single data line, 5V and GND. + +WS2812 LEDs on full brightness can consume quite a bit of current. It is recommended to verify the current draw and ensure your +supply can cope with the load. On a multirotor that uses multiple BEC ESC's you can try use a different BEC to the one the FC +uses. e.g. ESC1/BEC1 -> FC, ESC2/BEC2 -> LED strip. It's also possible to power one half of the strip from one BEC and the other half +from another BEC. Just ensure that the GROUND is the same for all BEC outputs and LEDs. + + +| 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: + +* `W` - `W`warnings. +* `F` - `F`light mode & Orientation +* `I` - `I`ndicator. +* `A` - `A`rmed state. + +Example: + +``` +led 0 0,15:SD:IAW +led 1 15,0:ND:IAW +led 2 0,0:ND:IAW +led 3 0,15:SD:IAW +``` + +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 + +#### Warning + +This mode simply uses the leds to flash when warnings occur. + +* Battery warning flashes the LEDs between red and off when the battery is low if battery monitoring is enabled. +* Failsafe warning flashes the LEDs between light blue and lime green when failsafe is active. + +#### 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 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. + +### Example 12 LED config + +The default configuration is as follows +``` +led 0 2,2:ES:IA +led 1 2,1:E:WF +led 2 2,0:NE:IA +led 3 1,0:N:F +led 4 0,0:NW:IA +led 5 0,1:W:WF +led 6 0,2:SW:IA +led 7 1,2:S:WF +led 8 1,1:U:WF +led 9 1,1:U:WF +led 10 1,1:D:WF +led 11 1,1:D:WF +``` + +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:FW +8,7:E:FW +15,0:ND:IA +7,7:N:FW +8,7:N:FW +0,0:ND:IA +7,7:W:FW +7,8:W:FW +0,15:SD:IA +7,8:S:FW +8,8:S:FW +7,7:D:FW +8,7:D:FW +7,7:U:FW +8,7:U:FW +``` + +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:FW +10,10:S:FW +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:FW +6,0:N:FW +5,0:N:FW +4,0:N:FW +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:FW +2,9:S:FW +``` + +``` + 17-19 10-12 +20-22 \ / 7-9 + \ 13-16 / + \ FRONT / + / BACK \ + / \ +23-25 / \ 4-6 + 26-28 1-3 +``` + +All LEDs should face outwards from the chassis in this configuration. + +Note: +This configuration is specifically designed for the Alien Spider AQ50D PRO 250mm frame. + +http://www.goodluckbuy.com/alien-spider-aq50d-pro-250mm-mini-quadcopter-carbon-fiber-micro-multicopter-frame.html + +## Troubleshooting + +On initial power up the LEDs on the strip will be set to WHITE. This means you can attach a current meter to verify +the current draw if your measurement equipment is fast enough. This also means that you can make sure that each R,G and B LED +in each LED module on the strip is also functioning. + +After a short delay the LEDs will show the unarmed color sequence and or low-battery warning sequence. + +If the LEDs flash intermittently or do not show the correct colors verify all connections and check the specifications of the +LEDs you have against the supported timings (for now, you'll have to look in the source). + +Also check that the feature `LED_STRIP` was correctly enabled and that it does not conflict with other features, as above. diff --git a/src/main/common/printf.c b/src/main/common/printf.c index 299e9d5789..41daaca1c2 100644 --- a/src/main/common/printf.c +++ b/src/main/common/printf.c @@ -139,6 +139,7 @@ void tfp_format(void *putp, putcf putf, char *fmt, va_list va) break; case '%': putf(putp, ch); + break; default: break; } diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c index 077453af96..7002375d07 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -122,23 +122,23 @@ char a2i(char ch, char **src, int base, int *nump) ** Code from http://groups.google.com/group/comp.lang.c/msg/66552ef8b04fe1ab?pli=1 */ -static char *_i2a(unsigned i, char *a, unsigned r) +static char *_i2a(unsigned i, char *a, unsigned base) { - if (i / r > 0) - a = _i2a(i / r, a, r); - *a = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"[i % r]; + if (i / base > 0) + a = _i2a(i / base, a, base); + *a = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"[i % base]; return a + 1; } -char *itoa(int i, char *a, int r) +char *itoa(int i, char *a, int base) { - if ((r < 2) || (r > 36)) - r = 10; + if ((base < 2) || (base > 36)) + base = 10; if (i < 0) { *a = '-'; - *_i2a(-(unsigned) i, a + 1, r) = 0; + *_i2a(-(unsigned) i, a + 1, base) = 0; } else - *_i2a(i, a, r) = 0; + *_i2a(i, a, base) = 0; return a; } diff --git a/src/main/config/config.c b/src/main/config/config.c index 61e2228ffb..4aeafee019 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" @@ -70,6 +71,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon #define FLASH_TO_RESERVE_FOR_CONFIG 0x800 +#ifndef FLASH_PAGE_COUNT #ifdef STM32F303xC #define FLASH_PAGE_COUNT 128 #define FLASH_PAGE_SIZE ((uint16_t)0x800) @@ -84,18 +86,19 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon #define FLASH_PAGE_COUNT 128 #define FLASH_PAGE_SIZE ((uint16_t)0x800) #endif +#endif -#ifndef FLASH_PAGE_COUNT +#if !defined(FLASH_PAGE_COUNT) || !defined(FLASH_PAGE_SIZE) #error "Flash page count not defined for target." #endif // use the last flash pages for storage -static uint32_t flashWriteAddress = (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)); +#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) 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) { @@ -372,6 +375,10 @@ static void resetConf(void) for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) masterConfig.customMixer[i].throttle = 0.0f; +#ifdef LED_STRIP + applyDefaultLedStripConfig(masterConfig.ledConfigs); +#endif + // copy first profile into remaining profile for (i = 1; i < 3; i++) memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); @@ -389,7 +396,7 @@ static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) static bool isEEPROMContentValid(void) { - const master_t *temp = (const master_t *) flashWriteAddress; + const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS; uint8_t checksum = 0; // check version number @@ -474,7 +481,7 @@ void validateAndFixConfig(void) featureClear(FEATURE_RX_PPM); } - if (feature(FEATURE_CURRENT_METER)) { + if (feature(FEATURE_RX_PARALLEL_PWM)) { #if defined(STM32F10X) // rssi adc needs the same ports featureClear(FEATURE_RSSI_ADC); @@ -523,15 +530,6 @@ void validateAndFixConfig(void) void initEEPROM(void) { -#if defined(STM32F10X) - -#define FLASH_SIZE_REGISTER 0x1FFFF7E0 - - const uint32_t flashSize = *((uint32_t *)FLASH_SIZE_REGISTER) & 0xFFFF; - - // calculate write address based on contents of Flash size register. Use last 2 kbytes for storage - flashWriteAddress = 0x08000000 + (FLASH_PAGE_SIZE * (flashSize - 2)); -#endif } void readEEPROM(void) @@ -541,7 +539,7 @@ void readEEPROM(void) failureMode(10); // Read flash - memcpy(&masterConfig, (char *) flashWriteAddress, sizeof(master_t)); + memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t)); // Copy current profile if (masterConfig.current_profile_index > 2) // sanity check masterConfig.current_profile_index = 0; @@ -587,13 +585,13 @@ void writeEEPROM(void) #endif for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { if (wordOffset % FLASH_PAGE_SIZE == 0) { - status = FLASH_ErasePage(flashWriteAddress + wordOffset); + status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset); if (status != FLASH_COMPLETE) { break; } } - status = FLASH_ProgramWord(flashWriteAddress + wordOffset, + status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset)); if (status != FLASH_COMPLETE) { break; @@ -619,22 +617,6 @@ void ensureEEPROMContainsValidData(void) resetEEPROM(); } -/* - * 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 . - */ void resetEEPROM(void) { 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/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index b64fe38459..354dd1ca42 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -302,6 +302,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf) break; case 5: mpuLowPassFilter = BITS_DLPF_CFG_5HZ; + break; case 0: mpuLowPassFilter = BITS_DLPF_CFG_2100HZ_NOLPF; break; diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 916e925641..ef776dec35 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -180,7 +180,7 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data) #ifdef STM32F10X return ((uint8_t)SPI_I2S_ReceiveData(instance)); #endif - } +} bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, uint8_t *in, int len) { 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..c3b02b962c 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -18,6 +18,7 @@ #include #include +#include #include "platform.h" @@ -44,6 +45,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..b3a05cfcd5 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" @@ -33,145 +42,107 @@ #include "config/config.h" #include "rx/rx.h" #include "io/rc_controls.h" +#include "flight/failsafe.h" #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} +static failsafe_t* failsafe; + +#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 }; +const rgbColor24bpp_t lightBlue = { LED_LIGHT_BLUE }; +const rgbColor24bpp_t limeGreen = { LED_LIME_GREEN }; + + +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_WARNING }, + { 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 }, + { 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_WARNING }, + { 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_WARNING }, + { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 1, 1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, +}; + /* - * 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', 'W', 'F', 'A' }; +#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 }; -#endif // grid offsets uint8_t highestYValueForNorth; @@ -179,10 +150,172 @@ 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; -uint32_t nextBatteryFlashAt = 0; +uint32_t nextWarningFlashAt = 0; #define LED_STRIP_20HZ ((1000 * 1000) / 20) #define LED_STRIP_10HZ ((1000 * 1000) / 10) @@ -207,69 +340,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 +444,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 +474,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 +498,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); @@ -346,23 +511,45 @@ void applyLedModeLayer(void) } } -void applyLedLowBatteryLayer(uint8_t batteryFlashState) +typedef enum { + WARNING_FLAG_NONE = 0, + WARNING_FLAG_LOW_BATTERY = (1 << 0), + WARNING_FLAG_FAILSAFE = (1 << 1) +} warningFlags_e; + +void applyLedWarningLayer(uint8_t warningState, uint8_t warningFlags) { const ledConfig_t *ledConfig; + static uint8_t warningFlashCounter = 0; + + if (warningState) { + warningFlashCounter++; + warningFlashCounter = warningFlashCounter % 4; + } uint8_t ledIndex; - for (ledIndex = 0; ledIndex < WS2811_LED_STRIP_LENGTH; ledIndex++) { + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; - if (!(ledConfig->flags & LED_FUNCTION_BATTERY)) { + if (!(ledConfig->flags & LED_FUNCTION_WARNING)) { continue; } - if (batteryFlashState == 0) { - setLedColor(ledIndex, &red); + if (warningState == 0) { + if (warningFlashCounter == 0 && warningFlags & WARNING_FLAG_LOW_BATTERY) { + setLedColor(ledIndex, &red); + } + if (warningFlashCounter > 1 && warningFlags & WARNING_FLAG_FAILSAFE) { + setLedColor(ledIndex, &lightBlue); + } } else { - setLedColor(ledIndex, &black); + if (warningFlashCounter == 0 && warningFlags & WARNING_FLAG_LOW_BATTERY) { + setLedColor(ledIndex, &black); + } + if (warningFlashCounter > 1 && warningFlags & WARNING_FLAG_FAILSAFE) { + setLedColor(ledIndex, &limeGreen); + } } } } @@ -381,7 +568,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 +615,7 @@ static void updateLedAnimationState(void) frameCounter = (frameCounter + 1) % animationFrames; } +#ifdef USE_LED_ANIMATION static void applyLedAnimationLayer(void) { const ledConfig_t *ledConfig; @@ -437,21 +625,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) { @@ -463,15 +652,15 @@ void updateLedStrip(void) bool animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L; bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; - bool batteryFlashNow = (int32_t)(now - nextBatteryFlashAt) >= 0L; + bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; - if (!(batteryFlashNow || indicatorFlashNow || animationUpdateNow)) { + if (!(warningFlashNow || indicatorFlashNow || animationUpdateNow)) { return; } static uint8_t indicatorFlashState = 0; - static uint8_t batteryFlashState = 0; - static bool batteryWarningEnabled = false; + static uint8_t warningState = 0; + static uint8_t warningFlags; // LAYER 1 @@ -479,21 +668,27 @@ void updateLedStrip(void) // LAYER 2 - if (batteryFlashNow) { - nextBatteryFlashAt = now + LED_STRIP_10HZ; + if (warningFlashNow) { + nextWarningFlashAt = now + LED_STRIP_10HZ; - if (batteryFlashState == 0) { - batteryFlashState = 1; + if (warningState == 0) { + warningState = 1; + + warningFlags = WARNING_FLAG_NONE; + if (feature(FEATURE_VBAT) && shouldSoundBatteryAlarm()) { + warningFlags |= WARNING_FLAG_LOW_BATTERY; + } + if (failsafe->vTable->hasTimerElapsed()) { + warningFlags |= WARNING_FLAG_FAILSAFE; + } - batteryWarningEnabled = feature(FEATURE_VBAT) && shouldSoundBatteryAlarm(); } else { - batteryFlashState = 0; - + warningState = 0; } } - if (batteryWarningEnabled) { - applyLedLowBatteryLayer(batteryFlashState); + if (warningFlags) { + applyLedWarningLayer(warningState, warningFlags); } // LAYER 3 @@ -519,57 +714,23 @@ 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, failsafe_t* failsafeToUse) { - 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; + failsafe = failsafeToUse; + reevalulateLedConfig(); } #endif diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index b107775ab5..27828c40ba 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_WARNING = (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.c b/src/main/io/serial.c index 5943d72114..e9f012f280 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -251,6 +251,13 @@ serialPortSearchResult_t *findNextSerialPort(serialPortFunction_e function, cons )) { continue; } + +#if (defined(NAZE) || defined(OLIMEXINO)) && defined(SONAR) + if (!feature(FEATURE_RX_PARALLEL_PWM) && (serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2)) { + continue; + } +#endif + #endif if (functionConstraint->requiredSerialPortFeatures != SPF_NONE) { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c4a6b6efc9..a3a496a61d 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,37 @@ static void cliCMix(char *cmdline) } } +static void cliLed(char *cmdline) +{ +#ifndef LED_STRIP + UNUSED(cmdline); +#else + 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); + } + } +#endif +} + static void dumpValues(uint8_t mask) { uint32_t i; @@ -592,6 +626,10 @@ static void cliDump(char *cmdline) buf[i] = '\0'; printf("map %s\r\n", buf); +#ifdef LED_STRIP + printf("\r\n\r\n# led\r\n"); + cliLed(""); +#endif 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 6567db82e1..8965325aee 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" @@ -66,8 +67,6 @@ #include "build_config.h" -extern rcReadRawDataPtr rcReadRawFunc; - extern uint32_t previousTime; #ifdef SOFTSERIAL_LOOPBACK @@ -90,8 +89,8 @@ 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 displayInit(void); +void ledStripInit(ledConfig_t *ledConfigsToUse, failsafe_t* failsafeToUse); void loop(void); // FIXME bad naming - this appears to be for some new board that hasn't been made available yet. @@ -243,7 +242,7 @@ void init(void) #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { ws2811LedStripInit(); - ledStripInit(); + ledStripInit(masterConfig.ledConfigs, failsafe); } #endif diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 847676e38a..ba5366e887 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -159,8 +159,6 @@ #undef USE_ACC_MMA8452 #endif -extern uint16_t batteryWarningVoltage; -extern uint8_t batteryCellCount; extern float magneticDeclination; extern gyro_t gyro; diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index b388f3c848..e11fdba345 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -17,6 +17,8 @@ #pragma once +#define FLASH_PAGE_COUNT 64 +#define FLASH_PAGE_SIZE ((uint16_t)0x400) #define LED0_GPIO GPIOC #define LED0_PIN Pin_13 // PC13 (LED) diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 85d2a8ca17..4e78b53d9f 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -66,8 +66,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..f34c355a19 --- /dev/null +++ b/src/test/unit/ledstrip_unittest.cc @@ -0,0 +1,323 @@ +/* + * 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 + +#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_WARNING }, + { CALCULATE_LED_XY(10, 10), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { 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_WARNING }, + { CALCULATE_LED_XY( 6, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 5, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 4, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + + { 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_WARNING }, + { CALCULATE_LED_XY( 2, 9), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + + { 0, 0 }, + { 0, 0 }, + { 0, 0 }, + { 0, 0 }, + }; + + // and + const char *ledStripConfigCommands[] = { + // Spider quad + + // right rear cluster + "9,9:S:FW", + "10,10:S:FW", + "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:FW", + "6,0:N:FW", + "5,0:N:FW", + "4,0:N:FW", + + // 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:FW", + "2,9:S:FW" + }; + // 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_WARNING }, + { 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_WARNING }, + { 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_WARNING }, + { 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_WARNING } + }; + 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_WARNING }, + { CALCULATE_LED_XY( 0, 7), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 0, 6), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { 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_WARNING }, + { CALCULATE_LED_XY( 4, 7), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { CALCULATE_LED_XY( 4, 8), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, + { 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