Commit 9ad93cf0 authored by Gus Grubba's avatar Gus Grubba
Browse files

RC noise and remote noise are defined as “unsigned”. Handle them as such.

parent 83b22d62
...@@ -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 // process telemetry status message
mavlink_radio_status_t rstatus; mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(&message, &rstatus); mavlink_msg_radio_status_decode(&message, &rstatus);
int rssi = rstatus.rssi, int rssi = rstatus.rssi;
remrssi = rstatus.remrssi; int remrssi = rstatus.remrssi;
int noise = rstatus.noise;
int remnoise = rstatus.remnoise;
// 3DR Si1k radio needs rssi fields to be converted to dBm // 3DR Si1k radio needs rssi fields to be converted to dBm
if (message.sysid == '3' && message.compid == 'D') { if (message.sysid == '3' && message.compid == 'D') {
/* Per the Si1K datasheet figure 23.25 and SI AN474 code /* Per the Si1K datasheet figure 23.25 and SI AN474 code
...@@ -230,12 +232,13 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -230,12 +232,13 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
rssi = qMin(qMax(qRound(static_cast<qreal>(rssi) / 1.9 - 127.0), - 120), 0); rssi = qMin(qMax(qRound(static_cast<qreal>(rssi) / 1.9 - 127.0), - 120), 0);
remrssi = qMin(qMax(qRound(static_cast<qreal>(remrssi) / 1.9 - 127.0), - 120), 0); remrssi = qMin(qMax(qRound(static_cast<qreal>(remrssi) / 1.9 - 127.0), - 120), 0);
} else { } else {
rssi = (int8_t) rstatus.rssi; rssi = (int8_t) rstatus.rssi;
remrssi = (int8_t) rstatus.remrssi; remrssi = (int8_t) rstatus.remrssi;
noise = (int8_t) rstatus.noise;
remnoise = (int8_t) rstatus.remnoise;
} }
emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rssi, remrssi, emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rssi, remrssi,
rstatus.txbuf, rstatus.noise, rstatus.remnoise); rstatus.txbuf, noise, remnoise);
} }
#ifndef __mobile__ #ifndef __mobile__
......
Supports Markdown
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