mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 22:35:19 +03:00
[RTH] Refactor RTH code to respect initial/final/hover RTH altitudes
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
58a51c28b0
commit
19faf75e75
4 changed files with 188 additions and 112 deletions
|
|
@ -185,6 +185,7 @@ uint16_t navEPV;
|
|||
int16_t navAccNEU[3];
|
||||
#endif
|
||||
|
||||
static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode);
|
||||
static void updateDesiredRTHAltitude(void);
|
||||
static void resetAltitudeController(bool useTerrainFollowing);
|
||||
static void resetPositionController(void);
|
||||
|
|
@ -197,6 +198,7 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos
|
|||
void calculateInitialHoldPosition(fpVector3_t * pos);
|
||||
void calculateFarAwayTarget(fpVector3_t * farAwayPos, 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);
|
||||
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 ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
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(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)); // Copters
|
||||
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.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
|
||||
if (STATE(FIXED_WING)) {
|
||||
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)) {
|
||||
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 {
|
||||
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
|
||||
}
|
||||
else {
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
|
||||
|
||||
/* For multi-rotors execute sanity check during initial ascent as well */
|
||||
if (!STATE(FIXED_WING)) {
|
||||
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
|
||||
if (STATE(FIXED_WING)) {
|
||||
fpVector3_t pos = posControl.homeWaypointAbove.pos;
|
||||
pos.z += FW_RTH_CLIMB_OVERSHOOT_CM;
|
||||
|
||||
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z);
|
||||
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.
|
||||
fpVector3_t pos = posControl.homeWaypointAbove.pos;
|
||||
pos.z += MR_RTH_CLIMB_OVERSHOOT_CM;
|
||||
tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
|
||||
|
||||
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 {
|
||||
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 ((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
|
||||
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);
|
||||
|
||||
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.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
|
||||
}
|
||||
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;
|
||||
}
|
||||
else {
|
||||
if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) {
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
|
||||
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;
|
||||
}
|
||||
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();
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
@ -1275,17 +1254,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
|
|||
if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
|
||||
if ((navConfig()->general.rth_home_altitude) && (posControl.desiredState.pos.z != navConfig()->general.rth_home_altitude)) {
|
||||
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;
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
|
||||
|
||||
if (navConfig()->general.rth_home_altitude) {
|
||||
float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate;
|
||||
|
||||
if (timeToReachHomeAltitude < 1) {
|
||||
// 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(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z);
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
|
||||
} else {
|
||||
float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
|
||||
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;
|
||||
}
|
||||
|
|
@ -1315,8 +1299,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
|
||||
}
|
||||
else {
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
|
||||
|
||||
// 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
|
||||
|
||||
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:
|
||||
default:
|
||||
posControl.rthState.rthInitialDistance = posControl.homeDistance;
|
||||
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
|
||||
};
|
||||
}
|
||||
|
|
@ -1398,22 +1385,28 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
|
||||
// If no position sensor available - land immediately
|
||||
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) {
|
||||
case NAV_WP_ACTION_WAYPOINT:
|
||||
case NAV_WP_ACTION_RTH:
|
||||
default:
|
||||
if (isWaypointReached(&posControl.activeWaypoint, isDoingRTH) || isWaypointMissed(&posControl.activeWaypoint)) {
|
||||
// Waypoint reached
|
||||
if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
|
||||
}
|
||||
else {
|
||||
// Update XY-position target to active waypoint
|
||||
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
|
||||
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
|
||||
}
|
||||
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 */
|
||||
|
|
@ -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
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
@ -1907,22 +1943,22 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
|
|||
*/
|
||||
navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE;
|
||||
if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE &&
|
||||
(posControl.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) &&
|
||||
(posControl.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
|
||||
(posControl.rthState.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) &&
|
||||
(posControl.rthState.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
|
||||
|
||||
// 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
|
||||
// to the home point.
|
||||
int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
|
||||
|
||||
posControl.homePosition.yaw += fakeToRealYawOffset;
|
||||
if (posControl.homePosition.yaw < 0) {
|
||||
posControl.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360);
|
||||
posControl.rthState.homePosition.yaw += fakeToRealYawOffset;
|
||||
if (posControl.rthState.homePosition.yaw < 0) {
|
||||
posControl.rthState.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360);
|
||||
}
|
||||
if (posControl.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) {
|
||||
posControl.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360);
|
||||
if (posControl.rthState.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.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
|
||||
}
|
||||
|
||||
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
|
||||
const uint32_t wpDistance = calculateDistanceToDestination(&waypoint->pos);
|
||||
const uint32_t wpDistance = calculateDistanceToDestination(pos);
|
||||
|
||||
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
|
||||
|
|
@ -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)
|
||||
{
|
||||
geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.homePosition.pos);
|
||||
geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
|
||||
GPS_distanceToHome = posControl.homeDistance / 100;
|
||||
GPS_directionToHome = posControl.homeDirection / 100;
|
||||
}
|
||||
|
||||
float calculateRTHAltitude(void) {
|
||||
switch (navConfig()->general.flags.rth_alt_control_mode) {
|
||||
case NAV_RTH_NO_ALT:
|
||||
return(posControl.actualState.abs.pos.z);
|
||||
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);
|
||||
}
|
||||
// Backdoor for RTH estimator
|
||||
float getFinalRTHAltitude(void)
|
||||
{
|
||||
return posControl.rthState.rthFinalAltitude;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -2047,11 +2077,42 @@ static void updateDesiredRTHAltitude(void)
|
|||
{
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
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 {
|
||||
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.initialPosition = *pos;
|
||||
posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.homePosition.pos);
|
||||
posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
|
||||
}
|
||||
|
||||
bool validateRTHSanityChecker(void)
|
||||
|
|
@ -2079,7 +2140,7 @@ bool validateRTHSanityChecker(void)
|
|||
|
||||
// Check at 10Hz rate
|
||||
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) {
|
||||
posControl.rthSanityChecker.minimalDistanceToHome = currentDistanceToHome;
|
||||
|
|
@ -2102,33 +2163,33 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
|
|||
{
|
||||
// XY-position
|
||||
if ((useMask & NAV_POS_UPDATE_XY) != 0) {
|
||||
posControl.homePosition.pos.x = pos->x;
|
||||
posControl.homePosition.pos.y = pos->y;
|
||||
posControl.rthState.homePosition.pos.x = pos->x;
|
||||
posControl.rthState.homePosition.pos.y = pos->y;
|
||||
if (homeFlags & NAV_HOME_VALID_XY) {
|
||||
posControl.homeFlags |= NAV_HOME_VALID_XY;
|
||||
posControl.rthState.homeFlags |= NAV_HOME_VALID_XY;
|
||||
} else {
|
||||
posControl.homeFlags &= ~NAV_HOME_VALID_XY;
|
||||
posControl.rthState.homeFlags &= ~NAV_HOME_VALID_XY;
|
||||
}
|
||||
}
|
||||
|
||||
// Z-position
|
||||
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) {
|
||||
posControl.homeFlags |= NAV_HOME_VALID_Z;
|
||||
posControl.rthState.homeFlags |= NAV_HOME_VALID_Z;
|
||||
} else {
|
||||
posControl.homeFlags &= ~NAV_HOME_VALID_Z;
|
||||
posControl.rthState.homeFlags &= ~NAV_HOME_VALID_Z;
|
||||
}
|
||||
}
|
||||
|
||||
// Heading
|
||||
if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
|
||||
// Heading
|
||||
posControl.homePosition.yaw = yaw;
|
||||
posControl.rthState.homePosition.yaw = yaw;
|
||||
if (homeFlags & NAV_HOME_VALID_HEADING) {
|
||||
posControl.homeFlags |= NAV_HOME_VALID_HEADING;
|
||||
posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
|
||||
} 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;
|
||||
|
||||
// Update target RTH altitude as a waypoint above home
|
||||
posControl.homeWaypointAbove = posControl.homePosition;
|
||||
updateDesiredRTHAltitude();
|
||||
|
||||
updateHomePositionCompatibility();
|
||||
|
|
@ -2167,7 +2227,7 @@ void updateHomePosition(void)
|
|||
if (!ARMING_FLAG(ARMED)) {
|
||||
if (posControl.flags.estPosStatus >= EST_USABLE) {
|
||||
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) {
|
||||
case NAV_RESET_NEVER:
|
||||
break;
|
||||
|
|
@ -2200,8 +2260,9 @@ void updateHomePosition(void)
|
|||
|
||||
// Update distance and direction to home if armed (home is not updated when armed)
|
||||
if (STATE(GPS_FIX_HOME)) {
|
||||
posControl.homeDistance = calculateDistanceToDestination(&posControl.homePosition.pos);
|
||||
posControl.homeDirection = calculateBearingToDestination(&posControl.homePosition.pos);
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
|
||||
posControl.homeDistance = calculateDistanceToDestination(tmpHomePos);
|
||||
posControl.homeDirection = calculateBearingToDestination(tmpHomePos);
|
||||
updateHomePositionCompatibility();
|
||||
}
|
||||
}
|
||||
|
|
@ -3239,7 +3300,7 @@ bool FAST_CODE isNavLaunchEnabled(void)
|
|||
|
||||
int32_t navigationGetHomeHeading(void)
|
||||
{
|
||||
return posControl.homePosition.yaw;
|
||||
return posControl.rthState.homePosition.yaw;
|
||||
}
|
||||
|
||||
// returns m/s
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue