Separate message for trajectory setpoint

2026-10-23 04:13:33 +08:00 · 2022-05-12 17:19:48 +02:00
parent 9166b6953d
commit 8ca28f3796
41 changed files with 126 additions and 137 deletions
@@ -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();
 	}

@@ -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 @@ private:
 	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 @@ private:
 	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)};
@@ -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++) {
@@ -87,7 +87,7 @@ public:
 	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;
@@ -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
@@ -46,7 +46,7 @@ public:
 	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,
@@ -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;
@@ -47,7 +47,7 @@ public:

 	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,
@@ -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()
@@ -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 @@ public:
 	 * @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 @@ public:
 	 * 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 @@ public:
 	 * 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.
@@ -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;
@@ -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;

 	/**
@@ -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;
@@ -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;

@@ -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;
@@ -48,7 +48,7 @@ public:
 	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;
@@ -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);
@@ -49,7 +49,7 @@ public:
 	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;

 	/**
@@ -38,14 +38,14 @@

 using namespace matrix;

-bool FlightTaskManualPositionSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskManualPositionSmoothVel::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualPosition::activate(last_setpoint);

 	// Check if the previous FlightTask provided setpoints
 	Vector3f accel_prev{last_setpoint.acceleration};
-	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};

 	for (int i = 0; i < 3; i++) {
 		// If the position setpoint is unknown, set to the current postion
@@ -53,7 +53,7 @@ public:

 	virtual ~FlightTaskManualPositionSmoothVel() = default;

-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	void reActivate() override;

 protected:
@@ -158,7 +158,7 @@ void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const
 	}
 }

-bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
 	_orbit_radius = _radius_min;
@@ -176,8 +176,8 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set
 	      && PX4_ISFINITE(_velocity(1))
 	      && PX4_ISFINITE(_velocity(2));

-	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 pos_prev{last_setpoint.position};
+	Vector3f vel_prev{last_setpoint.velocity};
 	Vector3f accel_prev{last_setpoint.acceleration};

 	for (int i = 0; i < 3; i++) {
@@ -57,7 +57,7 @@ public:
 	virtual ~FlightTaskOrbit() = default;

 	bool applyCommandParameters(const vehicle_command_s &command) override;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool update() override;

 protected:
@@ -70,14 +70,14 @@ void FlightTaskTransition::updateParameters()
 	}
 }

-bool FlightTaskTransition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);

 	_vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const);

-	if (PX4_ISFINITE(last_setpoint.vz)) {
-		_vel_z_filter.reset(last_setpoint.vz);
+	if (PX4_ISFINITE(last_setpoint.velocity[2])) {
+		_vel_z_filter.reset(last_setpoint.velocity[2]);

 	} else {
 		_vel_z_filter.reset(_velocity(2));
@@ -54,7 +54,7 @@ public:
 	FlightTaskTransition();

 	virtual ~FlightTaskTransition() = 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;