Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

[NAV] allow forward JUMP WPs (#5591)

Browse source 
Alllow forward as well as backward jumps
This commit is contained in:
stronnag 2020-04-13 11:08:15 +01:00 • committed by GitHub
parent b47d666702
commit 5d787cb5c9
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 8 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1384,7 +1384,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
if (STATE(ALTITUDE_CONTROL)) { if (STATE(ALTITUDE_CONTROL)) {
updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL); // FIXME updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL); // FIXME
} }
// Prevent I-terms growing when already landed // Prevent I-terms growing when already landed
pidResetErrorAccumulators(); pidResetErrorAccumulators();
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
@ -3236,12 +3236,16 @@ 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 * Don't allow arming if any of JUMP waypoint has invalid settings
* First WP can't be JUMP
* Can't jump to immediately adjacent WPs (pointless)
* Can't jump beyond WP list
*/
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((wp < 2) || (posControl.waypointList[wp].p1 > (wp-2)) || (posControl.waypointList[wp].p2 < -1)) { if((wp == 0) || ((posControl.waypointList[wp].p1 > (wp-2)) && (posControl.waypointList[wp].p1 < (wp+2)) ) || (posControl.waypointList[wp].p1 >= posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)) {
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR; return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
} }
} }