From 80ee43db7194cb545c9fce964b15a5d3b1375c55 Mon Sep 17 00:00:00 2001
From: bsongis <bsongis@8db61ce9-5e8f-94c8-fa6b-1d1c8bcca7d0>
Date: Thu, 9 Feb 2012 23:04:08 +0000
Subject: [PATCH] GPS maths prepared on GPS fix. Still needs to be tested!

---
 src/frsky.cpp | 20 ++++++++++++++------
 src/frsky.h   |  1 +
 2 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/src/frsky.cpp b/src/frsky.cpp
index f0956d5aa..25e2eb9af 100644
--- a/src/frsky.cpp
+++ b/src/frsky.cpp
@@ -158,6 +158,9 @@ void parseTelemHubByte(uint8_t byte)
     if (!frskyHubData.minCellVolts || cellVolts < frskyHubData.minCellVolts)
       frskyHubData.minCellVolts = cellVolts;
   }
+  if ((uint8_t)structPos == offsetof(FrskyHubData, gpsLatitudeNS/*TODO check that it is received at last!*/) && !frskyHubData.pilotLatitude) {
+    getGpsPilotCoords();
+  }
 
   state = TS_IDLE;
 }
@@ -528,12 +531,7 @@ void resetTelemetry()
   frskyHubData.cellsCount = 0;
   frskyHubData.minCellVolts = 0;
   if (frskyHubData.gpsLatitude_ap) {
-    frskyHubData.pilotLatitude = /*((uint32_t)frskyHubData.gpsLatitude_bp << 16) + */(((frskyHubData.gpsLatitude_bp % 100) * 10000 + frskyHubData.gpsLatitude_ap) * 5) / 3;
-    frskyHubData.pilotLongitude = /*((uint32_t)frskyHubData.gpsLongitude_bp << 16) + */(((frskyHubData.gpsLongitude_bp % 100) * 10000 + frskyHubData.gpsLongitude_ap) * 5) / 3;
-    uint32_t angle2 = (frskyHubData.gpsLatitude_bp*frskyHubData.gpsLatitude_bp) / 10000;
-    uint32_t angle4 = angle2 * angle2;
-    frskyHubData.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
-    // printf("frskyHubData.distFromEarthAxis=%d\n", frskyHubData.distFromEarthAxis); fflush(stdout);
+    getGpsPilotCoords();
   }
 #endif
 
@@ -565,6 +563,16 @@ uint16_t sqrt32(uint32_t n)
   }
 }
 
+void getGpsPilotCoords()
+{
+  frskyHubData.pilotLatitude = /*((uint32_t)frskyHubData.gpsLatitude_bp << 16) + */(((frskyHubData.gpsLatitude_bp % 100) * 10000 + frskyHubData.gpsLatitude_ap) * 5) / 3;
+  frskyHubData.pilotLongitude = /*((uint32_t)frskyHubData.gpsLongitude_bp << 16) + */(((frskyHubData.gpsLongitude_bp % 100) * 10000 + frskyHubData.gpsLongitude_ap) * 5) / 3;
+  uint32_t angle2 = (frskyHubData.gpsLatitude_bp*frskyHubData.gpsLatitude_bp) / 10000;
+  uint32_t angle4 = angle2 * angle2;
+  frskyHubData.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
+  // printf("frskyHubData.distFromEarthAxis=%d\n", frskyHubData.distFromEarthAxis); fflush(stdout);
+}
+
 uint32_t getGpsDistanceX2()
 {
   uint32_t lat = (((frskyHubData.gpsLatitude_bp % 100) * 10000 + frskyHubData.gpsLatitude_ap) * 5) / 3;
diff --git a/src/frsky.h b/src/frsky.h
index ee2afd645..2d1fcbc5a 100644
--- a/src/frsky.h
+++ b/src/frsky.h
@@ -144,6 +144,7 @@ bool FRSKY_alarmRaised(uint8_t idx);
 void resetTelemetry();
 
 #ifdef FRSKY_HUB
+void getGpsPilotCoords();
 uint32_t getGpsDistanceX2();
 uint32_t getGpsDistance();
 #endif