From 65e4d89d44849d8a1ab5afd7cd6f43af1af4fa61 Mon Sep 17 00:00:00 2001 From: Crashpilot1000 Date: Thu, 29 May 2014 20:27:26 +0200 Subject: [PATCH] Update telemetry_frsky.c Correct Frsky Gps output format DDDMM.MMMM --- src/telemetry_frsky.c | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/telemetry_frsky.c b/src/telemetry_frsky.c index e22439b8ed..d5683bb7cb 100644 --- a/src/telemetry_frsky.c +++ b/src/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'); }