Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 06:45:14 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'upstream/master' into RTH-Altitude-Override

Browse source
This commit is contained in:
breadoven 2021-04-12 23:57:54 +01:00
commit 99e787c96b
153 changed files with 36588 additions and 1558 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1159,8 +1159,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
fpVector3_t targetHoldPos;
if (STATE(FIXED_WING_LEGACY)) {
// Airplane - climbout before turning around
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away
// Airplane - climbout before heading home
if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) {
// Spiral climb centered at xy of RTH activation
calculateInitialHoldPosition(&targetHoldPos);
} else {
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb
}
} else {
// Multicopter, hover and climb
calculateInitialHoldPosition(&targetHoldPos);
@ -1206,7 +1211,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
// If we reached desired initial RTH altitude or we don't want to climb first
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first) || rthAltControlStickOverrideCheck(ROLL)) {
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL)) {
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
if (STATE(FIXED_WING_LEGACY)) {
@ -1237,8 +1242,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
// Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING_LEGACY)) {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) {
float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
} else {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
}
} else {
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
// it in a reasonable time. Immediately after we finish this phase - target the original altitude.