AC_PosControl: gain for accel feedforward

bnsgeyer · Jan 18, 2024 · ee2c40a · ee2c40a
1 parent 0c56b54
commit ee2c40a
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 1 deletion.
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -293,6 +293,15 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_JERK_Z", 11, AC_PosControl, _shaping_jerk_z, POSCONTROL_JERK_Z),
 
+
+    // @Param: _ACCXY_FF
+    // @DisplayName: Acceleration (horizontal) feed forward gain
+    // @Description: Acceleration (horizontal) feed forward gain.  Sets the amount of acceleration is fed to forward path.
+    // @Range: 0 1
+    // @Increment: 0.01
+    // @User: Advanced
+    AP_GROUPINFO("_ACCXY_FF", 12, AC_PosControl, _accel_xy_ff, 1.0f),
+
     AP_GROUPEND
 };
 
@@ -643,7 +652,7 @@ void AC_PosControl::update_xy_controller()
     _accel_target.xy() = accel_target;
 
     // Add feed forward into the target acceleration output
-    _accel_target.xy() += _accel_desired.xy();
+    _accel_target.xy() += _accel_desired.xy() * constrain_float(_accel_xy_ff, 0.0f, 1.0f);
 
     // Acceleration Controller
 

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -440,6 +440,7 @@ class AC_PosControl
     AC_PID_2D       _pid_vel_xy;        // XY axis velocity controller to convert velocity error to desired acceleration
     AC_PID_Basic    _pid_vel_z;         // Z axis velocity controller to convert climb rate error to desired acceleration
     AC_PID          _pid_accel_z;       // Z axis acceleration controller to convert desired acceleration to throttle output
+    AP_Float        _accel_xy_ff;       // Gain for amount of acceleration is fed to forward path
 
     // internal variables
     float       _dt;                    // time difference (in seconds) since the last loop time