From f556caeafd127581980a41e5fa67bd76359ceedb Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Thu, 4 May 2017 21:37:56 -0400 Subject: [PATCH] Noise and RSSI are negative values. --- src/Vehicle/Vehicle.cc | 13 +++++++++---- src/Vehicle/Vehicle.h | 16 ++++++++-------- src/ui/toolbar/TelemetryRSSIIndicator.qml | 8 ++------ 3 files changed, 19 insertions(+), 18 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 2503a6af8..3fdc19531 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -919,6 +919,8 @@ void Vehicle::_handleRadioStatus(mavlink_message_t& message) mavlink_msg_radio_status_decode(&message, &rstatus); int rssi = rstatus.rssi; int remrssi = rstatus.remrssi; + int lnoise = (int)(int8_t)rstatus.noise; + int rnoise = (int)(int8_t)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 @@ -933,6 +935,9 @@ void Vehicle::_handleRadioStatus(mavlink_message_t& message) */ 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); + } else { + rssi = (int)(int8_t)rstatus.rssi; + remrssi = (int)(int8_t)rstatus.remrssi; } //-- Check for changes if(_telemetryLRSSI != rssi) { @@ -955,12 +960,12 @@ void Vehicle::_handleRadioStatus(mavlink_message_t& message) _telemetryTXBuffer = rstatus.txbuf; emit telemetryTXBufferChanged(_telemetryTXBuffer); } - if(_telemetryLNoise != rstatus.noise) { - _telemetryLNoise = rstatus.noise; + if(_telemetryLNoise != lnoise) { + _telemetryLNoise = lnoise; emit telemetryLNoiseChanged(_telemetryLNoise); } - if(_telemetryRNoise != rstatus.remnoise) { - _telemetryRNoise = rstatus.remnoise; + if(_telemetryRNoise != rnoise) { + _telemetryRNoise = rnoise; emit telemetryRNoiseChanged(_telemetryRNoise); } } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 7b9944fb8..dc8a9cfd3 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -307,8 +307,8 @@ public: Q_PROPERTY(unsigned int telemetryRXErrors READ telemetryRXErrors NOTIFY telemetryRXErrorsChanged) Q_PROPERTY(unsigned int telemetryFixed READ telemetryFixed NOTIFY telemetryFixedChanged) Q_PROPERTY(unsigned int telemetryTXBuffer READ telemetryTXBuffer NOTIFY telemetryTXBufferChanged) - Q_PROPERTY(unsigned int telemetryLNoise READ telemetryLNoise NOTIFY telemetryLNoiseChanged) - Q_PROPERTY(unsigned int telemetryRNoise READ telemetryRNoise NOTIFY telemetryRNoiseChanged) + Q_PROPERTY(int telemetryLNoise READ telemetryLNoise NOTIFY telemetryLNoiseChanged) + Q_PROPERTY(int telemetryRNoise READ telemetryRNoise NOTIFY telemetryRNoiseChanged) Q_PROPERTY(QVariantList toolBarIndicators READ toolBarIndicators CONSTANT) Q_PROPERTY(QVariantList cameraList READ cameraList CONSTANT) @@ -587,8 +587,8 @@ public: unsigned int telemetryRXErrors () { return _telemetryRXErrors; } unsigned int telemetryFixed () { return _telemetryFixed; } unsigned int telemetryTXBuffer () { return _telemetryTXBuffer; } - unsigned int telemetryLNoise () { return _telemetryLNoise; } - unsigned int telemetryRNoise () { return _telemetryRNoise; } + int telemetryLNoise () { return _telemetryLNoise; } + int telemetryRNoise () { return _telemetryRNoise; } bool autoDisarm (); Fact* roll (void) { return &_rollFact; } @@ -717,8 +717,8 @@ signals: void telemetryRXErrorsChanged (unsigned int value); void telemetryFixedChanged (unsigned int value); void telemetryTXBufferChanged (unsigned int value); - void telemetryLNoiseChanged (unsigned int value); - void telemetryRNoiseChanged (unsigned int value); + void telemetryLNoiseChanged (int value); + void telemetryRNoiseChanged (int value); void autoDisarmChanged (void); void firmwareMajorVersionChanged(int major); @@ -881,8 +881,8 @@ private: uint32_t _telemetryRXErrors; uint32_t _telemetryFixed; uint32_t _telemetryTXBuffer; - uint32_t _telemetryLNoise; - uint32_t _telemetryRNoise; + int _telemetryLNoise; + int _telemetryRNoise; bool _vehicleCapabilitiesKnown; bool _supportsMissionItemInt; diff --git a/src/ui/toolbar/TelemetryRSSIIndicator.qml b/src/ui/toolbar/TelemetryRSSIIndicator.qml index d7c0ed0ba..3de4c8a97 100644 --- a/src/ui/toolbar/TelemetryRSSIIndicator.qml +++ b/src/ui/toolbar/TelemetryRSSIIndicator.qml @@ -26,11 +26,7 @@ Item { width: _hasTelemetry ? telemIcon.width * 1.1 : 0 visible: _hasTelemetry - //-- SiK Radio: -120 to < 0 - //-- Others: > 0 - 100 - property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle - property bool _isSiKRadio: _activeVehicle ? _activeVehicle.telemetryLRSSI < 0 : false property bool _hasTelemetry: _activeVehicle ? _activeVehicle.telemetryLRSSI !== 0 : false Component { @@ -60,9 +56,9 @@ Item { columns: 2 anchors.horizontalCenter: parent.horizontalCenter QGCLabel { text: qsTr("Local RSSI:") } - QGCLabel { text: _activeVehicle.telemetryLRSSI + _isSiKRadio ? " dBm" : ""} + QGCLabel { text: _activeVehicle.telemetryLRSSI + " dBm"} QGCLabel { text: qsTr("Remote RSSI:") } - QGCLabel { text: _activeVehicle.telemetryRRSSI + _isSiKRadio ? " dBm" : ""} + QGCLabel { text: _activeVehicle.telemetryRRSSI + " dBm"} QGCLabel { text: qsTr("RX Errors:") } QGCLabel { text: _activeVehicle.telemetryRXErrors } QGCLabel { text: qsTr("Errors Fixed:") } -- 2.22.0