Commit 48a3ea0d authored by dogmaphobic's avatar dogmaphobic

Changed RC RSSI to follow original spec.

parent 3f0592d2
......@@ -923,28 +923,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_rc_channels_t channels;
mavlink_msg_rc_channels_decode(&message, &channels);
/*
* Full RSSI field: 0b 1 111 1111
* | | |
* | | ^ These four bits encode a total of
* | | 16 RSSI levels. 15 = full, 0 = no signal
* | |
* | ^ These three bits encode a total of 8
* | digital RC input types.
* | 0: PPM, 1: SBUS, 2: Spektrum, 3: ST24
* |
* ^ If bit is set, RSSI encodes type + RSSI
*/
// TODO: Because the code on the firmware side never sets rssi to 0 on loss of connection
// we get a minimum value of 1 (3.5dB). This is what it does to compute it:
// msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
// Therefore, I'm eliminating bit 0 as well.
int tRssi = (channels.rssi & 0x0E) * 7;
if(tRssi > 100) {
tRssi = 100;
}
emit remoteControlRSSIChanged((uint8_t)tRssi);
emit remoteControlRSSIChanged(channels.rssi);
if (channels.chan1_raw != UINT16_MAX && channels.chancount > 0)
emit remoteControlChannelRawChanged(0, channels.chan1_raw);
......@@ -994,7 +973,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
const unsigned int portWidth = 8; // XXX magic number
// TODO: This makes an assumption the RSSI has been normalized to 0-100
emit remoteControlRSSIChanged(channels.rssi);
if (static_cast<uint16_t>(channels.chan1_scaled) != UINT16_MAX)
emit remoteControlChannelScaledChanged(channels.port * portWidth + 0, channels.chan1_scaled/10000.0f);
......
......@@ -377,7 +377,7 @@ void MainToolBar::_telemetryChanged(LinkInterface*, unsigned, unsigned, unsigned
}
}
void MainToolBar::_remoteControlRSSIChanged(float rssi)
void MainToolBar::_remoteControlRSSIChanged(uint8_t rssi)
{
// We only care if we haveone single connection
if(_connectionCount == 1) {
......
......@@ -159,7 +159,7 @@ private slots:
void _leaveMessageView ();
void _setSatLoc (UASInterface* uas, int fix);
void _setProgressBarValue (float value);
void _remoteControlRSSIChanged (float rssi);
void _remoteControlRSSIChanged (uint8_t rssi);
void _telemetryChanged (LinkInterface* link, unsigned rxerrors, unsigned fixed, unsigned rssi, unsigned remrssi, unsigned txbuf, unsigned noise, unsigned remnoise);
private:
......
......@@ -351,7 +351,7 @@ Rectangle {
id: rssiRC
width: getProportionalDimmension(55)
height: cellHeight
visible: showMavStatus() && mainToolBar.showRSSI
visible: showMavStatus() && mainToolBar.showRSSI && mainToolBar.remoteRSSI <= 100
anchors.verticalCenter: parent.verticalCenter
color: getRSSIColor(mainToolBar.remoteRSSI);
border.color: "#00000000"
......
Markdown is supported
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