From 38093e4887b4457b0146fcd7172b0c76cb43169a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 9 Apr 2020 10:04:52 +0200 Subject: [PATCH] mc_pos_control: correct sign of acceleration state Non-functional change, just change the sign in the correct place to avoid further confusion. --- .../mc_pos_control/PositionControl/PositionControl.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 906ab6475f..6f00dd1e25 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -145,7 +145,7 @@ 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); + Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d); // For backwards compatibility of the gains to non-acceleration based control, needs to be overcome with configuration conversion acc_sp_velocity *= CONSTANTS_ONE_G / _hover_thrust; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d32e72e9f5..a8307b6c86 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -480,8 +480,8 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) if (PX4_ISFINITE(_local_pos.vx) && PX4_ISFINITE(_local_pos.vy) && _local_pos.v_xy_valid) { _states.velocity(0) = _local_pos.vx; _states.velocity(1) = _local_pos.vy; - _states.acceleration(0) = _vel_x_deriv.update(-_states.velocity(0)); - _states.acceleration(1) = _vel_y_deriv.update(-_states.velocity(1)); + _states.acceleration(0) = _vel_x_deriv.update(_states.velocity(0)); + _states.acceleration(1) = _vel_y_deriv.update(_states.velocity(1)); } else { _states.velocity(0) = _states.velocity(1) = NAN;