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 : // Swift Navigation SBP GPS driver for ArduPilot.
18 : // Code by Niels Joubert
19 : //
20 : // Swift Binary Protocol format: http://docs.swift-nav.com/
21 : //
22 :
23 :
24 : #include "AP_GPS.h"
25 : #include "AP_GPS_SBP.h"
26 : #include <AP_Logger/AP_Logger.h>
27 :
28 : #if AP_GPS_SBP_ENABLED
29 :
30 : extern const AP_HAL::HAL& hal;
31 :
32 : #define SBP_DEBUGGING 1
33 :
34 : #define SBP_TIMEOUT_HEATBEAT 4000
35 : #define SBP_TIMEOUT_PVT 500
36 :
37 : #if SBP_DEBUGGING
38 : # define Debug(fmt, args ...) \
39 : do { \
40 : hal.console->printf("%s:%d: " fmt "\n", \
41 : __FUNCTION__, __LINE__, \
42 : ## args); \
43 : hal.scheduler->delay(1); \
44 : } while(0)
45 : #else
46 : # define Debug(fmt, args ...)
47 : #endif
48 :
49 1 : AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps,
50 : AP_GPS::Params &_params,
51 : AP_GPS::GPS_State &_state,
52 1 : AP_HAL::UARTDriver *_port) :
53 1 : AP_GPS_Backend(_gps, _params, _state, _port)
54 : {
55 :
56 1 : Debug("SBP Driver Initialized");
57 :
58 1 : parser_state.state = sbp_parser_state_t::WAITING;
59 :
60 : //Externally visible state
61 1 : state.status = AP_GPS_FixType::NONE;
62 1 : state.last_gps_time_ms = last_heatbeat_received_ms = AP_HAL::millis();
63 :
64 1 : }
65 :
66 : // Process all bytes available from the stream
67 : //
68 : bool
69 642 : AP_GPS_SBP::read(void)
70 : {
71 :
72 : //Invariant: Calling this function processes *all* data current in the UART buffer.
73 : //
74 : //IMPORTANT NOTICE: This function is NOT CALLED for several seconds
75 : // during arming. That should not cause the driver to die. Process *all* waiting messages
76 :
77 642 : _sbp_process();
78 :
79 642 : return _attempt_state_update();
80 :
81 : }
82 :
83 : void
84 0 : AP_GPS_SBP::inject_data(const uint8_t *data, uint16_t len)
85 : {
86 :
87 0 : if (port->txspace() > len) {
88 0 : last_injected_data_ms = AP_HAL::millis();
89 0 : port->write(data, len);
90 : } else {
91 0 : Debug("PIKSI: Not enough TXSPACE");
92 : }
93 :
94 0 : }
95 :
96 : //This attempts to reads all SBP messages from the incoming port.
97 : //Returns true if a new message was read, false if we failed to read a message.
98 : void
99 642 : AP_GPS_SBP::_sbp_process()
100 : {
101 :
102 8608 : while (port->available() > 0) {
103 7966 : uint8_t temp = port->read();
104 : #if AP_GPS_DEBUG_LOGGING_ENABLED
105 : log_data(&temp, 1);
106 : #endif
107 : uint16_t crc;
108 :
109 :
110 : //This switch reads one character at a time,
111 : //parsing it into buffers until a full message is dispatched
112 7966 : switch (parser_state.state) {
113 250 : case sbp_parser_state_t::WAITING:
114 250 : if (temp == SBP_PREAMBLE) {
115 250 : parser_state.n_read = 0;
116 250 : parser_state.state = sbp_parser_state_t::GET_TYPE;
117 : }
118 250 : break;
119 :
120 500 : case sbp_parser_state_t::GET_TYPE:
121 500 : *((uint8_t*)&(parser_state.msg_type) + parser_state.n_read) = temp;
122 500 : parser_state.n_read += 1;
123 500 : if (parser_state.n_read >= 2) {
124 250 : parser_state.n_read = 0;
125 250 : parser_state.state = sbp_parser_state_t::GET_SENDER;
126 : }
127 500 : break;
128 :
129 500 : case sbp_parser_state_t::GET_SENDER:
130 500 : *((uint8_t*)&(parser_state.sender_id) + parser_state.n_read) = temp;
131 500 : parser_state.n_read += 1;
132 500 : if (parser_state.n_read >= 2) {
133 250 : parser_state.n_read = 0;
134 250 : parser_state.state = sbp_parser_state_t::GET_LEN;
135 : }
136 500 : break;
137 :
138 250 : case sbp_parser_state_t::GET_LEN:
139 250 : parser_state.msg_len = temp;
140 250 : parser_state.n_read = 0;
141 250 : parser_state.state = sbp_parser_state_t::GET_MSG;
142 250 : break;
143 :
144 5966 : case sbp_parser_state_t::GET_MSG:
145 5966 : *((uint8_t*)&(parser_state.msg_buff) + parser_state.n_read) = temp;
146 5966 : parser_state.n_read += 1;
147 5966 : if (parser_state.n_read >= parser_state.msg_len) {
148 250 : parser_state.n_read = 0;
149 250 : parser_state.state = sbp_parser_state_t::GET_CRC;
150 : }
151 5966 : break;
152 :
153 500 : case sbp_parser_state_t::GET_CRC:
154 500 : *((uint8_t*)&(parser_state.crc) + parser_state.n_read) = temp;
155 500 : parser_state.n_read += 1;
156 500 : if (parser_state.n_read >= 2) {
157 250 : parser_state.state = sbp_parser_state_t::WAITING;
158 :
159 250 : crc = crc16_ccitt((uint8_t*)&(parser_state.msg_type), 2, 0);
160 250 : crc = crc16_ccitt((uint8_t*)&(parser_state.sender_id), 2, crc);
161 250 : crc = crc16_ccitt(&(parser_state.msg_len), 1, crc);
162 250 : crc = crc16_ccitt(parser_state.msg_buff, parser_state.msg_len, crc);
163 250 : if (parser_state.crc == crc) {
164 250 : _sbp_process_message();
165 : } else {
166 0 : Debug("CRC Error Occurred!");
167 0 : crc_error_counter += 1;
168 : }
169 :
170 250 : parser_state.state = sbp_parser_state_t::WAITING;
171 : }
172 500 : break;
173 :
174 0 : default:
175 0 : parser_state.state = sbp_parser_state_t::WAITING;
176 0 : break;
177 : }
178 : }
179 642 : }
180 :
181 :
182 : //INVARIANT: A fully received message with correct CRC is currently in parser_state
183 : void
184 250 : AP_GPS_SBP::_sbp_process_message() {
185 250 : switch (parser_state.msg_type) {
186 11 : case SBP_HEARTBEAT_MSGTYPE:
187 11 : last_heatbeat_received_ms = AP_HAL::millis();
188 11 : break;
189 :
190 57 : case SBP_GPS_TIME_MSGTYPE:
191 57 : memcpy(&last_gps_time, parser_state.msg_buff, sizeof(last_gps_time));
192 57 : check_new_itow(last_gps_time.tow, parser_state.msg_len);
193 57 : break;
194 :
195 57 : case SBP_VEL_NED_MSGTYPE:
196 57 : memcpy(&last_vel_ned, parser_state.msg_buff, sizeof(last_vel_ned));
197 57 : check_new_itow(last_vel_ned.tow, parser_state.msg_len);
198 57 : break;
199 :
200 114 : case SBP_POS_LLH_MSGTYPE: {
201 114 : struct sbp_pos_llh_t *pos_llh = (struct sbp_pos_llh_t*)parser_state.msg_buff;
202 114 : check_new_itow(pos_llh->tow, parser_state.msg_len);
203 : // Check if this is a single point or RTK solution
204 : // flags = 0 -> single point
205 114 : if (pos_llh->flags == 0) {
206 57 : last_pos_llh_spp = *pos_llh;
207 57 : } else if (pos_llh->flags == 1 || pos_llh->flags == 2) {
208 57 : last_pos_llh_rtk = *pos_llh;
209 : }
210 114 : break;
211 : }
212 :
213 11 : case SBP_DOPS_MSGTYPE:
214 11 : memcpy(&last_dops, parser_state.msg_buff, sizeof(last_dops));
215 11 : check_new_itow(last_dops.tow, parser_state.msg_len);
216 11 : break;
217 :
218 0 : case SBP_TRACKING_STATE_MSGTYPE:
219 : //INTENTIONALLY BLANK
220 : //Currently unhandled, but logged after switch statement.
221 0 : break;
222 :
223 0 : case SBP_IAR_STATE_MSGTYPE: {
224 0 : sbp_iar_state_t *iar = (struct sbp_iar_state_t*)parser_state.msg_buff;
225 0 : last_iar_num_hypotheses = iar->num_hypotheses;
226 0 : break;
227 : }
228 :
229 0 : default:
230 : // log anyway if it's an unsupported message.
231 : // The log mask will be used to adjust or suppress logging
232 0 : break;
233 : }
234 250 : }
235 :
236 : bool
237 642 : AP_GPS_SBP::_attempt_state_update()
238 : {
239 :
240 : // If we currently have heartbeats
241 : // - NO FIX
242 : //
243 : // If we have a full update available, save it
244 : //
245 642 : uint32_t now = AP_HAL::millis();
246 642 : bool ret = false;
247 :
248 642 : if (now - last_heatbeat_received_ms > SBP_TIMEOUT_HEATBEAT) {
249 :
250 0 : state.status = AP_GPS_FixType::NONE;
251 0 : Debug("No Heartbeats from Piksi! Driver Ready to Die!");
252 :
253 642 : } else if (last_pos_llh_rtk.tow == last_vel_ned.tow
254 642 : && abs((int32_t) (last_gps_time.tow - last_vel_ned.tow)) < 10000
255 642 : && abs((int32_t) (last_dops.tow - last_vel_ned.tow)) < 60000
256 602 : && last_vel_ned.tow > last_full_update_tow) {
257 :
258 : // Use the RTK position
259 53 : sbp_pos_llh_t *pos_llh = &last_pos_llh_rtk;
260 :
261 : // Update time state
262 53 : state.time_week = last_gps_time.wn;
263 53 : state.time_week_ms = last_vel_ned.tow;
264 :
265 53 : state.hdop = last_dops.hdop;
266 :
267 : // Update velocity state
268 53 : state.velocity[0] = (float)(last_vel_ned.n * 1.0e-3);
269 53 : state.velocity[1] = (float)(last_vel_ned.e * 1.0e-3);
270 53 : state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3);
271 53 : state.have_vertical_velocity = true;
272 :
273 53 : velocity_to_speed_course(state);
274 :
275 : // Update position state
276 :
277 53 : state.location.lat = (int32_t) (pos_llh->lat * (double)1e7);
278 53 : state.location.lng = (int32_t) (pos_llh->lon * (double)1e7);
279 53 : state.location.alt = (int32_t) (pos_llh->height * 100);
280 53 : state.num_sats = pos_llh->n_sats;
281 :
282 53 : if (pos_llh->flags == 0) {
283 0 : state.status = AP_GPS_FixType::FIX_3D;
284 53 : } else if (pos_llh->flags == 2) {
285 0 : state.status = AP_GPS_FixType::RTK_FLOAT;
286 53 : } else if (pos_llh->flags == 1) {
287 53 : state.status = AP_GPS_FixType::RTK_FIXED;
288 : }
289 :
290 53 : last_full_update_tow = last_vel_ned.tow;
291 53 : last_full_update_cpu_ms = now;
292 53 : state.rtk_iar_num_hypotheses = last_iar_num_hypotheses;
293 :
294 : #if HAL_LOGGING_ENABLED
295 53 : logging_log_full_update();
296 : #endif
297 53 : ret = true;
298 :
299 642 : } else if (now - last_full_update_cpu_ms > SBP_TIMEOUT_PVT) {
300 :
301 : //INVARIANT: If we currently have a fix, ONLY return true after a full update.
302 :
303 96 : state.status = AP_GPS_FixType::NONE;
304 96 : ret = true;
305 :
306 : } else {
307 :
308 : //No timeouts yet, no data yet, nothing has happened.
309 :
310 : }
311 :
312 642 : return ret;
313 :
314 : }
315 :
316 :
317 :
318 : bool
319 23038517 : AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data)
320 : {
321 : // This switch reads one character at a time, if we find something that
322 : // looks like our preamble we'll try to read the full message length,
323 : // calculating the CRC. If the CRC matches, we have an SBP GPS!
324 :
325 23038517 : switch (state.state) {
326 21915461 : case SBP_detect_state::WAITING:
327 21915461 : if (data == SBP_PREAMBLE) {
328 62802 : state.n_read = 0;
329 62802 : state.crc_so_far = 0;
330 62802 : state.state = SBP_detect_state::GET_TYPE;
331 : }
332 21915461 : break;
333 :
334 125565 : case SBP_detect_state::GET_TYPE:
335 125565 : *((uint8_t*)&(state.msg_type) + state.n_read) = data;
336 125565 : state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
337 125565 : state.n_read += 1;
338 125565 : if (state.n_read >= 2) {
339 62778 : state.n_read = 0;
340 62778 : state.state = SBP_detect_state::GET_SENDER;
341 : }
342 125565 : break;
343 :
344 125556 : case SBP_detect_state::GET_SENDER:
345 125556 : state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
346 125556 : state.n_read += 1;
347 125556 : if (state.n_read >= 2) {
348 62778 : state.n_read = 0;
349 62778 : state.state = SBP_detect_state::GET_LEN;
350 : }
351 125556 : break;
352 :
353 62778 : case SBP_detect_state::GET_LEN:
354 62778 : state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
355 62778 : state.msg_len = data;
356 62778 : state.n_read = 0;
357 62778 : state.state = SBP_detect_state::GET_MSG;
358 62778 : break;
359 :
360 683624 : case SBP_detect_state::GET_MSG:
361 683624 : if (state.msg_type == SBP_HEARTBEAT_MSGTYPE && state.n_read < 4) {
362 28 : *((uint8_t*)&(state.heartbeat_buff) + state.n_read) = data;
363 : }
364 683624 : state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
365 683624 : state.n_read += 1;
366 683624 : if (state.n_read >= state.msg_len) {
367 62767 : state.n_read = 0;
368 62767 : state.state = SBP_detect_state::GET_CRC;
369 : }
370 683624 : break;
371 :
372 125533 : case SBP_detect_state::GET_CRC:
373 125533 : *((uint8_t*)&(state.crc) + state.n_read) = data;
374 125533 : state.n_read += 1;
375 125533 : if (state.n_read >= 2) {
376 62766 : state.state = SBP_detect_state::WAITING;
377 62766 : if (state.crc == state.crc_so_far
378 45 : && state.msg_type == SBP_HEARTBEAT_MSGTYPE) {
379 1 : struct sbp_heartbeat_t* heartbeat = ((struct sbp_heartbeat_t*)state.heartbeat_buff);
380 1 : return heartbeat->protocol_major == 0;
381 : }
382 62765 : return false;
383 : }
384 62767 : break;
385 :
386 0 : default:
387 0 : state.state = SBP_detect_state::WAITING;
388 0 : break;
389 : }
390 22975751 : return false;
391 : }
392 :
393 : #if HAL_LOGGING_ENABLED
394 : void
395 53 : AP_GPS_SBP::logging_log_full_update()
396 : {
397 :
398 53 : if (!should_log()) {
399 0 : return;
400 : }
401 :
402 53 : struct log_SbpHealth pkt = {
403 : LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH),
404 53 : time_us : AP_HAL::micros64(),
405 53 : crc_error_counter : crc_error_counter,
406 53 : last_injected_data_ms : last_injected_data_ms,
407 53 : last_iar_num_hypotheses : last_iar_num_hypotheses,
408 53 : };
409 :
410 53 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
411 : };
412 : #endif // HAL_LOGGING_ENABLED
413 :
414 : #endif // AP_GPS_SBP_ENABLED
|
