Commit 356369eb authored by Lorenz Meier's avatar Lorenz Meier

Merge pull request #963 from DonLakeFlyer/RC_CHANNELS_RAW

Disregard RC_CHANNELS_RAW for signals
parents 4356914d 7a119bc4
......@@ -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
......
......@@ -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;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment