|
|
|
@ -956,32 +956,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
@@ -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; |
|
|
|
|