From 6fda6e43af6970d11170820223c2f731221e49fa Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 18 Mar 2021 11:19:59 +0100 Subject: [PATCH] StickAcceleration: synchronize XY acceleration profiles --- .../tasks/Utility/StickAccelerationXY.cpp | 10 ++++++++-- .../tasks/Utility/StickAccelerationXY.hpp | 1 + 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index 821ffdface..973471ea4e 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -99,6 +99,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, _velocity_setpoint += _acceleration_setpoint * dt; lockPosition(pos, vel_sp_feedback, dt); + _acceleration_setpoint_prev = _acceleration_setpoint; } void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vector3f &acc_sp) @@ -111,8 +112,13 @@ void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vecto void StickAccelerationXY::applyFeasibilityLimit(const float dt) { // Apply jerk limit - acceleration slew rate - _acceleration_slew_rate_x.setSlewRate(_param_mpc_jerk_max.get()); - _acceleration_slew_rate_y.setSlewRate(_param_mpc_jerk_max.get()); + // Scale each jerk limit with the normalized projection of the acceleration + // setpoint increment to produce a synchronized motion + const Vector2f dir = Vector2f(_acceleration_setpoint - _acceleration_setpoint_prev).unit_or_zero(); + const float jerk_max_x = fabsf(dir(0)) * _param_mpc_jerk_max.get(); + const float jerk_max_y = fabsf(dir(1)) * _param_mpc_jerk_max.get(); + _acceleration_slew_rate_x.setSlewRate(jerk_max_x); + _acceleration_slew_rate_y.setSlewRate(jerk_max_y); _acceleration_setpoint(0) = _acceleration_slew_rate_x.update(_acceleration_setpoint(0), dt); _acceleration_setpoint(1) = _acceleration_slew_rate_y.update(_acceleration_setpoint(1), dt); } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index f7e1b4bbaa..a9eda763c0 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -73,6 +73,7 @@ private: matrix::Vector2f _position_setpoint; matrix::Vector2f _velocity_setpoint; matrix::Vector2f _acceleration_setpoint; + matrix::Vector2f _acceleration_setpoint_prev; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_vel_manual,