fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 01:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: log individual portions of PID velocity control

Browse Source
This commit is contained in:
Matthias Grob
2022-05-12 17:21:20 +02:00
parent 57a0289627
commit 527bdbb783
3 changed files with 19 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_local_position_setpoint.msg
+4
View File
@@ -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
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+10 -3
View File
@@ -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
src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+5
View File
@@ -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;
};