fix nmea GSV/GNS/GSA issues
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
parent
1e4aefd8b6
commit
c3d1f8fb94
1 changed files with 123 additions and 47 deletions
|
|
@ -36,6 +36,7 @@
|
|||
#include <loc_cfg.h>
|
||||
|
||||
#define GLONASS_SV_ID_OFFSET 64
|
||||
#define MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION 64
|
||||
#define MAX_SATELLITES_IN_USE 12
|
||||
#define MSEC_IN_ONE_WEEK 604800000ULL
|
||||
#define UTC_GPS_OFFSET_MSECS 315964800000ULL
|
||||
|
|
@ -109,8 +110,9 @@ typedef struct loc_nmea_sv_meta_s
|
|||
{
|
||||
char talker[3];
|
||||
LocGnssConstellationType svType;
|
||||
uint32_t mask;
|
||||
uint64_t mask;
|
||||
uint32_t svCount;
|
||||
uint32_t totalSvUsedCount;
|
||||
uint32_t svIdOffset;
|
||||
uint32_t signalId;
|
||||
uint32_t systemId;
|
||||
|
|
@ -118,12 +120,12 @@ typedef struct loc_nmea_sv_meta_s
|
|||
|
||||
typedef struct loc_sv_cache_info_s
|
||||
{
|
||||
uint32_t gps_used_mask;
|
||||
uint32_t glo_used_mask;
|
||||
uint32_t gal_used_mask;
|
||||
uint32_t qzss_used_mask;
|
||||
uint32_t bds_used_mask;
|
||||
uint32_t navic_used_mask;
|
||||
uint64_t gps_used_mask;
|
||||
uint64_t glo_used_mask;
|
||||
uint64_t gal_used_mask;
|
||||
uint64_t qzss_used_mask;
|
||||
uint64_t bds_used_mask;
|
||||
uint64_t navic_used_mask;
|
||||
uint32_t gps_l1_count;
|
||||
uint32_t gps_l5_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
|
||||
|
||||
|
|
@ -438,6 +473,19 @@ static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta,
|
|||
return NULL;
|
||||
}
|
||||
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 &&
|
||||
(sv_cache_info.gps_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;
|
||||
|
||||
uint32_t svUsedCount = 0;
|
||||
uint32_t svUsedList[32] = {0};
|
||||
uint32_t svUsedList[64] = {0};
|
||||
|
||||
char fixType = '\0';
|
||||
|
||||
const char* talker = sv_meta_p->talker;
|
||||
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)
|
||||
svUsedList[svUsedCount++] = i + svIdOffset;
|
||||
mask = mask >> 1;
|
||||
}
|
||||
|
||||
if (svUsedCount == 0 && GNSS_SV_TYPE_GPS != sv_meta_p->svType)
|
||||
if (svUsedCount == 0)
|
||||
return 0;
|
||||
|
||||
if (svUsedCount == 0)
|
||||
if (sv_meta_p->totalSvUsedCount == 0)
|
||||
fixType = '1'; // no fix
|
||||
else if (svUsedCount <= 3)
|
||||
else if (sv_meta_p->totalSvUsedCount <= 3)
|
||||
fixType = '2'; // 2D fix
|
||||
else
|
||||
fixType = '3'; // 3D fix
|
||||
|
|
@ -665,10 +713,7 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
|
|||
int svCount = sv_meta_p->svCount;
|
||||
if (svCount <= 0)
|
||||
{
|
||||
// no svs in view, so just send a blank $--GSV sentence
|
||||
snprintf(sentence, lengthRemaining, "$%sGSV,1,1,0,%X", talker, sv_meta_p->signalId);
|
||||
length = loc_nmea_put_checksum(sentence, bufSize);
|
||||
nmeaArraystr.push_back(sentence);
|
||||
LOC_LOGV("No SV in view for talker ID:%s, signal ID:%X", talker, sv_meta_p->signalId);
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -1035,19 +1080,22 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
|
||||
if (GPS_LOCATION_EXTENDED_HAS_GNSS_SV_USED_DATA & locationExtended.flags) {
|
||||
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 =
|
||||
(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 =
|
||||
(uint32_t)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;
|
||||
locationExtended.gnss_sv_used_ids.gal_sv_used_ids_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) {
|
||||
char talker[3] = {'G', 'P', '\0'};
|
||||
char modeIndicator[7] = {0};
|
||||
uint32_t svUsedCount = 0;
|
||||
uint32_t count = 0;
|
||||
loc_nmea_sv_meta sv_meta;
|
||||
|
|
@ -1093,13 +1141,12 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
talker[1] = sv_meta.talker[1];
|
||||
}
|
||||
|
||||
// --------------------------
|
||||
// ---$PQGSA/$GNGSA (QZSS)---
|
||||
// --------------------------
|
||||
|
||||
// ----------------------------
|
||||
// ---$GBGSA/$GNGSA (BEIDOU)---
|
||||
// ----------------------------
|
||||
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
|
||||
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
|
||||
GNSS_SIGNAL_QZSS_L1CA, true), nmeaArraystr);
|
||||
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
|
||||
GNSS_SIGNAL_BEIDOU_B1I, true), nmeaArraystr);
|
||||
if (count > 0)
|
||||
{
|
||||
svUsedCount += count;
|
||||
|
|
@ -1107,12 +1154,13 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
talker[1] = sv_meta.talker[1];
|
||||
}
|
||||
|
||||
// ----------------------------
|
||||
// ---$PQGSA/$GNGSA (BEIDOU)---
|
||||
// ----------------------------
|
||||
// --------------------------
|
||||
// ---$GQGSA/$GNGSA (QZSS)---
|
||||
// --------------------------
|
||||
|
||||
count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
|
||||
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
|
||||
GNSS_SIGNAL_BEIDOU_B1I, true), nmeaArraystr);
|
||||
loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
|
||||
GNSS_SIGNAL_QZSS_L1CA, true), nmeaArraystr);
|
||||
if (count > 0)
|
||||
{
|
||||
svUsedCount += count;
|
||||
|
|
@ -1461,17 +1509,49 @@ 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');
|
||||
if(!(sv_cache_info.gps_used_mask ? 1 : 0))
|
||||
modeIndicator[0] = 'N';
|
||||
else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
|
||||
// D means differential
|
||||
length = snprintf(pMarker, lengthRemaining, "%c,", 'D');
|
||||
modeIndicator[0] = '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');
|
||||
modeIndicator[0] = 'E';
|
||||
else
|
||||
modeIndicator[0] = '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;
|
||||
lengthRemaining -= length;
|
||||
|
|
@ -1687,10 +1767,6 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
length = loc_nmea_put_checksum(sentence, sizeof(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));
|
||||
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
|
||||
nmeaArraystr.push_back(sentence);
|
||||
|
|
|
|||
Loading…
Reference in a new issue