diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 20864b9a73..27887aee3f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -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 diff --git a/msg/debug_vect.msg b/msg/debug_vect.msg new file mode 100644 index 0000000000..c9454216ed --- /dev/null +++ b/msg/debug_vect.msg @@ -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 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 268ebadf0c..aa441951a9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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"); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 279729e762..dfd1eabb6b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -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), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d5f176b8f1..87da425005 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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. */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9b4deeec86..bb86564bc8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -68,6 +68,7 @@ #include #include #include +#include #include #include #include @@ -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;