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:
commit
270c3d5867
2 changed files with 16 additions and 5 deletions
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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];
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue