mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-20 06:45:14 +03:00
turn smoothing changed method
This commit is contained in:
breadoven
parent
ec381c24ed
commit
a873fd06cc
7 changed files with 64 additions and 38 deletions
|
|
@ -3282,13 +3282,13 @@ Sets a reference distance to the waypoint course line that affects the accuracy
|
|||
|
||||
---
|
||||
|
||||
### nav_fw_wp_turn_smoothing_dist
|
||||
### nav_fw_wp_turn_smoothing
|
||||
|
||||
Provides a means of smoothing waypoint turns by reaching waypoints earlier as turns become tighter. Applied as an increasing multiple of nav_fw_wp_turn_smoothing_dist [cm] starting at 1 for turns exceeding 45 degrees to a maximum of 10 for turns of 90 degrees and over, e.g. a 70 degree turn is reached when 6 x nav_fw_wp_turn_smoothing_dist from waypoint. 200 is a good starting point. Set to 0 to disable turn smoothing.
|
||||
Smooths turns during WP missions by switching to a loiter path when within the nav_fw_loiter_radius from the waypoint. The loiter is centered relative to the waypoint such that the craft passes through the waypoint during the loiter at which point the waypoint is considered to have been reached.
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 0 | 0 | 2000 |
|
||||
| OFF | OFF | ON |
|
||||
|
||||
---
|
||||
|
||||
|
|
|
|||
|
|
@ -199,7 +199,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
|
|||
OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX),
|
||||
#endif
|
||||
OSD_SETTING_ENTRY("MISSION RESTART", SETTING_NAV_WP_MISSION_RESTART),
|
||||
OSD_SETTING_ENTRY("TURN SMOOTHING DIST", SETTING_NAV_FW_WP_TURN_SMOOTHING_DIST),
|
||||
OSD_SETTING_ENTRY("WP TURN SMOOTHING", SETTING_NAV_FW_WP_TURN_SMOOTHING),
|
||||
OSD_SETTING_ENTRY("WP TRACKING ACCURACY", SETTING_NAV_FW_WP_TRACKING_ACCURACY),
|
||||
OSD_BACK_AND_END_ENTRY,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -2426,12 +2426,11 @@ groups:
|
|||
field: fw.waypoint_tracking_accuracy
|
||||
min: 0
|
||||
max: 100
|
||||
- name: nav_fw_wp_turn_smoothing_dist
|
||||
description: "Provides a means of smoothing waypoint turns by reaching waypoints earlier as turns become tighter. Applied as an increasing multiple of nav_fw_wp_turn_smoothing_dist [cm] starting at 1 for turns exceeding 45 degrees to a maximum of 10 for turns of 90 degrees and over, e.g. a 70 degree turn is reached when 6 x nav_fw_wp_turn_smoothing_dist from waypoint. 200 is a good starting point. Set to 0 to disable turn smoothing."
|
||||
default_value: 0
|
||||
field: fw.wp_turn_smoothing_dist
|
||||
min: 0
|
||||
max: 2000
|
||||
- name: nav_fw_wp_turn_smoothing
|
||||
description: "Smooths turns during WP missions by switching to a loiter path when within the nav_fw_loiter_radius from the waypoint. The loiter is centered relative to the waypoint such that the craft passes through the waypoint during the loiter at which point the waypoint is considered to have been reached."
|
||||
default_value: OFF
|
||||
field: fw.waypoint_turn_smoothing
|
||||
type: bool
|
||||
- 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
|
||||
|
|
|
|||
|
|
@ -209,8 +209,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
|
||||
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
|
||||
.auto_disarm_delay = SETTING_NAV_FW_AUTO_DISARM_DELAY_DEFAULT, // ms - time delay to disarm when auto disarm after landing enabled
|
||||
.waypoint_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0 m
|
||||
.wp_turn_smoothing_dist = SETTING_NAV_FW_WP_TURN_SMOOTHING_DIST_DEFAULT, // 0 cm
|
||||
.waypoint_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0 m, improves course tracking during FW WP missions
|
||||
.waypoint_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // OFF, smooths turns during FW WP mode missions
|
||||
}
|
||||
);
|
||||
|
||||
|
|
@ -1203,6 +1203,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) {
|
||||
updateRthTrackback(true); // save final trackpoint for altitude and max trackback distance reference
|
||||
posControl.flags.rthTrackbackActive = true;
|
||||
posControl.wpReached = false;
|
||||
calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
|
||||
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK;
|
||||
}
|
||||
|
|
@ -1569,6 +1570,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
|
||||
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
|
||||
posControl.wpAltitudeReached = false;
|
||||
posControl.wpReached = false;
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
|
||||
|
||||
// We use p3 as the volatile jump counter (p2 is the static value)
|
||||
|
|
@ -2213,7 +2215,7 @@ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3
|
|||
return true;
|
||||
}
|
||||
|
||||
static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
|
||||
bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
|
||||
{
|
||||
// Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP.
|
||||
if (FLIGHT_MODE(NAV_WP_MODE) && !isLastMissionWaypoint()) {
|
||||
|
|
@ -2255,25 +2257,16 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
|
|||
return true;
|
||||
}
|
||||
|
||||
uint16_t turnEarlyDistance = 0;
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
|
||||
// Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypointYaw
|
||||
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000) {
|
||||
// Also check if WP reached when turn smoothing used
|
||||
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000 || posControl.wpReached) {
|
||||
posControl.wpReached = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
// fixed wing option to reach waypoint earlier to help smooth tighter turns to next waypoint.
|
||||
// factor applied from 1 to 10 for turns > 60 to 105 degs and over
|
||||
if (navConfig()->fw.wp_turn_smoothing_dist && STATE(AIRPLANE) && posControl.activeWaypointIndex > 0) {
|
||||
fpVector3_t nextWpPos;
|
||||
if (getLocalPosNextWaypoint(&nextWpPos)) {
|
||||
int32_t bearingToNextWP = ABS(wrap_18000(calculateBearingBetweenLocalPositions(waypointPos, &nextWpPos) - *waypointYaw));
|
||||
turnEarlyDistance = navConfig()->fw.wp_turn_smoothing_dist * constrain((bearingToNextWP - 4000) / 500, 0, 10);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return posControl.wpDistance <= (navConfig()->general.waypoint_radius + turnEarlyDistance);
|
||||
return posControl.wpDistance <= (navConfig()->general.waypoint_radius);
|
||||
}
|
||||
|
||||
bool isWaypointAltitudeReached(void)
|
||||
|
|
|
|||
|
|
@ -308,7 +308,7 @@ typedef struct navConfig_s {
|
|||
uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg)
|
||||
uint16_t auto_disarm_delay; // fixed wing disarm delay for landing detector
|
||||
uint8_t waypoint_tracking_accuracy; // fixed wing tracking distance accuracy [m]
|
||||
uint16_t wp_turn_smoothing_dist; // distance increment to smooth turns during fixed wing waypoint missions [cm]
|
||||
uint8_t waypoint_turn_smoothing; // Smooths turns during WP missions
|
||||
} fw;
|
||||
} navConfig_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -275,20 +275,48 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
|||
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
|
||||
|
||||
uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius);
|
||||
fpVector3_t loiterCenterPos = posControl.desiredState.pos;
|
||||
int8_t turnDirection = loiterDirection();
|
||||
|
||||
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
|
||||
// Detemine if a circular loiter is required.
|
||||
// For waypoints only use circular loiter when angular visibility is > 30 degs, otherwise head straight toward target
|
||||
#define TAN_15DEG 0.26795f
|
||||
needToCalculateCircularLoiter = isNavHoldPositionActive() &&
|
||||
(distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
|
||||
(distanceToActualTarget > 50.0f);
|
||||
|
||||
// Calculate virtual position for straight movement
|
||||
if (needToCalculateCircularLoiter) {
|
||||
// We are closing in on a waypoint, calculate circular loiter
|
||||
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f);
|
||||
/* WP turn smoothing option - switch to loiter path when distance to waypoint < navLoiterRadius.
|
||||
* Loiter centered on point inside turn at navLoiterRadius distance from waypoint and
|
||||
* on a bearing midway between current and next waypoint course bearings */
|
||||
if (navConfig()->fw.waypoint_turn_smoothing && isWaypointNavTrackingRoute() && !needToCalculateCircularLoiter) {
|
||||
if (posControl.wpDistance < navLoiterRadius) {
|
||||
fpVector3_t posNextWp;
|
||||
if (getLocalPosNextWaypoint(&posNextWp)) {
|
||||
int32_t bearingNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp);
|
||||
int32_t loiterCenterBearing = wrap_36000(((wrap_18000(bearingNextWp - posControl.activeWaypoint.yaw - 18000)) / 2) +
|
||||
posControl.activeWaypoint.yaw + 18000);
|
||||
loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
|
||||
loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
|
||||
posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
|
||||
float loiterTargetX = posControl.desiredState.pos.x + navLoiterRadius * cos_approx(loiterAngle);
|
||||
float loiterTargetY = posControl.desiredState.pos.y + navLoiterRadius * sin_approx(loiterAngle);
|
||||
// turn direction to next waypoint
|
||||
turnDirection = wrap_18000(bearingNextWp - posControl.activeWaypoint.yaw) > 0 ? 1 : -1; // 1 = right
|
||||
|
||||
// if waypoint not reached based on distance from waypoint then waypoint considered
|
||||
// reached if difference between actual heading and waypoint bearing exceeds 90 degs
|
||||
posControl.wpReached = ABS(wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw)) > 9000;
|
||||
|
||||
needToCalculateCircularLoiter = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// We are closing in on a waypoint, calculate circular loiter if required
|
||||
if (needToCalculateCircularLoiter) {
|
||||
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(turnDirection * 45.0f);
|
||||
float loiterTargetX = loiterCenterPos.x + navLoiterRadius * cos_approx(loiterAngle);
|
||||
float loiterTargetY = loiterCenterPos.y + navLoiterRadius * sin_approx(loiterAngle);
|
||||
|
||||
// We have temporary loiter target. Recalculate distance and position error
|
||||
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
|
|
@ -358,8 +386,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
// We have virtual position target, calculate heading error
|
||||
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
|
||||
|
||||
/* If waypoint tracking enabled force craft toward waypoint course line
|
||||
* and hold within set accuracy distance from line */
|
||||
/* If waypoint tracking enabled quickly force craft toward waypoint course line and track along it */
|
||||
if (navConfig()->fw.waypoint_tracking_accuracy && isWaypointNavTrackingRoute() && !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) {
|
||||
|
|
@ -370,16 +397,20 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.yaw));
|
||||
float distToCourseLine = calculateDistanceToDestination(&virtualCoursePoint);
|
||||
|
||||
// courseVirtualCorrection initially used to determine current position relative to course line for later use
|
||||
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
|
||||
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
|
||||
|
||||
// bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
|
||||
float courseCorrectionFactor = constrainf(distToCourseLine / (100.0f * navConfig()->fw.waypoint_tracking_accuracy), 0.0f, 1.0f);
|
||||
courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
|
||||
|
||||
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
|
||||
float courseHeadingFactor = constrainf(sq(courseHeadingError / 18000.0f), 0.0f, 1.0f);
|
||||
courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor;
|
||||
|
||||
courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
|
||||
|
||||
// final courseVirtualCorrection using max 80 deg heading adjustment toward course line
|
||||
courseVirtualCorrection = 8000 * courseCorrectionFactor;
|
||||
|
||||
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection);
|
||||
|
|
|
|||
|
|
@ -401,6 +401,7 @@ typedef struct {
|
|||
float wpInitialAltitude; // Altitude at start of WP
|
||||
float wpInitialDistance; // Distance when starting flight to WP
|
||||
float wpDistance; // Distance to active WP
|
||||
bool wpReached; // WP reached when using turn smoothing
|
||||
timeMs_t wpReachedTime; // Time the waypoint was reached
|
||||
bool wpAltitudeReached; // WP altitude achieved
|
||||
|
||||
|
|
@ -454,6 +455,8 @@ bool isNavHoldPositionActive(void);
|
|||
bool isLastMissionWaypoint(void);
|
||||
float getActiveWaypointSpeed(void);
|
||||
bool isWaypointNavTrackingRoute(void);
|
||||
bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos);
|
||||
int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, const fpVector3_t * endPos);
|
||||
|
||||
void updateActualHeading(bool headingValid, int32_t newHeading);
|
||||
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue