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

Display only geospatial WPs on OSD

Browse source 
Fix to ensure only geospatial WPs are displayed on OSD with sequential numbering maintained around nongeo WPs.
This commit is contained in:
breadoven 2020-12-12 23:51:38 +00:00
parent 99bfeeade7
commit 1a104a8f2a
3 changed files with 30 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1383,6 +1383,19 @@ static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY,
displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr);
}
int8_t getGeoWaypointNumber(int8_t waypointIndex)
{
if (posControl.waypointList[waypointIndex].flag == NAV_WP_FLAG_LAST) {
if ((posControl.waypointList[waypointIndex].action == NAV_WP_ACTION_JUMP || posControl.waypointList[waypointIndex].action == NAV_WP_ACTION_SET_POI || posControl.waypointList[waypointIndex].action == NAV_WP_ACTION_SET_HEAD)) {
return posControl.waypointList[waypointIndex - 1].flag;
} else {
return posControl.waypointList[waypointIndex - 1].flag + 1;
}
} else {
return posControl.waypointList[waypointIndex].flag;
}
}
static bool osdDrawSingleElement(uint8_t item)
{
uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item];
@ -3833,7 +3846,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
// Countdown display for remaining Waypoints
char buf[6];
osdFormatDistanceSymbol(buf, posControl.wpDistance, 0);
tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", posControl.activeWaypointIndex + 1, posControl.waypointCount, buf);
tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf);
messages[messageCount++] = messageBuf;
} else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
// WP hold time countdown in seconds

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

@ -2833,13 +2833,25 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) {
// Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
static int8_t nonGeoWaypointCount = 0;
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
posControl.waypointList[wpNumber - 1] = *wpData;
if(wpData->action == NAV_WP_ACTION_JUMP) {
if (wpData->action == NAV_WP_ACTION_JUMP) {
posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
nonGeoWaypointCount += 1;
}
if (wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD) {
nonGeoWaypointCount += 1;
}
posControl.waypointCount = wpNumber;
posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount;
if (posControl.waypointListValid) {
nonGeoWaypointCount = 0;
} else {
posControl.waypointList[wpNumber - 1].flag = wpNumber - nonGeoWaypointCount;
}
}
}
}
@ -2851,6 +2863,7 @@ void resetWaypointList(void)
if (!ARMING_FLAG(ARMED)) {
posControl.waypointCount = 0;
posControl.waypointListValid = false;
posControl.geoWaypointCount = 0;
}
}

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

@ -356,7 +356,8 @@ typedef struct {
navWaypoint_t waypointList[NAV_MAX_WAYPOINTS];
bool waypointListValid;
int8_t waypointCount;
int8_t geoWaypointCount; // total geospatial WPs in mission
navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation
int8_t activeWaypointIndex;
float wpInitialAltitude; // Altitude at start of WP