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

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

8
src/main/io/osd.c
View file

@ -675,6 +675,9 @@ static void osdFormatCraftName(char *buff)
static const char * osdArmingDisabledReasonMessage(void)
{
const char *message = NULL;
char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
switch (isArmingDisabledReason()) {
case ARMING_DISABLED_FAILSAFE_SYSTEM:
// See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c
@ -699,6 +702,7 @@ static const char * osdArmingDisabledReasonMessage(void)
#if defined(USE_NAV)
// Check the exact reason
switch (navigationIsBlockingArming(NULL)) {
char buf[6];
case NAV_ARMING_BLOCKER_NONE:
break;
case NAV_ARMING_BLOCKER_MISSING_GPS_FIX:
@ -706,7 +710,9 @@ static const char * osdArmingDisabledReasonMessage(void)
case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE:
return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST);
case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
return OSD_MESSAGE_STR(OSD_MSG_1ST_WP_TOO_FAR);
osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0);
tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf);
return message = messageBuf;
case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG);
}

1
src/main/io/osd.h
View file

@ -53,7 +53,6 @@
#define OSD_MSG_SYS_OVERLOADED "SYSTEM OVERLOADED"
#define OSD_MSG_WAITING_GPS_FIX "WAITING FOR GPS FIX"
#define OSD_MSG_DISABLE_NAV_FIRST "DISABLE NAVIGATION FIRST"
#define OSD_MSG_1ST_WP_TOO_FAR "FIRST WAYPOINT IS TOO FAR"
#define OSD_MSG_JUMP_WP_MISCONFIG "JUMP WAYPOINT MISCONFIGURED"
#define OSD_MSG_MAG_NOT_CAL "COMPASS NOT CALIBRATED"
#define OSD_MSG_ACC_NOT_CAL "ACCELEROMETER NOT CALIBRATED"

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;
}
}

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

@ -508,6 +508,7 @@ geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e da
/* Distance/bearing calculation */
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);
uint32_t distanceToFirstWP(void);
/* Failsafe-forced RTH mode */
void activateForcedRTH(void);