From 285798446a926397f7d498808509fb94fe8b96a4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 14 Aug 2021 16:06:34 +1000
Subject: [PATCH] AC_AttitudeControl: apply EKF Z gain scaler

for flying with DCM
---
 libraries/AC_AttitudeControl/AC_PosControl.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index 01c4a0457c..3ca19d220e 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -625,8 +625,8 @@ void AC_PosControl::update_xy_controller()
     }
     _last_update_xy_us = AP_HAL::micros64();
 
-    float ekfGndSpdLimit, ekfNavVelGainScaler;
-    AP::ahrs().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
+    float ahrsGndSpdLimit, ahrsControlScaleXY;
+    AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY);
 
     // Position Controller
 
@@ -634,7 +634,7 @@ void AC_PosControl::update_xy_controller()
     Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, curr_pos, _limit.pos_xy);
 
     // add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise
-    vel_target *= ekfNavVelGainScaler;
+    vel_target *= ahrsControlScaleXY;
     _vel_target.x = vel_target.x;
     _vel_target.y = vel_target.y;
     _vel_target.x += _vel_desired.x;
@@ -652,7 +652,7 @@ void AC_PosControl::update_xy_controller()
     }
     Vector2f accel_target = _pid_vel_xy.update_all(Vector2f{_vel_target.x, _vel_target.y}, _vehicle_horiz_vel, Vector2f(_limit_vector.x, _limit_vector.y));
     // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
-    accel_target *= ekfNavVelGainScaler;
+    accel_target *= ahrsControlScaleXY;
 
     // pass the correction acceleration to the target acceleration output
     _accel_target.x = accel_target.x;
@@ -962,6 +962,7 @@ void AC_PosControl::update_z_controller()
     float pos_target_zf = _pos_target.z;
 
     _vel_target.z = _p_pos_z.update_all(pos_target_zf, curr_alt, _limit.pos_down, _limit.pos_up);
+    _vel_target.z *= AP::ahrs().getControlScaleZ();
 
     _pos_target.z = pos_target_zf;
 
@@ -972,6 +973,7 @@ void AC_PosControl::update_z_controller()
 
     const Vector3f& curr_vel = _inav.get_velocity();
     _accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel.z, _motors.limit.throttle_lower, _motors.limit.throttle_upper);
+    _accel_target.z *= AP::ahrs().getControlScaleZ();
 
     // add feed forward component
     _accel_target.z += _accel_desired.z;