GotoControl: make interface over uORB

src/modules/mc_pos_control/GotoControl/GotoControl.cpp

@@ -38,15 +38,37 @@
 #include "GotoControl.hpp"
 #include "PositionControl.hpp"
 
+#include <drivers/drv_hrt.h>
 #include <float.h>
 #include <lib/mathlib/mathlib.h>
 
-void GotoControl::update(const float dt, const matrix::Vector3f &position, const float heading,
-			 const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint)
-{
-	trajectory_setpoint = PositionControl::empty_trajectory_setpoint;
+using namespace time_literals;
 
-	const Vector3f position_setpoint(goto_setpoint.position);
+bool GotoControl::checkForSetpoint(const hrt_abstime &now, const bool enabled)
+{
+	_goto_setpoint_sub.update();
+	const bool timestamp_initialized = _goto_setpoint_sub.get().timestamp != 0;
+	const bool no_timeout = now < (_goto_setpoint_sub.get().timestamp + 500_ms);
+	const bool need_to_run = timestamp_initialized && no_timeout && enabled;
+
+	if (!need_to_run) {
+		_is_initialized = false;
+	}
+
+	return need_to_run;
+}
+
+void GotoControl::update(const float dt, const matrix::Vector3f &position, const float heading)
+{
+	if (!_is_initialized) {
+		resetPositionSmoother(position);
+		resetHeadingSmoother(heading);
+		_is_initialized = true;
+	}
+
+	const goto_setpoint_s &goto_setpoint = _goto_setpoint_sub.get();
+
+	const Vector3f position_setpoint(_goto_setpoint_sub.get().position);
 
 	if (!position_setpoint.isAllFinite()) {
 		// TODO: error messaging
@@ -72,6 +94,8 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
 	_position_smoothing.generateSetpoints(position, position_setpoint, feedforward_velocity, dt,
 					      force_zero_velocity_setpoint, out_setpoints);
 
+	trajectory_setpoint_s trajectory_setpoint = PositionControl::empty_trajectory_setpoint;
+
 	_position_smoothing.getCurrentPosition().copyTo(trajectory_setpoint.position);
 	_position_smoothing.getCurrentVelocity().copyTo(trajectory_setpoint.velocity);
 	_position_smoothing.getCurrentAcceleration().copyTo(trajectory_setpoint.acceleration);
@@ -99,9 +123,18 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
 		_controlling_heading = false;
 	}
 
-	trajectory_setpoint.timestamp = goto_setpoint.timestamp;
-
 	_need_smoother_reset = false;
+
+	trajectory_setpoint.timestamp = goto_setpoint.timestamp;
+	_trajectory_setpoint_pub.publish(trajectory_setpoint);
+
+	vehicle_constraints_s vehicle_constraints{
+		.timestamp = goto_setpoint.timestamp,
+		.speed_up = NAN,
+		.speed_down = NAN,
+		.want_takeoff = false
+	};
+	_vehicle_constraints_pub.publish(vehicle_constraints);
 }
 
 void GotoControl::resetPositionSmoother(const matrix::Vector3f &position)

src/modules/mc_pos_control/GotoControl/GotoControl.hpp

@@ -47,8 +47,12 @@
 #include <mathlib/math/Limits.hpp>
 #include <matrix/matrix/math.hpp>
 #include <px4_platform_common/module_params.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/goto_setpoint.h>
 #include <uORB/topics/trajectory_setpoint.h>
+#include <uORB/topics/vehicle_constraints.h>
 
 class GotoControl : public ModuleParams
 {
@@ -59,6 +63,8 @@ public:
 	/** @param error [m] position smoother's maximum allowed horizontal position error at which trajectory integration halts */
 	void setMaxAllowedHorizontalPositionError(const float error) { _position_smoothing.setMaxAllowedHorizontalError(error); }
 
+	bool checkForSetpoint(const hrt_abstime &now, const bool enabled);
+
 	/**
 	 * @brief resets the position smoother at the current position with zero velocity and acceleration.
 	 *
@@ -83,10 +89,7 @@ public:
 	 * @param[in] goto_setpoint struct containing current go-to setpoints
 	 * @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints
 	 */
-	void update(const float dt, const matrix::Vector3f &position, const float heading,
-		    const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint);
-
-	bool is_initialized{false};
+	void update(const float dt, const matrix::Vector3f &position, const float heading);
 
 private:
 	void updateParams() override;
@@ -105,9 +108,15 @@ private:
 	 */
 	void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint);
 
+	uORB::SubscriptionData<goto_setpoint_s> _goto_setpoint_sub{ORB_ID(goto_setpoint)};
+	uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
+	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
+
 	PositionSmoothing _position_smoothing;
 	HeadingSmoothing _heading_smoothing;
 
+	bool _is_initialized{false}; ///< true if smoothers were reset to current state
+
 	// flags that the next update() requires a valid current vehicle position to reset the smoothers
 	bool _need_smoother_reset{true};