From eeb966d375ad0796edb84c8866d37ebdf0daf27c Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Wed, 7 Jun 2017 15:37:25 +0200 Subject: [PATCH] Add support for mavlink message DEBUG --- msg/CMakeLists.txt | 1 + msg/debug_value.msg | 3 ++ src/modules/mavlink/mavlink_main.cpp | 3 ++ src/modules/mavlink/mavlink_messages.cpp | 65 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 25 +++++++++ src/modules/mavlink/mavlink_receiver.h | 3 ++ 6 files changed, 100 insertions(+) create mode 100644 msg/debug_value.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ca35d80078..20864b9a73 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -47,6 +47,7 @@ set(msg_file_names control_state.msg cpuload.msg debug_key_value.msg + debug_value.msg differential_pressure.msg distance_sensor.msg ekf2_innovations.msg diff --git a/msg/debug_value.msg b/msg/debug_value.msg new file mode 100644 index 0000000000..06d5c34561 --- /dev/null +++ b/msg/debug_value.msg @@ -0,0 +1,3 @@ +uint32 timestamp_ms # in milliseconds since system start +int8 ind # index of debug variable +float32 value # the value to send as debug output diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bb28a62b17..268ebadf0c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2045,6 +2045,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ATTITUDE_TARGET", 2.0f); configure_stream("HOME_POSITION", 0.5f); configure_stream("NAMED_VALUE_FLOAT", 1.0f); + configure_stream("DEBUG", 1.0f); configure_stream("VFR_HUD", 4.0f); configure_stream("WIND_COV", 1.0f); configure_stream("CAMERA_IMAGE_CAPTURED"); @@ -2073,6 +2074,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("HOME_POSITION", 0.5f); configure_stream("NAMED_VALUE_FLOAT", 10.0f); + configure_stream("DEBUG", 10.0f); configure_stream("VFR_HUD", 10.0f); configure_stream("WIND_COV", 10.0f); configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); @@ -2132,6 +2134,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ATTITUDE_TARGET", 8.0f); configure_stream("HOME_POSITION", 0.5f); configure_stream("NAMED_VALUE_FLOAT", 50.0f); + configure_stream("DEBUG", 50.0f); configure_stream("VFR_HUD", 20.0f); configure_stream("WIND_COV", 10.0f); configure_stream("CAMERA_TRIGGER"); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7da967948d..279729e762 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -65,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -3171,6 +3172,69 @@ protected: } }; +class MavlinkStreamDebug : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamDebug::get_name_static(); + } + + static const char *get_name_static() + { + return "DEBUG"; + } + + static uint16_t get_id_static() + { + return MAVLINK_MSG_ID_DEBUG; + } + + uint16_t get_id() + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamDebug(mavlink); + } + + unsigned get_size() + { + return (_debug_time > 0) ? MAVLINK_MSG_ID_DEBUG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + MavlinkOrbSubscription *_debug_sub; + uint64_t _debug_time; + + /* do not allow top copying this class */ + MavlinkStreamDebug(MavlinkStreamDebug &); + MavlinkStreamDebug &operator = (const MavlinkStreamDebug &); + +protected: + explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink), + _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_value))), + _debug_time(0) + {} + + void send(const hrt_abstime t) + { + struct debug_value_s debug = {}; + + if (_debug_sub->update(&_debug_time, &debug)) { + mavlink_debug_t msg = {}; + + msg.time_boot_ms = debug.timestamp_ms; + msg.ind = debug.ind; + msg.value = debug.value; + + mavlink_msg_debug_send_struct(_mavlink->get_channel(), &msg); + } + } +}; + class MavlinkStreamNavControllerOutput : public MavlinkStream { public: @@ -4168,6 +4232,7 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static, &MavlinkStreamActuatorControlTarget<2>::get_id_static), 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(&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), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 90fcf79611..d5f176b8f1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -138,6 +138,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _collision_report_pub(nullptr), _control_state_pub(nullptr), _debug_key_value_pub(nullptr), + _debug_value_pub(nullptr), _gps_inject_data_pub(nullptr), _command_ack_pub(nullptr), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), @@ -312,6 +313,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_named_value_float(msg); break; + case MAVLINK_MSG_ID_DEBUG: + handle_message_debug(msg); + break; + default: break; } @@ -2384,6 +2389,26 @@ void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg) } } +void MavlinkReceiver::handle_message_debug(mavlink_message_t *msg) +{ + mavlink_debug_t debug_msg; + debug_value_s debug_topic = {}; + + mavlink_msg_debug_decode(msg, &debug_msg); + + debug_topic.timestamp = hrt_absolute_time(); + debug_topic.timestamp_ms = debug_msg.time_boot_ms; + debug_topic.ind = debug_msg.ind; + debug_topic.value = debug_msg.value; + + if (_debug_value_pub == nullptr) { + _debug_value_pub = orb_advertise(ORB_ID(debug_value), &debug_topic); + + } else { + orb_publish(ORB_ID(debug_value), _debug_value_pub, &debug_topic); + } +} + /** * Receive data from UART. */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 147426b80c..9b4deeec86 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -152,6 +153,7 @@ private: void handle_message_logging_ack(mavlink_message_t *msg); 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 *receive_thread(void *arg); @@ -238,6 +240,7 @@ private: orb_advert_t _collision_report_pub; orb_advert_t _control_state_pub; orb_advert_t _debug_key_value_pub; + orb_advert_t _debug_value_pub; static const int _gps_inject_data_queue_size = 6; orb_advert_t _gps_inject_data_pub; orb_advert_t _command_ack_pub;