void applyLedWarningLayer(uint8_t updateNow)
{
const ledConfig_t *ledConfig;
uint8_t ledIndex;
static uint8_t warningFlashCounter = 0;
if (updateNow && warningFlashCounter == 0) {
warningFlags = WARNING_FLAG_NONE;
if (feature(FEATURE_VBAT) && shouldSoundBatteryAlarm()) {
warningFlags |= WARNING_FLAG_LOW_BATTERY;
}
if (failsafe->vTable->hasTimerElapsed()) {
warningFlags |= WARNING_FLAG_FAILSAFE;
}
if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) {
warningFlags |= WARNING_FLAG_ARMING_DISABLED;
}
}
if (warningFlags || warningFlashCounter > 0) {
const hsvColor_t *warningColor = &hsv_black;
if ((warningFlashCounter & 1) == 0) {
if (warningFlashCounter < 4 && (warningFlags & WARNING_FLAG_ARMING_DISABLED)) {
warningColor = &hsv_green;
}
if (warningFlashCounter >= 4 && warningFlashCounter < 12 && (warningFlags & WARNING_FLAG_LOW_BATTERY)) {
warningColor = &hsv_red;
}
if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) {
warningColor = &hsv_yellow;
}
} else {
if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) {
warningColor = &hsv_blue;
}
}
for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {
ledConfig = &ledConfigs[ledIndex];
if (!(ledConfig->flags & LED_FUNCTION_WARNING)) {
continue;
}
setLedHsv(ledIndex, warningColor);
}
}
if (updateNow && (warningFlags || warningFlashCounter)) {
warningFlashCounter++;
if (warningFlashCounter == 20) {
warningFlashCounter = 0;
}
}
}
void annexCode(void)
{
int32_t tmp, tmp2;
int32_t axis, prop1, prop2;
static uint8_t batteryWarningEnabled = false;
static uint8_t vbatTimer = 0;
static int32_t vbatCycleTime = 0;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < currentProfile.tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentProfile.dynThrPID * (rcData[THROTTLE] - currentProfile.tpa_breakpoint) / (2000 - currentProfile.tpa_breakpoint);
} else {
prop2 = 100 - currentProfile.dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (currentProfile.deadband) {
if (tmp > currentProfile.deadband) {
tmp -= currentProfile.deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
}
if (axis == YAW) {
if (currentProfile.yaw_deadband) {
if (tmp > currentProfile.yaw_deadband) {
tmp -= currentProfile.yaw_deadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500;
}
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)currentProfile.pidProfile.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)currentProfile.pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile.pidProfile.D8[axis] * prop1 / 100;
if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (f.HEADFREE_MODE) {
float radDiff = degreesToRadians(heading - headFreeModeHold);
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) {
vbatCycleTime += cycleTime;
if (!(++vbatTimer % VBATFREQ)) {
if (feature(FEATURE_VBAT)) {
updateBatteryVoltage();
batteryWarningEnabled = shouldSoundBatteryAlarm();
}
if (feature(FEATURE_CURRENT_METER)) {
updateCurrentMeter(vbatCycleTime);
}
vbatCycleTime = 0;
}
}
beepcodeUpdateState(batteryWarningEnabled);
if (f.ARMED) {
LED0_ON;
} else {
if (isCalibrating()) {
LED0_TOGGLE;
f.OK_TO_ARM = 0;
}
f.OK_TO_ARM = 1;
if (!f.SMALL_ANGLE) {
f.OK_TO_ARM = 0;
}
if (rcOptions[BOXAUTOTUNE]) {
f.OK_TO_ARM = 0;
}
if (f.OK_TO_ARM) {
disableWarningLed();
} else {
enableWarningLed(currentTime);
}
updateWarningLed(currentTime);
}
#ifdef TELEMETRY
checkTelemetryState();
#endif
handleSerial();
#ifdef GPS
if (sensors(SENSOR_GPS)) {
updateGpsIndicator(currentTime);
}
#endif
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
}
void updateLedStrip(void)
{
if (!(ledStripInitialised && isWS2811LedStripReady())) {
return;
}
uint32_t now = micros();
bool animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L;
bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L;
bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L;
if (!(warningFlashNow || indicatorFlashNow || animationUpdateNow)) {
return;
}
static uint8_t indicatorFlashState = 0;
static uint8_t warningState = 0;
static uint8_t warningFlags;
// LAYER 1
applyLedModeLayer();
applyLedThrottleLayer();
// LAYER 2
if (warningFlashNow) {
nextWarningFlashAt = now + LED_STRIP_10HZ;
if (warningState == 0) {
warningState = 1;
warningFlags = WARNING_FLAG_NONE;
if (feature(FEATURE_VBAT) && shouldSoundBatteryAlarm()) {
warningFlags |= WARNING_FLAG_LOW_BATTERY;
}
if (failsafe->vTable->hasTimerElapsed()) {
warningFlags |= WARNING_FLAG_FAILSAFE;
}
} else {
warningState = 0;
}
}
if (warningFlags) {
applyLedWarningLayer(warningState, warningFlags);
}
// 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;
}
}
applyLedIndicatorLayer(indicatorFlashState);
if (animationUpdateNow) {
nextAnimationUpdateAt = now + LED_STRIP_20HZ;
updateLedAnimationState();
}
#ifdef USE_LED_ANIMATION
applyLedAnimationLayer();
#endif
ws2811UpdateStrip();
}
