Changeset 8963 in ntrip for trunk/BNC/src
- Timestamp:
- Jun 29, 2020, 2:46:25 PM (4 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/BNC/src/RTCM3/clock_and_orbit/clock_orbit_rtcm.c
r8410 r8963 1 1 /* Programheader 2 2 3 4 5 6 7 8 */3 Name: clock_orbit_rtcm.c 4 Project: RTCM3 5 Version: $Id$ 6 Authors: Dirk Stöcker 7 Description: state space approach for RTCM3 8 */ 9 9 10 10 #include <math.h> … … 18 18 #include "clock_orbit_rtcm.h" 19 19 20 static uint32_t CRC24(long size, const unsigned char *buf) 21 { 20 static uint32_t CRC24(long size, const unsigned char *buf) { 22 21 uint32_t crc = 0; 23 22 int i; 24 23 25 while(size--) 26 { 24 while (size--) { 27 25 crc ^= (*buf++) << (16); 28 for (i = 0; i < 8; i++)29 {26 for (i = 0; i < 8; i++) 27 { 30 28 crc <<= 1; 31 if (crc & 0x1000000)29 if (crc & 0x1000000) 32 30 crc ^= 0x01864cfb; 33 31 } … … 37 35 38 36 /* NOTE: These defines are interlinked with below functions and directly modify 39 the values. This may not be optimized in terms of final program code size but40 should be optimized in terms of speed.41 42 modified variables are:43 - everything defined in STARTDATA (only use ressize outside of the defines,44 45 - buffer46 - size47 */37 the values. This may not be optimized in terms of final program code size but 38 should be optimized in terms of speed. 39 40 modified variables are: 41 - everything defined in STARTDATA (only use ressize outside of the defines, 42 others are private) 43 - buffer 44 - size 45 */ 48 46 49 47 #ifndef NOENCODE 50 48 #define STOREBITS \ 51 while(numbits >= 8) \ 52 { \ 49 while(numbits >= 8) { \ 53 50 if(!size) return 0; \ 54 51 *(buffer++) = bitbuffer>>(numbits-8); \ … … 58 55 } 59 56 60 #define ADDBITS(a, b) \ 61 { \ 57 #define ADDBITS(a, b) { \ 62 58 bitbuffer = (bitbuffer<<(a))|((b)&((1<<a)-1)); \ 63 59 numbits += (a); \ … … 79 75 80 76 #define ENDBLOCK \ 81 if(numbits) { ADDBITS((8-numbits), 0) } \ 82 { \ 77 if(numbits) { ADDBITS((8-numbits), 0) } { \ 83 78 int len = buffer-blockstart-3; \ 84 79 blockstart[1] |= len>>8; \ … … 151 146 #define T_IONO_HEIGHT(a) SCALEADDBITS(8, 1/10000.0, a) 152 147 153 static double URAToValue(int ura) 154 { 148 static double URAToValue(int ura) { 155 149 int urac, urav; 156 150 urac = ura >> 3; 157 151 urav = ura & 7; 158 if (!ura)152 if (!ura) 159 153 return 0; 160 else if (ura == 63)154 else if (ura == 63) 161 155 return SSR_MAXURA; 162 return (pow(3, urac)*(1.0+urav/4.0)-1.0)/1000.0;156 return (pow(3, urac) * (1.0 + urav / 4.0) - 1.0) / 1000.0; 163 157 } 164 158 165 static int ValueToURA(double val) 166 { 159 static int ValueToURA(double val) { 167 160 int ura; 168 if (!val)161 if (!val) 169 162 return 0; 170 else if (val > 5.4665)163 else if (val > 5.4665) 171 164 return 63; 172 for (ura = 1; ura < 63 && val > URAToValue(ura); ++ura)165 for (ura = 1; ura < 63 && val > URAToValue(ura); ++ura) 173 166 ; 174 167 return ura; 175 168 } 176 169 177 static const enum ClockOrbitType corbase[CLOCKORBIT_SATNUM] = 178 { 179 (int) COBBASE_GPS, (int) COBBASE_GLONASS, (int) COBBASE_GALILEO, 180 (int) COBBASE_QZSS, (int) COBBASE_SBAS, (int) COBBASE_BDS 170 static const enum ClockOrbitType corbase[CLOCKORBIT_SATNUM] = { 171 (int) COBBASE_GPS, 172 (int) COBBASE_GLONASS, 173 (int) COBBASE_GALILEO, 174 (int) COBBASE_QZSS, 175 (int) COBBASE_SBAS, 176 (int) COBBASE_BDS 181 177 }; 182 static const enum COR_OFFSETS satoffset[CLOCKORBIT_SATNUM+1] = 183 { 184 CLOCKORBIT_OFFSETGPS, CLOCKORBIT_OFFSETGLONASS, CLOCKORBIT_OFFSETGALILEO, 185 CLOCKORBIT_OFFSETQZSS, CLOCKORBIT_OFFSETSBAS, CLOCKORBIT_OFFSETBDS, 178 179 static const enum COR_OFFSETS satoffset[CLOCKORBIT_SATNUM + 1] = { 180 CLOCKORBIT_OFFSETGPS, 181 CLOCKORBIT_OFFSETGLONASS, 182 CLOCKORBIT_OFFSETGALILEO, 183 CLOCKORBIT_OFFSETQZSS, 184 CLOCKORBIT_OFFSETSBAS, 185 CLOCKORBIT_OFFSETBDS, 186 186 CLOCKORBIT_COUNTSAT 187 187 }; 188 /* 189 //CNES PATCH MEDIAN 190 static double Median(int n, double x[50]) { 191 double temp; 192 int i, j; 193 // the following two loops sort the array x in ascending order 194 if (n <= 2) 195 return 0.0; 196 197 for (i = 0; i < n - 1; i++) 198 for (j = i + 1; j < n; j++) 199 if (x[j] < x[i]) { 200 // swap elements 201 temp = x[i]; 202 x[i] = x[j]; 203 x[j] = temp; 204 } 205 206 if (n % 2 == 0) 207 return ((x[n / 2] + x[n / 2 - 1]) / 2.0); 208 else 209 return x[n / 2]; 210 } 211 //PATCH MEDIAN 212 //PATCH CLKOFFSET 213 size_t MakeClockOrbit(struct ClockOrbit *co, enum ClockOrbitType type,*/ 188 214 189 215 size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type, 190 int moremessagesfollow, char *buffer, size_t size) 191 { 216 int moremessagesfollow, char *buffer, size_t size) { 192 217 unsigned int status[CLOCKORBIT_SATNUM][COBOFS_NUM], i, s; 193 218 194 219 memset(status, 0, sizeof(status)); 195 STARTDATA 196 197 for(s = 0; s < CLOCKORBIT_SATNUM; ++s) 198 { 199 for(i = 0; i < COBOFS_NUM; ++i) 200 { 201 if(co->NumberOfSat[s] && (type == COTYPE_AUTO 202 || type == corbase[s]+i) && (co->Supplied[i] || (i <= COBOFS_CLOCK 203 && co->Supplied[COBOFS_COMBINED]) || (i == COBOFS_COMBINED 204 && co->Supplied[COBOFS_ORBIT] && co->Supplied[COBOFS_CLOCK]))) 220 /* PATCH CLKOFFSET ///////////////////////////////////////////////////// 221 //double tSum 222 double tOffset, tClock[50]; 223 int j, tNb; 224 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) 205 225 { 226 //tSum = 0.0; 227 tOffset = 0.0; 228 tNb = 0; 229 for (j = 0; j < 50; j++) 230 tClock[j] = 0.0; 231 232 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) { 233 if (co->Sat[i].Clock.DeltaA0) { 234 //tSum += co->Sat[i].Clock.DeltaA0; //Average 235 tClock[tNb] = co->Sat[i].Clock.DeltaA0; //Median 236 tNb++; 237 } 238 } 239 //tOffset = tSum/(double)tNb; //Average 240 tOffset = Median(tNb, tClock); //Median 241 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) 242 if (co->Sat[i].Clock.DeltaA0) { 243 //clkoffset_test 244 // FILE *ficOffsetClk = fopen("ClkOffset.txt","a"); 245 // fprintf(ficOffsetClk,"%d %d %d %lf %lf %lf\n",co->EpochTime[s], s, i, co->Sat[i].Clock.DeltaA0, tOffset, co->Sat[i].Clock.DeltaA0-tOffset); 246 // fclose(ficOffsetClk); 247 248 co->Sat[i].Clock.DeltaA0 -= tOffset; 249 } 250 } 251 //PATCH CLKOFFSET ///////////////////////////////////////////////////// */ 252 253 STARTDATA 254 255 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) { 256 for (i = 0; i < COBOFS_NUM; ++i) { 257 if (co->NumberOfSat[s] && (type == COTYPE_AUTO || type == corbase[s] + i) && 258 (co->Supplied[i] || (i <= COBOFS_CLOCK && co->Supplied[COBOFS_COMBINED]) || 259 (i == COBOFS_COMBINED && co->Supplied[COBOFS_ORBIT] && co->Supplied[COBOFS_CLOCK]))) { 206 260 status[s][i] = 1; 207 if(i == COBOFS_COMBINED) 208 { 261 if (i == COBOFS_COMBINED) { 209 262 status[s][COBOFS_ORBIT] = status[s][COBOFS_CLOCK] = 0; 210 263 } /* disable single blocks for combined type */ … … 213 266 } /* iterate over satellite systems */ 214 267 215 for(s = 0; s < CLOCKORBIT_SATNUM; ++s) 216 { 217 if(status[s][COBOFS_ORBIT]) 218 { 268 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) { 269 if (status[s][COBOFS_ORBIT]) { 219 270 INITBLOCK 220 T_MESSAGE_NUMBER(corbase[s]+COBOFS_ORBIT) 221 switch(s) 222 { 223 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 224 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 225 case CLOCKORBIT_SATBDS: 226 T_GPS_EPOCH_TIME(co->EpochTime[s]) 227 break; 228 case CLOCKORBIT_SATGLONASS: 229 T_GLONASS_EPOCH_TIME(co->EpochTime[s]) 230 break; 271 T_MESSAGE_NUMBER(corbase[s] + COBOFS_ORBIT) 272 switch (s) { 273 case CLOCKORBIT_SATGPS: 274 case CLOCKORBIT_SATGALILEO: 275 case CLOCKORBIT_SATQZSS: 276 case CLOCKORBIT_SATSBAS: 277 case CLOCKORBIT_SATBDS: 278 T_GPS_EPOCH_TIME(co->EpochTime[s]) 279 break; 280 case CLOCKORBIT_SATGLONASS: 281 T_GLONASS_EPOCH_TIME(co->EpochTime[s]) 282 break; 231 283 } 232 284 T_SSR_UPDATE_INTERVAL(co->UpdateInterval) … … 237 289 T_SSR_SOLUTION_ID(co->SSRSolutionID) 238 290 T_NO_OF_SATELLITES(co->NumberOfSat[s]) 239 for(i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i) 240 { 241 switch(s) 242 { 243 case CLOCKORBIT_SATGPS: 244 T_GPS_SATELLITE_ID(co->Sat[i].ID) 245 T_GPS_IODE(co->Sat[i].IOD) 246 break; 247 case CLOCKORBIT_SATGLONASS: 248 T_GLONASS_SATELLITE_ID(co->Sat[i].ID) 249 T_GLONASS_IOD(co->Sat[i].IOD) 250 break; 251 case CLOCKORBIT_SATGALILEO: 252 T_GPS_SATELLITE_ID(co->Sat[i].ID) 253 T_GALILEO_IOD(co->Sat[i].IOD) 254 break; 255 case CLOCKORBIT_SATQZSS: 256 T_QZSS_SATELLITE_ID(co->Sat[i].ID) 257 T_GPS_IODE(co->Sat[i].IOD) 258 break; 259 case CLOCKORBIT_SATSBAS: 260 T_GPS_SATELLITE_ID(co->Sat[i].ID) 261 T_SBAS_T0MOD(co->Sat[i].toe) 262 T_SBAS_IODCRC(co->Sat[i].IOD) 263 break; 264 case CLOCKORBIT_SATBDS: 265 T_GPS_SATELLITE_ID(co->Sat[i].ID) 266 T_BDS_TOEMOD(co->Sat[i].toe) 267 T_BDS_IOD(co->Sat[i].IOD) 268 break; 291 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) { 292 switch (s) { 293 case CLOCKORBIT_SATGPS: 294 T_GPS_SATELLITE_ID(co->Sat[i].ID) 295 T_GPS_IODE(co->Sat[i].IOD) 296 break; 297 case CLOCKORBIT_SATGLONASS: 298 T_GLONASS_SATELLITE_ID(co->Sat[i].ID) 299 T_GLONASS_IOD(co->Sat[i].IOD) 300 break; 301 case CLOCKORBIT_SATGALILEO: 302 T_GPS_SATELLITE_ID(co->Sat[i].ID) 303 T_GALILEO_IOD(co->Sat[i].IOD) 304 break; 305 case CLOCKORBIT_SATQZSS: 306 T_QZSS_SATELLITE_ID(co->Sat[i].ID) 307 T_GPS_IODE(co->Sat[i].IOD) 308 break; 309 case CLOCKORBIT_SATSBAS: 310 T_GPS_SATELLITE_ID(co->Sat[i].ID) 311 T_SBAS_T0MOD(co->Sat[i].toe) 312 T_SBAS_IODCRC(co->Sat[i].IOD) 313 break; 314 case CLOCKORBIT_SATBDS: 315 T_GPS_SATELLITE_ID(co->Sat[i].ID) 316 T_BDS_TOEMOD(co->Sat[i].toe) 317 T_BDS_IOD(co->Sat[i].IOD) 318 break; 269 319 } 270 320 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial) … … 277 327 ENDBLOCK 278 328 } 279 if(status[s][COBOFS_CLOCK]) 280 { 329 if (status[s][COBOFS_CLOCK]) { 281 330 INITBLOCK 282 T_MESSAGE_NUMBER(corbase[s]+COBOFS_CLOCK) 283 switch(s) 284 { 285 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 286 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 287 case CLOCKORBIT_SATBDS: 288 T_GPS_EPOCH_TIME(co->EpochTime[s]) 289 break; 290 case CLOCKORBIT_SATGLONASS: 291 T_GLONASS_EPOCH_TIME(co->EpochTime[s]) 292 break; 331 T_MESSAGE_NUMBER(corbase[s] + COBOFS_CLOCK) 332 switch (s) { 333 case CLOCKORBIT_SATGPS: 334 case CLOCKORBIT_SATGALILEO: 335 case CLOCKORBIT_SATQZSS: 336 case CLOCKORBIT_SATSBAS: 337 case CLOCKORBIT_SATBDS: 338 T_GPS_EPOCH_TIME(co->EpochTime[s]) 339 break; 340 case CLOCKORBIT_SATGLONASS: 341 T_GLONASS_EPOCH_TIME(co->EpochTime[s]) 342 break; 293 343 } 294 344 T_SSR_UPDATE_INTERVAL(co->UpdateInterval) … … 298 348 T_SSR_SOLUTION_ID(co->SSRSolutionID) 299 349 T_NO_OF_SATELLITES(co->NumberOfSat[s]) 300 for (i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i)301 {302 switch(s)303 {304 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:305 case CLOCKORBIT_SATSBAS:case CLOCKORBIT_SATBDS:306 T_GPS_SATELLITE_ID(co->Sat[i].ID)307 break;308 case CLOCKORBIT_SATQZSS:309 T_QZSS_SATELLITE_ID(co->Sat[i].ID)310 break;311 case CLOCKORBIT_SATGLONASS:312 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)313 break;350 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) { 351 switch (s) { 352 case CLOCKORBIT_SATGPS: 353 case CLOCKORBIT_SATGALILEO: 354 case CLOCKORBIT_SATSBAS: 355 case CLOCKORBIT_SATBDS: 356 T_GPS_SATELLITE_ID(co->Sat[i].ID) 357 break; 358 case CLOCKORBIT_SATQZSS: 359 T_QZSS_SATELLITE_ID(co->Sat[i].ID) 360 break; 361 case CLOCKORBIT_SATGLONASS: 362 T_GLONASS_SATELLITE_ID(co->Sat[i].ID) 363 break; 314 364 } 315 365 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0) … … 319 369 ENDBLOCK 320 370 } 321 if(status[s][COBOFS_COMBINED]) 322 { 371 if (status[s][COBOFS_COMBINED]) { 323 372 #ifdef SPLITBLOCK 324 373 int nums = co->NumberOfSat[s]; 325 374 int left, start = satoffset[s]; 326 if(nums > 28) /* split block when more than 28 sats */ 327 { 375 if(nums > 28) {/* split block when more than 28 sats */ 328 376 left = nums - 28; 329 377 nums = 28; 330 378 } 331 else 332 { 379 else { 333 380 left = 0; 334 381 } 335 while(nums) 336 { 337 #endif 338 INITBLOCK339 T_MESSAGE_NUMBER(corbase[s]+COBOFS_COMBINED)340 switch(s)341 {342 case CLOCKORBIT_SAT GPS: case CLOCKORBIT_SATGALILEO:343 case CLOCKORBIT_SAT QZSS: case CLOCKORBIT_SATSBAS:382 while(nums) { 383 #endif 384 INITBLOCK 385 T_MESSAGE_NUMBER(corbase[s] + COBOFS_COMBINED) 386 switch (s) { 387 case CLOCKORBIT_SATGPS: 388 case CLOCKORBIT_SATGALILEO: 389 case CLOCKORBIT_SATQZSS: 390 case CLOCKORBIT_SATSBAS: 344 391 case CLOCKORBIT_SATBDS: 345 392 T_GPS_EPOCH_TIME(co->EpochTime[s]) … … 348 395 T_GLONASS_EPOCH_TIME(co->EpochTime[s]) 349 396 break; 350 351 397 } 398 T_SSR_UPDATE_INTERVAL(co->UpdateInterval) 352 399 #ifdef SPLITBLOCK 353 400 T_MULTIPLE_MESSAGE_INDICATOR((moremessagesfollow || left) ? 1 : 0) 354 401 #else 355 356 #endif 357 358 359 360 402 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0) 403 #endif 404 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum) 405 T_SSR_IOD(co->SSRIOD) 406 T_SSR_PROVIDER_ID(co->SSRProviderID) 407 T_SSR_SOLUTION_ID(co->SSRSolutionID) 361 408 #ifdef SPLITBLOCK 362 363 409 T_NO_OF_SATELLITES(nums) 410 for(i = start; i < start+nums; ++i) 364 411 #else 365 T_NO_OF_SATELLITES(co->NumberOfSat[s]) 366 for(i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i) 367 #endif 368 { 369 switch(s) 370 { 412 T_NO_OF_SATELLITES(co->NumberOfSat[s]) 413 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) 414 #endif 415 { 416 switch (s) { 371 417 case CLOCKORBIT_SATGPS: 372 418 T_GPS_SATELLITE_ID(co->Sat[i].ID) … … 395 441 T_BDS_IOD(co->Sat[i].IOD) 396 442 break; 397 }398 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)399 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)400 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)401 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)402 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)403 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)404 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)405 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)406 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)407 443 } 408 ENDBLOCK 444 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial) 445 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack) 446 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack) 447 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial) 448 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack) 449 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack) 450 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0) 451 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1) 452 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2) 453 } 454 ENDBLOCK 409 455 #ifdef SPLITBLOCK 410 start += nums; 411 nums = left; 412 left = 0; 413 } 414 #endif 415 } 416 if(status[s][COBOFS_HR]) 417 { 456 start += nums; 457 nums = left; 458 left = 0; 459 } 460 #endif 461 } 462 if (status[s][COBOFS_HR]) { 418 463 INITBLOCK 419 T_MESSAGE_NUMBER(corbase[s]+COBOFS_HR) 420 switch(s) 421 { 422 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 423 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 424 case CLOCKORBIT_SATBDS: 425 T_GPS_EPOCH_TIME(co->EpochTime[s]) 426 break; 427 case CLOCKORBIT_SATGLONASS: 428 T_GLONASS_EPOCH_TIME(co->EpochTime[s]) 429 break; 464 T_MESSAGE_NUMBER(corbase[s] + COBOFS_HR) 465 switch (s) { 466 case CLOCKORBIT_SATGPS: 467 case CLOCKORBIT_SATGALILEO: 468 case CLOCKORBIT_SATQZSS: 469 case CLOCKORBIT_SATSBAS: 470 case CLOCKORBIT_SATBDS: 471 T_GPS_EPOCH_TIME(co->EpochTime[s]) 472 break; 473 case CLOCKORBIT_SATGLONASS: 474 T_GLONASS_EPOCH_TIME(co->EpochTime[s]) 475 break; 430 476 } 431 477 T_SSR_UPDATE_INTERVAL(co->UpdateInterval) … … 435 481 T_SSR_SOLUTION_ID(co->SSRSolutionID) 436 482 T_NO_OF_SATELLITES(co->NumberOfSat[s]) 437 for (i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i)438 {439 switch(s)440 {441 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:442 case CLOCKORBIT_SATSBAS:case CLOCKORBIT_SATBDS:443 T_GPS_SATELLITE_ID(co->Sat[i].ID)444 break;445 case CLOCKORBIT_SATQZSS:446 T_QZSS_SATELLITE_ID(co->Sat[i].ID)447 break;448 case CLOCKORBIT_SATGLONASS:449 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)450 break;483 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) { 484 switch (s) { 485 case CLOCKORBIT_SATGPS: 486 case CLOCKORBIT_SATGALILEO: 487 case CLOCKORBIT_SATSBAS: 488 case CLOCKORBIT_SATBDS: 489 T_GPS_SATELLITE_ID(co->Sat[i].ID) 490 break; 491 case CLOCKORBIT_SATQZSS: 492 T_QZSS_SATELLITE_ID(co->Sat[i].ID) 493 break; 494 case CLOCKORBIT_SATGLONASS: 495 T_GLONASS_SATELLITE_ID(co->Sat[i].ID) 496 break; 451 497 } 452 498 T_HR_CLOCK_CORRECTION(co->Sat[i].hrclock) … … 454 500 ENDBLOCK 455 501 } 456 if(status[s][COBOFS_URA]) 457 { 502 if (status[s][COBOFS_URA]) { 458 503 INITBLOCK 459 T_MESSAGE_NUMBER(corbase[s]+COBOFS_URA) 460 switch(s) 461 { 462 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 463 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 464 case CLOCKORBIT_SATBDS: 465 T_GPS_EPOCH_TIME(co->EpochTime[s]) 466 break; 467 case CLOCKORBIT_SATGLONASS: 468 T_GLONASS_EPOCH_TIME(co->EpochTime[s]) 469 break; 504 T_MESSAGE_NUMBER(corbase[s] + COBOFS_URA) 505 switch (s) { 506 case CLOCKORBIT_SATGPS: 507 case CLOCKORBIT_SATGALILEO: 508 case CLOCKORBIT_SATQZSS: 509 case CLOCKORBIT_SATSBAS: 510 case CLOCKORBIT_SATBDS: 511 T_GPS_EPOCH_TIME(co->EpochTime[s]) 512 break; 513 case CLOCKORBIT_SATGLONASS: 514 T_GLONASS_EPOCH_TIME(co->EpochTime[s]) 515 break; 470 516 } 471 517 T_SSR_UPDATE_INTERVAL(co->UpdateInterval) … … 475 521 T_SSR_SOLUTION_ID(co->SSRSolutionID) 476 522 T_NO_OF_SATELLITES(co->NumberOfSat[s]) 477 for (i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i)478 {479 switch(s)480 {481 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:482 case CLOCKORBIT_SATSBAS:case CLOCKORBIT_SATBDS:483 T_GPS_SATELLITE_ID(co->Sat[i].ID)484 break;485 case CLOCKORBIT_SATQZSS:486 T_QZSS_SATELLITE_ID(co->Sat[i].ID)487 break;488 case CLOCKORBIT_SATGLONASS:489 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)490 break;523 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) { 524 switch (s) { 525 case CLOCKORBIT_SATGPS: 526 case CLOCKORBIT_SATGALILEO: 527 case CLOCKORBIT_SATSBAS: 528 case CLOCKORBIT_SATBDS: 529 T_GPS_SATELLITE_ID(co->Sat[i].ID) 530 break; 531 case CLOCKORBIT_SATQZSS: 532 T_QZSS_SATELLITE_ID(co->Sat[i].ID) 533 break; 534 case CLOCKORBIT_SATGLONASS: 535 T_GLONASS_SATELLITE_ID(co->Sat[i].ID) 536 break; 491 537 } 492 538 T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy)) … … 499 545 500 546 size_t MakeCodeBias(const struct CodeBias *b, enum CodeBiasType type, 501 int moremessagesfollow, char *buffer, size_t size) 502 { 547 int moremessagesfollow, char *buffer, size_t size) { 503 548 unsigned int s, i, j; 504 549 505 550 STARTDATA 506 551 507 for(s = 0; s < CLOCKORBIT_SATNUM; ++s) 508 { 509 if(b->NumberOfSat[s] && (type == BTYPE_AUTO || type == corbase[s]+COBOFS_BIAS)) 510 { 552 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) { 553 if (b->NumberOfSat[s] && (type == BTYPE_AUTO || type == corbase[s] + COBOFS_BIAS)) { 511 554 INITBLOCK 512 T_MESSAGE_NUMBER(corbase[s]+COBOFS_BIAS) 513 switch(s) 514 { 515 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 516 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 517 case CLOCKORBIT_SATBDS: 518 T_GPS_EPOCH_TIME(b->EpochTime[s]) 519 break; 520 case CLOCKORBIT_SATGLONASS: 521 T_GLONASS_EPOCH_TIME(b->EpochTime[s]) 522 break; 555 T_MESSAGE_NUMBER(corbase[s] + COBOFS_BIAS) 556 switch (s) { 557 case CLOCKORBIT_SATGPS: 558 case CLOCKORBIT_SATGALILEO: 559 case CLOCKORBIT_SATQZSS: 560 case CLOCKORBIT_SATSBAS: 561 case CLOCKORBIT_SATBDS: 562 T_GPS_EPOCH_TIME(b->EpochTime[s]) 563 break; 564 case CLOCKORBIT_SATGLONASS: 565 T_GLONASS_EPOCH_TIME(b->EpochTime[s]) 566 break; 523 567 } 524 568 T_SSR_UPDATE_INTERVAL(b->UpdateInterval) … … 528 572 T_SSR_SOLUTION_ID(b->SSRSolutionID) 529 573 T_NO_OF_SATELLITES(b->NumberOfSat[s]) 530 for (i = satoffset[s]; i < satoffset[s]+b->NumberOfSat[s]; ++i)531 {532 switch(s)533 {534 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:535 case CLOCKORBIT_SATSBAS:case CLOCKORBIT_SATBDS:536 T_GPS_SATELLITE_ID(b->Sat[i].ID)537 break;538 case CLOCKORBIT_SATQZSS:539 T_QZSS_SATELLITE_ID(b->Sat[i].ID)540 break;541 case CLOCKORBIT_SATGLONASS:542 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)543 break;574 for (i = satoffset[s]; i < satoffset[s] + b->NumberOfSat[s]; ++i) { 575 switch (s) { 576 case CLOCKORBIT_SATGPS: 577 case CLOCKORBIT_SATGALILEO: 578 case CLOCKORBIT_SATSBAS: 579 case CLOCKORBIT_SATBDS: 580 T_GPS_SATELLITE_ID(b->Sat[i].ID) 581 break; 582 case CLOCKORBIT_SATQZSS: 583 T_QZSS_SATELLITE_ID(b->Sat[i].ID) 584 break; 585 case CLOCKORBIT_SATGLONASS: 586 T_GLONASS_SATELLITE_ID(b->Sat[i].ID) 587 break; 544 588 } 545 589 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases) 546 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j) 547 { 590 for (j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j) { 548 591 T_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type) 549 592 T_CODE_BIAS(b->Sat[i].Biases[j].Bias) … … 557 600 558 601 size_t MakePhaseBias(const struct PhaseBias *b, enum PhaseBiasType type, 559 int moremessagesfollow, char *buffer, size_t size) 560 { 602 int moremessagesfollow, char *buffer, size_t size) { 561 603 unsigned int s, i, j; 562 604 563 605 STARTDATA 564 606 565 for(s = 0; s < CLOCKORBIT_SATNUM; ++s) 566 { 567 if(b->NumberOfSat[s] && (type == PBTYPE_AUTO || type == s+PBTYPE_BASE)) 568 { 607 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) { 608 if (b->NumberOfSat[s] && (type == PBTYPE_AUTO || type == s + PBTYPE_BASE)) { 569 609 INITBLOCK 570 T_MESSAGE_NUMBER(s+PBTYPE_BASE) 571 switch(s) 572 { 573 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 574 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 575 case CLOCKORBIT_SATBDS: 576 T_GPS_EPOCH_TIME(b->EpochTime[s]) 577 break; 578 case CLOCKORBIT_SATGLONASS: 579 T_GLONASS_EPOCH_TIME(b->EpochTime[s]) 580 break; 610 T_MESSAGE_NUMBER(s + PBTYPE_BASE) 611 switch (s) { 612 case CLOCKORBIT_SATGPS: 613 case CLOCKORBIT_SATGALILEO: 614 case CLOCKORBIT_SATQZSS: 615 case CLOCKORBIT_SATSBAS: 616 case CLOCKORBIT_SATBDS: 617 T_GPS_EPOCH_TIME(b->EpochTime[s]) 618 break; 619 case CLOCKORBIT_SATGLONASS: 620 T_GLONASS_EPOCH_TIME(b->EpochTime[s]) 621 break; 581 622 } 582 623 T_SSR_UPDATE_INTERVAL(b->UpdateInterval) … … 588 629 T_MW_CONSISTENCY_INDICATOR(b->MWConsistencyIndicator ? 1 : 0) 589 630 T_NO_OF_SATELLITES(b->NumberOfSat[s]) 590 for (i = satoffset[s]; i < satoffset[s]+b->NumberOfSat[s]; ++i)591 {592 switch(s)593 {594 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:595 case CLOCKORBIT_SATSBAS:case CLOCKORBIT_SATBDS:596 T_GPS_SATELLITE_ID(b->Sat[i].ID)597 break;598 case CLOCKORBIT_SATQZSS:599 T_QZSS_SATELLITE_ID(b->Sat[i].ID)600 break;601 case CLOCKORBIT_SATGLONASS:602 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)603 break;631 for (i = satoffset[s]; i < satoffset[s] + b->NumberOfSat[s]; ++i) { 632 switch (s) { 633 case CLOCKORBIT_SATGPS: 634 case CLOCKORBIT_SATGALILEO: 635 case CLOCKORBIT_SATSBAS: 636 case CLOCKORBIT_SATBDS: 637 T_GPS_SATELLITE_ID(b->Sat[i].ID) 638 break; 639 case CLOCKORBIT_SATQZSS: 640 T_QZSS_SATELLITE_ID(b->Sat[i].ID) 641 break; 642 case CLOCKORBIT_SATGLONASS: 643 T_GLONASS_SATELLITE_ID(b->Sat[i].ID) 644 break; 604 645 } 605 646 T_NO_OF_PHASE_BIASES(b->Sat[i].NumberOfPhaseBiases) 606 647 T_YAW_ANGLE(b->Sat[i].YawAngle) 607 648 T_YAW_RATE(b->Sat[i].YawRate) 608 for(j = 0; j < b->Sat[i].NumberOfPhaseBiases; ++j) 609 { 649 for (j = 0; j < b->Sat[i].NumberOfPhaseBiases; ++j) { 610 650 T_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type) 611 T_INTEGER_INDICATOR(b->Sat[i].Biases[j].SignalIntegerIndicator ? 1 : 0) 612 T_WIDE_LANE_INDICATOR(b->Sat[i].Biases[j].SignalsWideLaneIntegerIndicator) 613 T_DISCONTINUITY_COUNTER(b->Sat[i].Biases[j].SignalDiscontinuityCounter) 651 T_INTEGER_INDICATOR( 652 b->Sat[i].Biases[j].SignalIntegerIndicator ? 1 : 0) 653 T_WIDE_LANE_INDICATOR( 654 b->Sat[i].Biases[j].SignalsWideLaneIntegerIndicator) 655 T_DISCONTINUITY_COUNTER( 656 b->Sat[i].Biases[j].SignalDiscontinuityCounter) 614 657 T_PHASE_BIAS(b->Sat[i].Biases[j].Bias) 615 658 } … … 621 664 } 622 665 623 size_t MakeVTEC(const struct VTEC *v, int moremessagesfollow, char *buffer, 624 size_t size) 625 { 666 size_t MakeVTEC(const struct VTEC *v, int moremessagesfollow, char *buffer, size_t size) { 626 667 unsigned int l, o, d; 627 668 628 669 STARTDATA 629 INITBLOCK670 INITBLOCK 630 671 T_MESSAGE_NUMBER(VTEC_BASE) 631 672 … … 638 679 T_VTEC_QUALITY_INDICATOR(v->Quality) 639 680 T_NO_IONO_LAYERS(v->NumLayers) 640 for(l = 0; l < v->NumLayers; ++l) 641 { 681 for (l = 0; l < v->NumLayers; ++l) { 642 682 T_IONO_HEIGHT(v->Layers[l].Height) 643 683 T_IONO_DEGREE(v->Layers[l].Degree) 644 684 T_IONO_ORDER(v->Layers[l].Order) 645 for(o = 0; o <= v->Layers[l].Order; ++o) 646 { 647 for(d = o; d <= v->Layers[l].Degree; ++d) 648 { 685 for (o = 0; o <= v->Layers[l].Order; ++o) { 686 for (d = o; d <= v->Layers[l].Degree; ++d) { 649 687 T_IONO_COEFF(v->Layers[l].Cosinus[d][o]) 650 688 } 651 689 } 652 for(o = 1; o <= v->Layers[l].Order; ++o) 653 { 654 for(d = o; d <= v->Layers[l].Degree; ++d) 655 { 690 for (o = 1; o <= v->Layers[l].Order; ++o) { 691 for (d = o; d <= v->Layers[l].Degree; ++d) { 656 692 T_IONO_COEFF(v->Layers[l].Sinus[d][o]) 657 693 } … … 669 705 uint64_t bitbuffer=0; 670 706 671 #define LOADBITS(a) \ 672 { \ 707 #define LOADBITS(a) { \ 673 708 while((a) > numbits) \ 674 709 { \ … … 680 715 681 716 /* extract bits from data stream 682 b = variable to store result, a = number of bits */ 683 #define GETBITS(b, a) \ 684 { \ 717 b = variable to store result, a = number of bits */ 718 #define GETBITS(b, a) { \ 685 719 LOADBITS(a) \ 686 720 b = (bitbuffer<<(64-numbits))>>(64-(a)); \ … … 689 723 690 724 /* extract bits from data stream 691 b = variable to store result, a = number of bits */ 692 #define GETBITSFACTOR(b, a, c) \ 693 { \ 725 b = variable to store result, a = number of bits */ 726 #define GETBITSFACTOR(b, a, c) { \ 694 727 LOADBITS(a) \ 695 728 b = ((bitbuffer<<(64-numbits))>>(64-(a)))*(c); \ … … 698 731 699 732 /* extract signed floating value from data stream 700 b = variable to store result, a = number of bits */ 701 #define GETFLOATSIGN(b, a, c) \ 702 { \ 733 b = variable to store result, a = number of bits */ 734 #define GETFLOATSIGN(b, a, c) { \ 703 735 LOADBITS(a) \ 704 736 b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \ … … 707 739 708 740 /* extract floating value from data stream 709 b = variable to store result, a = number of bits, c = scale factor */ 710 #define GETFLOAT(b, a, c) \ 711 { \ 741 b = variable to store result, a = number of bits, c = scale factor */ 742 #define GETFLOAT(b, a, c) { \ 712 743 LOADBITS(a) \ 713 744 b = ((double)((bitbuffer<<(sizeof(bitbuffer)*8-numbits))>>(sizeof(bitbuffer)*8-(a))))*(c); \ … … 782 813 #define G_IONO_HEIGHT(a) GETFLOAT(a, 8 , 10000.0) 783 814 784 enum GCOB_RETURN GetSSR(struct ClockOrbit *co, struct CodeBias *b, struct VTEC *v, 785 struct PhaseBias *pb, const char *buffer, size_t size, int *bytesused) 786 { 787 int mmi=0, h, rs; 815 enum GCOB_RETURN GetSSR(struct ClockOrbit *co, struct CodeBias *b,struct VTEC *v, 816 struct PhaseBias *pb, const char *buffer, size_t size, int *bytesused) { 817 int mmi = 0, h, rs; 788 818 unsigned int type, pos, i, j, s, nums, id; 789 819 size_t sizeofrtcmblock; … … 791 821 DECODESTART 792 822 793 if (size < 7)823 if (size < 7) 794 824 return GCOBR_SHORTBUFFER; 795 825 … … 802 832 G_SIZE(sizeofrtcmblock); 803 833 804 if ((unsigned char)h != 0xD3 || rs)834 if ((unsigned char) h != 0xD3 || rs) 805 835 return GCOBR_UNKNOWNDATA; 806 if (size < sizeofrtcmblock + 3) /* 3 header bytes already removed */836 if (size < sizeofrtcmblock + 3) /* 3 header bytes already removed */ 807 837 return GCOBR_MESSAGEEXCEEDSBUFFER; 808 if (CRC24(sizeofrtcmblock+3, (const unsigned char *) blockstart) !=809 (uint32_t)((((unsigned char)buffer[sizeofrtcmblock])<<16)|810 (((unsigned char)buffer[sizeofrtcmblock+1])<<8)|811 (((unsigned char)buffer[sizeofrtcmblock+2]))))838 if (CRC24(sizeofrtcmblock + 3, (const unsigned char *) blockstart) != 839 (uint32_t) ((((unsigned char) buffer[sizeofrtcmblock]) << 16) | 840 (((unsigned char) buffer[sizeofrtcmblock + 1]) << 8) | 841 (((unsigned char) buffer[sizeofrtcmblock + 2])))) 812 842 return GCOBR_CRCMISMATCH; 813 843 size = sizeofrtcmblock; /* reduce size, so overflows are detected */ … … 815 845 G_MESSAGE_NUMBER(type) 816 846 #ifdef DEBUG 817 fprintf(stderr, "type %d size %d\n",type,sizeofrtcmblock); 818 #endif 819 if(bytesused) 820 *bytesused = sizeofrtcmblock+6; 821 if(type == VTEC_BASE) 822 { 847 fprintf(stderr, "type %d size %d\n",type,sizeofrtcmblock); 848 #endif 849 if (bytesused) 850 *bytesused = sizeofrtcmblock + 6; 851 if (type == VTEC_BASE) { 823 852 unsigned int l, o, d; 824 if(!v) return GCOBR_NOVTECPARAMETER; 853 if (!v) 854 return GCOBR_NOVTECPARAMETER; 825 855 memset(v, 0, sizeof(*v)); 826 856 G_EPOCH_TIME(v->EpochTime) … … 832 862 G_VTEC_QUALITY_INDICATOR(v->Quality) 833 863 G_NO_IONO_LAYERS(v->NumLayers) 834 for(l = 0; l < v->NumLayers; ++l) 835 { 864 for (l = 0; l < v->NumLayers; ++l) { 836 865 G_IONO_HEIGHT(v->Layers[l].Height) 837 866 G_IONO_DEGREE(v->Layers[l].Degree) 838 867 G_IONO_ORDER(v->Layers[l].Order) 839 for(o = 0; o <= v->Layers[l].Order; ++o) 840 { 841 for(d = o; d <= v->Layers[l].Degree; ++d) 842 { 868 for (o = 0; o <= v->Layers[l].Order; ++o) { 869 for (d = o; d <= v->Layers[l].Degree; ++d) { 843 870 G_IONO_COEFF(v->Layers[l].Cosinus[d][o]) 844 871 } 845 872 } 846 for(o = 1; o <= v->Layers[l].Order; ++o) 847 { 848 for(d = o; d <= v->Layers[l].Degree; ++d) 849 { 873 for (o = 1; o <= v->Layers[l].Order; ++o) { 874 for (d = o; d <= v->Layers[l].Degree; ++d) { 850 875 G_IONO_COEFF(v->Layers[l].Sinus[d][o]) 851 876 } … … 854 879 #ifdef DEBUG 855 880 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type) 856 881 numbits += 8; 857 882 fprintf(stderr, "numbits left %d\n",numbits); 858 883 #endif 859 884 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK; 860 885 } 861 for(s = CLOCKORBIT_SATNUM; s-- > 0;) 862 { 863 if(type == PBTYPE_BASE+s) 864 { 865 if(!pb) return GCOBR_NOPHASEBIASPARAMETER; 886 for (s = CLOCKORBIT_SATNUM; s-- > 0;) { 887 if (type == PBTYPE_BASE + s) { 888 if (!pb) 889 return GCOBR_NOPHASEBIASPARAMETER; 866 890 pb->messageType = type; 867 switch(s) 868 { 869 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 870 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 871 case CLOCKORBIT_SATBDS: 872 G_GPS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s]) 873 break; 874 case CLOCKORBIT_SATGLONASS: 875 G_GLONASS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s]) 876 break; 891 switch (s) { 892 case CLOCKORBIT_SATGPS: 893 case CLOCKORBIT_SATGALILEO: 894 case CLOCKORBIT_SATQZSS: 895 case CLOCKORBIT_SATSBAS: 896 case CLOCKORBIT_SATBDS: 897 G_GPS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s]) 898 break; 899 case CLOCKORBIT_SATGLONASS: 900 G_GLONASS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s]) 901 break; 877 902 } 878 903 G_SSR_UPDATE_INTERVAL(pb->UpdateInterval) … … 884 909 G_MW_CONSISTENCY_INDICATOR(pb->MWConsistencyIndicator) 885 910 G_NO_OF_SATELLITES(nums) 886 for (i = 0; i < nums; ++i)887 {888 switch(s)889 {890 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:891 case CLOCKORBIT_SATSBAS:case CLOCKORBIT_SATBDS:892 G_GPS_SATELLITE_ID(id)893 break;894 case CLOCKORBIT_SATQZSS:895 G_QZSS_SATELLITE_ID(id)896 break;897 case CLOCKORBIT_SATGLONASS:898 G_GLONASS_SATELLITE_ID(id)899 break;911 for (i = 0; i < nums; ++i) { 912 switch (s) { 913 case CLOCKORBIT_SATGPS: 914 case CLOCKORBIT_SATGALILEO: 915 case CLOCKORBIT_SATSBAS: 916 case CLOCKORBIT_SATBDS: 917 G_GPS_SATELLITE_ID(id) 918 break; 919 case CLOCKORBIT_SATQZSS: 920 G_QZSS_SATELLITE_ID(id) 921 break; 922 case CLOCKORBIT_SATGLONASS: 923 G_GLONASS_SATELLITE_ID(id) 924 break; 900 925 } 901 for(pos = satoffset[s]; pos < satoffset[s]+pb->NumberOfSat[s] && pb->Sat[pos].ID != id; ++pos) 926 for (pos = satoffset[s]; 927 pos < satoffset[s] + pb->NumberOfSat[s] && pb->Sat[pos].ID != id; 928 ++pos) 902 929 ; 903 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH; 904 else if(pos == pb->NumberOfSat[s] + satoffset[s]) ++pb->NumberOfSat[s]; 930 if (pos >= satoffset[s + 1]) 931 return GCOBR_DATAMISMATCH; 932 else if (pos == pb->NumberOfSat[s] + satoffset[s]) 933 ++pb->NumberOfSat[s]; 905 934 pb->Sat[pos].ID = id; 906 935 … … 908 937 G_YAW_ANGLE(pb->Sat[pos].YawAngle) 909 938 G_YAW_RATE(pb->Sat[pos].YawRate) 910 for(j = 0; j < pb->Sat[pos].NumberOfPhaseBiases; ++j) 911 { 939 for (j = 0; j < pb->Sat[pos].NumberOfPhaseBiases; ++j) { 912 940 G_SIGNAL_IDENTIFIER(pb->Sat[pos].Biases[j].Type) 913 941 G_INTEGER_INDICATOR(pb->Sat[pos].Biases[j].SignalIntegerIndicator) 914 G_WIDE_LANE_INDICATOR(pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator) 915 G_DISCONTINUITY_COUNTER(pb->Sat[pos].Biases[j].SignalDiscontinuityCounter) 942 G_WIDE_LANE_INDICATOR( 943 pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator) 944 G_DISCONTINUITY_COUNTER( 945 pb->Sat[pos].Biases[j].SignalDiscontinuityCounter) 916 946 G_PHASE_BIAS(pb->Sat[pos].Biases[j].Bias) 917 947 } … … 919 949 #ifdef DEBUG 920 950 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type) 921 951 numbits += 8; 922 952 fprintf(stderr, "numbits left %d\n",numbits); 923 953 #endif 924 954 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK; 925 955 } 926 else if (type >= corbase[s])927 {928 switch(type-corbase[s])929 {930 case COBOFS_ORBIT:931 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;932 co->messageType = type;933 switch(s)934 {935 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:936 case CLOCKORBIT_SATQZSS:case CLOCKORBIT_SATSBAS:937 case CLOCKORBIT_SATBDS:938 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])939 break;940 case CLOCKORBIT_SATGLONASS:941 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])942 break;943 }944 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)945 G_MULTIPLE_MESSAGE_INDICATOR(mmi)946 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)947 G_SSR_IOD(co->SSRIOD)948 G_SSR_PROVIDER_ID(co->SSRProviderID)949 G_SSR_SOLUTION_ID(co->SSRSolutionID)950 G_NO_OF_SATELLITES(nums)951 co->Supplied[COBOFS_ORBIT] |= 1;956 else if (type >= corbase[s]) { 957 switch (type - corbase[s]) { 958 case COBOFS_ORBIT: 959 if (!co) 960 return GCOBR_NOCLOCKORBITPARAMETER; 961 co->messageType = type; 962 switch (s) { 963 case CLOCKORBIT_SATGPS: 964 case CLOCKORBIT_SATGALILEO: 965 case CLOCKORBIT_SATQZSS: 966 case CLOCKORBIT_SATSBAS: 967 case CLOCKORBIT_SATBDS: 968 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 969 break; 970 case CLOCKORBIT_SATGLONASS: 971 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 972 break; 973 } 974 G_SSR_UPDATE_INTERVAL(co->UpdateInterval) 975 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 976 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum) 977 G_SSR_IOD(co->SSRIOD) 978 G_SSR_PROVIDER_ID(co->SSRProviderID) 979 G_SSR_SOLUTION_ID(co->SSRSolutionID) 980 G_NO_OF_SATELLITES(nums) 981 co->Supplied[COBOFS_ORBIT] |= 1; 952 982 #ifdef DEBUG 953 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->EpochTime[s], 954 co->UpdateInterval,mmi,co->NumberOfSat[s],nums, co->SatRefDatum); 955 #endif 956 for(i = 0; i < nums; ++i) 957 { 958 switch(s) 959 { 960 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 961 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS: 962 G_GPS_SATELLITE_ID(id) 963 break; 964 case CLOCKORBIT_SATQZSS: 965 G_QZSS_SATELLITE_ID(id) 966 break; 967 case CLOCKORBIT_SATGLONASS: 968 G_GLONASS_SATELLITE_ID(id) 969 break; 983 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->EpochTime[s], 984 co->UpdateInterval,mmi,co->NumberOfSat[s],nums, co->SatRefDatum); 985 #endif 986 for (i = 0; i < nums; ++i) { 987 switch (s) { 988 case CLOCKORBIT_SATGPS: 989 case CLOCKORBIT_SATGALILEO: 990 case CLOCKORBIT_SATSBAS: 991 case CLOCKORBIT_SATBDS: 992 G_GPS_SATELLITE_ID(id) 993 break; 994 case CLOCKORBIT_SATQZSS: 995 G_QZSS_SATELLITE_ID(id) 996 break; 997 case CLOCKORBIT_SATGLONASS: 998 G_GLONASS_SATELLITE_ID(id) 999 break; 1000 } 1001 for (pos = satoffset[s]; 1002 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id; 1003 ++pos) 1004 ; 1005 if (pos >= satoffset[s + 1]) 1006 return GCOBR_DATAMISMATCH; 1007 else if (pos == co->NumberOfSat[s] + satoffset[s]) 1008 ++co->NumberOfSat[s]; 1009 co->Sat[pos].ID = id; 1010 1011 switch (s) { 1012 case CLOCKORBIT_SATGPS: 1013 case CLOCKORBIT_SATQZSS: 1014 G_GPS_IODE(co->Sat[pos].IOD) 1015 break; 1016 case CLOCKORBIT_SATGLONASS: 1017 G_GLONASS_IOD(co->Sat[pos].IOD) 1018 break; 1019 case CLOCKORBIT_SATGALILEO: 1020 G_GALILEO_IOD(co->Sat[pos].IOD) 1021 break; 1022 case CLOCKORBIT_SATSBAS: 1023 G_SBAS_T0MOD(co->Sat[pos].toe) 1024 G_SBAS_IODCRC(co->Sat[pos].IOD) 1025 break; 1026 case CLOCKORBIT_SATBDS: 1027 G_BDS_TOEMOD(co->Sat[pos].toe) 1028 G_BDS_IOD(co->Sat[pos].IOD) 1029 break; 1030 } 1031 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial) 1032 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack) 1033 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack) 1034 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial) 1035 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack) 1036 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack) 1037 #ifdef DEBUG 1038 fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n", 1039 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial, 1040 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack, 1041 co->Sat[pos].Orbit.DotDeltaRadial, 1042 co->Sat[pos].Orbit.DotDeltaAlongTrack, 1043 co->Sat[pos].Orbit.DotDeltaCrossTrack); 1044 #endif 970 1045 } 971 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos) 972 ; 973 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH; 974 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s]; 975 co->Sat[pos].ID = id; 976 977 switch(s) 978 { 979 case CLOCKORBIT_SATGPS: 980 case CLOCKORBIT_SATQZSS: 981 G_GPS_IODE(co->Sat[pos].IOD) 982 break; 983 case CLOCKORBIT_SATGLONASS: 984 G_GLONASS_IOD(co->Sat[pos].IOD) 985 break; 986 case CLOCKORBIT_SATGALILEO: 987 G_GALILEO_IOD(co->Sat[pos].IOD) 988 break; 989 case CLOCKORBIT_SATSBAS: 990 G_SBAS_T0MOD(co->Sat[pos].toe) 991 G_SBAS_IODCRC(co->Sat[pos].IOD) 992 break; 993 case CLOCKORBIT_SATBDS: 994 G_BDS_TOEMOD(co->Sat[pos].toe) 995 G_BDS_IOD(co->Sat[pos].IOD) 996 break; 1046 break; 1047 case COBOFS_CLOCK: 1048 if (!co) 1049 return GCOBR_NOCLOCKORBITPARAMETER; 1050 co->messageType = type; 1051 switch (s) { 1052 case CLOCKORBIT_SATGPS: 1053 case CLOCKORBIT_SATGALILEO: 1054 case CLOCKORBIT_SATQZSS: 1055 case CLOCKORBIT_SATSBAS: 1056 case CLOCKORBIT_SATBDS: 1057 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1058 break; 1059 case CLOCKORBIT_SATGLONASS: 1060 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1061 break; 997 1062 } 998 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial) 999 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack) 1000 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack) 1001 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial) 1002 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack) 1003 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack) 1063 G_SSR_UPDATE_INTERVAL(co->UpdateInterval) 1064 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 1065 G_SSR_IOD(co->SSRIOD) 1066 G_SSR_PROVIDER_ID(co->SSRProviderID) 1067 G_SSR_SOLUTION_ID(co->SSRSolutionID) 1068 G_NO_OF_SATELLITES(nums) 1069 co->Supplied[COBOFS_CLOCK] |= 1; 1004 1070 #ifdef DEBUG 1005 fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n", 1006 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial, 1007 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack, 1008 co->Sat[pos].Orbit.DotDeltaRadial, 1009 co->Sat[pos].Orbit.DotDeltaAlongTrack, 1010 co->Sat[pos].Orbit.DotDeltaCrossTrack); 1011 #endif 1012 } 1013 break; 1014 case COBOFS_CLOCK: 1015 if(!co) return GCOBR_NOCLOCKORBITPARAMETER; 1016 co->messageType = type; 1017 switch(s) 1018 { 1019 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 1020 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 1021 case CLOCKORBIT_SATBDS: 1022 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1023 break; 1024 case CLOCKORBIT_SATGLONASS: 1025 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1026 break; 1027 } 1028 G_SSR_UPDATE_INTERVAL(co->UpdateInterval) 1029 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 1030 G_SSR_IOD(co->SSRIOD) 1031 G_SSR_PROVIDER_ID(co->SSRProviderID) 1032 G_SSR_SOLUTION_ID(co->SSRSolutionID) 1033 G_NO_OF_SATELLITES(nums) 1034 co->Supplied[COBOFS_CLOCK] |= 1; 1071 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->EpochTime[s], 1072 co->UpdateInterval,mmi,co->NumberOfSat[s],nums); 1073 #endif 1074 for (i = 0; i < nums; ++i) { 1075 switch (s) { 1076 case CLOCKORBIT_SATGPS: 1077 case CLOCKORBIT_SATGALILEO: 1078 case CLOCKORBIT_SATSBAS: 1079 case CLOCKORBIT_SATBDS: 1080 G_GPS_SATELLITE_ID(id) 1081 break; 1082 case CLOCKORBIT_SATQZSS: 1083 G_QZSS_SATELLITE_ID(id) 1084 break; 1085 case CLOCKORBIT_SATGLONASS: 1086 G_GLONASS_SATELLITE_ID(id) 1087 break; 1088 } 1089 for (pos = satoffset[s]; 1090 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id; 1091 ++pos) 1092 ; 1093 if (pos >= satoffset[s + 1]) 1094 return GCOBR_DATAMISMATCH; 1095 else if (pos == co->NumberOfSat[s] + satoffset[s]) 1096 ++co->NumberOfSat[s]; 1097 co->Sat[pos].ID = id; 1098 1099 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0) 1100 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1) 1101 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2) 1035 1102 #ifdef DEBUG 1036 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->EpochTime[s], 1037 co->UpdateInterval,mmi,co->NumberOfSat[s],nums); 1038 #endif 1039 for(i = 0; i < nums; ++i) 1040 { 1041 switch(s) 1042 { 1043 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 1044 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS: 1045 G_GPS_SATELLITE_ID(id) 1046 break; 1047 case CLOCKORBIT_SATQZSS: 1048 G_QZSS_SATELLITE_ID(id) 1049 break; 1050 case CLOCKORBIT_SATGLONASS: 1051 G_GLONASS_SATELLITE_ID(id) 1052 break; 1103 fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n", 1104 co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1, 1105 co->Sat[pos].Clock.DeltaA2); 1106 #endif 1053 1107 } 1054 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos) 1055 ; 1056 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH; 1057 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s]; 1058 co->Sat[pos].ID = id; 1059 1060 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0) 1061 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1) 1062 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2) 1063 #ifdef DEBUG 1064 fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n", 1065 co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1, 1066 co->Sat[pos].Clock.DeltaA2); 1067 #endif 1068 } 1069 break; 1070 case COBOFS_COMBINED: 1071 if(!co) return GCOBR_NOCLOCKORBITPARAMETER; 1072 co->messageType = type; 1073 switch(s) 1074 { 1075 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 1076 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 1077 case CLOCKORBIT_SATBDS: 1078 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1079 break; 1080 case CLOCKORBIT_SATGLONASS: 1081 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1082 break; 1083 } 1084 G_SSR_UPDATE_INTERVAL(co->UpdateInterval) 1085 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 1086 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum) 1087 G_SSR_IOD(co->SSRIOD) 1088 G_SSR_PROVIDER_ID(co->SSRProviderID) 1089 G_SSR_SOLUTION_ID(co->SSRSolutionID) 1090 G_NO_OF_SATELLITES(nums) 1091 co->Supplied[COBOFS_ORBIT] |= 1; 1092 co->Supplied[COBOFS_CLOCK] |= 1; 1093 for(i = 0; i < nums; ++i) 1094 { 1095 switch(s) 1096 { 1097 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 1098 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS: 1099 G_GPS_SATELLITE_ID(id) 1100 break; 1101 case CLOCKORBIT_SATQZSS: 1102 G_QZSS_SATELLITE_ID(id) 1103 break; 1104 case CLOCKORBIT_SATGLONASS: 1105 G_GLONASS_SATELLITE_ID(id) 1106 break; 1108 break; 1109 case COBOFS_COMBINED: 1110 if (!co) 1111 return GCOBR_NOCLOCKORBITPARAMETER; 1112 co->messageType = type; 1113 switch (s) { 1114 case CLOCKORBIT_SATGPS: 1115 case CLOCKORBIT_SATGALILEO: 1116 case CLOCKORBIT_SATQZSS: 1117 case CLOCKORBIT_SATSBAS: 1118 case CLOCKORBIT_SATBDS: 1119 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1120 break; 1121 case CLOCKORBIT_SATGLONASS: 1122 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1123 break; 1107 1124 } 1108 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos) 1109 ; 1110 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH; 1111 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s]; 1112 co->Sat[pos].ID = id; 1113 1114 switch(s) 1115 { 1116 case CLOCKORBIT_SATGPS: 1117 case CLOCKORBIT_SATQZSS: 1118 G_GPS_IODE(co->Sat[pos].IOD) 1119 break; 1120 case CLOCKORBIT_SATGLONASS: 1121 G_GLONASS_IOD(co->Sat[pos].IOD) 1122 break; 1123 case CLOCKORBIT_SATGALILEO: 1124 G_GALILEO_IOD(co->Sat[pos].IOD) 1125 break; 1126 case CLOCKORBIT_SATSBAS: 1127 G_SBAS_T0MOD(co->Sat[pos].toe) 1128 G_SBAS_IODCRC(co->Sat[pos].IOD) 1129 break; 1130 case CLOCKORBIT_SATBDS: 1131 G_BDS_TOEMOD(co->Sat[pos].toe) 1132 G_BDS_IOD(co->Sat[pos].IOD) 1133 break; 1125 G_SSR_UPDATE_INTERVAL(co->UpdateInterval) 1126 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 1127 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum) 1128 G_SSR_IOD(co->SSRIOD) 1129 G_SSR_PROVIDER_ID(co->SSRProviderID) 1130 G_SSR_SOLUTION_ID(co->SSRSolutionID) 1131 G_NO_OF_SATELLITES(nums) 1132 co->Supplied[COBOFS_ORBIT] |= 1; 1133 co->Supplied[COBOFS_CLOCK] |= 1; 1134 for (i = 0; i < nums; ++i) { 1135 switch (s) { 1136 case CLOCKORBIT_SATGPS: 1137 case CLOCKORBIT_SATGALILEO: 1138 case CLOCKORBIT_SATSBAS: 1139 case CLOCKORBIT_SATBDS: 1140 G_GPS_SATELLITE_ID(id) 1141 break; 1142 case CLOCKORBIT_SATQZSS: 1143 G_QZSS_SATELLITE_ID(id) 1144 break; 1145 case CLOCKORBIT_SATGLONASS: 1146 G_GLONASS_SATELLITE_ID(id) 1147 break; 1148 } 1149 for (pos = satoffset[s]; 1150 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id; 1151 ++pos) 1152 ; 1153 if (pos >= satoffset[s + 1]) 1154 return GCOBR_DATAMISMATCH; 1155 else if (pos == co->NumberOfSat[s] + satoffset[s]) 1156 ++co->NumberOfSat[s]; 1157 co->Sat[pos].ID = id; 1158 1159 switch (s) { 1160 case CLOCKORBIT_SATGPS: 1161 case CLOCKORBIT_SATQZSS: 1162 G_GPS_IODE(co->Sat[pos].IOD) 1163 break; 1164 case CLOCKORBIT_SATGLONASS: 1165 G_GLONASS_IOD(co->Sat[pos].IOD) 1166 break; 1167 case CLOCKORBIT_SATGALILEO: 1168 G_GALILEO_IOD(co->Sat[pos].IOD) 1169 break; 1170 case CLOCKORBIT_SATSBAS: 1171 G_SBAS_T0MOD(co->Sat[pos].toe) 1172 G_SBAS_IODCRC(co->Sat[pos].IOD) 1173 break; 1174 case CLOCKORBIT_SATBDS: 1175 G_BDS_TOEMOD(co->Sat[pos].toe) 1176 G_BDS_IOD(co->Sat[pos].IOD) 1177 break; 1178 } 1179 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial) 1180 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack) 1181 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack) 1182 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial) 1183 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack) 1184 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack) 1185 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0) 1186 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1) 1187 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2) 1134 1188 } 1135 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial) 1136 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack) 1137 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack) 1138 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial) 1139 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack) 1140 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack) 1141 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0) 1142 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1) 1143 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2) 1144 } 1145 break; 1146 case COBOFS_URA: 1147 if(!co) return GCOBR_NOCLOCKORBITPARAMETER; 1148 co->messageType = type; 1149 switch(s) 1150 { 1151 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 1152 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 1153 case CLOCKORBIT_SATBDS: 1154 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1155 break; 1156 case CLOCKORBIT_SATGLONASS: 1157 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1158 break; 1159 } 1160 G_SSR_UPDATE_INTERVAL(co->UpdateInterval) 1161 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 1162 G_SSR_IOD(co->SSRIOD) 1163 G_SSR_PROVIDER_ID(co->SSRProviderID) 1164 G_SSR_SOLUTION_ID(co->SSRSolutionID) 1165 G_NO_OF_SATELLITES(nums) 1166 co->Supplied[COBOFS_URA] |= 1; 1167 for(i = 0; i < nums; ++i) 1168 { 1169 switch(s) 1170 { 1171 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 1172 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS: 1173 G_GPS_SATELLITE_ID(id) 1174 break; 1175 case CLOCKORBIT_SATQZSS: 1176 G_QZSS_SATELLITE_ID(id) 1177 break; 1178 case CLOCKORBIT_SATGLONASS: 1179 G_GLONASS_SATELLITE_ID(id) 1180 break; 1189 break; 1190 case COBOFS_URA: 1191 if (!co) 1192 return GCOBR_NOCLOCKORBITPARAMETER; 1193 co->messageType = type; 1194 switch (s) { 1195 case CLOCKORBIT_SATGPS: 1196 case CLOCKORBIT_SATGALILEO: 1197 case CLOCKORBIT_SATQZSS: 1198 case CLOCKORBIT_SATSBAS: 1199 case CLOCKORBIT_SATBDS: 1200 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1201 break; 1202 case CLOCKORBIT_SATGLONASS: 1203 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1204 break; 1181 1205 } 1182 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos) 1183 ; 1184 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH; 1185 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s]; 1186 co->Sat[pos].ID = id; 1187 1188 G_SSR_URA(co->Sat[pos].UserRangeAccuracy) 1189 } 1190 break; 1191 case COBOFS_HR: 1192 if(!co) return GCOBR_NOCLOCKORBITPARAMETER; 1193 co->messageType = type; 1194 switch(s) 1195 { 1196 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 1197 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 1198 case CLOCKORBIT_SATBDS: 1199 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1200 break; 1201 case CLOCKORBIT_SATGLONASS: 1202 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1203 break; 1204 } 1205 G_SSR_UPDATE_INTERVAL(co->UpdateInterval) 1206 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 1207 G_SSR_IOD(co->SSRIOD) 1208 G_SSR_PROVIDER_ID(co->SSRProviderID) 1209 G_SSR_SOLUTION_ID(co->SSRSolutionID) 1210 G_NO_OF_SATELLITES(nums) 1211 co->Supplied[COBOFS_HR] |= 1; 1212 for(i = 0; i < nums; ++i) 1213 { 1214 switch(s) 1215 { 1216 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 1217 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS: 1218 G_GPS_SATELLITE_ID(id) 1219 break; 1220 case CLOCKORBIT_SATQZSS: 1221 G_QZSS_SATELLITE_ID(id) 1222 break; 1223 case CLOCKORBIT_SATGLONASS: 1224 G_GLONASS_SATELLITE_ID(id) 1225 break; 1206 G_SSR_UPDATE_INTERVAL(co->UpdateInterval) 1207 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 1208 G_SSR_IOD(co->SSRIOD) 1209 G_SSR_PROVIDER_ID(co->SSRProviderID) 1210 G_SSR_SOLUTION_ID(co->SSRSolutionID) 1211 G_NO_OF_SATELLITES(nums) 1212 co->Supplied[COBOFS_URA] |= 1; 1213 for (i = 0; i < nums; ++i) { 1214 switch (s) { 1215 case CLOCKORBIT_SATGPS: 1216 case CLOCKORBIT_SATGALILEO: 1217 case CLOCKORBIT_SATSBAS: 1218 case CLOCKORBIT_SATBDS: 1219 G_GPS_SATELLITE_ID(id) 1220 break; 1221 case CLOCKORBIT_SATQZSS: 1222 G_QZSS_SATELLITE_ID(id) 1223 break; 1224 case CLOCKORBIT_SATGLONASS: 1225 G_GLONASS_SATELLITE_ID(id) 1226 break; 1227 } 1228 for (pos = satoffset[s]; 1229 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id; 1230 ++pos) 1231 ; 1232 if (pos >= satoffset[s + 1]) 1233 return GCOBR_DATAMISMATCH; 1234 else if (pos == co->NumberOfSat[s] + satoffset[s]) 1235 ++co->NumberOfSat[s]; 1236 co->Sat[pos].ID = id; 1237 1238 G_SSR_URA(co->Sat[pos].UserRangeAccuracy) 1226 1239 } 1227 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos) 1228 ; 1229 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH; 1230 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s]; 1231 co->Sat[pos].ID = id; 1232 1233 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock) 1234 } 1235 break; 1236 case COBOFS_BIAS: 1237 if(!b) return GCOBR_NOCODEBIASPARAMETER; 1238 b->messageType = type; 1239 switch(s) 1240 { 1241 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 1242 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS: 1243 case CLOCKORBIT_SATBDS: 1244 G_GPS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s]) 1245 break; 1246 case CLOCKORBIT_SATGLONASS: 1247 G_GLONASS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s]) 1248 break; 1249 } 1250 G_SSR_UPDATE_INTERVAL(b->UpdateInterval) 1251 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 1252 G_SSR_IOD(b->SSRIOD) 1253 G_SSR_PROVIDER_ID(b->SSRProviderID) 1254 G_SSR_SOLUTION_ID(b->SSRSolutionID) 1255 G_NO_OF_SATELLITES(nums) 1256 for(i = 0; i < nums; ++i) 1257 { 1258 switch(s) 1259 { 1260 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO: 1261 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS: 1262 G_GPS_SATELLITE_ID(id) 1263 break; 1264 case CLOCKORBIT_SATQZSS: 1265 G_QZSS_SATELLITE_ID(id) 1266 break; 1267 case CLOCKORBIT_SATGLONASS: 1268 G_GLONASS_SATELLITE_ID(id) 1269 break; 1240 break; 1241 case COBOFS_HR: 1242 if (!co) 1243 return GCOBR_NOCLOCKORBITPARAMETER; 1244 co->messageType = type; 1245 switch (s) { 1246 case CLOCKORBIT_SATGPS: 1247 case CLOCKORBIT_SATGALILEO: 1248 case CLOCKORBIT_SATQZSS: 1249 case CLOCKORBIT_SATSBAS: 1250 case CLOCKORBIT_SATBDS: 1251 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1252 break; 1253 case CLOCKORBIT_SATGLONASS: 1254 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s]) 1255 break; 1270 1256 } 1271 for(pos = satoffset[s]; pos < satoffset[s]+b->NumberOfSat[s] && b->Sat[pos].ID != id; ++pos) 1272 ; 1273 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH; 1274 else if(pos == b->NumberOfSat[s] + satoffset[s]) ++b->NumberOfSat[s]; 1275 b->Sat[pos].ID = id; 1276 1277 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases) 1278 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j) 1279 { 1280 G_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type) 1281 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias) 1257 G_SSR_UPDATE_INTERVAL(co->UpdateInterval) 1258 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 1259 G_SSR_IOD(co->SSRIOD) 1260 G_SSR_PROVIDER_ID(co->SSRProviderID) 1261 G_SSR_SOLUTION_ID(co->SSRSolutionID) 1262 G_NO_OF_SATELLITES(nums) 1263 co->Supplied[COBOFS_HR] |= 1; 1264 for (i = 0; i < nums; ++i) { 1265 switch (s) { 1266 case CLOCKORBIT_SATGPS: 1267 case CLOCKORBIT_SATGALILEO: 1268 case CLOCKORBIT_SATSBAS: 1269 case CLOCKORBIT_SATBDS: 1270 G_GPS_SATELLITE_ID(id) 1271 break; 1272 case CLOCKORBIT_SATQZSS: 1273 G_QZSS_SATELLITE_ID(id) 1274 break; 1275 case CLOCKORBIT_SATGLONASS: 1276 G_GLONASS_SATELLITE_ID(id) 1277 break; 1278 } 1279 for (pos = satoffset[s]; 1280 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id; 1281 ++pos) 1282 ; 1283 if (pos >= satoffset[s + 1]) 1284 return GCOBR_DATAMISMATCH; 1285 else if (pos == co->NumberOfSat[s] + satoffset[s]) 1286 ++co->NumberOfSat[s]; 1287 co->Sat[pos].ID = id; 1288 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock) 1282 1289 } 1283 } 1284 break; 1285 default: 1286 continue; 1290 break; 1291 case COBOFS_BIAS: 1292 if (!b) 1293 return GCOBR_NOCODEBIASPARAMETER; 1294 b->messageType = type; 1295 switch (s) { 1296 case CLOCKORBIT_SATGPS: 1297 case CLOCKORBIT_SATGALILEO: 1298 case CLOCKORBIT_SATQZSS: 1299 case CLOCKORBIT_SATSBAS: 1300 case CLOCKORBIT_SATBDS: 1301 G_GPS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s]) 1302 break; 1303 case CLOCKORBIT_SATGLONASS: 1304 G_GLONASS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s]) 1305 break; 1306 } 1307 G_SSR_UPDATE_INTERVAL(b->UpdateInterval) 1308 G_MULTIPLE_MESSAGE_INDICATOR(mmi) 1309 G_SSR_IOD(b->SSRIOD) 1310 G_SSR_PROVIDER_ID(b->SSRProviderID) 1311 G_SSR_SOLUTION_ID(b->SSRSolutionID) 1312 G_NO_OF_SATELLITES(nums) 1313 for (i = 0; i < nums; ++i) { 1314 switch (s) { 1315 case CLOCKORBIT_SATGPS: 1316 case CLOCKORBIT_SATGALILEO: 1317 case CLOCKORBIT_SATSBAS: 1318 case CLOCKORBIT_SATBDS: 1319 G_GPS_SATELLITE_ID(id) 1320 break; 1321 case CLOCKORBIT_SATQZSS: 1322 G_QZSS_SATELLITE_ID(id) 1323 break; 1324 case CLOCKORBIT_SATGLONASS: 1325 G_GLONASS_SATELLITE_ID(id) 1326 break; 1327 } 1328 for (pos = satoffset[s]; 1329 pos < satoffset[s] + b->NumberOfSat[s] && b->Sat[pos].ID != id; 1330 ++pos) 1331 ; 1332 if (pos >= satoffset[s + 1]) 1333 return GCOBR_DATAMISMATCH; 1334 else if (pos == b->NumberOfSat[s] + satoffset[s]) 1335 ++b->NumberOfSat[s]; 1336 b->Sat[pos].ID = id; 1337 1338 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases) 1339 for (j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j) { 1340 G_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type) 1341 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias) 1342 } 1343 } 1344 break; 1345 default: 1346 continue; 1287 1347 } 1288 1348 #ifdef COR_LATENCY 1289 if(s == CLOCKORBIT_SATGPS && type-corbase[s] != COBOFS_BIAS) 1290 { 1349 if(s == CLOCKORBIT_SATGPS && type-corbase[s] != COBOFS_BIAS) { 1291 1350 co->epochGPS[co->epochSize] = co->EpochTime[s]; 1292 1351 if(co->epochSize < COR_LATENCYCOUNT) 1293 1352 ++co->epochSize; 1294 1353 } 1295 1354 #endif 1296 1355 #ifdef DEBUG 1297 1356 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type) 1298 1357 numbits += 8; 1299 1358 fprintf(stderr, "numbits left %d\n",numbits); 1300 1359 #endif
Note:
See TracChangeset
for help on using the changeset viewer.
