From 1730e3dfd3b489b84b0256b3492ff8ba68c41eed Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 9 Jun 2014 18:24:06 +0100 Subject: [PATCH] Relocated led strip user code into separate file. Added documentation. Added LED_STRIP feature, can only be enabled under certain circumstances depending on target due to pin/timer mappings - see documentation. --- Makefile | 7 +- docs/LedStrip.md | 97 ++++++++++++++ src/main/config/config.c | 71 +++++----- src/main/config/config.h | 3 +- src/main/drivers/pwm_mapping.c | 22 ++++ src/main/drivers/pwm_mapping.h | 1 + src/main/io/ledstrip.c | 230 +++++++++++++++++++++++++++++++++ src/main/io/ledstrip.h | 20 +++ src/main/io/serial_cli.c | 2 +- src/main/main.c | 5 +- src/main/mw.c | 203 +---------------------------- 11 files changed, 418 insertions(+), 243 deletions(-) create mode 100644 docs/LedStrip.md create mode 100644 src/main/io/ledstrip.c create mode 100644 src/main/io/ledstrip.h diff --git a/Makefile b/Makefile index c6dff6623..72d296851 100644 --- a/Makefile +++ b/Makefile @@ -139,6 +139,7 @@ COMMON_SRC = build_config.c \ io/buzzer.c \ io/gps.c \ io/gps_conversion.c \ + io/ledstrip.c \ io/rc_controls.c \ io/rc_curves.c \ io/serial.c \ @@ -227,6 +228,8 @@ STM32F30x_COMMON_SRC = startup_stm32f30x_md_gcc.S \ drivers/bus_spi.c \ drivers/gpio_stm32f30x.c \ drivers/light_led_stm32f30x.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ drivers/pwm_mapping.c \ drivers/pwm_output.c \ drivers/pwm_rssi.c \ @@ -252,9 +255,7 @@ NAZE32PRO_SRC = $(STM32F30x_COMMON_SRC) \ STM32F3DISCOVERY_COMMON_SRC = $(STM32F30x_COMMON_SRC) \ drivers/accgyro_l3gd20.c \ drivers/accgyro_l3gd20.c \ - drivers/accgyro_lsm303dlhc.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c + drivers/accgyro_lsm303dlhc.c STM32F3DISCOVERY_SRC = $(STM32F3DISCOVERY_COMMON_SRC) \ drivers/accgyro_adxl345.c \ diff --git a/docs/LedStrip.md b/docs/LedStrip.md new file mode 100644 index 000000000..d4c59cbca --- /dev/null +++ b/docs/LedStrip.md @@ -0,0 +1,97 @@ +# 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. + +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 10 LEDs is possible, it just requires additional development. 10 was chosen to +start with due to current draw and RAM usage. + +## Supported hardware + +Only strips of 10 WS2812 LEDs are supported currently. If the strip is longer than 10 leds it does not matter, +but only the first 10 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. + + +| 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 of 10 LED's in half, the first half goes at the front of the quad, from front left to from right. Position +the center LED in the middle. 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. + +The second half of the strip goes at the back of the aircraft, from back right to back left, again place the center LED in the middle. + +Orientation is when viewed with the front of the aircraft facing away from you and viewed from above. + +Example LED numbers and positions for a quad. + +``` + 1 2 4 5 + \ / + \ 3 / + \ FRONT / + / BACK \ + / 8 \ + / \ +10 9 7 6 +``` + +## 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 first 10 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/config/config.c b/src/main/config/config.c index c9adacf57..710a38ade 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -407,57 +407,50 @@ void validateAndFixConfig(void) } if (feature(FEATURE_RX_PPM)) { - if (feature(FEATURE_RX_PARALLEL_PWM)) { - featureClear(FEATURE_RX_PARALLEL_PWM); - } - } - - if (feature(FEATURE_RX_PARALLEL_PWM)) { -#if defined(NAZE) || defined(OLIMEXINO) - if (feature(FEATURE_RSSI_ADC)) { - featureClear(FEATURE_RSSI_ADC); - } - if (feature(FEATURE_CURRENT_METER)) { - featureClear(FEATURE_CURRENT_METER); - } -#endif + featureClear(FEATURE_RX_PARALLEL_PWM); } if (feature(FEATURE_RX_MSP)) { - if (feature(FEATURE_RX_SERIAL)) { - featureClear(FEATURE_RX_SERIAL); - } - if (feature(FEATURE_RX_PARALLEL_PWM)) { - featureClear(FEATURE_RX_PARALLEL_PWM); - } - if (feature(FEATURE_RX_PPM)) { - featureClear(FEATURE_RX_PPM); - } + featureClear(FEATURE_RX_SERIAL); + featureClear(FEATURE_RX_PARALLEL_PWM); + featureClear(FEATURE_RX_PPM); } if (feature(FEATURE_RX_SERIAL)) { - if (feature(FEATURE_RX_PARALLEL_PWM)) { - featureClear(FEATURE_RX_PARALLEL_PWM); - } - if (feature(FEATURE_RX_PPM)) { - featureClear(FEATURE_RX_PPM); - } + featureClear(FEATURE_RX_PARALLEL_PWM); + featureClear(FEATURE_RX_PPM); } + if (feature(FEATURE_RX_PARALLEL_PWM)) { +#if defined(STM32F103_MD) + // rssi adc needs the same ports + featureClear(FEATURE_RSSI_ADC); + // current meter needs the same ports + featureClear(FEATURE_CURRENT_METER); #ifdef SONAR - if (feature(FEATURE_SONAR)) { // sonar needs a free PWM port - if (!feature(FEATURE_RX_PARALLEL_PWM)) { - featureClear(FEATURE_SONAR); - } + featureClear(FEATURE_SONAR); +#endif +#endif + +#if defined(STM32F103_MD) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY) + // led strip needs the same ports + featureClear(FEATURE_LED_STRIP); +#endif + + + // software serial needs free PWM ports + featureClear(FEATURE_SOFTSERIAL); + } + + +#if defined(STM32F103_MD) + // led strip needs the same timer as softserial + if (feature(FEATURE_SOFTSERIAL)) { + featureClear(FEATURE_LED_STRIP); } #endif - if (feature(FEATURE_SOFTSERIAL)) { - // software serial needs free PWM ports - if (feature(FEATURE_RX_PARALLEL_PWM)) { - featureClear(FEATURE_SOFTSERIAL); - } - } + useRxConfig(&masterConfig.rxConfig); diff --git a/src/main/config/config.h b/src/main/config/config.h index e56a65714..99670e00b 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -33,7 +33,8 @@ typedef enum { FEATURE_3D = 1 << 12, FEATURE_RX_PARALLEL_PWM = 1 << 13, FEATURE_RX_MSP = 1 << 14, - FEATURE_RSSI_ADC = 1 << 15 + FEATURE_RSSI_ADC = 1 << 15, + FEATURE_LED_STRIP = 1 << 16 } AvailableFeatures; bool feature(uint32_t mask); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 6333657c3..d1e11a8a5 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -198,6 +198,28 @@ void pwmInit(drv_pwm_config_t *init) continue; #endif +#if defined(STM32F10X_MD) +#define LED_STRIP_PWM PWM4 +#endif + +#if defined(STM32F303xC) && !(defined(CHEBUZZF3) || defined(NAZE32PRO)) +#define LED_STRIP_PWM PWM2 +#endif + +#if defined(CHEBUZZF3) +#define LED_STRIP_PWM PWM2 +#endif + +#if defined(NAZE32PRO) +#define LED_STRIP_PWM PWM13 +#endif + +#ifdef LED_STRIP_PWM + // skip LED Strip output + if (init->useLEDStrip && timerIndex == LED_STRIP_PWM) + continue; +#endif + #ifdef STM32F10X_MD // skip ADC for RSSI if (init->useRSSIADC && timerIndex == PWM2) diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 2d33d7133..100901f42 100755 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -43,6 +43,7 @@ typedef struct drv_pwm_config_t { bool useUART2; #endif bool useSoftSerial; + bool useLEDStrip; bool useServos; bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors bool airplane; // fixed wing hardware config, lots of servos etc diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c new file mode 100644 index 000000000..a5e8f347d --- /dev/null +++ b/src/main/io/ledstrip.c @@ -0,0 +1,230 @@ +/* + * 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 + +#include "drivers/light_ws2811strip.h" +#include "drivers/system.h" + +#include "sensors/battery.h" + +#include "config/runtime_config.h" +#include "config/config.h" +#include "rx/rx.h" +#include "io/rc_controls.h" + +#include "io/ledstrip.h" + +#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 const rgbColor24bpp_t stripOrientation[] = +{ + {LED_GREEN}, + {LED_GREEN}, + {LED_GREEN}, + {LED_GREEN}, + {LED_GREEN}, + {LED_RED}, + {LED_RED}, + {LED_RED}, + {LED_RED}, + {LED_RED} +}; + +static const rgbColor24bpp_t stripHorizon[] = +{ + {LED_BLUE}, + {LED_BLUE}, + {LED_BLUE}, + {LED_BLUE}, + {LED_BLUE}, + {LED_YELLOW}, + {LED_YELLOW}, + {LED_YELLOW}, + {LED_YELLOW}, + {LED_YELLOW} +}; + +static const rgbColor24bpp_t stripAngle[] = +{ + {LED_CYAN}, + {LED_CYAN}, + {LED_CYAN}, + {LED_CYAN}, + {LED_CYAN}, + {LED_YELLOW}, + {LED_YELLOW}, + {LED_YELLOW}, + {LED_YELLOW}, + {LED_YELLOW} +}; + +static const rgbColor24bpp_t stripMag[] = +{ + {LED_PURPLE}, + {LED_PURPLE}, + {LED_PURPLE}, + {LED_PURPLE}, + {LED_PURPLE}, + {LED_ORANGE}, + {LED_ORANGE}, + {LED_ORANGE}, + {LED_ORANGE}, + {LED_ORANGE} +}; + +static const rgbColor24bpp_t stripHeadfree[] = +{ + {LED_PINK}, + {LED_PINK}, + {LED_PINK}, + {LED_PINK}, + {LED_PINK}, + {LED_ORANGE}, + {LED_ORANGE}, + {LED_ORANGE}, + {LED_ORANGE}, + {LED_ORANGE} +}; + +static const rgbColor24bpp_t stripReds[] = +{ + {{ 32, 0, 0}}, + {{ 96, 0, 0}}, + {{160, 0, 0}}, + {{224, 0, 0}}, + {{255, 0, 0}}, + {{255, 0, 0}}, + {{224, 0, 0}}, + {{160, 0, 0}}, + {{ 96, 0, 0}}, + {{ 32, 0, 0}}, +}; + +uint32_t nextIndicatorFlashAt = 0; +uint32_t nextBatteryFlashAt = 0; + +#define LED_STRIP_10HZ ((1000 * 1000) / 10) +#define LED_STRIP_5HZ ((1000 * 1000) / 5) + +void updateLedStrip(void) +{ + if (!isWS2811LedStripReady()) { + return; + } + + uint32_t now = micros(); + + bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; + bool batteryFlashNow = (int32_t)(now - nextBatteryFlashAt) >= 0L; + + if (!(batteryFlashNow || indicatorFlashNow)) { + return; + } + + static uint8_t indicatorFlashState = 0; + static uint8_t batteryFlashState = 0; + + static const rgbColor24bpp_t *flashColor; + + // LAYER 1 + + if (f.ARMED) { + setStripColors(stripOrientation); + } else { + setStripColors(stripReds); + } + + if (f.HEADFREE_MODE) { + setStripColors(stripHeadfree); +#ifdef MAG + } else if (f.MAG_MODE) { + setStripColors(stripMag); +#endif + } else if (f.HORIZON_MODE) { + setStripColors(stripHorizon); + } else if (f.ANGLE_MODE) { + setStripColors(stripAngle); + } + + // LAYER 2 + + if (batteryFlashNow) { + nextBatteryFlashAt = now + LED_STRIP_10HZ; + + if (batteryFlashState == 0) { + batteryFlashState = 1; + } else { + batteryFlashState = 0; + } + } + + if (batteryFlashState == 1 && feature(FEATURE_VBAT) && shouldSoundBatteryAlarm()) { + setStripColor(&black); + } + + // LAYER 3 + + if (indicatorFlashNow) { + + uint8_t rollScale = abs(rcCommand[ROLL]) / 50; + uint8_t pitchScale = abs(rcCommand[PITCH]) / 50; + uint8_t scale = max(rollScale, pitchScale); + nextIndicatorFlashAt = now + (LED_STRIP_5HZ / max(1, scale)); + + if (indicatorFlashState == 0) { + indicatorFlashState = 1; + } else { + indicatorFlashState = 0; + } + } + + if (indicatorFlashState == 0) { + flashColor = &orange; + } else { + flashColor = &black; + } + if (rcCommand[ROLL] < -50) { + setLedColor(0, flashColor); + setLedColor(9, flashColor); + } + if (rcCommand[ROLL] > 50) { + setLedColor(4, flashColor); + setLedColor(5, flashColor); + } + if (rcCommand[PITCH] > 50) { + setLedColor(0, flashColor); + setLedColor(4, flashColor); + } + if (rcCommand[PITCH] < -50) { + setLedColor(5, flashColor); + setLedColor(9, flashColor); + } + + ws2811UpdateStrip(); +} diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h new file mode 100644 index 000000000..b107775ab --- /dev/null +++ b/src/main/io/ledstrip.h @@ -0,0 +1,20 @@ +/* + * 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 . + */ + +#pragma once + +void updateLedStrip(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 55c1836a5..67543dc5f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -103,7 +103,7 @@ static const char * const featureNames[] = { "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", - "RX_MSP", "RSSI_ADC", NULL + "RX_MSP", "RSSI_ADC", "LED_STRIP", NULL }; // sync this with AvailableSensors enum from board.h diff --git a/src/main/main.c b/src/main/main.c index 3823e5f4d..ee7c4f84d 100755 --- a/src/main/main.c +++ b/src/main/main.c @@ -182,6 +182,7 @@ void init(void) pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); + pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useServos = isMixerUsingServos(); pwm_params.extraServos = currentProfile.gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; @@ -215,7 +216,9 @@ void init(void) } #endif - ws2811LedStripInit(); + if (feature(FEATURE_LED_STRIP)) { + ws2811LedStripInit(); + } if (feature(FEATURE_TELEMETRY)) initTelemetry(); diff --git a/src/main/mw.c b/src/main/mw.c index a34b09fd6..103be7bbd 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -28,7 +28,6 @@ #include "drivers/accgyro.h" #include "drivers/light_ledring.h" #include "drivers/light_led.h" -#include "drivers/light_ws2811strip.h" #include "drivers/gpio.h" #include "drivers/system.h" @@ -51,6 +50,7 @@ #include "flight/mixer.h" #include "io/gimbal.h" #include "io/gps.h" +#include "io/ledstrip.h" #include "io/serial_cli.h" #include "io/serial.h" #include "io/statusindicator.h" @@ -348,202 +348,6 @@ void updateInflightCalibrationState(void) } } -#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 const rgbColor24bpp_t stripOrientation[] = -{ - {LED_GREEN}, - {LED_GREEN}, - {LED_GREEN}, - {LED_GREEN}, - {LED_GREEN}, - {LED_RED}, - {LED_RED}, - {LED_RED}, - {LED_RED}, - {LED_RED} -}; - -static const rgbColor24bpp_t stripHorizon[] = -{ - {LED_BLUE}, - {LED_BLUE}, - {LED_BLUE}, - {LED_BLUE}, - {LED_BLUE}, - {LED_YELLOW}, - {LED_YELLOW}, - {LED_YELLOW}, - {LED_YELLOW}, - {LED_YELLOW} -}; - -static const rgbColor24bpp_t stripAngle[] = -{ - {LED_CYAN}, - {LED_CYAN}, - {LED_CYAN}, - {LED_CYAN}, - {LED_CYAN}, - {LED_YELLOW}, - {LED_YELLOW}, - {LED_YELLOW}, - {LED_YELLOW}, - {LED_YELLOW} -}; - -static const rgbColor24bpp_t stripMag[] = -{ - {LED_PURPLE}, - {LED_PURPLE}, - {LED_PURPLE}, - {LED_PURPLE}, - {LED_PURPLE}, - {LED_ORANGE}, - {LED_ORANGE}, - {LED_ORANGE}, - {LED_ORANGE}, - {LED_ORANGE} -}; - -static const rgbColor24bpp_t stripHeadfree[] = -{ - {LED_PINK}, - {LED_PINK}, - {LED_PINK}, - {LED_PINK}, - {LED_PINK}, - {LED_ORANGE}, - {LED_ORANGE}, - {LED_ORANGE}, - {LED_ORANGE}, - {LED_ORANGE} -}; - -static const rgbColor24bpp_t stripReds[] = -{ - {{ 32, 0, 0}}, - {{ 96, 0, 0}}, - {{160, 0, 0}}, - {{224, 0, 0}}, - {{255, 0, 0}}, - {{255, 0, 0}}, - {{224, 0, 0}}, - {{160, 0, 0}}, - {{ 96, 0, 0}}, - {{ 32, 0, 0}}, -}; - -uint32_t nextIndicatorFlashAt = 0; -uint32_t nextBatteryFlashAt = 0; - -#define LED_STRIP_10HZ ((1000 * 1000) / 10) -#define LED_STRIP_5HZ ((1000 * 1000) / 5) - -void updateLedStrip(void) -{ - if (!isWS2811LedStripReady()) { - return; - } - - uint32_t now = micros(); - - bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; - bool batteryFlashNow = (int32_t)(now - nextBatteryFlashAt) >= 0L; - - if (!(batteryFlashNow || indicatorFlashNow)) { - return; - } - - static uint8_t indicatorFlashState = 0; - static uint8_t batteryFlashState = 0; - - static const rgbColor24bpp_t *flashColor; - - // LAYER 1 - - if (f.ARMED) { - setStripColors(stripOrientation); - } else { - setStripColors(stripReds); - } - - if (f.HEADFREE_MODE) { - setStripColors(stripHeadfree); -#ifdef MAG - } else if (f.MAG_MODE) { - setStripColors(stripMag); -#endif - } else if (f.HORIZON_MODE) { - setStripColors(stripHorizon); - } else if (f.ANGLE_MODE) { - setStripColors(stripAngle); - } - - // LAYER 2 - - if (batteryFlashNow) { - nextBatteryFlashAt = now + LED_STRIP_10HZ; - - if (batteryFlashState == 0) { - batteryFlashState = 1; - } else { - batteryFlashState = 0; - } - } - - if (batteryFlashState == 1 && feature(FEATURE_VBAT) && shouldSoundBatteryAlarm()) { - setStripColor(&black); - } - - // LAYER 3 - - if (indicatorFlashNow) { - - uint8_t rollScale = abs(rcCommand[ROLL]) / 50; - uint8_t pitchScale = abs(rcCommand[PITCH]) / 50; - uint8_t scale = max(rollScale, pitchScale); - nextIndicatorFlashAt = now + (LED_STRIP_5HZ / max(1, scale)); - - if (indicatorFlashState == 0) { - indicatorFlashState = 1; - } else { - indicatorFlashState = 0; - } - } - - if (indicatorFlashState == 0) { - flashColor = &orange; - } else { - flashColor = &black; - } - if (rcCommand[ROLL] < -50) { - setLedColor(0, flashColor); - setLedColor(9, flashColor); - } - if (rcCommand[ROLL] > 50) { - setLedColor(4, flashColor); - setLedColor(5, flashColor); - } - if (rcCommand[PITCH] > 50) { - setLedColor(0, flashColor); - setLedColor(4, flashColor); - } - if (rcCommand[PITCH] < -50) { - setLedColor(5, flashColor); - setLedColor(9, flashColor); - } - - ws2811UpdateStrip(); -} - void loop(void) @@ -828,5 +632,8 @@ void loop(void) if (!cliMode && feature(FEATURE_TELEMETRY)) { handleTelemetry(); } - updateLedStrip(); + + if (feature(FEATURE_LED_STRIP)) { + updateLedStrip(); + } }