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

PZ90 datum support

Browse source 
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 2018-11-27 15:00:57 +08:00
parent 372ec44be0
commit 3203504a28
5 changed files with 368 additions and 14 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

7
etc/gps.conf
View file

@ -95,6 +95,13 @@ CAPABILITIES=0x37
# 3: Enable both LPP_User_Plane and LPP_Control_Plane # 3: Enable both LPP_User_Plane and LPP_Control_Plane
LPP_PROFILE = 2 LPP_PROFILE = 2
####################################
#Datum Type
####################################
# 0: WGS-84
# 1: PZ-90
DATUM_TYPE = 0
################################ ################################
# EXTRA SETTINGS # EXTRA SETTINGS
################################ ################################

25
utils/loc_cfg.cpp
View file

@ -57,12 +57,14 @@
/* Parameter data */ /* Parameter data */
static uint32_t DEBUG_LEVEL = 0xff; static uint32_t DEBUG_LEVEL = 0xff;
static uint32_t TIMESTAMP = 0; static uint32_t TIMESTAMP = 0;
static uint32_t DATUM_TYPE = 0;
/* Parameter spec table */ /* Parameter spec table */
static const loc_param_s_type loc_param_table[] = static const loc_param_s_type loc_param_table[] =
{ {
{"DEBUG_LEVEL", &DEBUG_LEVEL, NULL, 'n'}, {"DEBUG_LEVEL", &DEBUG_LEVEL, NULL, 'n'},
{"TIMESTAMP", &TIMESTAMP, 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); 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_XTWIFI_CONF[] = LOC_PATH_XTWIFI_CONF_STR;
const char LOC_PATH_QUIPC_CONF[] = LOC_PATH_QUIPC_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 FUNCTION loc_set_config_entry

2
utils/loc_cfg.h
View file

@ -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, int loc_read_process_conf(const char* conf_file_name, uint32_t * process_count_ptr,
loc_process_info_s_type** process_info_table_ptr); loc_process_info_s_type** process_info_table_ptr);
int loc_get_datum_type();
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

311
utils/loc_nmea.cpp
View file

@ -33,6 +33,7 @@
#include <math.h> #include <math.h>
#include <log_util.h> #include <log_util.h>
#include <loc_pla.h> #include <loc_pla.h>
#include <loc_cfg.h>
#define GLONASS_SV_ID_OFFSET 64 #define GLONASS_SV_ID_OFFSET 64
#define MAX_SATELLITES_IN_USE 12 #define MAX_SATELLITES_IN_USE 12
@ -113,6 +114,126 @@ typedef struct loc_sv_cache_info_s
float vdop; float vdop;
} loc_sv_cache_info; } 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 FUNCTION convert_signalType_to_signalId
@ -607,6 +728,110 @@ static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
} //while } //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 FUNCTION getUtcTimeWithLeapSecondTransition
@ -726,6 +951,9 @@ void loc_nmea_generate_pos(const UlpLocation &location,
} }
char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0}; 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; char* pMarker = sentence;
int lengthRemaining = sizeof(sentence); int lengthRemaining = sizeof(sentence);
int length = 0; int length = 0;
@ -736,6 +964,13 @@ void loc_nmea_generate_pos(const UlpLocation &location,
int utcMinutes = pTm->tm_min; int utcMinutes = pTm->tm_min;
int utcSeconds = pTm->tm_sec; int utcSeconds = pTm->tm_sec;
int utcMSeconds = (location.gpsLocation.timestamp)%1000; 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) { if (inLsTransition) {
// During leap second transition, we need to display the extra // 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)); length = loc_nmea_put_checksum(sentence, sizeof(sentence));
nmeaArraystr.push_back(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------- // ------$--RMC-------
// ------------------- // -------------------
pMarker = sentence; pMarker = sentence_RMC;
lengthRemaining = sizeof(sentence); lengthRemaining = sizeof(sentence_RMC);
length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," , length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," ,
talker, utcHours, utcMinutes, utcSeconds,utcMSeconds/10); 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) if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
{ {
double latitude = location.gpsLocation.latitude; double latitude = ref_lla.lat;
double longitude = location.gpsLocation.longitude; double longitude = ref_lla.lon;
char latHemisphere; char latHemisphere;
char lonHemisphere; char lonHemisphere;
double latMinutes; double latMinutes;
@ -1067,15 +1342,14 @@ void loc_nmea_generate_pos(const UlpLocation &location,
pMarker += length; pMarker += length;
lengthRemaining -= length; lengthRemaining -= length;
length = loc_nmea_put_checksum(sentence, sizeof(sentence)); length = loc_nmea_put_checksum(sentence_RMC, sizeof(sentence_RMC));
nmeaArraystr.push_back(sentence);
// ------------------- // -------------------
// ------$--GGA------- // ------$--GGA-------
// ------------------- // -------------------
pMarker = sentence; pMarker = sentence_GGA;
lengthRemaining = sizeof(sentence); lengthRemaining = sizeof(sentence_GGA);
length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," , length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," ,
talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10); 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) if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
{ {
double latitude = location.gpsLocation.latitude; double latitude = ref_lla.lat;
double longitude = location.gpsLocation.longitude; double longitude = ref_lla.lon;
char latHemisphere; char latHemisphere;
char lonHemisphere; char lonHemisphere;
double latMinutes; double latMinutes;
@ -1191,15 +1465,26 @@ void loc_nmea_generate_pos(const UlpLocation &location,
(locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)) (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL))
{ {
length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,", length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,",
location.gpsLocation.altitude - locationExtended.altitudeMeanSeaLevel); ref_lla.alt - locationExtended.altitudeMeanSeaLevel);
} }
else else
{ {
length = snprintf(pMarker, lengthRemaining,",,,"); length = snprintf(pMarker, lengthRemaining,",,,");
} }
length = loc_nmea_put_checksum(sentence, sizeof(sentence)); length = loc_nmea_put_checksum(sentence_GGA, sizeof(sentence_GGA));
nmeaArraystr.push_back(sentence);
// ------$--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 //Send blank NMEA reports for non-final fixes
else { else {

37
utils/loc_nmea.h
View file

@ -35,6 +35,43 @@
#include <string> #include <string>
#define NMEA_SENTENCE_MAX_LENGTH 200 #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, void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
std::vector<std::string> &nmeaArraystr); std::vector<std::string> &nmeaArraystr);