1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-21 07:15:16 +03:00

Merge pull request #5226 from hali9/wp-linear-climb-dive

WP linear climb dive
This commit is contained in:
Konstantin Sharlaimov 2019-11-29 08:22:45 +01:00 committed by GitHub
commit 270c3d5867
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 16 additions and 5 deletions

View file

@ -1368,6 +1368,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
switch (posControl.waypointList[posControl.activeWaypointIndex].action) { switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
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.wpInitialAltitude = posControl.actualState.abs.pos.z;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
@ -1392,7 +1394,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
} }
else { else {
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); fpVector3_t tmpWaypoint;
tmpWaypoint.x = posControl.activeWaypoint.pos.x;
tmpWaypoint.y = posControl.activeWaypoint.pos.y;
tmpWaypoint.z = scaleRange(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10, posControl.wpInitialDistance),
posControl.wpInitialDistance, posControl.wpInitialDistance / 10,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
} }
break; break;
@ -2040,15 +2048,15 @@ bool isWaypointMissed(const navWaypointPosition_t * waypoint)
static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome) static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome)
{ {
// We consider waypoint reached if within specified radius // We consider waypoint reached if within specified radius
const uint32_t wpDistance = calculateDistanceToDestination(pos); posControl.wpDistance = calculateDistanceToDestination(pos);
if (STATE(FIXED_WING) && isWaypointHome) { if (STATE(FIXED_WING) && isWaypointHome) {
// Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
return (wpDistance <= navConfig()->general.waypoint_radius) return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
|| (wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius || (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius
} }
else { else {
return (wpDistance <= navConfig()->general.waypoint_radius); return (posControl.wpDistance <= navConfig()->general.waypoint_radius);
} }
} }

View file

@ -354,6 +354,9 @@ typedef struct {
navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation
int8_t activeWaypointIndex; int8_t activeWaypointIndex;
float wpInitialAltitude; // Altitude at start of WP
float wpInitialDistance; // Distance when starting flight to WP
float wpDistance; // Distance to active WP
/* Internals & statistics */ /* Internals & statistics */
int16_t rcAdjustment[4]; int16_t rcAdjustment[4];