dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add support for mavlink message DEBUG_FLOAT_ARRAY

This commit is contained in:
Julien Lecoeur 2017-06-07 15:49:35 +02:00 committed by Daniel Agar
parent 7f665a70e7
commit cb3d86a609
6 changed files with 116 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -44,6 +44,7 @@ set(msg_files
collision_report.msg
commander_state.msg
cpuload.msg
debug_array.msg
debug_key_value.msg
debug_value.msg
debug_vect.msg

5
msg/debug_array.msg Normal file
View File

@ -0,0 +1,5 @@
uint8 ARRAY_SIZE = 58
uint64 timestamp # time since system start (microseconds)
uint16 id # unique ID of debug array, used to discriminate between arrays
char[10] name # name of the debug array (max. 10 characters)
float32[58] data # data

3
src/modules/mavlink/mavlink_main.cpp
View File

@ -1745,6 +1745,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DEBUG", 1.0f);
configure_stream_local("DEBUG_VECT", 1.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f);
configure_stream_local("DISTANCE_SENSOR", 0.5f);
configure_stream_local("ESTIMATOR_STATUS", 0.5f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
@ -1782,6 +1783,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DEBUG", 10.0f);
configure_stream_local("DEBUG_VECT", 10.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 10.0f);
configure_stream_local("DISTANCE_SENSOR", 10.0f);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
@ -1846,6 +1848,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DEBUG", 50.0f);
configure_stream_local("DEBUG_VECT", 50.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 50.0f);
configure_stream_local("DISTANCE_SENSOR", 10.0f);
configure_stream_local("GPS_RAW_INT", unlimited_rate);
configure_stream_local("GPS2_RAW", unlimited_rate);

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

@ -64,6 +64,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/debug_array.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/estimator_status.h>
@ -3707,6 +3708,79 @@ protected:
}
};
class MavlinkStreamDebugFloatArray : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamDebugFloatArray::get_name_static();
}
static const char *get_name_static()
{
return "DEBUG_FLOAT_ARRAY";
}
static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY;
}
uint16_t get_id()
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamDebugFloatArray(mavlink);
}
unsigned get_size()
{
return (_debug_time > 0) ? MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
MavlinkOrbSubscription *_debug_array_sub;
uint64_t _debug_time;
/* do not allow top copying this class */
MavlinkStreamDebugFloatArray(MavlinkStreamDebugFloatArray &);
MavlinkStreamDebugFloatArray &operator = (const MavlinkStreamDebugFloatArray &);
protected:
explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink),
_debug_array_sub(_mavlink->add_orb_subscription(ORB_ID(debug_array))),
_debug_time(0)
{}
bool send(const hrt_abstime t)
{
struct debug_array_s debug = {};
if (_debug_array_sub->update(&_debug_time, &debug)) {
mavlink_debug_float_array_t msg = {};
msg.time_usec = debug.timestamp;
msg.array_id = debug.id;
memcpy(msg.name, debug.name, sizeof(msg.name));
/* enforce null termination */
msg.name[sizeof(msg.name) - 1] = '\0';
for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) {
msg.data[i] = debug.data[i];
}
mavlink_msg_debug_float_array_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamNavControllerOutput : public MavlinkStream
{
public:
@ -4546,6 +4620,7 @@ static const StreamListItem streams_list[] = {
StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static, &MavlinkStreamNamedValueFloat::get_id_static),
StreamListItem(&MavlinkStreamDebug::new_instance, &MavlinkStreamDebug::get_name_static, &MavlinkStreamDebug::get_id_static),
StreamListItem(&MavlinkStreamDebugVect::new_instance, &MavlinkStreamDebugVect::get_name_static, &MavlinkStreamDebugVect::get_id_static),
StreamListItem(&MavlinkStreamDebugFloatArray::new_instance, &MavlinkStreamDebugFloatArray::get_name_static, &MavlinkStreamDebugFloatArray::get_id_static),
StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static, &MavlinkStreamNavControllerOutput::get_id_static),
StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static, &MavlinkStreamCameraCapture::get_id_static),
StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static, &MavlinkStreamCameraTrigger::get_id_static),

29
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -159,6 +159,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_debug_key_value_pub(nullptr),
_debug_value_pub(nullptr),
_debug_vect_pub(nullptr),
_debug_array_pub(nullptr),
_gps_inject_data_pub(nullptr),
_command_ack_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
@ -343,6 +344,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_debug_vect(msg);
break;
case MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY:
handle_message_debug_float_array(msg);
break;
default:
break;
}
@ -2562,6 +2567,30 @@ void MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg)
}
}
void MavlinkReceiver::handle_message_debug_float_array(mavlink_message_t *msg)
{
mavlink_debug_float_array_t debug_msg;
debug_array_s debug_topic = {};
mavlink_msg_debug_float_array_decode(msg, &debug_msg);
debug_topic.timestamp = hrt_absolute_time();
debug_topic.id = debug_msg.array_id;
memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name));
debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination
for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) {
debug_topic.data[i] = debug_msg.data[i];
}
if (_debug_array_pub == nullptr) {
_debug_array_pub = orb_advertise(ORB_ID(debug_array), &debug_topic);
} else {
orb_publish(ORB_ID(debug_array), _debug_array_pub, &debug_topic);
}
}
/**
* Receive data from UART/UDP
*/

3
src/modules/mavlink/mavlink_receiver.h
View File

@ -53,6 +53,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/debug_array.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/follow_target.h>
#include <uORB/topics/gps_inject_data.h>
@ -161,6 +162,7 @@ private:
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 handle_message_debug_float_array(mavlink_message_t *msg);
void *receive_thread(void *arg);
@ -242,6 +244,7 @@ private:
orb_advert_t _debug_key_value_pub;
orb_advert_t _debug_value_pub;
orb_advert_t _debug_vect_pub;
orb_advert_t _debug_array_pub;
static const int _gps_inject_data_queue_size = 6;
orb_advert_t _gps_inject_data_pub;
orb_advert_t _command_ack_pub;