PX4 Firmware
PX4 Autopilot Software http://px4.io
rtcm.cpp
Go to the documentation of this file.
1
/****************************************************************************
2
*
3
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
4
*
5
* Redistribution and use in source and binary forms, with or without
6
* modification, are permitted provided that the following conditions
7
* are met:
8
*
9
* 1. Redistributions of source code must retain the above copyright
10
* notice, this list of conditions and the following disclaimer.
11
* 2. Redistributions in binary form must reproduce the above copyright
12
* notice, this list of conditions and the following disclaimer in
13
* the documentation and/or other materials provided with the
14
* distribution.
15
* 3. Neither the name PX4 nor the names of its contributors may be
16
* used to endorse or promote products derived from this software
17
* without specific prior written permission.
18
*
19
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30
* POSSIBILITY OF SUCH DAMAGE.
31
*
32
****************************************************************************/
33
34
#include "
rtcm.h
"
35
#include <cstring>
36
37
RTCMParsing::RTCMParsing
()
38
{
39
reset
();
40
}
41
42
RTCMParsing::~RTCMParsing
()
43
{
44
if
(
_buffer
) {
45
delete
[](
_buffer
);
46
}
47
}
48
49
void
RTCMParsing::reset
()
50
{
51
if
(!
_buffer
) {
52
_buffer
=
new
uint8_t[
RTCM_INITIAL_BUFFER_LENGTH
];
53
_buffer_len
=
RTCM_INITIAL_BUFFER_LENGTH
;
54
}
55
56
_pos
= 0;
57
_message_length
=
_buffer_len
;
58
}
59
60
bool
RTCMParsing::addByte
(uint8_t b)
61
{
62
_buffer
[
_pos
++] = b;
63
64
if
(
_pos
== 3) {
65
_message_length
= (((uint16_t)
_buffer
[1] & 3) << 8) | (
_buffer
[2]);
66
67
if
(
_message_length
+ 6 >
_buffer_len
) {
68
uint16_t new_buffer_len =
_message_length
+ 6;
69
uint8_t *new_buffer =
new
uint8_t[new_buffer_len];
70
memcpy(new_buffer,
_buffer
, 3);
71
delete
[](
_buffer
);
72
_buffer
= new_buffer;
73
_buffer_len
= new_buffer_len;
74
}
75
}
76
77
return
_message_length
+ 6 ==
_pos
;
78
}
RTCMParsing::RTCMParsing
RTCMParsing()
Definition:
rtcm.cpp:37
RTCMParsing::reset
void reset()
reset the parsing state
Definition:
rtcm.cpp:49
rtcm.h
RTCMParsing::_message_length
uint16_t _message_length
message length without header & CRC (both 3 bytes)
Definition:
rtcm.h:68
RTCM_INITIAL_BUFFER_LENGTH
#define RTCM_INITIAL_BUFFER_LENGTH
initial maximum message length of an RTCM message
Definition:
rtcm.h:40
RTCMParsing::~RTCMParsing
~RTCMParsing()
Definition:
rtcm.cpp:42
RTCMParsing::_buffer
uint8_t * _buffer
Definition:
rtcm.h:65
RTCMParsing::_buffer_len
uint16_t _buffer_len
Definition:
rtcm.h:66
RTCMParsing::_pos
uint16_t _pos
next position in buffer
Definition:
rtcm.h:67
RTCMParsing::addByte
bool addByte(uint8_t b)
add a byte to the message
Definition:
rtcm.cpp:60
src
drivers
gps
devices
src
rtcm.cpp
Generated by
1.8.13