Lat long precision fix

vehicle/OVMS.X/utils.c

@@ -289,27 +289,74 @@ unsigned long axtoul(char *s)
   return val;
 }
 
-
+#ifndef uint8_t //C18 compiler defines
+#define int8_t    signed char
+#define int16_t   signed short
+#define int32_t   signed long
+#define uint8_t   unsigned char
+#define uint16_t  unsigned short
+#define uint32_t  unsigned long
+#endif
 // Convert GPS coordinate form to internal latlon value
 // SIM908: DDDMM.MMMMMM (separate South/West handling, see net.c)
 // SIM808: +-ddd.dddddd
 
 long gps2latlon(char *gpscoord)
 {
-  float f;
-#ifdef OVMS_SIMCOM_SIM908
-  long d;
-#endif //OVMS_SIMCOM_SIM908
+  int16_t degrees;
+  uint16_t rem;
+  int32_t frac;
+  char *s;
 
-  f = myatof(gpscoord);
+  degrees = atoi(gpscoord); //use built-in to read integer value
+  rem = 6;
+
+#ifdef OVMS_SIMCOM_SIM908 // SIM908: DDDMM.MMMMMM
+  frac = degrees % 100; // extract minutes: Fails with negatives
+  degrees /= 100;       // fix degrees
+#else //SIM808: (-)DDD.DDDDDD
+  frac = 0;
+#endif //OVMS_SIMCOM_SIM908
 
-#ifdef OVMS_SIMCOM_SIM908
-  // SIM908: DDDMM.MMMMMM
-  d = (long) (f / 100); // extract degrees
-  f = (float) d + (f - (d * 100)) / 60; // convert to decimal format
+  if ( (s = strchr(gpscoord, '.')) )
+  {
+    while (*++s)     //read and scale fractional part
+    {
+      if( (unsigned char)(*s - '0') > (unsigned char) 9 )
+        break;       //check for non-decimal
+      frac = frac * 10 + (*s - '0');
+      if(--rem==0)   //check for too many decimal places
+        break;
+    }
+    while (rem--!=0) //make up missing digits with zeros
+    {
+      frac = frac * 10;
+    }
+    //reuse rem here to minimize stack
+#ifdef OVMS_SIMCOM_SIM908 // SIM908: DDDMM.MMMMMM
+    //fractional part is at most 26bits (59999999 < 2^26)
+    // Drop precision to 2^24 (not including >7bits of whole degrees)
+    //multiply 12bits at a time with 19bit constant to keep within 32bits
+    //then bit shift into place and accumulate
+#define GPS_DIVIDER ((uint32_t) 515396) //(3600 * 2048 / 60000000) * 2^22
+    frac = ( frac + (1<<1) ) >> 2; //drop two bits we can't use
+    rem = (uint32_t)(((frac & 0x0FFF) * GPS_DIVIDER) + 524288 ) >> 20;
+    frac = ((((uint32_t)(frac & 0x00FFF000)>>12) * GPS_DIVIDER) + 128) >> 8;    
+#else //SIM808: (-)DDD.DDDDDD
+    //fractional part is at most 20bits (999999 < 2^20)
+    //multiply 10bits at a time with 22bit constant to keep within 32bits
+    //then bit shift into place and accumulate
+#define GPS_DIVIDER ((uint32_t) 1932735) //(3600 * 2048 / 1000000) * 2^18
+    rem = (uint32_t)(((frac & 0x03FF) * GPS_DIVIDER) + 131072 ) >> 18;
+    frac = ((((uint32_t)(frac & 0x000FFC00)>>10) * GPS_DIVIDER) + 128 ) >> 8;
 #endif //OVMS_SIMCOM_SIM908
 
-  return (long) (f * 3600 * 2048); // convert to raw format
+    frac += rem;
+  }
+
+  if(degrees <0)
+    frac = -frac;
+  return (int32_t)degrees * 7372800 + frac; //2048*3600
 }
 
 
@@ -612,18 +659,23 @@ char *stp_l2f_h(char *dst, const rom char *prefix, unsigned long val, int cdecim
 
 char *stp_latlon(char *dst, const rom char *prefix, long latlon)
 {
-  float res;
+  unsigned long res;
 
   if (prefix)
     dst = stp_rom(dst, prefix);
 
   if (latlon < 0)
   {
     *dst++ = '-';
-    latlon = ~latlon; // and invert value
+    latlon = -latlon;
   }
-  res = (float) latlon / 2048 / 3600; // Tesla specific GPS conversion
-  return stp_l2f(dst, NULL, res * 1000000, 6);
+#define STP_LATLON_SCALER1 (unsigned long) 8888 // 1000000 / (3600 * 2048) * 2^16
+#define STP_LATLON_SCALER2 (unsigned long) 58254 // remainder (0.888888) * 2^16
+  res  = (((unsigned long) latlon >>16) * STP_LATLON_SCALER1);
+  res += ((unsigned short) latlon * STP_LATLON_SCALER1 + 32768UL) >> 16;
+  res += (((unsigned long) latlon >>16) * STP_LATLON_SCALER2 + 32768UL) >> 16;
+  res += ((unsigned short) latlon * STP_LATLON_SCALER2) & 0x80000000UL ? 1 : 0;
+  return stp_l2f(dst, NULL, res , 6);
 }