Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 11:29:56 +03:00
Code Issues Wiki Activity

change D term to measurement

Browse source
This commit is contained in:
breadoven 2025-06-19 21:17:31 +01:00
parent b62e4ca132
commit 83d162c600
1 changed files with 6 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

8
src/main/navigation/navigation_fixedwing.c
View file

@ -136,7 +136,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
// Reduce max allowed climb rate by 2/3 if performing loiter (stall prevention) // Reduce max allowed climb rate by 2/3 if performing loiter (stall prevention)
if (needToCalculateCircularLoiter && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) { if (navGetCurrentStateFlags() & NAV_CTL_HOLD && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) {
desiredClimbRate = 0.67f * navConfig()->fw.max_auto_climb_rate; desiredClimbRate = 0.67f * navConfig()->fw.max_auto_climb_rate;
} }
@ -147,6 +147,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
// Default control based on climb rate (velocity) // Default control based on climb rate (velocity)
float targetValue = desiredClimbRate; float targetValue = desiredClimbRate;
float measuredValue = navGetCurrentActualPositionAndVelocity()->vel.z; float measuredValue = navGetCurrentActualPositionAndVelocity()->vel.z;
pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR;
// Optional control based on altitude (position) // Optional control based on altitude (position)
if (pidProfile()->fwAltControlUsePos) { if (pidProfile()->fwAltControlUsePos) {
@ -186,14 +187,17 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
trackingAltitude += adjustmentFactor * desiredClimbRate * US2S(deltaMicros); trackingAltitude += adjustmentFactor * desiredClimbRate * US2S(deltaMicros);
desiredAltitude = trackingAltitude; desiredAltitude = trackingAltitude;
} }
} else {
desiredClimbRate = 0;
} }
targetValue = desiredAltitude; targetValue = desiredAltitude;
measuredValue = currentAltitude; measuredValue = currentAltitude;
pidFlags = 0; // use measurement for D term
} }
// PID controller to translate desired target error (velocity or position) into pitch angle [decideg] // PID controller to translate desired target error (velocity or position) into pitch angle [decideg]
float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, targetValue, measuredValue, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR); float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, targetValue, measuredValue, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, pidFlags);
// Apply low-pass filter to prevent rapid correction // Apply low-pass filter to prevent rapid correction
targetPitchAngle = pt1FilterApply4(&pitchFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); targetPitchAngle = pt1FilterApply4(&pitchFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));