TEMP

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})

src/lib/motion_planning/PositionSmoothing.cpp

@@ -35,8 +35,6 @@
 #include "TrajectoryConstraints.hpp"
 #include <mathlib/mathlib.h>
 #include <matrix/matrix/math.hpp>
-#include <matrix/matrix/helper_functions.hpp>
-
 
 void PositionSmoothing::_generateSetpoints(
 	const Vector3f &position,

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,15 +193,15 @@ 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
 		// only limit acceleration once within velocity constraints
 		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);
 		}
 	}
 

src/modules/mc_pos_control/GotoControl/GotoControl.hpp

@@ -43,9 +43,10 @@
 
 #pragma once
 
-#include <matrix/matrix/math.hpp>
+#include <lib/motion_planning/HeadingSmoothing.hpp>
 #include <lib/motion_planning/PositionSmoothing.hpp>
-#include <lib/motion_planning/HeadingSmoother.hpp>
+#include <mathlib/math/Limits.hpp>
+#include <matrix/matrix/math.hpp>
 #include <uORB/topics/goto_setpoint.h>
 #include <uORB/topics/trajectory_setpoint.h>
 
@@ -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};