From ee68f2ba19fa46aed0b9b216553193e0eccf7b68 Mon Sep 17 00:00:00 2001 From: richardclli Date: Fri, 4 Aug 2023 13:30:10 +0800 Subject: [PATCH] fix: Some AFHDS2A/AFHDS3 GPS/ALT telemetry handling issues (#3582) * Fix widgets vs. sensors (TX16s/FS-IA6B FW2.8.3) and updated meaningful units. * Updated the GPS sensors to degree unit. * Fixed a bug in AFHDS2 Alt decoding. * Fixed a typo. * Update GPS sensors format. * Change Long/Lat unit to RAW. --- radio/src/telemetry/flysky_ibus.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/radio/src/telemetry/flysky_ibus.cpp b/radio/src/telemetry/flysky_ibus.cpp index 7c0effb2fcc..0ac0e677390 100644 --- a/radio/src/telemetry/flysky_ibus.cpp +++ b/radio/src/telemetry/flysky_ibus.cpp @@ -124,7 +124,6 @@ enum AFHDS3_FRM_TEMP = 0x57, //virtual AFHDS3_FRM_EXT_V = 0x58, //virtual - AFHDS2A_ID_PRES = 0x41, // Pressure AFHDS2A_ID_TX_V = 0x7F, // TX Voltage @@ -185,13 +184,13 @@ const FlySkySensor flySkySensors[] = { { AFHDS2A_ID_ODO1, STR_SENSOR_ODO1, UNIT_METERS, 2 }, // 2 bytes Odometer1 -- some magic with 330 needed { AFHDS2A_ID_ODO2, STR_SENSOR_ODO2, UNIT_METERS, 2 }, // 2 bytes Odometer2 -- some magic with 330 needed { AFHDS2A_ID_SPE, STR_SENSOR_ASPD, UNIT_KMH, 2 }, // 2 bytes Speed km/h -- some magic with 330 needed - { AFHDS2A_ID_GPS_LAT, STR_SENSOR_GPS, UNIT_RAW, 7 }, // 4 bytes signed WGS84 in degrees * 1E7 - { AFHDS2A_ID_GPS_LON, STR_SENSOR_GPS, UNIT_RAW, 7 }, // 4 bytes signed WGS84 in degrees * 1E7 + { AFHDS2A_ID_GPS_LAT, STR_SENSOR_GPS, UNIT_RAW, 0 }, // 4 bytes signed WGS84 in degrees * 1E7 + { AFHDS2A_ID_GPS_LON, STR_SENSOR_GPS, UNIT_RAW, 0 }, // 4 bytes signed WGS84 in degrees * 1E7 { AFHDS2A_ID_GPS_ALT, STR_SENSOR_GPSALT, UNIT_METERS, 2 }, // 4 bytes signed GPS alt m*100 { AFHDS2A_ID_ALT, STR_SENSOR_ALT, UNIT_METERS, 2 }, // 4 bytes signed Alt m*100 - { AFHDS2A_ID_RX_SIG_AFHDS3, STR_RX_QUALITY, UNIT_RAW, 0 }, // RX error rate - { AFHDS2A_ID_RX_SNR_AFHDS3, STR_RX_SNR, UNIT_DB, 1 }, // RX SNR - { AFHDS2A_ID_TX_RSSI, STR_SENSOR_TX_RSSI, UNIT_RAW, 0 }, // Pseudo sensor for TRSSI + { AFHDS2A_ID_RX_SIG_AFHDS3, STR_SENSOR_RX_QUALITY, UNIT_PERCENT, 0 }, // RX error rate + { AFHDS2A_ID_RX_SNR_AFHDS3, STR_SENSOR_RX_SNR, UNIT_DB, 1 }, // RX SNR + { AFHDS2A_ID_TX_RSSI, STR_SENSOR_TX_RSSI, UNIT_DBM, 0 }, // Pseudo sensor for TRSSI { 0x00, NULL, UNIT_RAW, 0 }, // sentinel }; @@ -306,7 +305,7 @@ void processFlySkySensor(const uint8_t * packet, uint8_t type) if (type == 0xAA) value = (packet[3] << 8) | packet[2]; else - value = (packet[5] << 24) | (packet[4] << 16) | (packet[3] << 8) | packet[2]; + value = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3]; id = id ? id : SENSOR_TYPE_RX_VOL; // Remapped @@ -322,7 +321,7 @@ void processFlySkySensor(const uint8_t * packet, uint8_t type) telemetryData.rssi.set(value); if(value>0) telemetryStreaming = TELEMETRY_TIMEOUT10ms; } - else if (id == AFHDS2A_ID_PRES && value) { + else if (id == SENSOR_TYPE_PRES && value) { // Extract temperature to a new sensor setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, id | 0x100, 0, instance, ((value >> 19) - 400), UNIT_CELSIUS, 1); // Extract alt to a new sensor @@ -389,8 +388,8 @@ void processFlySkyPacket(const uint8_t * packet) setFlyskyTelemetryValue(AFHDS2A_ID_TX_RSSI, 0, packet[0], UNIT_RAW, 0); const uint8_t * buffer = packet + 1; - int sesnor = 0; - while (sesnor++ < 7) { + int sensor = 0; + while (sensor++ < 7) { if (*buffer == SENSOR_TYPE_END) break; processFlySkySensor(buffer, 0xAA); buffer += 4;