diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 204c6bc7a..a895fc53e 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -150,20 +150,35 @@ static void sendTime(void) serialize16(seconds % 60); } +// Frsky pdf: dddmm.mmmm +// .mmmm is returned in decimal fraction of minutes. +static void GPStoDDDMM_MMMM(int32_t mwiigps, int16_t *dddmm, int16_t *mmmm) +{ + int32_t absgps, deg, min; + absgps = abs(mwiigps); + deg = absgps / 10000000; + absgps = (absgps - deg * 10000000) * 60; // absgps = Minutes left * 10^7 + min = absgps / 10000000; // minutes left + *dddmm = deg * 100 + min; + *mmmm = (absgps - min * 10000000) / 1000; +} + static void sendGPS(void) { + int16_t tmp1, tmp2; + GPStoDDDMM_MMMM(GPS_coord[LAT], &tmp1, &tmp2); sendDataHead(ID_LATITUDE_BP); - serialize16(abs(GPS_coord[LAT]) / 100000); + serialize16(tmp1); sendDataHead(ID_LATITUDE_AP); - serialize16((abs(GPS_coord[LAT]) / 10) % 10000); - + serialize16(tmp2); sendDataHead(ID_N_S); serialize16(GPS_coord[LAT] < 0 ? 'S' : 'N'); + GPStoDDDMM_MMMM(GPS_coord[LON], &tmp1, &tmp2); sendDataHead(ID_LONGITUDE_BP); - serialize16(abs(GPS_coord[LON]) / 100000); + serialize16(tmp1); sendDataHead(ID_LONGITUDE_AP); - serialize16((abs(GPS_coord[LON]) / 10) % 10000); + serialize16(tmp2); sendDataHead(ID_E_W); serialize16(GPS_coord[LON] < 0 ? 'W' : 'E'); }