fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 14:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

orbit: fix telemetry message content

Browse Source
This commit is contained in:
Matthias Grob
2018-12-05 17:21:19 +01:00
parent 14b83f7bfc
commit 25aa2b9c8c
3 changed files with 31 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp
+24 -12
View File
@@ -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;
}