FlightTaskOrbit: smooth yaw like in missions

PX4 · Jan 7, 2021 · 537ee5b · 537ee5b
1 parent 44606ca
commit 537ee5b
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 4 deletions.
diff --git a/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt b/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt
@@ -35,5 +35,5 @@ px4_add_library(FlightTaskOrbit
 	FlightTaskOrbit.cpp
 )
 
-target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmoothVel)
+target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmoothVel SlewRate)
 target_include_directories(FlightTaskOrbit PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
@@ -162,6 +162,8 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set
 	_center = Vector2f(_position);
 	_center(0) -= _r;
 	_initial_heading = _yaw;
+	_slew_rate_yaw.setForcedValue(_yaw);
+	_slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get()));
 
 	// need a valid position and velocity
 	ret = ret && PX4_ISFINITE(_position(0))
@@ -196,6 +198,9 @@ bool FlightTaskOrbit::update()
 		generate_circle_yaw_setpoints(center_to_position);
 	}
 
+	// Apply yaw smoothing
+	_yaw_setpoint = _slew_rate_yaw.update(_yaw_setpoint, _deltatime);
+
 	// publish information to UI
 	sendTelemetry();
 
@@ -204,16 +209,18 @@ bool FlightTaskOrbit::update()
 
 void FlightTaskOrbit::generate_circle_approach_setpoints(const Vector2f &center_to_position)
 {
+	const Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
+
 	if (_circle_approach_line.isEndReached()) {
 		// calculate target point on circle and plan a line trajectory
-		const Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
 		const Vector2f closest_circle_point = Vector2f(_position) + start_to_circle;
 		const Vector3f target = Vector3f(closest_circle_point(0), closest_circle_point(1), _position(2));
 		_circle_approach_line.setLineFromTo(_position, target);
 		_circle_approach_line.setSpeed(_param_mpc_xy_cruise.get());
-		_yaw_setpoint = atan2f(start_to_circle(1), start_to_circle(0));
 	}
 
+	_yaw_setpoint = atan2f(start_to_circle(1), start_to_circle(0));
+
 	// follow the planned line and switch to orbiting once the circle is reached
 	_circle_approach_line.generateSetpoints(_position_setpoint, _velocity_setpoint);
 	_in_circle_approach = !_circle_approach_line.isEndReached();

diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp
@@ -45,6 +45,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/orbit_status.h>
 #include <StraightLine.hpp>
+#include <SlewRateYaw.hpp>
 
 class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
 {
@@ -111,10 +112,12 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
 	/** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */
 	int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER;
 	float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */
+	SlewRateYaw<float> _slew_rate_yaw;
 
 	uORB::Publication<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise /**< cruise speed for circle approach */
+		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise, /**< cruise speed for circle approach */
+		(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max
 	)
 };