diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 6f00dd1e25..70d23c7885 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -67,7 +67,7 @@ void PositionControl::setThrustLimits(const float min, const float max) void PositionControl::updateHoverThrust(const float hover_thrust_new) { - _vel_int(2) += hover_thrust_new - _hover_thrust; + _vel_int(2) += (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new); setHoverThrust(hover_thrust_new); } @@ -147,8 +147,6 @@ void PositionControl::_velocityControl(const float dt) Vector3f vel_error = _vel_sp - _vel; 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; // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); @@ -179,7 +177,7 @@ void PositionControl::_velocityControl(const float dt) // Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 const Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust); - const float arw_gain = 2.f / (_gain_vel_p(0) * (CONSTANTS_ONE_G / _hover_thrust)); + const float arw_gain = 2.f / _gain_vel_p(0); vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited)); // Make sure integral doesn't get NAN @@ -188,7 +186,7 @@ void PositionControl::_velocityControl(const float dt) _vel_int += vel_error.emult(_gain_vel_i) * dt; // limit thrust integral - _vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * sign(_vel_int(2)); + _vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2)); } void PositionControl::_accelerationControl() diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index a581ec3529..4986c93a86 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -163,11 +163,6 @@ public: */ void resetIntegral() { _vel_int.setZero(); } - /** - * @return the value of the velocity integrator - */ - matrix::Vector3f getIntegral() const { return _vel_int; } - /** * Get the controllers output local position setpoint * These setpoints are the ones which were executed on including PID output and feed-forward. @@ -205,7 +200,7 @@ private: float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1 float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have - float _hover_thrust{}; ///< Thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation + float _hover_thrust{}; ///< Thrust [0,1] with which the vehicle hovers not accelerating down or up with level orientation // States matrix::Vector3f _pos; /**< current position */ diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 9a42f198e3..1facfc9234 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -76,7 +76,7 @@ public: PositionControlBasicTest() { _position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f)); - _position_control.setVelocityGains(Vector3f(1.f, 1.f, 1.f), Vector3f(1.f, 1.f, 1.f), Vector3f(1.f, 1.f, 1.f)); + _position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f)); _position_control.setVelocityLimits(1.f, 1.f, 1.f); _position_control.setThrustLimits(0.1f, 0.9f); _position_control.setTiltLimit(1.f); @@ -387,6 +387,4 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust) // THEN: the integral is updated to avoid discontinuities and // the output is still the same EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); - const Vector3f integrator_new(_position_control.getIntegral()); - EXPECT_EQ(integrator_new(2) - hover_thrust_new, -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 2ad528ec54..520c8ad3dd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -362,9 +362,12 @@ MulticopterPositionControl::parameters_update(bool force) } _control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get())); - _control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), _param_mpc_z_vel_p.get()), - Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()), - Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get())); + // backwards compatibility scale for velocity gains to non-acceleration based control, needs to be overcome with configuration conversion + const float hover_scale = 20.f; + _control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), + _param_mpc_z_vel_p.get()) * hover_scale, + Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()) * hover_scale, + Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get()) * hover_scale); _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); _control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get()); _control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians!