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(); }