diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 7d3f1211921c43801e27f2436e6cd63829a303d7..115021b1e70021e0f8f8e59844aa1a74d6bb8c80 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -291,8 +291,25 @@ 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; + // 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 + * samples the relationship between the RSSI register + * and received power is as follows: + * + * 10 + * inputPower = rssi * ------ 127 + * 19 + * + * Additionally limit to the only realistic range [-120,0] dBm + */ + rssi = qMin(qMax(qRound(static_cast(rssi) / 1.9 - 127.0), - 120), 0); + remrssi = qMin(qMax(qRound(static_cast(remrssi) / 1.9 - 127.0), - 120), 0); + } - emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rstatus.rssi, rstatus.remrssi, + emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rssi, remrssi, rstatus.txbuf, rstatus.noise, rstatus.remnoise); } diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index b8b4b7564b16b26f788eeaefbf49089b3db4c98b..b24491ee6f5c47063fe8c43d608edf4ea4d72d89 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -269,13 +269,13 @@ signals: * * @param rxerrors receive errors * @param fixed count of error corrected packets - * @param rssi local signal strength - * @param remrssi remote signal strength + * @param rssi local signal strength in dBm + * @param remrssi remote signal strength in dBm * @param txbuf how full the tx buffer is as a percentage * @param noise background noise level * @param remnoise remote background noise level */ - void radioStatusChanged(LinkInterface* link, unsigned rxerrors, unsigned fixed, unsigned rssi, unsigned remrssi, + void radioStatusChanged(LinkInterface* link, unsigned rxerrors, unsigned fixed, int rssi, int remrssi, unsigned txbuf, unsigned noise, unsigned remnoise); /// @brief Emitted when a temporary log file is ready for saving diff --git a/src/ui/toolbar/MainToolBarController.cc b/src/ui/toolbar/MainToolBarController.cc index 2f84d1feb21237321ac973e02df74f1bd736eec0..e19dfe9e3ca55f44779a403e7b6a0e7351314603 100644 --- a/src/ui/toolbar/MainToolBarController.cc +++ b/src/ui/toolbar/MainToolBarController.cc @@ -52,8 +52,8 @@ MainToolBarController::MainToolBarController(QObject* parent) // RSSI (didn't like standard connection) connect(qgcApp()->toolbox()->mavlinkProtocol(), - SIGNAL(radioStatusChanged(LinkInterface*, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned)), this, - SLOT(_telemetryChanged(LinkInterface*, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned, unsigned))); + SIGNAL(radioStatusChanged(LinkInterface*, unsigned, unsigned, int, int, unsigned, unsigned, unsigned)), this, + SLOT(_telemetryChanged(LinkInterface*, unsigned, unsigned, int, int, unsigned, unsigned, unsigned))); connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged, this, &MainToolBarController::_activeVehicleChanged); } @@ -96,16 +96,14 @@ void MainToolBarController::_activeVehicleChanged(Vehicle* vehicle) } } -void MainToolBarController::_telemetryChanged(LinkInterface*, unsigned, unsigned, unsigned rssi, unsigned remrssi, unsigned, unsigned, unsigned) +void MainToolBarController::_telemetryChanged(LinkInterface*, unsigned, unsigned, int rssi, int remrssi, unsigned, unsigned, unsigned) { - if((unsigned)_telemetryLRSSI != rssi) { - // According to the Silabs data sheet, the RSSI value is 0.5db per bit - _telemetryLRSSI = rssi >> 1; + if(_telemetryLRSSI != rssi) { + _telemetryLRSSI = rssi; emit telemetryLRSSIChanged(_telemetryLRSSI); } - if((unsigned)_telemetryRRSSI != remrssi) { - // According to the Silabs data sheet, the RSSI value is 0.5db per bit - _telemetryRRSSI = remrssi >> 1; + if(_telemetryRRSSI != remrssi) { + _telemetryRRSSI = remrssi; emit telemetryRRSSIChanged(_telemetryRRSSI); } } diff --git a/src/ui/toolbar/MainToolBarController.h b/src/ui/toolbar/MainToolBarController.h index 8b3cae72ec74f07d967b268db3bdf3b8e8cfc72d..cb0efc6d75d1b667b7a4403844eec8acfecaafe3 100644 --- a/src/ui/toolbar/MainToolBarController.h +++ b/src/ui/toolbar/MainToolBarController.h @@ -80,7 +80,7 @@ signals: private slots: void _activeVehicleChanged (Vehicle* vehicle); void _setProgressBarValue (float value); - void _telemetryChanged (LinkInterface* link, unsigned rxerrors, unsigned fixed, unsigned rssi, unsigned remrssi, unsigned txbuf, unsigned noise, unsigned remnoise); + void _telemetryChanged (LinkInterface* link, unsigned rxerrors, unsigned fixed, int rssi, int remrssi, unsigned txbuf, unsigned noise, unsigned remnoise); void _delayedShowToolBarMessage (void); private: diff --git a/src/ui/toolbar/MainToolBarIndicators.qml b/src/ui/toolbar/MainToolBarIndicators.qml index 909175dfdde058cd9f135d39c4bc5402f1eebe2e..4637a9d6c470bb6d8d8969dd666c09a8000eec61 100644 --- a/src/ui/toolbar/MainToolBarIndicators.qml +++ b/src/ui/toolbar/MainToolBarIndicators.qml @@ -536,7 +536,7 @@ Row { color: colorWhite } QGCLabel { - text: _controller.telemetryRRSSI + 'dB' + text: _controller.telemetryRRSSI + 'dBm' width: getProportionalDimmension(30) horizontalAlignment: Text.AlignRight font.pixelSize: ScreenTools.smallFontPixelSize @@ -553,7 +553,7 @@ Row { color: colorWhite } QGCLabel { - text: _controller.telemetryLRSSI + 'dB' + text: _controller.telemetryLRSSI + 'dBm' width: getProportionalDimmension(30) horizontalAlignment: Text.AlignRight font.pixelSize: ScreenTools.smallFontPixelSize