mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
add turn smoothing setting enum
This commit is contained in:
parent
cee00c57a6
commit
434b24d3a5
4 changed files with 16 additions and 8 deletions
|
@ -3284,11 +3284,11 @@ Waypoint tracking accuracy forces the craft to quickly head toward and track alo
|
||||||
|
|
||||||
### nav_fw_wp_turn_smoothing
|
### 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 |
|
| Default | Min | Max |
|
||||||
| --- | --- | --- |
|
| --- | --- | --- |
|
||||||
| 0 | 0 | 2 |
|
| OFF | | |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
|
|
@ -187,6 +187,9 @@ tables:
|
||||||
- name: rth_trackback_mode
|
- name: rth_trackback_mode
|
||||||
values: ["OFF", "ON", "FS"]
|
values: ["OFF", "ON", "FS"]
|
||||||
enum: rthTrackbackMode_e
|
enum: rthTrackbackMode_e
|
||||||
|
- name: nav_fw_wp_turn_smoothing
|
||||||
|
values: ["OFF", "ON", "ON-CUT"]
|
||||||
|
enum: wpFwTurnSmoothing_e
|
||||||
|
|
||||||
constants:
|
constants:
|
||||||
RPYL_PID_MIN: 0
|
RPYL_PID_MIN: 0
|
||||||
|
@ -2427,11 +2430,10 @@ groups:
|
||||||
min: 0
|
min: 0
|
||||||
max: 10
|
max: 10
|
||||||
- name: nav_fw_wp_turn_smoothing
|
- 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."
|
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: 0
|
default_value: "OFF"
|
||||||
field: fw.waypoint_turn_smoothing
|
field: fw.waypoint_turn_smoothing
|
||||||
min: 0
|
table: nav_fw_wp_turn_smoothing
|
||||||
max: 2
|
|
||||||
- name: nav_auto_speed
|
- 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]"
|
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
|
default_value: 300
|
||||||
|
|
|
@ -169,6 +169,12 @@ typedef enum {
|
||||||
RTH_TRACKBACK_FS,
|
RTH_TRACKBACK_FS,
|
||||||
} rthTrackbackMode_e;
|
} rthTrackbackMode_e;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
WP_TURN_SMOOTHING_OFF,
|
||||||
|
WP_TURN_SMOOTHING_ON,
|
||||||
|
WP_TURN_SMOOTHING_CUT,
|
||||||
|
} wpFwTurnSmoothing_e;
|
||||||
|
|
||||||
typedef struct positionEstimationConfig_s {
|
typedef struct positionEstimationConfig_s {
|
||||||
uint8_t automatic_mag_declination;
|
uint8_t automatic_mag_declination;
|
||||||
uint8_t reset_altitude_type; // from nav_reset_type_e
|
uint8_t reset_altitude_type; // from nav_reset_type_e
|
||||||
|
|
|
@ -295,7 +295,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
||||||
if (waypointTurnAngle > 3000 && waypointTurnAngle < 12000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
|
if (waypointTurnAngle > 3000 && waypointTurnAngle < 12000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
|
||||||
// turnFactor adjusts start of loiter based on turn angle
|
// turnFactor adjusts start of loiter based on turn angle
|
||||||
float turnFactor = 0.0f;
|
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;
|
turnFactor = waypointTurnAngle / 6000.0f;
|
||||||
} else {
|
} else {
|
||||||
turnFactor = tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)); // cut inside turn missing WP
|
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) {
|
if (posControl.wpDistance < navLoiterRadius * turnFactor) {
|
||||||
int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000);
|
int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000);
|
||||||
float distToTurnCentre = navLoiterRadius;
|
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));
|
distToTurnCentre = navLoiterRadius / cos_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f));
|
||||||
}
|
}
|
||||||
loiterCenterPos.x = posControl.activeWaypoint.pos.x + distToTurnCentre * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
|
loiterCenterPos.x = posControl.activeWaypoint.pos.x + distToTurnCentre * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue