diff --git a/radio/src/telemetry/flysky_ibus.cpp b/radio/src/telemetry/flysky_ibus.cpp index 0ac0e677390..7c0effb2fcc 100644 --- a/radio/src/telemetry/flysky_ibus.cpp +++ b/radio/src/telemetry/flysky_ibus.cpp @@ -124,6 +124,7 @@ enum AFHDS3_FRM_TEMP = 0x57, //virtual AFHDS3_FRM_EXT_V = 0x58, //virtual + AFHDS2A_ID_PRES = 0x41, // Pressure AFHDS2A_ID_TX_V = 0x7F, // TX Voltage @@ -184,13 +185,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, 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_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_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_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 + { 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 { 0x00, NULL, UNIT_RAW, 0 }, // sentinel }; @@ -305,7 +306,7 @@ void processFlySkySensor(const uint8_t * packet, uint8_t type) if (type == 0xAA) value = (packet[3] << 8) | packet[2]; else - value = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3]; + value = (packet[5] << 24) | (packet[4] << 16) | (packet[3] << 8) | packet[2]; id = id ? id : SENSOR_TYPE_RX_VOL; // Remapped @@ -321,7 +322,7 @@ void processFlySkySensor(const uint8_t * packet, uint8_t type) telemetryData.rssi.set(value); if(value>0) telemetryStreaming = TELEMETRY_TIMEOUT10ms; } - else if (id == SENSOR_TYPE_PRES && value) { + else if (id == AFHDS2A_ID_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 @@ -388,8 +389,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 sensor = 0; - while (sensor++ < 7) { + int sesnor = 0; + while (sesnor++ < 7) { if (*buffer == SENSOR_TYPE_END) break; processFlySkySensor(buffer, 0xAA); buffer += 4;