Skip to content

Commit

Permalink
fix: Some AFHDS2A/AFHDS3 GPS/ALT telemetry handling issues (#3582)
Browse files Browse the repository at this point in the history
* 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.
  • Loading branch information
richardclli authored and pfeerick committed Aug 4, 2023
1 parent ef6891f commit ee68f2b
Showing 1 changed file with 9 additions and 10 deletions.
19 changes: 9 additions & 10 deletions radio/src/telemetry/flysky_ibus.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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
};
Expand Down Expand Up @@ -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

Expand All @@ -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
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit ee68f2b

Please sign in to comment.