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

Add setting max convergence angle

Browse source
This commit is contained in:
breadoven 2022-08-14 13:32:21 +01:00
parent 67adc42e75
commit 15b82352b4
5 changed files with 31 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -295,7 +295,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
if (waypointTurnAngle > 3000 && waypointTurnAngle < 12000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// turnFactor adjusts start of loiter based on turn angle
float turnFactor = 0.0f;
if (navConfig()->fw.waypoint_turn_smoothing == WP_TURN_SMOOTHING_ON) { // passes through WP
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { // passes through WP
turnFactor = waypointTurnAngle / 6000.0f;
} else {
turnFactor = tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)); // cut inside turn missing WP
@ -304,7 +304,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
if (posControl.wpDistance < navLoiterRadius * turnFactor) {
int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000);
float distToTurnCentre = navLoiterRadius;
if (navConfig()->fw.waypoint_turn_smoothing == WP_TURN_SMOOTHING_CUT) {
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) {
distToTurnCentre = navLoiterRadius / cos_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f));
}
loiterCenterPos.x = posControl.activeWaypoint.pos.x + distToTurnCentre * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
@ -395,17 +395,16 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
/* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
if (navConfig()->fw.waypoint_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// only apply course tracking correction if target bearing error < 90 degs or when close to waypoint (within 10m)
if (ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) {
// courseVirtualCorrection initially used to determine current position relative to course line for later use
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
// bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
// initial courseCorrectionFactor based on distance to course line
float courseCorrectionFactor = constrainf(distToCourseLine / (1000.0f * navConfig()->fw.waypoint_tracking_accuracy), 0.0f, 1.0f);
float courseCorrectionFactor = constrainf(distToCourseLine / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
// course heading alignment factor
@ -417,7 +416,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
// final courseVirtualCorrection using max 80 deg heading adjustment toward course line
courseVirtualCorrection = 8000 * courseCorrectionFactor;
courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor;
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection);
}
}