Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity
betaflight/src/main/io/ledstrip.c
ctzsnooze 7156dc84a3
Update turtle / crashflip mode (#13905) 
* stop motors after 90 degrees of rotation and with max rate

* handle no accelerometer data

* improve check for acc, although seems to be OK without it

* disable all attenuation if rate is set to zero

* refactoring thanks Karate

* use sensors.h

* remove unnecessary arming check

* exit crashFlip immediately switch is reverted if throttle is zero

* add Crashflip Rate to OSD

* Revert unnecessary changes in crashflip core.c code

and clarify comments about crashflip switch

* update / minimise comments, thanks Karate

* ensure all names say `crashflip` consistently

* Undo quick re-arm because motrors were not reversed

* fix issue with reversed motors, we must disarm

* ignore yaw rotation and set gyro limit to 1900 deg/s

* default attenuation to off (crashflip_rate = 0)

* refactoring, increase rate limit to allow stronger inhibition

* enable in race_pro mode

* don't attenuate on attitude until a significant change occurs

* no attenuation for small changes

* Updates from review by PL

* remove whitespace

* refactor motorOutput, update comments, renaming variables

thanks PL

* changes from review PL

* only permit fast re-arm if crashflip rate set and crashflip was successful

* properly exit turtle mode

* add crashFlipSuccessful to unit test extern c

* small updates from review

* improved crashflip switch handling

* remove unnecessary motors normal check
2024-10-05 07:58:33 +10:00

1591 lines
53 KiB
C
Raw Blame History

/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
* 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdarg.h>
#include <math.h>
#include "platform.h"
#ifdef USE_LED_STRIP
#include "build/build_config.h"
#include "common/axis.h"
#include "common/color.h"
#include "common/maths.h"
#include "common/printf.h"
#include "common/typeconversion.h"
#include "common/utils.h"
#include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/rx.h"
#include "drivers/light_ws2811strip.h"
#include "drivers/serial.h"
#include "drivers/time.h"
#include "drivers/vtx_common.h"
#include "config/config.h"
#include "fc/core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "flight/position.h"
#include "io/beeper.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/serial.h"
#include "io/vtx.h"
#include "rx/rx.h"
#include "scheduler/scheduler.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "telemetry/telemetry.h"
#define COLOR_UNDEFINED 255
static bool ledStripEnabled = false;
static uint8_t previousProfileColorIndex = COLOR_UNDEFINED;
#define HZ_TO_US(hz) ((int32_t)((1000 * 1000) / (hz)))
#define MAX_TIMER_DELAY (5 * 1000 * 1000)
#define TASK_LEDSTRIP_RATE_WAIT_HZ 500 // Scheduling rate waiting for a timer to fire
#define TASK_LEDSTRIP_RATE_FAST_HZ 100000 // Reschedule as fast as possible
#define LED_OVERLAY_RAINBOW_RATE_HZ 60
#define LED_OVERLAY_LARSON_RATE_HZ 60
#define LED_OVERLAY_BLINK_RATE_HZ 10
#define LED_OVERLAY_VTX_RATE_HZ 5
#define LED_OVERLAY_INDICATOR_RATE_HZ 5
#define LED_OVERLAY_WARNING_RATE_HZ 10
#define LED_TASK_MARGIN 1
// Decay the estimated max task duration by 1/(1 << LED_EXEC_TIME_SHIFT) on every invocation
#define LED_EXEC_TIME_SHIFT 7
#define PROFILE_COLOR_UPDATE_INTERVAL_US 1e6 // normally updates when color changes but this is a 1 second forced update
#define VISUAL_BEEPER_COLOR COLOR_WHITE
#define BEACON_FAILSAFE_PERIOD_US 250 // 2Hz
#define BEACON_FAILSAFE_ON_PERCENT 50 // 50% duty cycle
const hsvColor_t hsv[] = {
// H S V
[COLOR_BLACK] = { 0, 0, 0},
[COLOR_WHITE] = { 0, 255, 255},
[COLOR_RED] = { 0, 0, 255},
[COLOR_ORANGE] = { 30, 0, 255},
[COLOR_YELLOW] = { 60, 0, 255},
[COLOR_LIME_GREEN] = { 90, 0, 255},
[COLOR_GREEN] = {120, 0, 255},
[COLOR_MINT_GREEN] = {150, 0, 255},
[COLOR_CYAN] = {180, 0, 255},
[COLOR_LIGHT_BLUE] = {210, 0, 255},
[COLOR_BLUE] = {240, 0, 255},
[COLOR_DARK_VIOLET] = {270, 0, 255},
[COLOR_MAGENTA] = {300, 0, 255},
[COLOR_DEEP_PINK] = {330, 0, 255},
};
// macro to save typing on default colors
#define HSV(color) (hsv[COLOR_ ## color])
PG_REGISTER_WITH_RESET_FN(ledStripConfig_t, ledStripConfig, PG_LED_STRIP_CONFIG, 3);
void pgResetFn_ledStripConfig(ledStripConfig_t *ledStripConfig)
{
ledStripConfig->ledstrip_visual_beeper = 0;
#ifdef USE_LED_STRIP_STATUS_MODE
ledStripConfig->ledstrip_profile = LED_PROFILE_STATUS;
#else
ledStripConfig->ledstrip_profile = LED_PROFILE_RACE;
#endif
ledStripConfig->ledstrip_race_color = COLOR_ORANGE;
ledStripConfig->ledstrip_beacon_color = COLOR_WHITE;
ledStripConfig->ledstrip_beacon_period_ms = 500; // 0.5 second (2hz)
ledStripConfig->ledstrip_beacon_percent = 50; // 50% duty cycle
ledStripConfig->ledstrip_beacon_armed_only = false; // blink always
ledStripConfig->ledstrip_visual_beeper_color = VISUAL_BEEPER_COLOR;
ledStripConfig->ledstrip_brightness = 100;
ledStripConfig->ledstrip_rainbow_delta = 0;
ledStripConfig->ledstrip_rainbow_freq = 120;
#ifndef UNIT_TEST
#ifdef LED_STRIP_PIN
ledStripConfig->ioTag = IO_TAG(LED_STRIP_PIN);
#else
ledStripConfig->ioTag = IO_TAG_NONE;
#endif
#endif
}
#ifdef USE_LED_STRIP_STATUS_MODE
#if LED_STRIP_MAX_LENGTH > WS2811_LED_STRIP_LENGTH
# error "Led strip length must match driver"
#endif
typedef enum {
LED_PROFILE_SLOW,
LED_PROFILE_FAST,
LED_PROFILE_ADVANCE
} ledProfileSequence_t;
const hsvColor_t *colors;
const modeColorIndexes_t *modeColors;
specialColorIndexes_t specialColors;
STATIC_UNIT_TESTED uint8_t ledGridRows;
// grid offsets
STATIC_UNIT_TESTED int8_t highestYValueForNorth;
STATIC_UNIT_TESTED int8_t lowestYValueForSouth;
STATIC_UNIT_TESTED int8_t highestXValueForWest;
STATIC_UNIT_TESTED int8_t lowestXValueForEast;
STATIC_UNIT_TESTED ledCounts_t ledCounts;
static const modeColorIndexes_t defaultModeColors[] = {
// NORTH EAST SOUTH WEST UP DOWN
[LED_MODE_ORIENTATION] = {{ COLOR_WHITE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
[LED_MODE_HEADFREE] = {{ COLOR_LIME_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
[LED_MODE_HORIZON] = {{ COLOR_BLUE, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
[LED_MODE_ANGLE] = {{ COLOR_CYAN, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
[LED_MODE_MAG] = {{ COLOR_MINT_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }},
};
static const specialColorIndexes_t defaultSpecialColors[] = {
{{ [LED_SCOLOR_DISARMED] = COLOR_GREEN,
[LED_SCOLOR_ARMED] = COLOR_BLUE,
[LED_SCOLOR_ANIMATION] = COLOR_WHITE,
[LED_SCOLOR_BACKGROUND] = COLOR_BLACK,
[LED_SCOLOR_BLINKBACKGROUND] = COLOR_BLACK,
[LED_SCOLOR_GPSNOSATS] = COLOR_RED,
[LED_SCOLOR_GPSNOLOCK] = COLOR_ORANGE,
[LED_SCOLOR_GPSLOCKED] = COLOR_GREEN,
}}
};
PG_REGISTER_WITH_RESET_FN(ledStripStatusModeConfig_t, ledStripStatusModeConfig, PG_LED_STRIP_STATUS_MODE_CONFIG, 0);
void pgResetFn_ledStripStatusModeConfig(ledStripStatusModeConfig_t *ledStripStatusModeConfig)
{
memset(ledStripStatusModeConfig->ledConfigs, 0, LED_STRIP_MAX_LENGTH * sizeof(ledConfig_t));
// copy hsv colors as default
memset(ledStripStatusModeConfig->colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t));
STATIC_ASSERT(LED_CONFIGURABLE_COLOR_COUNT >= ARRAYLEN(hsv), LED_CONFIGURABLE_COLOR_COUNT_invalid);
for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) {
ledStripStatusModeConfig->colors[colorIndex] = hsv[colorIndex];
}
memcpy_fn(&ledStripStatusModeConfig->modeColors, &defaultModeColors, sizeof(defaultModeColors));
memcpy_fn(&ledStripStatusModeConfig->specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors));
ledStripStatusModeConfig->ledstrip_aux_channel = THROTTLE;
}
#define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off
#define ROTATION_SEQUENCE_LED_WIDTH 2 // 2 on
static void updateLedRingCounts(void)
{
int seqLen;
// try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds
if ((ledCounts.ring % ROTATION_SEQUENCE_LED_COUNT) == 0) {
seqLen = ROTATION_SEQUENCE_LED_COUNT;
} else {
seqLen = ledCounts.ring;
// else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds
// TODO - improve partitioning (15 leds -> 3x5)
while ((seqLen > ROTATION_SEQUENCE_LED_COUNT) && ((seqLen % 2) == 0)) {
seqLen /= 2;
}
}
ledCounts.ringSeqLen = seqLen;
}
STATIC_UNIT_TESTED void updateDimensions(void)
{
int maxX = 0;
int minX = LED_XY_MASK;
int maxY = 0;
int minY = LED_XY_MASK;
for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
int ledX = ledGetX(ledConfig);
maxX = MAX(ledX, maxX);
minX = MIN(ledX, minX);
int ledY = ledGetY(ledConfig);
maxY = MAX(ledY, maxY);
minY = MIN(ledY, minY);
}
ledGridRows = maxY - minY + 1;
if (minX < maxX) {
lowestXValueForEast = (minX + maxX) / 2 + 1;
highestXValueForWest = (minX + maxX - 1) / 2;
} else {
lowestXValueForEast = LED_XY_MASK / 2;
highestXValueForWest = lowestXValueForEast - 1;
}
if (minY < maxY) {
lowestYValueForSouth = (minY + maxY) / 2 + 1;
highestYValueForNorth = (minY + maxY - 1) / 2;
} else {
lowestYValueForSouth = LED_XY_MASK / 2;
highestYValueForNorth = lowestYValueForSouth - 1;
}
}
enum ledBarIds {
LED_BAR_GPS,
LED_BAR_BATTERY,
LED_BAR_COUNT
};
static uint8_t ledBarStates[LED_BAR_COUNT] = {0};
void updateLedBars(void)
{
memset(ledBarStates, 0, sizeof(ledBarStates));
for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
int fn = ledGetFunction(ledConfig);
switch (fn) {
#ifdef USE_GPS
case LED_FUNCTION_GPS_BAR:
ledBarStates[LED_BAR_GPS]++;
break;
#endif
case LED_FUNCTION_BATTERY_BAR:
ledBarStates[LED_BAR_BATTERY]++;
break;
default:
break;
}
}
}
STATIC_UNIT_TESTED void updateLedCount(void)
{
int count = 0, countRing = 0, countScanner= 0;
for (int ledIndex = 0; ledIndex < LED_STRIP_MAX_LENGTH; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
if (!(*ledConfig))
break;
count++;
if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING)
countRing++;
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER))
countScanner++;
}
ledCounts.count = count;
ledCounts.ring = countRing;
ledCounts.larson = countScanner;
setUsedLedCount(ledCounts.count);
}
void reevaluateLedConfig(void)
{
updateLedCount();
updateDimensions();
updateLedRingCounts();
updateRequiredOverlay();
updateLedBars();
}
// get specialColor by index
static const hsvColor_t* getSC(ledSpecialColorIds_e index)
{
return &ledStripStatusModeConfig()->colors[ledStripStatusModeConfig()->specialColors.color[index]];
}
static const char directionCodes[LED_DIRECTION_COUNT] = {
[LED_DIRECTION_NORTH] = 'N',
[LED_DIRECTION_EAST] = 'E',
[LED_DIRECTION_SOUTH] = 'S',
[LED_DIRECTION_WEST] = 'W',
[LED_DIRECTION_UP] = 'U',
[LED_DIRECTION_DOWN] = 'D'
};
static const char baseFunctionCodes[LED_BASEFUNCTION_COUNT] = {
[LED_FUNCTION_COLOR] = 'C',
[LED_FUNCTION_FLIGHT_MODE] = 'F',
[LED_FUNCTION_ARM_STATE] = 'A',
[LED_FUNCTION_BATTERY] = 'L',
[LED_FUNCTION_RSSI] = 'S',
[LED_FUNCTION_GPS] = 'G',
[LED_FUNCTION_THRUST_RING] = 'R',
[LED_FUNCTION_GPS_BAR] = 'P',
[LED_FUNCTION_BATTERY_BAR] = 'E',
[LED_FUNCTION_ALTITUDE] = 'U'
};
static const char overlayCodes[LED_OVERLAY_COUNT] = {
[LED_OVERLAY_THROTTLE] = 'T',
[LED_OVERLAY_RAINBOW] = 'Y',
[LED_OVERLAY_LARSON_SCANNER] = 'O',
[LED_OVERLAY_BLINK] = 'B',
[LED_OVERLAY_VTX] = 'V',
[LED_OVERLAY_INDICATOR] = 'I',
[LED_OVERLAY_WARNING] = 'W'
};
#define CHUNK_BUFFER_SIZE 11
bool parseLedStripConfig(int ledIndex, const char *config)
{
if (ledIndex >= LED_STRIP_MAX_LENGTH)
return false;
enum parseState_e {
X_COORDINATE,
Y_COORDINATE,
DIRECTIONS,
FUNCTIONS,
RING_COLORS,
PARSE_STATE_COUNT
};
static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':', ':', '\0'};
ledConfig_t *ledConfig = &ledStripStatusModeConfigMutable()->ledConfigs[ledIndex];
memset(ledConfig, 0, sizeof(ledConfig_t));
int x = 0, y = 0, color = 0; // initialize to prevent warnings
int baseFunction = 0;
int overlay_flags = 0;
int direction_flags = 0;
for (enum parseState_e parseState = 0; parseState < PARSE_STATE_COUNT; parseState++) {
char chunk[CHUNK_BUFFER_SIZE];
{
char chunkSeparator = chunkSeparators[parseState];
int chunkIndex = 0;
while (*config && *config != chunkSeparator && chunkIndex < (CHUNK_BUFFER_SIZE - 1)) {
chunk[chunkIndex++] = *config++;
}
chunk[chunkIndex++] = 0; // zero-terminate chunk
if (*config != chunkSeparator) {
return false;
}
config++; // skip separator
}
switch (parseState) {
case X_COORDINATE:
x = atoi(chunk);
break;
case Y_COORDINATE:
y = atoi(chunk);
break;
case DIRECTIONS:
for (char *ch = chunk; *ch; ch++) {
for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) {
if (directionCodes[dir] == *ch) {
direction_flags |= LED_FLAG_DIRECTION(dir);
break;
}
}
}
break;
case FUNCTIONS:
for (char *ch = chunk; *ch; ch++) {
for (ledBaseFunctionId_e fn = 0; fn < LED_BASEFUNCTION_COUNT; fn++) {
if (baseFunctionCodes[fn] == *ch) {
baseFunction = fn;
break;
}
}
for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) {
if (overlayCodes[ol] == *ch) {
overlay_flags |= LED_FLAG_OVERLAY(ol);
break;
}
}
}
break;
case RING_COLORS:
color = atoi(chunk);
if (color >= LED_CONFIGURABLE_COLOR_COUNT)
color = 0;
break;
case PARSE_STATE_COUNT:; // prevent warning
}
}
*ledConfig = DEFINE_LED(x, y, color, direction_flags, baseFunction, overlay_flags);
reevaluateLedConfig();
return true;
}
void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t bufferSize)
{
char directions[LED_DIRECTION_COUNT + 1];
char baseFunctionOverlays[LED_OVERLAY_COUNT + 2];
memset(ledConfigBuffer, 0, bufferSize);
char *dptr = directions;
for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) {
if (ledGetDirectionBit(ledConfig, dir)) {
*dptr++ = directionCodes[dir];
}
}
*dptr = 0;
char *fptr = baseFunctionOverlays;
*fptr++ = baseFunctionCodes[ledGetFunction(ledConfig)];
for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) {
if (ledGetOverlayBit(ledConfig, ol)) {
*fptr++ = overlayCodes[ol];
}
}
*fptr = 0;
// TODO - check buffer length
tfp_sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig));
}
typedef enum {
// the ordering is important, see below how NSEW is mapped to NE/SE/NW/SW
QUADRANT_NORTH = 1 << 0,
QUADRANT_SOUTH = 1 << 1,
QUADRANT_EAST = 1 << 2,
QUADRANT_WEST = 1 << 3,
} quadrant_e;
static quadrant_e getLedQuadrant(const int ledIndex)
{
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
int x = ledGetX(ledConfig);
int y = ledGetY(ledConfig);
int quad = 0;
if (y <= highestYValueForNorth)
quad |= QUADRANT_NORTH;
else if (y >= lowestYValueForSouth)
quad |= QUADRANT_SOUTH;
if (x >= lowestXValueForEast)
quad |= QUADRANT_EAST;
else if (x <= highestXValueForWest)
quad |= QUADRANT_WEST;
return quad;
}
static const hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIndexes_t *modeColors)
{
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
const int ledDirection = ledGetDirection(ledConfig);
for (unsigned i = 0; i < LED_DIRECTION_COUNT; i++) {
if (ledDirection & (1 << i)) {
return &ledStripStatusModeConfig()->colors[modeColors->color[i]];
}
}
return NULL;
}
// map flight mode to led mode, in order of priority
// flightMode == 0 is always active
static const struct {
uint16_t flightMode;
uint8_t ledMode;
} flightModeToLed[] = {
{HEADFREE_MODE, LED_MODE_HEADFREE},
#ifdef USE_MAG
{MAG_MODE, LED_MODE_MAG},
#endif
{HORIZON_MODE, LED_MODE_HORIZON},
{ANGLE_MODE, LED_MODE_ANGLE},
{0, LED_MODE_ORIENTATION},
};
static void applyLedFixedLayers(void)
{
uint8_t ledBarCounters[LED_BAR_COUNT] = {0};
for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND);
int fn = ledGetFunction(ledConfig);
int hOffset = HSV_HUE_MAX + 1;
switch (fn) {
case LED_FUNCTION_COLOR:
color = ledStripStatusModeConfig()->colors[ledGetColor(ledConfig)];
hsvColor_t nextColor = ledStripStatusModeConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT];
hsvColor_t previousColor = ledStripStatusModeConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT];
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor
const int auxInput = rcData[ledStripStatusModeConfig()->ledstrip_aux_channel];
int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2;
if (auxInput < centerPWM) {
color.h = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.h, color.h);
color.s = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.s, color.s);
color.v = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.v, color.v);
} else {
color.h = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.h, nextColor.h);
color.s = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.s, nextColor.s);
color.v = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.v, nextColor.v);
}
}
break;
case LED_FUNCTION_FLIGHT_MODE:
for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++)
if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) {
const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripStatusModeConfig()->modeColors[flightModeToLed[i].ledMode]);
if (directionalColor) {
color = *directionalColor;
}
break; // stop on first match
}
break;
case LED_FUNCTION_ARM_STATE:
color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED);
break;
case LED_FUNCTION_BATTERY:
case LED_FUNCTION_BATTERY_BAR:
color = HSV(RED);
hOffset += MAX(scaleRange(calculateBatteryPercentageRemaining(), 0, 100, -30, 120), 0);
break;
#ifdef USE_GPS
case LED_FUNCTION_GPS_BAR:
{
uint8_t minSats = 8;
#ifdef USE_GPS_RESCUE
minSats = gpsRescueConfig()->minSats;
#endif
if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) {
color = HSV(RED);
} else {
if (gpsSol.numSat >= minSats) {
color = HSV(GREEN);
} else {
color = HSV(RED);
hOffset += MAX(scaleRange(gpsSol.numSat, 0, minSats, -30, 120), 0);
}
}
break;
}
#endif
#if defined(USE_BARO) || defined(USE_GPS)
case LED_FUNCTION_ALTITUDE:
color = ledStripStatusModeConfig()->colors[ledGetColor(ledConfig)];
hOffset += MAX(scaleRange(getEstimatedAltitudeCm(), 0, 500, -30, 120), 0);
break;
#endif
case LED_FUNCTION_RSSI:
color = HSV(RED);
hOffset += MAX(scaleRange(getRssiPercent(), 0, 100, -30, 120), 0);
break;
default:
break;
}
if ((fn != LED_FUNCTION_COLOR) && ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) {
const int auxInput = rcData[ledStripStatusModeConfig()->ledstrip_aux_channel];
hOffset += scaleRange(auxInput, PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1);
}
color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1);
switch (fn) {
#ifdef USE_GPS
case LED_FUNCTION_GPS_BAR:
if (ledBarCounters[LED_BAR_GPS] < gpsSol.numSat || ledBarCounters[LED_BAR_GPS] == 0) {
ledBarCounters[LED_BAR_GPS]++;
setLedHsv(ledIndex, &color);
} else {
setLedHsv(ledIndex, getSC(LED_SCOLOR_BACKGROUND));
}
break;
#endif
case LED_FUNCTION_BATTERY_BAR:
if (ledBarCounters[LED_BAR_BATTERY] < (calculateBatteryPercentageRemaining() * ledBarStates[LED_BAR_BATTERY]) / 100 || ledBarCounters[LED_BAR_BATTERY] == 0) {
ledBarCounters[LED_BAR_BATTERY]++;
setLedHsv(ledIndex, &color);
} else {
setLedHsv(ledIndex, getSC(LED_SCOLOR_BACKGROUND));
}
break;
default:
setLedHsv(ledIndex, &color);
break;
}
}
}
static void applyLedHsv(uint32_t mask, const hsvColor_t *color)
{
for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
if ((*ledConfig & mask) == mask)
setLedHsv(ledIndex, color);
}
}
typedef enum {
WARNING_ARMING_DISABLED,
WARNING_LOW_BATTERY,
WARNING_FAILSAFE,
WARNING_CRASHFLIP_ACTIVE,
} warningFlags_e;
static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
{
static uint8_t warningFlashCounter = 0;
static uint8_t warningFlags = 0; // non-zero during blinks
if (updateNow) {
// keep counter running, so it stays in sync with blink
warningFlashCounter++;
warningFlashCounter &= 0xF;
if (warningFlashCounter == 0) { // update when old flags was processed
warningFlags = 0;
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryState() != BATTERY_OK) {
warningFlags |= 1 << WARNING_LOW_BATTERY;
}
if (failsafeIsActive()) {
warningFlags |= 1 << WARNING_FAILSAFE;
}
if (!ARMING_FLAG(ARMED) && isArmingDisabled()) {
warningFlags |= 1 << WARNING_ARMING_DISABLED;
}
if (isCrashFlipModeActive()) {
warningFlags |= 1 << WARNING_CRASHFLIP_ACTIVE;
}
}
*timer += HZ_TO_US(LED_OVERLAY_WARNING_RATE_HZ);
}
const hsvColor_t *warningColor = NULL;
if (warningFlags) {
bool colorOn = (warningFlashCounter % 2) == 0; // w_w_
warningFlags_e warningId = warningFlashCounter / 4;
if (warningFlags & (1 << warningId)) {
switch (warningId) {
case WARNING_ARMING_DISABLED:
warningColor = colorOn ? &HSV(GREEN) : &HSV(BLACK);
break;
case WARNING_CRASHFLIP_ACTIVE:
warningColor = colorOn ? &HSV(MAGENTA) : &HSV(BLACK);
break;
case WARNING_LOW_BATTERY:
warningColor = colorOn ? &HSV(RED) : &HSV(BLACK);
break;
case WARNING_FAILSAFE:
warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE);
break;
default:;
}
}
} else {
if (isBeeperOn()) {
warningColor = &hsv[ledStripConfig()->ledstrip_visual_beeper_color];
}
}
if (warningColor) {
applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor);
}
}
#ifdef USE_VTX_COMMON
static const struct {
uint16_t freq_upper_limit;
uint8_t color_index;
} freq_to_color_lookup[] = {
{VTX_SETTINGS_MIN_FREQUENCY_MHZ, COLOR_BLACK}, // invalid
// Freqs are divided to match Raceband channels
{ 5672, COLOR_WHITE}, // R1
{ 5711, COLOR_RED}, // R2
{ 5750, COLOR_ORANGE}, // R3
{ 5789, COLOR_YELLOW}, // R4
{ 5829, COLOR_GREEN}, // R5
{ 5867, COLOR_BLUE}, // R6
{ 5906, COLOR_DARK_VIOLET}, // R7
{VTX_SETTINGS_MAX_FREQUENCY_MHZ, COLOR_DEEP_PINK}, // R8
};
static uint8_t getColorByVtxFrequency(const uint16_t freq)
{
for (unsigned iter = 0; iter < ARRAYLEN(freq_to_color_lookup); iter++) {
if (freq <= freq_to_color_lookup[iter].freq_upper_limit) {
return freq_to_color_lookup[iter].color_index;
}
}
return COLOR_BLACK; // invalid
}
static void applyLedVtxLayer(bool updateNow, timeUs_t *timer)
{
static uint16_t frequency = 0;
static uint8_t power = 255;
static unsigned vtxStatus = UINT32_MAX;
static uint8_t showSettings = false;
static uint16_t lastCheck = 0;
static bool blink = false;
const vtxDevice_t *vtxDevice = vtxCommonDevice();
if (!vtxDevice) {
return;
}
uint8_t band = 255, channel = 255;
if (updateNow) {
// keep counter running, so it stays in sync with vtx
vtxCommonGetBandAndChannel(vtxDevice, &band, &channel);
vtxCommonGetPowerIndex(vtxDevice, &power);
vtxCommonGetStatus(vtxDevice, &vtxStatus);
frequency = vtxCommonLookupFrequency(vtxDevice, band, channel);
// check if last vtx values have changed.
uint16_t check = ((vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0) + (power << 1) + (band << 4) + (channel << 8);
if (!showSettings && check != lastCheck) {
// display settings for 3 seconds.
showSettings = 15;
}
lastCheck = check; // quick way to check if any settings changed.
if (showSettings) {
showSettings--;
}
blink = !blink;
*timer += HZ_TO_US(LED_OVERLAY_VTX_RATE_HZ);
}
if (showSettings) { // show settings
uint8_t vtxLedCount = 0;
for (int i = 0; i < ledCounts.count && vtxLedCount < 6; ++i) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i];
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_VTX)) {
hsvColor_t color = {0, 0, 0};
if (vtxLedCount == 0) {
color.h = HSV(GREEN).h;
color.s = HSV(GREEN).s;
color.v = blink ? 15 : 0; // blink received settings
} else if (vtxLedCount > 0 && power >= vtxLedCount && !(vtxStatus & VTX_STATUS_PIT_MODE)) { // show power
color.h = HSV(ORANGE).h;
color.s = HSV(ORANGE).s;
color.v = blink ? 15 : 0; // blink received settings
} else { // turn rest off
color.h = HSV(BLACK).h;
color.s = HSV(BLACK).s;
color.v = HSV(BLACK).v;
}
setLedHsv(i, &color);
++vtxLedCount;
}
}
}
else { // show frequency
// calculate the VTX color based on frequency
uint8_t const colorIndex = getColorByVtxFrequency(frequency);
hsvColor_t color = ledStripStatusModeConfig()->colors[colorIndex];
color.v = (vtxStatus & VTX_STATUS_PIT_MODE) ? (blink ? 15 : 0) : 255; // blink when in pit mode
applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_VTX)), &color);
}
}
#endif
static void applyLedBatteryLayer(bool updateNow, timeUs_t *timer)
{
static bool flash = false;
int timerDelayUs = HZ_TO_US(1);
if (updateNow) {
switch (getBatteryState()) {
case BATTERY_OK:
flash = true;
timerDelayUs = HZ_TO_US(1);
break;
case BATTERY_WARNING:
flash = !flash;
timerDelayUs = HZ_TO_US(2);
break;
default:
flash = !flash;
timerDelayUs = HZ_TO_US(8);
break;
}
*timer += timerDelayUs;
}
if (!flash) {
const hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND);
applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc);
}
}
static void applyLedRssiLayer(bool updateNow, timeUs_t *timer)
{
static bool flash = false;
int timerDelay = HZ_TO_US(1);
if (updateNow) {
int state = getRssiPercent();
if (state > 50) {
flash = true;
timerDelay = HZ_TO_US(1);
} else if (state > 20) {
flash = !flash;
timerDelay = HZ_TO_US(2);
} else {
flash = !flash;
timerDelay = HZ_TO_US(8);
}
*timer += timerDelay;
}
if (!flash) {
const hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND);
applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc);
}
}
#ifdef USE_GPS
static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
{
static uint8_t gpsPauseCounter = 0;
const uint8_t blinkPauseLength = 4;
if (updateNow) {
static uint8_t gpsFlashCounter = 0;
if (gpsPauseCounter > 0) {
gpsPauseCounter--;
} else if (gpsFlashCounter >= gpsSol.numSat) {
gpsFlashCounter = 0;
gpsPauseCounter = blinkPauseLength;
} else {
gpsFlashCounter++;
gpsPauseCounter = 1;
}
*timer += HZ_TO_US(2.5f);
}
const hsvColor_t *gpsColor;
if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) {
gpsColor = getSC(LED_SCOLOR_GPSNOSATS);
} else {
bool colorOn = gpsPauseCounter == 0; // each interval starts with pause
if (STATE(GPS_FIX)) {
gpsColor = colorOn ? getSC(LED_SCOLOR_GPSLOCKED) : getSC(LED_SCOLOR_BACKGROUND);
} else {
gpsColor = colorOn ? getSC(LED_SCOLOR_GPSNOLOCK) : getSC(LED_SCOLOR_GPSNOSATS);
}
}
applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_GPS), gpsColor);
}
#endif
#define INDICATOR_DEADBAND 25
static void applyLedIndicatorLayer(bool updateNow, timeUs_t *timer)
{
static bool flash = 0;
if (updateNow) {
if (isRxReceivingSignal()) {
// calculate update frequency
int scale = MAX(fabsf(rcCommand[ROLL]), fabsf(rcCommand[PITCH])); // 0 - 500
scale = scale - INDICATOR_DEADBAND; // start increasing frequency right after deadband
*timer += HZ_TO_US(5 + (45 * scale) / (500 - INDICATOR_DEADBAND)); // 5 - 50Hz update, 2.5 - 25Hz blink
flash = !flash;
} else {
*timer += HZ_TO_US(LED_OVERLAY_INDICATOR_RATE_HZ);
}
}
if (!flash)
return;
const hsvColor_t *flashColor = &HSV(ORANGE); // TODO - use user color?
quadrant_e quadrants = 0;
if (rcCommand[ROLL] > INDICATOR_DEADBAND) {
quadrants |= QUADRANT_EAST;
} else if (rcCommand[ROLL] < -INDICATOR_DEADBAND) {
quadrants |= QUADRANT_WEST;
}
if (rcCommand[PITCH] > INDICATOR_DEADBAND) {
quadrants |= QUADRANT_NORTH;
} else if (rcCommand[PITCH] < -INDICATOR_DEADBAND) {
quadrants |= QUADRANT_SOUTH;
}
for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) {
if (getLedQuadrant(ledIndex) & quadrants)
setLedHsv(ledIndex, flashColor);
}
}
}
static void applyLedThrustRingLayer(bool updateNow, timeUs_t *timer)
{
static uint8_t rotationPhase;
int ledRingIndex = 0;
if (updateNow) {
rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1;
const int scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100) : 0;
*timer += HZ_TO_US(5 + (45 * scaledThrottle) / 100); // 5 - 50Hz update rate
}
for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) {
bool applyColor;
if (ARMING_FLAG(ARMED)) {
applyColor = (ledRingIndex + rotationPhase) % ledCounts.ringSeqLen < ROTATION_SEQUENCE_LED_WIDTH;
} else {
applyColor = !(ledRingIndex % 2); // alternating pattern
}
if (applyColor) {
const hsvColor_t *ringColor = &ledStripStatusModeConfig()->colors[ledGetColor(ledConfig)];
setLedHsv(ledIndex, ringColor);
}
ledRingIndex++;
}
}
}
static void applyRainbowLayer(bool updateNow, timeUs_t *timer)
{
//use offset as a fixed point number
static int offset = 0;
if (updateNow) {
offset += ledStripConfig()->ledstrip_rainbow_freq;
*timer += HZ_TO_US(LED_OVERLAY_RAINBOW_RATE_HZ);
}
uint8_t rainbowLedIndex = 0;
for (unsigned i = 0; i < ledCounts.count; i++) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i];
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_RAINBOW)) {
hsvColor_t ledColor;
ledColor.h = (offset / LED_OVERLAY_RAINBOW_RATE_HZ + rainbowLedIndex * ledStripConfig()->ledstrip_rainbow_delta) % (HSV_HUE_MAX + 1);
ledColor.s = 0;
ledColor.v = HSV_VALUE_MAX;
setLedHsv(i, &ledColor);
rainbowLedIndex++;
}
}
}
typedef struct larsonParameters_s {
uint8_t currentBrightness;
int8_t currentIndex;
int8_t direction;
} larsonParameters_t;
static int brightnessForLarsonIndex(larsonParameters_t *larsonParameters, uint8_t larsonIndex)
{
int offset = larsonIndex - larsonParameters->currentIndex;
static const int larsonLowValue = 8;
if (abs(offset) > 1)
return (larsonLowValue);
if (offset == 0)
return (larsonParameters->currentBrightness);
if (larsonParameters->direction == offset) {
return (larsonParameters->currentBrightness - 127);
}
return (255 - larsonParameters->currentBrightness);
}
static void larsonScannerNextStep(larsonParameters_t *larsonParameters, int delta)
{
if (larsonParameters->currentBrightness > (255 - delta)) {
larsonParameters->currentBrightness = 127;
if (larsonParameters->currentIndex >= ledCounts.larson || larsonParameters->currentIndex < 0) {
larsonParameters->direction = -larsonParameters->direction;
}
larsonParameters->currentIndex += larsonParameters->direction;
} else {
larsonParameters->currentBrightness += delta;
}
}
static void applyLarsonScannerLayer(bool updateNow, timeUs_t *timer)
{
static larsonParameters_t larsonParameters = { 0, 0, 1 };
if (updateNow) {
larsonScannerNextStep(&larsonParameters, 15);
*timer += HZ_TO_US(LED_OVERLAY_LARSON_RATE_HZ);
}
int scannerLedIndex = 0;
for (unsigned i = 0; i < ledCounts.count; i++) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i];
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) {
hsvColor_t ledColor;
getLedHsv(i, &ledColor);
ledColor.v = brightnessForLarsonIndex(&larsonParameters, scannerLedIndex);
setLedHsv(i, &ledColor);
scannerLedIndex++;
}
}
}
// blink twice, then wait ; either always or just when landing
static void applyLedBlinkLayer(bool updateNow, timeUs_t *timer)
{
const uint16_t blinkPattern = 0x8005; // 0b1000000000000101;
static uint16_t blinkMask;
if (updateNow) {
blinkMask = blinkMask >> 1;
if (blinkMask <= 1)
blinkMask = blinkPattern;
*timer += HZ_TO_US(LED_OVERLAY_BLINK_RATE_HZ);
}
bool ledOn = (blinkMask & 1); // b_b_____...
if (!ledOn) {
for (int i = 0; i < ledCounts.count; ++i) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[i];
if (ledGetOverlayBit(ledConfig, LED_OVERLAY_BLINK)) {
setLedHsv(i, getSC(LED_SCOLOR_BLINKBACKGROUND));
}
}
}
}
// In reverse order of priority
typedef enum {
timRainbow,
timBlink,
timLarson,
timRing,
timIndicator,
#ifdef USE_VTX_COMMON
timVtx,
#endif
#ifdef USE_GPS
timGps,
#endif
timBattery,
timRssi,
timWarning,
timTimerCount
} timId_e;
static timeUs_t timerVal[timTimerCount];
static uint16_t disabledTimerMask;
STATIC_ASSERT(timTimerCount <= sizeof(disabledTimerMask) * 8, disabledTimerMask_too_small);
// function to apply layer.
// function must replan self using timer pointer
// when updateNow is true (timer triggered), state must be updated first,
// before calculating led state. Otherwise update started by different trigger
// may modify LED state.
typedef void applyLayerFn_timed(bool updateNow, timeUs_t *timer);
static applyLayerFn_timed* layerTable[] = {
[timRainbow] = &applyRainbowLayer,
[timBlink] = &applyLedBlinkLayer,
[timLarson] = &applyLarsonScannerLayer,
[timBattery] = &applyLedBatteryLayer,
[timRssi] = &applyLedRssiLayer,
#ifdef USE_GPS
[timGps] = &applyLedGpsLayer,
#endif
[timWarning] = &applyLedWarningLayer,
#ifdef USE_VTX_COMMON
[timVtx] = &applyLedVtxLayer,
#endif
[timIndicator] = &applyLedIndicatorLayer,
[timRing] = &applyLedThrustRingLayer
};
bool isOverlayTypeUsed(ledOverlayId_e overlayType)
{
for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) {
const ledConfig_t *ledConfig = &ledStripStatusModeConfig()->ledConfigs[ledIndex];
if (ledGetOverlayBit(ledConfig, overlayType)) {
return true;
}
}
return false;
}
void updateRequiredOverlay(void)
{
disabledTimerMask = 0;
disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_RAINBOW) << timRainbow;
disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_BLINK) << timBlink;
disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_LARSON_SCANNER) << timLarson;
disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_WARNING) << timWarning;
#ifdef USE_VTX_COMMON
disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_VTX) << timVtx;
#endif
disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_INDICATOR) << timIndicator;
}
static ledProfileSequence_t applyStatusProfile(timeUs_t now)
{
static timId_e timId = 0;
static uint32_t timActive = 0;
static bool fixedLayersApplied = false;
timeUs_t startTime = micros();
if (!timActive) {
// apply all layers; triggered timed functions has to update timers
// test all led timers, setting corresponding bits
for (timId_e timId = 0; timId < timTimerCount; timId++) {
if (!(disabledTimerMask & (1 << timId))) {
// sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
const timeDelta_t delta = cmpTimeUs(now, timerVal[timId]);
// max delay is limited to 5s
if (delta > MAX_TIMER_DELAY) {
// Restart the interval on this timer; catches start condition following initialisation
timerVal[timId] = now;
}
if (delta >= 0) {
timActive |= 1 << timId;
}
}
}
if (!timActive) {
return LED_PROFILE_SLOW; // no change this update, keep old state
}
}
if (!fixedLayersApplied) {
applyLedFixedLayers();
fixedLayersApplied = true;
}
for (; timId < ARRAYLEN(layerTable); timId++) {
timeUs_t *timer = &timerVal[timId];
bool updateNow = timActive & (1 << timId);
(*layerTable[timId])(updateNow, timer);
if (cmpTimeUs(micros(), startTime) > LED_TARGET_UPDATE_US) {
// Come back and complete this quickly
timId++;
return LED_PROFILE_FAST;
}
}
// Reset state for next iteration
timActive = 0;
fixedLayersApplied = false;
timId = 0;
return LED_PROFILE_ADVANCE;
}
bool parseColor(int index, const char *colorConfig)
{
const char *remainingCharacters = colorConfig;
hsvColor_t *color = &ledStripStatusModeConfigMutable()->colors[index];
bool result = true;
static const uint16_t hsv_limit[HSV_COLOR_COMPONENT_COUNT] = {
[HSV_HUE] = HSV_HUE_MAX,
[HSV_SATURATION] = HSV_SATURATION_MAX,
[HSV_VALUE] = HSV_VALUE_MAX,
};
for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) {
int val = atoi(remainingCharacters);
if (val > hsv_limit[componentIndex]) {
result = false;
break;
}
switch (componentIndex) {
case HSV_HUE:
color->h = val;
break;
case HSV_SATURATION:
color->s = val;
break;
case HSV_VALUE:
color->v = val;
break;
}
remainingCharacters = strchr(remainingCharacters, ',');
if (remainingCharacters) {
remainingCharacters++; // skip separator
} else {
if (componentIndex < HSV_COLOR_COMPONENT_COUNT - 1) {
result = false;
}
}
}
if (!result) {
memset(color, 0, sizeof(*color));
}
return result;
}
/*
* Redefine a color in a mode.
* */
bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex)
{
// check color
if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT)
return false;
if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough
if (modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT)
return false;
ledStripStatusModeConfigMutable()->modeColors[modeIndex].color[modeColorIndex] = colorIndex;
} else if (modeIndex == LED_SPECIAL) {
if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT)
return false;
ledStripStatusModeConfigMutable()->specialColors.color[modeColorIndex] = colorIndex;
} else if (modeIndex == LED_AUX_CHANNEL) {
if (modeColorIndex < 0 || modeColorIndex >= 1)
return false;
ledStripStatusModeConfigMutable()->ledstrip_aux_channel = colorIndex;
} else {
return false;
}
return true;
}
#endif
void ledStripEnable(void)
{
ws2811LedStripEnable();
ledStripEnabled = true;
}
void ledStripDisable(void)
{
if (ledStripEnabled) {
ledStripEnabled = false;
previousProfileColorIndex = COLOR_UNDEFINED;
setStripColor(&HSV(BLACK));
// Multiple calls may be required as normally broken into multiple parts
while (!ws2811UpdateStrip((ledStripFormatRGB_e)ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness));
}
}
void ledStripInit(void)
{
#if defined(USE_LED_STRIP_STATUS_MODE)
colors = ledStripStatusModeConfig()->colors;
modeColors = ledStripStatusModeConfig()->modeColors;
specialColors = ledStripStatusModeConfig()->specialColors;
reevaluateLedConfig();
#endif
ws2811LedStripInit(ledStripConfig()->ioTag);
}
static uint8_t selectVisualBeeperColor(uint8_t colorIndex, bool *colorIndexIsCustom)
{
if (ledStripConfig()->ledstrip_visual_beeper && isBeeperOn()) {
if (colorIndexIsCustom)
*colorIndexIsCustom = false;
return ledStripConfig()->ledstrip_visual_beeper_color;
} else {
return colorIndex;
}
}
static ledProfileSequence_t applySimpleProfile(timeUs_t currentTimeUs)
{
static timeUs_t colorUpdateTimeUs = 0;
uint8_t colorIndex = COLOR_BLACK;
bool blinkLed = false;
bool visualBeeperOverride = true;
bool useCustomColors = false;
unsigned flashPeriod;
unsigned onPercent;
if (IS_RC_MODE_ACTIVE(BOXBEEPERON) || failsafeIsActive()) {
// RX_SET or failsafe - force the beacon on and override the profile settings
blinkLed = true;
visualBeeperOverride = false; // prevent the visual beeper from interfering
flashPeriod = BEACON_FAILSAFE_PERIOD_US;
onPercent = BEACON_FAILSAFE_ON_PERCENT;
colorIndex = ledStripConfig()->ledstrip_visual_beeper_color;
} else {
switch (ledStripConfig()->ledstrip_profile) {
case LED_PROFILE_RACE:
colorIndex = ledStripConfig()->ledstrip_race_color;
#ifdef USE_VTX_COMMON
if (colorIndex == COLOR_BLACK) {
// ledstrip_race_color is not set. Set color based on VTX frequency
const vtxDevice_t *vtxDevice = vtxCommonDevice();
if (vtxDevice) {
uint16_t freq;
uint8_t const band = vtxSettingsConfigMutable()->band;
uint8_t const channel = vtxSettingsConfig()->channel;
if (band && channel) {
freq = vtxCommonLookupFrequency(vtxDevice, band, channel);
} else {
// Direct frequency is used
freq = vtxSettingsConfig()->freq;
}
colorIndex = getColorByVtxFrequency(freq);
// getColorByVtxFrequency always uses custom colors
// as they may be reassigned by the race director
useCustomColors = true;
}
}
#endif
break;
case LED_PROFILE_BEACON: {
if (!ledStripConfig()->ledstrip_beacon_armed_only || ARMING_FLAG(ARMED)) {
flashPeriod = ledStripConfig()->ledstrip_beacon_period_ms;
onPercent = ledStripConfig()->ledstrip_beacon_percent;
colorIndex = ledStripConfig()->ledstrip_beacon_color;
blinkLed = true;
}
break;
}
default:
break;
}
}
if (blinkLed) {
const unsigned onPeriod = flashPeriod * onPercent / 100;
const bool beaconState = (millis() % flashPeriod) < onPeriod;
colorIndex = (beaconState) ? colorIndex : COLOR_BLACK;
}
if (visualBeeperOverride) {
colorIndex = selectVisualBeeperColor(colorIndex, &useCustomColors);
}
if ((colorIndex != previousProfileColorIndex) || (currentTimeUs >= colorUpdateTimeUs)) {
setStripColor((useCustomColors) ? &ledStripStatusModeConfig()->colors[colorIndex] : &hsv[colorIndex]);
previousProfileColorIndex = colorIndex;
colorUpdateTimeUs = currentTimeUs + PROFILE_COLOR_UPDATE_INTERVAL_US;
return LED_PROFILE_ADVANCE;
}
return LED_PROFILE_SLOW;
}
timeUs_t executeTimeUs;
void ledStripUpdate(timeUs_t currentTimeUs)
{
static uint16_t ledStateDurationFractionUs[2] = { 0 };
static bool applyProfile = true;
static timeUs_t updateStartTimeUs = 0;
bool ledCurrentState = applyProfile;
if (updateStartTimeUs != 0) {
// The LED task rate is considered to be the rate at which updates are sent to the LEDs as a consequence
// of the layer timers firing
schedulerIgnoreTaskExecRate();
}
if (!isWS2811LedStripReady()) {
// Call schedulerIgnoreTaskExecTime() unless data is being processed
schedulerIgnoreTaskExecTime();
return;
}
if (ledStripEnabled && IS_RC_MODE_ACTIVE(BOXLEDLOW)) {
ledStripDisable();
} else if (!IS_RC_MODE_ACTIVE(BOXLEDLOW)) {
ledStripEnable();
}
if (ledStripEnabled) {
if (applyProfile) {
ledProfileSequence_t ledProfileSequence = LED_PROFILE_SLOW;
if (updateStartTimeUs == 0) {
updateStartTimeUs = currentTimeUs;
}
switch (ledStripConfig()->ledstrip_profile) {
#ifdef USE_LED_STRIP_STATUS_MODE
case LED_PROFILE_STATUS: {
ledProfileSequence = applyStatusProfile(currentTimeUs);
break;
}
#endif
case LED_PROFILE_RACE:
case LED_PROFILE_BEACON: {
ledProfileSequence = applySimpleProfile(currentTimeUs);
break;
}
default:
break;
}
if (ledProfileSequence == LED_PROFILE_SLOW) {
// No timer was ready so no work was done
schedulerIgnoreTaskExecTime();
// Reschedule waiting for a timer to trigger a LED state change
rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_LEDSTRIP_RATE_WAIT_HZ));
} else {
static bool multipassProfile = false;
if (ledProfileSequence == LED_PROFILE_ADVANCE) {
// The state leading to advancing from applying the profile layers to updating the DMA buffer is always short
if (multipassProfile) {
schedulerIgnoreTaskExecTime();
multipassProfile = false;
}
// The profile is now fully applied
applyProfile = false;
} else {
multipassProfile = true;
}
// Reschedule for a fast period to update the DMA buffer
rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_LEDSTRIP_RATE_FAST_HZ));
}
} else {
static bool multipassUpdate = false;
// Profile is applied, so now update the LEDs
if (ws2811UpdateStrip((ledStripFormatRGB_e) ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness)) {
// Final pass updating the DMA buffer is always short
if (multipassUpdate) {
schedulerIgnoreTaskExecTime();
multipassUpdate = false;
}
applyProfile = true;
timeDelta_t lastUpdateDurationUs = cmpTimeUs(currentTimeUs, updateStartTimeUs);
lastUpdateDurationUs %= TASK_PERIOD_HZ(TASK_LEDSTRIP_RATE_HZ);
rescheduleTask(TASK_SELF, cmpTimeUs(TASK_PERIOD_HZ(TASK_LEDSTRIP_RATE_HZ), lastUpdateDurationUs));
updateStartTimeUs = 0;
} else {
multipassUpdate = true;
}
}
}
if (!schedulerGetIgnoreTaskExecTime()) {
executeTimeUs = micros() - currentTimeUs;
if (executeTimeUs > (ledStateDurationFractionUs[ledCurrentState] >> LED_EXEC_TIME_SHIFT)) {
ledStateDurationFractionUs[ledCurrentState] = executeTimeUs << LED_EXEC_TIME_SHIFT;
} else if (ledStateDurationFractionUs[ledCurrentState] > 0) {
// Slowly decay the max time
ledStateDurationFractionUs[ledCurrentState]--;
}
}
schedulerSetNextStateTime((ledStateDurationFractionUs[applyProfile] >> LED_EXEC_TIME_SHIFT) + LED_TASK_MARGIN);
}
uint8_t getLedProfile(void)
{
return ledStripConfig()->ledstrip_profile;
}
void setLedProfile(uint8_t profile)
{
if (profile < LED_PROFILE_COUNT) {
ledStripConfigMutable()->ledstrip_profile = profile;
}
}
#endif
Reference in a new issue View git blame Copy permalink