Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 11:29:56 +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

10
docs/Navigation.md
View file

@ -72,7 +72,7 @@ Parameters:
* `<alt>` - Altitude in cm. * `<alt>` - Altitude in cm.
* `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP. * `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP **index** (not number).
* `<p2>` - For a POSHOLD TIME it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For JUMP it is the number of iterations of the JUMP. * `<p2>` - For a POSHOLD TIME it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For JUMP it is the number of iterations of the JUMP.
@ -82,7 +82,7 @@ Parameters:
`wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`). `wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`).
`wp reset` - Resets the list, sets the waypoints number to 0 and mark it as invalid (but doesn't delete the waypoints). `wp reset` - Resets the list, sets the number of waypoints to 0 and marks the list as invalid (but doesn't delete the waypoint definitions).
### `wp` example ### `wp` example
@ -95,10 +95,10 @@ wp 0 1 543533193 -45179273 3500 0 0 0 0
wp 1 1 543535723 -45193913 3500 0 0 0 0 wp 1 1 543535723 -45193913 3500 0 0 0 0
wp 2 1 543544541 -45196617 5000 0 0 0 0 wp 2 1 543544541 -45196617 5000 0 0 0 0
wp 3 1 543546578 -45186895 5000 0 0 0 0 wp 3 1 543546578 -45186895 5000 0 0 0 0
wp 4 6 0 0 0 2 2 0 0 wp 4 6 0 0 0 1 2 0 0
wp 5 1 543546688 -45176009 3500 0 0 0 0 wp 5 1 543546688 -45176009 3500 0 0 0 0
wp 6 1 543541225 -45172673 3500 0 0 0 0 wp 6 1 543541225 -45172673 3500 0 0 0 0
wp 7 6 0 0 0 1 1 0 0 wp 7 6 0 0 0 0 1 0 0
wp 8 3 543531383 -45190405 3500 45 0 0 0 wp 8 3 543531383 -45190405 3500 45 0 0 0
wp 9 1 543548470 -45182104 3500 0 0 0 0 wp 9 1 543548470 -45182104 3500 0 0 0 0
wp 10 8 543540521 -45178091 6000 0 0 0 165 wp 10 8 543540521 -45178091 6000 0 0 0 165
@ -106,3 +106,5 @@ wp 11 0 0 0 0 0 0 0 0
... ...
wp 59 0 0 0 0 0 0 0 0 wp 59 0 0 0 0 0 0 0 0
``` ```
Note that the `wp` CLI command shows waypoint list indices, while the MW-XML definition used by mwp, ezgui and the configurator use WP numbers.

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 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->lon = wpLLH.lon;
wpData->alt = wpLLH.alt; 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)) { else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
if (wpNumber <= posControl.waypointCount) { if (wpNumber <= posControl.waypointCount) {
*wpData = posControl.waypointList[wpNumber - 1]; *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) // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) { if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
posControl.waypointList[wpNumber - 1] = *wpData; 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.waypointCount = wpNumber;
posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST); 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 // 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) { if (posControl.waypointCount > 0) {
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){ for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){ 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; return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
} }
} }