1 | /* rtigs.cpp
|
---|
2 | * - Source functions
|
---|
3 | *
|
---|
4 | * Copyright (c) 2006
|
---|
5 | * Geoscience Australia
|
---|
6 | *
|
---|
7 | * Written by James Stewart
|
---|
8 | * E-mail: James.Stewart@ga.gov.au
|
---|
9 | *
|
---|
10 | * Enquiries contact
|
---|
11 | * Michael Moore
|
---|
12 | * E-mail: Michael.Moore@ga.gov.au
|
---|
13 | *
|
---|
14 | * Based on the GNU General Public License published Icecast 1.3.12
|
---|
15 | *
|
---|
16 | * This program is free software; you can redistribute it and/or
|
---|
17 | * modify it under the terms of the GNU General Public License
|
---|
18 | * as published by the Free Software Foundation; either version 2
|
---|
19 | * of the License, or (at your option) any later version.
|
---|
20 | *
|
---|
21 | * This program is distributed in the hope that it will be useful,
|
---|
22 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
---|
23 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
---|
24 | * GNU General Public License for more details.
|
---|
25 | *
|
---|
26 | * You should have received a copy of the GNU General Public License
|
---|
27 | * along with this program; if not, write to the Free Software
|
---|
28 | * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
---|
29 | */
|
---|
30 |
|
---|
31 | #include <string.h>
|
---|
32 | #include "rtigs.h"
|
---|
33 |
|
---|
34 | void bytes_to_rtigsh(RTIGSH *rtigsh, unsigned char *packet) {
|
---|
35 | memcpy(rtigsh, packet, sizeof(RTIGSH));
|
---|
36 | revbytes((unsigned char *)&(rtigsh->rec_id), 2);
|
---|
37 | revbytes((unsigned char *)&(rtigsh->sta_id), 2);
|
---|
38 | revbytes((unsigned char *)&(rtigsh->GPSTime), 4);
|
---|
39 | revbytes((unsigned char *)&(rtigsh->num_bytes), 2);
|
---|
40 | }
|
---|
41 |
|
---|
42 | int rtigso_to_raw(RTIGSO *rtigso, GPSEpoch *gpse) {
|
---|
43 | struct JPL_COMP_OBS_T soc;
|
---|
44 | int ptr = 0;
|
---|
45 | unsigned char * p1;
|
---|
46 | unsigned char * p2;
|
---|
47 |
|
---|
48 | double CA_Range = 0.00;
|
---|
49 | double P1_Range, P2_Range, L1_Cycles, L2_Cycles;
|
---|
50 | double L1_Diff = 0.00;
|
---|
51 | double L2_Diff = 0.00;
|
---|
52 |
|
---|
53 | long long ABS_CA_Range = 0;
|
---|
54 |
|
---|
55 | double L1_Phase = 0.00;
|
---|
56 | double L2_Phase = 0.00;
|
---|
57 |
|
---|
58 | int ABS_L1_Diff = 0;
|
---|
59 | int ABS_L2_Diff = 0;
|
---|
60 | int ABS_L1_Phase = 0;
|
---|
61 | int ABS_L2_Phase = 0;
|
---|
62 |
|
---|
63 | gpse->num_sv = rtigso->num_obs;
|
---|
64 | gpse->GPSTime = rtigso->GPSTime;
|
---|
65 | gpse->sta_id = rtigso->sta_id;
|
---|
66 |
|
---|
67 | for(ptr = 0; ptr < rtigso->num_obs; ptr++) {
|
---|
68 | memset(&soc, 0, sizeof(struct JPL_COMP_OBS_T));
|
---|
69 | memcpy(&(soc.prn), &(rtigso->data[ptr][0]), 1);
|
---|
70 | memcpy(&(soc.epoch_sequence), &(rtigso->data[ptr][1]), 2);
|
---|
71 | memcpy(&(soc.ca_range[0]), &(rtigso->data[ptr][3]), 18);
|
---|
72 |
|
---|
73 | p1 = ((unsigned char *)&ABS_CA_Range);
|
---|
74 | p2 = &(soc.ca_range[0]);
|
---|
75 |
|
---|
76 | p1[4] = (p2[0] & 0xF);
|
---|
77 | p1[3] = p2[1];
|
---|
78 | p1[2] = p2[2];
|
---|
79 | p1[1] = p2[3];
|
---|
80 | p1[0] = p2[4];
|
---|
81 |
|
---|
82 | p1 = (unsigned char *)&ABS_L1_Diff;
|
---|
83 | p2 = (unsigned char *)&(soc.L1_range_phase[0]);
|
---|
84 |
|
---|
85 | if(p2[0] & 0x80)
|
---|
86 | L1_Diff = -1.000;
|
---|
87 | else
|
---|
88 | L1_Diff = 1.000;
|
---|
89 |
|
---|
90 | p1[2] = (p2[0] & 0x40) >> 6;
|
---|
91 | p1[1] = ((p2[0] & 0x3F) << 2) + ((p2[1] & 0xC0) >> 6);
|
---|
92 | p1[0] = ((p2[1] & 0x3F) << 2) + ((p2[2] & 0xC0) >> 6);
|
---|
93 |
|
---|
94 | if(p2[2] & 0x20)
|
---|
95 | L1_Phase = -1.000;
|
---|
96 | else
|
---|
97 | L1_Phase = 1.000;
|
---|
98 |
|
---|
99 | p1 = (unsigned char *)&ABS_L1_Phase;
|
---|
100 | p1[2] = (p2[2] & 0x1F);
|
---|
101 | p1[1] = p2[3];
|
---|
102 | p1[0] = p2[4];
|
---|
103 |
|
---|
104 |
|
---|
105 |
|
---|
106 | p1 = (unsigned char *)&ABS_L2_Diff;
|
---|
107 | p2 = (unsigned char *)&(soc.L2_range_phase[0]);
|
---|
108 |
|
---|
109 | if(p2[0] & 0x80)
|
---|
110 | L2_Diff = -1.000;
|
---|
111 | else
|
---|
112 | L2_Diff = 1.000;
|
---|
113 |
|
---|
114 | p1[2] = (p2[0] & 0x40) >> 6;
|
---|
115 | p1[1] = ((p2[0] & 0x3F) << 2) + ((p2[1] & 0xC0) >> 6);
|
---|
116 | p1[0] = ((p2[1] & 0x3F) << 2) + ((p2[2] & 0xC0) >> 6);
|
---|
117 |
|
---|
118 | if(p2[2] & 0x20)
|
---|
119 | L2_Phase = -1.000;
|
---|
120 | else
|
---|
121 | L2_Phase = 1.000;
|
---|
122 |
|
---|
123 | p1 = (unsigned char *)&ABS_L2_Phase;
|
---|
124 | p1[2] = (p2[2] & 0x1F);
|
---|
125 | p1[1] = p2[3];
|
---|
126 | p1[0] = p2[4];
|
---|
127 |
|
---|
128 | p2 = &(soc.ca_range[0]);
|
---|
129 | if(p2[0] & 0x20)
|
---|
130 | ABS_L2_Phase += (1 << 22);
|
---|
131 | if(p2[0] & 0x10)
|
---|
132 | ABS_L1_Phase += (1 << 22);
|
---|
133 |
|
---|
134 | L1_Phase = L1_Phase * (((double)ABS_L1_Phase) / 50000.00);
|
---|
135 | L2_Phase = L2_Phase * (((double)ABS_L2_Phase) / 50000.00);
|
---|
136 |
|
---|
137 | L1_Diff = L1_Diff * (((double)ABS_L1_Diff) / 1000.00);
|
---|
138 | L2_Diff = L2_Diff * (((double)ABS_L2_Diff) / 1000.00);
|
---|
139 |
|
---|
140 | CA_Range = (double)ABS_CA_Range / 1000;
|
---|
141 | P1_Range = CA_Range + L1_Diff;
|
---|
142 | P2_Range = CA_Range + L2_Diff;
|
---|
143 |
|
---|
144 | L1_Cycles = ((CA_Range + L1_Phase) - (SF2 * L2_Diff)) / W1;
|
---|
145 | L2_Cycles = ((CA_Range + L2_Phase) - (SF1 * L2_Diff)) / W2;
|
---|
146 |
|
---|
147 | gpse->sv[ptr].prn = soc.prn;
|
---|
148 | gpse->sv[ptr].locktime = soc.epoch_sequence;
|
---|
149 | gpse->sv[ptr].CARange = CA_Range;
|
---|
150 | gpse->sv[ptr].CASnr = soc.CA_snr;
|
---|
151 |
|
---|
152 | gpse->sv[ptr].P1Range = P1_Range;
|
---|
153 | gpse->sv[ptr].L1Phase = L1_Cycles;
|
---|
154 | gpse->sv[ptr].L1Snr = soc.L1_snr;
|
---|
155 |
|
---|
156 | gpse->sv[ptr].P2Range = P2_Range;
|
---|
157 | gpse->sv[ptr].L2Phase = L2_Cycles;
|
---|
158 | gpse->sv[ptr].L2Snr = soc.L2_snr;
|
---|
159 | }
|
---|
160 | return 0;
|
---|
161 | }
|
---|
162 |
|
---|
163 | int rtigsm_to_raw(RTIGSM *rtigsm, GPSMet *met) {
|
---|
164 | met->GPSTime = rtigsm->GPSTime;
|
---|
165 | met->sta_id = rtigsm->sta_id;
|
---|
166 | met->temperature = rtigsm->temp;
|
---|
167 | met->pressure = rtigsm->press;
|
---|
168 | met->humidity = rtigsm->humid;
|
---|
169 | revbytes((unsigned char *)&met->temperature, 4);
|
---|
170 | revbytes((unsigned char *)&met->pressure, 4);
|
---|
171 | revbytes((unsigned char *)&met->humidity, 4);
|
---|
172 | return 0;
|
---|
173 | }
|
---|
174 |
|
---|
175 | void revbytes(unsigned char *buf, unsigned char size) {
|
---|
176 | unsigned char tmp;
|
---|
177 | unsigned char *ptr1 = buf;
|
---|
178 | unsigned char *ptr2 = buf + (size - 1);
|
---|
179 |
|
---|
180 | while(ptr1 < ptr2) {
|
---|
181 | tmp = *ptr1;
|
---|
182 | *ptr1 = *ptr2;
|
---|
183 | *ptr2 = tmp;
|
---|
184 | ptr1++;
|
---|
185 | ptr2--;
|
---|
186 | }
|
---|
187 | }
|
---|