From aa6e77df64b38b74ad4485313ba46e06c0d68bc0 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 2 Jul 2022 10:28:15 +0100 Subject: [PATCH] first build --- docs/Settings.md | 20 ++++ src/main/cms/cms_menu_navigation.c | 2 + src/main/fc/settings.yaml | 12 +++ src/main/navigation/navigation.c | 115 +++++++++++++++------ src/main/navigation/navigation.h | 2 + src/main/navigation/navigation_fixedwing.c | 33 +++++- src/main/navigation/navigation_private.h | 3 +- 7 files changed, 151 insertions(+), 36 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 2768acec6e..364476755d 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3272,6 +3272,26 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within --- +### nav_fw_wp_tracking_accuracy + +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. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 1000 | + +--- + +### nav_fw_wp_turn_smoothing_dist + +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. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 2000 | + +--- + ### nav_fw_yaw_deadband Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 7693a79d7f..b86413d202 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -199,6 +199,8 @@ 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("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, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f40814cf5f..28569200d2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2422,6 +2422,18 @@ groups: condition: USE_MULTI_MISSION 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." + default_value: 0 + field: fw.waypoint_tracking_accuracy + min: 0 + max: 1000 + - 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." + default_value: 0 + field: fw.wp_turn_smoothing_dist + min: 0 + max: 2000 - 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 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 2eace1ac8a..d0744c9487 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -100,7 +100,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 1); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 1); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -209,6 +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 cm + .wp_turn_smoothing_dist = SETTING_NAV_FW_WP_TURN_SMOOTHING_DIST_DEFAULT, // 0 cm } ); @@ -249,7 +251,7 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance); void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance); -static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome); +static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw); bool isWaypointAltitudeReached(void); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static navigationFSMEvent_t nextForNonGeoStates(void); @@ -1332,7 +1334,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; // procede to home after final trackback point } - if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) { + if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) { posControl.activeRthTBPointIndex--; if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) { @@ -1366,7 +1368,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio if ((posControl.flags.estPosStatus >= EST_USABLE)) { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); - if (isWaypointPositionReached(tmpHomePos, true)) { + if (isWaypointReached(tmpHomePos, 0)) { // Successfully reached position target - update XYZ-position setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING @@ -1622,7 +1624,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na case NAV_WP_ACTION_HOLD_TIME: case NAV_WP_ACTION_WAYPOINT: case NAV_WP_ACTION_LAND: - if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) { + if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) { return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED } else { @@ -2186,6 +2188,14 @@ int32_t calculateBearingToDestination(const fpVector3_t * destinationPos) return calculateBearingFromDelta(deltaX, deltaY); } +int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, const fpVector3_t * endPos) +{ + const float deltaX = endPos->x - startPos->x; + const float deltaY = endPos->y - startPos->y; + + return calculateBearingFromDelta(deltaX, deltaY); +} + bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos) { if (posControl.flags.estPosStatus == EST_NONE || @@ -2203,35 +2213,67 @@ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3 return true; } +static 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()) { + navWaypointActions_e nextWpAction = posControl.waypointList[posControl.activeWaypointIndex + 1].action; + + if (!(nextWpAction == NAV_WP_ACTION_SET_POI || nextWpAction == NAV_WP_ACTION_SET_HEAD)) { + uint8_t nextWpIndex = posControl.activeWaypointIndex + 1; + if (nextWpAction == NAV_WP_ACTION_JUMP) { + if (posControl.waypointList[posControl.activeWaypointIndex + 1].p3 != 0 || + posControl.waypointList[posControl.activeWaypointIndex + 1].p2 == -1) { + nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1; + } else if (posControl.activeWaypointIndex + 2 <= posControl.waypointCount - 1) { + if (posControl.waypointList[posControl.activeWaypointIndex + 2].action != NAV_WP_ACTION_JUMP) { + nextWpIndex++; + } else { + return false; // give up - too complicated + } + } + } + mapWaypointToLocalPosition(nextWpPos, &posControl.waypointList[nextWpIndex], 0); + return true; + } + } + + return false; // no position available +} + /*----------------------------------------------------------- - * Check if waypoint is/was reached. Assume that waypoint-yaw stores initial bearing + * Check if waypoint is/was reached. + * waypointYaw stores initial bearing to waypoint *-----------------------------------------------------------*/ -bool isWaypointMissed(const navWaypointPosition_t * waypoint) +static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw) { - int32_t bearingError = calculateBearingToDestination(&waypoint->pos) - waypoint->yaw; - bearingError = wrap_18000(bearingError); + posControl.wpDistance = calculateDistanceToDestination(waypointPos); - return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees -} - -static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome) -{ - // We consider waypoint reached if within specified radius - posControl.wpDistance = calculateDistanceToDestination(pos); - - if (STATE(FIXED_WING_LEGACY) && isWaypointHome) { - // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check - return (posControl.wpDistance <= navConfig()->general.waypoint_radius) - || (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius + // Airplane will do a circular loiter at hold waypoints and might never approach them closer than waypoint_radius + // Check within 10% margin of circular loiter radius + if (STATE(AIRPLANE) && isNavHoldPositionActive() && posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)) { + return true; } - else { - return (posControl.wpDistance <= navConfig()->general.waypoint_radius); - } -} -bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome) -{ - return isWaypointPositionReached(&waypoint->pos, isWaypointHome); + uint16_t turnEarlyDistance = 0; + if (FLIGHT_MODE(NAV_WP_MODE)) { + // 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; + } + + // 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 - 5500) / 500, 0, 10); + } + } + } + + return posControl.wpDistance <= (navConfig()->general.waypoint_radius + turnEarlyDistance); } bool isWaypointAltitudeReached(void) @@ -3293,10 +3335,14 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos) { - posControl.activeWaypoint.pos = *pos; + // Calculate bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints) + if (isWaypointNavTrackingRoute()) { + posControl.activeWaypoint.yaw = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos); + } else { + posControl.activeWaypoint.yaw = calculateBearingToDestination(pos); + } - // Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints) - posControl.activeWaypoint.yaw = calculateBearingToDestination(pos); + posControl.activeWaypoint.pos = *pos; // Set desired position to next waypoint (XYZ-controller) setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); @@ -3361,6 +3407,13 @@ float getActiveWaypointSpeed(void) } } +bool isWaypointNavTrackingRoute(void) +{ + // true when established on route beyond first waypoint + return (FLIGHT_MODE(NAV_WP_MODE) && posControl.activeWaypointIndex > 0) || + (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex); +} + /*----------------------------------------------------------- * Process adjustments to alt, pos and yaw controllers *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 01585d4524..ee1df1f06c 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -307,6 +307,8 @@ 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] + uint16_t wp_turn_smoothing_dist; // distance increment to smooth turns during fixed wing waypoint missions [cm] } fw; } navConfig_t; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index f05e5fc0f4..3dfa5d7ffa 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -73,6 +73,7 @@ static float throttleSpeedAdjustment = 0; static bool isAutoThrottleManuallyIncreased = false; static int32_t navHeadingError; static int8_t loiterDirYaw = 1; +static bool needToCalculateCircularLoiter = false; // Calculates the cutoff frequency for smoothing out roll/pitch commands // control_smoothness valid range from 0 to 9 @@ -277,9 +278,9 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target #define TAN_15DEG 0.26795f - bool needToCalculateCircularLoiter = isNavHoldPositionActive() && - (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) && - (distanceToActualTarget > 50.0f); + needToCalculateCircularLoiter = isNavHoldPositionActive() && + (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) && + (distanceToActualTarget > 50.0f); // Calculate virtual position for straight movement if (needToCalculateCircularLoiter) { @@ -357,6 +358,32 @@ 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 (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) { + fpVector3_t virtualCoursePoint; + virtualCoursePoint.x = posControl.activeWaypoint.pos.x - + posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.yaw)); + virtualCoursePoint.y = posControl.activeWaypoint.pos.y - + 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); + } + } + } + /* * Calculate NAV heading error * Units are centidegrees diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 552be2f0de..7de65e6ec5 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -450,11 +450,10 @@ void setDesiredSurfaceOffset(float surfaceOffset); void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode); -bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome); -bool isWaypointMissed(const navWaypointPosition_t * waypoint); bool isNavHoldPositionActive(void); bool isLastMissionWaypoint(void); float getActiveWaypointSpeed(void); +bool isWaypointNavTrackingRoute(void); void updateActualHeading(bool headingValid, int32_t newHeading); void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);