Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +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; 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()) { if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded()) {
targetAltitude = MIN(targetAltitude, navConfig()->general.max_altitude); 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. */ * 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) { 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) { } else if (mode == ROC_TO_ALT_TARGET) {
posControl.desiredState.pos.z = targetAltitude; posControl.desiredState.pos.z = targetAltitude;
} }
@ -3098,6 +3095,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
*/ */
if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) { if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) {
posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude); 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 posControl.desiredState.vel.z = desiredClimbRate; // only used for ROC_TO_ALT_CONSTANT

2
src/main/navigation/navigation_private.h
View file

@ -483,7 +483,7 @@ bool isWaypointNavTrackingActive(void);
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse); void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY); void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError); void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError);
int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros); float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros);
bool checkForPositionSensorTimeout(void); bool checkForPositionSensorTimeout(void);