MiPad4/android_device_xiaomi_sdm660-common
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

fix nmea GSV/GNS/GSA issues

Browse source 
1. remove blank GSV sentences
2. modify GNS mode indicator field
3. modify GSA mode field, remove blank sentence

Change-Id: Ibe196328938e2674f0100224209cef0b229328e1
CRs-Fixed: 2460129
This commit is contained in:
Hoss Zhou 2019-05-29 15:45:08 +08:00
parent 1e4aefd8b6
commit c3d1f8fb94
1 changed files with 123 additions and 47 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

170
utils/loc_nmea.cpp
View file

@ -36,6 +36,7 @@
#include <loc_cfg.h> #include <loc_cfg.h>
#define GLONASS_SV_ID_OFFSET 64 #define GLONASS_SV_ID_OFFSET 64
#define MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION 64
#define MAX_SATELLITES_IN_USE 12 #define MAX_SATELLITES_IN_USE 12
#define MSEC_IN_ONE_WEEK 604800000ULL #define MSEC_IN_ONE_WEEK 604800000ULL
#define UTC_GPS_OFFSET_MSECS 315964800000ULL #define UTC_GPS_OFFSET_MSECS 315964800000ULL
@ -109,8 +110,9 @@ typedef struct loc_nmea_sv_meta_s
{ {
char talker[3]; char talker[3];
LocGnssConstellationType svType; LocGnssConstellationType svType;
uint32_t mask; uint64_t mask;
uint32_t svCount; uint32_t svCount;
uint32_t totalSvUsedCount;
uint32_t svIdOffset; uint32_t svIdOffset;
uint32_t signalId; uint32_t signalId;
uint32_t systemId; uint32_t systemId;
@ -118,12 +120,12 @@ typedef struct loc_nmea_sv_meta_s
typedef struct loc_sv_cache_info_s typedef struct loc_sv_cache_info_s
{ {
uint32_t gps_used_mask; uint64_t gps_used_mask;
uint32_t glo_used_mask; uint64_t glo_used_mask;
uint32_t gal_used_mask; uint64_t gal_used_mask;
uint32_t qzss_used_mask; uint64_t qzss_used_mask;
uint32_t bds_used_mask; uint64_t bds_used_mask;
uint32_t navic_used_mask; uint64_t navic_used_mask;
uint32_t gps_l1_count; uint32_t gps_l1_count;
uint32_t gps_l5_count; uint32_t gps_l5_count;
uint32_t glo_g1_count; uint32_t glo_g1_count;
@ -337,6 +339,39 @@ static uint32_t convert_signalType_to_signalId(GnssSignalTypeMask signalType)
} }
/*===========================================================================
FUNCTION get_sv_count_from_mask
DESCRIPTION
get the sv count from bit mask
DEPENDENCIES
NONE
RETURN VALUE
value of sv count
SIDE EFFECTS
N/A
===========================================================================*/
static uint32_t get_sv_count_from_mask(uint64_t svMask, int totalSvCount)
{
int index = 0;
uint32_t svCount = 0;
if(totalSvCount > MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION) {
LOC_LOGE("total SV count in this constellation %d exceeded limit %d",
totalSvCount, MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION);
}
for(index = 0; index < totalSvCount; index++) {
if(svMask & 0x1)
svCount += 1;
svMask >>= 1;
}
return svCount;
}
/*=========================================================================== /*===========================================================================
FUNCTION loc_nmea_sv_meta_init FUNCTION loc_nmea_sv_meta_init
@ -438,6 +473,19 @@ static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta,
return NULL; return NULL;
} }
sv_meta.signalId = convert_signalType_to_signalId(signalType); sv_meta.signalId = convert_signalType_to_signalId(signalType);
sv_meta.totalSvUsedCount =
get_sv_count_from_mask(sv_cache_info.gps_used_mask,
GPS_SV_PRN_MAX - GPS_SV_PRN_MIN + 1) +
get_sv_count_from_mask(sv_cache_info.glo_used_mask,
GLO_SV_PRN_MAX - GLO_SV_PRN_MIN + 1) +
get_sv_count_from_mask(sv_cache_info.gal_used_mask,
GAL_SV_PRN_MAX - GAL_SV_PRN_MIN + 1) +
get_sv_count_from_mask(sv_cache_info.qzss_used_mask,
QZSS_SV_PRN_MAX - QZSS_SV_PRN_MIN + 1) +
get_sv_count_from_mask(sv_cache_info.bds_used_mask,
BDS_SV_PRN_MAX - BDS_SV_PRN_MIN + 1) +
get_sv_count_from_mask(sv_cache_info.navic_used_mask,
NAVIC_SV_PRN_MAX - NAVIC_SV_PRN_MIN + 1);
if (needCombine && if (needCombine &&
(sv_cache_info.gps_used_mask ? 1 : 0) + (sv_cache_info.gps_used_mask ? 1 : 0) +
(sv_cache_info.glo_used_mask ? 1 : 0) + (sv_cache_info.glo_used_mask ? 1 : 0) +
@ -532,27 +580,27 @@ static uint32_t loc_nmea_generate_GSA(const GpsLocationExtended &locationExtende
int length = 0; int length = 0;
uint32_t svUsedCount = 0; uint32_t svUsedCount = 0;
uint32_t svUsedList[32] = {0}; uint32_t svUsedList[64] = {0};
char fixType = '\0'; char fixType = '\0';
const char* talker = sv_meta_p->talker; const char* talker = sv_meta_p->talker;
uint32_t svIdOffset = sv_meta_p->svIdOffset; uint32_t svIdOffset = sv_meta_p->svIdOffset;
uint32_t mask = sv_meta_p->mask; uint64_t mask = sv_meta_p->mask;
for (uint8_t i = 1; mask > 0 && svUsedCount < 32; i++) for (uint8_t i = 1; mask > 0 && svUsedCount < 64; i++)
{ {
if (mask & 1) if (mask & 1)
svUsedList[svUsedCount++] = i + svIdOffset; svUsedList[svUsedCount++] = i + svIdOffset;
mask = mask >> 1; mask = mask >> 1;
} }
if (svUsedCount == 0 && GNSS_SV_TYPE_GPS != sv_meta_p->svType) if (svUsedCount == 0)
return 0; return 0;
if (svUsedCount == 0) if (sv_meta_p->totalSvUsedCount == 0)
fixType = '1'; // no fix fixType = '1'; // no fix
else if (svUsedCount <= 3) else if (sv_meta_p->totalSvUsedCount <= 3)
fixType = '2'; // 2D fix fixType = '2'; // 2D fix
else else
fixType = '3'; // 3D fix fixType = '3'; // 3D fix
@ -665,10 +713,7 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
int svCount = sv_meta_p->svCount; int svCount = sv_meta_p->svCount;
if (svCount <= 0) if (svCount <= 0)
{ {
// no svs in view, so just send a blank $--GSV sentence LOC_LOGV("No SV in view for talker ID:%s, signal ID:%X", talker, sv_meta_p->signalId);
snprintf(sentence, lengthRemaining, "$%sGSV,1,1,0,%X", talker, sv_meta_p->signalId);
length = loc_nmea_put_checksum(sentence, bufSize);
nmeaArraystr.push_back(sentence);
return; return;
} }
@ -1035,19 +1080,22 @@ void loc_nmea_generate_pos(const UlpLocation &location,
if (GPS_LOCATION_EXTENDED_HAS_GNSS_SV_USED_DATA & locationExtended.flags) { if (GPS_LOCATION_EXTENDED_HAS_GNSS_SV_USED_DATA & locationExtended.flags) {
sv_cache_info.gps_used_mask = sv_cache_info.gps_used_mask =
(uint32_t)locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask; locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask;
sv_cache_info.glo_used_mask = sv_cache_info.glo_used_mask =
(uint32_t)locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask; locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask;
sv_cache_info.gal_used_mask = sv_cache_info.gal_used_mask =
(uint32_t)locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask; locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask;
sv_cache_info.qzss_used_mask =
(uint32_t)locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask;
sv_cache_info.bds_used_mask = sv_cache_info.bds_used_mask =
(uint32_t)locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask; locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask;
sv_cache_info.qzss_used_mask =
locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask;
sv_cache_info.navic_used_mask =
locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask;
} }
if (generate_nmea) { if (generate_nmea) {
char talker[3] = {'G', 'P', '\0'}; char talker[3] = {'G', 'P', '\0'};
char modeIndicator[7] = {0};
uint32_t svUsedCount = 0; uint32_t svUsedCount = 0;
uint32_t count = 0; uint32_t count = 0;
loc_nmea_sv_meta sv_meta; loc_nmea_sv_meta sv_meta;
@ -1093,13 +1141,12 @@ void loc_nmea_generate_pos(const UlpLocation &location,
talker[1] = sv_meta.talker[1]; talker[1] = sv_meta.talker[1];
} }
// -------------------------- // ----------------------------
// ---$PQGSA/$GNGSA (QZSS)--- // ---$GBGSA/$GNGSA (BEIDOU)---
// -------------------------- // ----------------------------
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS, loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
GNSS_SIGNAL_QZSS_L1CA, true), nmeaArraystr); GNSS_SIGNAL_BEIDOU_B1I, true), nmeaArraystr);
if (count > 0) if (count > 0)
{ {
svUsedCount += count; svUsedCount += count;
@ -1107,12 +1154,13 @@ void loc_nmea_generate_pos(const UlpLocation &location,
talker[1] = sv_meta.talker[1]; talker[1] = sv_meta.talker[1];
} }
// ---------------------------- // --------------------------
// ---$PQGSA/$GNGSA (BEIDOU)--- // ---$GQGSA/$GNGSA (QZSS)---
// ---------------------------- // --------------------------
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence), count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU, loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
GNSS_SIGNAL_BEIDOU_B1I, true), nmeaArraystr); GNSS_SIGNAL_QZSS_L1CA, true), nmeaArraystr);
if (count > 0) if (count > 0)
{ {
svUsedCount += count; svUsedCount += count;
@ -1461,17 +1509,49 @@ void loc_nmea_generate_pos(const UlpLocation &location,
pMarker += length; pMarker += length;
lengthRemaining -= length; lengthRemaining -= length;
if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)) if(!(sv_cache_info.gps_used_mask ? 1 : 0))
// N means no fix modeIndicator[0] = 'N';
length = snprintf(pMarker, lengthRemaining, "%c,", 'N');
else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask) else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
// D means differential modeIndicator[0] = 'D';
length = snprintf(pMarker, lengthRemaining, "%c,", 'D');
else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask) else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
// E means estimated (dead reckoning) modeIndicator[0] = 'E';
length = snprintf(pMarker, lengthRemaining, "%c,", 'E'); else
else // A means autonomous modeIndicator[0] = 'A';
length = snprintf(pMarker, lengthRemaining, "%c,", 'A'); if(!(sv_cache_info.glo_used_mask ? 1 : 0))
modeIndicator[1] = 'N';
else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
modeIndicator[1] = 'E';
else
modeIndicator[1] = 'A';
if(!(sv_cache_info.gal_used_mask ? 1 : 0))
modeIndicator[2] = 'N';
else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
modeIndicator[2] = 'E';
else
modeIndicator[2] = 'A';
if(!(sv_cache_info.bds_used_mask ? 1 : 0))
modeIndicator[3] = 'N';
else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
modeIndicator[3] = 'E';
else
modeIndicator[3] = 'A';
if(!(sv_cache_info.qzss_used_mask ? 1 : 0))
modeIndicator[4] = 'N';
else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
modeIndicator[4] = 'E';
else
modeIndicator[4] = 'A';
if(!(sv_cache_info.navic_used_mask ? 1 : 0))
modeIndicator[5] = 'N';
else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
modeIndicator[5] = 'E';
else
modeIndicator[5] = 'A';
modeIndicator[6] = '\0';
for(int index = 5; index > 0 && 'N' == modeIndicator[index]; index--) {
modeIndicator[index] = '\0';
}
length = snprintf(pMarker, lengthRemaining,"%s,", modeIndicator);
pMarker += length; pMarker += length;
lengthRemaining -= length; lengthRemaining -= length;
@ -1687,10 +1767,6 @@ void loc_nmea_generate_pos(const UlpLocation &location,
length = loc_nmea_put_checksum(sentence, sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence); nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GNGSA,A,1,,,,,,,,,,,,,,,,", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence);
strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence)); strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence));
length = loc_nmea_put_checksum(sentence, sizeof(sentence)); length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(sentence); nmeaArraystr.push_back(sentence);