orbit: publish uorb message

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp

@@ -46,6 +46,11 @@ FlightTaskOrbit::FlightTaskOrbit()
 	_sticks_data_required = false;
 }
 
+FlightTaskOrbit::~FlightTaskOrbit()
+{
+	orb_unadvertise(_orbit_status_pub);
+}
+
 bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 {
 	bool ret = true;
@@ -165,5 +170,19 @@ bool FlightTaskOrbit::update()
 	// yawspeed feed-forward because we know the necessary angular rate
 	_yawspeed_setpoint = _v / _r;
 
+	// publish telemetry
+	_orbit_status.radius = _r;
+	_orbit_status.frame = 6;  // MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
+	_orbit_status.x = _center(0);
+	_orbit_status.y = _center(1);
+	_orbit_status.z = _center(2);
+
+	if (_orbit_status_pub == nullptr) {
+		_orbit_status_pub = orb_advertise(ORB_ID(orbit_status), &_orbit_status);
+
+	} else {
+		orb_publish(ORB_ID(orbit_status), _orbit_status_pub, &_orbit_status);
+	}
+
 	return true;
 }

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp

@@ -42,12 +42,14 @@
 #pragma once
 
 #include "FlightTaskManualAltitudeSmooth.hpp"
+#include <uORB/uORB.h>
+#include <uORB/topics/orbit_status.h>
 
 class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
 {
 public:
 	FlightTaskOrbit();
-	virtual ~FlightTaskOrbit() = default;
+	virtual ~FlightTaskOrbit();
 
 	bool applyCommandParameters(const vehicle_command_s &command) override;
 	bool activate() override;
@@ -89,4 +91,7 @@ private:
 	const float _radius_max = 100.f;
 	const float _velocity_max = 10.f;
 	const float _acceleration_max = 2.f;
+
+	orb_advert_t _orbit_status_pub = nullptr;
+	orbit_status_s _orbit_status = {};
 };