Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +03:00
Code Issues Wiki Activity

add limits for turn smoothing

Browse source
This commit is contained in:
breadoven 2022-07-18 22:47:36 +01:00
parent a873fd06cc
commit 81fb470f4e
4 changed files with 36 additions and 28 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2215,7 +2215,7 @@ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3
return true;
}
bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
{
// Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP.
if (FLIGHT_MODE(NAV_WP_MODE) && !isLastMissionWaypoint()) {
@ -2258,10 +2258,11 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
}
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
// Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypointYaw
// Also check if WP reached when turn smoothing used
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000 || posControl.wpReached) {
posControl.wpReached = false;
// Check if WP reached when turn smoothing used
// Otherwise check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
if (navConfig()->fw.waypoint_turn_smoothing && posControl.activeWaypoint.bearingToNextWp != -1) {
return posControl.wpReached;
} else if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000) {
return true;
}
}
@ -3334,6 +3335,7 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos
} else {
posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
}
posControl.activeWaypoint.bearingToNextWp = -1; // reset to NO next WP (-1), will be set by WP mode as required
posControl.activeWaypoint.pos = *pos;
@ -3351,6 +3353,13 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
fpVector3_t localPos;
mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
calculateAndSetActiveWaypointToLocalPosition(&localPos);
if (navConfig()->fw.waypoint_turn_smoothing) {
fpVector3_t posNextWp;
if (getLocalPosNextWaypoint(&posNextWp)) {
posControl.activeWaypoint.bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp);
}
}
}
/* Checks if active waypoint is last in mission */

3
src/main/navigation/navigation.h
View file

@ -375,7 +375,8 @@ extern radar_pois_t radar_pois[RADAR_MAX_POIS];
typedef struct {
fpVector3_t pos;
int32_t yaw; // deg * 100
int32_t yaw; // centidegrees
int32_t bearingToNextWp; // centidegrees
} navWaypointPosition_t;
typedef struct navDestinationPath_s {

24
src/main/navigation/navigation_fixedwing.c
View file

@ -287,30 +287,30 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
/* WP turn smoothing option - switch to loiter path when distance to waypoint < navLoiterRadius.
* Loiter centered on point inside turn at navLoiterRadius distance from waypoint and
* on a bearing midway between current and next waypoint course bearings */
if (navConfig()->fw.waypoint_turn_smoothing && isWaypointNavTrackingRoute() && !needToCalculateCircularLoiter) {
if (posControl.wpDistance < navLoiterRadius) {
fpVector3_t posNextWp;
if (getLocalPosNextWaypoint(&posNextWp)) {
int32_t bearingNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp);
int32_t loiterCenterBearing = wrap_36000(((wrap_18000(bearingNextWp - posControl.activeWaypoint.yaw - 18000)) / 2) +
posControl.activeWaypoint.yaw + 18000);
* on a bearing midway between current and next waypoint course bearings
* Only for turns > 30 degs, navLoiterRadius factored down between 30 to 60 degs to align with course line */
if (navConfig()->fw.waypoint_turn_smoothing && isWaypointNavTrackingRoute() && !needToCalculateCircularLoiter &&
posControl.activeWaypoint.bearingToNextWp != -1) {
int32_t turnAngle = wrap_18000(posControl.activeWaypoint.bearingToNextWp - posControl.activeWaypoint.yaw);
float turnFactor = ABS(turnAngle) < 3000 ? 0.0f : constrainf(ABS(turnAngle) / 6000.0f, 0.5f, 1.0f);
if (posControl.wpDistance < navLoiterRadius * turnFactor) {
int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.bearingToNextWp - posControl.activeWaypoint.yaw - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000);
loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
// turn direction to next waypoint
turnDirection = wrap_18000(bearingNextWp - posControl.activeWaypoint.yaw) > 0 ? 1 : -1; // 1 = right
// if waypoint not reached based on distance from waypoint then waypoint considered
// reached if difference between actual heading and waypoint bearing exceeds 90 degs
posControl.wpReached = ABS(wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw)) > 9000;
// turn direction to next waypoint
turnDirection = turnAngle > 0 ? 1 : -1; // 1 = right
needToCalculateCircularLoiter = true;
}
}
}
// We are closing in on a waypoint, calculate circular loiter if required
if (needToCalculateCircularLoiter) {

4
src/main/navigation/navigation_private.h
View file

@ -396,7 +396,7 @@ typedef struct {
int8_t loadedMultiMissionStartWP; // selected multi mission start WP
int8_t loadedMultiMissionWPCount; // number of WPs in selected multi mission
#endif
navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation
navWaypointPosition_t activeWaypoint; // Local position, current bearing and bearing to next WP, filled on waypoint activation
int8_t activeWaypointIndex;
float wpInitialAltitude; // Altitude at start of WP
float wpInitialDistance; // Distance when starting flight to WP
@ -455,8 +455,6 @@ bool isNavHoldPositionActive(void);
bool isLastMissionWaypoint(void);
float getActiveWaypointSpeed(void);
bool isWaypointNavTrackingRoute(void);
bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos);
int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, const fpVector3_t * endPos);
void updateActualHeading(bool headingValid, int32_t newHeading);
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);