From ce9f46d4e75678729e014b3f94f01875acac2322 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 31 Mar 2015 10:30:57 -0600 Subject: [PATCH 1/2] Added ping message handling to mavlink receiver --- src/modules/mavlink/mavlink_receiver.cpp | 15 +++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 16 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e3f274b8bf..1f91654dab 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -157,6 +157,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_optical_flow_rad(msg); break; + case MAVLINK_MSG_ID_PING: + handle_message_ping(msg); + break; + case MAVLINK_MSG_ID_SET_MODE: handle_message_set_mode(msg); break; @@ -946,6 +950,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) +{ + mavlink_ping_t ping; + mavlink_msg_ping_decode( msg, &ping); + if (mavlink_system.sysid == ping.target_system && + mavlink_system.compid == ping.target_component) { + _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg); + } +} + void MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 1d214b948f..ffacb59a63 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -127,6 +127,7 @@ private: void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); + void handle_message_ping(mavlink_message_t *msg); void handle_message_request_data_stream(mavlink_message_t *msg); void handle_message_system_time(mavlink_message_t *msg); void handle_message_timesync(mavlink_message_t *msg); From fa9f7075fa42197e2daed6513647d07761dde090 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 31 Mar 2015 10:52:26 -0600 Subject: [PATCH 2/2] Changed message staging --- src/modules/mavlink/mavlink_receiver.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1f91654dab..3f9f7e1392 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -955,9 +955,11 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) { mavlink_ping_t ping; mavlink_msg_ping_decode( msg, &ping); - if (mavlink_system.sysid == ping.target_system && - mavlink_system.compid == ping.target_component) { - _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg); + if ((mavlink_system.sysid == ping.target_system) && + (mavlink_system.compid == ping.target_component)) { + mavlink_message_t msg_out; + mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping); + _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out); } }