orbit: fix telemetry message content

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

@@ -38,6 +38,7 @@
 #include "FlightTaskOrbit.hpp"
 #include <mathlib/mathlib.h>
 #include <lib/ecl/geo/geo.h>
+#include <uORB/topics/orbit_status.h>
 
 using namespace matrix;
 
@@ -91,6 +92,28 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 	return ret;
 }
 
+bool FlightTaskOrbit::sendTelemetry()
+{
+	orbit_status_s _orbit_status = {};
+	_orbit_status.timestamp = hrt_absolute_time();
+	_orbit_status.radius = _r;
+	_orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
+
+	if (globallocalconverter_toglobal(_center(0), _center(1), _position_setpoint(2),  &_orbit_status.x, &_orbit_status.y,
+					  &_orbit_status.z)) {
+		return false; // don't send the message if the transformation failed
+	}
+
+	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;
+}
+
 bool FlightTaskOrbit::setRadius(const float r)
 {
 	if (math::isInRange(r, _radius_min, _radius_max)) {
@@ -171,18 +194,7 @@ bool FlightTaskOrbit::update()
 	_yawspeed_setpoint = _v / _r;
 
 	// publish telemetry
-	_orbit_status.radius = _r;
+	sendTelemetry();
-	_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

@@ -43,7 +43,6 @@
 
 #include "FlightTaskManualAltitudeSmooth.hpp"
 #include <uORB/uORB.h>
-#include <uORB/topics/orbit_status.h>
 
 class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
 {
@@ -66,6 +65,11 @@ public:
 	bool checkAcceleration(float r, float v, float a);
 
 protected:
+	/**
+	 * Send out telemetry information for the log and MAVLink.
+	 * @return true on success, false on error
+	 */
+	bool sendTelemetry();
 
 	/**
 	 * Change the radius of the circle.
@@ -93,5 +97,4 @@ private:
 	const float _acceleration_max = 2.f;
 
 	orb_advert_t _orbit_status_pub = nullptr;
-	orbit_status_s _orbit_status = {};
 };

src/modules/mavlink/mavlink_messages.cpp

@@ -4826,8 +4826,8 @@ protected:
 			_msg_orbit_execution_status.time_usec = _orbit_status.timestamp;
 			_msg_orbit_execution_status.radius = _orbit_status.radius;
 			_msg_orbit_execution_status.frame  = _orbit_status.frame;
-			_msg_orbit_execution_status.x = _orbit_status.x;
+			_msg_orbit_execution_status.x = _orbit_status.x * 1e7;
-			_msg_orbit_execution_status.y = _orbit_status.y;
+			_msg_orbit_execution_status.y = _orbit_status.y * 1e7;
 			_msg_orbit_execution_status.z = _orbit_status.z;
 
 			mavlink_msg_orbit_execution_status_send_struct(_mavlink->get_channel(), &_msg_orbit_execution_status);