2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
32 #include "build/build_config.h"
34 #include "common/axis.h"
35 #include "common/color.h"
36 #include "common/maths.h"
37 #include "common/printf.h"
38 #include "common/typeconversion.h"
39 #include "common/utils.h"
41 #include "config/feature.h"
43 #include "pg/pg_ids.h"
46 #include "drivers/light_ws2811strip.h"
47 #include "drivers/serial.h"
48 #include "drivers/time.h"
49 #include "drivers/vtx_common.h"
51 #include "config/config.h"
53 #include "fc/rc_controls.h"
54 #include "fc/rc_modes.h"
55 #include "fc/runtime_config.h"
57 #include "flight/failsafe.h"
58 #include "flight/imu.h"
59 #include "flight/mixer.h"
60 #include "flight/pid.h"
61 #include "flight/servos.h"
63 #include "io/beeper.h"
64 #include "io/gimbal.h"
66 #include "io/ledstrip.h"
67 #include "io/serial.h"
71 #include "sensors/acceleration.h"
72 #include "sensors/barometer.h"
73 #include "sensors/battery.h"
74 #include "sensors/boardalignment.h"
75 #include "sensors/gyro.h"
76 #include "sensors/sensors.h"
78 #include "telemetry/telemetry.h"
80 #define COLOR_UNDEFINED 255
82 static bool ledStripEnabled
= false;
83 static uint8_t previousProfileColorIndex
= COLOR_UNDEFINED
;
85 #define HZ_TO_US(hz) ((int32_t)((1000 * 1000) / (hz)))
87 #define MAX_TIMER_DELAY (5 * 1000 * 1000)
89 #define PROFILE_COLOR_UPDATE_INTERVAL_US 1e6 // normally updates when color changes but this is a 1 second forced update
91 #define VISUAL_BEEPER_COLOR COLOR_WHITE
93 #define BEACON_FAILSAFE_PERIOD_US 250 // 2Hz
94 #define BEACON_FAILSAFE_ON_PERCENT 50 // 50% duty cycle
96 const hsvColor_t hsv
[] = {
98 [COLOR_BLACK
] = { 0, 0, 0},
99 [COLOR_WHITE
] = { 0, 255, 255},
100 [COLOR_RED
] = { 0, 0, 255},
101 [COLOR_ORANGE
] = { 30, 0, 255},
102 [COLOR_YELLOW
] = { 60, 0, 255},
103 [COLOR_LIME_GREEN
] = { 90, 0, 255},
104 [COLOR_GREEN
] = {120, 0, 255},
105 [COLOR_MINT_GREEN
] = {150, 0, 255},
106 [COLOR_CYAN
] = {180, 0, 255},
107 [COLOR_LIGHT_BLUE
] = {210, 0, 255},
108 [COLOR_BLUE
] = {240, 0, 255},
109 [COLOR_DARK_VIOLET
] = {270, 0, 255},
110 [COLOR_MAGENTA
] = {300, 0, 255},
111 [COLOR_DEEP_PINK
] = {330, 0, 255},
113 // macro to save typing on default colors
114 #define HSV(color) (hsv[COLOR_ ## color])
116 PG_REGISTER_WITH_RESET_FN(ledStripConfig_t
, ledStripConfig
, PG_LED_STRIP_CONFIG
, 1);
118 void pgResetFn_ledStripConfig(ledStripConfig_t
*ledStripConfig
)
120 ledStripConfig
->ledstrip_visual_beeper
= 0;
121 #ifdef USE_LED_STRIP_STATUS_MODE
122 ledStripConfig
->ledstrip_profile
= LED_PROFILE_STATUS
;
124 ledStripConfig
->ledstrip_profile
= LED_PROFILE_RACE
;
126 ledStripConfig
->ledstrip_race_color
= COLOR_ORANGE
;
127 ledStripConfig
->ledstrip_beacon_color
= COLOR_WHITE
;
128 ledStripConfig
->ledstrip_beacon_period_ms
= 500; // 0.5 second (2hz)
129 ledStripConfig
->ledstrip_beacon_percent
= 50; // 50% duty cycle
130 ledStripConfig
->ledstrip_beacon_armed_only
= false; // blink always
131 ledStripConfig
->ledstrip_visual_beeper_color
= VISUAL_BEEPER_COLOR
;
133 ledStripConfig
->ioTag
= timerioTagGetByUsage(TIM_USE_LED
, 0);
137 #ifdef USE_LED_STRIP_STATUS_MODE
139 #if LED_MAX_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH
140 # error "Led strip length must match driver"
143 const hsvColor_t
*colors
;
144 const modeColorIndexes_t
*modeColors
;
145 specialColorIndexes_t specialColors
;
147 STATIC_UNIT_TESTED
uint8_t ledGridRows
;
149 STATIC_UNIT_TESTED
int8_t highestYValueForNorth
;
150 STATIC_UNIT_TESTED
int8_t lowestYValueForSouth
;
151 STATIC_UNIT_TESTED
int8_t highestXValueForWest
;
152 STATIC_UNIT_TESTED
int8_t lowestXValueForEast
;
154 STATIC_UNIT_TESTED ledCounts_t ledCounts
;
156 static const modeColorIndexes_t defaultModeColors
[] = {
157 // NORTH EAST SOUTH WEST UP DOWN
158 [LED_MODE_ORIENTATION
] = {{ COLOR_WHITE
, COLOR_DARK_VIOLET
, COLOR_RED
, COLOR_DEEP_PINK
, COLOR_BLUE
, COLOR_ORANGE
}},
159 [LED_MODE_HEADFREE
] = {{ COLOR_LIME_GREEN
, COLOR_DARK_VIOLET
, COLOR_ORANGE
, COLOR_DEEP_PINK
, COLOR_BLUE
, COLOR_ORANGE
}},
160 [LED_MODE_HORIZON
] = {{ COLOR_BLUE
, COLOR_DARK_VIOLET
, COLOR_YELLOW
, COLOR_DEEP_PINK
, COLOR_BLUE
, COLOR_ORANGE
}},
161 [LED_MODE_ANGLE
] = {{ COLOR_CYAN
, COLOR_DARK_VIOLET
, COLOR_YELLOW
, COLOR_DEEP_PINK
, COLOR_BLUE
, COLOR_ORANGE
}},
162 [LED_MODE_MAG
] = {{ COLOR_MINT_GREEN
, COLOR_DARK_VIOLET
, COLOR_ORANGE
, COLOR_DEEP_PINK
, COLOR_BLUE
, COLOR_ORANGE
}},
165 static const specialColorIndexes_t defaultSpecialColors
[] = {
166 {{ [LED_SCOLOR_DISARMED
] = COLOR_GREEN
,
167 [LED_SCOLOR_ARMED
] = COLOR_BLUE
,
168 [LED_SCOLOR_ANIMATION
] = COLOR_WHITE
,
169 [LED_SCOLOR_BACKGROUND
] = COLOR_BLACK
,
170 [LED_SCOLOR_BLINKBACKGROUND
] = COLOR_BLACK
,
171 [LED_SCOLOR_GPSNOSATS
] = COLOR_RED
,
172 [LED_SCOLOR_GPSNOLOCK
] = COLOR_ORANGE
,
173 [LED_SCOLOR_GPSLOCKED
] = COLOR_GREEN
,
177 PG_REGISTER_WITH_RESET_FN(ledStripStatusModeConfig_t
, ledStripStatusModeConfig
, PG_LED_STRIP_STATUS_MODE_CONFIG
, 0);
179 void pgResetFn_ledStripStatusModeConfig(ledStripStatusModeConfig_t
*ledStripStatusModeConfig
)
181 memset(ledStripStatusModeConfig
->ledConfigs
, 0, LED_MAX_STRIP_LENGTH
* sizeof(ledConfig_t
));
182 // copy hsv colors as default
183 memset(ledStripStatusModeConfig
->colors
, 0, ARRAYLEN(hsv
) * sizeof(hsvColor_t
));
184 STATIC_ASSERT(LED_CONFIGURABLE_COLOR_COUNT
>= ARRAYLEN(hsv
), LED_CONFIGURABLE_COLOR_COUNT_invalid
);
185 for (unsigned colorIndex
= 0; colorIndex
< ARRAYLEN(hsv
); colorIndex
++) {
186 ledStripStatusModeConfig
->colors
[colorIndex
] = hsv
[colorIndex
];
188 memcpy_fn(&ledStripStatusModeConfig
->modeColors
, &defaultModeColors
, sizeof(defaultModeColors
));
189 memcpy_fn(&ledStripStatusModeConfig
->specialColors
, &defaultSpecialColors
, sizeof(defaultSpecialColors
));
190 ledStripStatusModeConfig
->ledstrip_aux_channel
= THROTTLE
;
193 #define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off
194 #define ROTATION_SEQUENCE_LED_WIDTH 2 // 2 on
196 static void updateLedRingCounts(void)
199 // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds
200 if ((ledCounts
.ring
% ROTATION_SEQUENCE_LED_COUNT
) == 0) {
201 seqLen
= ROTATION_SEQUENCE_LED_COUNT
;
203 seqLen
= ledCounts
.ring
;
204 // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds
205 // TODO - improve partitioning (15 leds -> 3x5)
206 while ((seqLen
> ROTATION_SEQUENCE_LED_COUNT
) && ((seqLen
% 2) == 0)) {
210 ledCounts
.ringSeqLen
= seqLen
;
213 STATIC_UNIT_TESTED
void updateDimensions(void)
216 int minX
= LED_XY_MASK
;
218 int minY
= LED_XY_MASK
;
220 for (int ledIndex
= 0; ledIndex
< ledCounts
.count
; ledIndex
++) {
221 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[ledIndex
];
223 int ledX
= ledGetX(ledConfig
);
224 maxX
= MAX(ledX
, maxX
);
225 minX
= MIN(ledX
, minX
);
226 int ledY
= ledGetY(ledConfig
);
227 maxY
= MAX(ledY
, maxY
);
228 minY
= MIN(ledY
, minY
);
231 ledGridRows
= maxY
- minY
+ 1;
234 lowestXValueForEast
= (minX
+ maxX
) / 2 + 1;
235 highestXValueForWest
= (minX
+ maxX
- 1) / 2;
237 lowestXValueForEast
= LED_XY_MASK
/ 2;
238 highestXValueForWest
= lowestXValueForEast
- 1;
241 lowestYValueForSouth
= (minY
+ maxY
) / 2 + 1;
242 highestYValueForNorth
= (minY
+ maxY
- 1) / 2;
244 lowestYValueForSouth
= LED_XY_MASK
/ 2;
245 highestYValueForNorth
= lowestYValueForSouth
- 1;
250 STATIC_UNIT_TESTED
void updateLedCount(void)
252 int count
= 0, countRing
= 0, countScanner
= 0;
254 for (int ledIndex
= 0; ledIndex
< LED_MAX_STRIP_LENGTH
; ledIndex
++) {
255 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[ledIndex
];
262 if (ledGetFunction(ledConfig
) == LED_FUNCTION_THRUST_RING
)
265 if (ledGetOverlayBit(ledConfig
, LED_OVERLAY_LARSON_SCANNER
))
269 ledCounts
.count
= count
;
270 ledCounts
.ring
= countRing
;
271 ledCounts
.larson
= countScanner
;
272 setUsedLedCount(ledCounts
.count
);
275 void reevaluateLedConfig(void)
279 updateLedRingCounts();
280 updateRequiredOverlay();
283 // get specialColor by index
284 static const hsvColor_t
* getSC(ledSpecialColorIds_e index
)
286 return &ledStripStatusModeConfig()->colors
[ledStripStatusModeConfig()->specialColors
.color
[index
]];
289 static const char directionCodes
[LED_DIRECTION_COUNT
] = { 'N', 'E', 'S', 'W', 'U', 'D' };
290 static const char baseFunctionCodes
[LED_BASEFUNCTION_COUNT
] = { 'C', 'F', 'A', 'L', 'S', 'G', 'R' };
291 static const char overlayCodes
[LED_OVERLAY_COUNT
] = { 'T', 'O', 'B', 'V', 'I', 'W' };
293 #define CHUNK_BUFFER_SIZE 11
294 bool parseLedStripConfig(int ledIndex
, const char *config
)
296 if (ledIndex
>= LED_MAX_STRIP_LENGTH
)
307 static const char chunkSeparators
[PARSE_STATE_COUNT
] = {',', ':', ':', ':', '\0'};
309 ledConfig_t
*ledConfig
= &ledStripStatusModeConfigMutable()->ledConfigs
[ledIndex
];
310 memset(ledConfig
, 0, sizeof(ledConfig_t
));
312 int x
= 0, y
= 0, color
= 0; // initialize to prevent warnings
313 int baseFunction
= 0;
314 int overlay_flags
= 0;
315 int direction_flags
= 0;
317 for (enum parseState_e parseState
= 0; parseState
< PARSE_STATE_COUNT
; parseState
++) {
318 char chunk
[CHUNK_BUFFER_SIZE
];
320 char chunkSeparator
= chunkSeparators
[parseState
];
322 while (*config
&& *config
!= chunkSeparator
&& chunkIndex
< (CHUNK_BUFFER_SIZE
- 1)) {
323 chunk
[chunkIndex
++] = *config
++;
325 chunk
[chunkIndex
++] = 0; // zero-terminate chunk
326 if (*config
!= chunkSeparator
) {
329 config
++; // skip separator
331 switch (parseState
) {
339 for (char *ch
= chunk
; *ch
; ch
++) {
340 for (ledDirectionId_e dir
= 0; dir
< LED_DIRECTION_COUNT
; dir
++) {
341 if (directionCodes
[dir
] == *ch
) {
342 direction_flags
|= LED_FLAG_DIRECTION(dir
);
349 for (char *ch
= chunk
; *ch
; ch
++) {
350 for (ledBaseFunctionId_e fn
= 0; fn
< LED_BASEFUNCTION_COUNT
; fn
++) {
351 if (baseFunctionCodes
[fn
] == *ch
) {
357 for (ledOverlayId_e ol
= 0; ol
< LED_OVERLAY_COUNT
; ol
++) {
358 if (overlayCodes
[ol
] == *ch
) {
359 overlay_flags
|= LED_FLAG_OVERLAY(ol
);
367 if (color
>= LED_CONFIGURABLE_COLOR_COUNT
)
370 case PARSE_STATE_COUNT
:; // prevent warning
374 *ledConfig
= DEFINE_LED(x
, y
, color
, direction_flags
, baseFunction
, overlay_flags
, 0);
376 reevaluateLedConfig();
381 void generateLedConfig(ledConfig_t
*ledConfig
, char *ledConfigBuffer
, size_t bufferSize
)
383 char directions
[LED_DIRECTION_COUNT
+ 1];
384 char baseFunctionOverlays
[LED_OVERLAY_COUNT
+ 2];
386 memset(ledConfigBuffer
, 0, bufferSize
);
388 char *dptr
= directions
;
389 for (ledDirectionId_e dir
= 0; dir
< LED_DIRECTION_COUNT
; dir
++) {
390 if (ledGetDirectionBit(ledConfig
, dir
)) {
391 *dptr
++ = directionCodes
[dir
];
396 char *fptr
= baseFunctionOverlays
;
397 *fptr
++ = baseFunctionCodes
[ledGetFunction(ledConfig
)];
399 for (ledOverlayId_e ol
= 0; ol
< LED_OVERLAY_COUNT
; ol
++) {
400 if (ledGetOverlayBit(ledConfig
, ol
)) {
401 *fptr
++ = overlayCodes
[ol
];
406 // TODO - check buffer length
407 tfp_sprintf(ledConfigBuffer
, "%u,%u:%s:%s:%u", ledGetX(ledConfig
), ledGetY(ledConfig
), directions
, baseFunctionOverlays
, ledGetColor(ledConfig
));
411 // the ordering is important, see below how NSEW is mapped to NE/SE/NW/SW
412 QUADRANT_NORTH
= 1 << 0,
413 QUADRANT_SOUTH
= 1 << 1,
414 QUADRANT_EAST
= 1 << 2,
415 QUADRANT_WEST
= 1 << 3,
418 static quadrant_e
getLedQuadrant(const int ledIndex
)
420 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[ledIndex
];
422 int x
= ledGetX(ledConfig
);
423 int y
= ledGetY(ledConfig
);
426 if (y
<= highestYValueForNorth
)
427 quad
|= QUADRANT_NORTH
;
428 else if (y
>= lowestYValueForSouth
)
429 quad
|= QUADRANT_SOUTH
;
430 if (x
>= lowestXValueForEast
)
431 quad
|= QUADRANT_EAST
;
432 else if (x
<= highestXValueForWest
)
433 quad
|= QUADRANT_WEST
;
438 static const hsvColor_t
* getDirectionalModeColor(const int ledIndex
, const modeColorIndexes_t
*modeColors
)
440 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[ledIndex
];
441 const int ledDirection
= ledGetDirection(ledConfig
);
443 for (unsigned i
= 0; i
< LED_DIRECTION_COUNT
; i
++) {
444 if (ledDirection
& (1 << i
)) {
445 return &ledStripStatusModeConfig()->colors
[modeColors
->color
[i
]];
452 // map flight mode to led mode, in order of priority
453 // flightMode == 0 is always active
454 static const struct {
457 } flightModeToLed
[] = {
458 {HEADFREE_MODE
, LED_MODE_HEADFREE
},
460 {MAG_MODE
, LED_MODE_MAG
},
462 {HORIZON_MODE
, LED_MODE_HORIZON
},
463 {ANGLE_MODE
, LED_MODE_ANGLE
},
464 {0, LED_MODE_ORIENTATION
},
467 static void applyLedFixedLayers(void)
469 for (int ledIndex
= 0; ledIndex
< ledCounts
.count
; ledIndex
++) {
470 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[ledIndex
];
471 hsvColor_t color
= *getSC(LED_SCOLOR_BACKGROUND
);
473 int fn
= ledGetFunction(ledConfig
);
474 int hOffset
= HSV_HUE_MAX
+ 1;
477 case LED_FUNCTION_COLOR
:
478 color
= ledStripStatusModeConfig()->colors
[ledGetColor(ledConfig
)];
480 hsvColor_t nextColor
= ledStripStatusModeConfig()->colors
[(ledGetColor(ledConfig
) + 1 + LED_CONFIGURABLE_COLOR_COUNT
) % LED_CONFIGURABLE_COLOR_COUNT
];
481 hsvColor_t previousColor
= ledStripStatusModeConfig()->colors
[(ledGetColor(ledConfig
) - 1 + LED_CONFIGURABLE_COLOR_COUNT
) % LED_CONFIGURABLE_COLOR_COUNT
];
483 if (ledGetOverlayBit(ledConfig
, LED_OVERLAY_THROTTLE
)) { //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor
484 const int auxInput
= rcData
[ledStripStatusModeConfig()->ledstrip_aux_channel
];
485 int centerPWM
= (PWM_RANGE_MIN
+ PWM_RANGE_MAX
) / 2;
486 if (auxInput
< centerPWM
) {
487 color
.h
= scaleRange(auxInput
, PWM_RANGE_MIN
, centerPWM
, previousColor
.h
, color
.h
);
488 color
.s
= scaleRange(auxInput
, PWM_RANGE_MIN
, centerPWM
, previousColor
.s
, color
.s
);
489 color
.v
= scaleRange(auxInput
, PWM_RANGE_MIN
, centerPWM
, previousColor
.v
, color
.v
);
491 color
.h
= scaleRange(auxInput
, centerPWM
, PWM_RANGE_MAX
, color
.h
, nextColor
.h
);
492 color
.s
= scaleRange(auxInput
, centerPWM
, PWM_RANGE_MAX
, color
.s
, nextColor
.s
);
493 color
.v
= scaleRange(auxInput
, centerPWM
, PWM_RANGE_MAX
, color
.v
, nextColor
.v
);
499 case LED_FUNCTION_FLIGHT_MODE
:
500 for (unsigned i
= 0; i
< ARRAYLEN(flightModeToLed
); i
++)
501 if (!flightModeToLed
[i
].flightMode
|| FLIGHT_MODE(flightModeToLed
[i
].flightMode
)) {
502 const hsvColor_t
*directionalColor
= getDirectionalModeColor(ledIndex
, &ledStripStatusModeConfig()->modeColors
[flightModeToLed
[i
].ledMode
]);
503 if (directionalColor
) {
504 color
= *directionalColor
;
507 break; // stop on first match
511 case LED_FUNCTION_ARM_STATE
:
512 color
= ARMING_FLAG(ARMED
) ? *getSC(LED_SCOLOR_ARMED
) : *getSC(LED_SCOLOR_DISARMED
);
515 case LED_FUNCTION_BATTERY
:
517 hOffset
+= MAX(scaleRange(calculateBatteryPercentageRemaining(), 0, 100, -30, 120), 0);
520 case LED_FUNCTION_RSSI
:
522 hOffset
+= MAX(scaleRange(getRssiPercent(), 0, 100, -30, 120), 0);
529 if ((fn
!= LED_FUNCTION_COLOR
) && ledGetOverlayBit(ledConfig
, LED_OVERLAY_THROTTLE
)) {
530 const int auxInput
= rcData
[ledStripStatusModeConfig()->ledstrip_aux_channel
];
531 hOffset
+= scaleRange(auxInput
, PWM_RANGE_MIN
, PWM_RANGE_MAX
, 0, HSV_HUE_MAX
+ 1);
534 color
.h
= (color
.h
+ hOffset
) % (HSV_HUE_MAX
+ 1);
535 setLedHsv(ledIndex
, &color
);
539 static void applyLedHsv(uint32_t mask
, const hsvColor_t
*color
)
541 for (int ledIndex
= 0; ledIndex
< ledCounts
.count
; ledIndex
++) {
542 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[ledIndex
];
543 if ((*ledConfig
& mask
) == mask
)
544 setLedHsv(ledIndex
, color
);
549 WARNING_ARMING_DISABLED
,
552 WARNING_CRASH_FLIP_ACTIVE
,
555 static void applyLedWarningLayer(bool updateNow
, timeUs_t
*timer
)
557 static uint8_t warningFlashCounter
= 0;
558 static uint8_t warningFlags
= 0; // non-zero during blinks
561 // keep counter running, so it stays in sync with blink
562 warningFlashCounter
++;
563 warningFlashCounter
&= 0xF;
565 if (warningFlashCounter
== 0) { // update when old flags was processed
567 if (batteryConfig()->voltageMeterSource
!= VOLTAGE_METER_NONE
&& getBatteryState() != BATTERY_OK
) {
568 warningFlags
|= 1 << WARNING_LOW_BATTERY
;
570 if (failsafeIsActive()) {
571 warningFlags
|= 1 << WARNING_FAILSAFE
;
573 if (!ARMING_FLAG(ARMED
) && isArmingDisabled()) {
574 warningFlags
|= 1 << WARNING_ARMING_DISABLED
;
576 if (isFlipOverAfterCrashActive()) {
577 warningFlags
|= 1 << WARNING_CRASH_FLIP_ACTIVE
;
580 *timer
+= HZ_TO_US(10);
583 const hsvColor_t
*warningColor
= NULL
;
586 bool colorOn
= (warningFlashCounter
% 2) == 0; // w_w_
587 warningFlags_e warningId
= warningFlashCounter
/ 4;
588 if (warningFlags
& (1 << warningId
)) {
590 case WARNING_ARMING_DISABLED
:
591 warningColor
= colorOn
? &HSV(GREEN
) : &HSV(BLACK
);
593 case WARNING_CRASH_FLIP_ACTIVE
:
594 warningColor
= colorOn
? &HSV(MAGENTA
) : &HSV(BLACK
);
596 case WARNING_LOW_BATTERY
:
597 warningColor
= colorOn
? &HSV(RED
) : &HSV(BLACK
);
599 case WARNING_FAILSAFE
:
600 warningColor
= colorOn
? &HSV(YELLOW
) : &HSV(BLUE
);
607 warningColor
= &hsv
[ledStripConfig()->ledstrip_visual_beeper_color
];
612 applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING
)), warningColor
);
616 #ifdef USE_VTX_COMMON
617 static void applyLedVtxLayer(bool updateNow
, timeUs_t
*timer
)
619 static uint16_t frequency
= 0;
620 static uint8_t power
= 255;
621 static unsigned vtxStatus
= UINT32_MAX
;
622 static uint8_t showSettings
= false;
623 static uint16_t lastCheck
= 0;
624 static bool blink
= false;
626 const vtxDevice_t
*vtxDevice
= vtxCommonDevice();
631 uint8_t band
= 255, channel
= 255;
635 // keep counter running, so it stays in sync with vtx
636 vtxCommonGetBandAndChannel(vtxDevice
, &band
, &channel
);
637 vtxCommonGetPowerIndex(vtxDevice
, &power
);
638 vtxCommonGetStatus(vtxDevice
, &vtxStatus
);
640 frequency
= vtxCommonLookupFrequency(vtxDevice
, band
, channel
);
642 // check if last vtx values have changed.
643 check
= ((vtxStatus
& VTX_STATUS_PIT_MODE
) ? 1 : 0) + (power
<< 1) + (band
<< 4) + (channel
<< 8);
644 if (!showSettings
&& check
!= lastCheck
) {
645 // display settings for 3 seconds.
648 lastCheck
= check
; // quick way to check if any settings changed.
654 *timer
+= HZ_TO_US(5); // check 5 times a second
657 hsvColor_t color
= {0, 0, 0};
658 if (showSettings
) { // show settings
659 uint8_t vtxLedCount
= 0;
660 for (int i
= 0; i
< ledCounts
.count
&& vtxLedCount
< 6; ++i
) {
661 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[i
];
662 if (ledGetOverlayBit(ledConfig
, LED_OVERLAY_VTX
)) {
663 if (vtxLedCount
== 0) {
664 color
.h
= HSV(GREEN
).h
;
665 color
.s
= HSV(GREEN
).s
;
666 color
.v
= blink
? 15 : 0; // blink received settings
668 else if (vtxLedCount
> 0 && power
>= vtxLedCount
&& !(vtxStatus
& VTX_STATUS_PIT_MODE
)) { // show power
669 color
.h
= HSV(ORANGE
).h
;
670 color
.s
= HSV(ORANGE
).s
;
671 color
.v
= blink
? 15 : 0; // blink received settings
673 else { // turn rest off
674 color
.h
= HSV(BLACK
).h
;
675 color
.s
= HSV(BLACK
).s
;
676 color
.v
= HSV(BLACK
).v
;
678 setLedHsv(i
, &color
);
683 else { // show frequency
684 // calculate the VTX color based on frequency
686 if (frequency
<= 5672) {
687 colorIndex
= COLOR_WHITE
;
688 } else if (frequency
<= 5711) {
689 colorIndex
= COLOR_RED
;
690 } else if (frequency
<= 5750) {
691 colorIndex
= COLOR_ORANGE
;
692 } else if (frequency
<= 5789) {
693 colorIndex
= COLOR_YELLOW
;
694 } else if (frequency
<= 5829) {
695 colorIndex
= COLOR_GREEN
;
696 } else if (frequency
<= 5867) {
697 colorIndex
= COLOR_BLUE
;
698 } else if (frequency
<= 5906) {
699 colorIndex
= COLOR_DARK_VIOLET
;
701 colorIndex
= COLOR_DEEP_PINK
;
703 hsvColor_t color
= ledStripStatusModeConfig()->colors
[colorIndex
];
704 color
.v
= (vtxStatus
& VTX_STATUS_PIT_MODE
) ? (blink
? 15 : 0) : 255; // blink when in pit mode
705 applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_VTX
)), &color
);
710 static void applyLedBatteryLayer(bool updateNow
, timeUs_t
*timer
)
712 static bool flash
= false;
714 int timerDelayUs
= HZ_TO_US(1);
718 switch (getBatteryState()) {
721 timerDelayUs
= HZ_TO_US(1);
724 case BATTERY_WARNING
:
726 timerDelayUs
= HZ_TO_US(2);
731 timerDelayUs
= HZ_TO_US(8);
737 *timer
+= timerDelayUs
;
740 const hsvColor_t
*bgc
= getSC(LED_SCOLOR_BACKGROUND
);
741 applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY
), bgc
);
745 static void applyLedRssiLayer(bool updateNow
, timeUs_t
*timer
)
747 static bool flash
= false;
749 int timerDelay
= HZ_TO_US(1);
752 int state
= getRssiPercent();
756 timerDelay
= HZ_TO_US(1);
757 } else if (state
> 20) {
759 timerDelay
= HZ_TO_US(2);
762 timerDelay
= HZ_TO_US(8);
766 *timer
+= timerDelay
;
769 const hsvColor_t
*bgc
= getSC(LED_SCOLOR_BACKGROUND
);
770 applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI
), bgc
);
775 static void applyLedGpsLayer(bool updateNow
, timeUs_t
*timer
)
778 static uint8_t gpsPauseCounter
= 0;
779 const uint8_t blinkPauseLength
= 4;
782 static uint8_t gpsFlashCounter
= 0;
783 if (gpsPauseCounter
> 0) {
785 } else if (gpsFlashCounter
>= gpsSol
.numSat
) {
787 gpsPauseCounter
= blinkPauseLength
;
792 *timer
+= HZ_TO_US(2.5f
);
795 const hsvColor_t
*gpsColor
;
797 if (gpsSol
.numSat
== 0 || !sensors(SENSOR_GPS
)) {
798 gpsColor
= getSC(LED_SCOLOR_GPSNOSATS
);
800 bool colorOn
= gpsPauseCounter
== 0; // each interval starts with pause
801 if (STATE(GPS_FIX
)) {
802 gpsColor
= colorOn
? getSC(LED_SCOLOR_GPSLOCKED
) : getSC(LED_SCOLOR_BACKGROUND
);
804 gpsColor
= colorOn
? getSC(LED_SCOLOR_GPSNOLOCK
) : getSC(LED_SCOLOR_GPSNOSATS
);
808 applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_GPS
), gpsColor
);
812 #define INDICATOR_DEADBAND 25
814 static void applyLedIndicatorLayer(bool updateNow
, timeUs_t
*timer
)
816 static bool flash
= 0;
819 if (rxIsReceivingSignal()) {
820 // calculate update frequency
821 int scale
= MAX(fabsf(rcCommand
[ROLL
]), fabsf(rcCommand
[PITCH
])); // 0 - 500
822 scale
= scale
- INDICATOR_DEADBAND
; // start increasing frequency right after deadband
823 *timer
+= HZ_TO_US(5 + (45 * scale
) / (500 - INDICATOR_DEADBAND
)); // 5 - 50Hz update, 2.5 - 25Hz blink
827 *timer
+= HZ_TO_US(5);
834 const hsvColor_t
*flashColor
= &HSV(ORANGE
); // TODO - use user color?
836 quadrant_e quadrants
= 0;
837 if (rcCommand
[ROLL
] > INDICATOR_DEADBAND
) {
838 quadrants
|= QUADRANT_EAST
;
839 } else if (rcCommand
[ROLL
] < -INDICATOR_DEADBAND
) {
840 quadrants
|= QUADRANT_WEST
;
842 if (rcCommand
[PITCH
] > INDICATOR_DEADBAND
) {
843 quadrants
|= QUADRANT_NORTH
;
844 } else if (rcCommand
[PITCH
] < -INDICATOR_DEADBAND
) {
845 quadrants
|= QUADRANT_SOUTH
;
848 for (int ledIndex
= 0; ledIndex
< ledCounts
.count
; ledIndex
++) {
849 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[ledIndex
];
850 if (ledGetOverlayBit(ledConfig
, LED_OVERLAY_INDICATOR
)) {
851 if (getLedQuadrant(ledIndex
) & quadrants
)
852 setLedHsv(ledIndex
, flashColor
);
857 static void applyLedThrustRingLayer(bool updateNow
, timeUs_t
*timer
)
859 static uint8_t rotationPhase
;
860 int ledRingIndex
= 0;
863 rotationPhase
= rotationPhase
> 0 ? rotationPhase
- 1 : ledCounts
.ringSeqLen
- 1;
865 const int scaledThrottle
= ARMING_FLAG(ARMED
) ? scaleRange(rcData
[THROTTLE
], PWM_RANGE_MIN
, PWM_RANGE_MAX
, 0, 100) : 0;
866 *timer
+= HZ_TO_US(5 + (45 * scaledThrottle
) / 100); // 5 - 50Hz update rate
869 for (int ledIndex
= 0; ledIndex
< ledCounts
.count
; ledIndex
++) {
870 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[ledIndex
];
871 if (ledGetFunction(ledConfig
) == LED_FUNCTION_THRUST_RING
) {
874 if (ARMING_FLAG(ARMED
)) {
875 applyColor
= (ledRingIndex
+ rotationPhase
) % ledCounts
.ringSeqLen
< ROTATION_SEQUENCE_LED_WIDTH
;
877 applyColor
= !(ledRingIndex
% 2); // alternating pattern
881 const hsvColor_t
*ringColor
= &ledStripStatusModeConfig()->colors
[ledGetColor(ledConfig
)];
882 setLedHsv(ledIndex
, ringColor
);
890 typedef struct larsonParameters_s
{
891 uint8_t currentBrightness
;
894 } larsonParameters_t
;
896 static int brightnessForLarsonIndex(larsonParameters_t
*larsonParameters
, uint8_t larsonIndex
)
898 int offset
= larsonIndex
- larsonParameters
->currentIndex
;
899 static const int larsonLowValue
= 8;
902 return (larsonLowValue
);
905 return (larsonParameters
->currentBrightness
);
907 if (larsonParameters
->direction
== offset
) {
908 return (larsonParameters
->currentBrightness
- 127);
911 return (255 - larsonParameters
->currentBrightness
);
915 static void larsonScannerNextStep(larsonParameters_t
*larsonParameters
, int delta
)
917 if (larsonParameters
->currentBrightness
> (255 - delta
)) {
918 larsonParameters
->currentBrightness
= 127;
919 if (larsonParameters
->currentIndex
>= ledCounts
.larson
|| larsonParameters
->currentIndex
< 0) {
920 larsonParameters
->direction
= -larsonParameters
->direction
;
922 larsonParameters
->currentIndex
+= larsonParameters
->direction
;
924 larsonParameters
->currentBrightness
+= delta
;
928 static void applyLarsonScannerLayer(bool updateNow
, timeUs_t
*timer
)
930 static larsonParameters_t larsonParameters
= { 0, 0, 1 };
933 larsonScannerNextStep(&larsonParameters
, 15);
934 *timer
+= HZ_TO_US(60);
937 int scannerLedIndex
= 0;
938 for (unsigned i
= 0; i
< ledCounts
.count
; i
++) {
940 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[i
];
942 if (ledGetOverlayBit(ledConfig
, LED_OVERLAY_LARSON_SCANNER
)) {
944 getLedHsv(i
, &ledColor
);
945 ledColor
.v
= brightnessForLarsonIndex(&larsonParameters
, scannerLedIndex
);
946 setLedHsv(i
, &ledColor
);
952 // blink twice, then wait ; either always or just when landing
953 static void applyLedBlinkLayer(bool updateNow
, timeUs_t
*timer
)
955 const uint16_t blinkPattern
= 0x8005; // 0b1000000000000101;
956 static uint16_t blinkMask
;
959 blinkMask
= blinkMask
>> 1;
961 blinkMask
= blinkPattern
;
963 *timer
+= HZ_TO_US(10);
966 bool ledOn
= (blinkMask
& 1); // b_b_____...
968 for (int i
= 0; i
< ledCounts
.count
; ++i
) {
969 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[i
];
971 if (ledGetOverlayBit(ledConfig
, LED_OVERLAY_BLINK
)) {
972 setLedHsv(i
, getSC(LED_SCOLOR_BLINKBACKGROUND
));
978 // In reverse order of priority
984 #ifdef USE_VTX_COMMON
996 static timeUs_t timerVal
[timTimerCount
];
997 static uint16_t disabledTimerMask
;
999 STATIC_ASSERT(timTimerCount
<= sizeof(disabledTimerMask
) * 8, disabledTimerMask_too_small
);
1001 // function to apply layer.
1002 // function must replan self using timer pointer
1003 // when updateNow is true (timer triggered), state must be updated first,
1004 // before calculating led state. Otherwise update started by different trigger
1005 // may modify LED state.
1006 typedef void applyLayerFn_timed(bool updateNow
, timeUs_t
*timer
);
1008 static applyLayerFn_timed
* layerTable
[] = {
1009 [timBlink
] = &applyLedBlinkLayer
,
1010 [timLarson
] = &applyLarsonScannerLayer
,
1011 [timBattery
] = &applyLedBatteryLayer
,
1012 [timRssi
] = &applyLedRssiLayer
,
1014 [timGps
] = &applyLedGpsLayer
,
1016 [timWarning
] = &applyLedWarningLayer
,
1017 #ifdef USE_VTX_COMMON
1018 [timVtx
] = &applyLedVtxLayer
,
1020 [timIndicator
] = &applyLedIndicatorLayer
,
1021 [timRing
] = &applyLedThrustRingLayer
1024 bool isOverlayTypeUsed(ledOverlayId_e overlayType
)
1026 for (int ledIndex
= 0; ledIndex
< ledCounts
.count
; ledIndex
++) {
1027 const ledConfig_t
*ledConfig
= &ledStripStatusModeConfig()->ledConfigs
[ledIndex
];
1028 if (ledGetOverlayBit(ledConfig
, overlayType
)) {
1035 void updateRequiredOverlay(void)
1037 disabledTimerMask
= 0;
1038 disabledTimerMask
|= !isOverlayTypeUsed(LED_OVERLAY_BLINK
) << timBlink
;
1039 disabledTimerMask
|= !isOverlayTypeUsed(LED_OVERLAY_LARSON_SCANNER
) << timLarson
;
1040 disabledTimerMask
|= !isOverlayTypeUsed(LED_OVERLAY_WARNING
) << timWarning
;
1041 #ifdef USE_VTX_COMMON
1042 disabledTimerMask
|= !isOverlayTypeUsed(LED_OVERLAY_VTX
) << timVtx
;
1044 disabledTimerMask
|= !isOverlayTypeUsed(LED_OVERLAY_INDICATOR
) << timIndicator
;
1047 static void applyStatusProfile(timeUs_t now
) {
1049 // apply all layers; triggered timed functions has to update timers
1050 // test all led timers, setting corresponding bits
1051 uint32_t timActive
= 0;
1052 for (timId_e timId
= 0; timId
< timTimerCount
; timId
++) {
1053 if (!(disabledTimerMask
& (1 << timId
))) {
1054 // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
1055 const timeDelta_t delta
= cmpTimeUs(now
, timerVal
[timId
]);
1056 // max delay is limited to 5s
1057 if (delta
< 0 && delta
> -MAX_TIMER_DELAY
)
1058 continue; // not ready yet
1059 timActive
|= 1 << timId
;
1060 if (delta
>= 100 * 1000 || delta
< 0) {
1061 timerVal
[timId
] = now
;
1067 return; // no change this update, keep old state
1069 applyLedFixedLayers();
1070 for (timId_e timId
= 0; timId
< ARRAYLEN(layerTable
); timId
++) {
1071 uint32_t *timer
= &timerVal
[timId
];
1072 bool updateNow
= timActive
& (1 << timId
);
1073 (*layerTable
[timId
])(updateNow
, timer
);
1075 ws2811UpdateStrip((ledStripFormatRGB_e
) ledStripConfig()->ledstrip_grb_rgb
);
1078 bool parseColor(int index
, const char *colorConfig
)
1080 const char *remainingCharacters
= colorConfig
;
1082 hsvColor_t
*color
= &ledStripStatusModeConfigMutable()->colors
[index
];
1085 static const uint16_t hsv_limit
[HSV_COLOR_COMPONENT_COUNT
] = {
1086 [HSV_HUE
] = HSV_HUE_MAX
,
1087 [HSV_SATURATION
] = HSV_SATURATION_MAX
,
1088 [HSV_VALUE
] = HSV_VALUE_MAX
,
1090 for (int componentIndex
= 0; result
&& componentIndex
< HSV_COLOR_COMPONENT_COUNT
; componentIndex
++) {
1091 int val
= atoi(remainingCharacters
);
1092 if (val
> hsv_limit
[componentIndex
]) {
1096 switch (componentIndex
) {
1100 case HSV_SATURATION
:
1107 remainingCharacters
= strchr(remainingCharacters
, ',');
1108 if (remainingCharacters
) {
1109 remainingCharacters
++; // skip separator
1111 if (componentIndex
< HSV_COLOR_COMPONENT_COUNT
- 1) {
1118 memset(color
, 0, sizeof(*color
));
1125 * Redefine a color in a mode.
1127 bool setModeColor(ledModeIndex_e modeIndex
, int modeColorIndex
, int colorIndex
)
1130 if (colorIndex
< 0 || colorIndex
>= LED_CONFIGURABLE_COLOR_COUNT
)
1132 if (modeIndex
< LED_MODE_COUNT
) { // modeIndex_e is unsigned, so one-sided test is enough
1133 if (modeColorIndex
< 0 || modeColorIndex
>= LED_DIRECTION_COUNT
)
1135 ledStripStatusModeConfigMutable()->modeColors
[modeIndex
].color
[modeColorIndex
] = colorIndex
;
1136 } else if (modeIndex
== LED_SPECIAL
) {
1137 if (modeColorIndex
< 0 || modeColorIndex
>= LED_SPECIAL_COLOR_COUNT
)
1139 ledStripStatusModeConfigMutable()->specialColors
.color
[modeColorIndex
] = colorIndex
;
1140 } else if (modeIndex
== LED_AUX_CHANNEL
) {
1141 if (modeColorIndex
< 0 || modeColorIndex
>= 1)
1143 ledStripStatusModeConfigMutable()->ledstrip_aux_channel
= colorIndex
;
1151 void ledStripEnable(void)
1153 ws2811LedStripEnable();
1155 ledStripEnabled
= true;
1158 void ledStripDisable(void)
1160 ledStripEnabled
= false;
1161 previousProfileColorIndex
= COLOR_UNDEFINED
;
1163 setStripColor(&HSV(BLACK
));
1164 ws2811UpdateStrip((ledStripFormatRGB_e
)ledStripConfig()->ledstrip_grb_rgb
);
1167 void ledStripInit(void)
1169 #if defined(USE_LED_STRIP_STATUS_MODE)
1170 colors
= ledStripStatusModeConfig()->colors
;
1171 modeColors
= ledStripStatusModeConfig()->modeColors
;
1172 specialColors
= ledStripStatusModeConfig()->specialColors
;
1174 reevaluateLedConfig();
1177 ws2811LedStripInit(ledStripConfig()->ioTag
);
1180 static uint8_t selectVisualBeeperColor(uint8_t colorIndex
)
1182 if (ledStripConfig()->ledstrip_visual_beeper
&& isBeeperOn()) {
1183 return ledStripConfig()->ledstrip_visual_beeper_color
;
1189 static void applySimpleProfile(timeUs_t currentTimeUs
)
1191 static timeUs_t colorUpdateTimeUs
= 0;
1192 uint8_t colorIndex
= COLOR_BLACK
;
1193 bool blinkLed
= false;
1194 bool visualBeeperOverride
= true;
1195 unsigned flashPeriod
;
1198 if (IS_RC_MODE_ACTIVE(BOXBEEPERON
) || failsafeIsActive()) {
1199 // RX_SET or failsafe - force the beacon on and override the profile settings
1201 visualBeeperOverride
= false; // prevent the visual beeper from interfering
1202 flashPeriod
= BEACON_FAILSAFE_PERIOD_US
;
1203 onPercent
= BEACON_FAILSAFE_ON_PERCENT
;
1204 colorIndex
= ledStripConfig()->ledstrip_visual_beeper_color
;
1206 switch (ledStripConfig()->ledstrip_profile
) {
1207 case LED_PROFILE_RACE
:
1208 colorIndex
= ledStripConfig()->ledstrip_race_color
;
1211 case LED_PROFILE_BEACON
: {
1212 if (!ledStripConfig()->ledstrip_beacon_armed_only
|| ARMING_FLAG(ARMED
)) {
1213 flashPeriod
= ledStripConfig()->ledstrip_beacon_period_ms
;
1214 onPercent
= ledStripConfig()->ledstrip_beacon_percent
;
1215 colorIndex
= ledStripConfig()->ledstrip_beacon_color
;
1227 const unsigned onPeriod
= flashPeriod
* onPercent
/ 100;
1228 const bool beaconState
= (millis() % flashPeriod
) < onPeriod
;
1229 colorIndex
= (beaconState
) ? colorIndex
: COLOR_BLACK
;
1232 if (visualBeeperOverride
) {
1233 colorIndex
= selectVisualBeeperColor(colorIndex
);
1236 if ((colorIndex
!= previousProfileColorIndex
) || (currentTimeUs
>= colorUpdateTimeUs
)) {
1237 setStripColor(&hsv
[colorIndex
]);
1238 ws2811UpdateStrip((ledStripFormatRGB_e
)ledStripConfig()->ledstrip_grb_rgb
);
1239 previousProfileColorIndex
= colorIndex
;
1240 colorUpdateTimeUs
= currentTimeUs
+ PROFILE_COLOR_UPDATE_INTERVAL_US
;
1244 void ledStripUpdate(timeUs_t currentTimeUs
)
1246 #ifndef USE_LED_STRIP_STATUS_MODE
1247 UNUSED(currentTimeUs
);
1250 if (!isWS2811LedStripReady()) {
1254 if (ledStripEnabled
&& IS_RC_MODE_ACTIVE(BOXLEDLOW
)) {
1256 } else if (!IS_RC_MODE_ACTIVE(BOXLEDLOW
)) {
1260 if (ledStripEnabled
) {
1261 switch (ledStripConfig()->ledstrip_profile
) {
1262 #ifdef USE_LED_STRIP_STATUS_MODE
1263 case LED_PROFILE_STATUS
: {
1264 applyStatusProfile(currentTimeUs
);
1268 case LED_PROFILE_RACE
:
1269 case LED_PROFILE_BEACON
: {
1270 applySimpleProfile(currentTimeUs
);
1280 uint8_t getLedProfile(void)
1282 return ledStripConfig()->ledstrip_profile
;
1285 void setLedProfile(uint8_t profile
)
1287 if (profile
< LED_PROFILE_COUNT
) {
1288 ledStripConfigMutable()->ledstrip_profile
= profile
;