PZ90 datum support
add a configuration DATUM_TYPE in gps.conf to configure datum type. Default datum type is WGS84. Change-Id: Ia8404aa5cf6d49741b9f487f8b086abd095f21ea CRs-fixed: 2338883
This commit is contained in:
Hoss Zhou
parent
372ec44be0
commit
3203504a28
5 changed files with 368 additions and 14 deletions
|
|
@ -95,6 +95,13 @@ CAPABILITIES=0x37
|
|||
# 3: Enable both LPP_User_Plane and LPP_Control_Plane
|
||||
LPP_PROFILE = 2
|
||||
|
||||
####################################
|
||||
#Datum Type
|
||||
####################################
|
||||
# 0: WGS-84
|
||||
# 1: PZ-90
|
||||
DATUM_TYPE = 0
|
||||
|
||||
################################
|
||||
# EXTRA SETTINGS
|
||||
################################
|
||||
|
|
|
|||
|
|
@ -57,12 +57,14 @@
|
|||
/* Parameter data */
|
||||
static uint32_t DEBUG_LEVEL = 0xff;
|
||||
static uint32_t TIMESTAMP = 0;
|
||||
static uint32_t DATUM_TYPE = 0;
|
||||
|
||||
/* Parameter spec table */
|
||||
static const loc_param_s_type loc_param_table[] =
|
||||
{
|
||||
{"DEBUG_LEVEL", &DEBUG_LEVEL, NULL, 'n'},
|
||||
{"TIMESTAMP", &TIMESTAMP, NULL, 'n'},
|
||||
{"DATUM_TYPE", &DATUM_TYPE, NULL, 'n'},
|
||||
};
|
||||
static const int loc_param_num = sizeof(loc_param_table) / sizeof(loc_param_s_type);
|
||||
|
||||
|
|
@ -85,6 +87,29 @@ const char LOC_PATH_APDR_CONF[] = LOC_PATH_APDR_CONF_STR;
|
|||
const char LOC_PATH_XTWIFI_CONF[] = LOC_PATH_XTWIFI_CONF_STR;
|
||||
const char LOC_PATH_QUIPC_CONF[] = LOC_PATH_QUIPC_CONF_STR;
|
||||
|
||||
/*===========================================================================
|
||||
FUNCTION loc_get_datum_type
|
||||
|
||||
DESCRIPTION
|
||||
get datum type
|
||||
|
||||
PARAMETERS:
|
||||
N/A
|
||||
|
||||
DEPENDENCIES
|
||||
N/A
|
||||
|
||||
RETURN VALUE
|
||||
DATUM TYPE
|
||||
|
||||
SIDE EFFECTS
|
||||
N/A
|
||||
===========================================================================*/
|
||||
int loc_get_datum_type()
|
||||
{
|
||||
return DATUM_TYPE;
|
||||
}
|
||||
|
||||
/*===========================================================================
|
||||
FUNCTION loc_set_config_entry
|
||||
|
||||
|
|
|
|||
|
|
@ -132,7 +132,7 @@ extern const char LOC_PATH_QUIPC_CONF[];
|
|||
|
||||
int loc_read_process_conf(const char* conf_file_name, uint32_t * process_count_ptr,
|
||||
loc_process_info_s_type** process_info_table_ptr);
|
||||
|
||||
int loc_get_datum_type();
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@
|
|||
#include <math.h>
|
||||
#include <log_util.h>
|
||||
#include <loc_pla.h>
|
||||
#include <loc_cfg.h>
|
||||
|
||||
#define GLONASS_SV_ID_OFFSET 64
|
||||
#define MAX_SATELLITES_IN_USE 12
|
||||
|
|
@ -113,6 +114,126 @@ typedef struct loc_sv_cache_info_s
|
|||
float vdop;
|
||||
} loc_sv_cache_info;
|
||||
|
||||
/*===========================================================================
|
||||
FUNCTION convert_Lla_to_Ecef
|
||||
|
||||
DESCRIPTION
|
||||
Convert LLA to ECEF
|
||||
|
||||
DEPENDENCIES
|
||||
NONE
|
||||
|
||||
RETURN VALUE
|
||||
NONE
|
||||
|
||||
SIDE EFFECTS
|
||||
N/A
|
||||
|
||||
===========================================================================*/
|
||||
static void convert_Lla_to_Ecef(const LocLla& plla, LocEcef& pecef)
|
||||
{
|
||||
double r;
|
||||
|
||||
r = MAJA / sqrt(1.0 - ESQR * sin(plla.lat) * sin(plla.lat));
|
||||
pecef.X = (r + plla.alt) * cos(plla.lat) * cos(plla.lon);
|
||||
pecef.Y = (r + plla.alt) * cos(plla.lat) * sin(plla.lon);
|
||||
pecef.Z = (r * OMES + plla.alt) * sin(plla.lat);
|
||||
}
|
||||
|
||||
/*===========================================================================
|
||||
FUNCTION convert_WGS84_to_PZ90
|
||||
|
||||
DESCRIPTION
|
||||
Convert datum from WGS84 to PZ90
|
||||
|
||||
DEPENDENCIES
|
||||
NONE
|
||||
|
||||
RETURN VALUE
|
||||
NONE
|
||||
|
||||
SIDE EFFECTS
|
||||
N/A
|
||||
|
||||
===========================================================================*/
|
||||
static void convert_WGS84_to_PZ90(const LocEcef& pWGS84, LocEcef& pPZ90)
|
||||
{
|
||||
double deltaX = DatumConstFromWGS84[0];
|
||||
double deltaY = DatumConstFromWGS84[1];
|
||||
double deltaZ = DatumConstFromWGS84[2];
|
||||
double deltaScale = DatumConstFromWGS84[3];
|
||||
double rotX = DatumConstFromWGS84[4];
|
||||
double rotY = DatumConstFromWGS84[5];
|
||||
double rotZ = DatumConstFromWGS84[6];
|
||||
|
||||
pPZ90.X = deltaX + deltaScale * (pWGS84.X + rotZ * pWGS84.Y - rotY * pWGS84.Z);
|
||||
pPZ90.Y = deltaY + deltaScale * (pWGS84.Y - rotZ * pWGS84.X + rotX * pWGS84.Z);
|
||||
pPZ90.Z = deltaZ + deltaScale * (pWGS84.Z + rotY * pWGS84.X - rotX * pWGS84.Y);
|
||||
}
|
||||
|
||||
/*===========================================================================
|
||||
FUNCTION convert_Ecef_to_Lla
|
||||
|
||||
DESCRIPTION
|
||||
Convert ECEF to LLA
|
||||
|
||||
DEPENDENCIES
|
||||
NONE
|
||||
|
||||
RETURN VALUE
|
||||
NONE
|
||||
|
||||
SIDE EFFECTS
|
||||
N/A
|
||||
|
||||
===========================================================================*/
|
||||
static void convert_Ecef_to_Lla(const LocEcef& pecef, LocLla& plla)
|
||||
{
|
||||
double p, r;
|
||||
double EcefA = C_PZ90A;
|
||||
double EcefB = C_PZ90B;
|
||||
double Ecef1Mf;
|
||||
double EcefE2;
|
||||
double Mu;
|
||||
double Smu;
|
||||
double Cmu;
|
||||
double Phi;
|
||||
double Sphi;
|
||||
double N;
|
||||
|
||||
p = sqrt(pecef.X * pecef.X + pecef.Y * pecef.Y);
|
||||
r = sqrt(p * p + pecef.Z * pecef.Z);
|
||||
if (r < 1.0) {
|
||||
plla.lat = 1.0;
|
||||
plla.lon = 1.0;
|
||||
plla.alt = 1.0;
|
||||
}
|
||||
Ecef1Mf = 1.0 - (EcefA - EcefB) / EcefA;
|
||||
EcefE2 = 1.0 - (EcefB * EcefB) / (EcefA * EcefA);
|
||||
if (p > 1.0) {
|
||||
Mu = atan2(pecef.Z * (Ecef1Mf + EcefE2 * EcefA / r), p);
|
||||
} else {
|
||||
if (pecef.Z > 0.0) {
|
||||
Mu = M_PI / 2.0;
|
||||
} else {
|
||||
Mu = -M_PI / 2.0;
|
||||
}
|
||||
}
|
||||
Smu = sin(Mu);
|
||||
Cmu = cos(Mu);
|
||||
Phi = atan2(pecef.Z * Ecef1Mf + EcefE2 * EcefA * Smu * Smu * Smu,
|
||||
Ecef1Mf * (p - EcefE2 * EcefA * Cmu * Cmu * Cmu));
|
||||
Sphi = sin(Phi);
|
||||
N = EcefA / sqrt(1.0 - EcefE2 * Sphi * Sphi);
|
||||
plla.alt = p * cos(Phi) + pecef.Z * Sphi - EcefA * EcefA/N;
|
||||
plla.lat = Phi;
|
||||
if ( p > 1.0) {
|
||||
plla.lon = atan2(pecef.Y, pecef.X);
|
||||
} else {
|
||||
plla.lon = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/*===========================================================================
|
||||
FUNCTION convert_signalType_to_signalId
|
||||
|
||||
|
|
@ -607,6 +728,110 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
|
|||
} //while
|
||||
}
|
||||
|
||||
/*===========================================================================
|
||||
FUNCTION loc_nmea_generate_DTM
|
||||
|
||||
DESCRIPTION
|
||||
Generate NMEA DTM sentences generated based on position report
|
||||
|
||||
DEPENDENCIES
|
||||
NONE
|
||||
|
||||
RETURN VALUE
|
||||
NONE
|
||||
|
||||
SIDE EFFECTS
|
||||
N/A
|
||||
|
||||
===========================================================================*/
|
||||
static void loc_nmea_generate_DTM(const LocLla &ref_lla,
|
||||
const LocLla &local_lla,
|
||||
char *sentence,
|
||||
int bufSize)
|
||||
{
|
||||
char* pMarker = sentence;
|
||||
int lengthRemaining = bufSize;
|
||||
int length = 0;
|
||||
int datum_type;
|
||||
char ref_datum[4] = {0};
|
||||
char local_datum[4] = {0};
|
||||
double lla_offset[3] = {0};
|
||||
char latHem, longHem;
|
||||
double latMins, longMins;
|
||||
|
||||
|
||||
|
||||
datum_type = loc_get_datum_type();
|
||||
switch (datum_type) {
|
||||
case LOC_GNSS_DATUM_WGS84:
|
||||
ref_datum[0] = 'W';
|
||||
ref_datum[1] = '8';
|
||||
ref_datum[2] = '4';
|
||||
local_datum[0] = 'P';
|
||||
local_datum[1] = '9';
|
||||
local_datum[2] = '0';
|
||||
break;
|
||||
case LOC_GNSS_DATUM_PZ90:
|
||||
ref_datum[0] = 'P';
|
||||
ref_datum[1] = '9';
|
||||
ref_datum[2] = '0';
|
||||
local_datum[0] = 'W';
|
||||
local_datum[1] = '8';
|
||||
local_datum[2] = '4';
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
length = snprintf(pMarker , lengthRemaining , "$GPDTM,%s,," , local_datum);
|
||||
if (length < 0 || length >= lengthRemaining) {
|
||||
LOC_LOGE("NMEA Error in string formatting");
|
||||
return;
|
||||
}
|
||||
pMarker += length;
|
||||
lengthRemaining -= length;
|
||||
|
||||
lla_offset[0] = local_lla.lat - ref_lla.lat;
|
||||
lla_offset[1] = fmod(local_lla.lon - ref_lla.lon, 360.0);
|
||||
if (lla_offset[1] < -180.0) {
|
||||
lla_offset[1] += 360.0;
|
||||
} else if ( lla_offset[1] > 180.0) {
|
||||
lla_offset[1] -= 360.0;
|
||||
}
|
||||
lla_offset[2] = local_lla.alt - ref_lla.alt;
|
||||
if (lla_offset[0] > 0.0) {
|
||||
latHem = 'N';
|
||||
} else {
|
||||
latHem = 'S';
|
||||
lla_offset[0] *= -1.0;
|
||||
}
|
||||
latMins = fmod(lla_offset[0] * 60.0, 60.0);
|
||||
if (lla_offset[1] < 0.0) {
|
||||
longHem = 'W';
|
||||
lla_offset[1] *= -1.0;
|
||||
}else {
|
||||
longHem = 'E';
|
||||
}
|
||||
longMins = fmod(lla_offset[1] * 60.0, 60.0);
|
||||
length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,%.3lf,",
|
||||
(uint8_t)floor(lla_offset[0]), latMins, latHem,
|
||||
(uint8_t)floor(lla_offset[1]), longMins, longHem, lla_offset[2]);
|
||||
if (length < 0 || length >= lengthRemaining) {
|
||||
LOC_LOGE("NMEA Error in string formatting");
|
||||
return;
|
||||
}
|
||||
pMarker += length;
|
||||
lengthRemaining -= length;
|
||||
length = snprintf(pMarker , lengthRemaining , "%s" , ref_datum);
|
||||
if (length < 0 || length >= lengthRemaining) {
|
||||
LOC_LOGE("NMEA Error in string formatting");
|
||||
return;
|
||||
}
|
||||
pMarker += length;
|
||||
lengthRemaining -= length;
|
||||
|
||||
length = loc_nmea_put_checksum(sentence, bufSize);
|
||||
}
|
||||
|
||||
/*===========================================================================
|
||||
FUNCTION getUtcTimeWithLeapSecondTransition
|
||||
|
||||
|
|
@ -726,6 +951,9 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
}
|
||||
|
||||
char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
|
||||
char sentence_DTM[NMEA_SENTENCE_MAX_LENGTH] = {0};
|
||||
char sentence_RMC[NMEA_SENTENCE_MAX_LENGTH] = {0};
|
||||
char sentence_GGA[NMEA_SENTENCE_MAX_LENGTH] = {0};
|
||||
char* pMarker = sentence;
|
||||
int lengthRemaining = sizeof(sentence);
|
||||
int length = 0;
|
||||
|
|
@ -736,6 +964,13 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
int utcMinutes = pTm->tm_min;
|
||||
int utcSeconds = pTm->tm_sec;
|
||||
int utcMSeconds = (location.gpsLocation.timestamp)%1000;
|
||||
int datum_type = loc_get_datum_type();
|
||||
LocEcef ecef_w84;
|
||||
LocEcef ecef_p90;
|
||||
LocLla lla_w84;
|
||||
LocLla lla_p90;
|
||||
LocLla ref_lla;
|
||||
LocLla local_lla;
|
||||
|
||||
if (inLsTransition) {
|
||||
// During leap second transition, we need to display the extra
|
||||
|
|
@ -904,12 +1139,52 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
|
||||
nmeaArraystr.push_back(sentence);
|
||||
|
||||
memset(&ecef_w84, 0, sizeof(ecef_w84));
|
||||
memset(&ecef_p90, 0, sizeof(ecef_p90));
|
||||
memset(&lla_w84, 0, sizeof(lla_w84));
|
||||
memset(&lla_p90, 0, sizeof(lla_p90));
|
||||
memset(&ref_lla, 0, sizeof(ref_lla));
|
||||
memset(&local_lla, 0, sizeof(local_lla));
|
||||
lla_w84.lat = location.gpsLocation.latitude / 180.0 * M_PI;
|
||||
lla_w84.lon = location.gpsLocation.longitude / 180.0 * M_PI;
|
||||
lla_w84.alt = location.gpsLocation.altitude;
|
||||
|
||||
convert_Lla_to_Ecef(lla_w84, ecef_w84);
|
||||
convert_WGS84_to_PZ90(ecef_w84, ecef_p90);
|
||||
convert_Ecef_to_Lla(ecef_p90, lla_p90);
|
||||
|
||||
switch (datum_type) {
|
||||
case LOC_GNSS_DATUM_WGS84:
|
||||
ref_lla.lat = location.gpsLocation.latitude;
|
||||
ref_lla.lon = location.gpsLocation.longitude;
|
||||
ref_lla.alt = location.gpsLocation.altitude;
|
||||
local_lla.lat = lla_p90.lat / M_PI * 180.0;
|
||||
local_lla.lon = lla_p90.lon / M_PI * 180.0;
|
||||
local_lla.alt = lla_p90.alt;
|
||||
break;
|
||||
case LOC_GNSS_DATUM_PZ90:
|
||||
ref_lla.lat = lla_p90.lat / M_PI * 180.0;
|
||||
ref_lla.lon = lla_p90.lon / M_PI * 180.0;
|
||||
ref_lla.alt = lla_p90.alt;
|
||||
local_lla.lat = location.gpsLocation.latitude;
|
||||
local_lla.lon = location.gpsLocation.longitude;
|
||||
local_lla.alt = location.gpsLocation.altitude;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// -------------------
|
||||
// ------$--DTM-------
|
||||
// -------------------
|
||||
loc_nmea_generate_DTM(ref_lla, local_lla, sentence_DTM, sizeof(sentence_DTM));
|
||||
|
||||
// -------------------
|
||||
// ------$--RMC-------
|
||||
// -------------------
|
||||
|
||||
pMarker = sentence;
|
||||
lengthRemaining = sizeof(sentence);
|
||||
pMarker = sentence_RMC;
|
||||
lengthRemaining = sizeof(sentence_RMC);
|
||||
|
||||
length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," ,
|
||||
talker, utcHours, utcMinutes, utcSeconds,utcMSeconds/10);
|
||||
|
|
@ -924,8 +1199,8 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
|
||||
if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
|
||||
{
|
||||
double latitude = location.gpsLocation.latitude;
|
||||
double longitude = location.gpsLocation.longitude;
|
||||
double latitude = ref_lla.lat;
|
||||
double longitude = ref_lla.lon;
|
||||
char latHemisphere;
|
||||
char lonHemisphere;
|
||||
double latMinutes;
|
||||
|
|
@ -1067,15 +1342,14 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
pMarker += length;
|
||||
lengthRemaining -= length;
|
||||
|
||||
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
|
||||
nmeaArraystr.push_back(sentence);
|
||||
length = loc_nmea_put_checksum(sentence_RMC, sizeof(sentence_RMC));
|
||||
|
||||
// -------------------
|
||||
// ------$--GGA-------
|
||||
// -------------------
|
||||
|
||||
pMarker = sentence;
|
||||
lengthRemaining = sizeof(sentence);
|
||||
pMarker = sentence_GGA;
|
||||
lengthRemaining = sizeof(sentence_GGA);
|
||||
|
||||
length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," ,
|
||||
talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
|
||||
|
|
@ -1090,8 +1364,8 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
|
||||
if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
|
||||
{
|
||||
double latitude = location.gpsLocation.latitude;
|
||||
double longitude = location.gpsLocation.longitude;
|
||||
double latitude = ref_lla.lat;
|
||||
double longitude = ref_lla.lon;
|
||||
char latHemisphere;
|
||||
char lonHemisphere;
|
||||
double latMinutes;
|
||||
|
|
@ -1191,15 +1465,26 @@ void loc_nmea_generate_pos(const UlpLocation &location,
|
|||
(locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL))
|
||||
{
|
||||
length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,",
|
||||
location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel);
|
||||
ref_lla.alt - locationExtended.altitudeMeanSeaLevel);
|
||||
}
|
||||
else
|
||||
{
|
||||
length = snprintf(pMarker, lengthRemaining,",,,");
|
||||
}
|
||||
|
||||
length = loc_nmea_put_checksum(sentence, sizeof(sentence));
|
||||
nmeaArraystr.push_back(sentence);
|
||||
length = loc_nmea_put_checksum(sentence_GGA, sizeof(sentence_GGA));
|
||||
|
||||
// ------$--DTM-------
|
||||
nmeaArraystr.push_back(sentence_DTM);
|
||||
// ------$--RMC-------
|
||||
nmeaArraystr.push_back(sentence_RMC);
|
||||
if(LOC_GNSS_DATUM_PZ90 == datum_type) {
|
||||
// ------$--DTM-------
|
||||
nmeaArraystr.push_back(sentence_DTM);
|
||||
}
|
||||
// ------$--GGA-------
|
||||
nmeaArraystr.push_back(sentence_GGA);
|
||||
|
||||
}
|
||||
//Send blank NMEA reports for non-final fixes
|
||||
else {
|
||||
|
|
|
|||
|
|
@ -35,6 +35,43 @@
|
|||
#include <string>
|
||||
#define NMEA_SENTENCE_MAX_LENGTH 200
|
||||
|
||||
/** gnss datum type */
|
||||
#define LOC_GNSS_DATUM_WGS84 0
|
||||
#define LOC_GNSS_DATUM_PZ90 1
|
||||
|
||||
/* len of semi major axis of ref ellips*/
|
||||
#define MAJA (6378137.0)
|
||||
/* flattening coef of ref ellipsoid*/
|
||||
#define FLAT (1.0/298.2572235630)
|
||||
/* 1st eccentricity squared*/
|
||||
#define ESQR (FLAT*(2.0 - FLAT))
|
||||
/*1 minus eccentricity squared*/
|
||||
#define OMES (1.0 - ESQR)
|
||||
#define MILARCSEC2RAD (4.848136811095361e-09)
|
||||
/*semi major axis */
|
||||
#define C_PZ90A (6378136.0)
|
||||
/*semi minor axis */
|
||||
#define C_PZ90B (6356751.3618)
|
||||
/* Transformation from WGS84 to PZ90
|
||||
* Cx,Cy,Cz,Rs,Rx,Ry,Rz,C_SYS_A,C_SYS_B*/
|
||||
const double DatumConstFromWGS84[9] =
|
||||
{+0.003, +0.001, 0.000, (1.0+(0.000*1E-6)), (-0.019*MILARCSEC2RAD),
|
||||
(+0.042*MILARCSEC2RAD), (-0.002*MILARCSEC2RAD), C_PZ90A, C_PZ90B};
|
||||
|
||||
/** Represents a LTP*/
|
||||
typedef struct {
|
||||
double lat;
|
||||
double lon;
|
||||
double alt;
|
||||
} LocLla;
|
||||
|
||||
/** Represents a ECEF*/
|
||||
typedef struct {
|
||||
double X;
|
||||
double Y;
|
||||
double Z;
|
||||
} LocEcef;
|
||||
|
||||
void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
|
||||
std::vector<std::string> &nmeaArraystr);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue