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

Support WP LAND (#5485)

Browse source 
Add LAND way point type
This commit is contained in:
stronnag 2020-03-20 14:15:58 +00:00 • committed by GitHub
parent 588732d24c
commit 7655d1871e
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 21 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1396,6 +1396,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_HOLD_TIME:
case NAV_WP_ACTION_WAYPOINT:
case NAV_WP_ACTION_LAND:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
@ -1445,6 +1446,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_HOLD_TIME:
case NAV_WP_ACTION_WAYPOINT:
case NAV_WP_ACTION_LAND:
if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
@ -1504,6 +1506,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
else {
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
}
case NAV_WP_ACTION_LAND:
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
case NAV_WP_ACTION_HOLD_TIME:
// Save the current time for the time the waypoint was reached
posControl.wpReachedTime = millis();
@ -2685,7 +2691,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 || wpData->action == NAV_WP_ACTION_HOLD_TIME) {
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 || wpData->action == NAV_WP_ACTION_LAND) {
// 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;
@ -2827,12 +2833,13 @@ 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 || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)) {
if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
float wpSpecificSpeed = 0.0f;
if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT)
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
else
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2;
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
waypointSpeed = wpSpecificSpeed;
}
@ -3313,7 +3320,7 @@ void navigationUsePIDs(void)
0.0f,
NAV_DTERM_CUT_HZ
);
navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
@ -3428,6 +3435,9 @@ bool navigationIsFlyingAutonomousMode(void)
bool navigationRTHAllowsLanding(void)
{
if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)
return true;
navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
(allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));