diff --git a/gnss/GnssAdapter.cpp b/gnss/GnssAdapter.cpp index ef835aee..e22bee09 100644 --- a/gnss/GnssAdapter.cpp +++ b/gnss/GnssAdapter.cpp @@ -380,34 +380,12 @@ GnssAdapter::convertLocationInfo(GnssLocationInfoNotification& out, out.navSolutionMask = locationExtended.navSolutionMask; } if (GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK & locationExtended.flags) { - out.flags |= GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK; + out.flags |= GNSS_LOCATION_INFO_POS_TECH_MASK_BIT; out.posTechMask = locationExtended.tech_mask; } if (GPS_LOCATION_EXTENDED_HAS_POS_DYNAMICS_DATA & locationExtended.flags) { - out.flags |= GPS_LOCATION_EXTENDED_HAS_POS_DYNAMICS_DATA; - if (locationExtended.bodyFrameData.bodyFrameDatamask & - LOCATION_NAV_DATA_HAS_LONG_ACCEL_BIT) { - out.bodyFrameData.bodyFrameDataMask |= LOCATION_NAV_DATA_HAS_LONG_ACCEL_BIT; - } - if (locationExtended.bodyFrameData.bodyFrameDatamask & - LOCATION_NAV_DATA_HAS_LAT_ACCEL_BIT) { - out.bodyFrameData.bodyFrameDataMask |= LOCATION_NAV_DATA_HAS_LAT_ACCEL_BIT; - } - if (locationExtended.bodyFrameData.bodyFrameDatamask & - LOCATION_NAV_DATA_HAS_VERT_ACCEL_BIT) { - out.bodyFrameData.bodyFrameDataMask |= LOCATION_NAV_DATA_HAS_VERT_ACCEL_BIT; - } - if (locationExtended.bodyFrameData.bodyFrameDatamask & LOCATION_NAV_DATA_HAS_YAW_RATE_BIT) { - out.bodyFrameData.bodyFrameDataMask |= LOCATION_NAV_DATA_HAS_YAW_RATE_BIT; - } - if (locationExtended.bodyFrameData.bodyFrameDatamask & LOCATION_NAV_DATA_HAS_PITCH_BIT) { - out.bodyFrameData.bodyFrameDataMask |= LOCATION_NAV_DATA_HAS_PITCH_BIT; - } - out.bodyFrameData.longAccel = locationExtended.bodyFrameData.longAccel; - out.bodyFrameData.latAccel = locationExtended.bodyFrameData.latAccel; - out.bodyFrameData.vertAccel = locationExtended.bodyFrameData.vertAccel; - out.bodyFrameData.yawRate = locationExtended.bodyFrameData.yawRate; - out.bodyFrameData.pitch = locationExtended.bodyFrameData.pitch; + out.flags |= GNSS_LOCATION_INFO_POS_DYNAMICS_DATA_BIT; + out.bodyFrameData = locationExtended.bodyFrameData; } // Validity of this structure is established from the timeSrc of the GnssSystemTime structure. diff --git a/location/LocationDataTypes.h b/location/LocationDataTypes.h index 8bb11cc7..e40dcaab 100644 --- a/location/LocationDataTypes.h +++ b/location/LocationDataTypes.h @@ -119,7 +119,17 @@ typedef enum { LOCATION_NAV_DATA_HAS_LAT_ACCEL_BIT = (1<<1), // Navigation data has Sideward Acceleration LOCATION_NAV_DATA_HAS_VERT_ACCEL_BIT = (1<<2), // Navigation data has Vertical Acceleration LOCATION_NAV_DATA_HAS_YAW_RATE_BIT = (1<<3), // Navigation data has Heading Rate - LOCATION_NAV_DATA_HAS_PITCH_BIT = (1<<4) // Navigation data has Body pitch + LOCATION_NAV_DATA_HAS_PITCH_BIT = (1<<4), // Navigation data has Body pitch + // Navigation data has Forward Acceleration uncertainty + LOCATION_NAV_DATA_HAS_LONG_ACCEL_UNC_BIT = (1<<5), + // Navigation data has Sideward Acceleration uncertainty + LOCATION_NAV_DATA_HAS_LAT_ACCEL_UNC_BIT = (1<<6), + // Navigation data has Vertical Acceleration uncertainty + LOCATION_NAV_DATA_HAS_VERT_ACCEL_UNC_BIT = (1<<7), + // Navigation data has Heading Rate uncertainty + LOCATION_NAV_DATA_HAS_YAW_RATE_UNC_BIT = (1<<8), + // Navigation data has Body pitch uncertainty + LOCATION_NAV_DATA_HAS_PITCH_UNC_BIT = (1<<9) } GnssLocationPosDataBits; typedef uint32_t GnssLocationInfoFlagMask; @@ -658,6 +668,11 @@ typedef struct { float vertAccel; // Vertical Acceleration in body frame (m/s2) float yawRate; // Heading Rate (Radians/second) float pitch; // Body pitch (Radians) + float longAccelUnc; // Uncertainty of Forward Acceleration in body frame + float latAccelUnc; // Uncertainty of Side-ward Acceleration in body frame + float vertAccelUnc; // Uncertainty of Vertical Acceleration in body frame + float yawRateUnc; // Uncertainty of Heading Rate + float pitchUnc; // Uncertainty of Body pitch } GnssLocationPositionDynamics; typedef struct { diff --git a/utils/gps_extended_c.h b/utils/gps_extended_c.h index 5f9dead9..aff87e96 100644 --- a/utils/gps_extended_c.h +++ b/utils/gps_extended_c.h @@ -353,28 +353,6 @@ typedef uint32_t LocNavSolutionMask; /**< Bitmask to specify whether Position Report is PPP corrected */ #define LOC_NAV_MASK_PPP_CORRECTION ((LocNavSolutionMask)0x0040) -typedef uint32_t LocPosDataMask; -/* Bitmask to specify whether Navigation data has Forward Acceleration */ -#define LOC_NAV_DATA_HAS_LONG_ACCEL ((LocPosDataMask)0x0001) -/* Bitmask to specify whether Navigation data has Sideward Acceleration */ -#define LOC_NAV_DATA_HAS_LAT_ACCEL ((LocPosDataMask)0x0002) -/* Bitmask to specify whether Navigation data has Vertical Acceleration */ -#define LOC_NAV_DATA_HAS_VERT_ACCEL ((LocPosDataMask)0x0004) -/* Bitmask to specify whether Navigation data has Heading Rate */ -#define LOC_NAV_DATA_HAS_YAW_RATE ((LocPosDataMask)0x0008) -/* Bitmask to specify whether Navigation data has Body pitch */ -#define LOC_NAV_DATA_HAS_PITCH ((LocPosDataMask)0x0010) -/* Bitmask to specify whether Navigation data has Forward Acceleration Unc */ -#define LOC_NAV_DATA_HAS_LONG_ACCEL_UNC ((LocPosDataMask)0x0020) -/* Bitmask to specify whether Navigation data has Sideward Acceleration Unc*/ -#define LOC_NAV_DATA_HAS_LAT_ACCEL_UNC ((LocPosDataMask)0x0040) -/* Bitmask to specify whether Navigation data has Vertical Acceleration Unc*/ -#define LOC_NAV_DATA_HAS_VERT_ACCEL_UNC ((LocPosDataMask)0x0080) -/* Bitmask to specify whether Navigation data has Heading Rate Unc*/ -#define LOC_NAV_DATA_HAS_YAW_RATE_UNC ((LocPosDataMask)0x0100) -/* Bitmask to specify whether Navigation data has Body pitch Unc*/ -#define LOC_NAV_DATA_HAS_PITCH_UNC ((LocPosDataMask)0x0200) - /** GPS PRN Range */ #define GPS_SV_PRN_MIN 1 #define GPS_SV_PRN_MAX 32 @@ -421,32 +399,6 @@ typedef struct { uint64_t qzss_sv_used_ids_mask; } GnssSvUsedInPosition; -/* Body Frame parameters */ -typedef struct { - /** Contains Body frame LocPosDataMask bits. */ - uint32_t bodyFrameDatamask; - /* Forward Acceleration in body frame (m/s2)*/ - float longAccel; - /** Uncertainty of Forward Acceleration in body frame */ - float longAccelUnc; - /* Sideward Acceleration in body frame (m/s2)*/ - float latAccel; - /** Uncertainty of Side-ward Acceleration in body frame */ - float latAccelUnc; - /* Vertical Acceleration in body frame (m/s2)*/ - float vertAccel; - /** Uncertainty of Vertical Acceleration in body frame */ - float vertAccelUnc; - /* Heading Rate (Radians/second) */ - float yawRate; - /** Uncertainty of Heading Rate */ - float yawRateUnc; - /* Body pitch (Radians) */ - float pitch; - /** Uncertainty of Body pitch */ - float pitchRadUnc; -}LocPositionDynamics; - typedef struct { /** Position dilution of precision. @@ -636,7 +588,7 @@ typedef struct { /** SV Info source used in computing this fix */ LocSvInfoSource sv_source; /** Body Frame Dynamics: 4wayAcceleration and pitch set with validity */ - LocPositionDynamics bodyFrameData; + GnssLocationPositionDynamics bodyFrameData; /** GPS Time */ GPSTimeStruct gpsTime; GnssSystemTime gnssSystemTime;