kopia lustrzana https://github.com/rs1729/RS
rs_module: rs92 pseudochips range check
rodzic
4e89733d06
commit
363bc0b427
|
@ -426,27 +426,34 @@ static int rs92_get_pseudorange(rs_data_t *rs_data) {
|
||||||
Sat[j].prn = prns[j];
|
Sat[j].prn = prns[j];
|
||||||
Sat[j].status = sat_status[j];
|
Sat[j].status = sat_status[j];
|
||||||
|
|
||||||
// Pseudorange/chips
|
if (pos_GPS_DATA+8*(j+1) > rs_data->pos) {
|
||||||
pseudobytes = frame+pos_GPS_DATA+8*j;
|
Sat[j].prn = 0;
|
||||||
memcpy(&chipbytes, pseudobytes, 4);
|
|
||||||
|
|
||||||
// delta_pseudochips / 385
|
|
||||||
pseudobytes = frame+pos_GPS_DATA+8*j+4;
|
|
||||||
deltabytes = 0; // bzw. pseudobytes[3]=0 (24 bit);
|
|
||||||
memcpy(&deltabytes, pseudobytes, 3);
|
|
||||||
|
|
||||||
if ( chipbytes == 0x7FFFFFFF || chipbytes == 0x55555555 )
|
|
||||||
//or ( chipbytes > 0x10000000 && chipbytes < 0xF0000000 )
|
|
||||||
{
|
|
||||||
chipbytes = 0;
|
|
||||||
deltabytes = 0;
|
|
||||||
Sat[j].prn = 0;
|
|
||||||
}
|
}
|
||||||
//0x01400000 //0x013FB0A4
|
else {
|
||||||
Sat[j].pseudorange = - chipbytes * df;
|
|
||||||
Sat[j].pseudorate = - deltabytes * df / dl;
|
|
||||||
|
|
||||||
if ((Sat[j].status & 0x0F) != 0xF) Sat[j].prn = 0;
|
// Pseudorange/chips
|
||||||
|
pseudobytes = frame+pos_GPS_DATA+8*j;
|
||||||
|
memcpy(&chipbytes, pseudobytes, 4);
|
||||||
|
|
||||||
|
// delta_pseudochips / 385
|
||||||
|
pseudobytes = frame+pos_GPS_DATA+8*j+4;
|
||||||
|
deltabytes = 0; // bzw. pseudobytes[3]=0 (24 bit);
|
||||||
|
memcpy(&deltabytes, pseudobytes, 3);
|
||||||
|
|
||||||
|
// check deltabytes, status_deltabytes (24..31)
|
||||||
|
if (( chipbytes == 0x7FFFFFFF || chipbytes == 0x55555555 ) ||
|
||||||
|
( (ui32_t)chipbytes > 0x10000000 && (ui32_t)chipbytes < 0xF0000000 ))
|
||||||
|
{
|
||||||
|
chipbytes = 0;
|
||||||
|
deltabytes = 0;
|
||||||
|
Sat[j].prn = 0;
|
||||||
|
}
|
||||||
|
//0x01400000 //0x013FB0A4
|
||||||
|
Sat[j].pseudorange = - chipbytes * df;
|
||||||
|
Sat[j].pseudorate = - deltabytes * df / dl;
|
||||||
|
|
||||||
|
if ((Sat[j].status & 0x0F) != 0xF) Sat[j].prn = 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -566,6 +573,7 @@ static int rs92_get_GPSkoord(rs_data_t *rs_data, int opt_gg2) {
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
// 4er-Kombinationen probieren
|
// 4er-Kombinationen probieren
|
||||||
|
gdop = 100.0;
|
||||||
k = N;
|
k = N;
|
||||||
j0 = j1 = j2 = j3 = 0;
|
j0 = j1 = j2 = j3 = 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<k;i0++) { for (i1=i0+1;i1<k;i1++) { for (i2=i1+1;i2<k;i2++) { for (i3=i2+1;i3<k;i3++) {
|
||||||
|
@ -598,8 +606,9 @@ static int rs92_get_GPSkoord(rs_data_t *rs_data, int opt_gg2) {
|
||||||
&& (vH0 < 200.0 && vU0*vU0 < 200.0*200.0) // validen Positionen vergleichen
|
&& (vH0 < 200.0 && vU0*vU0 < 200.0*200.0) // validen Positionen vergleichen
|
||||||
) {
|
) {
|
||||||
|
|
||||||
if ( diter0 < diter ) {
|
if ( diter0 < diter && gdop0 < gdop+2.0 ) {
|
||||||
diter = diter0;
|
diter = diter0;
|
||||||
|
gdop = gdop0;
|
||||||
j0 = i0; j1 = i1; j2 = i2; j3 = i3;
|
j0 = i0; j1 = i1; j2 = i2; j3 = i3;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Ładowanie…
Reference in New Issue