From ce90293591b6d47c28599155666fa6e1eefb6d00 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Wed, 11 Sep 2024 10:15:37 +0200
Subject: [PATCH] TEMPORARY hack to test goto setpoint using Orbit commands

---
 .../tasks/Orbit/FlightTaskOrbit.cpp            | 18 ++++--------------
 .../tasks/Orbit/FlightTaskOrbit.hpp            |  2 ++
 2 files changed, 6 insertions(+), 14 deletions(-)

diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
index 144c6c7ba158..e6a1e09279a8 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
@@ -223,20 +223,10 @@ bool FlightTaskOrbit::update()
 		}
 	}
 
-	if (_in_circle_approach) {
-		_generate_circle_approach_setpoints();
-
-	} else {
-		// update altitude
-		ret = ret && FlightTaskManualAltitudeSmoothVel::update();
-
-		// this generates x, y and yaw setpoints
-		_generate_circle_setpoints();
-		_generate_circle_yaw_setpoints();
-	}
-
-	// Apply yaw smoothing
-	_yaw_setpoint = _slew_rate_yaw.update(_yaw_setpoint, _deltatime);
+	goto_setpoint_s goto_setpoint{};
+	_center.copyTo(goto_setpoint.position);
+	goto_setpoint.timestamp = hrt_absolute_time();
+	_goto_setpoint_pub.publish(goto_setpoint);
 
 	// publish information to UI
 	sendTelemetry();
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
index 2c55bbc15ba7..df3e54956013 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
@@ -44,6 +44,7 @@
 #include "FlightTaskManualAltitudeSmoothVel.hpp"
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/orbit_status.h>
+#include <uORB/topics/goto_setpoint.h>
 #include <lib/slew_rate/SlewRateYaw.hpp>
 #include <lib/motion_planning/PositionSmoothing.hpp>
 #include <lib/motion_planning/VelocitySmoothing.hpp>
@@ -130,6 +131,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
 
 	orb_advert_t _mavlink_log_pub{nullptr};
 	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
+	uORB::Publication<goto_setpoint_s> _goto_setpoint_pub{ORB_ID(goto_setpoint)};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MC_ORBIT_RAD_MAX>) _param_mc_orbit_rad_max,