Separate message for trajectory setpoint

msg/CMakeLists.txt

@@ -164,6 +164,7 @@ set(msg_files
 	timesync.msg
 	timesync_status.msg
 	trajectory_bezier.msg
+	trajectory_setpoint.msg
 	trajectory_waypoint.msg
 	transponder_report.msg
 	tune_control.msg

msg/tools/urtps_bridge_topics.yaml

@@ -59,8 +59,7 @@ rtps:
     send:    true
   - msg:     vehicle_local_position_setpoint
     receive: true
-  - msg:     trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint
-    base:    vehicle_local_position_setpoint
+  - msg:     trajectory_setpoint
     receive: true
   - msg:     vehicle_odometry
     send:    true

msg/trajectory_setpoint.msg

@@ -0,0 +1,15 @@
+# Trajectory setpoint in NED frame
+# Input to PID position controller.
+# Needs to be cinematically consistent and feasible for smooth flight.
+# setting a value to NaN means the state should not be controlled
+
+uint64 timestamp # time since system start (microseconds)
+
+# NED local world frame
+float32[3] position # in meters
+float32[3] velocity # in meters/second
+float32[3] acceleration # in meters/second^2
+float32[3] jerk # in meters/second^3 (for logging only)
+
+float32 yaw # euler angle of desired attitude in radians -PI..+PI
+float32 yawspeed # angular velocity around NED frame z-axis in radians/second

msg/vehicle_local_position_setpoint.msg

@@ -1,5 +1,6 @@
 # Local position setpoint in NED frame
-# setting something to NaN means the state should not be controlled
+# Telemetry of PID position controller to monitor tracking.
+# NaN means the state was not controlled
 
 uint64 timestamp	# time since system start (microseconds)
 
@@ -12,7 +13,4 @@ float32 vx		# in meters/sec
 float32 vy		# in meters/sec
 float32 vz		# in meters/sec
 float32[3] acceleration # in meters/sec^2
-float32[3] jerk # in meters/sec^3
 float32[3] thrust	# normalized thrust vector in NED
-
-# TOPICS vehicle_local_position_setpoint trajectory_setpoint

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -459,12 +459,12 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	_current_task.task->setYawHandler(_wv_controller);
 
 	// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
-	vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
+	trajectory_setpoint_s setpoint = FlightTask::empty_setpoint;
 	vehicle_constraints_s constraints = FlightTask::empty_constraints;
 
 	if (_current_task.task->updateInitialize() && _current_task.task->update()) {
 		// setpoints and constraints for the position controller from flighttask
-		setpoint = _current_task.task->getPositionSetpoint();
+		setpoint = _current_task.task->getTrajectorySetpoint();
 		constraints = _current_task.task->getConstraints();
 	}
 
@@ -504,7 +504,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	_old_landing_gear_position = landing_gear.landing_gear;
 }
 
-void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint,
+void FlightModeManager::limitAltitude(trajectory_setpoint_s &setpoint,
 				      const vehicle_local_position_s &vehicle_local_position)
 {
 	if (_param_lndmc_alt_max.get() < 0.0f || !_home_position_sub.get().valid_alt
@@ -518,8 +518,8 @@ void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoin
 
 	if (vehicle_local_position.z < min_z) {
 		// above maximum altitude, only allow downwards flight == positive vz-setpoints (NED)
-		setpoint.z = min_z;
-		setpoint.vz = math::max(setpoint.vz, 0.f);
+		setpoint.position[2] = min_z;
+		setpoint.velocity[2] = math::max(setpoint.velocity[2], 0.f);
 	}
 }
 
@@ -531,11 +531,11 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 	}
 
 	// Save current setpoints for the next FlightTask
-	vehicle_local_position_setpoint_s last_setpoint = FlightTask::empty_setpoint;
+	trajectory_setpoint_s last_setpoint = FlightTask::empty_setpoint;
 	ekf_reset_counters_s last_reset_counters{};
 
 	if (isAnyTaskActive()) {
-		last_setpoint = _current_task.task->getPositionSetpoint();
+		last_setpoint = _current_task.task->getTrajectorySetpoint();
 		last_reset_counters = _current_task.task->getResetCounters();
 	}
 

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -47,6 +47,7 @@
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/takeoff_status.h>
+#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
@@ -92,7 +93,7 @@ class FlightModeManager : public ModuleBase<FlightModeManager>, public ModulePar
 	void send_vehicle_cmd_do(uint8_t nav_state);
 	void handleCommand();
 	void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
-	void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
+	void limitAltitude(trajectory_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
 
 	/**
 	 * Switch to a specific task (for normal usage)
@@ -152,7 +153,7 @@ class FlightModeManager : public ModuleBase<FlightModeManager>, public ModulePar
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
-	uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
+	uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
 	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -48,7 +48,7 @@ FlightTaskAuto::FlightTaskAuto() :
 
 }
 
-bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 
@@ -61,8 +61,8 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp
 	_velocity_setpoint = _velocity;
 	_position_setpoint = _position;
 
-	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
-	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
+	Vector3f vel_prev{last_setpoint.velocity};
+	Vector3f pos_prev{last_setpoint.position};
 	Vector3f accel_prev{last_setpoint.acceleration};
 
 	for (int i = 0; i < 3; i++) {

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -87,7 +87,7 @@ class FlightTaskAuto : public FlightTask
 	FlightTaskAuto();
 
 	virtual ~FlightTaskAuto() = default;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	void reActivate() override;
 	bool updateInitialize() override;
 	bool update() override;

src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp

@@ -36,7 +36,7 @@
 
 #include "FlightTaskDescend.hpp"
 
-bool FlightTaskDescend::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskDescend::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	// stay level to minimize horizontal drift

src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp

@@ -46,7 +46,7 @@ class FlightTaskDescend : public FlightTask
 	virtual ~FlightTaskDescend() = default;
 
 	bool update() override;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 
 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,

src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp

@@ -36,7 +36,7 @@
 
 #include "FlightTaskFailsafe.hpp"
 
-bool FlightTaskFailsafe::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskFailsafe::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	_position_setpoint = _position;

src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.hpp

@@ -47,7 +47,7 @@ class FlightTaskFailsafe : public FlightTask
 
 	virtual ~FlightTaskFailsafe() = default;
 	bool update() override;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 
 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -4,12 +4,12 @@
 
 constexpr uint64_t FlightTask::_timeout;
 // First index of empty_setpoint corresponds to time-stamp and requires a finite number.
-const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
+const trajectory_setpoint_s FlightTask::empty_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
 
 const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}};
 const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
 
-bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	_resetSetpoints();
 	_setDefaultConstraints();
@@ -21,8 +21,8 @@ bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint
 void FlightTask::reActivate()
 {
 	// Preserve vertical velocity while on the ground to allow descending by stick for reliable land detection
-	vehicle_local_position_setpoint_s setpoint_preserve_vertical{empty_setpoint};
-	setpoint_preserve_vertical.vz = _velocity_setpoint(2);
+	trajectory_setpoint_s setpoint_preserve_vertical{empty_setpoint};
+	setpoint_preserve_vertical.velocity[2] = _velocity_setpoint(2);
 	activate(setpoint_preserve_vertical);
 }
 
@@ -76,30 +76,20 @@ void FlightTask::_checkEkfResetCounters()
 	}
 }
 
-const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
+const trajectory_setpoint_s FlightTask::getTrajectorySetpoint()
 {
-	/* fill position setpoint message */
-	vehicle_local_position_setpoint_s vehicle_local_position_setpoint{};
-	vehicle_local_position_setpoint.timestamp = hrt_absolute_time();
+	trajectory_setpoint_s trajectory_setpoint{};
+	trajectory_setpoint.timestamp = hrt_absolute_time();
 
-	vehicle_local_position_setpoint.x = _position_setpoint(0);
-	vehicle_local_position_setpoint.y = _position_setpoint(1);
-	vehicle_local_position_setpoint.z = _position_setpoint(2);
+	_position_setpoint.copyTo(trajectory_setpoint.position);
+	_velocity_setpoint.copyTo(trajectory_setpoint.velocity);
+	_acceleration_setpoint.copyTo(trajectory_setpoint.acceleration);
+	_jerk_setpoint.copyTo(trajectory_setpoint.jerk);
 
-	vehicle_local_position_setpoint.vx = _velocity_setpoint(0);
-	vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
-	vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
+	trajectory_setpoint.yaw = _yaw_setpoint;
+	trajectory_setpoint.yawspeed = _yawspeed_setpoint;
 
-	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
-	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
-
-	_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
-	_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
-
-	// deprecated, only kept for output logging
-	matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
-
-	return vehicle_local_position_setpoint;
+	return trajectory_setpoint;
 }
 
 void FlightTask::_resetSetpoints()

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -46,6 +46,7 @@
 #include <matrix/matrix/math.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/landing_gear.h>
+#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
@@ -81,7 +82,7 @@ class FlightTask : public ModuleParams
 	 * @param last_setpoint last output of the previous task
 	 * @return true on success, false on error
 	 */
-	virtual bool activate(const vehicle_local_position_setpoint_s &last_setpoint);
+	virtual bool activate(const trajectory_setpoint_s &last_setpoint);
 
 	/**
 	 * Call this to reset an active Flight Task
@@ -112,7 +113,7 @@ class FlightTask : public ModuleParams
 	 * Get the output data
 	 * @return task output setpoints that get executed by the positon controller
 	 */
-	const vehicle_local_position_setpoint_s getPositionSetpoint();
+	const trajectory_setpoint_s getTrajectorySetpoint();
 
 	const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
 	void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
@@ -141,7 +142,7 @@ class FlightTask : public ModuleParams
 	 * Empty setpoint.
 	 * All setpoints are set to NAN.
 	 */
-	static const vehicle_local_position_setpoint_s empty_setpoint;
+	static const trajectory_setpoint_s empty_setpoint;
 
 	/**
 	 * Empty constraints.

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -43,21 +43,21 @@ FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
 	_stick_acceleration_xy(this)
 {};
 
-bool FlightTaskManualAcceleration::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
 
 	_stick_acceleration_xy.resetPosition();
 
-	if (PX4_ISFINITE(last_setpoint.vx) && PX4_ISFINITE(last_setpoint.vy)) {
-		_stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.vx, last_setpoint.vy));
+	if (PX4_ISFINITE(last_setpoint.velocity[0]) && PX4_ISFINITE(last_setpoint.velocity[1])) {
+		_stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.velocity));
 
 	} else {
 		_stick_acceleration_xy.resetVelocity(_velocity.xy());
 	}
 
 	if (PX4_ISFINITE(last_setpoint.acceleration[0]) && PX4_ISFINITE(last_setpoint.acceleration[1])) {
-		_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration[0], last_setpoint.acceleration[1]));
+		_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));
 	}
 
 	return ret;

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -49,7 +49,7 @@ class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
 public:
 	FlightTaskManualAcceleration();
 	virtual ~FlightTaskManualAcceleration() = default;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool update() override;
 
 	/**

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -60,7 +60,7 @@ bool FlightTaskManualAltitude::updateInitialize()
 	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
 }
 
-bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	_yaw_setpoint = NAN;

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -49,7 +49,7 @@ class FlightTaskManualAltitude : public FlightTask
 public:
 	FlightTaskManualAltitude();
 	virtual ~FlightTaskManualAltitude() = default;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;
 

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -41,17 +41,17 @@
 
 using namespace matrix;
 
-bool FlightTaskManualAltitudeSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskManualAltitudeSmoothVel::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
 
 	// Check if the previous FlightTask provided setpoints
 
 	// If the position setpoint is unknown, set to the current postion
-	float z_sp_last = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2);
+	float z_sp_last = PX4_ISFINITE(last_setpoint.position[2]) ? last_setpoint.position[2] : _position(2);
 
 	// If the velocity setpoint is unknown, set to the current velocity
-	float vz_sp_last = PX4_ISFINITE(last_setpoint.vz) ? last_setpoint.vz : _velocity(2);
+	float vz_sp_last = PX4_ISFINITE(last_setpoint.velocity[2]) ? last_setpoint.velocity[2] : _velocity(2);
 
 	// No acceleration estimate available, set to zero if the setpoint is NAN
 	float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : 0.f;

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -48,7 +48,7 @@ class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 	FlightTaskManualAltitudeSmoothVel() = default;
 	virtual ~FlightTaskManualAltitudeSmoothVel() = default;
 
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 
 protected:
 	virtual void _updateSetpoints() override;

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -56,7 +56,7 @@ bool FlightTaskManualPosition::updateInitialize()
 	       && PX4_ISFINITE(_velocity(1));
 }
 
-bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskManualPosition::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	// all requirements from altitude-mode still have to hold
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -49,7 +49,7 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude
 	FlightTaskManualPosition();
 
 	virtual ~FlightTaskManualPosition() = default;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 
 	/**