Experimental carrier phase support
This commit is contained in:
OSQZSS
parent
8cee8d2dfa
commit
b41678f8d4
232
gpssim.c
232
gpssim.c
@ -56,114 +56,76 @@
|
||||
|
||||
#define ADC_GAIN (250) // for bladeRF txvga1 = -25dB with 50dB external attenuation
|
||||
|
||||
//#define _SINE_LUT
|
||||
#define _SINE_LUT
|
||||
#ifdef _SINE_LUT
|
||||
int sinTable512[] = {
|
||||
0,6,12,18,25,31,37,43,50,56,62,
|
||||
68,75,81,87,93,99,106,112,118,124,
|
||||
130,136,142,148,154,160,166,172,178,184,
|
||||
190,195,201,207,213,218,224,230,235,241,
|
||||
246,252,257,263,268,273,279,284,289,294,
|
||||
299,304,310,314,319,324,329,334,339,343,
|
||||
348,353,357,362,366,370,375,379,383,387,
|
||||
391,395,399,403,407,411,414,418,422,425,
|
||||
429,432,435,439,442,445,448,451,454,457,
|
||||
460,462,465,468,470,473,475,477,479,482,
|
||||
484,486,488,489,491,493,495,496,498,499,
|
||||
500,502,503,504,505,506,507,508,508,509,
|
||||
510,510,511,511,511,511,511,512,511,511,
|
||||
511,511,511,510,510,509,508,508,507,506,
|
||||
505,504,503,502,500,499,498,496,495,493,
|
||||
491,489,488,486,484,482,479,477,475,473,
|
||||
470,468,465,462,460,457,454,451,448,445,
|
||||
442,439,435,432,429,425,422,418,414,411,
|
||||
407,403,399,395,391,387,383,379,375,370,
|
||||
366,362,357,353,348,343,339,334,329,324,
|
||||
319,314,310,304,299,294,289,284,279,273,
|
||||
268,263,257,252,246,241,235,230,224,218,
|
||||
213,207,201,195,190,184,178,172,166,160,
|
||||
154,148,142,136,130,124,118,112,106,99,
|
||||
93,87,81,75,68,62,56,50,43,37,
|
||||
31,25,18,12,6,0,-7,-13,-19,-26,
|
||||
-32,-38,-44,-51,-57,-63,-69,-76,-82,-88,
|
||||
-94,-100,-107,-113,-119,-125,-131,-137,-143,-149,
|
||||
-155,-161,-167,-173,-179,-185,-191,-196,-202,-208,
|
||||
-214,-219,-225,-231,-236,-242,-247,-253,-258,-264,
|
||||
-269,-274,-280,-285,-290,-295,-300,-305,-311,-315,
|
||||
-320,-325,-330,-335,-340,-344,-349,-354,-358,-363,
|
||||
-367,-371,-376,-380,-384,-388,-392,-396,-400,-404,
|
||||
-408,-412,-415,-419,-423,-426,-430,-433,-436,-440,
|
||||
-443,-446,-449,-452,-455,-458,-461,-463,-466,-469,
|
||||
-471,-474,-476,-478,-480,-483,-485,-487,-489,-490,
|
||||
-492,-494,-496,-497,-499,-500,-501,-503,-504,-505,
|
||||
-506,-507,-508,-509,-509,-510,-511,-511,-512,-512,
|
||||
-512,-512,-512,-512,-512,-512,-512,-512,-512,-511,
|
||||
-511,-510,-509,-509,-508,-507,-506,-505,-504,-503,
|
||||
-501,-500,-499,-497,-496,-494,-492,-490,-489,-487,
|
||||
-485,-483,-480,-478,-476,-474,-471,-469,-466,-463,
|
||||
-461,-458,-455,-452,-449,-446,-443,-440,-436,-433,
|
||||
-430,-426,-423,-419,-415,-412,-408,-404,-400,-396,
|
||||
-392,-388,-384,-380,-376,-371,-367,-363,-358,-354,
|
||||
-349,-344,-340,-335,-330,-325,-320,-315,-311,-305,
|
||||
-300,-295,-290,-285,-280,-274,-269,-264,-258,-253,
|
||||
-247,-242,-236,-231,-225,-219,-214,-208,-202,-196,
|
||||
-191,-185,-179,-173,-167,-161,-155,-149,-143,-137,
|
||||
-131,-125,-119,-113,-107,-100,-94,-88,-82,-76,
|
||||
-69,-63,-57,-51,-44,-38,-32,-26,-19,-13,-7
|
||||
2, 5, 8, 11, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47,
|
||||
50, 53, 56, 59, 62, 65, 68, 71, 74, 77, 80, 83, 86, 89, 91, 94,
|
||||
97, 100, 103, 105, 108, 111, 114, 116, 119, 122, 125, 127, 130, 132, 135, 138,
|
||||
140, 143, 145, 148, 150, 153, 155, 157, 160, 162, 164, 167, 169, 171, 173, 176,
|
||||
178, 180, 182, 184, 186, 188, 190, 192, 194, 196, 198, 200, 202, 204, 205, 207,
|
||||
209, 210, 212, 214, 215, 217, 218, 220, 221, 223, 224, 225, 227, 228, 229, 230,
|
||||
232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 241, 242, 243, 244, 244, 245,
|
||||
245, 246, 247, 247, 248, 248, 248, 249, 249, 249, 249, 250, 250, 250, 250, 250,
|
||||
250, 250, 250, 250, 250, 249, 249, 249, 249, 248, 248, 248, 247, 247, 246, 245,
|
||||
245, 244, 244, 243, 242, 241, 241, 240, 239, 238, 237, 236, 235, 234, 233, 232,
|
||||
230, 229, 228, 227, 225, 224, 223, 221, 220, 218, 217, 215, 214, 212, 210, 209,
|
||||
207, 205, 204, 202, 200, 198, 196, 194, 192, 190, 188, 186, 184, 182, 180, 178,
|
||||
176, 173, 171, 169, 167, 164, 162, 160, 157, 155, 153, 150, 148, 145, 143, 140,
|
||||
138, 135, 132, 130, 127, 125, 122, 119, 116, 114, 111, 108, 105, 103, 100, 97,
|
||||
94, 91, 89, 86, 83, 80, 77, 74, 71, 68, 65, 62, 59, 56, 53, 50,
|
||||
47, 44, 41, 38, 35, 32, 29, 26, 23, 20, 17, 14, 11, 8, 5, 2,
|
||||
-2, -5, -8, -11, -14, -17, -20, -23, -26, -29, -32, -35, -38, -41, -44, -47,
|
||||
-50, -53, -56, -59, -62, -65, -68, -71, -74, -77, -80, -83, -86, -89, -91, -94,
|
||||
-97,-100,-103,-105,-108,-111,-114,-116,-119,-122,-125,-127,-130,-132,-135,-138,
|
||||
-140,-143,-145,-148,-150,-153,-155,-157,-160,-162,-164,-167,-169,-171,-173,-176,
|
||||
-178,-180,-182,-184,-186,-188,-190,-192,-194,-196,-198,-200,-202,-204,-205,-207,
|
||||
-209,-210,-212,-214,-215,-217,-218,-220,-221,-223,-224,-225,-227,-228,-229,-230,
|
||||
-232,-233,-234,-235,-236,-237,-238,-239,-240,-241,-241,-242,-243,-244,-244,-245,
|
||||
-245,-246,-247,-247,-248,-248,-248,-249,-249,-249,-249,-250,-250,-250,-250,-250,
|
||||
-250,-250,-250,-250,-250,-249,-249,-249,-249,-248,-248,-248,-247,-247,-246,-245,
|
||||
-245,-244,-244,-243,-242,-241,-241,-240,-239,-238,-237,-236,-235,-234,-233,-232,
|
||||
-230,-229,-228,-227,-225,-224,-223,-221,-220,-218,-217,-215,-214,-212,-210,-209,
|
||||
-207,-205,-204,-202,-200,-198,-196,-194,-192,-190,-188,-186,-184,-182,-180,-178,
|
||||
-176,-173,-171,-169,-167,-164,-162,-160,-157,-155,-153,-150,-148,-145,-143,-140,
|
||||
-138,-135,-132,-130,-127,-125,-122,-119,-116,-114,-111,-108,-105,-103,-100, -97,
|
||||
-94, -91, -89, -86, -83, -80, -77, -74, -71, -68, -65, -62, -59, -56, -53, -50,
|
||||
-47, -44, -41, -38, -35, -32, -29, -26, -23, -20, -17, -14, -11, -8, -5, -2
|
||||
};
|
||||
|
||||
int cosTable512[] = {
|
||||
512,511,511,511,511,511,510,510,509,508,508,
|
||||
507,506,505,504,503,502,500,499,498,496,
|
||||
495,493,491,489,488,486,484,482,479,477,
|
||||
475,473,470,468,465,462,460,457,454,451,
|
||||
448,445,442,439,435,432,429,425,422,418,
|
||||
414,411,407,403,399,395,391,387,383,379,
|
||||
375,370,366,362,357,353,348,343,339,334,
|
||||
329,324,319,314,310,304,299,294,289,284,
|
||||
279,273,268,263,257,252,246,241,235,230,
|
||||
224,218,213,207,201,195,190,184,178,172,
|
||||
166,160,154,148,142,136,130,124,118,112,
|
||||
106,99,93,87,81,75,68,62,56,50,
|
||||
43,37,31,25,18,12,6,0,-7,-13,
|
||||
-19,-26,-32,-38,-44,-51,-57,-63,-69,-76,
|
||||
-82,-88,-94,-100,-107,-113,-119,-125,-131,-137,
|
||||
-143,-149,-155,-161,-167,-173,-179,-185,-191,-196,
|
||||
-202,-208,-214,-219,-225,-231,-236,-242,-247,-253,
|
||||
-258,-264,-269,-274,-280,-285,-290,-295,-300,-305,
|
||||
-311,-315,-320,-325,-330,-335,-340,-344,-349,-354,
|
||||
-358,-363,-367,-371,-376,-380,-384,-388,-392,-396,
|
||||
-400,-404,-408,-412,-415,-419,-423,-426,-430,-433,
|
||||
-436,-440,-443,-446,-449,-452,-455,-458,-461,-463,
|
||||
-466,-469,-471,-474,-476,-478,-480,-483,-485,-487,
|
||||
-489,-490,-492,-494,-496,-497,-499,-500,-501,-503,
|
||||
-504,-505,-506,-507,-508,-509,-509,-510,-511,-511,
|
||||
-512,-512,-512,-512,-512,-512,-512,-512,-512,-512,
|
||||
-512,-511,-511,-510,-509,-509,-508,-507,-506,-505,
|
||||
-504,-503,-501,-500,-499,-497,-496,-494,-492,-490,
|
||||
-489,-487,-485,-483,-480,-478,-476,-474,-471,-469,
|
||||
-466,-463,-461,-458,-455,-452,-449,-446,-443,-440,
|
||||
-436,-433,-430,-426,-423,-419,-415,-412,-408,-404,
|
||||
-400,-396,-392,-388,-384,-380,-376,-371,-367,-363,
|
||||
-358,-354,-349,-344,-340,-335,-330,-325,-320,-315,
|
||||
-311,-305,-300,-295,-290,-285,-280,-274,-269,-264,
|
||||
-258,-253,-247,-242,-236,-231,-225,-219,-214,-208,
|
||||
-202,-196,-191,-185,-179,-173,-167,-161,-155,-149,
|
||||
-143,-137,-131,-125,-119,-113,-107,-100,-94,-88,
|
||||
-82,-76,-69,-63,-57,-51,-44,-38,-32,-26,
|
||||
-19,-13,-7,-1,6,12,18,25,31,37,
|
||||
43,50,56,62,68,75,81,87,93,99,
|
||||
106,112,118,124,130,136,142,148,154,160,
|
||||
166,172,178,184,190,195,201,207,213,218,
|
||||
224,230,235,241,246,252,257,263,268,273,
|
||||
279,284,289,294,299,304,310,314,319,324,
|
||||
329,334,339,343,348,353,357,362,366,370,
|
||||
375,379,383,387,391,395,399,403,407,411,
|
||||
414,418,422,425,429,432,435,439,442,445,
|
||||
448,451,454,457,460,462,465,468,470,473,
|
||||
475,477,479,482,484,486,488,489,491,493,
|
||||
495,496,498,499,500,502,503,504,505,506,
|
||||
507,508,508,509,510,510,511,511,511,511,511
|
||||
250, 250, 250, 250, 250, 249, 249, 249, 249, 248, 248, 248, 247, 247, 246, 245,
|
||||
245, 244, 244, 243, 242, 241, 241, 240, 239, 238, 237, 236, 235, 234, 233, 232,
|
||||
230, 229, 228, 227, 225, 224, 223, 221, 220, 218, 217, 215, 214, 212, 210, 209,
|
||||
207, 205, 204, 202, 200, 198, 196, 194, 192, 190, 188, 186, 184, 182, 180, 178,
|
||||
176, 173, 171, 169, 167, 164, 162, 160, 157, 155, 153, 150, 148, 145, 143, 140,
|
||||
138, 135, 132, 130, 127, 125, 122, 119, 116, 114, 111, 108, 105, 103, 100, 97,
|
||||
94, 91, 89, 86, 83, 80, 77, 74, 71, 68, 65, 62, 59, 56, 53, 50,
|
||||
47, 44, 41, 38, 35, 32, 29, 26, 23, 20, 17, 14, 11, 8, 5, 2,
|
||||
-2, -5, -8, -11, -14, -17, -20, -23, -26, -29, -32, -35, -38, -41, -44, -47,
|
||||
-50, -53, -56, -59, -62, -65, -68, -71, -74, -77, -80, -83, -86, -89, -91, -94,
|
||||
-97,-100,-103,-105,-108,-111,-114,-116,-119,-122,-125,-127,-130,-132,-135,-138,
|
||||
-140,-143,-145,-148,-150,-153,-155,-157,-160,-162,-164,-167,-169,-171,-173,-176,
|
||||
-178,-180,-182,-184,-186,-188,-190,-192,-194,-196,-198,-200,-202,-204,-205,-207,
|
||||
-209,-210,-212,-214,-215,-217,-218,-220,-221,-223,-224,-225,-227,-228,-229,-230,
|
||||
-232,-233,-234,-235,-236,-237,-238,-239,-240,-241,-241,-242,-243,-244,-244,-245,
|
||||
-245,-246,-247,-247,-248,-248,-248,-249,-249,-249,-249,-250,-250,-250,-250,-250,
|
||||
-250,-250,-250,-250,-250,-249,-249,-249,-249,-248,-248,-248,-247,-247,-246,-245,
|
||||
-245,-244,-244,-243,-242,-241,-241,-240,-239,-238,-237,-236,-235,-234,-233,-232,
|
||||
-230,-229,-228,-227,-225,-224,-223,-221,-220,-218,-217,-215,-214,-212,-210,-209,
|
||||
-207,-205,-204,-202,-200,-198,-196,-194,-192,-190,-188,-186,-184,-182,-180,-178,
|
||||
-176,-173,-171,-169,-167,-164,-162,-160,-157,-155,-153,-150,-148,-145,-143,-140,
|
||||
-138,-135,-132,-130,-127,-125,-122,-119,-116,-114,-111,-108,-105,-103,-100, -97,
|
||||
-94, -91, -89, -86, -83, -80, -77, -74, -71, -68, -65, -62, -59, -56, -53, -50,
|
||||
-47, -44, -41, -38, -35, -32, -29, -26, -23, -20, -17, -14, -11, -8, -5, -2,
|
||||
2, 5, 8, 11, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47,
|
||||
50, 53, 56, 59, 62, 65, 68, 71, 74, 77, 80, 83, 86, 89, 91, 94,
|
||||
97, 100, 103, 105, 108, 111, 114, 116, 119, 122, 125, 127, 130, 132, 135, 138,
|
||||
140, 143, 145, 148, 150, 153, 155, 157, 160, 162, 164, 167, 169, 171, 173, 176,
|
||||
178, 180, 182, 184, 186, 188, 190, 192, 194, 196, 198, 200, 202, 204, 205, 207,
|
||||
209, 210, 212, 214, 215, 217, 218, 220, 221, 223, 224, 225, 227, 228, 229, 230,
|
||||
232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 241, 242, 243, 244, 244, 245,
|
||||
245, 246, 247, 247, 248, 248, 248, 249, 249, 249, 249, 250, 250, 250, 250, 250
|
||||
};
|
||||
#endif
|
||||
|
||||
@ -1155,15 +1117,15 @@ int main(int argc, char *argv[])
|
||||
void *iq_buff = NULL;
|
||||
|
||||
gpstime_t grx;
|
||||
range_t rho_backup[MAX_SAT];
|
||||
range_t rho0[MAX_SAT];
|
||||
|
||||
double delt; // = 1.0/SAMP_FREQ;
|
||||
double delt;
|
||||
int isamp;
|
||||
|
||||
int iumd;
|
||||
int numd;
|
||||
double umd[USER_MOTION_SIZE][3];
|
||||
char umfile[MAX_CHAR];
|
||||
double xyz[USER_MOTION_SIZE][3];
|
||||
|
||||
char navfile[MAX_CHAR];
|
||||
char outfile[MAX_CHAR];
|
||||
@ -1253,7 +1215,7 @@ int main(int argc, char *argv[])
|
||||
////////////////////////////////////////////////////////////
|
||||
|
||||
// Read user motion file
|
||||
numd = readUserMotion(umd, umfile);
|
||||
numd = readUserMotion(xyz, umfile);
|
||||
|
||||
if (numd==-1)
|
||||
{
|
||||
@ -1269,9 +1231,9 @@ int main(int argc, char *argv[])
|
||||
printf("User motion data = %d\n", numd);
|
||||
|
||||
// Initial location in Geodetic coordinate system
|
||||
xyz2llh(umd[0], llh);
|
||||
xyz2llh(xyz[0], llh);
|
||||
|
||||
printf("xyz = %11.1f, %11.1f, %11.1f\n", umd[0][0], umd[0][1], umd[0][2]);
|
||||
printf("xyz = %11.1f, %11.1f, %11.1f\n", xyz[0][0], xyz[0][1], xyz[0][2]);
|
||||
printf("llh = %11.6f, %11.6f, %11.1f\n", llh[0]*R2D, llh[1]*R2D, llh[2]);
|
||||
|
||||
////////////////////////////////////////////////////////////
|
||||
@ -1320,7 +1282,7 @@ int main(int argc, char *argv[])
|
||||
if (eph[sv].vflg==1)
|
||||
{
|
||||
satpos(eph[sv], g0, pos, vel, clk);
|
||||
subVect(los, pos, umd[0]);
|
||||
subVect(los, pos, xyz[0]);
|
||||
ecef2neu(los, tmat, neu);
|
||||
neu2azel(azel, neu);
|
||||
|
||||
@ -1371,9 +1333,6 @@ int main(int argc, char *argv[])
|
||||
// C/A code generation
|
||||
codegen(chan[i].ca, chan[i].prn);
|
||||
|
||||
// Initialize carrier phase
|
||||
chan[i].carr_phase = 0.0; // !!FIXME!! Need proper initialization for RTK
|
||||
|
||||
// Allocate I/Q buffer
|
||||
chan[i].iq_buff = (short *)calloc(2*iq_buff_size, 2);
|
||||
|
||||
@ -1384,6 +1343,29 @@ int main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize carrier phase
|
||||
for (i=0; i<nsat; i++)
|
||||
{
|
||||
range_t tmp;
|
||||
double ref[3]={0.0};
|
||||
double phase_offset,phase_offset_time;
|
||||
double phase_ini,phase_ini_time;
|
||||
|
||||
sv = chan[i].prn-1;
|
||||
|
||||
computeRange(&tmp, eph[sv], grx, ref);
|
||||
phase_offset_time = grx.sec - tmp.range/SPEED_OF_LIGHT;
|
||||
phase_offset = tmp.range/LAMBDA_L1;
|
||||
phase_offset -= floor(phase_offset);
|
||||
|
||||
computeRange(&tmp, eph[sv], grx, xyz[0]);
|
||||
phase_ini_time = grx.sec - tmp.range/SPEED_OF_LIGHT;
|
||||
phase_ini = phase_offset + (phase_ini_time - phase_offset_time)*SPEED_OF_LIGHT/LAMBDA_L1;
|
||||
phase_ini -= floor(phase_ini);
|
||||
|
||||
chan[i].carr_phase = phase_ini;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////
|
||||
// Generate subframes and data bits
|
||||
////////////////////////////////////////////////////////////
|
||||
@ -1443,10 +1425,12 @@ int main(int argc, char *argv[])
|
||||
for (i=0; i<nsat; i++)
|
||||
{
|
||||
sv = chan[i].prn-1;
|
||||
computeRange(&rho_backup[sv], eph[sv], grx, umd[0]);
|
||||
computeRange(&rho0[sv], eph[sv], grx, xyz[0]);
|
||||
}
|
||||
|
||||
// Update receiver time
|
||||
grx.sec += 0.1;
|
||||
|
||||
for (iumd=1; iumd<numd; iumd++)
|
||||
{
|
||||
#pragma omp parallel for private(isamp)
|
||||
@ -1458,11 +1442,13 @@ int main(int argc, char *argv[])
|
||||
range_t rho;
|
||||
|
||||
// Current pseudorange
|
||||
computeRange(&rho, eph[sv], grx, umd[iumd]);
|
||||
computeRange(&rho, eph[sv], grx, xyz[iumd]);
|
||||
|
||||
// Update code phase and data bit counters
|
||||
computeCodePhase(&chan[i], rho_backup[sv], rho, 0.1);
|
||||
rho_backup[sv] = rho;
|
||||
computeCodePhase(&chan[i], rho0[sv], rho, 0.1);
|
||||
|
||||
// Save current pseudorange
|
||||
rho0[sv] = rho;
|
||||
|
||||
for (isamp=0; isamp<iq_buff_size; isamp++)
|
||||
{
|
||||
@ -1473,8 +1459,8 @@ int main(int argc, char *argv[])
|
||||
qp = chan[i].dataBit * chan[i].codeCA * sinTable512[iTable];
|
||||
|
||||
// Store I/Q samples into buffer
|
||||
chan[i].iq_buff[isamp*2] = (short)(ip>>1);
|
||||
chan[i].iq_buff[isamp*2+1] = (short)(qp>>1);
|
||||
chan[i].iq_buff[isamp*2] = (short)ip;
|
||||
chan[i].iq_buff[isamp*2+1] = (short)qp;
|
||||
#else
|
||||
ip = chan[i].dataBit * chan[i].codeCA * cos(2.0*PI*chan[i].carr_phase);
|
||||
qp = chan[i].dataBit * chan[i].codeCA * sin(2.0*PI*chan[i].carr_phase);
|
||||
@ -1525,13 +1511,15 @@ int main(int argc, char *argv[])
|
||||
{
|
||||
for (isamp=0; isamp<2*iq_buff_size; isamp++)
|
||||
{
|
||||
char sample = 0;
|
||||
signed char sample = 0;
|
||||
for (i=0; i<nsat; i++)
|
||||
sample += (signed char)(chan[i].iq_buff[isamp]>>4); // 12-bit bladeRF -> 8-bit HackRF
|
||||
((signed char*)iq_buff)[isamp] = sample;
|
||||
}
|
||||
fwrite(iq_buff, 1, 2*iq_buff_size, fp);
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
for (isamp=0; isamp<2*iq_buff_size; isamp++)
|
||||
{
|
||||
short sample = 0;
|
||||
@ -1542,7 +1530,7 @@ int main(int argc, char *argv[])
|
||||
fwrite(iq_buff, 2, 2*iq_buff_size, fp);
|
||||
}
|
||||
|
||||
// Next second
|
||||
// Update receiver time
|
||||
grx.sec += 0.1;
|
||||
|
||||
// Update time counter
|
||||
|
||||
Loading…
Reference in New Issue
Block a user