Merge "GnssAdapter: add support for ppe fix type and calibration info"

4 files changed, 155 insertions, 49 deletions

diff --git a/gnss/GnssAdapter.cpp b/gnss/GnssAdapter.cpp
index a2a507a..747ddf9 100644
--- a/gnss/GnssAdapter.cpp
+++ b/gnss/GnssAdapter.cpp
@@ -475,6 +475,16 @@ GnssAdapter::convertLocationInfo(GnssLocationInfoNotification& out,
         out.flags |= GNSS_LOCATION_INFO_TIME_UNC_BIT;
         out.timeUncMs = locationExtended.timeUncMs;
     }
+
+    if (GPS_LOCATION_EXTENDED_HAS_CALIBRATION_CONFIDENCE & locationExtended.flags) {
+        out.flags |= GNSS_LOCATION_INFO_CALIBRATION_CONFIDENCE_BIT;
+        out.calibrationConfidence = locationExtended.calibrationConfidence;
+    }
+
+    if (GPS_LOCATION_EXTENDED_HAS_CALIBRATION_STATUS & locationExtended.flags) {
+        out.flags |= GNSS_LOCATION_INFO_CALIBRATION_STATUS_BIT;
+        out.calibrationStatus = locationExtended.calibrationStatus;
+    }
 }
 
 
diff --git a/location/LocationDataTypes.h b/location/LocationDataTypes.h
index 406a75c..0d0753f 100644
--- a/location/LocationDataTypes.h
+++ b/location/LocationDataTypes.h
@@ -172,8 +172,9 @@ typedef enum {
     GNSS_LOCATION_INFO_UP_VEL_UNC_BIT                   = (1<<21),// valid Up Velocity Uncertainty
     GNSS_LOCATION_INFO_LEAP_SECONDS_BIT                 = (1<<22),// valid leap seconds
     GNSS_LOCATION_INFO_TIME_UNC_BIT                     = (1<<23),// valid time uncertainty
-    GNSS_LOCATION_INFO_NUM_SV_USED_IN_POSITION_BIT      = (1<<24) // number of SV used in position
-
+    GNSS_LOCATION_INFO_NUM_SV_USED_IN_POSITION_BIT      = (1<<24), // number of SV used in position
+    GNSS_LOCATION_INFO_CALIBRATION_CONFIDENCE_BIT       = (1<<25), // valid sensor cal confidence
+    GNSS_LOCATION_INFO_CALIBRATION_STATUS_BIT           = (1<<26), // valid sensor cal status
 } GnssLocationInfoFlagBits;
 
 typedef enum {
@@ -601,6 +602,8 @@ typedef enum
 {
     GNSS_LOC_SV_SYSTEM_UNKNOWN                = 0,
     /** unknown sv system. */
+    GNSS_LOC_SV_SYSTEM_MIN                    = 1,
+    /**< Min enum of valid SV system. */
     GNSS_LOC_SV_SYSTEM_GPS                    = 1,
     /**< GPS satellite. */
     GNSS_LOC_SV_SYSTEM_GALILEO                = 2,
@@ -613,8 +616,10 @@ typedef enum
     /**< BDS satellite. */
     GNSS_LOC_SV_SYSTEM_QZSS                   = 6,
     /**< QZSS satellite. */
-    GNSS_LOC_SV_SYSTEM_NAVIC                  = 7
+    GNSS_LOC_SV_SYSTEM_NAVIC                  = 7,
     /**< QZSS satellite. */
+    GNSS_LOC_SV_SYSTEM_MAX                    = 7,
+    /**< Max enum of valid SV system. */
 } Gnss_LocSvSystemEnumType;
 
 typedef enum {
@@ -695,6 +700,20 @@ typedef struct {
     GnssAidingDataCommon common; // common aiding data
 } GnssAidingData;
 
+typedef uint16_t DrCalibrationStatusMask;
+typedef enum {
+    // Indicate that roll calibration is needed. Need to take more turns on level ground
+    DR_ROLL_CALIBRATION_NEEDED  = (1<<0),
+    // Indicate that pitch calibration is needed. Need to take more turns on level ground
+    DR_PITCH_CALIBRATION_NEEDED = (1<<1),
+    // Indicate that yaw calibration is needed. Need to accelerate in a straight line
+    DR_YAW_CALIBRATION_NEEDED   = (1<<2),
+    // Indicate that odo calibration is needed. Need to accelerate in a straight line
+    DR_ODO_CALIBRATION_NEEDED   = (1<<3),
+    // Indicate that gyro calibration is needed. Need to take more turns on level ground
+    DR_GYRO_CALIBRATION_NEEDED  = (1<<4)
+} DrCalibrationStatusBits;
+
 typedef struct {
     uint32_t size;           // set to sizeof(Location)
     LocationFlagsMask flags; // bitwise OR of LocationFlagsBits to mark which params are valid
@@ -975,6 +994,9 @@ typedef struct {
     GnssMeasUsageInfo measUsageInfo[GNSS_SV_MAX]; // GNSS Measurement Usage info
     uint8_t leapSeconds;                          // leap second
     float timeUncMs;                              // Time uncertainty in milliseconds
+    uint8_t calibrationConfidence;                // Sensor calibration confidence percent,
+                                                  // in range of [0, 100]
+    DrCalibrationStatusMask calibrationStatus;    // Sensor calibration status
 } GnssLocationInfoNotification;
 
 typedef struct {
diff --git a/utils/gps_extended_c.h b/utils/gps_extended_c.h
index 1ccfc46..d1a2811 100644
--- a/utils/gps_extended_c.h
+++ b/utils/gps_extended_c.h
@@ -372,15 +372,19 @@ typedef uint64_t GpsLocationExtendedFlags;
 /** GpsLocationExtended has Clock drift*/
 #define GPS_LOCATION_EXTENDED_HAS_CLOCK_DRIFT   0x20000000
 /** GpsLocationExtended has Clock drift std deviation**/
-#define GPS_LOCATION_EXTENDED_HAS_CLOCK_DRIFT_STD_DEV   0x40000000
+#define GPS_LOCATION_EXTENDED_HAS_CLOCK_DRIFT_STD_DEV    0x40000000
 /** GpsLocationExtended has leap seconds **/
-#define GPS_LOCATION_EXTENDED_HAS_LEAP_SECONDS   0x80000000
+#define GPS_LOCATION_EXTENDED_HAS_LEAP_SECONDS           0x80000000
 /** GpsLocationExtended has time uncertainty **/
-#define GPS_LOCATION_EXTENDED_HAS_TIME_UNC   0x100000000
+#define GPS_LOCATION_EXTENDED_HAS_TIME_UNC               0x100000000
 /** GpsLocationExtended has heading rate  **/
-#define GPS_LOCATION_EXTENDED_HAS_HEADING_RATE 0x200000000
+#define GPS_LOCATION_EXTENDED_HAS_HEADING_RATE           0x200000000
 /** GpsLocationExtended has multiband signals  **/
-#define GPS_LOCATION_EXTENDED_HAS_MULTIBAND 0x400000000
+#define GPS_LOCATION_EXTENDED_HAS_MULTIBAND              0x400000000
+/** GpsLocationExtended has sensor calibration confidence */
+#define GPS_LOCATION_EXTENDED_HAS_CALIBRATION_CONFIDENCE 0x800000000
+/** GpsLocationExtended has sensor calibration status */
+#define GPS_LOCATION_EXTENDED_HAS_CALIBRATION_STATUS     0x1000000000
 
 typedef uint32_t LocNavSolutionMask;
 /* Bitmask to specify whether SBAS ionospheric correction is used  */
@@ -397,6 +401,8 @@ typedef uint32_t LocNavSolutionMask;
 #define LOC_NAV_MASK_RTK_CORRECTION ((LocNavSolutionMask)0x0020)
 /**<  Bitmask to specify whether Position Report is PPP corrected   */
 #define LOC_NAV_MASK_PPP_CORRECTION ((LocNavSolutionMask)0x0040)
+/**<  Bitmask to specify whether Position Report is RTK fixed corrected   */
+#define LOC_NAV_MASK_RTK_FIXED_CORRECTION ((LocNavSolutionMask)0x0080)
 
 typedef uint32_t LocPosDataMask;
 /* Bitmask to specify whether Navigation data has Forward Acceleration  */
@@ -792,6 +798,9 @@ typedef struct {
         Range: 0 to 359.999. 946
         Unit: Degrees per Seconds */
     float headingRateDeg;
+    /** Sensor calibration confidence percent. Range: 0 - 100 */
+    uint8_t calibrationConfidence;
+    DrCalibrationStatusMask calibrationStatus;
 } GpsLocationExtended;
 
 enum loc_sess_status {
diff --git a/utils/loc_nmea.cpp b/utils/loc_nmea.cpp
index abcf5b7..12f6ca6 100644
--- a/utils/loc_nmea.cpp
+++ b/utils/loc_nmea.cpp
@@ -880,7 +880,7 @@ static void loc_nmea_generate_DTM(const LocLla &ref_lla,
 }
 
 /*===========================================================================
-FUNCTION    getUtcTimeWithLeapSecondTransition
+FUNCTION    get_utctime_with_leapsecond_transition
 
 DESCRIPTION
    This function returns true if the position report is generated during
@@ -900,10 +900,12 @@ SIDE EFFECTS
    N/A
 
 ===========================================================================*/
-bool getUtcTimeWithLeapSecondTransition(const UlpLocation &location,
-                                        const GpsLocationExtended &locationExtended,
-                                        const LocationSystemInfo &systemInfo,
-                                        LocGpsUtcTime &utcPosTimestamp) {
+static bool get_utctime_with_leapsecond_transition(
+        const UlpLocation &location,
+        const GpsLocationExtended &locationExtended,
+        const LocationSystemInfo &systemInfo,
+        LocGpsUtcTime &utcPosTimestamp)
+{
     bool inTransition = false;
 
     // position report is not generated during leap second transition,
@@ -953,6 +955,94 @@ bool getUtcTimeWithLeapSecondTransition(const UlpLocation &location,
 }
 
 /*===========================================================================
+FUNCTION    loc_nmea_get_fix_quality
+
+DESCRIPTION
+   This function obtains the fix quality for GGA sentence, mode indicator
+   for RMC and VTG sentence based on nav solution mask and tech mask in
+   the postion report.
+
+DEPENDENCIES
+   NONE
+
+Output parameter
+   ggaGpsQuality: gps quality field in GGA sentence
+   rmcModeIndicator: mode indicator field in RMC sentence
+   vtgModeIndicator: mode indicator field in VTG sentence
+
+SIDE EFFECTS
+   N/A
+
+===========================================================================*/
+static void loc_nmea_get_fix_quality(const UlpLocation & location,
+                                     const GpsLocationExtended & locationExtended,
+                                     char & ggaGpsQuality,
+                                     char & rmcModeIndicator,
+                                     char & vtgModeIndicator) {
+
+    ggaGpsQuality = '0';
+    rmcModeIndicator = 'N';
+    vtgModeIndicator = 'N';
+
+    do {
+        if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)){
+            ggaGpsQuality = '0'; // 0 means no fix
+            rmcModeIndicator = 'N';
+            vtgModeIndicator = 'N';
+            break;
+        }
+        // NOTE: Order of the check is important
+        if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_NAV_SOLUTION_MASK) {
+            if (LOC_NAV_MASK_PPP_CORRECTION & locationExtended.navSolutionMask) {
+                ggaGpsQuality = '2';    // 2 means DGPS fix
+                rmcModeIndicator = 'P'; // P means precise
+                vtgModeIndicator = 'P'; // P means precise
+                break;
+            } else if (LOC_NAV_MASK_RTK_FIXED_CORRECTION & locationExtended.navSolutionMask){
+                ggaGpsQuality = '4';    // 4 means RTK Fixed fix
+                rmcModeIndicator = 'R'; // use R (RTK fixed)
+                vtgModeIndicator = 'D'; // use D (differential) as
+                                        // no RTK fixed defined for VTG in NMEA 183 spec
+                break;
+            } else if (LOC_NAV_MASK_RTK_CORRECTION & locationExtended.navSolutionMask){
+                ggaGpsQuality = '5';    // 5 means RTK float fix
+                rmcModeIndicator = 'F'; // F means RTK float fix
+                vtgModeIndicator = 'D'; // use D (differential) as
+                                        // no RTK float defined for VTG in NMEA 183 spec
+                break;
+            } else if (LOC_NAV_MASK_DGNSS_CORRECTION & locationExtended.navSolutionMask){
+                ggaGpsQuality = '2';    // 2 means DGPS fix
+                rmcModeIndicator = 'D'; // D means differential
+                vtgModeIndicator = 'D'; // D means differential
+                break;
+            } else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask){
+                ggaGpsQuality = '2';    // 2 means DGPS fix
+                rmcModeIndicator = 'D'; // D means differential
+                vtgModeIndicator = 'D'; // D means differential
+                break;
+            }
+        }
+        // NOTE: Order of the check is important
+        if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) {
+            if (LOC_POS_TECH_MASK_SATELLITE & locationExtended.tech_mask){
+                ggaGpsQuality = '1'; // 1 means GPS
+                rmcModeIndicator = 'A'; // A means autonomous
+                vtgModeIndicator = 'A'; // A means autonomous
+                break;
+            } else if (LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask){
+                ggaGpsQuality = '6'; // 6 means estimated (dead reckoning)
+                rmcModeIndicator = 'E'; // E means estimated (dead reckoning)
+                vtgModeIndicator = 'E'; // E means estimated (dead reckoning)
+                break;
+            }
+        }
+    } while (0);
+
+    LOC_LOGv("gps quality: %c, rmc mode indicator: %c, vtg mode indicator: %c",
+             ggaGpsQuality, rmcModeIndicator, vtgModeIndicator);
+}
+
+/*===========================================================================
 FUNCTION    loc_nmea_generate_pos
 
 DESCRIPTION
@@ -987,7 +1077,7 @@ void loc_nmea_generate_pos(const UlpLocation &location,
     LocGpsUtcTime utcPosTimestamp = 0;
     bool inLsTransition = false;
 
-    inLsTransition = getUtcTimeWithLeapSecondTransition
+    inLsTransition = get_utctime_with_leapsecond_transition
                     (location, locationExtended, systemInfo, utcPosTimestamp);
 
     time_t utcTime(utcPosTimestamp/1000);
@@ -1120,6 +1210,12 @@ void loc_nmea_generate_pos(const UlpLocation &location,
             talker[1] = sv_meta.talker[1];
         }
 
+        char ggaGpsQuality = '0';
+        char rmcModeIndicator = 'N';
+        char vtgModeIndicator = 'N';
+        loc_nmea_get_fix_quality(location, locationExtended,
+                                 ggaGpsQuality, rmcModeIndicator, vtgModeIndicator);
+
         // -------------------
         // ------$--VTG-------
         // -------------------
@@ -1174,17 +1270,7 @@ void loc_nmea_generate_pos(const UlpLocation &location,
         pMarker += length;
         lengthRemaining -= length;
 
-        if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG))
-            // N means no fix
-            length = snprintf(pMarker, lengthRemaining, "%c", 'N');
-        else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
-            // D means differential
-            length = snprintf(pMarker, lengthRemaining, "%c", 'D');
-        else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
-            // E means estimated (dead reckoning)
-            length = snprintf(pMarker, lengthRemaining, "%c", 'E');
-        else // A means autonomous
-            length = snprintf(pMarker, lengthRemaining, "%c", 'A');
+        length = snprintf(pMarker, lengthRemaining, "%c", vtgModeIndicator);
 
         length = loc_nmea_put_checksum(sentence, sizeof(sentence));
         nmeaArraystr.push_back(sentence);
@@ -1372,18 +1458,7 @@ void loc_nmea_generate_pos(const UlpLocation &location,
         pMarker += length;
         lengthRemaining -= length;
 
-        if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG))
-            // N means no fix
-            length = snprintf(pMarker, lengthRemaining, "%c", 'N');
-        else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
-            // D means differential
-            length = snprintf(pMarker, lengthRemaining, "%c", 'D');
-        else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
-            // E means estimated (dead reckoning)
-            length = snprintf(pMarker, lengthRemaining, "%c", 'E');
-        else  // A means autonomous
-            length = snprintf(pMarker, lengthRemaining, "%c", 'A');
-
+        length = snprintf(pMarker, lengthRemaining, "%c", rmcModeIndicator);
         pMarker += length;
         lengthRemaining -= length;
 
@@ -1600,28 +1675,18 @@ void loc_nmea_generate_pos(const UlpLocation &location,
         pMarker += length;
         lengthRemaining -= length;
 
-        char gpsQuality;
-        if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG))
-            gpsQuality = '0'; // 0 means no fix
-        else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
-            gpsQuality = '2'; // 2 means DGPS fix
-        else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
-            gpsQuality = '6'; // 6 means estimated (dead reckoning)
-        else
-            gpsQuality = '1'; // 1 means GPS fix
-
         // Number of satellites in use, 00-12
         if (svUsedCount > MAX_SATELLITES_IN_USE)
             svUsedCount = MAX_SATELLITES_IN_USE;
         if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
         {
             length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,",
-                              gpsQuality, svUsedCount, locationExtended.hdop);
+                              ggaGpsQuality, svUsedCount, locationExtended.hdop);
         }
         else
         {   // no hdop
             length = snprintf(pMarker, lengthRemaining, "%c,%02d,,",
-                              gpsQuality, svUsedCount);
+                              ggaGpsQuality, svUsedCount);
         }
 
         if (length < 0 || length >= lengthRemaining)