const uint32_t taskCount = TASK_COUNT; cfTask_t* taskQueueArray[TASK_QUEUE_ARRAY_SIZE]; cfTask_t cfTasks[] = { [TASK_SYSTEM] = { .taskName = "SYSTEM", .taskFunc = taskSystem, .desiredPeriod = TASK_PERIOD_MS(100), .staticPriority = TASK_PRIORITY_HIGH, }, [TASK_GYRO] = { .taskName = "GYRO", .checkFunc = taskGyroCheck, .taskFunc = taskGyro, .desiredPeriod = TASK_PERIOD_HZ(8000), .staticPriority = TASK_PRIORITY_REALTIME, }, [TASK_PID] = { .taskName = "PID", .checkFunc = taskPidCheck, .taskFunc = taskPid, .desiredPeriod = TASK_PERIOD_HZ(8000), .staticPriority = TASK_PRIORITY_REALTIME, }, [TASK_ACCEL] = { .taskName = "ACCEL", .taskFunc = taskUpdateAccelerometer, .desiredPeriod = TASK_PERIOD_MS(1), .staticPriority = TASK_PRIORITY_MEDIUM,
void fcTasksInit(void) { schedulerInit(); #ifdef ASYNC_GYRO_PROCESSING rescheduleTask(TASK_PID, getPidUpdateRate()); setTaskEnabled(TASK_PID, true); if (getAsyncMode() != ASYNC_MODE_NONE) { rescheduleTask(TASK_GYRO, getGyroUpdateRate()); setTaskEnabled(TASK_GYRO, true); } if (getAsyncMode() == ASYNC_MODE_ALL && sensors(SENSOR_ACC)) { rescheduleTask(TASK_ACC, getAccUpdateRate()); setTaskEnabled(TASK_ACC, true); rescheduleTask(TASK_ATTI, getAttitudeUpdateRate()); setTaskEnabled(TASK_ATTI, true); } #else rescheduleTask(TASK_GYROPID, gyro.targetLooptime); setTaskEnabled(TASK_GYROPID, true); #endif setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40)); #endif #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #ifdef PITOT setTaskEnabled(TASK_PITOT, sensors(SENSOR_PITOT)); #endif #ifdef SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif #ifdef USE_DASHBOARD setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif #ifdef STACK_CHECK setTaskEnabled(TASK_STACK_CHECK, true); #endif #ifdef USE_PMW_SERVO_DRIVER setTaskEnabled(TASK_PWMDRIVER, feature(FEATURE_PWM_SERVO_DRIVER)); #endif #ifdef OSD setTaskEnabled(TASK_OSD, feature(FEATURE_OSD)); #endif #ifdef CMS #ifdef USE_MSP_DISPLAYPORT setTaskEnabled(TASK_CMS, true); #else setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD)); #endif #endif }
setTaskEnabled(TASK_OSD, feature(FEATURE_OSD)); #endif #ifdef CMS #ifdef USE_MSP_DISPLAYPORT setTaskEnabled(TASK_CMS, true); #else setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD)); #endif #endif } cfTask_t cfTasks[TASK_COUNT] = { [TASK_SYSTEM] = { .taskName = "SYSTEM", .taskFunc = taskSystem, .desiredPeriod = TASK_PERIOD_HZ(10), // run every 100 ms, 10Hz .staticPriority = TASK_PRIORITY_HIGH, }, #ifdef ASYNC_GYRO_PROCESSING [TASK_PID] = { .taskName = "PID", .taskFunc = taskMainPidLoop, .desiredPeriod = TASK_PERIOD_HZ(500), // Run at 500Hz .staticPriority = TASK_PRIORITY_HIGH, }, [TASK_GYRO] = { .taskName = "GYRO", .taskFunc = taskGyro, .desiredPeriod = TASK_PERIOD_HZ(1000), //Run at 1000Hz
/* * processRx called from taskUpdateRxMain */ bool processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { return false; } // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) disarm(); } updateRSSI(currentTimeUs); if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); const throttleStatus_e throttleStatus = calculateThrottleStatus(); const uint8_t throttlePercent = calculateThrottlePercent(); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (throttlePercent >= rxConfig()->airModeActivateThreshold) { airmodeIsActivated = true; // Prevent Iterm from being reset } } else { airmodeIsActivated = false; } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetITerm(); if (currentPidProfile->pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); } else { pidStabilisationState(PID_STABILISATION_ON); } #ifdef USE_RUNAWAY_TAKEOFF // If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle // is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected // to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low. // Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable // prevention for the remainder of the battery. if (ARMING_FLAG(ARMED) && pidConfig()->runaway_takeoff_prevention && !runawayTakeoffCheckDisabled && !flipOverAfterCrashMode && !runawayTakeoffTemporarilyDisabled && !STATE(FIXED_WING)) { // Determine if we're in "flight" // - motors running // - throttle over runaway_takeoff_deactivate_throttle_percent // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit bool inStableFlight = false; if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running? const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) && (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) { inStableFlight = true; if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } } } // If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds if (inStableFlight) { if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay; // at high throttle levels reduce deactivation delay by 50% if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) { deactivateDelay = deactivateDelay / 2; } if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) { runawayTakeoffCheckDisabled = true; } } else { if (runawayTakeoffDeactivateUs != 0) { runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs); } runawayTakeoffDeactivateUs = 0; } if (runawayTakeoffDeactivateUs == 0) { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000); } else { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000); } } else { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE); } #endif // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && !feature(FEATURE_3D) && !isAirmodeActive() ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (armingConfig()->auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over disarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed beeper(BEEPER_ARMED); armedBeeperOn = true; } } else { // throttle is not low if (armingConfig()->auto_disarm_delay != 0) { // extend disarm time disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; } if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } else { // arming is via AUX switch; beep while throttle low if (throttleStatus == THROTTLE_LOW) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } processRcStickPositions(); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(); #ifdef USE_DSHOT /* Enable beep warning when the crash flip mode is active */ if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { beeper(BEEPER_CRASH_FLIP_MODE); } #endif if (!cliMode) { updateAdjustmentStates(); processRcAdjustments(currentControlRateProfile); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } #ifdef USE_GPS_RESCUE if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) { if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } } else { DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } #endif if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; // increase frequency of attitude task to reduce drift when in angle or horizon mode rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500)); } else { LED1_OFF; rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100)); } if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) { DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM); } #if defined(USE_ACC) || defined(USE_MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { #if defined(USE_GPS) || defined(USE_MAG) if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } #endif if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { if (imuQuaternionHeadfreeOffsetSet()){ beeper(BEEPER_RX_SET); } } } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef USE_TELEMETRY static bool sharedPortTelemetryEnabled = false; if (feature(FEATURE_TELEMETRY)) { bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY)); if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) { mspSerialReleaseSharedTelemetryPorts(); telemetryCheckState(); sharedPortTelemetryEnabled = true; } else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspSerialAllocatePorts(); sharedPortTelemetryEnabled = false; } } #endif #ifdef USE_VTX_CONTROL vtxUpdateActivatedChannel(); if (canUpdateVTX()) { handleVTXControlButton(); } #endif #ifdef USE_ACRO_TRAINER pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC)); #endif // USE_ACRO_TRAINER #ifdef USE_RC_SMOOTHING_FILTER if (ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) { beeper(BEEPER_RC_SMOOTHING_INIT_FAIL); } #endif pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); return true; }
void fcTasksInit(void) { schedulerInit(); if (sensors(SENSOR_GYRO)) { rescheduleTask(TASK_GYROPID, gyro.targetLooptime); setTaskEnabled(TASK_GYROPID, true); } if (sensors(SENSOR_ACC)) { setTaskEnabled(TASK_ACCEL, true); rescheduleTask(TASK_ACCEL, acc.accSamplingInterval); } setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); setTaskEnabled(TASK_SERIAL, true); rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz)); bool useBatteryVoltage = batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE; setTaskEnabled(TASK_BATTERY_VOLTAGE, useBatteryVoltage); bool useBatteryCurrent = batteryConfig()->currentMeterSource != CURRENT_METER_NONE; setTaskEnabled(TASK_BATTERY_CURRENT, useBatteryCurrent); bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts || feature(FEATURE_OSD); setTaskEnabled(TASK_BATTERY_ALERTS, (useBatteryVoltage || useBatteryCurrent) && useBatteryAlerts); setTaskEnabled(TASK_RX, true); setTaskEnabled(TASK_DISPATCH, dispatchIsEnabled()); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #if defined(USE_SPI) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40)); #endif #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #ifdef SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif #if defined(BARO) || defined(SONAR) setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif #ifdef USE_DASHBOARD setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); if (feature(FEATURE_TELEMETRY)) { if (rxConfig()->serialrx_provider == SERIALRX_JETIEXBUS) { // Reschedule telemetry to 500hz for Jeti Exbus rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500)); } else if (rxConfig()->serialrx_provider == SERIALRX_CRSF) { // Reschedule telemetry to 500hz, 2ms for CRSF rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500)); } } #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif #ifdef TRANSPONDER setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif #ifdef OSD setTaskEnabled(TASK_OSD, feature(FEATURE_OSD)); #endif #ifdef USE_OSD_SLAVE setTaskEnabled(TASK_OSD_SLAVE, true); #endif #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif #ifdef USE_ESC_SENSOR setTaskEnabled(TASK_ESC_SENSOR, feature(FEATURE_ESC_SENSOR)); #endif #ifdef CMS #ifdef USE_MSP_DISPLAYPORT setTaskEnabled(TASK_CMS, true); #else setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD)); #endif #endif #ifdef STACK_CHECK setTaskEnabled(TASK_STACK_CHECK, true); #endif #ifdef VTX_CONTROL #if defined(VTX_RTC6705) || defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif }
#ifdef STACK_CHECK setTaskEnabled(TASK_STACK_CHECK, true); #endif #ifdef VTX_CONTROL #if defined(VTX_RTC6705) || defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif } #endif cfTask_t cfTasks[TASK_COUNT] = { [TASK_SYSTEM] = { .taskName = "SYSTEM", .taskFunc = taskSystem, .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz, every 100 ms .staticPriority = TASK_PRIORITY_MEDIUM_HIGH, }, #ifndef USE_OSD_SLAVE [TASK_GYROPID] = { .taskName = "PID", .subTaskName = "GYRO", .taskFunc = taskMainPidLoop, .desiredPeriod = TASK_GYROPID_DESIRED_PERIOD, .staticPriority = TASK_PRIORITY_REALTIME, }, [TASK_ACCEL] = { .taskName = "ACCEL", .taskFunc = taskUpdateAccelerometer,