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

fixes

Browse source
This commit is contained in:
breadoven 2023-11-09 11:24:37 +00:00
parent 7ae41ad928
commit a8079c2842
2 changed files with 4 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -3040,7 +3040,7 @@ static bool isMaxAltitudeLimitExceeded(void)
return navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude;
}
int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
{
if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded()) {
targetAltitude = MIN(targetAltitude, navConfig()->general.max_altitude);
@ -3082,11 +3082,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
*
* ROC_TO_ALT_CONSTANT - constant climb rate. Climb rate and direction required. Target alt not required. */
// Terrain following uses different altitude measurement
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
if (mode == ROC_TO_ALT_RESET) {
posControl.desiredState.pos.z = altitudeToUse;
posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
} else if (mode == ROC_TO_ALT_TARGET) {
posControl.desiredState.pos.z = targetAltitude;
}
@ -3098,6 +3095,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
*/
if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) {
posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
}
posControl.desiredState.vel.z = desiredClimbRate; // only used for ROC_TO_ALT_CONSTANT