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 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
docs/Settings.md
View file

@ -3284,11 +3284,11 @@ Waypoint tracking accuracy forces the craft to quickly head toward and track alo
### nav_fw_wp_turn_smoothing
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.
Smooths turns during WP missions by switching to a loiter turn at waypoints. When set to ON the craft will reach the waypoint during the turn. When set to ON-CUT the craft will turn inside the waypoint without actually reaching it (cuts the corner).
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 2 |
| OFF | | |
---

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

@ -187,6 +187,9 @@ tables:
- name: rth_trackback_mode
values: ["OFF", "ON", "FS"]
enum: rthTrackbackMode_e
- name: nav_fw_wp_turn_smoothing
values: ["OFF", "ON", "ON-CUT"]
enum: wpFwTurnSmoothing_e
constants:
RPYL_PID_MIN: 0
@ -2427,11 +2430,10 @@ groups:
min: 0
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
description: "Smooths turns during WP missions by switching to a loiter turn at waypoints. When set to ON the craft will reach the waypoint during the turn. When set to ON-CUT the craft will turn inside the waypoint without actually reaching it (cuts the corner)."
default_value: "OFF"
field: fw.waypoint_turn_smoothing
min: 0
max: 2
table: nav_fw_wp_turn_smoothing
- name: nav_auto_speed
description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]"
default_value: 300

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

@ -169,6 +169,12 @@ typedef enum {
RTH_TRACKBACK_FS,
} rthTrackbackMode_e;
typedef enum {
WP_TURN_SMOOTHING_OFF,
WP_TURN_SMOOTHING_ON,
WP_TURN_SMOOTHING_CUT,
} wpFwTurnSmoothing_e;
typedef struct positionEstimationConfig_s {
uint8_t automatic_mag_declination;
uint8_t reset_altitude_type; // from nav_reset_type_e

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));