Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00
Code Issues Wiki Activity

update

Browse source
This commit is contained in:
breadoven 2022-07-11 23:02:41 +01:00
parent 9f9d1fdd55
commit 278d86fe5a
5 changed files with 21 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cms/cms_menu_navigation.c
View file

@ -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,

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

@ -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

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

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

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

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

24
src/main/navigation/navigation_fixedwing.c
View file

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