trajectory-planning: Added option for PositionSmoothing library with single target waypoint that disables L1 guidance

src/lib/motion_planning/PositionSmoothing.cpp

@@ -38,9 +38,10 @@
 #include <matrix/matrix/helper_functions.hpp>
 
 
-void PositionSmoothing::generateSetpoints(
+void PositionSmoothing::_generateSetpoints(
 	const Vector3f &position,
 	const Vector3f(&waypoints)[3],
+	bool is_single_waypoint,
 	const Vector3f &feedforward_velocity,
 	float delta_time,
 	bool force_zero_velocity_setpoint,
@@ -49,7 +50,7 @@ void PositionSmoothing::generateSetpoints(
 	Vector3f velocity_setpoint{0.f, 0.f, 0.f};
 
 	if (!force_zero_velocity_setpoint) {
-		velocity_setpoint = _generateVelocitySetpoint(position, waypoints, feedforward_velocity);
+		velocity_setpoint = _generateVelocitySetpoint(position, waypoints, is_single_waypoint, feedforward_velocity);
 	}
 
 	out_setpoints.unsmoothed_velocity = velocity_setpoint;
@@ -184,6 +185,7 @@ const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve
 }
 
 const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
+		bool is_single_waypoint,
 		const Vector3f &feedforward_velocity_setpoint)
 {
 	// Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
@@ -201,13 +203,13 @@ const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &posi
 		Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
 				  _trajectory[1].getCurrentPosition(),
 				  _trajectory[2].getCurrentPosition());
-
+		const Vector3f crossing_point = is_single_waypoint ? target : _getCrossingPoint(position, waypoints);
-		const Vector3f u_pos_traj_to_dest((_getCrossingPoint(position, waypoints) - pos_traj).unit_or_zero());
+		const Vector3f u_pos_traj_to_dest{(crossing_point - pos_traj).unit_or_zero()};
 
 		float xy_speed = _getMaxXYSpeed(waypoints);
 		const float z_speed = _getMaxZSpeed(waypoints);
 
-		if (_isTurning(target)) {
+		if (!is_single_waypoint && _isTurning(target)) {
 			// Limit speed during a turn
 			xy_speed = math::min(_max_speed_previous, xy_speed);
 
@@ -235,7 +237,8 @@ const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &posi
 
 		// Get various path specific vectors
 		Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
-		Vector2f pos_traj_to_dest_xy = Vector2f(_getCrossingPoint(position, waypoints)) - pos_traj;
+		Vector2f crossing_point = is_single_waypoint ? Vector2f(target) : Vector2f(_getCrossingPoint(position, waypoints));
+		Vector2f pos_traj_to_dest_xy = crossing_point - pos_traj;
 		Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
 
 		float xy_speed = _getMaxXYSpeed(waypoints);

src/lib/motion_planning/PositionSmoothing.hpp

@@ -68,7 +68,7 @@ public:
 
 	/**
 	 * @brief Generates new setpoints for jerk, acceleration, velocity and position
-	 * to reach the given waypoint smoothly from current position.
+	 * to reach the given waypoint triplet smoothly from current position
 	 *
 	 * @param position Current position of the vehicle
 	 * @param waypoints 0: Past waypoint, 1: target, 2: Target after next target
@@ -77,14 +77,44 @@ public:
 	 * @param force_zero_velocity_setpoint Force vehicle to stop. Generate trajectory that ends with still vehicle.
 	 * @param out_setpoints Output of the generated setpoints
 	 */
-	void generateSetpoints(
+	inline void generateSetpoints(
 		const Vector3f &position,
 		const Vector3f(&waypoints)[3],
 		const Vector3f &feedforward_velocity,
 		float delta_time,
 		bool force_zero_velocity_setpoint,
 		PositionSmoothingSetpoints &out_setpoints
-	);
+	)
+	{
+		_generateSetpoints(position, waypoints, false, feedforward_velocity, delta_time, force_zero_velocity_setpoint,
+				   out_setpoints);
+	}
+
+	/**
+	 * @brief Generates new setpoints for jerk, acceleration, velocity and position
+	 * to reach the given waypoint smoothly from current position.
+	 *
+	 * @param position Current position of the vehicle
+	 * @param waypoint desired waypoint
+	 * @param feedforward_velocity FF velocity
+	 * @param delta_time Time since last invocation of the function
+	 * @param force_zero_velocity_setpoint Force vehicle to stop. Generate trajectory that ends with still vehicle.
+	 * @param out_setpoints Output of the generated setpoints
+	 */
+	inline void generateSetpoints(
+		const Vector3f &position,
+		const Vector3f &waypoint,
+		const Vector3f &feedforward_velocity,
+		float delta_time,
+		bool force_zero_velocity_setpoint,
+		PositionSmoothingSetpoints &out_setpoints
+	)
+	{
+		Vector3f waypoints[3] = {waypoint, waypoint, waypoint};
+		_generateSetpoints(position, waypoints, true, feedforward_velocity, delta_time, force_zero_velocity_setpoint,
+				   out_setpoints);
+	}
+
 
 	/**
 	 * @brief Reset internal state to the given values
@@ -394,7 +424,19 @@ private:
 
 	/* Internal functions */
 	bool _isTurning(const Vector3f &target) const;
+
+	void _generateSetpoints(
+		const Vector3f &position,
+		const Vector3f(&waypoints)[3],
+		bool is_single_waypoint,
+		const Vector3f &feedforward_velocity,
+		float delta_time,
+		bool force_zero_velocity_setpoint,
+		PositionSmoothingSetpoints &out_setpoints
+	);
+
 	const Vector3f _generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
+			bool is_single_waypoint,
 			const Vector3f &feedforward_velocity_setpoint);
 	const Vector2f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
 	const Vector3f _getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const;

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp

@@ -282,9 +282,7 @@ void FlightTaskOrbit::_generate_circle_approach_setpoints()
 	const Vector3f target_circle_point{closest_point_on_circle(0), closest_point_on_circle(1), _center(2)};
 
 	PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
-	_position_smoothing.generateSetpoints(_position, {
+	_position_smoothing.generateSetpoints(_position, target_circle_point,
-		_circle_approach_start_position, target_circle_point, target_circle_point
-	},
 	{0.f, 0.f, 0.f}, _deltatime, false, out_setpoints);
 
 	_yaw_setpoint = atan2f(position_to_center_xy(1), position_to_center_xy(0));