Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-21 15:25:29 +03:00
Code Issues Wiki Activity

Code improvement

Browse source
This commit is contained in:
breadoven 2022-08-05 10:42:26 +01:00
parent 222ba4aea0
commit d2aea480c6
2 changed files with 16 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/fc/settings.yaml
View file

@ -2421,11 +2421,11 @@ groups:
min: 0
max: 9
- name: nav_fw_wp_tracking_accuracy
description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 5 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 2 is a good starting point. Set to 0 to disable."
description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 5 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable."
default_value: 0
field: fw.waypoint_tracking_accuracy
min: 0
max: 5
max: 10
- name: nav_fw_wp_turn_smoothing
description: "Smooths turns during WP missions by switching to a loiter turn at waypoints. 2 settings are possible. Setting 1 uses a loiter path that passes through the waypoint. Setting 2 uses a loiter path that cuts inside the turn without passing through the waypoint. Set to 0 to disable."
default_value: 0

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

@ -333,20 +333,17 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
} else if (navConfig()->fw.waypoint_tracking_accuracy && isWpTrackingActive) {
// track along waypoint course line if tracking accuracy used
fpVector3_t tempPos;
float tempPosErrorX;
float tempPosErrorY;
fpVector3_t currentWPPos = posControl.activeWaypoint.pos;
uint32_t currentCourse = posControl.activeWaypoint.yaw;
uint8_t distanceFactor;
uint8_t setting = navConfig()->fw.waypoint_tracking_accuracy;
if (currentCourse == 9000 || currentCourse == 27000) {
distanceFactor = constrain(4 + setting - (fabsf(posErrorX) / 750), setting, setting + 4);
tempPos.x = currentWPPos.x;
tempPos.y = navGetCurrentActualPositionAndVelocity()->pos.y + trackingDistance * distanceFactor * (currentCourse == 9000 ? 1 : -1);
tempPosErrorX = currentWPPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
tempPosErrorY = navGetCurrentActualPositionAndVelocity()->pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
} else if (currentCourse == 0 || currentCourse == 18000) {
distanceFactor = constrain(4 + setting - (fabsf(posErrorY) / 750), setting, setting + 4);
tempPos.x = navGetCurrentActualPositionAndVelocity()->pos.x + trackingDistance * distanceFactor * (currentCourse == 0 ? 1 : -1);
tempPos.y = currentWPPos.y;
tempPosErrorX = navGetCurrentActualPositionAndVelocity()->pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
tempPosErrorY = currentWPPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
} else {
// using y = mx + c
// constant2 = y axis intercept of line normal to course line passing through actual position
@ -354,19 +351,15 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
float gradient = tan_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.yaw));
float constant = currentWPPos.y - gradient * currentWPPos.x;
float constant2 = navGetCurrentActualPositionAndVelocity()->pos.y + navGetCurrentActualPositionAndVelocity()->pos.x / gradient;
tempPos.x = (constant2 - constant) / (gradient + 1 / gradient);
tempPos.y = gradient * tempPos.x + constant;
distanceFactor = constrain(4 + setting - (calculateDistanceToDestination(&tempPos) / 750), setting, setting + 4);
if (fabsf(gradient) > 1) { // increment in y
tempPos.y += trackingDistance * distanceFactor * (currentCourse < 18000 ? 1 : -1);
tempPos.x = (tempPos.y - constant) / gradient;
} else { // increment in x
tempPos.x += trackingDistance * distanceFactor * (currentCourse > 27000 || currentCourse < 9000 ? 1 : -1);
tempPos.y = gradient * tempPos.x + constant;
float tempPosX = (constant2 - constant) / (gradient + 1 / gradient);
tempPosErrorX = tempPosX - navGetCurrentActualPositionAndVelocity()->pos.x;
tempPosErrorY = (gradient * tempPosX + constant) - navGetCurrentActualPositionAndVelocity()->pos.y;
}
}
posErrorX = tempPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = tempPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
// uint32_t distanceline = calc_length_pythagorean_2D(tempPosErrorX, tempPosErrorY);
float distanceRatio = constrainf(posControl.wpDistance > 0 ? trackingDistance * navConfig()->fw.waypoint_tracking_accuracy / posControl.wpDistance :
0, 0.0f, 1.0f);
posErrorX = distanceRatio * (posErrorX - tempPosErrorX) + tempPosErrorX;
posErrorY = distanceRatio * (posErrorY - tempPosErrorY) + tempPosErrorY;
distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
}