Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00
Code Issues Wiki Activity

Add the AT_LEAST_LINEAR_DESCENT RTH altitude mode (#4188)

Browse source
This commit is contained in:
Michel Pastor 2019-05-09 14:50:15 +02:00 • committed by GitHub
parent 5ba4e5b674
commit 644259f936
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
9 changed files with 105 additions and 66 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1119,6 +1119,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
}
posControl.rthInitialHomeDistance = posControl.homeDistance;
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);
}
else {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME
}
else {
@ -1144,8 +1153,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
if (navConfig()->general.flags.rth_tail_first) {
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
}
else {
} else {
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
}
@ -1179,13 +1187,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
else {
// Update XYZ-position target
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);
}
else {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) {
fpVector3_t pos;
uint16_t loiterDistanceFromHome = STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0;
uint32_t distanceToLoiterToTravelFromRTHStart = posControl.rthInitialHomeDistance - loiterDistanceFromHome;
uint32_t distanceToLoiterTraveled = constrain((int32_t)posControl.rthInitialHomeDistance - posControl.homeDistance, 0, distanceToLoiterToTravelFromRTHStart);
float RTHStartAltitude = posControl.homeWaypointAbove.pos.z;
float RTHFinalAltitude = posControl.homePosition.pos.z + navConfig()->general.rth_altitude;
pos.z = RTHStartAltitude - scaleRange(distanceToLoiterTraveled, 0, distanceToLoiterToTravelFromRTHStart, 0, RTHStartAltitude - RTHFinalAltitude);
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z);
}
return NAV_FSM_EVENT_NONE;
}
}