diff --git a/radio/src/telemetry/flysky_ibus.cpp b/radio/src/telemetry/flysky_ibus.cpp index 296562ac8..db025d074 100644 --- a/radio/src/telemetry/flysky_ibus.cpp +++ b/radio/src/telemetry/flysky_ibus.cpp @@ -189,8 +189,8 @@ const FlySkySensor flySkySensors[] = { FS( AFHDS2A_ID_ODO1, STR_SENSOR_ODO1, UNIT_METERS, 2 ), // 2 bytes Odometer1 -- some magic with 330 needed FS( AFHDS2A_ID_ODO2, STR_SENSOR_ODO2, UNIT_METERS, 2 ), // 2 bytes Odometer2 -- some magic with 330 needed FS( AFHDS2A_ID_SPE, STR_SENSOR_ASPD, UNIT_KMH, 2 ), // 2 bytes Speed km/h -- some magic with 330 needed - FS( AFHDS2A_ID_GPS_LAT, STR_SENSOR_GPS, UNIT_RAW, 0 ), // 4 bytes signed WGS84 in degrees * 1E7 - FS( AFHDS2A_ID_GPS_LON, STR_SENSOR_GPS, UNIT_RAW, 0 ), // 4 bytes signed WGS84 in degrees * 1E7 + FS( AFHDS2A_ID_GPS_LAT, STR_SENSOR_GPS, UNIT_GPS, 0 ), // 4 bytes signed WGS84 in degrees * 1E7 +//FS( AFHDS2A_ID_GPS_LON, STR_SENSOR_GPS, UNIT_GPS, 0 ), // 4 bytes signed WGS84 in degrees * 1E7 FS( AFHDS2A_ID_GPS_ALT, STR_SENSOR_GPSALT, UNIT_METERS, 2 ), // 4 bytes signed GPS alt m*100 FS( AFHDS2A_ID_ALT, STR_SENSOR_ALT, UNIT_METERS, 2 ), // 4 bytes signed Alt m*100 FS( AFHDS2A_ID_RX_SIG_AFHDS3, STR_SENSOR_RX_QUALITY, UNIT_PERCENT, 0 ), // RX error rate @@ -347,6 +347,7 @@ void processFlySkySensor(const uint8_t * packet, uint8_t type) else if (id == AFHDS2A_ID_GPS_FULL) { //(AC FRAME)[ID][inst][size][fix][sats][LAT]x4[LON]x4[ALT]x4 setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_GPS_STATUS, 0, instance, packet[4], UNIT_RAW, 0); + for (uint8_t sensorID = AFHDS2A_ID_GPS_LAT; sensorID <= AFHDS2A_ID_GPS_ALT; sensorID++) { int index = 5 + (sensorID - AFHDS2A_ID_GPS_LAT) * 4; buffer[0] = sensorID; @@ -355,9 +356,22 @@ void processFlySkySensor(const uint8_t * packet, uint8_t type) memcpy(buffer + 3, packet + index, 4); processFlySkySensor(buffer, 0xAC); } + return; - } - else if (id == AFHDS2A_ID_VOLT_FULL) { + } else if (id == AFHDS2A_ID_GPS_LAT) { + uint8_t instance2 = 0; // Assume one instance, RX would only have one GPS + value = value / 10; + setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_GPS_LAT, 0, + instance2, value, UNIT_GPS_LATITUDE, 0); + return; + } else if (id == AFHDS2A_ID_GPS_LON) { // Remapped to single GPS sensor: + // AFHDS2A_ID_GPS_LAT + uint8_t instance2 = 0; + value = value / 10; + setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_GPS_LAT, 0, + instance2, value, UNIT_GPS_LONGITUDE, 0); + return; + } else if (id == AFHDS2A_ID_VOLT_FULL) { //(AC FRAME)[ID][inst][size][ACC_X]x2[ACC_Y]x2[ACC_Z]x2[ROLL]x2[PITCH]x2[YAW]x2 for (uint8_t sensorID = AFHDS2A_ID_EXTV; sensorID <= AFHDS2A_ID_RPM; sensorID++) { int index = 3 + (sensorID - AFHDS2A_ID_EXTV) * 2; @@ -368,8 +382,7 @@ void processFlySkySensor(const uint8_t * packet, uint8_t type) processFlySkySensor(buffer, 0xAA); } return; - } - else if (id == AFHDS2A_ID_ACC_FULL) { + } else if (id == AFHDS2A_ID_ACC_FULL) { //(AC FRAME)[ID][inst][size] for (uint8_t sensorID = AFHDS2A_ID_ACC_X; sensorID <= AFHDS2A_ID_YAW; sensorID++) { int index = 3 + (sensorID - AFHDS2A_ID_ACC_X) * 2;