Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

[RTH] Linear descent further refactoring [ci skip]

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2019-06-02 22:52:45 +02:00
parent 76ecb84bd4
commit 58a51c28b0
1 changed files with 16 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1121,8 +1121,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
} }
posControl.rthInitialHomeDistance = posControl.homeDistance;
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) { if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
} }
@ -1130,6 +1128,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
} }
// Save initial home distance for future use
posControl.rthInitialHomeDistance = posControl.homeDistance;
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME
} }
else { else {
@ -1181,7 +1182,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if ((posControl.flags.estPosStatus >= EST_USABLE)) { if ((posControl.flags.estPosStatus >= EST_USABLE)) {
if (isWaypointReached(&posControl.homeWaypointAbove, true)) { if (isWaypointReached(&posControl.homeWaypointAbove, true)) {
// Successfully reached position target - update XYZ-position // Successfully reached position target - update XYZ-position
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); fpVector3_t pos = posControl.homeWaypointAbove.pos;
// NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT changes the behavior of altitude controller and meaning of homeWaypointAbove.pos position
if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) {
pos.z = posControl.homePosition.pos.z + navConfig()->general.rth_altitude;
}
setDesiredPosition(&pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
} }
else if (!validateRTHSanityChecker()) { else if (!validateRTHSanityChecker()) {
@ -1192,18 +1201,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) { if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) {
// Calculate required travel and actual travel distance // Calculate required travel and actual travel distance
float rthTotalDistanceToTravel = posControl.rthInitialHomeDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0); float rthTotalDistanceToTravel = posControl.rthInitialHomeDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0);
float rthTravelledDistance = MIN((posControl.rthInitialHomeDistance - posControl.homeDistance), rthTotalDistanceToTravel);
// At this point `posControl.homeWaypointAbove.pos.z` is initial altitude at RTH start // At this point `posControl.homeWaypointAbove.pos.z` is initial altitude at RTH start
// while (posControl.homePosition.pos.z + navConfig()->general.rth_altitude) is a target altitude above home point // while (posControl.homePosition.pos.z + navConfig()->general.rth_altitude) is a target altitude above home point
// We will scale gradually between the two, but only if rthTotalDistanceToTravel is more than 1m (otherwise probably no point) // We will scale gradually between the two, but only if rthTotalDistanceToTravel is more than 1m (otherwise probably no point)
if (rthTotalDistanceToTravel >= 100) { if (rthTotalDistanceToTravel >= 100) {
fpVector3_t pos; fpVector3_t pos;
float ratioTravelled = rthTravelledDistance / rthTotalDistanceToTravel; float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f);
pos.z = posControl.homeWaypointAbove.pos.z -
(posControl.homeWaypointAbove.pos.z - (posControl.homePosition.pos.z + navConfig()->general.rth_altitude)) * ratioTravelled; pos.z = (posControl.homeWaypointAbove.pos.z * ratioNotTravelled) +
((posControl.homePosition.pos.z + navConfig()->general.rth_altitude) * (1.0f - ratioNotTravelled));
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z); setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z);
} }