add pos_z_ff

iNavFlight · Jan 8, 2025 · 512b677 · 512b677
1 parent 47d03d8
commit 512b677
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 8 deletions.
diff --git a/docs/Settings.md b/docs/Settings.md
@@ -3602,6 +3602,16 @@ D gain of altitude PID controller (Fixedwing)
 
 ---
 
+### nav_fw_pos_z_ff
+
+FF gain of altitude PID controller (Fixedwing)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 0 | 255 |
+
+---
+
 ### nav_fw_pos_z_i
 
 I gain of altitude PID controller (Fixedwing)

diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
@@ -227,6 +227,7 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] =
     OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
     OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
     OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),
+    OTHER_PIDFF_ENTRY("ALT FF", &cmsx_pidPosZ.FF),
 
     OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelZ.P),
     OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelZ.I),

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -2147,6 +2147,12 @@ groups:
         field: bank_fw.pid[PID_POS_Z].D
         min: 0
         max: 255
+      - name: nav_fw_pos_z_ff
+        description: "FF gain of altitude PID controller (Fixedwing)"
+        default_value: 10
+        field: bank_fw.pid[PID_POS_Z].FF
+        min: 0
+        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: 40
@@ -4390,15 +4396,15 @@ groups:
         field: safeAltitudeDistance
         min: 0
         max: 10000
-      - name: geozone_safehome_as_inclusive 
+      - name: geozone_safehome_as_inclusive
         description: "Treat nearest safehome as inclusive geozone"
         type: bool
         field: nearestSafeHomeAsInclusivZone
         default_value: OFF
       - name: geozone_safehome_zone_action
         description:  "Fence action for safehome zone"
         default_value: "NONE"
-        field: safeHomeFenceAction    
+        field: safeHomeFenceAction
         table: fence_action
         type: uint8_t
       - name: geozone_mr_stop_distance
@@ -4412,4 +4418,4 @@ groups:
         default_value: RTH
         field: noWayHomeAction
         table: geozone_rth_no_way_home
-        type: uint8_t    
+        type: uint8_t
diff --git 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, 9);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -242,8 +242,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
                 [PID_POS_Z] = {
                     .P = SETTING_NAV_FW_POS_Z_P_DEFAULT,      // FW_POS_Z_P * 100
                     .I = SETTING_NAV_FW_POS_Z_I_DEFAULT,      // FW_POS_Z_I * 100
-                    .D = SETTING_NAV_FW_POS_Z_D_DEFAULT,      // FW_POS_Z_D * 100
-                    .FF = 0,
+                    .D = SETTING_NAV_FW_POS_Z_D_DEFAULT,      // FW_POS_Z_D * 200
+                    .FF = SETTING_NAV_FW_POS_Z_FF_DEFAULT,    // FW_POS_Z_FF * 100
                 },
                 [PID_POS_XY] = {
                     .P = SETTING_NAV_FW_POS_XY_P_DEFAULT,     // FW_POS_XY_P * 100
@@ -1206,7 +1206,7 @@ void FAST_CODE pidController(float dT)
 
         // Limit desired rate to something gyro can measure reliably
         pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
-    
+
 #ifdef USE_ADAPTIVE_FILTER
         adaptiveFilterPushRate(axis, pidState[axis].rateTarget, currentControlRateProfile->stabilized.rates[axis]);
 #endif

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -4961,7 +4961,7 @@ void navigationUsePIDs(void)
     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 / 200.0f,
-                                        0.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,
                                         NAV_DTERM_CUT_HZ,
                                         0.0f
     );