diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 0151a98aff..de5e576598 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -261,6 +261,9 @@ void MulticopterPositionControl::parameters_update(bool force) _takeoff.setSpoolupTime(_param_com_spoolup_time.get()); _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); _takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get()); + + // set position controller jerk limit to MPC_JERK_MAX + 10% margin to not interfere with reference trajectory + _control.setJerkLimit(_param_mpc_jerk_max.get() * 1.1f); } } diff --git a/src/modules/mc_pos_control/PositionControl/CMakeLists.txt b/src/modules/mc_pos_control/PositionControl/CMakeLists.txt index 8136317376..787fa3c56b 100644 --- a/src/modules/mc_pos_control/PositionControl/CMakeLists.txt +++ b/src/modules/mc_pos_control/PositionControl/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_library(PositionControl PositionControl.hpp ) target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(PositionControl PRIVATE SlewRate) px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl) px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index b770ab5a68..fd068c95b9 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -146,7 +146,7 @@ void PositionControl::_velocityControl(const float dt) // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); - _accelerationControl(); + _accelerationControl(dt); // Integrator anti-windup in vertical direction if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) || @@ -201,8 +201,13 @@ void PositionControl::_velocityControl(const float dt) _vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2)); } -void PositionControl::_accelerationControl() +void PositionControl::_accelerationControl(const float dt) { + // don't allow accel setpoint to change faster than MPC_JERK_MAX m/s^2/s + for (int i = 0; i < 3; i++) { + _acc_sp(i) = _acc_sp_slew_rate[i].update(_acc_sp(i), dt); + } + // Assume standard acceleration due to gravity in vertical direction for attitude generation Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized(); ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index fa2fd35f80..116abfdb88 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -75,7 +76,11 @@ class PositionControl { public: - PositionControl() = default; + PositionControl() + { + setJerkLimit(INFINITY); + }; + ~PositionControl() = default; /** @@ -119,6 +124,13 @@ public: */ void setTiltLimit(const float tilt) { _lim_tilt = tilt; } + void setJerkLimit(const float jerk_max) + { + _acc_sp_slew_rate[0].setSlewRate(jerk_max); + _acc_sp_slew_rate[1].setSlewRate(jerk_max); + _acc_sp_slew_rate[2].setSlewRate(jerk_max); + } + /** * Set the normalized hover thrust * @param thrust [0.1, 0.9] with which the vehicle hovers not acelerating down or up with level orientation @@ -183,7 +195,7 @@ private: void _positionControl(); ///< Position proportional control void _velocityControl(const float dt); ///< Velocity PID control - void _accelerationControl(); ///< Acceleration setpoint processing + void _accelerationControl(const float dt); ///< Acceleration setpoint processing // Gains matrix::Vector3f _gain_pos_p; ///< Position control proportional gain @@ -216,4 +228,6 @@ private: matrix::Vector3f _thr_sp; /**< desired thrust */ float _yaw_sp{}; /**< desired heading */ float _yawspeed_sp{}; /** desired yaw-speed */ + + SlewRate _acc_sp_slew_rate[3]; };