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

Merge pull request #7107 from breadoven/abo_first_wp_arm_blocker_override

Browse source 
First waypoint too far arm blocker override
This commit is contained in:
Paweł Spychalski 2021-07-07 09:17:41 +02:00 • committed by GitHub
commit 38e6594216
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 17 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -3339,6 +3339,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)));
@ -3355,7 +3362,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
// Apply extra arming safety only if pilot has any of GPS modes configured
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS &&
(STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || rxGetChannelValue(YAW) > 1750)) {
(STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || checkStickPosition(YAW_HI))) {
if (usedBypass) {
*usedBypass = true;
}
@ -3371,12 +3378,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);
const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
if (navWpMissionStartTooFar) {
if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
}
}