kopia lustrzana https://github.com/rs1729/RS
bancroft DOP, PRN32
Zilog80
rodzic
4f6d0683a6
commit
95a7c023cf
223
rs92/nav_gps.c
223
rs92/nav_gps.c
|
|
@ -528,19 +528,19 @@ void GPS_SatellitePosition_Ephem(
|
|||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
int NAV_ClosedFormPositionSolution_FromPseudorange(
|
||||
SAT_t sat1, SAT_t sat2, SAT_t sat3, SAT_t sat4,
|
||||
double* latitude, // ellipsoid latitude [rad]
|
||||
double* longitude, // ellipsoid longitude [rad]
|
||||
double* height, // ellipsoid height [m]
|
||||
double* rx_clock_bias, // receiver clock bias [m]
|
||||
double pos_ecef[3] ) // ECEF coordinates [m]
|
||||
int NAV_ClosedFormSolution_FromPseudorange(
|
||||
SAT_t sats[4], // input: satellite position and pseudorange
|
||||
double* latitude, // output: ellipsoid latitude [rad]
|
||||
double* longitude, // ellipsoid longitude [rad]
|
||||
double* height, // ellipsoid height [m]
|
||||
double* rx_clock_bias, // receiver clock bias [m]
|
||||
double pos_ecef[3] ) // ECEF coordinates [m]
|
||||
{
|
||||
|
||||
double p1 = sat1.pseudorange + sat1.clock_corr; // pseudorange measurement + clock correction [m]
|
||||
double p2 = sat2.pseudorange + sat2.clock_corr;
|
||||
double p3 = sat3.pseudorange + sat3.clock_corr;
|
||||
double p4 = sat4.pseudorange + sat4.clock_corr;
|
||||
double p1 = sats[0].pseudorange + sats[0].clock_corr; // pseudorange measurement + clock correction [m]
|
||||
double p2 = sats[1].pseudorange + sats[1].clock_corr;
|
||||
double p3 = sats[2].pseudorange + sats[2].clock_corr;
|
||||
double p4 = sats[3].pseudorange + sats[3].clock_corr;
|
||||
|
||||
double x1, y1, z1; // Sat1 ECEF
|
||||
double x2, y2, z2; // Sat2 ECEF
|
||||
|
|
@ -548,10 +548,10 @@ int NAV_ClosedFormPositionSolution_FromPseudorange(
|
|||
double x4, y4, z4; // Sat4 ECEF
|
||||
|
||||
// Erdrotation ECEF-ECI, 0.070s*299792458m/s=20985472m, 0.072s*299792458m/s=21585057m
|
||||
rotZ(sat1.X, sat1.Y, sat1.Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x1, &y1, &z1);
|
||||
rotZ(sat2.X, sat2.Y, sat2.Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x2, &y2, &z2);
|
||||
rotZ(sat3.X, sat3.Y, sat3.Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x3, &y3, &z3);
|
||||
rotZ(sat4.X, sat4.Y, sat4.Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x4, &y4, &z4);
|
||||
rotZ(sats[0].X, sats[0].Y, sats[0].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x1, &y1, &z1);
|
||||
rotZ(sats[1].X, sats[1].Y, sats[1].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x2, &y2, &z2);
|
||||
rotZ(sats[2].X, sats[2].Y, sats[2].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x3, &y3, &z3);
|
||||
rotZ(sats[3].X, sats[3].Y, sats[3].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x4, &y4, &z4);
|
||||
|
||||
|
||||
double x12, x13, x14; // 'xij' = 'xi' - 'xj' [m]
|
||||
|
|
@ -656,7 +656,7 @@ int NAV_ClosedFormPositionSolution_FromPseudorange(
|
|||
|
||||
tmp1 = m2*m2 - 4.0*m1*m3;
|
||||
if ( tmp1 < 0 ) { // not good, there is no solution
|
||||
return 0; //FALSE
|
||||
return -1; //FALSE
|
||||
}
|
||||
|
||||
d1 = ( -m2 - sqrt( tmp1 ) ) * 0.5 / m1; // +-Reihenfolge vertauscht
|
||||
|
|
@ -716,18 +716,18 @@ int NAV_ClosedFormPositionSolution_FromPseudorange(
|
|||
pos_ecef[2] = s1.z;
|
||||
|
||||
if( *height < -1500.0 || *height > 50000.0 ) {
|
||||
return 0; //FALSE
|
||||
return -2;
|
||||
}
|
||||
|
||||
return 1; //TRUE
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
int trace_invert(double mat[4][4], double inv[4][4]) // trace-invert
|
||||
/* selected elements from 4x4 matrox inversion */
|
||||
int trace_invert(double mat[4][4], double trinv[4]) // trace-invert
|
||||
/* selected elements from 4x4 matrix inversion */
|
||||
{
|
||||
// Find all NECESSARY 2x2 subdeterminants
|
||||
double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
|
||||
|
|
@ -778,52 +778,48 @@ int trace_invert(double mat[4][4], double inv[4][4]) // trace-invert
|
|||
if (fabs(det) < 0.0001)
|
||||
return -1;
|
||||
|
||||
inv[0][0] = Det3_123_123 / det;
|
||||
//inv[0][0] = Det3_123_123 / det;
|
||||
//inv[0][1] = -Det3_023_123 / det;
|
||||
//inv[0][2] = Det3_013_123 / det;
|
||||
//inv[0][3] = -Det3_012_123 / det;
|
||||
|
||||
//inv[1][0] = -Det3_123_023 / det;
|
||||
inv[1][1] = Det3_023_023 / det;
|
||||
//inv[1][1] = Det3_023_023 / det;
|
||||
//inv[1][2] = -Det3_013_023 / det;
|
||||
//inv[1][3] = Det3_012_023 / det;
|
||||
|
||||
//inv[2][0] = Det3_123_013 / det;
|
||||
//inv[2][1] = -Det3_023_013 / det;
|
||||
inv[2][2] = Det3_013_013 / det;
|
||||
//inv[2][2] = Det3_013_013 / det;
|
||||
//inv[2][3] = -Det3_012_013 / det;
|
||||
|
||||
//inv[3][0] = -Det3_123_012 / det;
|
||||
//inv[3][1] = Det3_023_012 / det;
|
||||
//inv[3][2] = -Det3_013_012 / det;
|
||||
inv[3][3] = Det3_012_012 / det;
|
||||
//inv[3][3] = Det3_012_012 / det;
|
||||
|
||||
trinv[0] = Det3_123_123 / det;
|
||||
|
||||
trinv[1] = Det3_023_023 / det;
|
||||
trinv[2] = Det3_013_013 / det;
|
||||
trinv[3] = Det3_012_012 / det;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int calc_DOP(SAT_t sat0, SAT_t sat1, SAT_t sat2, SAT_t sat3, double pos_ecef[3], double DOP[4][4]) {
|
||||
int calc_DOPn(int n, SAT_t satss[], double pos_ecef[3], double DOP[4]) {
|
||||
int i, j, k;
|
||||
double norm[4], satpos[4][3];
|
||||
double SATS[4][4], AtA[4][4];
|
||||
double norm[n], satpos[n][3];
|
||||
double SATS[n][4], AtA[4][4];
|
||||
|
||||
satpos[0][0] = sat0.X-pos_ecef[0];
|
||||
satpos[0][1] = sat0.Y-pos_ecef[1];
|
||||
satpos[0][2] = sat0.Z-pos_ecef[2];
|
||||
for (i = 0; i < n; i++) {
|
||||
satpos[i][0] = satss[i].X-pos_ecef[0];
|
||||
satpos[i][1] = satss[i].Y-pos_ecef[1];
|
||||
satpos[i][2] = satss[i].Z-pos_ecef[2];
|
||||
}
|
||||
|
||||
satpos[1][0] = sat1.X-pos_ecef[0];
|
||||
satpos[1][1] = sat1.Y-pos_ecef[1];
|
||||
satpos[1][2] = sat1.Z-pos_ecef[2];
|
||||
|
||||
satpos[2][0] = sat2.X-pos_ecef[0];
|
||||
satpos[2][1] = sat2.Y-pos_ecef[1];
|
||||
satpos[2][2] = sat2.Z-pos_ecef[2];
|
||||
|
||||
satpos[3][0] = sat3.X-pos_ecef[0];
|
||||
satpos[3][1] = sat3.Y-pos_ecef[1];
|
||||
satpos[3][2] = sat3.Z-pos_ecef[2];
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
for (i = 0; i < n; i++) {
|
||||
norm[i] = sqrt( satpos[i][0]*satpos[i][0] + satpos[i][1]*satpos[i][1] + satpos[i][2]*satpos[i][2] );
|
||||
for (j = 0; j < 3; j++) {
|
||||
SATS[i][j] = satpos[i][j] / norm[i];
|
||||
|
|
@ -834,7 +830,7 @@ int calc_DOP(SAT_t sat0, SAT_t sat1, SAT_t sat2, SAT_t sat3, double pos_ecef[3],
|
|||
for (i = 0; i < 4; i++) {
|
||||
for (j = 0; j < 4; j++) {
|
||||
AtA[i][j] = 0.0;
|
||||
for (k = 0; k < 4; k++) {
|
||||
for (k = 0; k < n; k++) {
|
||||
AtA[i][j] += SATS[k][i]*SATS[k][j];
|
||||
}
|
||||
}
|
||||
|
|
@ -859,9 +855,8 @@ double lorentz(double a[4], double b[4]) {
|
|||
}
|
||||
|
||||
int matrix_invert(double mat[4][4], double inv[4][4])
|
||||
/* selected elements from 4x4 matrox inversion */
|
||||
{
|
||||
// Find all NECESSARY 2x2 subdeterminants
|
||||
// 2x2 subdeterminants
|
||||
double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
|
||||
double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
|
||||
double Det2_12_03 = mat[1][0] * mat[2][3] - mat[1][3] * mat[2][0];
|
||||
|
|
@ -881,7 +876,7 @@ int matrix_invert(double mat[4][4], double inv[4][4])
|
|||
double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
|
||||
double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
|
||||
|
||||
// Find all NECESSARY 3x3 subdeterminants
|
||||
// 3x3 subdeterminants
|
||||
double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02 + mat[0][2] * Det2_12_01;
|
||||
double Det3_012_013 = mat[0][0] * Det2_12_13 - mat[0][1] * Det2_12_03 + mat[0][3] * Det2_12_01;
|
||||
double Det3_012_023 = mat[0][0] * Det2_12_23 - mat[0][2] * Det2_12_03 + mat[0][3] * Det2_12_02;
|
||||
|
|
@ -899,7 +894,7 @@ int matrix_invert(double mat[4][4], double inv[4][4])
|
|||
double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03 + mat[1][3] * Det2_23_02;
|
||||
double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13 + mat[1][3] * Det2_23_12;
|
||||
|
||||
// Find the 4x4 determinant
|
||||
// 4x4 determinant
|
||||
static double det;
|
||||
det = mat[0][0] * Det3_123_123
|
||||
- mat[0][1] * Det3_123_023
|
||||
|
|
@ -933,7 +928,7 @@ int matrix_invert(double mat[4][4], double inv[4][4])
|
|||
return 0;
|
||||
}
|
||||
|
||||
int NAV_bancroft1(int N, SAT_t sats[], double *X, double *Y, double *Z, double *cc) {
|
||||
int NAV_bancroft1(int N, SAT_t sats[], double pos_ecef[3], double *cc) {
|
||||
|
||||
int i, j, k;
|
||||
double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N];
|
||||
|
|
@ -943,15 +938,16 @@ int NAV_bancroft1(int N, SAT_t sats[], double *X, double *Y, double *Z, double *
|
|||
double Lsg1[4], Lsg2[4];
|
||||
|
||||
double tmp1, tmp2;
|
||||
double X, Y, Z;
|
||||
|
||||
|
||||
if (N < 4 || N > 12) return -1;
|
||||
|
||||
for (i = 0; i < N; i++) {
|
||||
rotZ(sats[i].X, sats[i].Y, sats[i].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, B[i], B[i]+1, B[i]+2);
|
||||
B[i][3] = sats[i].pseudorange + sats[i].clock_corr;
|
||||
}
|
||||
|
||||
if (N < 4 || N > 12) return -1;
|
||||
|
||||
if (N == 4) {
|
||||
matrix_invert(B, BBB);
|
||||
}
|
||||
|
|
@ -1023,15 +1019,18 @@ int NAV_bancroft1(int N, SAT_t sats[], double *X, double *Y, double *Z, double *
|
|||
tmp2 = fabs( tmp2 - 6371000.0 );
|
||||
|
||||
if (tmp1 < tmp2) {
|
||||
*X = Lsg1[0]; *Y = Lsg1[1]; *Z = Lsg1[2]; *cc = Lsg1[3];
|
||||
X = Lsg1[0]; Y = Lsg1[1]; Z = Lsg1[2]; *cc = Lsg1[3];
|
||||
} else {
|
||||
*X = Lsg2[0]; *Y = Lsg2[1]; *Z = Lsg2[2]; *cc = Lsg2[3];
|
||||
X = Lsg2[0]; Y = Lsg2[1]; Z = Lsg2[2]; *cc = Lsg2[3];
|
||||
}
|
||||
pos_ecef[0] = X;
|
||||
pos_ecef[1] = Y;
|
||||
pos_ecef[2] = Z;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int NAV_bancroft2(int N, SAT_t sats[], double *X, double *Y, double *Z, double *cc) {
|
||||
int NAV_bancroft2(int N, SAT_t sats[], double pos_ecef[3], double *cc) {
|
||||
|
||||
int i, j, k;
|
||||
double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N];
|
||||
|
|
@ -1041,15 +1040,16 @@ int NAV_bancroft2(int N, SAT_t sats[], double *X, double *Y, double *Z, double *
|
|||
double Lsg1[4], Lsg2[4];
|
||||
|
||||
double tmp1, tmp2;
|
||||
double X, Y, Z;
|
||||
|
||||
|
||||
if (N < 4 || N > 12) return -1;
|
||||
|
||||
for (i = 0; i < N; i++) {
|
||||
rotE(sats[i], B[i], B[i]+1, B[i]+2);
|
||||
B[i][3] = sats[i].PR;
|
||||
}
|
||||
|
||||
if (N < 4 || N > 12) return -1;
|
||||
|
||||
if (N == 4) {
|
||||
matrix_invert(B, BBB);
|
||||
}
|
||||
|
|
@ -1121,11 +1121,118 @@ int NAV_bancroft2(int N, SAT_t sats[], double *X, double *Y, double *Z, double *
|
|||
tmp2 = fabs( tmp2 - 6371000.0 );
|
||||
|
||||
if (tmp1 < tmp2) {
|
||||
*X = Lsg1[0]; *Y = Lsg1[1]; *Z = Lsg1[2]; *cc = Lsg1[3];
|
||||
X = Lsg1[0]; Y = Lsg1[1]; Z = Lsg1[2]; *cc = Lsg1[3];
|
||||
} else {
|
||||
*X = Lsg2[0]; *Y = Lsg2[1]; *Z = Lsg2[2]; *cc = Lsg2[3];
|
||||
X = Lsg2[0]; Y = Lsg2[1]; Z = Lsg2[2]; *cc = Lsg2[3];
|
||||
}
|
||||
pos_ecef[0] = X;
|
||||
pos_ecef[1] = Y;
|
||||
pos_ecef[2] = Z;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
|
||||
int NAV_bancroft3(int N, SAT_t sats[], double pos_ecef[3], double *cc) {
|
||||
|
||||
int i, j, k;
|
||||
double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N];
|
||||
double a[N], Be[4], Ba[4];
|
||||
|
||||
double q0, q1, q2, p, q, sq, x1, x2;
|
||||
double Lsg1[4], Lsg2[4];
|
||||
|
||||
double tmp1, tmp2;
|
||||
double X, Y, Z;
|
||||
|
||||
|
||||
if (N < 4 || N > 12) return -1;
|
||||
|
||||
for (i = 0; i < N; i++) { // Test: nicht hier rotieren, sondern spaeter Lsg rotieren...
|
||||
rotZ(sats[i].X, sats[i].Y, sats[i].Z, 0.0, B[i], B[i]+1, B[i]+2);
|
||||
B[i][3] = sats[i].PR;
|
||||
}
|
||||
|
||||
if (N == 4) {
|
||||
matrix_invert(B, BBB);
|
||||
}
|
||||
else {
|
||||
for (i = 0; i < 4; i++) {
|
||||
for (j = 0; j < 4; j++) {
|
||||
BtB[i][j] = 0.0;
|
||||
for (k = 0; k < N; k++) {
|
||||
BtB[i][j] += B[k][i]*B[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
matrix_invert(BtB, BBinv);
|
||||
for (i = 0; i < 4; i++) {
|
||||
for (j = 0; j < N; j++) {
|
||||
BBB[i][j] = 0.0;
|
||||
for (k = 0; k < 4; k++) {
|
||||
BBB[i][j] += BBinv[i][k]*B[j][k];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
Be[i] = 0.0;
|
||||
for (k = 0; k < N; k++) {
|
||||
Be[i] += BBB[i][k]*1.0;
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < N; i++) {
|
||||
a[i] = 0.5 * lorentz(B[i], B[i]);
|
||||
}
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
Ba[i] = 0.0;
|
||||
for (k = 0; k < N; k++) {
|
||||
Ba[i] += BBB[i][k]*a[k];
|
||||
}
|
||||
}
|
||||
|
||||
q2 = lorentz(Be, Be);
|
||||
q1 = lorentz(Ba, Be)-1;
|
||||
q0 = lorentz(Ba, Ba);
|
||||
|
||||
if (q2 == 0) return -2;
|
||||
|
||||
p = q1/q2;
|
||||
q = q0/q2;
|
||||
|
||||
sq = p*p - q;
|
||||
if (sq < 0) return -2;
|
||||
|
||||
x1 = -p + sqrt(sq);
|
||||
x2 = -p - sqrt(sq);
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
Lsg1[i] = x1*Be[i]+Ba[i];
|
||||
Lsg2[i] = x2*Be[i]+Ba[i];
|
||||
}
|
||||
Lsg1[3] = -Lsg1[3];
|
||||
Lsg2[3] = -Lsg2[3];
|
||||
|
||||
|
||||
tmp1 = sqrt( Lsg1[0]*Lsg1[0] + Lsg1[1]*Lsg1[1] + Lsg1[2]*Lsg1[2] );
|
||||
tmp2 = sqrt( Lsg2[0]*Lsg2[0] + Lsg2[1]*Lsg2[1] + Lsg2[2]*Lsg2[2] );
|
||||
|
||||
tmp1 = fabs( tmp1 - 6371000.0 );
|
||||
tmp2 = fabs( tmp2 - 6371000.0 );
|
||||
|
||||
if (tmp1 < tmp2) {
|
||||
X = Lsg1[0]; Y = Lsg1[1]; Z = Lsg1[2]; *cc = Lsg1[3];
|
||||
} else {
|
||||
X = Lsg2[0]; Y = Lsg2[1]; Z = Lsg2[2]; *cc = Lsg2[3];
|
||||
}
|
||||
|
||||
rotZ(X, Y, Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, pos_ecef, pos_ecef+1, pos_ecef+2);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -607,7 +607,7 @@ int get_pseudorange() {
|
|||
for (i = 0; i < 4; i++) {
|
||||
gpstime_bytes[i] = xorbyte(pos_GPSTOW + i);
|
||||
}
|
||||
memcpy(&gpstime, gpstime_bytes, 4);
|
||||
memcpy(&gpstime, gpstime_bytes, 4); // GPS-TOW in ms
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
for (j = 0; j < 2; j++) {
|
||||
|
|
@ -616,8 +616,7 @@ int get_pseudorange() {
|
|||
memcpy(&byte16, bytes, 2);
|
||||
prnbits_le(byte16, prn_le+15*i);
|
||||
}
|
||||
|
||||
prn12(prn_le, prns);
|
||||
prn12(prn_le, prns); // PRN-Nummern
|
||||
|
||||
|
||||
if (almanac) calc_satpos_alm(alm, gpstime/1000.0, sat);
|
||||
|
|
@ -628,6 +627,7 @@ int get_pseudorange() {
|
|||
|
||||
for (i = 0; i < 4; i++) { pseudobytes[i] = frame[posGPS_DATA+8*j+i]; }
|
||||
memcpy(&byteval, pseudobytes, 4);
|
||||
if ( prns[j] == 0 && byteval != 0x7FFFFFFF ) prns[j] = 32;
|
||||
range[prns[j]].chips = byteval;
|
||||
|
||||
for (i = 0; i < 4; i++) { pseudobytes[i] = frame[posGPS_DATA+8*j+4+i]; }
|
||||
|
|
@ -641,7 +641,9 @@ int get_pseudorange() {
|
|||
&& (range[prns[j]].chips != 0x55555555 /* && range[prns[j]].ca != 0x555555 */ )
|
||||
)
|
||||
{
|
||||
prn[k++] = prns[j];
|
||||
prn[k] = prns[j];
|
||||
for (i = 0; i < k; i++) { if (prn[i] == prn[k]) break; }
|
||||
k++;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -663,9 +665,9 @@ int get_pseudorange() {
|
|||
return k;
|
||||
}
|
||||
|
||||
double DOP[4][4];
|
||||
double DOP[4];
|
||||
|
||||
int get_GPSkoord(int k) {
|
||||
int get_GPSkoord(int N) {
|
||||
double lat, lon, alt, rx_cl_bias;
|
||||
double pos_ecef[3];
|
||||
double gdop, gdop0 = 1000.0;
|
||||
|
|
@ -673,38 +675,38 @@ int get_GPSkoord(int k) {
|
|||
int i0, i1, i2, i3, j;
|
||||
int nav_ret = 0;
|
||||
int num = 0;
|
||||
double xB, yB, zB, ccB;
|
||||
SAT_t Sat_B[12];
|
||||
SAT_t Sat_A[4];
|
||||
SAT_t Sat_B[12]; // N <= 12
|
||||
|
||||
if (option_vergps == 2) {
|
||||
printf(" sats: ");
|
||||
for (j = 0; j < k; j++) fprintf(stdout, "%2d ", prn[j]);
|
||||
for (j = 0; j < N; j++) fprintf(stdout, "%2d ", prn[j]);
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
gpx.lat = gpx.lon = gpx.h = 0;
|
||||
|
||||
for (i0=0;i0<k;i0++) { for (i1=i0+1;i1<k;i1++) { for (i2=i1+1;i2<k;i2++) { for (i3=i2+1;i3<k;i3++) {
|
||||
for (i0=0;i0<N;i0++) { for (i1=i0+1;i1<N;i1++) { for (i2=i1+1;i2<N;i2++) { for (i3=i2+1;i3<N;i3++) {
|
||||
|
||||
nav_ret = NAV_ClosedFormPositionSolution_FromPseudorange(
|
||||
sat[prn[i0]], sat[prn[i1]], sat[prn[i2]], sat[prn[i3]],
|
||||
&lat, &lon, &alt, &rx_cl_bias,
|
||||
pos_ecef
|
||||
);
|
||||
Sat_A[0] = sat[prn[i0]];
|
||||
Sat_A[1] = sat[prn[i1]];
|
||||
Sat_A[2] = sat[prn[i2]];
|
||||
Sat_A[3] = sat[prn[i3]];
|
||||
nav_ret = NAV_ClosedFormSolution_FromPseudorange( Sat_A, &lat, &lon, &alt, &rx_cl_bias, pos_ecef );
|
||||
|
||||
if (nav_ret != 0) { // not FALSE
|
||||
if (nav_ret == 0) {
|
||||
num += 1;
|
||||
if (calc_DOP(sat[prn[i0]], sat[prn[i1]], sat[prn[i2]], sat[prn[i3]], pos_ecef, DOP) == 0) {
|
||||
gdop = sqrt(DOP[0][0]+DOP[1][1]+DOP[2][2]+DOP[3][3]);
|
||||
if (calc_DOPn(4, Sat_A, pos_ecef, DOP) == 0) {
|
||||
gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]);
|
||||
//printf(" DOP : %.1f ", gdop);
|
||||
if (option_vergps == 2) {
|
||||
//gdop = sqrt(DOP[0][0]+DOP[1][1]+DOP[2][2]+DOP[3][3]);
|
||||
hdop = sqrt(DOP[0][0]+DOP[1][1]);
|
||||
vdop = sqrt(DOP[2][2]);
|
||||
pdop = sqrt(DOP[0][0]+DOP[1][1]+DOP[2][2]);
|
||||
//gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]);
|
||||
hdop = sqrt(DOP[0]+DOP[1]);
|
||||
vdop = sqrt(DOP[2]);
|
||||
pdop = sqrt(DOP[0]+DOP[1]+DOP[2]);
|
||||
if (gdop < dop_limit) {
|
||||
printf(" ");
|
||||
printf("lat: %.5f , lon: %.5f , alt: %7.1f ", lat, lon, alt);
|
||||
printf("lat: %.5f , lon: %.5f , alt: %.1f ", lat, lon, alt);
|
||||
printf(" sats: ");
|
||||
printf("%02d %02d %02d %02d ", prn[i0], prn[i1], prn[i2], prn[i3]);
|
||||
printf(" GDOP : %.1f ", gdop);
|
||||
|
|
@ -730,13 +732,26 @@ int get_GPSkoord(int k) {
|
|||
|
||||
|
||||
if (option_vergps == 2) {
|
||||
for (j = 0; j < k; j++) Sat_B[j] = sat[prn[j]];
|
||||
NAV_bancroft1(k, Sat_B, &xB, &yB, &zB, &ccB);
|
||||
ecef2elli(xB, yB, zB, &lat, &lon, &alt);
|
||||
printf("bancroft1[%d] lat: %.6f , lon: %.6f , alt: %8.2f \n", k, lat, lon, alt);
|
||||
NAV_bancroft2(k, Sat_B, &xB, &yB, &zB, &ccB);
|
||||
ecef2elli(xB, yB, zB, &lat, &lon, &alt);
|
||||
printf("bancroft2[%d] lat: %.6f , lon: %.6f , alt: %8.2f \n", k, lat, lon, alt);
|
||||
|
||||
for (j = 0; j < N; j++) Sat_B[j] = sat[prn[j]];
|
||||
|
||||
NAV_bancroft1(N, Sat_B, pos_ecef, &rx_cl_bias);
|
||||
ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt);
|
||||
printf("bancroft1[%d] lat: %.6f , lon: %.6f , alt: %.2f ", N, lat, lon, alt);
|
||||
printf("\n");
|
||||
|
||||
NAV_bancroft2(N, Sat_B, pos_ecef, &rx_cl_bias);
|
||||
ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt);
|
||||
printf("bancroft2[%d] lat: %.6f , lon: %.6f , alt: %.2f ", N, lat, lon, alt);
|
||||
if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) {
|
||||
gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]);
|
||||
printf(" GDOP[");
|
||||
for (j = 0; j < N; j++) {
|
||||
printf("%d", prn[j]);
|
||||
if (j < N-1) printf(","); else printf("] %.1f ", gdop);
|
||||
}
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -779,8 +794,8 @@ int print_position() { // GPS-Hoehe ueber Ellipsoid
|
|||
if ((almanac || ephem) && (k >= 4)) {
|
||||
if (get_GPSkoord(k) > 0) {
|
||||
fprintf(stdout, " ");
|
||||
if (almanac) fprintf(stdout, " lat: %.4f lon: %.4f h: %7.1f ", gpx.lat, gpx.lon, gpx.h);
|
||||
else fprintf(stdout, " lat: %.5f lon: %.5f h: %7.1f ", gpx.lat, gpx.lon, gpx.h);
|
||||
if (almanac) fprintf(stdout, " lat: %.4f lon: %.4f alt: %.1f ", gpx.lat, gpx.lon, gpx.h);
|
||||
else fprintf(stdout, " lat: %.5f lon: %.5f alt: %.1f ", gpx.lat, gpx.lon, gpx.h);
|
||||
if (option_vergps) {
|
||||
fprintf(stdout, " sats: ");
|
||||
for (j = 0; j < 4; j++) fprintf(stdout, "%02d ", gpx.sats[j]);
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue