S.PORT Push now working on internal module

This commit is contained in:
Bertrand Songis
2019-04-01 17:48:30 +02:00
parent 99c413dbac
commit 3bc75c1d55
5 changed files with 93 additions and 49 deletions
+3 -29
View File
@@ -150,42 +150,16 @@ void sportProcessTelemetryPacket(const uint8_t * packet)
sportProcessTelemetryPacketWithoutCrc(TELEMETRY_ENDPOINT_SPORT, packet);
}
void sportOutputByteStuffAndCrc()
{
uint16_t crc = 0;
uint8_t count = sizeof(SportTelemetryPacket);
for (uint8_t i = 1/*no bytestuffing for the physicalID*/; i < count; i++) {
uint8_t byte = outputTelemetryBuffer.data[i];
if (byte == 0x7E || byte == 0x7D) {
memmove(&outputTelemetryBuffer.data[i + 2], &outputTelemetryBuffer.data[i + 1], sizeof(SportTelemetryPacket) - i - 1);
outputTelemetryBuffer.data[i] = 0x7D;
outputTelemetryBuffer.data[i + 1] = 0x20 ^ byte;
i += 1, count += 1;
}
crc += byte; // 0-1FF
crc += crc >> 8; // 0-100
crc &= 0x00ff;
}
outputTelemetryBuffer.data[count] = 0xFF - crc;
outputTelemetryBuffer.size = count + 1;
}
void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet)
void sportProcessTelemetryPacketWithoutCrc(uint8_t originValue, const uint8_t * packet)
{
TelemetryEndpoint & origin = (TelemetryEndpoint &)originValue;
uint8_t physicalId = packet[0] & 0x1F;
uint8_t primId = packet[1];
uint16_t dataId = *((uint16_t *)(packet+2));
uint32_t data = SPORT_DATA_S32(packet);
if (primId == DATA_FRAME) {
if (outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_ANY && dataId == outputTelemetryBuffer.sport.dataId) {
// outputTelemetryBuffer.sport.physicalId = packet[0];
if (origin == SPORT_MODULE)
sportOutputByteStuffAndCrc();
outputTelemetryBuffer.setDestination(origin);
}
uint8_t instance = physicalId + 1;
uint8_t instance = physicalId + (origin.rxUid << 5);
if (dataId == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {
telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
data = SPORT_DATA_U8(packet);