From cb3d86a609be9dec01928aef965ad59e59c91b73 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Wed, 7 Jun 2017 15:49:35 +0200 Subject: [PATCH] Add support for mavlink message DEBUG_FLOAT_ARRAY --- msg/CMakeLists.txt | 1 + msg/debug_array.msg | 5 ++ src/modules/mavlink/mavlink_main.cpp | 3 + src/modules/mavlink/mavlink_messages.cpp | 75 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 29 +++++++++ src/modules/mavlink/mavlink_receiver.h | 3 + 6 files changed, 116 insertions(+) create mode 100644 msg/debug_array.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 6ff6d5893d..5780155feb 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 0000000000..4763a0f5a0 --- /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 c7b4f2b2af..9ffe637744 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 641c5e9b16..7817e884c8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -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 760d9bf596..486971c7be 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 50569c4f69..b720f49175 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -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;