diff --git a/src/lib/motion_planning/CMakeLists.txt b/src/lib/motion_planning/CMakeLists.txt index f913c495d6a6..6b04cded05fe 100644 --- a/src/lib/motion_planning/CMakeLists.txt +++ b/src/lib/motion_planning/CMakeLists.txt @@ -32,10 +32,11 @@ ############################################################################ px4_add_library(motion_planning - VelocitySmoothing.cpp - PositionSmoothing.cpp + HeadingSmoothing.cpp ManualVelocitySmoothingXY.cpp ManualVelocitySmoothingZ.cpp + PositionSmoothing.cpp + VelocitySmoothing.cpp ) target_include_directories(motion_planning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index 8f05f39e2dd5..28c70c690d60 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -35,8 +35,6 @@ #include "TrajectoryConstraints.hpp" #include #include -#include - void PositionSmoothing::_generateSetpoints( const Vector3f &position, diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp index dd05e635a3b5..6891db9f0672 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp @@ -147,7 +147,7 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint if (_position_smoother.getCurrentVelocityXY().norm() <= max_horizontal_speed) { const float speed_scale = max_horizontal_speed / _vehicle_constraints.max_horizontal_speed; max_horizontal_accel = math::constrain(_vehicle_constraints.max_horizontal_accel * speed_scale, - VelocitySmoothing::kMinAccel, + 0.f, _vehicle_constraints.max_horizontal_accel); } } @@ -175,7 +175,7 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint // only limit acceleration once within velocity constraints if (fabsf(_position_smoother.getCurrentVelocityZ()) <= max_vertical_speed) { const float speed_scale = max_vertical_speed / vehicle_max_vertical_speed; - max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, VelocitySmoothing::kMinAccel, + max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, 0.f, vehicle_max_vertical_accel); } } @@ -193,7 +193,7 @@ void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint) float max_heading_accel = _vehicle_constraints.max_heading_accel; if (goto_setpoint.flag_set_max_heading_rate && PX4_ISFINITE(goto_setpoint.max_heading_rate)) { - max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, HeadingSmoother::kMinHeadingRate, + max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, 0.f, _vehicle_constraints.max_heading_rate); // linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic @@ -201,7 +201,7 @@ void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint) if (fabsf(_heading_smoother.getSmoothedHeadingRate()) <= max_heading_rate) { const float rate_scale = max_heading_rate / _vehicle_constraints.max_heading_rate; max_heading_accel = math::constrain(_vehicle_constraints.max_heading_accel * rate_scale, - HeadingSmoother::kMinHeadingAccel, _vehicle_constraints.max_heading_accel); + 0.f, _vehicle_constraints.max_heading_accel); } } diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp index ee9d4d2a9ca6..03ec538134ce 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp @@ -43,9 +43,10 @@ #pragma once -#include +#include #include -#include +#include +#include #include #include @@ -63,12 +64,12 @@ class GotoControl float max_horizontal_speed = kMinSpeed; // [m/s] float max_down_speed = kMinSpeed; // [m/s] float max_up_speed = kMinSpeed; // [m/s] - float max_horizontal_accel = VelocitySmoothing::kMinAccel; // [m/s^2] - float max_down_accel = VelocitySmoothing::kMinAccel; // [m/s^2] - float max_up_accel = VelocitySmoothing::kMinAccel; // [m/s^2] - float max_jerk = VelocitySmoothing::kMinJerk; // [m/s^3] - float max_heading_rate = HeadingSmoother::kMinHeadingRate; // [rad/s] - float max_heading_accel = HeadingSmoother::kMinHeadingAccel; // [rad/s^2] + float max_horizontal_accel = 0.f; // [m/s^2] + float max_down_accel = 0.f; // [m/s^2] + float max_up_accel = 0.f; // [m/s^2] + float max_jerk = 0.f; // [m/s^3] + float max_heading_rate = 0.f; // [rad/s] + float max_heading_accel = 0.f; // [rad/s^2] }; /** @@ -109,14 +110,14 @@ class GotoControl _vehicle_constraints.max_horizontal_speed = math::max(kMinSpeed, vehicle_constraints.max_horizontal_speed); _vehicle_constraints.max_down_speed = math::max(kMinSpeed, vehicle_constraints.max_down_speed); _vehicle_constraints.max_up_speed = math::max(kMinSpeed, vehicle_constraints.max_up_speed); - _vehicle_constraints.max_horizontal_accel = math::max(VelocitySmoothing::kMinAccel, + _vehicle_constraints.max_horizontal_accel = math::max(0.f, vehicle_constraints.max_horizontal_accel); - _vehicle_constraints.max_down_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_down_accel); - _vehicle_constraints.max_up_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_up_accel); - _vehicle_constraints.max_jerk = math::max(VelocitySmoothing::kMinJerk, vehicle_constraints.max_jerk); - _vehicle_constraints.max_heading_rate = math::max(HeadingSmoother::kMinHeadingRate, + _vehicle_constraints.max_down_accel = math::max(0.f, vehicle_constraints.max_down_accel); + _vehicle_constraints.max_up_accel = math::max(0.f, vehicle_constraints.max_up_accel); + _vehicle_constraints.max_jerk = math::max(0.f, vehicle_constraints.max_jerk); + _vehicle_constraints.max_heading_rate = math::max(0.f, vehicle_constraints.max_heading_rate); - _vehicle_constraints.max_heading_accel = math::max(HeadingSmoother::kMinHeadingAccel, + _vehicle_constraints.max_heading_accel = math::max(0.f, vehicle_constraints.max_heading_accel); } @@ -151,7 +152,7 @@ class GotoControl VehicleConstraints _vehicle_constraints{}; PositionSmoothing _position_smoother; - HeadingSmoother _heading_smoother; + HeadingSmoothing _heading_smoother; // flags that the next update() requires a valid current vehicle position to reset the smoothers bool _need_smoother_reset{true};