diff --git a/docs/Settings.md b/docs/Settings.md index 9eb340ab8dc..a41cf73903d 100644 --- 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 index 5fd9df7d973..1f8eb9bb55e 100644 --- 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 index f0c3dddb0e7..1d77210841f 100644 --- 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,7 +4396,7 @@ 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 @@ -4398,7 +4404,7 @@ groups: - 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 index f1b2f567812..ae8eb57fb30 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, 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 index 3786b0d2f71..02d035ecffd 100644 --- 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 );