void taskMainPidLoop(void) { cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; imuUpdateAccelerometer(); imuUpdateGyroAndAttitude(); annexCode(); if (masterConfig.rxConfig.rcSmoothing) { filterRc(isRXDataNew); } #if defined(NAV) if (isRXDataNew) { updateWaypointsAndNavigationMode(); } #endif isRXDataNew = false; #if defined(NAV) updatePositionEstimator(); applyWaypointNavigationAndAltitudeHold(); #endif // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the // motors do not spin up while we are trying to arm or disarm. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo) #endif && masterConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerMode != MIXER_FLYING_WING && masterConfig.mixerMode != MIXER_CUSTOM_AIRPLANE #endif ) { rcCommand[YAW] = 0; } // Apply throttle tilt compensation if (!STATE(FIXED_WING)) { int16_t thrTiltCompStrength = 0; if (navigationRequiresThrottleTiltCompensation()) { thrTiltCompStrength = 100; } else if (currentProfile->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { thrTiltCompStrength = currentProfile->throttle_tilt_compensation_strength; } if (thrTiltCompStrength) { rcCommand[THROTTLE] = constrain(masterConfig.motorConfig.minthrottle + (rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength), masterConfig.motorConfig.minthrottle, masterConfig.motorConfig.maxthrottle); } } pidController(¤tProfile->pidProfile, currentControlRateProfile, &masterConfig.rxConfig); #ifdef HIL if (hilActive) { hilUpdateControlState(); motorControlEnable = false; } #endif mixTable(); #ifdef USE_SERVOS if (isMixerUsingServos()) { servoMixer(currentProfile->flaperon_throw_offset, currentProfile->flaperon_throw_inverted); } if (feature(FEATURE_SERVO_TILT)) { processServoTilt(); } //Servos should be filtered or written only when mixer is using servos or special feaures are enabled if (isServoOutputEnabled()) { filterServos(); writeServos(); } #endif if (motorControlEnable) { writeMotors(); } #ifdef USE_SDCARD afatfs_poll(); #endif #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif }
void taskMainPidLoop(void) { cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; // Calculate average cycle time and average jitter filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT); debug[0] = cycleTime; debug[1] = cycleTime - filteredCycleTime; imuUpdateGyroAndAttitude(); annexCode(); if (rxConfig()->rcSmoothing) { filterRc(); } #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; #endif #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #if defined(BARO) || defined(SONAR) if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(); } } #endif // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the // motors do not spin up while we are trying to arm or disarm. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && mixerConfig()->tri_unarmed_servo) #endif && mixerConfig()->mixerMode != MIXER_AIRPLANE && mixerConfig()->mixerMode != MIXER_FLYING_WING #endif ) { rcCommand[YAW] = 0; } if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value); } #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif // PID - note this is function pointer set by setPIDController() pid_controller( pidProfile(), currentControlRateProfile, imuConfig()->max_angle_inclination, &accelerometerConfig()->accelerometerTrims, rxConfig() ); mixTable(); #ifdef USE_SERVOS filterServos(); writeServos(); #endif if (motorControlEnable) { writeMotors(); } #ifdef USE_SDCARD afatfs_poll(); #endif #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif }