From 1b3897a4250b0b2e5f8a850cd20f26ce053965d2 Mon Sep 17 00:00:00 2001
From: marbalon <marcin@baliniak.pl>
Date: Mon, 21 May 2018 10:37:09 +0200
Subject: [PATCH] Validate RSSI instance to allow user defined RSSI source
 (issue #5842) (#5846)

---
 radio/src/telemetry/frsky_sport.cpp       |  2 +-
 radio/src/telemetry/telemetry_sensors.cpp | 22 +++++++++++++++++++++-
 radio/src/telemetry/telemetry_sensors.h   |  1 +
 3 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp
index 9c03dfca9..f75858337 100644
--- a/radio/src/telemetry/frsky_sport.cpp
+++ b/radio/src/telemetry/frsky_sport.cpp
@@ -148,7 +148,7 @@ void sportProcessTelemetryPacket(const uint8_t * packet)
 
   if (primId == DATA_FRAME) {
     uint8_t instance = physicalId + 1;
-    if (id == RSSI_ID) {
+    if (id == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {
       telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
       data = SPORT_DATA_U8(packet);
       if (data == 0)
diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp
index 36330f4fa..049f3742a 100644
--- a/radio/src/telemetry/telemetry_sensors.cpp
+++ b/radio/src/telemetry/telemetry_sensors.cpp
@@ -28,7 +28,7 @@ bool isFaiForbidden(source_t idx)
   if (idx < MIXSRC_FIRST_TELEM) {
     return false;
   }
-  
+
   TelemetrySensor * sensor = &g_model.telemetrySensors[(idx-MIXSRC_FIRST_TELEM)/3];
 
   switch (telemetryProtocol) {
@@ -479,6 +479,26 @@ int lastUsedTelemetryIndex()
   return -1;
 }
 
+bool isValidIdAndInstance(uint16_t id, uint8_t instance)
+{
+  bool sensorFound = false;
+
+  for (int index=0; index<MAX_TELEMETRY_SENSORS; index++) {
+    TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
+    if (telemetrySensor.type == TELEM_TYPE_CUSTOM && telemetrySensor.id == id)
+    {
+      sensorFound = true;
+      if (telemetrySensor.instance == instance || g_model.ignoreSensorIds)
+        return true;
+    }
+  }
+
+  if (sensorFound)
+    return false;
+
+  return true;
+}
+
 int setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec)
 {
   bool available = false;
diff --git a/radio/src/telemetry/telemetry_sensors.h b/radio/src/telemetry/telemetry_sensors.h
index 78e9e48ad..a0f856a87 100644
--- a/radio/src/telemetry/telemetry_sensors.h
+++ b/radio/src/telemetry/telemetry_sensors.h
@@ -137,5 +137,6 @@ class TelemetryItem
 extern TelemetryItem telemetryItems[MAX_TELEMETRY_SENSORS];
 extern uint8_t allowNewSensors;
 bool isFaiForbidden(source_t idx);
+bool isValidIdAndInstance(uint16_t id, uint8_t instance);
 
 #endif // _TELEMETRY_SENSORS_H_