Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-22 15:55:40 +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; 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. // Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP.
if (FLIGHT_MODE(NAV_WP_MODE) && !isLastMissionWaypoint()) { 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) { 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 // Check if WP reached when turn smoothing used
// Also 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 (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000 || posControl.wpReached) { if (navConfig()->fw.waypoint_turn_smoothing && posControl.activeWaypoint.bearingToNextWp != -1) {
posControl.wpReached = false; return posControl.wpReached;
} else if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000) {
return true; return true;
} }
} }
@ -3334,6 +3335,7 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos
} else { } else {
posControl.activeWaypoint.yaw = calculateBearingToDestination(pos); 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; posControl.activeWaypoint.pos = *pos;
@ -3351,6 +3353,13 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
fpVector3_t localPos; fpVector3_t localPos;
mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3)); mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
calculateAndSetActiveWaypointToLocalPosition(&localPos); 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 */ /* 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 { typedef struct {
fpVector3_t pos; fpVector3_t pos;
int32_t yaw; // deg * 100 int32_t yaw; // centidegrees
int32_t bearingToNextWp; // centidegrees
} navWaypointPosition_t; } navWaypointPosition_t;
typedef struct navDestinationPath_s { typedef struct navDestinationPath_s {

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

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

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 loadedMultiMissionStartWP; // selected multi mission start WP
int8_t loadedMultiMissionWPCount; // number of WPs in selected multi mission int8_t loadedMultiMissionWPCount; // number of WPs in selected multi mission
#endif #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; int8_t activeWaypointIndex;
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
@ -455,8 +455,6 @@ bool isNavHoldPositionActive(void);
bool isLastMissionWaypoint(void); bool isLastMissionWaypoint(void);
float getActiveWaypointSpeed(void); float getActiveWaypointSpeed(void);
bool isWaypointNavTrackingRoute(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 updateActualHeading(bool headingValid, int32_t newHeading);
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY); void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);