From 7a119bc4e7af58d27c1b7c3b0ae8126cf02d1607 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 5 Nov 2014 14:20:11 -0800 Subject: [PATCH] Disregard RC_CHANNELS_RAW for signals This removes double emit of signals. --- src/comm/MAVLinkSimulationLink.cc | 5 ++--- src/uas/UAS.cc | 26 -------------------------- 2 files changed, 2 insertions(+), 29 deletions(-) diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index ce9eb79b0..1bced51fc 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -438,9 +438,8 @@ void MAVLinkSimulationLink::mainloop() static int rcCounter = 0; if (rcCounter == 2) { - mavlink_rc_channels_raw_t chan; + mavlink_rc_channels_t chan; chan.time_boot_ms = 0; - chan.port = 0; chan.chan1_raw = 1000 + ((int)(fabs(x) * 1000) % 2000); chan.chan2_raw = 1000 + ((int)(fabs(y) * 1000) % 2000); chan.chan3_raw = 1000 + ((int)(fabs(z) * 1000) % 2000); @@ -450,7 +449,7 @@ void MAVLinkSimulationLink::mainloop() chan.chan7_raw = (chan.chan4_raw + chan.chan2_raw) / 2.0f; chan.chan8_raw = 0; chan.rssi = 100; - mavlink_msg_rc_channels_raw_encode(systemId, componentId, &msg, &chan); + mavlink_msg_rc_channels_encode(systemId, componentId, &msg, &chan); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index dd3bb227f..422edb5ac 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -956,32 +956,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit homePositionChanged(uasId, pos.latitude / 10000000.0, pos.longitude / 10000000.0, pos.altitude / 1000.0); } break; - case MAVLINK_MSG_ID_RC_CHANNELS_RAW: - { - mavlink_rc_channels_raw_t channels; - mavlink_msg_rc_channels_raw_decode(&message, &channels); - - const unsigned int portWidth = 8; // XXX magic number - - emit remoteControlRSSIChanged(channels.rssi/255.0f); - if (channels.chan1_raw != UINT16_MAX) - emit remoteControlChannelRawChanged(channels.port * portWidth + 0, channels.chan1_raw); - if (channels.chan2_raw != UINT16_MAX) - emit remoteControlChannelRawChanged(channels.port * portWidth + 1, channels.chan2_raw); - if (channels.chan3_raw != UINT16_MAX) - emit remoteControlChannelRawChanged(channels.port * portWidth + 2, channels.chan3_raw); - if (channels.chan4_raw != UINT16_MAX) - emit remoteControlChannelRawChanged(channels.port * portWidth + 3, channels.chan4_raw); - if (channels.chan5_raw != UINT16_MAX) - emit remoteControlChannelRawChanged(channels.port * portWidth + 4, channels.chan5_raw); - if (channels.chan6_raw != UINT16_MAX) - emit remoteControlChannelRawChanged(channels.port * portWidth + 5, channels.chan6_raw); - if (channels.chan7_raw != UINT16_MAX) - emit remoteControlChannelRawChanged(channels.port * portWidth + 6, channels.chan7_raw); - if (channels.chan8_raw != UINT16_MAX) - emit remoteControlChannelRawChanged(channels.port * portWidth + 7, channels.chan8_raw); - } - break; case MAVLINK_MSG_ID_RC_CHANNELS: { mavlink_rc_channels_t channels; -- 2.22.0