mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 00:05:28 +03:00
update
This commit is contained in:
parent
9f9d1fdd55
commit
278d86fe5a
5 changed files with 21 additions and 19 deletions
|
@ -50,7 +50,6 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
|
|||
OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE),
|
||||
OSD_SETTING_ENTRY("MID THR FOR AH", SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD),
|
||||
OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR),
|
||||
OSD_SETTING_ENTRY("MISSION RESTART", SETTING_NAV_WP_MISSION_RESTART),
|
||||
|
||||
OSD_BACK_AND_END_ENTRY,
|
||||
};
|
||||
|
@ -199,6 +198,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
|
|||
#ifdef USE_MULTI_MISSION
|
||||
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 TRACKING ACCURACY", SETTING_NAV_FW_WP_TRACKING_ACCURACY),
|
||||
OSD_BACK_AND_END_ENTRY,
|
||||
|
|
|
@ -2423,13 +2423,13 @@ groups:
|
|||
min: 0
|
||||
max: 9
|
||||
- name: nav_fw_wp_tracking_accuracy
|
||||
description: "Sets the maximum allowed distance from the waypoint course line when waypoint tracking is active [cm]. When the craft is outside this distance it will head toward the the course line as quickly as possible. The course tracking response can be tuned by making small changes to the accuracy setting, higher values damp the response. A value of 250 is a good starting point. If set to 0 course tracking is disabled and the craft will head directly to the next waypoint from whatever position was achieved after the last waypoint turn."
|
||||
description: "Sets a reference distance to the waypoint course line that affects the accuracy of waypoint course tracking [m]. If set to 0 course tracking is disabled and the craft will head directly to the next waypoint from whatever position was achieved after the last waypoint turn. Otherwise course tracking will be enabled which will force the craft to quickly head toward and track along the the course line as closely as possible. The course tracking response can be tuned by changing the accuracy setting. Lower values will push the craft toward the course line quicker but may result in overshoot. 40 is a good starting point."
|
||||
default_value: 0
|
||||
field: fw.waypoint_tracking_accuracy
|
||||
min: 0
|
||||
max: 1000
|
||||
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 60 degrees to a maximum of 10 for turns of 105 degrees and over, e.g. a 90 degree turn is reached when 7 x nav_fw_wp_turn_smoothing_dist from waypoint. Set to 0 to disable turn smoothing."
|
||||
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
|
||||
|
|
|
@ -209,7 +209,7 @@ 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 cm
|
||||
.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
|
||||
}
|
||||
);
|
||||
|
@ -2256,7 +2256,7 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
|
|||
}
|
||||
|
||||
uint16_t turnEarlyDistance = 0;
|
||||
if (navGetStateFlags(posControl.navState) & NAV_WP_MODE) {
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
||||
// Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypointYaw
|
||||
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000) {
|
||||
return true;
|
||||
|
@ -2268,7 +2268,7 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
|
|||
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 - 5500) / 500, 0, 10);
|
||||
turnEarlyDistance = navConfig()->fw.wp_turn_smoothing_dist * constrain((bearingToNextWP - 4000) / 500, 0, 10);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -307,7 +307,7 @@ typedef struct navConfig_s {
|
|||
uint8_t yawControlDeadband;
|
||||
uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg)
|
||||
uint16_t auto_disarm_delay; // fixed wing disarm delay for landing detector
|
||||
uint16_t waypoint_tracking_accuracy; // fixed wing tracking distance accuracy [cm]
|
||||
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]
|
||||
} fw;
|
||||
} navConfig_t;
|
||||
|
|
|
@ -370,17 +370,19 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.yaw));
|
||||
float distToCourseLine = calculateDistanceToDestination(&virtualCoursePoint);
|
||||
|
||||
int32_t courseCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
|
||||
// lock virtualTargetBearing to wp course heading if within accuracy dead band and wp course heading error < 10 degrees
|
||||
if (distToCourseLine < navConfig()->fw.waypoint_tracking_accuracy &&
|
||||
ABS(wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw)) < 1000) {
|
||||
virtualTargetBearing = posControl.activeWaypoint.yaw;
|
||||
} else {
|
||||
float courseCorrectionFactor = constrainf((distToCourseLine - navConfig()->fw.waypoint_tracking_accuracy) /
|
||||
(15.0f * navConfig()->fw.waypoint_tracking_accuracy), 0.0f, 1.0f);
|
||||
courseCorrection = courseCorrection < 0 ? -8000 * courseCorrectionFactor : 8000 * courseCorrectionFactor;
|
||||
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseCorrection);
|
||||
}
|
||||
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
|
||||
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
|
||||
|
||||
float courseCorrectionFactor = constrainf(distToCourseLine / (100.0f * navConfig()->fw.waypoint_tracking_accuracy), 0.0f, 1.0f);
|
||||
courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
|
||||
|
||||
float courseHeadingFactor = constrainf(sq(courseHeadingError / 18000.0f), 0.0f, 1.0f);
|
||||
courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor;
|
||||
|
||||
courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
|
||||
courseVirtualCorrection = 8000 * courseCorrectionFactor;
|
||||
|
||||
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue