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

Change to function

Browse source
This commit is contained in:
breadoven 2021-06-12 11:44:30 +01:00
parent beb72ddf1a
commit ed95ca9d23
4 changed files with 10 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -3327,6 +3327,13 @@ bool navigationTerrainFollowingEnabled(void)
return posControl.flags.isTerrainFollowEnabled;
}
uint32_t distanceToFirstWP(void)
{
fpVector3_t startingWaypointPos;
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
return calculateDistanceToDestination(&startingWaypointPos);
}
navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
{
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)));
@ -3359,11 +3366,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
// Don't allow arming if first waypoint is farther than configured safe distance
if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
fpVector3_t startingWaypointPos;
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
posControl.distanceToFirstWP = calculateDistanceToDestination(&startingWaypointPos);
if (posControl.distanceToFirstWP > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
}
}