From 527bdbb783110b988838373644ea21146b7925cd Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 12 May 2022 17:21:05 +0200 Subject: [PATCH] PositionControl: log individual portions of PID velocity control --- msg/vehicle_local_position_setpoint.msg | 4 ++++ .../PositionControl/PositionControl.cpp | 13 ++++++++++--- .../PositionControl/PositionControl.hpp | 5 +++++ 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg index 0093d52d77..fc543b77a0 100644 --- a/msg/vehicle_local_position_setpoint.msg +++ b/msg/vehicle_local_position_setpoint.msg @@ -17,3 +17,7 @@ float32[3] thrust # normalized thrust vector in NED float32 yaw # in radians NED -PI..+PI float32 yawspeed # in radians/sec + +float32[3] velocity_proportional_portion +float32[3] velocity_integral_potion +float32[3] velocity_derivative_portion diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 8dd2c521e7..bcd23e3c95 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -141,7 +141,10 @@ void PositionControl::_velocityControl(const float dt) { // PID velocity control Vector3f vel_error = _vel_sp - _vel; - Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d); + _velocity_proportional_portion = vel_error.emult(_gain_vel_p); + _velocity_integral_potion = _vel_int; + _velocity_derivative_portion = - _vel_dot.emult(_gain_vel_d); + Vector3f acc_sp_velocity = _velocity_proportional_portion + _velocity_integral_potion + _velocity_derivative_portion; // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); @@ -240,13 +243,17 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s local_position_setpoint.x = _pos_sp(0); local_position_setpoint.y = _pos_sp(1); local_position_setpoint.z = _pos_sp(2); - local_position_setpoint.yaw = _yaw_sp; - local_position_setpoint.yawspeed = _yawspeed_sp; local_position_setpoint.vx = _vel_sp(0); local_position_setpoint.vy = _vel_sp(1); local_position_setpoint.vz = _vel_sp(2); _acc_sp.copyTo(local_position_setpoint.acceleration); _thr_sp.copyTo(local_position_setpoint.thrust); + local_position_setpoint.yaw = _yaw_sp; + local_position_setpoint.yawspeed = _yawspeed_sp; + + _velocity_proportional_portion.copyTo(local_position_setpoint.velocity_proportional_portion); + _velocity_integral_potion.copyTo(local_position_setpoint.velocity_integral_potion); + _velocity_derivative_portion.copyTo(local_position_setpoint.velocity_derivative_portion); } void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index fa2fd35f80..346aa31f0e 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -216,4 +216,9 @@ private: matrix::Vector3f _thr_sp; /**< desired thrust */ float _yaw_sp{}; /**< desired heading */ float _yawspeed_sp{}; /** desired yaw-speed */ + + // Velocity PID portions for logging + matrix::Vector3f _velocity_proportional_portion; + matrix::Vector3f _velocity_integral_potion; + matrix::Vector3f _velocity_derivative_portion; };