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

Add support for mavlink message DEBUG_VECT

Browse Source
This commit is contained in:
Julien Lecoeur
2017-08-30 14:45:02 +01:00
committed by Nuno Marques
parent eeb966d375
commit 12353f4da7
6 changed files with 109 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+69
View File
@@ -66,6 +66,7 @@
#include <uORB/topics/cpuload.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/estimator_status.h>
@@ -3235,6 +3236,73 @@ protected:
}
};
class MavlinkStreamDebugVect : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamDebugVect::get_name_static();
}
static const char *get_name_static()
{
return "DEBUG_VECT";
}
static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_DEBUG_VECT;
}
uint16_t get_id()
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamDebugVect(mavlink);
}
unsigned get_size()
{
return (_debug_time > 0) ? MAVLINK_MSG_ID_DEBUG_VECT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
MavlinkOrbSubscription *_debug_sub;
uint64_t _debug_time;
/* do not allow top copying this class */
MavlinkStreamDebugVect(MavlinkStreamDebugVect &);
MavlinkStreamDebugVect &operator = (const MavlinkStreamDebugVect &);
protected:
explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink),
_debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_vect))),
_debug_time(0)
{}
void send(const hrt_abstime t)
{
struct debug_vect_s debug = {};
if (_debug_sub->update(&_debug_time, &debug)) {
mavlink_debug_vect_t msg = {};
msg.time_usec = debug.timestamp_us;
memcpy(msg.name, debug.name, sizeof(msg.name));
/* enforce null termination */
msg.name[sizeof(msg.name) - 1] = '\0';
msg.x = debug.x;
msg.y = debug.y;
msg.z = debug.z;
mavlink_msg_debug_vect_send_struct(_mavlink->get_channel(), &msg);
}
}
};
class MavlinkStreamNavControllerOutput : public MavlinkStream
{
public:
@@ -4233,6 +4301,7 @@ const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static, &MavlinkStreamActuatorControlTarget<3>::get_id_static),
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static, &MavlinkStreamNamedValueFloat::get_id_static),
new StreamListItem(&MavlinkStreamDebug::new_instance, &MavlinkStreamDebug::get_name_static, &MavlinkStreamDebug::get_id_static),
new StreamListItem(&MavlinkStreamDebugVect::new_instance, &MavlinkStreamDebugVect::get_name_static, &MavlinkStreamDebugVect::get_id_static),
new StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static, &MavlinkStreamNavControllerOutput::get_id_static),
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static, &MavlinkStreamCameraCapture::get_id_static),
new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static, &MavlinkStreamCameraTrigger::get_id_static),