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

[RTH] Refactor RTH code to respect initial/final/hover RTH altitudes

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2019-06-03 21:41:23 +02:00
parent 58a51c28b0
commit 19faf75e75
4 changed files with 188 additions and 112 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/flight/rth_estimator.c
View file

@ -138,7 +138,7 @@ static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChan
// output in Watt // output in Watt
static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) { static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) {
const float timeToHome = distanceToHome / speedToHome; // seconds const float timeToHome = distanceToHome / speedToHome; // seconds
const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - calculateRTHAltitude()) : 0); const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - getFinalRTHAltitude()) : 0);
const float pitchToHome = MIN(RADIANS_TO_DEGREES(atan2_approx(altitudeChangeDescentToHome, distanceToHome)), navConfig()->fw.max_dive_angle); const float pitchToHome = MIN(RADIANS_TO_DEGREES(atan2_approx(altitudeChangeDescentToHome, distanceToHome)), navConfig()->fw.max_dive_angle);
return estimatePitchPower(pitchToHome) * timeToHome / 3600; return estimatePitchPower(pitchToHome) * timeToHome / 3600;
} }
@ -156,7 +156,7 @@ static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
)) ))
return -1; return -1;
const float RTH_initial_altitude_change = MAX(0, (calculateRTHAltitude() - getEstimatedActualPosition(Z)) / 100); const float RTH_initial_altitude_change = MAX(0, (getFinalRTHAltitude() - getEstimatedActualPosition(Z)) / 100);
float RTH_heading; // degrees float RTH_heading; // degrees
#ifdef USE_WIND_ESTIMATOR #ifdef USE_WIND_ESTIMATOR

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

@ -185,6 +185,7 @@ uint16_t navEPV;
int16_t navAccNEU[3]; int16_t navAccNEU[3];
#endif #endif
static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode);
static void updateDesiredRTHAltitude(void); static void updateDesiredRTHAltitude(void);
static void resetAltitudeController(bool useTerrainFollowing); static void resetAltitudeController(bool useTerrainFollowing);
static void resetPositionController(void); static void resetPositionController(void);
@ -197,6 +198,7 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos
void calculateInitialHoldPosition(fpVector3_t * pos); void calculateInitialHoldPosition(fpVector3_t * pos);
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance); void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
void initializeRTHSanityChecker(const fpVector3_t * pos); void initializeRTHSanityChecker(const fpVector3_t * pos);
bool validateRTHSanityChecker(void); bool validateRTHSanityChecker(void);
@ -1112,28 +1114,31 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
// If we have valid pos sensor OR we are configured to ignore GPS loss // If we have valid pos sensor OR we are configured to ignore GPS loss
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) { if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
const float rthAltitudeMargin = STATE(FIXED_WING) ? const float rthAltitudeMargin = STATE(FIXED_WING) ?
MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)) : // Airplane MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)) : // Airplane
MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)); // Copters MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // Copters
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homeWaypointAbove.pos.z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
} }
// Save initial home distance for future use
posControl.rthState.rthInitialDistance = posControl.homeDistance;
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
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(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
} }
else { else {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); setDesiredPosition(tmpHomePos, 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 {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
/* For multi-rotors execute sanity check during initial ascent as well */ /* For multi-rotors execute sanity check during initial ascent as well */
if (!STATE(FIXED_WING)) { if (!STATE(FIXED_WING)) {
if (!validateRTHSanityChecker()) { if (!validateRTHSanityChecker()) {
@ -1143,21 +1148,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
// Climb to safe altitude and turn to correct direction // Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
fpVector3_t pos = posControl.homeWaypointAbove.pos; tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
pos.z += FW_RTH_CLIMB_OVERSHOOT_CM; setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z);
} }
else { else {
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach // 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. // it in a reasonable time. Immediately after we finish this phase - target the original altitude.
fpVector3_t pos = posControl.homeWaypointAbove.pos; tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
pos.z += MR_RTH_CLIMB_OVERSHOOT_CM;
if (navConfig()->general.flags.rth_tail_first) { if (navConfig()->general.flags.rth_tail_first) {
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
} else { } else {
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
} }
} }
@ -1180,17 +1182,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
// If we have position sensor - continue home // If we have position sensor - continue home
if ((posControl.flags.estPosStatus >= EST_USABLE)) { if ((posControl.flags.estPosStatus >= EST_USABLE)) {
if (isWaypointReached(&posControl.homeWaypointAbove, true)) { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
if (isWaypointPositionReached(tmpHomePos, true)) {
// Successfully reached position target - update XYZ-position // Successfully reached position target - update XYZ-position
fpVector3_t pos = posControl.homeWaypointAbove.pos; setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
// 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()) {
@ -1198,25 +1194,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
else { else {
if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) { setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
// Calculate required travel and actual travel distance
float rthTotalDistanceToTravel = posControl.rthInitialHomeDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0);
// 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
// We will scale gradually between the two, but only if rthTotalDistanceToTravel is more than 1m (otherwise probably no point)
if (rthTotalDistanceToTravel >= 100) {
fpVector3_t pos;
float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f);
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);
}
}
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
} }
@ -1248,7 +1226,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
} }
else { else {
if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { if (ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) {
resetLandingDetector(); resetLandingDetector();
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
@ -1258,7 +1236,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
else { else {
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
} }
@ -1275,17 +1254,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout())) if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()))
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
if ((navConfig()->general.rth_home_altitude) && (posControl.desiredState.pos.z != navConfig()->general.rth_home_altitude)) { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
int8_t altitudeChangeDirection = navConfig()->general.rth_home_altitude > posControl.homeWaypointAbove.pos.z ? 1 : -1;
float timeToReachHomeAltitude = altitudeChangeDirection * (navConfig()->general.rth_home_altitude - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate; if (navConfig()->general.rth_home_altitude) {
float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate;
if (timeToReachHomeAltitude < 1) { if (timeToReachHomeAltitude < 1) {
// we almost reached the target home altitude so set the desired altitude to the desired home altitude // we almost reached the target home altitude so set the desired altitude to the desired home altitude
posControl.homeWaypointAbove.pos.z = navConfig()->general.rth_home_altitude; setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z);
} else { } else {
float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
} }
} }
else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
}
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -1315,8 +1299,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f); descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
} }
else { else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt) float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - tmpHomePos->z - navConfig()->general.land_slowdown_minalt)
/ (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt / (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f); descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
@ -1386,8 +1372,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
default: default:
posControl.rthState.rthInitialDistance = posControl.homeDistance;
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
calculateAndSetActiveWaypointToLocalPosition(&posControl.homeWaypointAbove.pos); calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL));
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
}; };
} }
@ -1398,22 +1385,28 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
// If no position sensor available - land immediately // If no position sensor available - land immediately
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) { if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
const bool isDoingRTH = (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) { switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT: case NAV_WP_ACTION_WAYPOINT:
case NAV_WP_ACTION_RTH:
default: default:
if (isWaypointReached(&posControl.activeWaypoint, isDoingRTH) || isWaypointMissed(&posControl.activeWaypoint)) { if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
// Waypoint reached
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
} }
else { else {
// Update XY-position target to active waypoint
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
} }
break; break;
case NAV_WP_ACTION_RTH:
if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
else {
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z);
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
break;
} }
} }
/* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
@ -1659,6 +1652,49 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
} }
} }
static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
{
posControl.rthState.homeTmpWaypoint = posControl.rthState.homePosition.pos;
switch (mode) {
case RTH_HOME_ENROUTE_INITIAL:
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude;
break;
case RTH_HOME_ENROUTE_PROPORTIONAL:
{
float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0);
if (rthTotalDistanceToTravel >= 100) {
float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f);
posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled));
}
else {
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
}
}
break;
case RTH_HOME_ENROUTE_FINAL:
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
break;
case RTH_HOME_FINAL_HOVER:
if (navConfig()->general.rth_home_altitude) {
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
}
else {
// If home altitude not defined - fall back to final ENROUTE altitude
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
}
break;
case RTH_HOME_FINAL_LAND:
break;
}
return &posControl.rthState.homeTmpWaypoint;
}
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Float point PID-controller implementation * Float point PID-controller implementation
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
@ -1907,22 +1943,22 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
*/ */
navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE; navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE;
if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE && if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE &&
(posControl.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) && (posControl.rthState.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) &&
(posControl.homeFlags & NAV_HOME_VALID_HEADING) == 0) { (posControl.rthState.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
// Home was stored using the fake heading (assuming boot as 0deg). Calculate // Home was stored using the fake heading (assuming boot as 0deg). Calculate
// the offset from the fake to the actual yaw and apply the same rotation // the offset from the fake to the actual yaw and apply the same rotation
// to the home point. // to the home point.
int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw; int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
posControl.homePosition.yaw += fakeToRealYawOffset; posControl.rthState.homePosition.yaw += fakeToRealYawOffset;
if (posControl.homePosition.yaw < 0) { if (posControl.rthState.homePosition.yaw < 0) {
posControl.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360); posControl.rthState.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360);
} }
if (posControl.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) { if (posControl.rthState.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) {
posControl.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360); posControl.rthState.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360);
} }
posControl.homeFlags |= NAV_HOME_VALID_HEADING; posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
} }
posControl.actualState.yaw = newHeading; posControl.actualState.yaw = newHeading;
posControl.flags.estHeadingStatus = newEstHeading; posControl.flags.estHeadingStatus = newEstHeading;
@ -2001,10 +2037,10 @@ bool isWaypointMissed(const navWaypointPosition_t * waypoint)
return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees
} }
bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome) static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome)
{ {
// We consider waypoint reached if within specified radius // We consider waypoint reached if within specified radius
const uint32_t wpDistance = calculateDistanceToDestination(&waypoint->pos); const uint32_t wpDistance = calculateDistanceToDestination(pos);
if (STATE(FIXED_WING) && isWaypointHome) { if (STATE(FIXED_WING) && isWaypointHome) {
// Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
@ -2016,28 +2052,22 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp
} }
} }
bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome)
{
return isWaypointPositionReached(&waypoint->pos, isWaypointHome);
}
static void updateHomePositionCompatibility(void) static void updateHomePositionCompatibility(void)
{ {
geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.homePosition.pos); geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
GPS_distanceToHome = posControl.homeDistance / 100; GPS_distanceToHome = posControl.homeDistance / 100;
GPS_directionToHome = posControl.homeDirection / 100; GPS_directionToHome = posControl.homeDirection / 100;
} }
float calculateRTHAltitude(void) { // Backdoor for RTH estimator
switch (navConfig()->general.flags.rth_alt_control_mode) { float getFinalRTHAltitude(void)
case NAV_RTH_NO_ALT: {
return(posControl.actualState.abs.pos.z); return posControl.rthState.rthFinalAltitude;
case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
return(posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude);
case NAV_RTH_MAX_ALT:
return(MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z));
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
case NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT:
return(MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z));
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
default:
return(posControl.homePosition.pos.z + navConfig()->general.rth_altitude);
}
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
@ -2047,11 +2077,42 @@ static void updateDesiredRTHAltitude(void)
{ {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) { if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
posControl.homeWaypointAbove.pos.z = calculateRTHAltitude(); switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT:
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
break;
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.rthFinalAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
break;
case NAV_RTH_MAX_ALT:
posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.actualState.abs.pos.z);
posControl.rthState.rthFinalAltitude = MAX(posControl.rthState.rthFinalAltitude, posControl.actualState.abs.pos.z);
break;
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
posControl.rthState.rthFinalAltitude = MAX(posControl.rthState.rthFinalAltitude + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
break;
case NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT:
posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
break;
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
default:
posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
}
} }
} }
else { else {
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z; posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
} }
} }
@ -2064,7 +2125,7 @@ void initializeRTHSanityChecker(const fpVector3_t * pos)
posControl.rthSanityChecker.lastCheckTime = currentTimeMs; posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
posControl.rthSanityChecker.initialPosition = *pos; posControl.rthSanityChecker.initialPosition = *pos;
posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.homePosition.pos); posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
} }
bool validateRTHSanityChecker(void) bool validateRTHSanityChecker(void)
@ -2079,7 +2140,7 @@ bool validateRTHSanityChecker(void)
// Check at 10Hz rate // Check at 10Hz rate
if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) { if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) {
const float currentDistanceToHome = calculateDistanceToDestination(&posControl.homePosition.pos); const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
if (currentDistanceToHome < posControl.rthSanityChecker.minimalDistanceToHome) { if (currentDistanceToHome < posControl.rthSanityChecker.minimalDistanceToHome) {
posControl.rthSanityChecker.minimalDistanceToHome = currentDistanceToHome; posControl.rthSanityChecker.minimalDistanceToHome = currentDistanceToHome;
@ -2102,33 +2163,33 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
{ {
// XY-position // XY-position
if ((useMask & NAV_POS_UPDATE_XY) != 0) { if ((useMask & NAV_POS_UPDATE_XY) != 0) {
posControl.homePosition.pos.x = pos->x; posControl.rthState.homePosition.pos.x = pos->x;
posControl.homePosition.pos.y = pos->y; posControl.rthState.homePosition.pos.y = pos->y;
if (homeFlags & NAV_HOME_VALID_XY) { if (homeFlags & NAV_HOME_VALID_XY) {
posControl.homeFlags |= NAV_HOME_VALID_XY; posControl.rthState.homeFlags |= NAV_HOME_VALID_XY;
} else { } else {
posControl.homeFlags &= ~NAV_HOME_VALID_XY; posControl.rthState.homeFlags &= ~NAV_HOME_VALID_XY;
} }
} }
// Z-position // Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) { if ((useMask & NAV_POS_UPDATE_Z) != 0) {
posControl.homePosition.pos.z = pos->z; posControl.rthState.homePosition.pos.z = pos->z;
if (homeFlags & NAV_HOME_VALID_Z) { if (homeFlags & NAV_HOME_VALID_Z) {
posControl.homeFlags |= NAV_HOME_VALID_Z; posControl.rthState.homeFlags |= NAV_HOME_VALID_Z;
} else { } else {
posControl.homeFlags &= ~NAV_HOME_VALID_Z; posControl.rthState.homeFlags &= ~NAV_HOME_VALID_Z;
} }
} }
// Heading // Heading
if ((useMask & NAV_POS_UPDATE_HEADING) != 0) { if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
// Heading // Heading
posControl.homePosition.yaw = yaw; posControl.rthState.homePosition.yaw = yaw;
if (homeFlags & NAV_HOME_VALID_HEADING) { if (homeFlags & NAV_HOME_VALID_HEADING) {
posControl.homeFlags |= NAV_HOME_VALID_HEADING; posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
} else { } else {
posControl.homeFlags &= ~NAV_HOME_VALID_HEADING; posControl.rthState.homeFlags &= ~NAV_HOME_VALID_HEADING;
} }
} }
@ -2136,7 +2197,6 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
posControl.homeDirection = 0; posControl.homeDirection = 0;
// Update target RTH altitude as a waypoint above home // Update target RTH altitude as a waypoint above home
posControl.homeWaypointAbove = posControl.homePosition;
updateDesiredRTHAltitude(); updateDesiredRTHAltitude();
updateHomePositionCompatibility(); updateHomePositionCompatibility();
@ -2167,7 +2227,7 @@ void updateHomePosition(void)
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
if (posControl.flags.estPosStatus >= EST_USABLE) { if (posControl.flags.estPosStatus >= EST_USABLE) {
const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z; const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
bool setHome = (posControl.homeFlags & validHomeFlags) != validHomeFlags; bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) { switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
case NAV_RESET_NEVER: case NAV_RESET_NEVER:
break; break;
@ -2200,8 +2260,9 @@ void updateHomePosition(void)
// Update distance and direction to home if armed (home is not updated when armed) // Update distance and direction to home if armed (home is not updated when armed)
if (STATE(GPS_FIX_HOME)) { if (STATE(GPS_FIX_HOME)) {
posControl.homeDistance = calculateDistanceToDestination(&posControl.homePosition.pos); fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
posControl.homeDirection = calculateBearingToDestination(&posControl.homePosition.pos); posControl.homeDistance = calculateDistanceToDestination(tmpHomePos);
posControl.homeDirection = calculateBearingToDestination(tmpHomePos);
updateHomePositionCompatibility(); updateHomePositionCompatibility();
} }
} }
@ -3239,7 +3300,7 @@ bool FAST_CODE isNavLaunchEnabled(void)
int32_t navigationGetHomeHeading(void) int32_t navigationGetHomeHeading(void)
{ {
return posControl.homePosition.yaw; return posControl.rthState.homePosition.yaw;
} }
// returns m/s // returns m/s

2
src/main/navigation/navigation.h
View file

@ -416,7 +416,7 @@ void resetWaypointList(void);
bool loadNonVolatileWaypointList(void); bool loadNonVolatileWaypointList(void);
bool saveNonVolatileWaypointList(void); bool saveNonVolatileWaypointList(void);
float calculateRTHAltitude(void); float getFinalRTHAltitude(void);
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch); int16_t fixedWingPitchToThrottleCorrection(int16_t pitch);
/* Geodetic functions */ /* Geodetic functions */

23
src/main/navigation/navigation_private.h
View file

@ -297,6 +297,23 @@ typedef struct {
timeMs_t lastYawAdjustmentTime; timeMs_t lastYawAdjustmentTime;
} navCruise_t; } navCruise_t;
typedef struct {
navigationHomeFlags_t homeFlags;
navWaypointPosition_t homePosition; // Original home position and base altitude
float rthInitialAltitude; // Altitude at start of RTH
float rthFinalAltitude; // Altitude at end of RTH approach
float rthInitialDistance; // Distance when starting flight home
fpVector3_t homeTmpWaypoint; // Temporary storage for home target
} rthState_t;
typedef enum {
RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach
RTH_HOME_ENROUTE_PROPORTIONAL, // Prorpotional position for RTH approach
RTH_HOME_ENROUTE_FINAL, // Final position for RTH approach
RTH_HOME_FINAL_HOVER, // Final hover altitude (if rth_home_altitude is set)
RTH_HOME_FINAL_LAND, // Home position and altitude
} rthTargetMode_e;
typedef struct { typedef struct {
/* Flags and navigation system state */ /* Flags and navigation system state */
navigationFSMState_t navState; navigationFSMState_t navState;
@ -321,11 +338,9 @@ typedef struct {
/* Home parameters (NEU coordinated), geodetic position of home (LLH) is stores in GPS_home variable */ /* Home parameters (NEU coordinated), geodetic position of home (LLH) is stores in GPS_home variable */
rthSanityChecker_t rthSanityChecker; rthSanityChecker_t rthSanityChecker;
navWaypointPosition_t homePosition; // Special waypoint, stores original yaw (heading when launched) rthState_t rthState;
navWaypointPosition_t homeWaypointAbove; // NEU-coordinates and initial bearing + desired RTH altitude
navigationHomeFlags_t homeFlags;
uint32_t rthInitialHomeDistance; // Distance to home after RTH has been initiated and the initial climb/descent is done
/* Home parameters */
uint32_t homeDistance; // cm uint32_t homeDistance; // cm
int32_t homeDirection; // deg*100 int32_t homeDirection; // deg*100