From e47aba8bc977933c56cccf89f574d3a8a9fbde90 Mon Sep 17 00:00:00 2001
From: Thomas Stastny <thomas.stastny@auterion.com>
Date: Wed, 15 Nov 2023 11:24:10 +0100
Subject: [PATCH] msg: add go-to setpoint interface

---
 msg/CMakeLists.txt                           |  1 +
 msg/GotoSetpoint.msg                         | 25 ++++++++++++++++++++
 src/modules/logger/logged_topics.cpp         |  1 +
 src/modules/uxrce_dds_client/dds_topics.yaml |  3 +++
 4 files changed, 30 insertions(+)
 create mode 100644 msg/GotoSetpoint.msg

diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index d214bcfe82..286607edb0 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -104,6 +104,7 @@ set(msg_files
 	GimbalManagerSetAttitude.msg
 	GimbalManagerSetManualControl.msg
 	GimbalManagerStatus.msg
+	GotoSetpoint.msg
 	GpioConfig.msg
 	GpioIn.msg
 	GpioOut.msg
diff --git a/msg/GotoSetpoint.msg b/msg/GotoSetpoint.msg
new file mode 100644
index 0000000000..e67082a072
--- /dev/null
+++ b/msg/GotoSetpoint.msg
@@ -0,0 +1,25 @@
+# Position and (optional) heading setpoints with corresponding speed constraints
+# Setpoints are intended as inputs to position and heading smoothers, respectively
+# Setpoints do not need to be kinematically consistent
+# Optional heading setpoints may be specified as controlled by the respective flag
+# Unset optional setpoints are not controlled
+# Unset optional constraints default to vehicle specifications
+
+uint64 timestamp # time since system start (microseconds)
+
+# setpoints
+float32[3] position # [m] NED local world frame
+float32 heading # (optional) [rad] [-pi,pi] from North
+
+# setpoint flags
+bool flag_control_heading # true if heading is to be controlled
+
+# constraints
+float32 max_horizontal_speed # (optional) [m/s] maximum speed (absolute) in the NE-plane
+float32 max_vertical_speed # (optional) [m/s] maximum speed (absolute) in the D-axis
+float32 max_heading_rate # (optional) [rad/s] maximum heading rate (absolute)
+
+# constraint flags
+bool flag_set_max_horizontal_speed # true if setting a non-default horizontal speed limit
+bool flag_set_max_vertical_speed # true if setting a non-default vertical speed limit
+bool flag_set_max_heading_rate # true if setting a non-default heading rate limit
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index 7ae4ba2578..52f52aa9b8 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -94,6 +94,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("parameter_update");
 	add_topic("position_controller_status", 500);
 	add_topic("position_controller_landing_status", 100);
+	add_topic("position_heading_setpoint", 200);
 	add_topic("position_setpoint_triplet", 200);
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml
index 06a8b69853..e630abc3ef 100644
--- a/src/modules/uxrce_dds_client/dds_topics.yaml
+++ b/src/modules/uxrce_dds_client/dds_topics.yaml
@@ -106,6 +106,9 @@ subscriptions:
   - topic: /fmu/in/sensor_optical_flow
     type: px4_msgs::msg::SensorOpticalFlow
 
+  - topic: /fmu/in/goto_setpoint
+    type: px4_msgs::msg::GotoSetpoint
+
   - topic: /fmu/in/telemetry_status
     type: px4_msgs::msg::TelemetryStatus