TEMPORARY hack to test goto setpoint using Orbit commands

MaEtUgR · Sep 11, 2024 · ce90293 · ce90293
1 parent c94c1ce
commit ce90293
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 14 deletions.
diff --git 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
@@ -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,