From f11465bb26af1d4685a9ad4706ef5f1beff548f7 Mon Sep 17 00:00:00 2001
From: Alessandro Simovic <alessandro@yuneecresearch.com>
Date: Tue, 4 Dec 2018 15:15:23 +0100
Subject: [PATCH] orbit: publish uorb message
---
.../tasks/Orbit/FlightTaskOrbit.cpp | 19 +++++++++++++++++++
.../tasks/Orbit/FlightTaskOrbit.hpp | 7 ++++++-
2 files changed, 25 insertions(+), 1 deletion(-)
diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp
index 2cf0d6cad6..1257ac71f8 100644
--- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/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;
}
diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp
index 24f2ce208a..889a5b84a3 100644
--- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp
+++ b/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 = {};
};
--
GitLab