fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp
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;
}

7
src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp
View File

@ -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 = {};
};

4
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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);