AC_AttitudeControl: Add PosControl PID logging

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -896,6 +896,8 @@ void AC_PosControl::write_log()
 
    AP::logger().Write_PSC(get_pos_target(), _inav.get_position(), get_vel_target(), _inav.get_velocity(), get_accel_target(), accel_x, accel_y);
 
+   AP::logger().Write_PSCP(_vel_error, _vel_xy_p, _vel_xy_i, _vel_xy_d);
+
 }
 
 /// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
@@ -1038,7 +1040,7 @@ void AC_PosControl::run_xy_controller(float dt)
 
     // the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
 
-    Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;
+    Vector2f accel_target;
 
     // check if vehicle velocity is being overridden
     if (_flags.vehicle_horiz_vel_override) {
@@ -1057,22 +1059,22 @@ void AC_PosControl::run_xy_controller(float dt)
     _pid_vel_xy.set_input(_vel_error);
 
     // get p
-    vel_xy_p = _pid_vel_xy.get_p();
+    _vel_xy_p = _pid_vel_xy.get_p();
 
     // update i term if we have not hit the accel or throttle limits OR the i term will reduce
     // TODO: move limit handling into the PI and PID controller
     if (!_limit.accel_xy && !_motors.limit.throttle_upper) {
-        vel_xy_i = _pid_vel_xy.get_i();
+        _vel_xy_i = _pid_vel_xy.get_i();
     } else {
-        vel_xy_i = _pid_vel_xy.get_i_shrink();
+       _vel_xy_i = _pid_vel_xy.get_i_shrink();
     }
 
     // get d
-    vel_xy_d = _pid_vel_xy.get_d();
+    _vel_xy_d = _pid_vel_xy.get_d();
 
     // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
-    accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;
-    accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;
+    accel_target.x = (_vel_xy_p.x + _vel_xy_i.x + _vel_xy_d.x) * ekfNavVelGainScaler;
+    accel_target.y = (_vel_xy_p.y + _vel_xy_i.y + _vel_xy_d.y) * ekfNavVelGainScaler;
 
     // reset accel to current desired acceleration
     if (_flags.reset_accel_to_lean_xy) {

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -421,6 +421,9 @@ protected:
     Vector2f    _vehicle_horiz_vel;     // velocity to use if _flags.vehicle_horiz_vel_override is set
     LowPassFilterFloat _vel_error_filter;   // low-pass-filter on z-axis velocity error
 
+    // XY PID terms for logging
+    Vector2f _vel_xy_p, _vel_xy_i, _vel_xy_d;
+
     LowPassFilterVector2f _accel_target_filter; // acceleration target filter
 
     // ekf reset handling