Add support for mavlink message DEBUG_VECT

This commit is contained in:
Julien Lecoeur 2017-06-07 15:45:55 +02:00 committed by Nuno Marques
parent eeb966d375
commit 12353f4da7
6 changed files with 109 additions and 0 deletions

View File

@ -48,6 +48,7 @@ set(msg_file_names
cpuload.msg
debug_key_value.msg
debug_value.msg
debug_vect.msg
differential_pressure.msg
distance_sensor.msg
ekf2_innovations.msg

5
msg/debug_vect.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp_us # in microseconds since system start
int8[10] name # max. 10 characters as key / name
float32 x # x value
float32 y # y value
float32 z # z value

View File

@ -2046,6 +2046,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("HOME_POSITION", 0.5f);
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
configure_stream("DEBUG", 1.0f);
configure_stream("DEBUG_VECT", 1.0f);
configure_stream("VFR_HUD", 4.0f);
configure_stream("WIND_COV", 1.0f);
configure_stream("CAMERA_IMAGE_CAPTURED");
@ -2075,6 +2076,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("HOME_POSITION", 0.5f);
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
configure_stream("DEBUG", 10.0f);
configure_stream("DEBUG_VECT", 10.0f);
configure_stream("VFR_HUD", 10.0f);
configure_stream("WIND_COV", 10.0f);
configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
@ -2135,6 +2137,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("HOME_POSITION", 0.5f);
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
configure_stream("DEBUG", 50.0f);
configure_stream("DEBUG_VECT", 50.0f);
configure_stream("VFR_HUD", 20.0f);
configure_stream("WIND_COV", 10.0f);
configure_stream("CAMERA_TRIGGER");

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),

View File

@ -139,6 +139,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_control_state_pub(nullptr),
_debug_key_value_pub(nullptr),
_debug_value_pub(nullptr),
_debug_vect_pub(nullptr),
_gps_inject_data_pub(nullptr),
_command_ack_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
@ -317,6 +318,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_debug(msg);
break;
case MAVLINK_MSG_ID_DEBUG_VECT:
handle_message_debug_vect(msg);
break;
default:
break;
}
@ -2409,6 +2414,29 @@ void MavlinkReceiver::handle_message_debug(mavlink_message_t *msg)
}
}
void MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg)
{
mavlink_debug_vect_t debug_msg;
debug_vect_s debug_topic = {};
mavlink_msg_debug_vect_decode(msg, &debug_msg);
debug_topic.timestamp = hrt_absolute_time();
debug_topic.timestamp_us = debug_msg.time_usec;
memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name));
debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination
debug_topic.x = debug_msg.x;
debug_topic.y = debug_msg.y;
debug_topic.z = debug_msg.z;
if (_debug_vect_pub == nullptr) {
_debug_vect_pub = orb_advertise(ORB_ID(debug_vect), &debug_topic);
} else {
orb_publish(ORB_ID(debug_vect), _debug_vect_pub, &debug_topic);
}
}
/**
* Receive data from UART.
*/

View File

@ -68,6 +68,7 @@
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_force_setpoint.h>
@ -154,6 +155,7 @@ private:
void handle_message_play_tune(mavlink_message_t *msg);
void handle_message_named_value_float(mavlink_message_t *msg);
void handle_message_debug(mavlink_message_t *msg);
void handle_message_debug_vect(mavlink_message_t *msg);
void *receive_thread(void *arg);
@ -241,6 +243,7 @@ private:
orb_advert_t _control_state_pub;
orb_advert_t _debug_key_value_pub;
orb_advert_t _debug_value_pub;
orb_advert_t _debug_vect_pub;
static const int _gps_inject_data_queue_size = 6;
orb_advert_t _gps_inject_data_pub;
orb_advert_t _command_ack_pub;