diff --git a/radio/src/bluetooth.cpp b/radio/src/bluetooth.cpp index 984c0aa0b..1d3cc92bb 100644 --- a/radio/src/bluetooth.cpp +++ b/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; - if (data == START_STOP && bluetoothBufferIndex >= 2*FRSKY_SPORT_PACKET_SIZE) { - bluetoothWrite(bluetoothBuffer, bluetoothBufferIndex); - bluetoothBufferIndex = 0; + bluetoothBufferIndex = 0; + bluetoothCrc = 0x00; + + bluetoothBuffer[bluetoothBufferIndex++] = START_STOP; // start byte + for (uint8_t i=0; iinstance ^ 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; + } + } ); }); diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h index ed45cccfd..da6bc98c4 100644 --- a/radio/src/telemetry/frsky.h +++ b/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; diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index bdb84fac5..146b74bef 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/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[i]); - } + bluetoothForwardTelemetry(packet); } #endif diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp index bc26f87b6..190d036a3 100644 --- a/radio/src/telemetry/telemetry_sensors.cpp +++ b/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