diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 6ff6d5893d6e2b9f59c0b3a34e27128b37922c62..5780155febd8757b3092846ca15fdee0799be32b 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -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
diff --git a/msg/debug_array.msg b/msg/debug_array.msg
new file mode 100644
index 0000000000000000000000000000000000000000..4763a0f5a0eee51c66e8918795403efcbdaff302
--- /dev/null
+++ b/msg/debug_array.msg
@@ -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
\ No newline at end of file
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c7b4f2b2af721f74ed9ff122aa67187b200e32d1..9ffe637744eeb1f81b3d4a027fa66fda1f9e181f 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -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);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 641c5e9b1600ccc81dad2706e6bab82fdea49d4b..7817e884c80bd48486c81871dd4f66181daeb4c6 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -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),
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 760d9bf5961f03fbea5c69bdcd38941e3001374b..486971c7bed7da0609b4180d14bc6925134b9796 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -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
*/
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 50569c4f69f4324027548a9d9d1c7a98ace46a14..b720f4917588f0822b4d9a2a5ca3e3c5dd5dc123 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -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;