From a362feed917ccbf83ce4851b0d2ad58a2bc7b2bf Mon Sep 17 00:00:00 2001 From: Frankie Arzu <32604366+frankiearzu@users.noreply.github.com> Date: Tue, 30 Jul 2024 20:07:56 -0500 Subject: [PATCH] feat(dsm): process new format for Spektrum Sky_ID GPS frames (#5355) --- radio/src/telemetry/spektrum.cpp | 43 ++++++++++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/radio/src/telemetry/spektrum.cpp b/radio/src/telemetry/spektrum.cpp index 17a385679..5b21799d9 100644 --- a/radio/src/telemetry/spektrum.cpp +++ b/radio/src/telemetry/spektrum.cpp @@ -76,6 +76,7 @@ #define I2C_ESC 0x20 // Electronic Speed Control #define I2C_ALPHA6 0x24 // Alpha6 Stabilizer - Blade Helis #define I2C_GPS_BIN 0x26 // GPS, binary format +#define I2C_REMOTE_ID 0x27 // Spektrum SkyID/RemoteID #define I2C_FP_BATT 0x34 // Flight Battery Capacity (Dual) #define I2C_CELLS 0x3A // 6S Cell Monitor (LiPo taps) #define I2C_VARIO 0x40 // Vario @@ -564,6 +565,7 @@ void processSpektrumPacket(const uint8_t *packet) #if TEST_CAPTURED_MESSAGE // Only for Testing when we don't have the sensor, but have sample data i2cAddress = replaceForTestingPackage(packet); + instance = packet[3]; #endif if (i2cAddress == I2C_FWD_PGM) { @@ -613,13 +615,13 @@ void processSpektrumPacket(const uint8_t *packet) else if (i2cAddress == I2C_FLITECTRL) { // AS3X + SAFE information: Flight Mode processAS3XPacket(packet); - // Continue for backward compatibility with scripts using 05XX sensors + return; // not a sensor... this is to cleanup many auto-generated 05XX sensors } // I2C_FLITECTRL else if (i2cAddress == I2C_ALPHA6) { // Alpha6 Flight Controller (Blade Helis): Flight Mode processAlpha6Packet(packet); - // Continue for backward compatibility with scripts using 24XX sensors + return; // not a sensor... this is to cleanup many auto-generated 24XX sensors } // I2C_ALPHA6 else if (i2cAddress == I2C_SMART_BAT_BASE_ADDRESS) { @@ -628,6 +630,24 @@ void processSpektrumPacket(const uint8_t *packet) i2cAddress = i2cAddress + (packet[4] >> 4); } // I2C_SMART_BAT_BASE_ADDRESS + else if (i2cAddress == I2C_REMOTE_ID) { + if (instance == I2C_GPS_LOC || instance == I2C_GPS_STAT) { + // RemoteID/SkyID GPS Data embeded in RemoteID packages + // The format is exactly the same (with the exception of the I2C_ID and Instance), + // so we just need to continue processing it as if the frame was a GPS telemetry data. + // The instance is populated with 0x16/0x17 when is GPS, and 0x00 when it is the + // usual RemoteID data + i2cAddress = instance; + instance = 0; + } else { + // Currently we are not processing any other of the RemoteID telemetry frames + // we can add in the future a new sensor(s) to record the Remote system ID if we want + // to log it as part of the telemetry data. + return; // not a sensor... this is to cleanup many auto-generated 27XX sensors + } + } // I2C_REMOTE_ID + + bool handled = false; for (const SpektrumSensor * sensor = spektrumSensors; sensor->i2caddress; sensor++) { // Optimization... the sensor table is sorted incrementally by i2cAddress @@ -1094,6 +1114,7 @@ static bool real0x16 = false; static bool real0x17 = false; static bool real0x34 = false; static bool real0x3A = false; +static bool real0x27 = false; // *********** GPS LOC (BCD) ****************************** // Example 0x16: 0 1 2 3 4 5 6 7 8 9 10 11 12 13 @@ -1128,6 +1149,13 @@ static char test3Adata[] = {0x3A, 0x00, 0x01, 0x9A, 0x01, 0x9B, 0x01, 0x9C, 0x01, 0x91 }; +// RemoteID (0x27), embeds Gps data in its frames, but by puting the real I2C frame +// address as the instance, everything else is the same +static char test27data_16[] = {0x27, 0x16, 0x97, 0x00, 0x54, 0x71, 0x12, 0x28, + 0x40, 0x80, 0x09, 0x11, 0x85, 0x14, 0x13, 0xBD}; // > 99 Flag +static char test27data_17[] = {0x27, 0x17, 0x25, 0x00, 0x00, + 0x28, 0x18, 0x21, 0x06, 0x00}; + static uint8_t replaceForTestingPackage(const uint8_t *packet) { uint8_t i2cAddress = packet[2] & 0x7f; @@ -1137,6 +1165,7 @@ static uint8_t replaceForTestingPackage(const uint8_t *packet) else if (i2cAddress == I2C_GPS_STAT) real0x17 = true; else if (i2cAddress == I2C_FP_BATT) real0x34 = true; else if (i2cAddress == I2C_CELLS) real0x3A = true; + else if (i2cAddress == I2C_REMOTE_ID) real0x27 = true; // Only Substiture AS3X/SAFE I2C_FLITECTRL packages, since they are constantly brodcast if (i2cAddress != I2C_FLITECTRL) { @@ -1148,6 +1177,12 @@ static uint8_t replaceForTestingPackage(const uint8_t *packet) // return original packet break; case 1: // return GSP LOG + if (!real0x27) { + test27data_16[4]=test27data_16[4]+1; + test27data_16[8]=test27data_16[8]+1; + memcpy((char *)packet + 2, test27data_16, 16); + real0x16=true; // disable test of regular GPS frames, and use the RemoteID + } if (!real0x16) { test16data[4]=test16data[4]+1; test16data[8]=test16data[8]+1; @@ -1155,6 +1190,10 @@ static uint8_t replaceForTestingPackage(const uint8_t *packet) } break; case 2: // Return GPS STAT + if (!real0x27) { + memcpy((char *)packet + 2, test27data_17, 10); + real0x17=true; // disable test of regular GPS frames, and use the RemoteID + } if (!real0x17) memcpy((char *)packet + 2, test17data, 10); break; case 3: // Return Dual Bat monitor