From e43a0bbf1de98cef7b70981c36753cd43b72031e Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 5 Mar 2019 10:29:55 -0700 Subject: [PATCH] Cut case MAVLINK_MSG_ID_RC_CHANNELS content and paste into handle_message_rc_channels() method. --- src/modules/simulator/simulator.h | 1 + src/modules/simulator/simulator_mavlink.cpp | 24 ++++++++++++--------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 5aa663542c..a74cce6e58 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -337,6 +337,7 @@ private: void handle_message_hil_state_quaternion(const mavlink_message_t *msg); void handle_message_landing_target(const mavlink_message_t *msg); void handle_message_optical_flow(const mavlink_message_t *msg); + void handle_message_rc_channels(const mavlink_message_t *msg); void parameters_update(bool force); void poll_topics(); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index f13661f48e..5d527e7a8b 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -314,16 +314,7 @@ void Simulator::handle_message(mavlink_message_t *msg) break; case MAVLINK_MSG_ID_RC_CHANNELS: - mavlink_rc_channels_t rc_channels; - mavlink_msg_rc_channels_decode(msg, &rc_channels); - fill_rc_input_msg(&_rc_input, &rc_channels); - - // publish message - if (_publish) { - int rc_multi; - orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH); - } - + handle_message_rc_channels(msg); break; case MAVLINK_MSG_ID_LANDING_TARGET: { @@ -539,6 +530,19 @@ void Simulator::handle_message_optical_flow(const mavlink_message_t *msg) publish_flow_topic(&flow); } +void Simulator::handle_message_rc_channels(const mavlink_message_t *msg) +{ + mavlink_rc_channels_t rc_channels; + mavlink_msg_rc_channels_decode(msg, &rc_channels); + fill_rc_input_msg(&_rc_input, &rc_channels); + + // publish message + if (_publish) { + int rc_multi; + orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH); + } +} + void Simulator::send_mavlink_message(const mavlink_message_t &aMsg) { uint8_t buf[MAVLINK_MAX_PACKET_LEN];