39 #include <mathlib/mathlib.h> 41 #include <px4_platform_common/defines.h> 46 ModuleParams(parent), _vel_sp_prev(vel)
55 const float &yawrate_sp,
const float dt)
64 const float &yawrate_sp,
const float dt)
75 const float &yawrate_sp)
97 const bool is_aligned = (vel_sp_heading * vel_heading) > -0.707
f;
102 bool yawspeed_demand = fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp);
103 bool direction_change = !is_aligned && (vel_sp.
length() > 0.7f *
_vel_max) && !yawspeed_demand
107 if (direction_change) {
110 }
else if (deceleration) {
144 if (_param_mpc_jerk_max.get() > _param_mpc_jerk_min.get() && _param_mpc_jerk_min.get() >
FLT_EPSILON) {
147 /
_vel_max * vel.
length() + _param_mpc_jerk_min.get(), _param_mpc_jerk_max.get());
matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw)
Rotate vector from local frame into heading frame.
ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel)
Vector3< Type > conjugate(const Vector3< Type > &vec) const
Rotates vector v_1 in frame 1 to vector v_2 in frame 2 using the rotation quaternion q_21 describing ...
float _acc_state_dependent
velocity change limit that depends on Intention
matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw)
Rotate vector from heading frame to local frame.
Vector normalized() const
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Vector2< float > Vector2f
float _jerk_state_dependent
acceleration change limit during brake
Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, const float &yawrate_sp)
Gets user intention.
constexpr _Tp min(_Tp a, _Tp b)
matrix::Vector2f _vel_sp_prev
previous velocity setpoint
void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, const float &yawrate_sp, const float dt)
Sets velocity change limits (=acceleration).
Vector3< float > Vector3f
float _vel_max
maximum horizontal speed allowed
void smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, const float &yawrate_sp, const float dt)
Smoothing of velocity setpoint horizontally based on flight direction.
AxisAngle< float > AxisAnglef
void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
Limits the velocity setpoint change.
Intention _intention
user intention
void _setStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention, const float dt)
Set acceleration depending on Intention.
Vector3< Type > conjugate_inversed(const Vector3< Type > &vec) const
Rotates vector v_2 in frame 2 to vector v_1 in frame 1 using the rotation quaternion q_21 describing ...