PZ90 datum support

add a configuration DATUM_TYPE in gps.conf to configure
datum type. Default datum type is WGS84.

Change-Id: Ia8404aa5cf6d49741b9f487f8b086abd095f21ea
CRs-fixed: 2338883

4 files changed, 361 insertions, 14 deletions

diff --git a/utils/loc_cfg.cpp b/utils/loc_cfg.cpp
index 71dfc66..3676bd5 100644
--- a/utils/loc_cfg.cpp
+++ b/utils/loc_cfg.cpp
@@ -57,12 +57,14 @@
 /* Parameter data */
 static uint32_t DEBUG_LEVEL = 0xff;
 static uint32_t TIMESTAMP = 0;
+static uint32_t DATUM_TYPE = 0;
 
 /* Parameter spec table */
 static const loc_param_s_type loc_param_table[] =
 {
     {"DEBUG_LEVEL",        &DEBUG_LEVEL,        NULL,    'n'},
     {"TIMESTAMP",          &TIMESTAMP,          NULL,    'n'},
+    {"DATUM_TYPE",         &DATUM_TYPE,         NULL,    'n'},
 };
 static const int loc_param_num = sizeof(loc_param_table) / sizeof(loc_param_s_type);
 
@@ -86,6 +88,29 @@ const char LOC_PATH_XTWIFI_CONF[] = LOC_PATH_XTWIFI_CONF_STR;
 const char LOC_PATH_QUIPC_CONF[] = LOC_PATH_QUIPC_CONF_STR;
 
 /*===========================================================================
+FUNCTION loc_get_datum_type
+
+DESCRIPTION
+   get datum type
+
+PARAMETERS:
+   N/A
+
+DEPENDENCIES
+   N/A
+
+RETURN VALUE
+   DATUM TYPE
+
+SIDE EFFECTS
+   N/A
+===========================================================================*/
+int loc_get_datum_type()
+{
+    return DATUM_TYPE;
+}
+
+/*===========================================================================
 FUNCTION loc_set_config_entry
 
 DESCRIPTION
diff --git a/utils/loc_cfg.h b/utils/loc_cfg.h
index a8bdaed..aa00148 100644
--- a/utils/loc_cfg.h
+++ b/utils/loc_cfg.h
@@ -132,7 +132,7 @@ extern const char LOC_PATH_QUIPC_CONF[];
 
 int loc_read_process_conf(const char* conf_file_name, uint32_t * process_count_ptr,
                           loc_process_info_s_type** process_info_table_ptr);
-
+int loc_get_datum_type();
 #ifdef __cplusplus
 }
 #endif
diff --git a/utils/loc_nmea.cpp b/utils/loc_nmea.cpp
index b4de6db..e9a8f84 100644
--- a/utils/loc_nmea.cpp
+++ b/utils/loc_nmea.cpp
@@ -33,6 +33,7 @@
 #include <math.h>
 #include <log_util.h>
 #include <loc_pla.h>
+#include <loc_cfg.h>
 
 #define GLONASS_SV_ID_OFFSET 64
 #define MAX_SATELLITES_IN_USE 12
@@ -114,6 +115,126 @@ typedef struct loc_sv_cache_info_s
 } loc_sv_cache_info;
 
 /*===========================================================================
+FUNCTION    convert_Lla_to_Ecef
+
+DESCRIPTION
+   Convert LLA to ECEF
+
+DEPENDENCIES
+   NONE
+
+RETURN VALUE
+   NONE
+
+SIDE EFFECTS
+   N/A
+
+===========================================================================*/
+static void convert_Lla_to_Ecef(const LocLla& plla, LocEcef& pecef)
+{
+    double r;
+
+    r = MAJA / sqrt(1.0 - ESQR * sin(plla.lat) * sin(plla.lat));
+    pecef.X = (r + plla.alt) * cos(plla.lat) * cos(plla.lon);
+    pecef.Y = (r + plla.alt) * cos(plla.lat) * sin(plla.lon);
+    pecef.Z = (r * OMES + plla.alt) * sin(plla.lat);
+}
+
+/*===========================================================================
+FUNCTION    convert_WGS84_to_PZ90
+
+DESCRIPTION
+   Convert datum from WGS84 to PZ90
+
+DEPENDENCIES
+   NONE
+
+RETURN VALUE
+   NONE
+
+SIDE EFFECTS
+   N/A
+
+===========================================================================*/
+static void convert_WGS84_to_PZ90(const LocEcef& pWGS84, LocEcef& pPZ90)
+{
+    double deltaX     = DatumConstFromWGS84[0];
+    double deltaY     = DatumConstFromWGS84[1];
+    double deltaZ     = DatumConstFromWGS84[2];
+    double deltaScale = DatumConstFromWGS84[3];
+    double rotX       = DatumConstFromWGS84[4];
+    double rotY       = DatumConstFromWGS84[5];
+    double rotZ       = DatumConstFromWGS84[6];
+
+    pPZ90.X = deltaX + deltaScale * (pWGS84.X + rotZ * pWGS84.Y - rotY * pWGS84.Z);
+    pPZ90.Y = deltaY + deltaScale * (pWGS84.Y - rotZ * pWGS84.X + rotX * pWGS84.Z);
+    pPZ90.Z = deltaZ + deltaScale * (pWGS84.Z + rotY * pWGS84.X - rotX * pWGS84.Y);
+}
+
+/*===========================================================================
+FUNCTION    convert_Ecef_to_Lla
+
+DESCRIPTION
+   Convert ECEF to LLA
+
+DEPENDENCIES
+   NONE
+
+RETURN VALUE
+   NONE
+
+SIDE EFFECTS
+   N/A
+
+===========================================================================*/
+static void convert_Ecef_to_Lla(const LocEcef& pecef, LocLla& plla)
+{
+    double p, r;
+    double EcefA = C_PZ90A;
+    double EcefB = C_PZ90B;
+    double Ecef1Mf;
+    double EcefE2;
+    double Mu;
+    double Smu;
+    double Cmu;
+    double Phi;
+    double Sphi;
+    double N;
+
+    p = sqrt(pecef.X * pecef.X + pecef.Y * pecef.Y);
+    r = sqrt(p * p + pecef.Z * pecef.Z);
+    if (r < 1.0) {
+        plla.lat = 1.0;
+        plla.lon = 1.0;
+        plla.alt = 1.0;
+    }
+    Ecef1Mf = 1.0 - (EcefA - EcefB) / EcefA;
+    EcefE2 = 1.0 - (EcefB * EcefB) / (EcefA * EcefA);
+    if (p > 1.0) {
+        Mu = atan2(pecef.Z * (Ecef1Mf + EcefE2 * EcefA / r), p);
+    } else {
+        if (pecef.Z > 0.0) {
+            Mu = M_PI / 2.0;
+        } else {
+            Mu = -M_PI / 2.0;
+        }
+    }
+    Smu = sin(Mu);
+    Cmu = cos(Mu);
+    Phi = atan2(pecef.Z * Ecef1Mf + EcefE2 * EcefA * Smu * Smu * Smu,
+                Ecef1Mf * (p - EcefE2 * EcefA * Cmu * Cmu * Cmu));
+    Sphi = sin(Phi);
+    N = EcefA / sqrt(1.0 - EcefE2 * Sphi * Sphi);
+    plla.alt = p * cos(Phi) + pecef.Z * Sphi - EcefA * EcefA/N;
+    plla.lat = Phi;
+    if ( p > 1.0) {
+        plla.lon = atan2(pecef.Y, pecef.X);
+    } else {
+        plla.lon = 0.0;
+    }
+}
+
+/*===========================================================================
 FUNCTION    convert_signalType_to_signalId
 
 DESCRIPTION
@@ -608,6 +729,110 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
 }
 
 /*===========================================================================
+FUNCTION    loc_nmea_generate_DTM
+
+DESCRIPTION
+   Generate NMEA DTM sentences generated based on position report
+
+DEPENDENCIES
+   NONE
+
+RETURN VALUE
+   NONE
+
+SIDE EFFECTS
+   N/A
+
+===========================================================================*/
+static void loc_nmea_generate_DTM(const LocLla &ref_lla,
+                                  const LocLla &local_lla,
+                                  char *sentence,
+                                  int bufSize)
+{
+    char* pMarker = sentence;
+    int lengthRemaining = bufSize;
+    int length = 0;
+    int datum_type;
+    char ref_datum[4] = {0};
+    char local_datum[4] = {0};
+    double lla_offset[3] = {0};
+    char latHem, longHem;
+    double latMins, longMins;
+
+
+
+    datum_type = loc_get_datum_type();
+    switch (datum_type) {
+        case LOC_GNSS_DATUM_WGS84:
+            ref_datum[0] = 'W';
+            ref_datum[1] = '8';
+            ref_datum[2] = '4';
+            local_datum[0] = 'P';
+            local_datum[1] = '9';
+            local_datum[2] = '0';
+            break;
+        case LOC_GNSS_DATUM_PZ90:
+            ref_datum[0] = 'P';
+            ref_datum[1] = '9';
+            ref_datum[2] = '0';
+            local_datum[0] = 'W';
+            local_datum[1] = '8';
+            local_datum[2] = '4';
+            break;
+        default:
+            break;
+    }
+    length = snprintf(pMarker , lengthRemaining , "$GPDTM,%s,," , local_datum);
+    if (length < 0 || length >= lengthRemaining) {
+        LOC_LOGE("NMEA Error in string formatting");
+        return;
+    }
+    pMarker += length;
+    lengthRemaining -= length;
+
+    lla_offset[0] = local_lla.lat - ref_lla.lat;
+    lla_offset[1] = fmod(local_lla.lon - ref_lla.lon, 360.0);
+    if (lla_offset[1] < -180.0) {
+        lla_offset[1] += 360.0;
+    } else if ( lla_offset[1] > 180.0) {
+        lla_offset[1] -= 360.0;
+    }
+    lla_offset[2] = local_lla.alt - ref_lla.alt;
+    if (lla_offset[0] > 0.0) {
+        latHem = 'N';
+    } else {
+        latHem = 'S';
+        lla_offset[0] *= -1.0;
+    }
+    latMins = fmod(lla_offset[0] * 60.0, 60.0);
+    if (lla_offset[1] < 0.0) {
+        longHem = 'W';
+        lla_offset[1] *= -1.0;
+    }else {
+        longHem = 'E';
+    }
+    longMins = fmod(lla_offset[1] * 60.0, 60.0);
+    length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,%.3lf,",
+                     (uint8_t)floor(lla_offset[0]), latMins, latHem,
+                     (uint8_t)floor(lla_offset[1]), longMins, longHem, lla_offset[2]);
+    if (length < 0 || length >= lengthRemaining) {
+        LOC_LOGE("NMEA Error in string formatting");
+        return;
+    }
+    pMarker += length;
+    lengthRemaining -= length;
+    length = snprintf(pMarker , lengthRemaining , "%s" , ref_datum);
+    if (length < 0 || length >= lengthRemaining) {
+        LOC_LOGE("NMEA Error in string formatting");
+        return;
+    }
+    pMarker += length;
+    lengthRemaining -= length;
+
+    length = loc_nmea_put_checksum(sentence, bufSize);
+}
+
+/*===========================================================================
 FUNCTION    getUtcTimeWithLeapSecondTransition
 
 DESCRIPTION
@@ -726,6 +951,9 @@ void loc_nmea_generate_pos(const UlpLocation &location,
     }
 
     char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
+    char sentence_DTM[NMEA_SENTENCE_MAX_LENGTH] = {0};
+    char sentence_RMC[NMEA_SENTENCE_MAX_LENGTH] = {0};
+    char sentence_GGA[NMEA_SENTENCE_MAX_LENGTH] = {0};
     char* pMarker = sentence;
     int lengthRemaining = sizeof(sentence);
     int length = 0;
@@ -736,6 +964,13 @@ void loc_nmea_generate_pos(const UlpLocation &location,
     int utcMinutes = pTm->tm_min;
     int utcSeconds = pTm->tm_sec;
     int utcMSeconds = (location.gpsLocation.timestamp)%1000;
+    int datum_type = loc_get_datum_type();
+    LocEcef ecef_w84;
+    LocEcef ecef_p90;
+    LocLla  lla_w84;
+    LocLla  lla_p90;
+    LocLla  ref_lla;
+    LocLla  local_lla;
 
     if (inLsTransition) {
         // During leap second transition, we need to display the extra
@@ -904,12 +1139,52 @@ void loc_nmea_generate_pos(const UlpLocation &location,
         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
         nmeaArraystr.push_back(sentence);
 
+        memset(&ecef_w84, 0, sizeof(ecef_w84));
+        memset(&ecef_p90, 0, sizeof(ecef_p90));
+        memset(&lla_w84, 0, sizeof(lla_w84));
+        memset(&lla_p90, 0, sizeof(lla_p90));
+        memset(&ref_lla, 0, sizeof(ref_lla));
+        memset(&local_lla, 0, sizeof(local_lla));
+        lla_w84.lat = location.gpsLocation.latitude / 180.0 * M_PI;
+        lla_w84.lon = location.gpsLocation.longitude / 180.0 * M_PI;
+        lla_w84.alt = location.gpsLocation.altitude;
+
+        convert_Lla_to_Ecef(lla_w84, ecef_w84);
+        convert_WGS84_to_PZ90(ecef_w84, ecef_p90);
+        convert_Ecef_to_Lla(ecef_p90, lla_p90);
+
+        switch (datum_type) {
+            case LOC_GNSS_DATUM_WGS84:
+                ref_lla.lat = location.gpsLocation.latitude;
+                ref_lla.lon = location.gpsLocation.longitude;
+                ref_lla.alt = location.gpsLocation.altitude;
+                local_lla.lat = lla_p90.lat / M_PI * 180.0;
+                local_lla.lon = lla_p90.lon / M_PI * 180.0;
+                local_lla.alt = lla_p90.alt;
+                break;
+            case LOC_GNSS_DATUM_PZ90:
+                ref_lla.lat = lla_p90.lat / M_PI * 180.0;
+                ref_lla.lon = lla_p90.lon / M_PI * 180.0;
+                ref_lla.alt = lla_p90.alt;
+                local_lla.lat = location.gpsLocation.latitude;
+                local_lla.lon = location.gpsLocation.longitude;
+                local_lla.alt = location.gpsLocation.altitude;
+                break;
+            default:
+                break;
+        }
+
+        // -------------------
+        // ------$--DTM-------
+        // -------------------
+        loc_nmea_generate_DTM(ref_lla, local_lla, sentence_DTM, sizeof(sentence_DTM));
+
         // -------------------
         // ------$--RMC-------
         // -------------------
 
-        pMarker = sentence;
-        lengthRemaining = sizeof(sentence);
+        pMarker = sentence_RMC;
+        lengthRemaining = sizeof(sentence_RMC);
 
         length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," ,
                           talker, utcHours, utcMinutes, utcSeconds,utcMSeconds/10);
@@ -924,8 +1199,8 @@ void loc_nmea_generate_pos(const UlpLocation &location,
 
         if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
         {
-            double latitude = location.gpsLocation.latitude;
-            double longitude = location.gpsLocation.longitude;
+            double latitude = ref_lla.lat;
+            double longitude = ref_lla.lon;
             char latHemisphere;
             char lonHemisphere;
             double latMinutes;
@@ -1067,15 +1342,14 @@ void loc_nmea_generate_pos(const UlpLocation &location,
         pMarker += length;
         lengthRemaining -= length;
 
-        length = loc_nmea_put_checksum(sentence, sizeof(sentence));
-        nmeaArraystr.push_back(sentence);
+        length = loc_nmea_put_checksum(sentence_RMC, sizeof(sentence_RMC));
 
         // -------------------
         // ------$--GGA-------
         // -------------------
 
-        pMarker = sentence;
-        lengthRemaining = sizeof(sentence);
+        pMarker = sentence_GGA;
+        lengthRemaining = sizeof(sentence_GGA);
 
         length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," ,
                           talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
@@ -1090,8 +1364,8 @@ void loc_nmea_generate_pos(const UlpLocation &location,
 
         if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
         {
-            double latitude = location.gpsLocation.latitude;
-            double longitude = location.gpsLocation.longitude;
+            double latitude = ref_lla.lat;
+            double longitude = ref_lla.lon;
             char latHemisphere;
             char lonHemisphere;
             double latMinutes;
@@ -1191,15 +1465,26 @@ void loc_nmea_generate_pos(const UlpLocation &location,
             (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL))
         {
             length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,",
-                              location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel);
+                              ref_lla.alt - locationExtended.altitudeMeanSeaLevel);
         }
         else
         {
             length = snprintf(pMarker, lengthRemaining,",,,");
         }
 
-        length = loc_nmea_put_checksum(sentence, sizeof(sentence));
-        nmeaArraystr.push_back(sentence);
+        length = loc_nmea_put_checksum(sentence_GGA, sizeof(sentence_GGA));
+
+        // ------$--DTM-------
+        nmeaArraystr.push_back(sentence_DTM);
+        // ------$--RMC-------
+        nmeaArraystr.push_back(sentence_RMC);
+        if(LOC_GNSS_DATUM_PZ90 == datum_type) {
+            // ------$--DTM-------
+            nmeaArraystr.push_back(sentence_DTM);
+        }
+        // ------$--GGA-------
+        nmeaArraystr.push_back(sentence_GGA);
+
     }
     //Send blank NMEA reports for non-final fixes
     else {
diff --git a/utils/loc_nmea.h b/utils/loc_nmea.h
index fbf88a7..c6c83db 100644
--- a/utils/loc_nmea.h
+++ b/utils/loc_nmea.h
@@ -35,6 +35,43 @@
 #include <string>
 #define NMEA_SENTENCE_MAX_LENGTH 200
 
+/** gnss datum type */
+#define LOC_GNSS_DATUM_WGS84          0
+#define LOC_GNSS_DATUM_PZ90           1
+
+/* len of semi major axis of ref ellips*/
+#define MAJA               (6378137.0)
+/* flattening coef of ref ellipsoid*/
+#define FLAT               (1.0/298.2572235630)
+/* 1st eccentricity squared*/
+#define ESQR               (FLAT*(2.0 - FLAT))
+/*1 minus eccentricity squared*/
+#define OMES               (1.0 - ESQR)
+#define MILARCSEC2RAD      (4.848136811095361e-09)
+/*semi major axis */
+#define C_PZ90A            (6378136.0)
+/*semi minor axis */
+#define C_PZ90B            (6356751.3618)
+/* Transformation from WGS84 to PZ90
+ * Cx,Cy,Cz,Rs,Rx,Ry,Rz,C_SYS_A,C_SYS_B*/
+const double DatumConstFromWGS84[9] =
+        {+0.003, +0.001, 0.000, (1.0+(0.000*1E-6)), (-0.019*MILARCSEC2RAD),
+        (+0.042*MILARCSEC2RAD), (-0.002*MILARCSEC2RAD), C_PZ90A, C_PZ90B};
+
+/** Represents a LTP*/
+typedef struct {
+    double     lat;
+    double     lon;
+    double     alt;
+} LocLla;
+
+/** Represents a ECEF*/
+typedef struct {
+    double     X;
+    double     Y;
+    double     Z;
+} LocEcef;
+
 void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
                               std::vector<std::string> &nmeaArraystr);