diff --git a/docs/Settings.md b/docs/Settings.md index b400f31c5b..922299100d 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3152,6 +3152,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] @@ -3634,11 +3644,11 @@ 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 | | --- | --- | --- | -| 10 | 0 | 255 | +| 30 | 0 | 255 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bea4876418..811269afa9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2160,8 +2160,8 @@ groups: min: 0 max: 255 - name: nav_fw_pos_z_ff - description: "FF gain of altitude PID controller (Fixedwing)" - default_value: 10 + description: "FF gain of altitude PID controller. Not used if nav_fw_alt_use_position is set ON (Fixedwing)" + default_value: 30 field: bank_fw.pid[PID_POS_Z].FF min: 0 max: 255 @@ -2171,6 +2171,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..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 = { @@ -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 14f4f20274..4f22d1cac5 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4956,13 +4956,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 / 100.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..4e0fda62dc 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -131,12 +131,12 @@ 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); // 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; } @@ -144,12 +144,61 @@ 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 || 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 adjustmentFactor = constrainf(scaleRangef(accelerationZ, 0.0f, 200.0f, 1.0f, 0.0f), 0.0f, 1.0f); + + 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; + } + } else { + desiredClimbRate = 0; + } + + 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, 0); // 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 +220,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 +230,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