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

[NAV] store jump targets as indices (vice WP #) for consistency (#5569)

Browse source 
* [NAV] store jump targets as indices (vice WP #) for consistency

* [NAV] improve JUMP validity test
This commit is contained in:
stronnag 2020-04-06 18:33:58 +01:00 • committed by GitHub
parent fe5ada21ad
commit 2ff5ced4ee
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 17 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1432,7 +1432,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
}
}
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 - 1;
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
@ -2679,10 +2679,14 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
wpData->lon = wpLLH.lon;
wpData->alt = wpLLH.alt;
}
// WP #1 - #15 - common waypoints - pre-programmed mission
// WP #1 - #60 - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
if (wpNumber <= posControl.waypointCount) {
*wpData = posControl.waypointList[wpNumber - 1];
if(wpData->action == NAV_WP_ACTION_JUMP) {
wpData->p1 += 1; // make WP # (vice index)
}
}
}
}
@ -2730,6 +2734,9 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
// Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
posControl.waypointList[wpNumber - 1] = *wpData;
if(wpData->action == NAV_WP_ACTION_JUMP) {
posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
}
posControl.waypointCount = wpNumber;
posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
}
@ -3211,10 +3218,11 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
}
// Don't allow arming if any of JUMP waypoint has invalid settings
// Note JUMP only goes to previous WPs, which must be 2 indices back
if (posControl.waypointCount > 0) {
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
if((posControl.waypointList[wp].p1 > posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)){
if((wp < 2) || (posControl.waypointList[wp].p1 > (wp-2)) || (posControl.waypointList[wp].p2 < -1)) {
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
}
}