Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 06:15:14 +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

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

@ -689,7 +689,7 @@ static const char * osdArmingDisabledReasonMessage(void)
case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE:
return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST); return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST);
case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: 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); tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf);
return message = messageBuf; return message = messageBuf;
case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:

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

@ -3327,6 +3327,13 @@ bool navigationTerrainFollowingEnabled(void)
return posControl.flags.isTerrainFollowEnabled; 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) 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))); 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 // Don't allow arming if first waypoint is farther than configured safe distance
if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) { if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
fpVector3_t startingWaypointPos; if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
posControl.distanceToFirstWP = calculateDistanceToDestination(&startingWaypointPos);
if (posControl.distanceToFirstWP > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR; return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
} }
} }

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

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

2
src/main/navigation/navigation_private.h
View file

@ -364,8 +364,6 @@ typedef struct {
float wpDistance; // Distance to active WP float wpDistance; // Distance to active WP
timeMs_t wpReachedTime; // Time the waypoint was reached timeMs_t wpReachedTime; // Time the waypoint was reached
uint32_t distanceToFirstWP; // Distance to first waypoint from arming location
/* Internals & statistics */ /* Internals & statistics */
int16_t rcAdjustment[4]; int16_t rcAdjustment[4];
float totalTripDistance; float totalTripDistance;