From cf56dbe00e89af9c5270bb2d8709d506fd04e81f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 9 Jun 2025 17:55:05 +0100 Subject: [PATCH 1/6] use position for altitude control --- docs/Settings.md | 12 ++++- src/main/fc/settings.yaml | 7 ++- src/main/flight/pid.c | 1 + src/main/flight/pid.h | 1 + src/main/navigation/navigation.c | 24 ++++++--- src/main/navigation/navigation_fixedwing.c | 58 +++++++++++++++++++--- 6 files changed, 87 insertions(+), 16 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 815944ebba..7170a244a0 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3132,6 +3132,16 @@ Adjusts the deceleration response of fixed wing altitude control as the target a --- +### nav_fw_alt_use_position + +Use position for fixed wing altitude control rather than velocity (default method). + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### nav_fw_auto_climb_rate Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] @@ -3614,7 +3624,7 @@ D gain of altitude PID controller (Fixedwing) ### nav_fw_pos_z_ff -FF gain of altitude PID controller (Fixedwing) +FF gain of altitude PID controller. Not used if nav_fw_alt_use_position is set ON (Fixedwing) | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e361d62e10..902a37cd51 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2148,7 +2148,7 @@ groups: min: 0 max: 255 - name: nav_fw_pos_z_ff - description: "FF gain of altitude PID controller (Fixedwing)" + description: "FF gain of altitude PID controller. Not used if nav_fw_alt_use_position is set ON (Fixedwing)" default_value: 10 field: bank_fw.pid[PID_POS_Z].FF min: 0 @@ -2159,6 +2159,11 @@ groups: field: fwAltControlResponseFactor min: 5 max: 100 + - name: nav_fw_alt_use_position + description: "Use position for fixed wing altitude control rather than velocity (default method)." + default_value: OFF + field: fwAltControlUsePos + type: bool - name: nav_fw_pos_xy_p description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH" default_value: 75 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4ca0e3c9c0..93106ae1c6 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -308,6 +308,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT, .fwAltControlResponseFactor = SETTING_NAV_FW_ALT_CONTROL_RESPONSE_DEFAULT, + .fwAltControlUsePos = SETTING_NAV_FW_ALT_USE_POSITION_DEFAULT, #ifdef USE_SMITH_PREDICTOR .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT, .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 26aeb86990..cb76a129f1 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -150,6 +150,7 @@ typedef struct pidProfile_s { float fixedWingLevelTrimGain; uint8_t fwAltControlResponseFactor; + bool fwAltControlUsePos; #ifdef USE_SMITH_PREDICTOR float smithPredictorStrength; float smithPredictorDelay; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b5e9e8a7e5..e37f89e941 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4959,13 +4959,23 @@ void navigationUsePIDs(void) 0.0f ); - navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 300.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, - NAV_DTERM_CUT_HZ, - 0.0f - ); + if (pidProfile()->fwAltControlUsePos) { + navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 30.0f, + 0.0f, + NAV_DTERM_CUT_HZ, + 0.0f + ); + } else { + navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 300.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, + NAV_DTERM_CUT_HZ, + 0.0f + ); + } navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f, diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 49c693fa0c..af4538ac6b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -131,7 +131,7 @@ bool adjustFixedWingAltitudeFromRCInput(void) // Position to velocity controller for Z axis static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) { - static pt1Filter_t velzFilterState; + static pt1Filter_t pitchFilterState; float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); @@ -144,12 +144,57 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); - // 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); + // Default control based on climb rate (velocity) + float targetValue = desiredClimbRate; + float measuredValue = navGetCurrentActualPositionAndVelocity()->vel.z; + + // Optional control based on altitude (position) + if (pidProfile()->fwAltControlUsePos) { + static float currentDesiredPosZ = 0.0f; + static float trackingAltitude = 0.0f; + static bool altitudeRateControlActive = false; + + float desiredAltitude = posControl.desiredState.pos.z; + float currentAltitude = navGetCurrentActualPositionAndVelocity()->pos.z; + + /* Determine if altitude rate control required based on magnitude of change in target altitude. + * No rate control used during trackback to allow max climb rates based on pitch limits */ + if (fabsf(currentDesiredPosZ - desiredAltitude) > 100.0f || !isPitchAdjustmentValid) { + altitudeRateControlActive = desiredClimbRate && fabsf(desiredAltitude - currentAltitude) > navConfig()->fw.max_auto_climb_rate && + !posControl.flags.rthTrackbackActive; + trackingAltitude = currentAltitude; + } + currentDesiredPosZ = desiredAltitude; + + if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { + posControl.desiredState.pos.z += desiredClimbRate * US2S(deltaMicros); + desiredAltitude = posControl.desiredState.pos.z; + } else if (altitudeRateControlActive) { + /* Disable rate control if no longer required (remaining altitude change is small) */ + altitudeRateControlActive = fabsf(desiredAltitude - currentAltitude) > MAX(fabsf(desiredClimbRate), 50.0f); + + /* Tracking altitude used to control altitude rate where changing posControl.desiredState.pos.z not possible. + * Adjustment factor based on vertical acceleration used to control rate of change of tracking altitude + * to better control desired vertical velocity. Helps avoid overcontrol by PID loop. */ + static float absLastClimbRate = 0.0f; + float absClimbRate = fabsf(measuredValue); + float accelerationZ = (absClimbRate - absLastClimbRate) / US2S(deltaMicros); + absLastClimbRate = absClimbRate; + float accDerivedAdjustmentFactor = constrainf(scaleRangef(accelerationZ, 0.0f, 200.0f, 1.0f, 0.0f), 0.0f, 1.0f); + + trackingAltitude += accDerivedAdjustmentFactor * desiredClimbRate * US2S(deltaMicros); + desiredAltitude = trackingAltitude; + } + + targetValue = desiredAltitude; + measuredValue = currentAltitude; + } + + // PID controller to translate desired target error (velocity or position) into pitch angle [decideg] + float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, targetValue, measuredValue, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR); // Apply low-pass filter to prevent rapid correction - targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); + targetPitchAngle = pt1FilterApply4(&pitchFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); // Reconstrain pitch angle (> 0 - climb, < 0 - dive) targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg); @@ -171,6 +216,7 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) // Check if last correction was not too long ago if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate); + isPitchAdjustmentValid = true; } else { // Position update has not occurred in time (first iteration or glitch), reset altitude controller @@ -180,8 +226,6 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) // Indicate that information is no longer usable posControl.flags.verticalPositionDataConsumed = true; } - - isPitchAdjustmentValid = true; } else { // No valid altitude sensor data, don't adjust pitch automatically, rcCommand[PITCH] is passed through to PID controller From b62e4ca13290052a58048b76f8afad62170a9f86 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 11 Jun 2025 10:39:54 +0100 Subject: [PATCH 2/6] Improvements --- src/main/navigation/navigation_fixedwing.c | 28 ++++++++++++---------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index af4538ac6b..4503bae6bb 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -166,24 +166,26 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) } currentDesiredPosZ = desiredAltitude; - if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { - posControl.desiredState.pos.z += desiredClimbRate * US2S(deltaMicros); - desiredAltitude = posControl.desiredState.pos.z; - } else if (altitudeRateControlActive) { - /* Disable rate control if no longer required (remaining altitude change is small) */ - altitudeRateControlActive = fabsf(desiredAltitude - currentAltitude) > MAX(fabsf(desiredClimbRate), 50.0f); - - /* Tracking altitude used to control altitude rate where changing posControl.desiredState.pos.z not possible. - * Adjustment factor based on vertical acceleration used to control rate of change of tracking altitude - * to better control desired vertical velocity. Helps avoid overcontrol by PID loop. */ + if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT || altitudeRateControlActive) { + /* Adjustment factor based on vertical acceleration used to better control altitude position change + * driving vertical velocity control. Helps avoid lag induced overcontrol by PID loop. */ static float absLastClimbRate = 0.0f; float absClimbRate = fabsf(measuredValue); float accelerationZ = (absClimbRate - absLastClimbRate) / US2S(deltaMicros); absLastClimbRate = absClimbRate; - float accDerivedAdjustmentFactor = constrainf(scaleRangef(accelerationZ, 0.0f, 200.0f, 1.0f, 0.0f), 0.0f, 1.0f); + float adjustmentFactor = constrainf(scaleRangef(accelerationZ, 0.0f, 200.0f, 1.0f, 0.0f), 0.0f, 1.0f); - trackingAltitude += accDerivedAdjustmentFactor * desiredClimbRate * US2S(deltaMicros); - desiredAltitude = trackingAltitude; + if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { + posControl.desiredState.pos.z += adjustmentFactor * desiredClimbRate * US2S(deltaMicros); + desiredAltitude = posControl.desiredState.pos.z; + } else { + /* Disable rate control if no longer required, i.e. remaining altitude change is small */ + altitudeRateControlActive = fabsf(desiredAltitude - currentAltitude) > MAX(fabsf(desiredClimbRate), 50.0f); + + /* Tracking altitude used to control altitude rate where changing posControl.desiredState.pos.z not possible */ + trackingAltitude += adjustmentFactor * desiredClimbRate * US2S(deltaMicros); + desiredAltitude = trackingAltitude; + } } targetValue = desiredAltitude; From 83d162c600a18431d30c04d36fa30fb55746b77b Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 19 Jun 2025 21:17:31 +0100 Subject: [PATCH 3/6] change D term to measurement --- src/main/navigation/navigation_fixedwing.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 4503bae6bb..90d0ddebe8 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -136,7 +136,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); // Reduce max allowed climb rate by 2/3 if performing loiter (stall prevention) - if (needToCalculateCircularLoiter && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) { + if (navGetCurrentStateFlags() & NAV_CTL_HOLD && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) { desiredClimbRate = 0.67f * navConfig()->fw.max_auto_climb_rate; } @@ -147,6 +147,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) // Default control based on climb rate (velocity) float targetValue = desiredClimbRate; float measuredValue = navGetCurrentActualPositionAndVelocity()->vel.z; + pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR; // Optional control based on altitude (position) if (pidProfile()->fwAltControlUsePos) { @@ -186,14 +187,17 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) trackingAltitude += adjustmentFactor * desiredClimbRate * US2S(deltaMicros); desiredAltitude = trackingAltitude; } + } else { + desiredClimbRate = 0; } targetValue = desiredAltitude; measuredValue = currentAltitude; + pidFlags = 0; // use measurement for D term } // PID controller to translate desired target error (velocity or position) into pitch angle [decideg] - float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, targetValue, measuredValue, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR); + float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, targetValue, measuredValue, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, pidFlags); // Apply low-pass filter to prevent rapid correction targetPitchAngle = pt1FilterApply4(&pitchFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); From 9d8b1a24d6c990653c50510eaff92cba6dc318ac Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 24 Jun 2025 17:59:21 +0100 Subject: [PATCH 4/6] use original D term factor --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e37f89e941..b4975fb321 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4962,7 +4962,7 @@ void navigationUsePIDs(void) if (pidProfile()->fwAltControlUsePos) { navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 30.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f, 0.0f, NAV_DTERM_CUT_HZ, 0.0f From b71547a03b74bc0dc85888b5d02de49c5432ba72 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 24 Jun 2025 22:42:12 +0100 Subject: [PATCH 5/6] Update navigation_fixedwing.c --- src/main/navigation/navigation_fixedwing.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 90d0ddebe8..73942df894 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -147,7 +147,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) // Default control based on climb rate (velocity) float targetValue = desiredClimbRate; float measuredValue = navGetCurrentActualPositionAndVelocity()->vel.z; - pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR; + pidControllerFlags_e pidFlags = 0; // PID_DTERM_FROM_ERROR; // Optional control based on altitude (position) if (pidProfile()->fwAltControlUsePos) { From 4d63f88b5c426857b529d3f7c6b9f496dc69176b Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 7 Jul 2025 17:44:30 +0100 Subject: [PATCH 6/6] cleanup + change PID FF --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/flight/pid.c | 2 +- src/main/navigation/navigation_fixedwing.c | 4 +--- 4 files changed, 4 insertions(+), 6 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 7170a244a0..854d2af6b0 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3628,7 +3628,7 @@ FF gain of altitude PID controller. Not used if nav_fw_alt_use_position is set O | Default | Min | Max | | --- | --- | --- | -| 10 | 0 | 255 | +| 30 | 0 | 255 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 902a37cd51..bafb901675 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2149,7 +2149,7 @@ groups: max: 255 - name: nav_fw_pos_z_ff description: "FF gain of altitude PID controller. Not used if nav_fw_alt_use_position is set ON (Fixedwing)" - default_value: 10 + default_value: 30 field: bank_fw.pid[PID_POS_Z].FF min: 0 max: 255 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 93106ae1c6..6f598bebb5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -179,7 +179,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 73942df894..4e0fda62dc 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -147,7 +147,6 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) // Default control based on climb rate (velocity) float targetValue = desiredClimbRate; float measuredValue = navGetCurrentActualPositionAndVelocity()->vel.z; - pidControllerFlags_e pidFlags = 0; // PID_DTERM_FROM_ERROR; // Optional control based on altitude (position) if (pidProfile()->fwAltControlUsePos) { @@ -193,11 +192,10 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) targetValue = desiredAltitude; measuredValue = currentAltitude; - pidFlags = 0; // use measurement for D term } // PID controller to translate desired target error (velocity or position) into pitch angle [decideg] - float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, targetValue, measuredValue, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, pidFlags); + float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, targetValue, measuredValue, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0); // Apply low-pass filter to prevent rapid correction targetPitchAngle = pt1FilterApply4(&pitchFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));