Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : //
17 : // SiRF Binary GPS driver for ArduPilot and ArduPilotMega.
18 : // Code by Michael Smith.
19 : //
20 :
21 : #include "AP_GPS_config.h"
22 :
23 : #if AP_GPS_SIRF_ENABLED
24 :
25 : #include "AP_GPS_SIRF.h"
26 : #include <AP_HAL/utility/sparse-endian.h>
27 : #include <stdint.h>
28 :
29 : // Initialisation messages
30 : //
31 : // Turn off all messages except for 0x29.
32 : //
33 : // XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them.
34 : //
35 : const uint8_t AP_GPS_SIRF::_initialisation_blob[] = {
36 : 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3,
37 : 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
38 : };
39 :
40 0 : AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
41 0 : AP_GPS_Backend(_gps, _params, _state, _port)
42 : {
43 0 : gps.send_blob_start(state.instance, (const char *)_initialisation_blob, sizeof(_initialisation_blob));
44 0 : }
45 :
46 :
47 : // Process bytes available from the stream
48 : //
49 : // The stream is assumed to contain only messages we recognise. If it
50 : // contains other messages, and those messages contain the preamble
51 : // bytes, it is possible for this code to fail to synchronise to the
52 : // stream immediately. Without buffering the entire message and
53 : // re-processing it from the top, this is unavoidable. The parser
54 : // attempts to avoid this when possible.
55 : //
56 : bool
57 0 : AP_GPS_SIRF::read(void)
58 : {
59 : uint8_t data;
60 : int16_t numc;
61 0 : bool parsed = false;
62 :
63 0 : numc = port->available();
64 0 : while(numc--) {
65 :
66 : // read the next byte
67 0 : data = port->read();
68 :
69 0 : switch(_step) {
70 :
71 : // Message preamble detection
72 : //
73 : // If we fail to match any of the expected bytes, we reset
74 : // the state machine and re-consider the failed byte as
75 : // the first byte of the preamble. This improves our
76 : // chances of recovering from a mismatch and makes it less
77 : // likely that we will be fooled by the preamble appearing
78 : // as data in some other message.
79 : //
80 0 : case 1:
81 0 : if (PREAMBLE2 == data) {
82 0 : _step++;
83 0 : break;
84 : }
85 0 : _step = 0;
86 : FALLTHROUGH;
87 0 : case 0:
88 0 : if(PREAMBLE1 == data)
89 0 : _step++;
90 0 : break;
91 :
92 : // Message length
93 : //
94 : // We always collect the length so that we can avoid being
95 : // fooled by preamble bytes in messages.
96 : //
97 0 : case 2:
98 0 : _step++;
99 0 : _payload_length = (uint16_t)data << 8;
100 0 : break;
101 0 : case 3:
102 0 : _step++;
103 0 : _payload_length |= data;
104 0 : _payload_counter = 0;
105 0 : _checksum = 0;
106 0 : break;
107 :
108 : // Message header processing
109 : //
110 : // We sniff the message ID to determine whether we are going
111 : // to gather the message bytes or just discard them.
112 : //
113 0 : case 4:
114 0 : _step++;
115 0 : _accumulate(data);
116 0 : _payload_length--;
117 0 : _gather = false;
118 0 : switch(data) {
119 0 : case MSG_GEONAV:
120 0 : if (_payload_length == sizeof(sirf_geonav)) {
121 0 : _gather = true;
122 0 : _msg_id = data;
123 : }
124 0 : break;
125 : }
126 0 : break;
127 :
128 : // Receive message data
129 : //
130 : // Note that we are effectively guaranteed by the protocol
131 : // that the checksum and postamble cannot be mistaken for
132 : // the preamble, so if we are discarding bytes in this
133 : // message when the payload is done we return directly
134 : // to the preamble detector rather than bothering with
135 : // the checksum logic.
136 : //
137 0 : case 5:
138 0 : if (_gather) { // gather data if requested
139 0 : _accumulate(data);
140 0 : _buffer[_payload_counter] = data;
141 0 : if (++_payload_counter == _payload_length)
142 0 : _step++;
143 : } else {
144 0 : if (++_payload_counter == _payload_length)
145 0 : _step = 0;
146 : }
147 0 : break;
148 :
149 : // Checksum and message processing
150 : //
151 0 : case 6:
152 0 : _step++;
153 0 : if ((_checksum >> 8) != data) {
154 0 : _step = 0;
155 : }
156 0 : break;
157 0 : case 7:
158 0 : _step = 0;
159 0 : if ((_checksum & 0xff) != data) {
160 0 : break;
161 : }
162 0 : if (_gather) {
163 0 : parsed = _parse_gps(); // Parse the new GPS packet
164 : }
165 : }
166 : }
167 0 : return(parsed);
168 : }
169 :
170 : bool
171 0 : AP_GPS_SIRF::_parse_gps(void)
172 : {
173 0 : switch(_msg_id) {
174 0 : case MSG_GEONAV:
175 : //time = _swapl(&_buffer.nav.time);
176 : // parse fix type
177 0 : if (_buffer.nav.fix_invalid) {
178 0 : state.status = AP_GPS_FixType::NONE;
179 0 : }else if ((_buffer.nav.fix_type & FIX_MASK) == FIX_3D) {
180 0 : state.status = AP_GPS_FixType::FIX_3D;
181 : }else{
182 0 : state.status = AP_GPS_FixType::FIX_2D;
183 : }
184 0 : state.location.lat = int32_t(be32toh(_buffer.nav.latitude));
185 0 : state.location.lng = int32_t(be32toh(_buffer.nav.longitude));
186 0 : const int32_t alt_amsl = int32_t(be32toh(_buffer.nav.altitude_msl));
187 0 : const int32_t alt_ellipsoid = int32_t(be32toh(_buffer.nav.altitude_ellipsoid));
188 0 : state.undulation = (alt_amsl - alt_ellipsoid)*0.01;
189 0 : state.have_undulation = true;
190 0 : set_alt_amsl_cm(state, alt_amsl);
191 0 : state.ground_speed = int32_t(be32toh(_buffer.nav.ground_speed))*0.01f;
192 0 : state.ground_course = wrap_360(int16_t(be16toh(_buffer.nav.ground_course)*0.01f));
193 0 : state.num_sats = _buffer.nav.satellites;
194 0 : fill_3d_velocity();
195 0 : return true;
196 : }
197 0 : return false;
198 : }
199 :
200 : void
201 0 : AP_GPS_SIRF::_accumulate(uint8_t val)
202 : {
203 0 : _checksum = (_checksum + val) & 0x7fff;
204 0 : }
205 :
206 :
207 :
208 : /*
209 : detect a SIRF GPS
210 : */
211 23038516 : bool AP_GPS_SIRF::_detect(struct SIRF_detect_state &state, uint8_t data)
212 : {
213 23038516 : switch (state.step) {
214 18058 : case 1:
215 18058 : if (PREAMBLE2 == data) {
216 33 : state.step++;
217 33 : break;
218 : }
219 18025 : state.step = 0;
220 : FALLTHROUGH;
221 23038450 : case 0:
222 23038450 : state.payload_length = state.payload_counter = state.checksum = 0;
223 23038450 : if (PREAMBLE1 == data)
224 18065 : state.step++;
225 23038450 : break;
226 33 : case 2:
227 33 : state.step++;
228 33 : if (data != 0) {
229 : // only look for short messages
230 33 : state.step = 0;
231 : }
232 33 : break;
233 0 : case 3:
234 0 : state.step++;
235 0 : state.payload_length = data;
236 0 : break;
237 0 : case 4:
238 0 : state.checksum = (state.checksum + data) & 0x7fff;
239 0 : if (++state.payload_counter == state.payload_length) {
240 0 : state.step++;
241 : }
242 0 : break;
243 0 : case 5:
244 0 : state.step++;
245 0 : if ((state.checksum >> 8) != data) {
246 0 : state.step = 0;
247 : }
248 0 : break;
249 0 : case 6:
250 0 : state.step = 0;
251 0 : if ((state.checksum & 0xff) == data) {
252 0 : return true;
253 : }
254 : }
255 23038516 : return false;
256 : }
257 :
258 : #endif // AP_GPS_SIRF_ENABLED
|
