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

Merge pull request #10903 from breadoven/abo_fw_alt_cont_use_pos

Browse source 
Option to use position for fixed wing nav altitude control
This commit is contained in:
breadoven 2025-07-09 17:24:52 +01:00 • committed by GitHub
commit ab3411ad4e
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
6 changed files with 95 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

14
docs/Settings.md
View file

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

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

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

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

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

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

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

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

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