mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 08:45:31 +03:00
Waypoint hold time feature (#5398)
* Added support for waypoint hold time
This commit is contained in:
parent
8b54326d90
commit
6eda2af359
7 changed files with 75 additions and 11 deletions
|
@ -66,7 +66,9 @@ Parameters:
|
||||||
|
|
||||||
* `<alt>` - Altitude in cm.
|
* `<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.
|
* `<flag>` - Last waypoint must have set `flag` to 165 (0xA5), otherwise 0.
|
||||||
|
|
||||||
|
|
|
@ -1322,7 +1322,7 @@ static void cliWaypoints(char *cmdline)
|
||||||
} else if (sl_strcasecmp(cmdline, "save") == 0) {
|
} else if (sl_strcasecmp(cmdline, "save") == 0) {
|
||||||
posControl.waypointListValid = false;
|
posControl.waypointListValid = false;
|
||||||
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
|
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) {
|
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
|
||||||
posControl.waypointCount = i + 1;
|
posControl.waypointCount = i + 1;
|
||||||
posControl.waypointListValid = true;
|
posControl.waypointListValid = true;
|
||||||
|
@ -1374,7 +1374,7 @@ static void cliWaypoints(char *cmdline)
|
||||||
}
|
}
|
||||||
if (validArgumentCount < 4) {
|
if (validArgumentCount < 4) {
|
||||||
cliShowParseError();
|
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();
|
cliShowParseError();
|
||||||
} else {
|
} else {
|
||||||
posControl.waypointList[i].action = action;
|
posControl.waypointList[i].action = action;
|
||||||
|
|
|
@ -787,7 +787,8 @@ static const char * navigationStateMessage(void)
|
||||||
// Used by HOLD flight modes. No information to add.
|
// Used by HOLD flight modes. No information to add.
|
||||||
break;
|
break;
|
||||||
case MW_NAV_STATE_HOLD_TIMED:
|
case MW_NAV_STATE_HOLD_TIMED:
|
||||||
// Not used anymore
|
// TODO: Maybe we can display a count down
|
||||||
|
return OSD_MESSAGE_STR("HOLDING WAYPOINT");
|
||||||
break;
|
break;
|
||||||
case MW_NAV_STATE_WP_ENROUTE:
|
case MW_NAV_STATE_WP_ENROUTE:
|
||||||
// TODO: Show WP number
|
// TODO: Show WP number
|
||||||
|
|
|
@ -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_PRE_ACTION(navigationFSMState_t previousState);
|
||||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(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_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_NEXT(navigationFSMState_t previousState);
|
||||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(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);
|
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 = {
|
.onEvent = {
|
||||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state
|
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state
|
||||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT,
|
[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_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
|
||||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
|
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
|
||||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
[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] = {
|
[NAV_STATE_WAYPOINT_RTH_LAND] = {
|
||||||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
|
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
|
||||||
.onEntry = navOnEnteringState_NAV_STATE_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);
|
UNUSED(previousState);
|
||||||
|
|
||||||
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
|
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
|
||||||
|
case NAV_WP_ACTION_HOLD_TIME:
|
||||||
case NAV_WP_ACTION_WAYPOINT:
|
case NAV_WP_ACTION_WAYPOINT:
|
||||||
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
|
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
|
||||||
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
|
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 no position sensor available - land immediately
|
||||||
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||||
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
|
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
|
||||||
|
case NAV_WP_ACTION_HOLD_TIME:
|
||||||
case NAV_WP_ACTION_WAYPOINT:
|
case NAV_WP_ACTION_WAYPOINT:
|
||||||
if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
|
if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
|
||||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
|
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 {
|
else {
|
||||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
|
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();
|
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)
|
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
|
||||||
{
|
{
|
||||||
UNUSED(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
|
// WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
|
||||||
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
|
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)
|
// 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;
|
||||||
|
@ -2768,6 +2812,11 @@ bool isApproachingLastWaypoint(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool isWaypointWait(void)
|
||||||
|
{
|
||||||
|
return NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
|
||||||
|
}
|
||||||
|
|
||||||
float getActiveWaypointSpeed(void)
|
float getActiveWaypointSpeed(void)
|
||||||
{
|
{
|
||||||
if (posControl.flags.isAdjustingPosition) {
|
if (posControl.flags.isAdjustingPosition) {
|
||||||
|
@ -2778,8 +2827,12 @@ float getActiveWaypointSpeed(void)
|
||||||
uint16_t waypointSpeed = navConfig()->general.max_auto_speed;
|
uint16_t waypointSpeed = navConfig()->general.max_auto_speed;
|
||||||
|
|
||||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
||||||
if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) {
|
if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)) {
|
||||||
const float wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
|
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) {
|
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
|
||||||
waypointSpeed = wpSpecificSpeed;
|
waypointSpeed = wpSpecificSpeed;
|
||||||
}
|
}
|
||||||
|
|
|
@ -233,9 +233,10 @@ typedef struct gpsOrigin_s {
|
||||||
} gpsOrigin_t;
|
} gpsOrigin_t;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
NAV_WP_ACTION_WAYPOINT = 0x01,
|
NAV_WP_ACTION_WAYPOINT = 0x01,
|
||||||
NAV_WP_ACTION_JUMP = 0x06,
|
NAV_WP_ACTION_HOLD_TIME = 0x03,
|
||||||
NAV_WP_ACTION_RTH = 0x04
|
NAV_WP_ACTION_JUMP = 0x06,
|
||||||
|
NAV_WP_ACTION_RTH = 0x04
|
||||||
} navWaypointActions_e;
|
} navWaypointActions_e;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
|
|
@ -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
|
// 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
|
#define TAN_15DEG 0.26795f
|
||||||
bool needToCalculateCircularLoiter = isApproachingLastWaypoint()
|
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
|
||||||
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
|
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
|
||||||
&& (distanceToActualTarget > 50.0f)
|
&& (distanceToActualTarget > 50.0f)
|
||||||
&& !FLIGHT_MODE(NAV_CRUISE_MODE);
|
&& !FLIGHT_MODE(NAV_CRUISE_MODE);
|
||||||
|
|
|
@ -139,9 +139,11 @@ typedef enum {
|
||||||
|
|
||||||
NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event
|
NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event
|
||||||
NAV_FSM_EVENT_STATE_SPECIFIC_2, // 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_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_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_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_2D,
|
||||||
NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D,
|
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_INITIALIZE = 32,
|
||||||
NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS = 33,
|
NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS = 33,
|
||||||
NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING = 34,
|
NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING = 34,
|
||||||
|
|
||||||
|
NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35,
|
||||||
} navigationPersistentId_e;
|
} navigationPersistentId_e;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
@ -224,6 +228,7 @@ typedef enum {
|
||||||
NAV_STATE_WAYPOINT_PRE_ACTION,
|
NAV_STATE_WAYPOINT_PRE_ACTION,
|
||||||
NAV_STATE_WAYPOINT_IN_PROGRESS,
|
NAV_STATE_WAYPOINT_IN_PROGRESS,
|
||||||
NAV_STATE_WAYPOINT_REACHED,
|
NAV_STATE_WAYPOINT_REACHED,
|
||||||
|
NAV_STATE_WAYPOINT_HOLD_TIME,
|
||||||
NAV_STATE_WAYPOINT_NEXT,
|
NAV_STATE_WAYPOINT_NEXT,
|
||||||
NAV_STATE_WAYPOINT_FINISHED,
|
NAV_STATE_WAYPOINT_FINISHED,
|
||||||
NAV_STATE_WAYPOINT_RTH_LAND,
|
NAV_STATE_WAYPOINT_RTH_LAND,
|
||||||
|
@ -357,6 +362,7 @@ typedef struct {
|
||||||
float wpInitialAltitude; // Altitude at start of WP
|
float wpInitialAltitude; // Altitude at start of WP
|
||||||
float wpInitialDistance; // Distance when starting flight to WP
|
float wpInitialDistance; // Distance when starting flight to WP
|
||||||
float wpDistance; // Distance to active WP
|
float wpDistance; // Distance to active WP
|
||||||
|
timeMs_t wpReachedTime; // Time the waypoint was reached
|
||||||
|
|
||||||
/* Internals & statistics */
|
/* Internals & statistics */
|
||||||
int16_t rcAdjustment[4];
|
int16_t rcAdjustment[4];
|
||||||
|
@ -393,6 +399,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
||||||
|
|
||||||
bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome);
|
bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome);
|
||||||
bool isWaypointMissed(const navWaypointPosition_t * waypoint);
|
bool isWaypointMissed(const navWaypointPosition_t * waypoint);
|
||||||
|
bool isWaypointWait(void);
|
||||||
bool isApproachingLastWaypoint(void);
|
bool isApproachingLastWaypoint(void);
|
||||||
float getActiveWaypointSpeed(void);
|
float getActiveWaypointSpeed(void);
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue