mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 14:25:16 +03:00
Waypoint hold time feature (#5398)
* Added support for waypoint hold time
This commit is contained in:
Jacky2k
GitHub
parent
8b54326d90
commit
6eda2af359
7 changed files with 75 additions and 11 deletions
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue