Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 03:19:58 +03:00
Code Issues Wiki Activity

use position for altitude control

Browse source
This commit is contained in:
breadoven 2025-06-09 17:55:05 +01:00
parent 8345ad4aa9
commit cf56dbe00e
6 changed files with 87 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

12
docs/Settings.md
View file

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

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

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

1
src/main/flight/pid.c
View file

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

1
src/main/flight/pid.h
View file

@ -150,6 +150,7 @@ typedef struct pidProfile_s {
float fixedWingLevelTrimGain;
uint8_t fwAltControlResponseFactor;
bool fwAltControlUsePos;
#ifdef USE_SMITH_PREDICTOR
float smithPredictorStrength;
float smithPredictorDelay;

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

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

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

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