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

add turn smoothing setting enum

Browse source
This commit is contained in:
breadoven 2022-08-12 12:35:57 +01:00
parent cee00c57a6
commit 434b24d3a5
4 changed files with 16 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

4
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 == 1) { // passes through WP
if (navConfig()->fw.waypoint_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 == 2) {
if (navConfig()->fw.waypoint_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));