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

enable embedded / rearmed JUMPs (#5515)

Browse source 
* Missions may contain multiple, embedded JUMPs
*The jump counter is reset to the designed value at the end of each jump so JUMP is re-entrant
* Save to EEPROM of a completed JUMP mission is correct and safe
This commit is contained in:
stronnag 2020-03-24 15:26:21 +00:00 • committed by GitHub
parent c872613fb8
commit 659fef8338
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 50 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -197,6 +197,10 @@ static void setupAltitudeController(void);
static void resetHeadingController(void);
void resetGCSFlags(void);
static void setupJumpCounters(void);
static void resetJumpCounter(void);
static void clearJumpCounters(void);
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
void calculateInitialHoldPosition(fpVector3_t * pos);
@ -1364,7 +1368,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
// Prevent I-terms growing when already landed
pidResetErrorAccumulators();
return NAV_FSM_EVENT_NONE;
}
@ -1382,7 +1385,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
// Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
resetAltitudeController(false);
setupAltitudeController();
/*
Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
Using p3 minimises the risk of saving an invalid counter if a mission is aborted.
*/
setupJumpCounters();
posControl.activeWaypointIndex = 0;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
}
@ -1402,9 +1409,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
// We use p3 as the volatile jump counter (p2 is the static value)
case NAV_WP_ACTION_JUMP:
if(posControl.waypointList[posControl.activeWaypointIndex].p2 != -1){
if(posControl.waypointList[posControl.activeWaypointIndex].p2 == 0){
if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
resetJumpCounter();
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
@ -1419,7 +1428,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
}
else
{
posControl.waypointList[posControl.activeWaypointIndex].p2--;
posControl.waypointList[posControl.activeWaypointIndex].p3--;
}
}
@ -1571,6 +1580,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
{
UNUSED(previousState);
clearJumpCounters();
// If no position sensor available - land immediately
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
return NAV_FSM_EVENT_NONE;
@ -2545,6 +2555,36 @@ static bool adjustAltitudeFromRCInput(void)
}
}
/*-----------------------------------------------------------
* Jump Counter support functions
*-----------------------------------------------------------*/
static void setupJumpCounters(void)
{
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
}
}
}
static void resetJumpCounter(void)
{
// reset the volatile counter from the set / static value
posControl.waypointList[posControl.activeWaypointIndex].p3 =
posControl.waypointList[posControl.activeWaypointIndex].p2;
}
static void clearJumpCounters(void)
{
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) {
posControl.waypointList[wp].p3 = 0;
}
}
}
/*-----------------------------------------------------------
* Heading controller (pass-through to MAG mode)
*-----------------------------------------------------------*/