From beb72ddf1a4fae749172fef285363ba287a01633 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 11 Jun 2021 23:36:07 +0100 Subject: [PATCH 1/2] First build --- src/main/io/osd.c | 8 +++++++- src/main/io/osd.h | 1 - src/main/navigation/navigation.c | 11 +++++------ src/main/navigation/navigation_private.h | 2 ++ 4 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f0c4af8c24..a868c923ed 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -654,6 +654,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 @@ -678,6 +681,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: @@ -685,7 +689,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, posControl.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); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 8f4e4d4bf9..0c53bbed16 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -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" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 37a6c3c000..da4e96108b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2579,11 +2579,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti } else { - /* + /* * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 * In other words, when altitude is reached, allow it only to shrink */ - if (navConfig()->general.max_altitude > 0 && + if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0 ) { @@ -3343,7 +3343,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; } @@ -3361,10 +3361,9 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) 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); - const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance; - - if (navWpMissionStartTooFar) { + if (posControl.distanceToFirstWP > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) { return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR; } } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index b26299e707..0fd6c731c6 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -364,6 +364,8 @@ typedef struct { float wpDistance; // Distance to active WP timeMs_t wpReachedTime; // Time the waypoint was reached + uint32_t distanceToFirstWP; // Distance to first waypoint from arming location + /* Internals & statistics */ int16_t rcAdjustment[4]; float totalTripDistance; From ed95ca9d2371e008d049afe1e3152429c32b86a9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 12 Jun 2021 11:44:30 +0100 Subject: [PATCH 2/2] Change to function --- src/main/io/osd.c | 2 +- src/main/navigation/navigation.c | 13 ++++++++----- src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_private.h | 2 -- 4 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a868c923ed..22ad0e5686 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -689,7 +689,7 @@ 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: - osdFormatDistanceSymbol(buf, posControl.distanceToFirstWP, 0); + osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0); tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf); return message = messageBuf; case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index da4e96108b..2c6594f70b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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; } } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b71f932f99..fa674bdbc2 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -513,6 +513,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); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 0fd6c731c6..b26299e707 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -364,8 +364,6 @@ typedef struct { float wpDistance; // Distance to active WP timeMs_t wpReachedTime; // Time the waypoint was reached - uint32_t distanceToFirstWP; // Distance to first waypoint from arming location - /* Internals & statistics */ int16_t rcAdjustment[4]; float totalTripDistance;