Telemetry fixes

radio/src/bluetooth.cpp

@@ -253,13 +253,19 @@ void bluetoothSendTrainer()
   bluetoothBufferIndex = 0;
 }
 
-void bluetoothForwardTelemetry(uint8_t data)
+void bluetoothForwardTelemetry(const uint8_t * packet)
 {
-  bluetoothBuffer[bluetoothBufferIndex++] = data;
+  bluetoothBufferIndex = 0;
-  if (data == START_STOP && bluetoothBufferIndex >= 2*FRSKY_SPORT_PACKET_SIZE) {
+  bluetoothCrc = 0x00;
-    bluetoothWrite(bluetoothBuffer, bluetoothBufferIndex);
+
-    bluetoothBufferIndex = 0;
+  bluetoothBuffer[bluetoothBufferIndex++] = START_STOP; // start byte
+  for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
+    bluetoothPushByte(packet[i]);
   }
+  bluetoothBuffer[bluetoothBufferIndex++] = bluetoothCrc;
+  bluetoothBuffer[bluetoothBufferIndex++] = START_STOP; // end byte
+  bluetoothWrite(bluetoothBuffer, bluetoothBufferIndex);
+  bluetoothBufferIndex = 0;
 }
 
 void bluetoothReceiveTrainer()

radio/src/bluetooth.h

@@ -50,5 +50,5 @@ extern char bluetoothDistantAddr[LEN_BLUETOOTH_ADDR+1];
 
 char * bluetoothReadline(bool error_reset=true);
 void bluetoothWriteString(const char * command);
-void bluetoothForwardTelemetry(uint8_t data);
+void bluetoothForwardTelemetry(const uint8_t * packet);
 void bluetoothWakeup();

radio/src/dataconstants.h

@@ -246,6 +246,16 @@ enum UartModes {
   #define LEN_BLUETOOTH_NAME           10
 #endif
 
+enum TelemetryProtocol
+{
+  TELEM_PROTO_FRSKY_D,
+  TELEM_PROTO_FRSKY_SPORT,
+  TELEM_PROTO_CROSSFIRE,
+  TELEM_PROTO_SPEKTRUM,
+  TELEM_PROTO_LUA,
+  TELEM_PROTO_FLYSKY_IBUS,
+};
+
 #define TELEM_LABEL_LEN                4
 enum TelemetryUnit {
   UNIT_RAW,

radio/src/datastructs.h

@@ -400,6 +400,21 @@ PACK(struct TelemetrySensor {
     bool isPrecConfigurable() const;
     int32_t getPrecMultiplier() const;
     int32_t getPrecDivisor() const;
+    bool isSameInstance(TelemetryProtocol protocol, uint8_t instance)
+    {
+      if (protocol == TELEM_PROTO_FRSKY_SPORT) {
+        if (((this->instance ^ instance) & 0x9F) == 0) {
+          this->instance = instance; // update the instance in case we had telemetry switching
+          return true;
+        }
+        else {
+          return false;
+        }
+      }
+      else {
+        return this->instance == instance;
+      }
+    }
   );
 });
 

radio/src/telemetry/frsky.h

@@ -292,16 +292,6 @@ void telemetryInit(uint8_t protocol);
 
 void telemetryInterrupt10ms();
 
-enum TelemetryProtocol
-{
-  TELEM_PROTO_FRSKY_D,
-  TELEM_PROTO_FRSKY_SPORT,
-  TELEM_PROTO_CROSSFIRE,
-  TELEM_PROTO_SPEKTRUM,
-  TELEM_PROTO_LUA,
-  TELEM_PROTO_FLYSKY_IBUS,
-};
-
 struct TelemetryData {
   TelemetryValue swr;
   FilteredTelemetryValue rssi;

radio/src/telemetry/frsky_sport.cpp

@@ -159,9 +159,7 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
 
 #if defined(BLUETOOTH)
   if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetoothState == BLUETOOTH_STATE_CONNECTED) {
-    for (uint8_t i = 0; i < sizeof(SportTelemetryPacket); i++) {
+    bluetoothForwardTelemetry(packet);
-      bluetoothForwardTelemetry(packet[i]);
-    }
   }
 #endif
 

radio/src/telemetry/telemetry_sensors.cpp

@@ -485,7 +485,7 @@ int setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, ui
 
   for (int index=0; index<MAX_TELEMETRY_SENSORS; index++) {
     TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
-    if (telemetrySensor.type == TELEM_TYPE_CUSTOM && telemetrySensor.id == id && telemetrySensor.subId == subId && (telemetrySensor.instance == instance || g_model.ignoreSensorIds)) {
+    if (telemetrySensor.type == TELEM_TYPE_CUSTOM && telemetrySensor.id == id && telemetrySensor.subId == subId && (telemetrySensor.isSameInstance(protocol, instance) || g_model.ignoreSensorIds)) {
       telemetryItems[index].setValue(telemetrySensor, value, unit, prec);
       available = true;
       // we continue search here, because sensors can share the same id and instance