Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

Update navigation.c

Browse source
This commit is contained in:
breadoven 2022-11-28 13:23:48 +00:00
parent 4f49dfb621
commit f1e945493d
1 changed files with 8 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/main/navigation/navigation.c
View file

@ -2910,9 +2910,14 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500); // FIXME: calculate sanity limits in a smarter way
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
/* Only update target altitude when actual altitude within 5m of target in required direction of change
* otherwise hold target altitude and wait for actual altitude to catch up */
float targetAlt = posControl.desiredState.pos.z + desiredClimbRate * timeDelta;
if ((desiredClimbRate > 0 && targetAlt < altitudeToUse + 500) || (desiredClimbRate < 0 && targetAlt > altitudeToUse - 500)) {
posControl.desiredState.pos.z = targetAlt;
}
}
}
else {