1 /* Copyright 2002-2026 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.gnss.metric.ntrip;
18
19 import java.io.IOException;
20 import java.io.InputStream;
21 import java.net.HttpURLConnection;
22 import java.net.SocketTimeoutException;
23 import java.net.URISyntaxException;
24 import java.util.ArrayList;
25 import java.util.Arrays;
26 import java.util.HashMap;
27 import java.util.List;
28 import java.util.Map;
29 import java.util.concurrent.atomic.AtomicBoolean;
30 import java.util.concurrent.atomic.AtomicReference;
31
32 import org.hipparchus.util.FastMath;
33 import org.orekit.errors.OrekitException;
34 import org.orekit.errors.OrekitInternalError;
35 import org.orekit.errors.OrekitMessages;
36 import org.orekit.frames.Frame;
37 import org.orekit.gnss.metric.messages.ParsedMessage;
38 import org.orekit.gnss.metric.parser.AbstractEncodedMessage;
39 import org.orekit.gnss.metric.parser.MessagesParser;
40
41 /** Monitor for retrieving streamed data from one mount point.
42 * @author Luc Maisonobe
43 * @since 11.0
44 */
45 public class StreamMonitor extends AbstractEncodedMessage implements Runnable {
46
47 /** GGA header key. */
48 private static final String GGA_HEADER_KEY = "Ntrip-GGA";
49
50 /** Content type for GNSS data. */
51 private static final String GNSS_DATA_CONTENT_TYPE = "gnss/data";
52
53 /** Size of buffer for retrieving data. */
54 private static final int BUFFER_SIZE = 0x4000;
55
56 /** Frame preamble. */
57 private static final int PREAMBLE = 0xD3;
58
59 /** Frame preamble size. */
60 private static final int PREAMBLE_SIZE = 3;
61
62 /** Frame CRC size. */
63 private static final int CRC_SIZE = 3;
64
65 /** Generator polynomial for CRC. */
66 private static final int GENERATOR = 0x1864CFB;
67
68 /** High bit of the generator polynomial. */
69 private static final int HIGH = 0x1000000;
70
71 /** CRC 24Q lookup table. */
72 private static final int[] CRC_LOOKUP = new int[256];
73
74 static {
75
76 // set up lookup table
77 CRC_LOOKUP[0] = 0;
78 CRC_LOOKUP[1] = GENERATOR;
79
80 int h = GENERATOR;
81 for (int i = 2; i < 256; i <<= 1) {
82 h <<= 1;
83 if ((h & HIGH) != 0) {
84 h ^= GENERATOR;
85 }
86 for (int j = 0; j < i; ++j) {
87 CRC_LOOKUP[i + j] = CRC_LOOKUP[j] ^ h;
88 }
89 }
90
91 }
92
93 /** Associated NTRIP client. */
94 private final NtripClient client;
95
96 /** Mount point providing the stream. */
97 private final String mountPoint;
98
99 /** Messages type of the mount point. */
100 private final Type type;
101
102 /** Indicator for required NMEA. */
103 private final boolean nmeaRequired;
104
105 /** Indicator for ignoring unknown messages. */
106 private final boolean ignoreUnknownMessageTypes;
107
108 /** Delay before we reconnect after connection close. */
109 private final double reconnectDelay;
110
111 /** Multiplication factor for reconnection delay. */
112 private final double reconnectDelayFactor;
113
114 /** Max number of reconnections. */
115 private final int maxRetries;
116
117 /** Reference inertial frame.
118 * @since 14.0
119 */
120 private final Frame inertial;
121
122 /** Body fixed frame.
123 * @since 14.0
124 */
125 private final Frame bodyFixed;
126
127 /** Stop flag. */
128 private final AtomicBoolean stop;
129
130 /** Circular buffer. */
131 private byte[] buffer;
132
133 /** Read index. */
134 private int readIndex;
135
136 /** Message end index. */
137 private int messageEndIndex;
138
139 /** Write index. */
140 private int writeIndex;
141
142 /** Observers for encoded messages. */
143 private final Map<Integer, List<MessageObserver>> observers;
144
145 /** Last available message for each type. */
146 private final Map<Integer, ParsedMessage> lastMessages;
147
148 /** Exception caught during monitoring. */
149 private final AtomicReference<OrekitException> exception;
150
151 /** Build a monitor for streaming data from a mount point.
152 * @param client associated NTRIP client
153 * @param mountPoint mount point providing the stream
154 * @param type messages type of the mount point
155 * @param requiresNMEA if true, the mount point requires a NMEA GGA sentence in the request
156 * @param ignoreUnknownMessageTypes if true, unknown messages types are silently ignored
157 * @param reconnectDelay delay before we reconnect after connection close
158 * @param reconnectDelayFactor factor by which reconnection delay is multiplied after each attempt
159 * @param maxRetries max number of reconnect attempts without reading any data
160 * @param inertial reference inertial frame
161 * @param bodyFixed body fixed frame (will be frozen at {@code date} to build the orbital elements
162 * @since 14.0
163 */
164 public StreamMonitor(final NtripClient client,
165 final String mountPoint, final Type type,
166 final boolean requiresNMEA, final boolean ignoreUnknownMessageTypes,
167 final double reconnectDelay, final double reconnectDelayFactor,
168 final int maxRetries, final Frame inertial, final Frame bodyFixed) {
169 this.client = client;
170 this.mountPoint = mountPoint;
171 this.type = type;
172 this.nmeaRequired = requiresNMEA;
173 this.ignoreUnknownMessageTypes = ignoreUnknownMessageTypes;
174 this.reconnectDelay = reconnectDelay;
175 this.reconnectDelayFactor = reconnectDelayFactor;
176 this.maxRetries = maxRetries;
177 this.inertial = inertial;
178 this.bodyFixed = bodyFixed;
179 this.stop = new AtomicBoolean(false);
180 this.observers = new HashMap<>();
181 this.lastMessages = new HashMap<>();
182 this.exception = new AtomicReference<>(null);
183 }
184
185 /** Add an observer for encoded messages.
186 * <p>
187 * If messages of the specified type have already been retrieved from
188 * a stream, the observer will be immediately notified with the last
189 * message as a side effect of being added.
190 * </p>
191 * @param typeCode code for the message type (if set to 0, notification
192 * will be triggered regardless of message type)
193 * @param observer observer for this message type
194 */
195 public void addObserver(final int typeCode, final MessageObserver observer) {
196 synchronized (observers) {
197
198 // register the observer
199 observers.computeIfAbsent(typeCode, tc -> new ArrayList<>()).add(observer);
200
201 // if we already have a message of the proper type
202 // immediately notify the new observer about it
203 final ParsedMessage last = lastMessages.get(typeCode);
204 if (last != null) {
205 observer.messageAvailable(mountPoint, last);
206 }
207
208 }
209 }
210
211 /** Stop monitoring. */
212 public void stopMonitoring() {
213 stop.set(true);
214 }
215
216 /** Retrieve exception caught during monitoring.
217 * @return exception caught
218 */
219 public OrekitException getException() {
220 return exception.get();
221 }
222
223 /** {@inheritDoc} */
224 @Override
225 public void run() {
226
227 try {
228
229 final MessagesParser parser = type.getParser(extractUsedMessages(), client.getTimeScales(),
230 inertial, bodyFixed);
231 int nbAttempts = 0;
232 double delay = reconnectDelay;
233 while (nbAttempts < maxRetries) {
234
235 try {
236 // prepare request
237 final HttpURLConnection connection = client.connect(mountPoint);
238 if (nmeaRequired) {
239 if (client.getGGA() == null) {
240 throw new OrekitException(OrekitMessages.STREAM_REQUIRES_NMEA_FIX, mountPoint);
241 } else {
242 // update NMEA GGA sentence in the extra headers for this mount point
243 connection.setRequestProperty(GGA_HEADER_KEY, client.getGGA());
244 }
245 }
246
247 // perform request
248 final int responseCode = connection.getResponseCode();
249 if (responseCode == HttpURLConnection.HTTP_UNAUTHORIZED) {
250 throw new OrekitException(OrekitMessages.FAILED_AUTHENTICATION, mountPoint);
251 } else if (responseCode != HttpURLConnection.HTTP_OK) {
252 throw new OrekitException(OrekitMessages.CONNECTION_ERROR,
253 connection.getURL().getHost(),
254 connection.getResponseMessage());
255 }
256
257 // for this request, we MUST get GNSS data
258 if (!GNSS_DATA_CONTENT_TYPE.equals(connection.getContentType())) {
259 throw new OrekitException(OrekitMessages.UNEXPECTED_CONTENT_TYPE, connection.getContentType());
260 }
261
262 // data extraction loop
263 resetCircularBuffer();
264 try (InputStream is = connection.getInputStream()) {
265
266 for (int r = fillUp(is); r >= 0; r = fillUp(is)) {
267
268 // we have read something, reset reconnection attempts counters
269 nbAttempts = 0;
270 delay = reconnectDelay;
271
272 if (stop.get()) {
273 // stop monitoring immediately
274 // (returning closes the input stream automatically)
275 return;
276 }
277
278 while (bufferSize() >= 3) {
279 if (peekByte(0) != PREAMBLE) {
280 // we are out of synch with respect to frame structure
281 // drop the unknown byte
282 moveRead(1);
283 } else {
284 final int size = (peekByte(1) & 0x03) << 8 | peekByte(2);
285 if (bufferSize() >= PREAMBLE_SIZE + size + CRC_SIZE) {
286 // check CRC
287 final int crc = (peekByte(PREAMBLE_SIZE + size) << 16) |
288 (peekByte(PREAMBLE_SIZE + size + 1) << 8) |
289 peekByte(PREAMBLE_SIZE + size + 2);
290 if (crc == computeCRC(PREAMBLE_SIZE + size)) {
291 // we have a complete and consistent frame
292 // we can extract the message it contains
293 messageEndIndex = (readIndex + PREAMBLE_SIZE + size) % BUFFER_SIZE;
294 moveRead(PREAMBLE_SIZE);
295 start();
296 final ParsedMessage message = parser.parse(this, ignoreUnknownMessageTypes);
297 if (message != null) {
298 storeAndNotify(message);
299 }
300 // jump to expected message end, in case the message was corrupted
301 // and parsing did not reach message end
302 readIndex = (messageEndIndex + CRC_SIZE) % BUFFER_SIZE;
303 } else {
304 // CRC is not consistent, we are probably not really synched
305 // and the preamble byte was just a random byte
306 // we drop this single byte and continue looking for sync
307 moveRead(1);
308 }
309 } else {
310 // the frame is not complete, we need more data
311 break;
312 }
313 }
314 }
315
316 }
317
318 }
319 } catch (SocketTimeoutException ste) {
320 // ignore exception, it will be handled by reconnection attempt below
321 } catch (IOException | URISyntaxException e) {
322 throw new OrekitException(e, OrekitMessages.CANNOT_PARSE_GNSS_DATA, client.getHost());
323 }
324
325 // manage reconnection
326 try {
327 Thread.sleep((int) FastMath.rint(delay * 1000));
328 } catch (InterruptedException ie) {
329 // Restore interrupted state...
330 Thread.currentThread().interrupt();
331 }
332 ++nbAttempts;
333 delay *= reconnectDelayFactor;
334
335 }
336
337 } catch (OrekitException oe) {
338 // store the exception so it can be retrieved by Ntrip client
339 exception.set(oe);
340 }
341
342 }
343
344 /** Store a parsed encoded message and notify observers.
345 * @param message parsed message
346 */
347 private void storeAndNotify(final ParsedMessage message) {
348 synchronized (observers) {
349
350 for (int typeCode : Arrays.asList(0, message.getTypeCode())) {
351
352 // store message
353 lastMessages.put(typeCode, message);
354
355 // notify observers
356 final List<MessageObserver> list = observers.get(typeCode);
357 if (list != null) {
358 for (final MessageObserver observer : list) {
359 // notify observer
360 observer.messageAvailable(mountPoint, message);
361 }
362 }
363
364 }
365
366 }
367 }
368
369 /** Reset the circular buffer.
370 */
371 private void resetCircularBuffer() {
372 buffer = new byte[BUFFER_SIZE];
373 readIndex = 0;
374 writeIndex = 0;
375 }
376
377 /** Extract data from input stream.
378 * @param is input stream to extract data from
379 * @return number of byes read or -1
380 * @throws IOException if data cannot be extracted properly
381 */
382 private int fillUp(final InputStream is) throws IOException {
383 final int max = bufferMaxWrite();
384 if (max == 0) {
385 // this should never happen
386 // the buffer is large enough for almost 16 encoded messages, including wrapping frame
387 throw new OrekitInternalError(null);
388 }
389 final int r = is.read(buffer, writeIndex, max);
390 if (r >= 0) {
391 writeIndex = (writeIndex + r) % BUFFER_SIZE;
392 }
393 return r;
394 }
395
396 /** {@inheritDoc} */
397 @Override
398 protected int fetchByte() {
399 if (readIndex == messageEndIndex || readIndex == writeIndex) {
400 return -1;
401 }
402
403 final int ret = buffer[readIndex] & 0xFF;
404 moveRead(1);
405 return ret;
406 }
407
408 /** Get the number of bytes currently in the buffer.
409 * @return number of bytes currently in the buffer
410 */
411 private int bufferSize() {
412 final int n = writeIndex - readIndex;
413 return n >= 0 ? n : BUFFER_SIZE + n;
414 }
415
416 /** Peek a buffer byte without moving read pointer.
417 * @param offset offset counted from read pointer
418 * @return value of the byte at given offset
419 */
420 private int peekByte(final int offset) {
421 return buffer[(readIndex + offset) % BUFFER_SIZE] & 0xFF;
422 }
423
424 /** Move read pointer.
425 * @param n number of bytes to move read pointer
426 */
427 private void moveRead(final int n) {
428 readIndex = (readIndex + n) % BUFFER_SIZE;
429 }
430
431 /** Get the number of bytes that can be added to the buffer without wrapping around.
432 * @return number of bytes that can be added
433 */
434 private int bufferMaxWrite() {
435 if (writeIndex >= readIndex) {
436 return (readIndex == 0 ? BUFFER_SIZE - 1 : BUFFER_SIZE) - writeIndex;
437 } else {
438 return readIndex - writeIndex - 1;
439 }
440 }
441
442 /** Compute QualCom CRC.
443 * @param length length of the byte stream
444 * @return QualCom CRC
445 */
446 private int computeCRC(final int length) {
447 int crc = 0;
448 for (int i = 0; i < length; ++i) {
449 crc = ((crc << 8) ^ CRC_LOOKUP[peekByte(i) ^ (crc >>> 16)]) & (HIGH - 1);
450 }
451 return crc;
452 }
453
454 /** Extract the used messages.
455 * @return the extracted messages
456 */
457 private List<Integer> extractUsedMessages() {
458 synchronized (observers) {
459
460 // List of needed messages
461 final List<Integer> messages = new ArrayList<>();
462
463 // Loop on observers entries
464 for (Map.Entry<Integer, List<MessageObserver>> entry : observers.entrySet()) {
465 // Extract message type code
466 final int typeCode = entry.getKey();
467 // Add to the list
468 messages.add(typeCode);
469 }
470
471 return messages;
472 }
473 }
474
475 }