|
|
|
@ -213,8 +213,10 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
@@ -213,8 +213,10 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
|
|
|
|
|
// process telemetry status message
|
|
|
|
|
mavlink_radio_status_t rstatus; |
|
|
|
|
mavlink_msg_radio_status_decode(&message, &rstatus); |
|
|
|
|
int rssi = rstatus.rssi, |
|
|
|
|
remrssi = rstatus.remrssi; |
|
|
|
|
int rssi = rstatus.rssi; |
|
|
|
|
int remrssi = rstatus.remrssi; |
|
|
|
|
int noise = rstatus.noise; |
|
|
|
|
int remnoise = rstatus.remnoise; |
|
|
|
|
// 3DR Si1k radio needs rssi fields to be converted to dBm
|
|
|
|
|
if (message.sysid == '3' && message.compid == 'D') { |
|
|
|
|
/* Per the Si1K datasheet figure 23.25 and SI AN474 code
|
|
|
|
@ -232,10 +234,11 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
@@ -232,10 +234,11 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
|
|
|
|
|
} else { |
|
|
|
|
rssi = (int8_t) rstatus.rssi; |
|
|
|
|
remrssi = (int8_t) rstatus.remrssi; |
|
|
|
|
noise = (int8_t) rstatus.noise; |
|
|
|
|
remnoise = (int8_t) rstatus.remnoise; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rssi, remrssi, |
|
|
|
|
rstatus.txbuf, rstatus.noise, rstatus.remnoise); |
|
|
|
|
rstatus.txbuf, noise, remnoise); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#ifndef __mobile__ |
|
|
|
|