From 25aa2b9c8ca229bf880387476fd3a40772bd5bcb Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Wed, 5 Dec 2018 17:21:19 +0100
Subject: [PATCH] orbit: fix telemetry message content
---
.../tasks/Orbit/FlightTaskOrbit.cpp | 36 ++++++++++++-------
.../tasks/Orbit/FlightTaskOrbit.hpp | 7 ++--
src/modules/mavlink/mavlink_messages.cpp | 4 +--
3 files changed, 31 insertions(+), 16 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp
index 1257ac71f8..7dd8c84986 100644
--- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/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;
- _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);
- }
+ sendTelemetry();
return true;
}
diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp
index 889a5b84a3..f0e15d14c2 100644
--- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp
+++ b/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 = {};
};
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7e51c7986b..57ddc0ba28 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/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.y = _orbit_status.y;
+ _msg_orbit_execution_status.x = _orbit_status.x * 1e7;
+ _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);
--
GitLab