From f3a48941a41c32ea09bb8be0fde233a5d8ac8f85 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 2 Sep 2024 22:22:14 +0100 Subject: [PATCH] CR132 + 133 --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation.c | 28 ++--- src/main/navigation/navigation_fixedwing.c | 125 +++++++++++++-------- 4 files changed, 92 insertions(+), 65 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 1196ffee73f..2a51d5fd604 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2988,7 +2988,7 @@ Adjusts the deceleration response of fixed wing altitude control as the target a | Default | Min | Max | | --- | --- | --- | -| 20 | 5 | 100 | +| 30 | 5 | 100 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 853d58471fa..57a9355a24a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2143,7 +2143,7 @@ groups: max: 255 - name: nav_fw_alt_control_response description: "Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude." - default_value: 20 + default_value: 30 # CR133 field: fwAltControlResponseFactor min: 5 max: 100 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b6ac50044ce..eb2a909c8a3 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1866,6 +1866,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav case NAV_WP_ACTION_LAND: calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]); posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); + posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; // CR133 posControl.wpAltitudeReached = false; return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS @@ -1926,20 +1927,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED } else { + // CR133 fpVector3_t tmpWaypoint; tmpWaypoint.x = posControl.activeWaypoint.pos.x; tmpWaypoint.y = posControl.activeWaypoint.pos.y; - setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); - - // Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP - // Update climb rate until within 100cm of total climb xy distance to WP - float climbRate = 0.0f; - if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f) { - climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) / - (posControl.wpDistance - 0.1f * posControl.wpInitialDistance); - } - updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET); + tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, 0.1f * posControl.wpInitialDistance, posControl.wpInitialDistance), + posControl.wpInitialDistance, 0.1f * posControl.wpInitialDistance, + posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); + setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + // CR133 if(STATE(MULTIROTOR)) { switch (wpHeadingControl.mode) { case NAV_WP_HEAD_MODE_NONE: @@ -4765,12 +4762,11 @@ void navigationUsePIDs(void) NAV_DTERM_CUT_HZ, 0.0f ); - - navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 50.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 50.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 50.0f, - // (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, // poss new FF, original was 0.0f - 0.0f, + // CR133 + navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 50.0f, // 20 @ 30 so 35 @ 50 All for response factor of 50 + (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 30.0f, // 10 @ 30 + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 66.7f, // 20 @ 100 so 10 @ 67 + 0.0f, //(float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, // poss new FF, original was 0.0f NAV_DTERM_CUT_HZ, 0.0f ); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 0f1c7b6691a..f0e8d3d00fe 100644 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -146,8 +146,36 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) // PID controller to translate desired climb rate error into pitch angle [decideg] float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z; - float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR); + // CR133 + // DEBUG_SET(DEBUG_ALWAYS, 0, currentClimbRate); + // static pt1Filter_t velz2FilterState; + // currentClimbRate = pt1FilterApply4(&velz2FilterState, currentClimbRate, navConfig()->fw.wp_tracking_accuracy, US2S(deltaMicros)); + // DEBUG_SET(DEBUG_ALWAYS, 1, currentClimbRate); + + // const float climbRateError = currentClimbRate - desiredClimbRate; + + // static timeUs_t previousTimeMonitoringUpdate; + // static int32_t previousClimbRateError; + // static bool errorIsDecreasing; + + // // Slow error monitoring (2Hz rate) + // if ((micros() - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) { + // // Check if error is decreasing over time + // errorIsDecreasing = (ABS(previousClimbRateError) > ABS(climbRateError)); + + // // Save values for next iteration + // previousClimbRateError = climbRateError; + // previousTimeMonitoringUpdate = micros(); + // } + + // // Only allow PID integrator to shrink if error is decreasing over time + // const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0); + + + float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR); + // float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, 0, climbRateError, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, pidFlags); + // DEBUG_SET(DEBUG_ALWAYS, 6, targetPitchAngle); // CR133 // Apply low-pass filter to prevent rapid correction targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); @@ -404,57 +432,61 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); } // CR132 - // Calculate cross track error - fpVector3_t virtualCoursePoint; - virtualCoursePoint.x = posControl.activeWaypoint.pos.x - - posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); - virtualCoursePoint.y = posControl.activeWaypoint.pos.y - - posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); - navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); - - /* If waypoint tracking enabled force craft toward waypoint course line and hold on course line */ - if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - static float crossTrackErrorRate; - static timeUs_t previousCrossTrackErrorUpdateTime; - static float previousCrossTrackError = 0.0f; - static pt1Filter_t fwCrossTrackErrorRateFilterState; - - if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { - const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); - if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) { - crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec; + if (isWaypointNavTrackingActive()) { + // Calculate cross track error + posControl.wpDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); + + fpVector3_t virtualCoursePoint; + virtualCoursePoint.x = posControl.activeWaypoint.pos.x - + posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); + virtualCoursePoint.y = posControl.activeWaypoint.pos.y - + posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); + navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); + + /* If waypoint tracking enabled force craft toward waypoint course line and hold on course line */ + if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter) { + static float crossTrackErrorRate; + static timeUs_t previousCrossTrackErrorUpdateTime; + static float previousCrossTrackError = 0.0f; + static pt1Filter_t fwCrossTrackErrorRateFilterState; + + if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { + const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); + if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) { + crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec; + } + crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec); + previousCrossTrackErrorUpdateTime = currentTimeUs; + previousCrossTrackError = navCrossTrackError; } - crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec); - previousCrossTrackErrorUpdateTime = currentTimeUs; - previousCrossTrackError = navCrossTrackError; - } - DEBUG_SET(DEBUG_ALWAYS, 0, navCrossTrackError); - DEBUG_SET(DEBUG_ALWAYS, 3, virtualTargetBearing); - DEBUG_SET(DEBUG_ALWAYS, 2, crossTrackErrorRate); + DEBUG_SET(DEBUG_ALWAYS, 0, navCrossTrackError); + DEBUG_SET(DEBUG_ALWAYS, 3, virtualTargetBearing); + DEBUG_SET(DEBUG_ALWAYS, 2, crossTrackErrorRate); - uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy); + uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy); - if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) { - float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); - uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle); + if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) { + float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); + uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle); - /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */ - float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit)); - float desiredApproachSpeed = constrainf(navCrossTrackError, 50.0f, maxApproachSpeed); - adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed); - DEBUG_SET(DEBUG_ALWAYS, 1, desiredApproachSpeed); - DEBUG_SET(DEBUG_ALWAYS, 5, adjustmentFactor); + /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */ + float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit)); + float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed); + adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed); + DEBUG_SET(DEBUG_ALWAYS, 1, desiredApproachSpeed); + DEBUG_SET(DEBUG_ALWAYS, 5, adjustmentFactor); - /* Calculate final adjusted virtualTargetBearing */ - uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit); - adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); - virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); + /* Calculate final adjusted virtualTargetBearing */ + uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit); + adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); + virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); - DEBUG_SET(DEBUG_ALWAYS, 7, limit); - DEBUG_SET(DEBUG_ALWAYS, 6, adjustmentFactor); + DEBUG_SET(DEBUG_ALWAYS, 7, limit); + DEBUG_SET(DEBUG_ALWAYS, 6, adjustmentFactor); + } + DEBUG_SET(DEBUG_ALWAYS, 4, virtualTargetBearing); } - DEBUG_SET(DEBUG_ALWAYS, 4, virtualTargetBearing); } // CR132 /* @@ -490,10 +522,9 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // Only allow PID integrator to shrink if error is decreasing over time const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0); - // DEBUG_SET(DEBUG_ALWAYS, 5, navHeadingError); - // Input error in (deg*100), output roll angle (deg*100) - float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog, US2S(deltaMicros), + float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog, + US2S(deltaMicros), -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), pidFlags);