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

WP Linear climb + Update max alt limter

Browse source
This commit is contained in:
breadoven 2023-11-16 11:13:21 +00:00
parent 1fcac6134a
commit 87faeb11ee
1 changed files with 22 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1691,10 +1691,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
fpVector3_t tmpWaypoint; fpVector3_t tmpWaypoint;
tmpWaypoint.x = posControl.activeWaypoint.pos.x; tmpWaypoint.x = posControl.activeWaypoint.pos.x;
tmpWaypoint.y = posControl.activeWaypoint.pos.y; tmpWaypoint.y = posControl.activeWaypoint.pos.y;
tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance), setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); // Use linear climb bewtween WPs until within 10% of total distance to current active WP
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); float linearClimbDistRemaining = posControl.wpDistance - 0.1f * posControl.wpInitialDistance;
if (linearClimbDistRemaining > 0) {
int16_t climbRate = constrainf(posControl.actualState.velXY *
(posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) / linearClimbDistRemaining,
-navConfig()->general.max_auto_climb_rate,
navConfig()->general.max_auto_climb_rate);
updateClimbRateToAltitudeController(climbRate, 0, ROC_TO_ALT_CONSTANT);
} else {
updateClimbRateToAltitudeController(0, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
}
if(STATE(MULTIROTOR)) { if(STATE(MULTIROTOR)) {
switch (wpHeadingControl.mode) { switch (wpHeadingControl.mode) {
case NAV_WP_HEAD_MODE_NONE: case NAV_WP_HEAD_MODE_NONE:
@ -3035,17 +3045,8 @@ bool isProbablyStillFlying(void)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Z-position controller * Z-position controller
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
static bool isMaxAltitudeLimitExceeded(void)
{
return navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude;
}
float 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);
}
const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
float maxClimbRate = navConfig()->general.max_auto_climb_rate; float maxClimbRate = navConfig()->general.max_auto_climb_rate;
@ -3093,12 +3094,17 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
posControl.flags.rocToAltMode = mode; posControl.flags.rocToAltMode = mode;
/* /*
* If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve * If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve.
* Inhibit during RTH mode and also WP mode with altitude enforce active to avoid climbs getting stuck at max alt limit.
*/ */
if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) { if (navConfig()->general.max_altitude && !FLIGHT_MODE(NAV_RTH_MODE) && !(FLIGHT_MODE(NAV_WP_MODE) && navConfig()->general.waypoint_enforce_altitude)) {
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);
if (navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude && desiredClimbRate >= 0.0f) {
posControl.desiredState.pos.z = navConfig()->general.max_altitude;
posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET; posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
} }
}
} }
static void resetAltitudeController(bool useTerrainFollowing) static void resetAltitudeController(bool useTerrainFollowing)