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

Merge pull request #10908 from breadoven/abo_climb_first_fix

Browse source 
Nav RTH climb first fix
This commit is contained in:
breadoven 2025-06-20 10:57:22 +01:00 • committed by GitHub
commit 48ba62aa98
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
1 changed files with 4 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2718,9 +2718,10 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
case RTH_HOME_ENROUTE_PROPORTIONAL: case RTH_HOME_ENROUTE_PROPORTIONAL:
{ {
float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING_LEGACY) ? navConfig()->fw.loiter_radius : 0); uint16_t endPointDistance = STATE(FIXED_WING_LEGACY) ? navConfig()->fw.loiter_radius : 0;
float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - endPointDistance;
if (rthTotalDistanceToTravel >= 100) { if (rthTotalDistanceToTravel >= 100) {
float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f); float ratioNotTravelled = constrainf((posControl.homeDistance - endPointDistance) / rthTotalDistanceToTravel, 0.0f, 1.0f);
posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled)); posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled));
} }
else { else {
@ -3099,12 +3100,10 @@ static void updateDesiredRTHAltitude(void)
switch (navConfig()->general.flags.rth_alt_control_mode) { switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT: case NAV_RTH_NO_ALT:
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
break; break;
case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude; posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
break; break;
case NAV_RTH_MAX_ALT: case NAV_RTH_MAX_ALT:
@ -3112,19 +3111,17 @@ static void updateDesiredRTHAltitude(void)
if (navConfig()->general.rth_altitude > 0) { if (navConfig()->general.rth_altitude > 0) {
posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude); posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude);
} }
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
break; break;
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z); posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
break; break;
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
default: default:
posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude; posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
} }
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
if ((navConfig()->general.flags.rth_use_linear_descent) && (navConfig()->general.rth_home_altitude > 0) && (navConfig()->general.rth_linear_descent_start_distance == 0) ) { if ((navConfig()->general.flags.rth_use_linear_descent) && (navConfig()->general.rth_home_altitude > 0) && (navConfig()->general.rth_linear_descent_start_distance == 0) ) {
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude; posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;