Merge remote-tracking branch 'quic/location.lnx.4.0' into HEAD

Change-Id: I28d5e29a9790ca3f28f44b5c5d9e4c5037c646d8
CRs-Fixed: 2794929

5 files changed, 160 insertions, 12 deletions

diff --git a/utils/gps_extended_c.h b/utils/gps_extended_c.h
index 51afd0c..8e47ef7 100644
--- a/utils/gps_extended_c.h
+++ b/utils/gps_extended_c.h
@@ -115,6 +115,9 @@ typedef uint32_t LocPosTechMask;
 #define LOC_POS_TECH_MASK_AFLT ((LocPosTechMask)0x00000040)
 #define LOC_POS_TECH_MASK_HYBRID ((LocPosTechMask)0x00000080)
 #define LOC_POS_TECH_MASK_PPE ((LocPosTechMask)0x00000100)
+#define LOC_POS_TECH_MASK_VEH ((LocPosTechMask)0x00000200)
+#define LOC_POS_TECH_MASK_VIS ((LocPosTechMask)0x00000400)
+
 
 enum loc_registration_mask_status {
     LOC_REGISTRATION_MASK_ENABLED,
diff --git a/utils/loc_misc_utils.cpp b/utils/loc_misc_utils.cpp
index 7143466..43856c6 100644
--- a/utils/loc_misc_utils.cpp
+++ b/utils/loc_misc_utils.cpp
@@ -32,6 +32,7 @@
 #include <string.h>
 #include <inttypes.h>
 #include <dlfcn.h>
+#include <math.h>
 #include <log_util.h>
 #include <loc_misc_utils.h>
 #include <ctype.h>
@@ -232,3 +233,119 @@ uint64_t getBootTimeMilliSec()
     clock_gettime(CLOCK_BOOTTIME, &curTs);
     return (uint64_t)GET_MSEC_FROM_TS(curTs);
 }
+
+// Used for convert position/velocity from GSNS antenna based to VRP based
+void Matrix_MxV(float a[3][3],  float b[3], float c[3]) {
+    int i, j;
+
+    for (i=0; i<3; i++) {
+        c[i] = 0.0f;
+        for (j=0; j<3; j++)
+            c[i] += a[i][j] * b[j];
+    }
+}
+
+// Used for convert position/velocity from GNSS antenna based to VRP based
+void Matrix_Skew(float a[3], float c[3][3]) {
+    c[0][0] = 0.0f;
+    c[0][1] = -a[2];
+    c[0][2] = a[1];
+    c[1][0] = a[2];
+    c[1][1] = 0.0f;
+    c[1][2] = -a[0];
+    c[2][0] = -a[1];
+    c[2][1] = a[0];
+    c[2][2] = 0.0f;
+}
+
+// Used for convert position/velocity from GNSS antenna based to VRP based
+void Euler2Dcm(float euler[3], float dcm[3][3]) {
+    float cr = 0.0, sr = 0.0, cp = 0.0, sp = 0.0, ch = 0.0, sh = 0.0;
+
+    cr = cosf(euler[0]);
+    sr = sinf(euler[0]);
+    cp = cosf(euler[1]);
+    sp = sinf(euler[1]);
+    ch = cosf(euler[2]);
+    sh = sinf(euler[2]);
+
+    dcm[0][0] = cp * ch;
+    dcm[0][1] = (sp*sr*ch) - (cr*sh);
+    dcm[0][2] = (cr*sp*ch) + (sh*sr);
+
+    dcm[1][0] = cp * sh;
+    dcm[1][1] = (sr*sp*sh) + (cr*ch);
+    dcm[1][2] = (cr*sp*sh) - (sr*ch);
+
+    dcm[2][0] = -sp;
+    dcm[2][1] = sr * cp;
+    dcm[2][2] = cr * cp;
+}
+
+// Used for convert position from GSNS based to VRP based
+// The converted position will be stored in the llaInfo parameter.
+#define A6DOF_WGS_A (6378137.0f)
+#define A6DOF_WGS_B (6335439.0f)
+#define A6DOF_WGS_E2 (0.00669437999014f)
+void loc_convert_lla_gnss_to_vrp(double lla[3], float rollPitchYaw[3],
+                                 float leverArm[3]) {
+    LOC_LOGv("lla: %f, %f, %f, lever arm: %f %f %f, "
+             "rollpitchyaw: %f %f %f",
+             lla[0], lla[1], lla[2],
+             leverArm[0], leverArm[1], leverArm[2],
+             rollPitchYaw[0], rollPitchYaw[1], rollPitchYaw[2]);
+
+    float cnb[3][3];
+    memset(cnb, 0, sizeof(cnb));
+    Euler2Dcm(rollPitchYaw, cnb);
+
+    float sl = sin(lla[0]);
+    float cl = cos(lla[0]);
+    float sf = 1.0f / (1.0f - A6DOF_WGS_E2 * sl* sl);
+    float sfr = sqrtf(sf);
+
+    float rn = A6DOF_WGS_B * sf * sfr + lla[2];
+    float re = A6DOF_WGS_A * sfr + lla[2];
+
+    float deltaNED[3];
+
+    // gps_pos_lla = imu_pos_lla + Cbn*la_b .* [1/geo.Rn; 1/(geo.Re*geo.cL); -1];
+    Matrix_MxV(cnb, leverArm, deltaNED);
+
+    // NED to lla conversion
+    lla[0] = lla[0] + deltaNED[0] / rn;
+    lla[1] = lla[1] + deltaNED[1] / (re * cl);
+    lla[2] = lla[2] - deltaNED[2];
+}
+
+// Used for convert velocity from GSNS based to VRP based
+// The converted velocity will be stored in the enuVelocity parameter.
+void loc_convert_velocity_gnss_to_vrp(float enuVelocity[3], float rollPitchYaw[3],
+                                      float rollPitchYawRate[3], float leverArm[3]) {
+
+    LOC_LOGv("enu velocity: %f, %f, %f, lever arm: %f %f %f, roll pitch yaw: %f %f %f,"
+             "rollpitchyawRate: %f %f %f",
+             enuVelocity[0], enuVelocity[1], enuVelocity[2],
+             leverArm[0], leverArm[1], leverArm[2],
+             rollPitchYaw[0], rollPitchYaw[1], rollPitchYaw[2],
+             rollPitchYawRate[0], rollPitchYawRate[1], rollPitchYawRate[2]);
+
+    float cnb[3][3];
+    memset(cnb, 0, sizeof(cnb));
+    Euler2Dcm(rollPitchYaw, cnb);
+
+    float skewLA[3][3];
+    memset(skewLA, 0, sizeof(skewLA));
+    Matrix_Skew(leverArm, skewLA);
+
+    float tmp[3];
+    float deltaEnuVelocity[3];
+    memset(tmp, 0, sizeof(tmp));
+    memset(deltaEnuVelocity, 0, sizeof(deltaEnuVelocity));
+    Matrix_MxV(skewLA, rollPitchYawRate, tmp);
+    Matrix_MxV(cnb, tmp, deltaEnuVelocity);
+
+    enuVelocity[0] = enuVelocity[0] - deltaEnuVelocity[0];
+    enuVelocity[1] = enuVelocity[1] - deltaEnuVelocity[1];
+    enuVelocity[2] = enuVelocity[2] - deltaEnuVelocity[2];
+}
diff --git a/utils/loc_misc_utils.h b/utils/loc_misc_utils.h
index 796beab..2335e57 100644
--- a/utils/loc_misc_utils.h
+++ b/utils/loc_misc_utils.h
@@ -279,4 +279,43 @@ inline uint64_t qTimerTicksToNanos(double qTimer) {
     return (uint64_t((qTimer * double(10000ull)) / (double)192ull));
 }
 
+/*===========================================================================
+FUNCTION loc_convert_lla_gnss_to_vrp
+
+DESCRIPTION
+   This function converts lat/long/altitude from GNSS antenna based
+   to vehicle reference point based.
+
+DEPENDENCIES
+   N/A
+
+RETURN VALUE
+    The converted lat/long/altitude will be stored in the parameter of llaInfo.
+
+SIDE EFFECTS
+   N/A
+===========================================================================*/
+void loc_convert_lla_gnss_to_vrp(double lla[3], float rollPitchYaw[3],
+                                 float leverArm[3]);
+
+/*===========================================================================
+FUNCTION loc_convert_velocity_gnss_to_vrp
+
+DESCRIPTION
+   This function converts east/north/up velocity from GNSS antenna based
+   to vehicle reference point based.
+
+DEPENDENCIES
+   N/A
+
+RETURN VALUE
+    The converted east/north/up velocity will be stored in the parameter of
+    enuVelocity.
+
+SIDE EFFECTS
+   N/A
+===========================================================================*/
+void loc_convert_velocity_gnss_to_vrp(float enuVelocity[3], float rollPitchYaw[3],
+                                      float rollPitchYawRate[3], float leverArm[3]);
+
 #endif //_LOC_MISC_UTILS_H_
diff --git a/utils/loc_nmea.cpp b/utils/loc_nmea.cpp
index 1cadac4..237910c 100644
--- a/utils/loc_nmea.cpp
+++ b/utils/loc_nmea.cpp
@@ -1083,17 +1083,6 @@ static void loc_nmea_get_fix_quality(const UlpLocation & location,
     memset(gnsModeIndicator, 'N', 6); // N means no fix
     gnsModeIndicator[6] = '\0';
     do {
-        // GGA fix quality is defined in NMEA spec as below:
-        // https://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_GGA.html
-        // Fix quality: 0 = invalid
-        //              1 = GPS fix (SPS)
-        //              2 = DGPS fix
-        //              3 = PPS fix
-        //              4 = Real Time Kinematic
-        //              5 = Float RTK
-        //              6 = estimated (dead reckoning) (2.3 feature)
-        //              7 = Manual input mode
-        //              8 = Simulation mode
         if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)){
             break;
         }
diff --git a/utils/log_util.h b/utils/log_util.h
index 5cdcf25..78ed478 100644
--- a/utils/log_util.h
+++ b/utils/log_util.h
@@ -53,7 +53,7 @@
 #endif /* LOG_TAG */
 
 // LE targets with no logcat support
-#ifdef FEATURE_EXTERNAL_AP
+#if defined(FEATURE_EXTERNAL_AP) || defined(USE_SYSLOG_LOGGING)
 #include <syslog.h>
 #define ALOGE(...) syslog(LOG_ERR,     "LOC_LOGE: " __VA_ARGS__);
 #define ALOGW(...) syslog(LOG_WARNING, "LOC_LOGW: " __VA_ARGS__);