static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs) { uint32_t startTime = 0; if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} // PID - note this is function pointer set by setPIDController() pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs); DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime); #ifdef USE_RUNAWAY_TAKEOFF // Check to see if runaway takeoff detection is active (anti-taz), the pidSum is over the threshold, // and gyro rate for any axis is above the limit for at least the activate delay period. // If so, disarm for safety if (ARMING_FLAG(ARMED) && !STATE(FIXED_WING) && pidConfig()->runaway_takeoff_prevention && !runawayTakeoffCheckDisabled && !flipOverAfterCrashMode && !runawayTakeoffTemporarilyDisabled && (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) { if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) || (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)) && ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) || (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) || (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) { if (runawayTakeoffTriggerUs == 0) { runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY; } else if (currentTimeUs > runawayTakeoffTriggerUs) { setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF); disarm(); } } else { runawayTakeoffTriggerUs = 0; } DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_TRUE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, runawayTakeoffTriggerUs == 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE : DEBUG_RUNAWAY_TAKEOFF_TRUE); } else { runawayTakeoffTriggerUs = 0; DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); } #endif #ifdef USE_PID_AUDIO if (isModeActivationConditionPresent(BOXPIDAUDIO)) { pidAudioUpdate(); } #endif }
/* * Called periodically by the scheduler */ void osdUpdate(timeUs_t currentTimeUs) { static uint32_t counter = 0; if (isBeeperOn()) { showVisualBeeper = true; } #ifdef MAX7456_DMA_CHANNEL_TX // don't touch buffers if DMA transaction is in progress if (displayIsTransferInProgress(osdDisplayPort)) { return; } #endif // MAX7456_DMA_CHANNEL_TX #ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED static uint32_t idlecounter = 0; if (!ARMING_FLAG(ARMED)) { if (idlecounter++ % 4 != 0) { return; } } #endif // redraw values in buffer #ifdef USE_MAX7456 #define DRAW_FREQ_DENOM 5 #else #define DRAW_FREQ_DENOM 10 // MWOSD @ 115200 baud ( #endif #define STATS_FREQ_DENOM 50 if (counter % DRAW_FREQ_DENOM == 0) { osdRefresh(currentTimeUs); showVisualBeeper = false; } else { // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle displayDrawScreen(osdDisplayPort); } ++counter; #ifdef USE_CMS // do not allow ARM if we are in menu if (displayIsGrabbed(osdDisplayPort)) { setArmingDisabled(ARMING_DISABLED_OSD_MENU); } else { unsetArmingDisabled(ARMING_DISABLED_OSD_MENU); } #endif }
void updateArmingStatus(void) { if (ARMING_FLAG(ARMED)) { LED0_ON; } else { // Check if the power on arming grace time has elapsed if ((getArmingDisableFlags() & ARMING_DISABLED_BOOT_GRACE_TIME) && (millis() >= systemConfig()->powerOnArmingGraceTime * 1000)) { // If so, unset the grace time arming disable flag unsetArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME); } // Clear the crash flip active status flipOverAfterCrashMode = false; // If switch is used for arming then check it is not defaulting to on when the RX link recovers from a fault if (!isUsingSticksForArming()) { static bool hadRx = false; const bool haveRx = rxIsReceivingSignal(); const bool justGotRxBack = !hadRx && haveRx; if (justGotRxBack && IS_RC_MODE_ACTIVE(BOXARM)) { // If the RX has just started to receive a signal again and the arm switch is on, apply arming restriction setArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY); } else if (haveRx && !IS_RC_MODE_ACTIVE(BOXARM)) { // If RX signal is OK and the arm switch is off, remove arming restriction unsetArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY); } hadRx = haveRx; } if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE); } else { unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE); } if (calculateThrottleStatus() != THROTTLE_LOW) { setArmingDisabled(ARMING_DISABLED_THROTTLE); } else { unsetArmingDisabled(ARMING_DISABLED_THROTTLE); } if (!STATE(SMALL_ANGLE) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { setArmingDisabled(ARMING_DISABLED_ANGLE); } else { unsetArmingDisabled(ARMING_DISABLED_ANGLE); } if (averageSystemLoadPercent > 100) { setArmingDisabled(ARMING_DISABLED_LOAD); } else { unsetArmingDisabled(ARMING_DISABLED_LOAD); } if (isCalibrating()) { setArmingDisabled(ARMING_DISABLED_CALIBRATING); } else { unsetArmingDisabled(ARMING_DISABLED_CALIBRATING); } if (isModeActivationConditionPresent(BOXPREARM)) { if (IS_RC_MODE_ACTIVE(BOXPREARM) && !ARMING_FLAG(WAS_ARMED_WITH_PREARM)) { unsetArmingDisabled(ARMING_DISABLED_NOPREARM); } else { setArmingDisabled(ARMING_DISABLED_NOPREARM); } } #ifdef USE_GPS_RESCUE if (isModeActivationConditionPresent(BOXGPSRESCUE)) { if (!gpsRescueConfig()->minSats || STATE(GPS_FIX_HOME) || ARMING_FLAG(WAS_EVER_ARMED)) { unsetArmingDisabled(ARMING_DISABLED_GPS); } else { setArmingDisabled(ARMING_DISABLED_GPS); } } #endif if (IS_RC_MODE_ACTIVE(BOXPARALYZE)) { setArmingDisabled(ARMING_DISABLED_PARALYZE); } if (!isUsingSticksForArming()) { /* Ignore ARMING_DISABLED_CALIBRATING if we are going to calibrate gyro on first arm */ bool ignoreGyro = armingConfig()->gyro_cal_on_first_arm && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_CALIBRATING)); /* Ignore ARMING_DISABLED_THROTTLE (once arm switch is on) if we are in 3D mode */ bool ignoreThrottle = feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3D) && !flight3DConfig()->switched_mode3d && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE)); #ifdef USE_RUNAWAY_TAKEOFF if (!IS_RC_MODE_ACTIVE(BOXARM)) { unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF); } #endif // If arming is disabled and the ARM switch is on if (isArmingDisabled() && !ignoreGyro && !ignoreThrottle && IS_RC_MODE_ACTIVE(BOXARM)) { setArmingDisabled(ARMING_DISABLED_ARM_SWITCH); } else if (!IS_RC_MODE_ACTIVE(BOXARM)) { unsetArmingDisabled(ARMING_DISABLED_ARM_SWITCH); } } if (isArmingDisabled()) { warningLedFlash(); } else { warningLedDisable(); } warningLedUpdate(); } }