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

Waypoint hold time feature (#5398)

Browse source 
* Added support for waypoint hold time
This commit is contained in:
Jacky2k 2020-03-09 15:21:26 +01:00 • committed by GitHub
parent 8b54326d90
commit 6eda2af359
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 75 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

4
docs/Navigation.md
View file

@ -66,7 +66,9 @@ Parameters:
* `<alt>` - Altitude in cm.
* `<p1>` - For a "RTH waypoint" p1 > 0 alow landing. For a normal waypoint it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed.
* `<p1>` - For a "RTH waypoint" p1 > 0 alow landing. For a normal waypoint it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time time to wait in seconds.
* `<p2>` - For a POSHOLD TIME it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed.
* `<flag>` - Last waypoint must have set `flag` to 165 (0xA5), otherwise 0.

4
src/main/fc/cli.c
View file

@ -1322,7 +1322,7 @@ static void cliWaypoints(char *cmdline)
} else if (sl_strcasecmp(cmdline, "save") == 0) {
posControl.waypointListValid = false;
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH)) break;
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME)) break;
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
posControl.waypointCount = i + 1;
posControl.waypointListValid = true;
@ -1374,7 +1374,7 @@ static void cliWaypoints(char *cmdline)
}
if (validArgumentCount < 4) {
cliShowParseError();
} else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
} else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_HOLD_TIME) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
cliShowParseError();
} else {
posControl.waypointList[i].action = action;

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

@ -787,7 +787,8 @@ static const char * navigationStateMessage(void)
// Used by HOLD flight modes. No information to add.
break;
case MW_NAV_STATE_HOLD_TIMED:
// Not used anymore
// TODO: Maybe we can display a count down
return OSD_MESSAGE_STR("HOLDING WAYPOINT");
break;
case MW_NAV_STATE_WP_ENROUTE:
// TODO: Show WP number

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

@ -231,6 +231,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState);
@ -665,6 +666,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
@ -677,6 +679,27 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
}
},
[NAV_STATE_WAYPOINT_HOLD_TIME] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold?
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_HOLD_TIMED,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOLD_TIME, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, // successfully reached waypoint
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
[NAV_STATE_WAYPOINT_RTH_LAND] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
@ -1371,6 +1394,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
UNUSED(previousState);
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_HOLD_TIME:
case NAV_WP_ACTION_WAYPOINT:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
@ -1419,6 +1443,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
// If no position sensor available - land immediately
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_HOLD_TIME:
case NAV_WP_ACTION_WAYPOINT:
if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
@ -1479,11 +1504,30 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
else {
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
}
case NAV_WP_ACTION_HOLD_TIME:
// Save the current time for the time the waypoint was reached
posControl.wpReachedTime = millis();
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
}
UNREACHABLE();
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState)
{
UNUSED(previousState);
timeMs_t currentTime = millis();
if(posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0)
return NAV_FSM_EVENT_SUCCESS;
if(posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)
return NAV_FSM_EVENT_SUCCESS;
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
{
UNUSED(previousState);
@ -2641,7 +2685,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
}
// WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
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) {
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) {
// 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;
@ -2768,6 +2812,11 @@ bool isApproachingLastWaypoint(void)
}
}
bool isWaypointWait(void)
{
return NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
}
float getActiveWaypointSpeed(void)
{
if (posControl.flags.isAdjustingPosition) {
@ -2778,8 +2827,12 @@ float getActiveWaypointSpeed(void)
uint16_t waypointSpeed = navConfig()->general.max_auto_speed;
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) {
const float wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)) {
float wpSpecificSpeed = 0.0f;
if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT)
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
else
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2;
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
waypointSpeed = wpSpecificSpeed;
}

7
src/main/navigation/navigation.h
View file

@ -233,9 +233,10 @@ typedef struct gpsOrigin_s {
} gpsOrigin_t;
typedef enum {
NAV_WP_ACTION_WAYPOINT = 0x01,
NAV_WP_ACTION_JUMP = 0x06,
NAV_WP_ACTION_RTH = 0x04
NAV_WP_ACTION_WAYPOINT = 0x01,
NAV_WP_ACTION_HOLD_TIME = 0x03,
NAV_WP_ACTION_JUMP = 0x06,
NAV_WP_ACTION_RTH = 0x04
} navWaypointActions_e;
typedef enum {

2
src/main/navigation/navigation_fixedwing.c
View file

@ -243,7 +243,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
#define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = isApproachingLastWaypoint()
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f)
&& !FLIGHT_MODE(NAV_CRUISE_MODE);

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

@ -139,9 +139,11 @@ typedef enum {
NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event
NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event
NAV_FSM_EVENT_STATE_SPECIFIC_3, // State-specific event
NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D,
NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D,
@ -198,6 +200,8 @@ typedef enum {
NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE = 32,
NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS = 33,
NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING = 34,
NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35,
} navigationPersistentId_e;
typedef enum {
@ -224,6 +228,7 @@ typedef enum {
NAV_STATE_WAYPOINT_PRE_ACTION,
NAV_STATE_WAYPOINT_IN_PROGRESS,
NAV_STATE_WAYPOINT_REACHED,
NAV_STATE_WAYPOINT_HOLD_TIME,
NAV_STATE_WAYPOINT_NEXT,
NAV_STATE_WAYPOINT_FINISHED,
NAV_STATE_WAYPOINT_RTH_LAND,
@ -357,6 +362,7 @@ typedef struct {
float wpInitialAltitude; // Altitude at start of WP
float wpInitialDistance; // Distance when starting flight to WP
float wpDistance; // Distance to active WP
timeMs_t wpReachedTime; // Time the waypoint was reached
/* Internals & statistics */
int16_t rcAdjustment[4];
@ -393,6 +399,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome);
bool isWaypointMissed(const navWaypointPosition_t * waypoint);
bool isWaypointWait(void);
bool isApproachingLastWaypoint(void);
float getActiveWaypointSpeed(void);