mirror
/
RS-tracker
kopia lustrzana https://github.com/rs1729/RS
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Wahl der Lsg nahe Oberflaeche

dump
Zilog80 2015-10-30 14:18:02 +01:00
rodzic cea339868b
commit 43cf3d2f5a
1 zmienionych plików z 54 dodań i 54 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

34
rs92/nav_gps.c
Wyświetl plik

@ -662,12 +662,6 @@ int NAV_ClosedFormPositionSolution_FromPseudorange(
s2.y = c2*d2 - f2 + y1; s2.y = c2*d2 - f2 + y1;
s2.z = c3*d2 - f3 + z1; s2.z = c3*d2 - f3 + z1;
ecef2elli( s1.x, s1.y, s1.z, &lat1, &lon1, &alt1 );
*latitude = lat1;
*longitude = lon1;
*height = alt1;
tmp1 = sqrt( s1.x*s1.x + s1.y*s1.y + s1.z*s1.z ); tmp1 = sqrt( s1.x*s1.x + s1.y*s1.y + s1.z*s1.z );
tmp2 = sqrt( s2.x*s2.x + s2.y*s2.y + s2.z*s2.z ); tmp2 = sqrt( s2.x*s2.x + s2.y*s2.y + s2.z*s2.z );
@ -676,16 +670,19 @@ int NAV_ClosedFormPositionSolution_FromPseudorange(
// the Earth's surface // the Earth's surface
tmp1 = fabs( tmp1 - 6371000.0 ); tmp1 = fabs( tmp1 - 6371000.0 );
tmp2 = fabs( tmp2 - 6371000.0 ); tmp2 = fabs( tmp2 - 6371000.0 );
// geht manchmal schief
// wenn beide Werte nahe Erdoberflaeche // nur (tmp2 < tmp1) geht manchmal schief
// ungefaehre Position kann aus den Positionen der empfangen Sats abgeleitet werden if ( tmp2 < tmp1 && tmp1 >= 60000 ) { // swap solutions
if ( tmp2 < tmp1 /*alt2 < alt1 && alt2 > -1500*/ && tmp1 < 60000 ) stmp = s1; s1 = s2; s2 = stmp; // s1 = s2;
{ dtmp = d1; d1 = d2; d2 = dtmp; // d1 = d2;
x0 = (x1+x2+x3+x4)/4.0; }
y0 = (y1+y2+y3+y4)/4.0; else if ( tmp2 < 60000 ) { // interessant wenn tmp1<tmp2<60k oder tmp2<tmp1<60k,
z0 = (z1+z2+z3+z4)/4.0; x0 = (x1+x2+x3+x4)/4.0; // d.h. beide Werte nahe Erdoberflaeche;
y0 = (y1+y2+y3+y4)/4.0; // ungefaehre Position kann aus den Positionen
z0 = (z1+z2+z3+z4)/4.0; // der empfangen Sats abgeleitet werden
ecef2elli( x0, y0, z0, &latS, &lonS, &altS ); ecef2elli( x0, y0, z0, &latS, &lonS, &altS );
ecef2elli( s1.x, s1.y, s1.z, &lat1, &lon1, &alt1 );
ecef2elli( s2.x, s2.y, s2.z, &lat2, &lon2, &alt2 ); ecef2elli( s2.x, s2.y, s2.z, &lat2, &lon2, &alt2 );
d2_1 = sqrt( (latS-lat1)*(latS-lat1) + (lonS-lon1)*(lonS-lon1) ); d2_1 = sqrt( (latS-lat1)*(latS-lat1) + (lonS-lon1)*(lonS-lon1) );
@ -694,12 +691,15 @@ int NAV_ClosedFormPositionSolution_FromPseudorange(
if ( d2_2 < d2_1 ) { if ( d2_2 < d2_1 ) {
stmp = s1; s1 = s2; s2 = stmp; // s1 = s2; stmp = s1; s1 = s2; s2 = stmp; // s1 = s2;
dtmp = d1; d1 = d2; d2 = dtmp; // d1 = d2; dtmp = d1; d1 = d2; d2 = dtmp; // d1 = d2;
*latitude = lat2;
*longitude = lon2;
*height = alt2;
} }
} }
ecef2elli( s1.x, s1.y, s1.z, &lat1, &lon1, &alt1 );
*latitude = lat1;
*longitude = lon1;
*height = alt1;
*rx_clock_bias = d1; *rx_clock_bias = d1;
pos_ecef[0] = s1.x; pos_ecef[0] = s1.x;