mc_pos_control: correct sign of acceleration state

Non-functional change, just change the sign in the correct place to avoid further confusion.
PX4 · Apr 9, 2020 · 38093e4 · 38093e4
1 parent 74e99fa
commit 38093e4
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git 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
@@ -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;