562 lines
16 KiB
C
562 lines
16 KiB
C
#ifndef NAVIGATION_H
|
|
#define NAVIGATION_H
|
|
|
|
#define SC2RAD 3.1415926535898 /* semi-circle to radian (IS-GPS) */
|
|
|
|
#define INT_SEC_IN_DAY 86400
|
|
#define P2_5 0.03125 /* 2^-5 */
|
|
#define P2_6 0.015625 /* 2^-6 */
|
|
#define P2_11 4.882812500000000E-04 /* 2^-11 */
|
|
#define P2_15 3.051757812500000E-05 /* 2^-15 */
|
|
#define P2_17 7.629394531250000E-06 /* 2^-17 */
|
|
#define P2_19 1.907348632812500E-06 /* 2^-19 */
|
|
#define P2_20 9.536743164062500E-07 /* 2^-20 */
|
|
#define P2_21 4.768371582031250E-07 /* 2^-21 */
|
|
#define P2_23 1.192092895507810E-07 /* 2^-23 */
|
|
#define P2_24 5.960464477539063E-08 /* 2^-24 */
|
|
#define P2_27 7.450580596923828E-09 /* 2^-27 */
|
|
#define P2_29 1.862645149230957E-09 /* 2^-29 */
|
|
#define P2_30 9.313225746154785E-10 /* 2^-30 */
|
|
#define P2_31 0.0000000004656612873077392578125 /* 2^-31 */
|
|
#define P2_32 2.328306436538696E-10 /* 2^-32 */
|
|
#define P2_33 1.164153218269348E-10 /* 2^-33 */
|
|
#define P2_35 2.910383045673370E-11 /* 2^-35 */
|
|
#define P2_38 3.637978807091710E-12 /* 2^-38 */
|
|
#define P2_39 1.818989403545856E-12 /* 2^-39 */
|
|
#define P2_40 9.094947017729280E-13 /* 2^-40 */
|
|
#define P2_43 1.136868377216160297393798828125e-13 /* 2^-43 */
|
|
#define P2_48 3.552713678800501E-15 /* 2^-48 */
|
|
#define P2_50 8.881784197001252E-16 /* 2^-50 */
|
|
#define P2_55 2.775557561562891E-17 /* 2^-55 */
|
|
|
|
#define DAY_IN_WEEK 7
|
|
#define DAY_IN_YEAR 365
|
|
#define DAY_IN_MONTH 30
|
|
#define MAX_22_BIT_INT 0x200000
|
|
#define MAX_16_INT 0x8000
|
|
#define WN_INDEX 2
|
|
#define POSITION_VECTOR_COL 3
|
|
#define MAX_24_BIT_INT 0x00800000
|
|
// ADC sample frekansi
|
|
#define SAMPLES_PER_CODE 16368.0
|
|
|
|
#define CODE_FREQ_BASIS 1.023E+6
|
|
|
|
// typedef unsigned short u16t_t ;
|
|
|
|
typedef enum {
|
|
e_invalid_code,
|
|
e_CA_code,
|
|
e_P_code,
|
|
e_invalid_3
|
|
}te_codeTypes;
|
|
|
|
typedef struct
|
|
{
|
|
unsigned int previousbit30 : 1;
|
|
unsigned int previousbit29 : 1;
|
|
|
|
unsigned int bit30 : 1;
|
|
unsigned int bit29 : 1;
|
|
unsigned int bit28 : 1;
|
|
unsigned int bit27 : 1;
|
|
unsigned int bit26 : 1;
|
|
unsigned int bit25 : 1;
|
|
unsigned int bit24 : 1;
|
|
|
|
unsigned int bit23 : 1;
|
|
unsigned int bit22 : 1;
|
|
unsigned int bit21 : 1;
|
|
unsigned int bit20 : 1;
|
|
unsigned int bit19 : 1;
|
|
|
|
unsigned int bit18 : 1;
|
|
unsigned int bit17 : 1;
|
|
unsigned int bit16 : 1;
|
|
unsigned int bit15 : 1;
|
|
unsigned int bit14 : 1;
|
|
unsigned int bit13 : 1;
|
|
unsigned int bit12 : 1;
|
|
|
|
unsigned int bit11 : 1;
|
|
unsigned int bit10 : 1;
|
|
unsigned int bit9 : 1;
|
|
unsigned int bit8 : 1;
|
|
unsigned int bit7 : 1;
|
|
unsigned int bit6 : 1;
|
|
unsigned int bit5 : 1;
|
|
unsigned int bit4 : 1;
|
|
|
|
unsigned int bit3 : 1;
|
|
unsigned int bit2 : 1;
|
|
unsigned int bit1 : 1;
|
|
|
|
}ts_word;
|
|
|
|
|
|
/**************************************64 bits TLM & HOW Word handling***************/
|
|
|
|
typedef struct
|
|
{
|
|
unsigned long how_32L;
|
|
unsigned long tlm_32H;
|
|
}ts_TLMHOW;
|
|
|
|
typedef struct
|
|
{
|
|
unsigned int parity_1 : 6;
|
|
unsigned int intgrtySts : 1;
|
|
unsigned int rsdvdBit : 1;
|
|
unsigned int tlmMsg : 14;
|
|
unsigned int preamble : 8;
|
|
unsigned int reserved_1 : 2;
|
|
|
|
unsigned int parity_2 : 6;
|
|
unsigned int parityCheck : 2; // non information bearing bits
|
|
unsigned int subframeID : 3;
|
|
unsigned int antispoof : 1;
|
|
unsigned int alertFlag : 1;
|
|
unsigned int countMsg : 17;
|
|
unsigned int reserved_2 : 2;
|
|
|
|
}ts_tlmhowBits;
|
|
|
|
typedef union
|
|
{
|
|
unsigned long long llval;
|
|
ts_TLMHOW TLMHOW;
|
|
ts_tlmhowBits bits;
|
|
}tu_tlmHowWords;
|
|
|
|
/**************************************WN Word handling*****************************/
|
|
// Subframe 1 word 3
|
|
typedef struct
|
|
{
|
|
unsigned int parity : 6;
|
|
unsigned int IODC : 2;
|
|
unsigned int SV_Health : 6;
|
|
unsigned int URAindex : 4;
|
|
unsigned int CAorP : 2;
|
|
unsigned int WN : 10;
|
|
unsigned int reserved_1 : 2;
|
|
|
|
}ts_WN;
|
|
typedef union {
|
|
unsigned long WNword;
|
|
ts_WN WN;
|
|
}tu_WN;
|
|
/**************************************GPS Word4 handling*****************************/
|
|
// Subframe 1 word 4
|
|
typedef struct {
|
|
unsigned int parity : 6;
|
|
unsigned int reserved : 23;
|
|
unsigned int PDataFlag : 1;
|
|
unsigned int reserved_1 : 2;
|
|
}tsWord_4;
|
|
|
|
typedef union {
|
|
unsigned int Word_4;
|
|
tsWord_4 w4bits;
|
|
}tu_Word4;
|
|
/**************************************GPS Word5 handling*****************************/
|
|
// Subframe 1 word 5
|
|
typedef struct {
|
|
unsigned int parity : 6;
|
|
unsigned int reserved : 24;
|
|
unsigned int reserved_1 : 2;
|
|
}ts_Word5;
|
|
typedef union {
|
|
unsigned int Word_5;
|
|
ts_Word5 w5bits;
|
|
}tu_Word5;
|
|
/**************************************GPS Word6 handling*****************************/
|
|
// Subframe 1 word 6
|
|
typedef struct {
|
|
unsigned int parity : 6;
|
|
unsigned int reserved : 24;
|
|
unsigned int reserved_1 : 2;
|
|
}ts_Word6;
|
|
|
|
typedef union {
|
|
unsigned int Word_6;
|
|
ts_Word6 w6bits;
|
|
}tu_Word6;
|
|
/**************************************GPS Word7 handling*****************************/
|
|
// Subframe 1 word 7
|
|
|
|
typedef struct {
|
|
unsigned int parity : 6;
|
|
unsigned int Tgd : 8;
|
|
unsigned int reserved : 16;
|
|
unsigned int reserved_1 : 2;
|
|
}ts_Word7;
|
|
typedef union {
|
|
unsigned int Word_7;
|
|
ts_Word7 w7bits;
|
|
}tu_Word7;
|
|
/**************************************GPS Word8 handling*****************************/
|
|
// Subframe 1 word 8
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int Toc : 16; /* TODO check bit numbers*/
|
|
unsigned int IODC : 8; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_Word8;
|
|
typedef union {
|
|
unsigned int Word_8;
|
|
ts_Word8 w8bits;
|
|
}tu_Word8;
|
|
/**************************************GPS Word9 handling*****************************/
|
|
// Subframe 1 word 9
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int af1 : 16; /* TODO check bit numbers*/
|
|
unsigned int af2 : 8; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_Word9;
|
|
|
|
typedef union {
|
|
unsigned int Word_9;
|
|
ts_Word9 w9bits;
|
|
// unsigned int reserved_1 : 2;
|
|
}tu_Word9;
|
|
/**************************************GPS Word10 handling*****************************/
|
|
// Subframe 1 word 10
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int t : 2; /* non information bearing bits*/
|
|
unsigned int af0 : 22; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_Word10;
|
|
typedef union {
|
|
unsigned int Word_10;
|
|
ts_Word10 w10bits;
|
|
}tu_Word10;
|
|
|
|
/**************************************************************************************/
|
|
|
|
typedef struct subframes {
|
|
|
|
tu_tlmHowWords s_tlmhow;
|
|
tu_WN s_weekNumber;
|
|
tu_Word4 s_word4;
|
|
tu_Word5 s_word5;
|
|
tu_Word6 s_word6;
|
|
|
|
tu_Word7 s_word7;
|
|
tu_Word8 s_word8;
|
|
tu_Word9 s_word9;
|
|
tu_Word10 s_word10;
|
|
|
|
}ts_subframes;
|
|
|
|
// Subframe 2 word definitions:
|
|
|
|
/**************************************GPS Word10 handling*****************************/
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int c_rs : 16; /* non information bearing bits*/
|
|
unsigned int iode : 8; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf2Word3;
|
|
typedef union {
|
|
unsigned int Word_23;
|
|
ts_sf2Word3 w23bits;
|
|
}tu_Word23;
|
|
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int m0_msb : 8; /* non information bearing bits*/
|
|
unsigned int delta_n : 16; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf2Word4;
|
|
typedef union {
|
|
unsigned int Word_24;
|
|
ts_sf2Word4 w24bits;
|
|
}tu_Word24;
|
|
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int m0_lsb : 24; /* non information bearing bits*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf2Word5;
|
|
|
|
typedef union {
|
|
unsigned int Word_25;
|
|
ts_sf2Word5 w25bits;
|
|
}tu_Word25;
|
|
/* ************ word 6 ************* */
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int e_msb : 8; /* non information bearing bits*/
|
|
unsigned int c_uc : 16;
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf2Word6;
|
|
|
|
typedef union {
|
|
unsigned int Word_26;
|
|
ts_sf2Word6 w26bits;
|
|
}tu_Word26;
|
|
|
|
/* ************ word 7 ************* */
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int e_lsb : 24; /* non information bearing bits*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf2Word7;
|
|
|
|
typedef union {
|
|
unsigned int Word_27;
|
|
ts_sf2Word7 w27bits;
|
|
}tu_Word27;
|
|
|
|
|
|
/* ************ word 8 ************* */
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int sqrtA_msb : 8;
|
|
unsigned int c_us : 16;
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf2Word8;
|
|
|
|
typedef union {
|
|
unsigned int Word_28;
|
|
ts_sf2Word8 w28bits;
|
|
}tu_Word28;
|
|
|
|
/* ************ word 9 ************* */
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int sqrtA_lsb : 24;
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf2Word9;
|
|
|
|
typedef union {
|
|
unsigned int Word_29;
|
|
ts_sf2Word9 w29bits;
|
|
}tu_Word29;
|
|
|
|
/* ************ word 10 ************* */
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int nonInfoBearing : 2;
|
|
unsigned int aODO : 5;
|
|
unsigned int fitInterval : 1;
|
|
unsigned int toe : 16;
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf2Word10;
|
|
|
|
typedef union {
|
|
unsigned int Word_210;
|
|
ts_sf2Word10 w210bits;
|
|
}tu_Word210;
|
|
|
|
// Subframe 3 word definitions:
|
|
|
|
/**************************************GPS SF3 Word3 handling*****************************/
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int omega_0_h: 8; /* non information bearing bits*/
|
|
unsigned int cic : 16; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf3Word3;
|
|
typedef union {
|
|
unsigned int Word_33;
|
|
ts_sf3Word3 w33bits;
|
|
}tu_Word33;
|
|
|
|
/**************************************GPS Word4 handling*****************************/
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int omega_0_l : 24; /* non information bearing bits*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf3Word4;
|
|
typedef union {
|
|
unsigned int Word_34;
|
|
ts_sf3Word4 w34bits;
|
|
}tu_Word34;
|
|
|
|
/**************************************GPS Word5 handling*****************************/
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int i0_msb : 8; /* non information bearing bits*/
|
|
unsigned int c_is : 16; /* non information bearing bits*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf3Word5;
|
|
typedef union {
|
|
unsigned int Word_35;
|
|
ts_sf3Word5 w35bits;
|
|
}tu_Word35;
|
|
|
|
/**************************************GPS Word6 handling*****************************/
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int i0_lsb : 24; /* non information bearing bits*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf3Word6;
|
|
typedef union {
|
|
unsigned int Word_36;
|
|
ts_sf3Word6 w36bits;
|
|
}tu_Word36;
|
|
|
|
/**************************************GPS Word7 handling*****************************/
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int w_h : 8; /* non information bearing bits*/
|
|
unsigned int c_rc : 16; /* non information bearing bits*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf3Word7;
|
|
|
|
typedef union {
|
|
unsigned int Word_37;
|
|
ts_sf3Word7 w37bits;
|
|
}tu_Word37;
|
|
|
|
/**************************************GPS Word8 handling*****************************/
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int w_l : 24; /* non information bearing bits*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf3Word8;
|
|
|
|
typedef union {
|
|
unsigned int Word_38;
|
|
ts_sf3Word8 w38bits;
|
|
}tu_Word38;
|
|
|
|
/**************************************GPS Word9 handling*****************************/
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int omega_dot : 24; /* non information bearing bits*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf3Word9;
|
|
|
|
typedef union {
|
|
unsigned int Word_39;
|
|
ts_sf3Word9 w39bits;
|
|
}tu_Word39;
|
|
|
|
/**************************************GPS Word10 handling*****************************/
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int t : 2; /* non information bearing bits*/
|
|
unsigned int idot : 14;
|
|
unsigned int iode : 8; /* iode */
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf3Word10;
|
|
|
|
typedef union {
|
|
unsigned int Word_310;
|
|
ts_sf3Word10 w310bits;
|
|
}tu_Word310;
|
|
|
|
// Subframe 4 word definitions:
|
|
|
|
/**************************************GPS SF4 Word4 handling*****************************/
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int alpha_1 : 8;
|
|
unsigned int alpha_0 : 8;
|
|
unsigned int svPageID : 6; /* non information bearing bits*/
|
|
unsigned int dataID : 2; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf4Word3P18;
|
|
typedef union {
|
|
unsigned int Word_43_P18;
|
|
ts_sf4Word3P18 w43_P18bits;
|
|
}tu_Word43_Page18;
|
|
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int beta_0 : 8;
|
|
unsigned int alpha_3 : 8; /* non information bearing bits*/
|
|
unsigned int alpha_2 : 8; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf4Word4P18;
|
|
typedef union {
|
|
unsigned int Word_44_P18;
|
|
ts_sf4Word4P18 w44_P18bits;
|
|
}tu_Word44_Page18;
|
|
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int beta_3 : 8;
|
|
unsigned int beta_2 : 8; /* non information bearing bits*/
|
|
unsigned int beta_1 : 8; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf4Word5P18;
|
|
typedef union {
|
|
unsigned int Word_45_P18;
|
|
ts_sf4Word5P18 w45_P18bits;
|
|
}tu_Word45_Page18;
|
|
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int A_1 : 24; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf4Word6P18;
|
|
typedef union {
|
|
unsigned int Word_46_P18;
|
|
ts_sf4Word6P18 w46_P18bits;
|
|
}tu_Word46_Page18;
|
|
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int A_0_MSB : 24; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf4Word7P18;
|
|
typedef union {
|
|
unsigned int Word_47_P18;
|
|
ts_sf4Word7P18 w47_P18bits;
|
|
}tu_Word47_Page18;
|
|
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int WNt : 8;
|
|
unsigned int tot: 8;
|
|
unsigned int A_0_LSB : 8; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf4Word8P18;
|
|
typedef union {
|
|
unsigned int Word_48_P18;
|
|
ts_sf4Word8P18 w48_P18bits;
|
|
}tu_Word48_Page18;
|
|
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int DN : 8;
|
|
unsigned int WN_lsf : 8;
|
|
unsigned int delta_tls : 8; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf4Word9P18;
|
|
typedef union {
|
|
unsigned int Word_49_P18;
|
|
ts_sf4Word9P18 w49_P18bits;
|
|
}tu_Word49_Page18;
|
|
|
|
typedef struct {
|
|
unsigned int parity : 6; /* TODO check bit numbers if 4? */
|
|
unsigned int bearingBits : 2;
|
|
unsigned int undef : 14;
|
|
unsigned int delta_t_lsf : 8; /* TODO check bit numbers*/
|
|
unsigned int reserved_1 : 2;
|
|
}ts_sf4Word10P18;
|
|
typedef union {
|
|
unsigned int Word_410_P18;
|
|
ts_sf4Word10P18 w410_P18bits;
|
|
}tu_Word410_Page18;
|
|
|
|
|
|
//union PACK {
|
|
// PACK(int a, int b = 0, int c = 0, int d = 0) { byte[3] = a, byte[2] = b, byte[1] = c, byte[0] = d; }
|
|
// char byte[4];
|
|
// unsigned u32;
|
|
// signed s32;
|
|
// unsigned u(int n) { return u32 >> (32 - n); }
|
|
// signed s(int n) { return s32 >> (32 - n); }
|
|
//};
|
|
|
|
|
|
#endif
|
|
|