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_SBP2.h"
26 : #include <AP_Logger/AP_Logger.h>
27 : #include <GCS_MAVLink/GCS_MAVLink.h>
28 : #include <GCS_MAVLink/GCS.h>
29 :
30 : #if AP_GPS_SBP2_ENABLED
31 :
32 : extern const AP_HAL::HAL& hal;
33 :
34 : #define SBP_DEBUGGING 0
35 : #define SBP_INFOREPORTING 1
36 :
37 : //INVARIANT: We expect SBP to give us a heartbeat in less than 2 seconds.
38 : // This is more lax than the default Piksi settings,
39 : // and we assume the user hasn't reconfigured their Piksi to longer heartbeat intervals
40 : #define SBP_TIMEOUT_HEARTBEAT 2000
41 :
42 : #if SBP_DEBUGGING
43 : # define Debug(fmt, args ...) \
44 : do { \
45 : hal.console->printf("%s:%d: " fmt "\n", \
46 : __FUNCTION__, __LINE__, \
47 : ## args); \
48 : hal.scheduler->delay(1); \
49 : } while(0)
50 : #else
51 : # define Debug(fmt, args ...)
52 : #endif
53 :
54 : #if SBP_INFOREPORTING
55 : # define Info(fmt, args ...) \
56 : do { \
57 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ## args); \
58 : } while(0)
59 : #else
60 : # define Info(fmt, args ...)
61 : #endif
62 :
63 :
64 1 : AP_GPS_SBP2::AP_GPS_SBP2(AP_GPS &_gps,
65 : AP_GPS::Params &_params,
66 : AP_GPS::GPS_State &_state,
67 1 : AP_HAL::UARTDriver *_port) :
68 1 : AP_GPS_Backend(_gps, _params, _state, _port)
69 : {
70 : Debug("SBP Driver Initialized");
71 1 : parser_state.state = sbp_parser_state_t::WAITING;
72 1 : }
73 :
74 : // Process all bytes available from the stream
75 : //
76 : bool
77 642 : AP_GPS_SBP2::read(void)
78 : {
79 : //Invariant: Calling this function processes *all* data current in the UART buffer.
80 : //
81 : //IMPORTANT NOTICE: This function is NOT CALLED for several seconds
82 : // during arming. That should not cause the driver to die. Process *all* waiting messages
83 :
84 642 : _sbp_process();
85 642 : return _attempt_state_update();
86 : }
87 :
88 : void
89 0 : AP_GPS_SBP2::inject_data(const uint8_t *data, uint16_t len)
90 : {
91 0 : if (port->txspace() > len) {
92 0 : last_injected_data_ms = AP_HAL::millis();
93 0 : port->write(data, len);
94 : } else {
95 : Debug("PIKSI: Not enough TXSPACE");
96 : }
97 0 : }
98 :
99 : //This attempts to reads all SBP messages from the incoming port.
100 : //Returns true if a new message was read, false if we failed to read a message.
101 : void
102 642 : AP_GPS_SBP2::_sbp_process()
103 : {
104 642 : uint32_t nleft = port->available();
105 9476 : while (nleft > 0) {
106 8834 : nleft--;
107 8834 : uint8_t temp = port->read();
108 : #if AP_GPS_DEBUG_LOGGING_ENABLED
109 : log_data(&temp, 1);
110 : #endif
111 : uint16_t crc;
112 :
113 : //This switch reads one character at a time,
114 : //parsing it into buffers until a full message is dispatched
115 8834 : switch (parser_state.state) {
116 1118 : case sbp_parser_state_t::WAITING:
117 1118 : if (temp == SBP_PREAMBLE) {
118 250 : parser_state.n_read = 0;
119 250 : parser_state.state = sbp_parser_state_t::GET_TYPE;
120 : }
121 1118 : break;
122 :
123 500 : case sbp_parser_state_t::GET_TYPE:
124 500 : *((uint8_t*)&(parser_state.msg_type) + parser_state.n_read) = temp;
125 500 : parser_state.n_read += 1;
126 500 : if (parser_state.n_read >= 2) {
127 250 : parser_state.n_read = 0;
128 250 : parser_state.state = sbp_parser_state_t::GET_SENDER;
129 : }
130 500 : break;
131 :
132 500 : case sbp_parser_state_t::GET_SENDER:
133 500 : *((uint8_t*)&(parser_state.sender_id) + parser_state.n_read) = temp;
134 500 : parser_state.n_read += 1;
135 500 : if (parser_state.n_read >= 2) {
136 250 : parser_state.n_read = 0;
137 250 : parser_state.state = sbp_parser_state_t::GET_LEN;
138 : }
139 500 : break;
140 :
141 250 : case sbp_parser_state_t::GET_LEN:
142 250 : parser_state.msg_len = temp;
143 250 : parser_state.n_read = 0;
144 250 : parser_state.state = sbp_parser_state_t::GET_MSG;
145 250 : break;
146 :
147 5966 : case sbp_parser_state_t::GET_MSG:
148 5966 : *((uint8_t*)&(parser_state.msg_buff) + parser_state.n_read) = temp;
149 5966 : parser_state.n_read += 1;
150 5966 : if (parser_state.n_read >= parser_state.msg_len) {
151 250 : parser_state.n_read = 0;
152 250 : parser_state.state = sbp_parser_state_t::GET_CRC;
153 : }
154 5966 : break;
155 :
156 500 : case sbp_parser_state_t::GET_CRC:
157 500 : *((uint8_t*)&(parser_state.crc) + parser_state.n_read) = temp;
158 500 : parser_state.n_read += 1;
159 500 : if (parser_state.n_read >= 2) {
160 250 : parser_state.state = sbp_parser_state_t::WAITING;
161 :
162 250 : crc = crc16_ccitt((uint8_t*)&(parser_state.msg_type), 2, 0);
163 250 : crc = crc16_ccitt((uint8_t*)&(parser_state.sender_id), 2, crc);
164 250 : crc = crc16_ccitt(&(parser_state.msg_len), 1, crc);
165 250 : crc = crc16_ccitt(parser_state.msg_buff, parser_state.msg_len, crc);
166 250 : if (parser_state.crc == crc) {
167 250 : _sbp_process_message();
168 : } else {
169 : Debug("CRC Error Occurred!");
170 0 : crc_error_counter += 1;
171 : }
172 : }
173 500 : break;
174 :
175 0 : default:
176 0 : parser_state.state = sbp_parser_state_t::WAITING;
177 0 : break;
178 : }
179 : }
180 642 : }
181 :
182 :
183 : //INVARIANT: A fully received message with correct CRC is currently in parser_state
184 : void
185 250 : AP_GPS_SBP2::_sbp_process_message() {
186 : //Here, we copy messages into local structs.
187 250 : switch (parser_state.msg_type) {
188 11 : case SBP_HEARTBEAT_MSGTYPE:
189 11 : memcpy(&last_heartbeat, parser_state.msg_buff, sizeof(struct sbp_heartbeat_t));
190 11 : last_heartbeat_received_ms = AP_HAL::millis();
191 11 : break;
192 :
193 57 : case SBP_GPS_TIME_MSGTYPE:
194 57 : memcpy(&last_gps_time, parser_state.msg_buff, sizeof(struct sbp_gps_time_t));
195 57 : check_new_itow(last_gps_time.tow, parser_state.msg_len);
196 57 : break;
197 :
198 57 : case SBP_VEL_NED_MSGTYPE:
199 57 : memcpy(&last_vel_ned, parser_state.msg_buff, sizeof(struct sbp_vel_ned_t));
200 57 : check_new_itow(last_vel_ned.tow, parser_state.msg_len);
201 57 : break;
202 :
203 114 : case SBP_POS_LLH_MSGTYPE:
204 114 : memcpy(&last_pos_llh, parser_state.msg_buff, sizeof(struct sbp_pos_llh_t));
205 114 : check_new_itow(last_pos_llh.tow, parser_state.msg_len);
206 114 : break;
207 :
208 11 : case SBP_DOPS_MSGTYPE:
209 11 : memcpy(&last_dops, parser_state.msg_buff, sizeof(struct sbp_dops_t));
210 11 : check_new_itow(last_dops.tow, parser_state.msg_len);
211 11 : break;
212 :
213 0 : case SBP_EXT_EVENT_MSGTYPE:
214 0 : memcpy(&last_event, parser_state.msg_buff, sizeof(struct sbp_ext_event_t));
215 0 : check_new_itow(last_event.tow, parser_state.msg_len);
216 : #if HAL_LOGGING_ENABLED
217 0 : logging_ext_event();
218 : #endif
219 0 : break;
220 :
221 0 : default:
222 0 : break;
223 : }
224 250 : }
225 :
226 : int32_t
227 1186 : AP_GPS_SBP2::distMod(int32_t tow1_ms, int32_t tow2_ms, int32_t mod) {
228 1186 : return MIN(abs(tow1_ms - tow2_ms), mod - abs(tow1_ms - tow2_ms));
229 : }
230 :
231 : bool
232 642 : AP_GPS_SBP2::_attempt_state_update()
233 : {
234 642 : if (last_heartbeat_received_ms == 0)
235 49 : return false;
236 :
237 593 : uint32_t now = AP_HAL::millis();
238 :
239 593 : if (now - last_heartbeat_received_ms > SBP_TIMEOUT_HEARTBEAT) {
240 :
241 0 : state.status = AP_GPS_FixType::NONE;
242 0 : Info("No Heartbeats from Piksi! Status to NO_FIX.");
243 0 : return false;
244 :
245 593 : } else if (last_heartbeat.protocol_major != 2) {
246 :
247 0 : state.status = AP_GPS_FixType::NONE;
248 0 : Info("Received a heartbeat from non-SBPv2 device. Current driver only supports SBPv2. Status to NO_FIX.");
249 0 : return false;
250 :
251 593 : } else if (last_heartbeat.nap_error == 1 ||
252 593 : last_heartbeat.io_error == 1 ||
253 593 : last_heartbeat.sys_error == 1) {
254 :
255 0 : state.status = AP_GPS_FixType::NONE;
256 :
257 0 : Info("Piksi reported an error. Status to NO_FIX.");
258 : Debug(" ext_antenna: %d", last_heartbeat.ext_antenna);
259 : Debug(" res2: %d", last_heartbeat.res2);
260 : Debug(" protocol_major: %d", last_heartbeat.protocol_major);
261 : Debug(" protocol_minor: %d", last_heartbeat.protocol_minor);
262 : Debug(" res: %d", last_heartbeat.res);
263 : Debug(" nap_error: %d", last_heartbeat.nap_error);
264 : Debug(" io_error: %d", last_heartbeat.io_error);
265 : Debug(" sys_error: %d", last_heartbeat.sys_error);
266 :
267 0 : return false;
268 :
269 1186 : } else if (last_pos_llh.tow == last_vel_ned.tow
270 593 : && (distMod(last_gps_time.tow, last_vel_ned.tow, AP_MSEC_PER_WEEK) < 10000)
271 593 : && (distMod(last_dops.tow, last_vel_ned.tow, AP_MSEC_PER_WEEK) < 60000)
272 1186 : && (last_vel_ned.tow > last_full_update_tow || (last_gps_time.wn > last_full_update_wn && last_vel_ned.tow < last_full_update_tow))) {
273 :
274 : //We have an aligned VEL and LLH, and a recent DOPS and TIME.
275 :
276 : //
277 : // Check Flags for Valid Messages
278 : //
279 53 : if (last_gps_time.flags.time_src == 0 ||
280 53 : last_vel_ned.flags.vel_mode == 0 ||
281 53 : last_pos_llh.flags.fix_mode == 0 ||
282 53 : last_dops.flags.fix_mode == 0) {
283 :
284 : Debug("Message Marked as Invalid. NO FIX! Flags: {GPS_TIME: %d, VEL_NED: %d, POS_LLH: %d, DOPS: %d}",
285 : last_gps_time.flags.time_src,
286 : last_vel_ned.flags.vel_mode,
287 : last_pos_llh.flags.fix_mode,
288 : last_dops.flags.fix_mode);
289 :
290 0 : state.status = AP_GPS_FixType::NONE;
291 0 : return false;
292 : }
293 :
294 : //
295 : // Update external time and accuracy state
296 : //
297 53 : state.time_week = last_gps_time.wn;
298 53 : state.time_week_ms = last_vel_ned.tow;
299 53 : state.hdop = last_dops.hdop;
300 53 : state.vdop = last_dops.vdop;
301 53 : state.last_gps_time_ms = now;
302 :
303 : //
304 : // Update velocity state
305 : //
306 53 : state.velocity[0] = (float)(last_vel_ned.n * 1.0e-3);
307 53 : state.velocity[1] = (float)(last_vel_ned.e * 1.0e-3);
308 53 : state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3);
309 :
310 53 : velocity_to_speed_course(state);
311 :
312 106 : state.speed_accuracy = safe_sqrt(
313 53 : powf((float)last_vel_ned.h_accuracy * 1.0e-3f, 2) +
314 53 : powf((float)last_vel_ned.v_accuracy * 1.0e-3f, 2));
315 53 : state.horizontal_accuracy = (float) last_pos_llh.h_accuracy * 1.0e-3f;
316 53 : state.vertical_accuracy = (float) last_pos_llh.v_accuracy * 1.0e-3f;
317 :
318 : //
319 : // Set flags appropriately
320 : //
321 53 : state.have_vertical_velocity = true;
322 53 : state.have_speed_accuracy = !is_zero(state.speed_accuracy);
323 53 : state.have_horizontal_accuracy = !is_zero(state.horizontal_accuracy);
324 53 : state.have_vertical_accuracy = !is_zero(state.vertical_accuracy);
325 :
326 : //
327 : // Update position state
328 : //
329 53 : state.location.lat = (int32_t) (last_pos_llh.lat * (double)1e7);
330 53 : state.location.lng = (int32_t) (last_pos_llh.lon * (double)1e7);
331 53 : state.location.alt = (int32_t) (last_pos_llh.height * 100);
332 53 : state.num_sats = last_pos_llh.n_sats;
333 :
334 53 : switch (last_pos_llh.flags.fix_mode) {
335 0 : case 1:
336 0 : state.status = AP_GPS_FixType::FIX_3D;
337 0 : break;
338 0 : case 2:
339 0 : state.status = AP_GPS_FixType::DGPS;
340 0 : break;
341 0 : case 3:
342 0 : state.status = AP_GPS_FixType::RTK_FLOAT;
343 0 : break;
344 53 : case 4:
345 53 : state.status = AP_GPS_FixType::RTK_FIXED;
346 53 : break;
347 0 : case 6:
348 0 : state.status = AP_GPS_FixType::DGPS;
349 0 : break;
350 0 : default:
351 0 : state.status = AP_GPS_FixType::NONE;
352 0 : break;
353 : }
354 :
355 : //
356 : // Update Internal Timing
357 : //
358 53 : last_full_update_tow = last_vel_ned.tow;
359 53 : last_full_update_wn = last_gps_time.wn;
360 :
361 53 : return true;
362 : }
363 540 : return false;
364 : }
365 :
366 :
367 :
368 : bool
369 23038518 : AP_GPS_SBP2::_detect(struct SBP2_detect_state &state, uint8_t data)
370 : {
371 : // This switch reads one character at a time, if we find something that
372 : // looks like our preamble we'll try to read the full message length,
373 : // calculating the CRC. If the CRC matches, we have an SBP GPS!
374 23038518 : switch (state.state) {
375 21915461 : case SBP2_detect_state::WAITING:
376 21915461 : if (data == SBP_PREAMBLE) {
377 62802 : state.n_read = 0;
378 62802 : state.crc_so_far = 0;
379 62802 : state.state = SBP2_detect_state::GET_TYPE;
380 : }
381 21915461 : break;
382 :
383 125565 : case SBP2_detect_state::GET_TYPE:
384 125565 : *((uint8_t*)&(state.msg_type) + state.n_read) = data;
385 125565 : state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
386 125565 : state.n_read += 1;
387 125565 : if (state.n_read >= 2) {
388 62778 : state.n_read = 0;
389 62778 : state.state = SBP2_detect_state::GET_SENDER;
390 : }
391 125565 : break;
392 :
393 125556 : case SBP2_detect_state::GET_SENDER:
394 125556 : state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
395 125556 : state.n_read += 1;
396 125556 : if (state.n_read >= 2) {
397 62778 : state.n_read = 0;
398 62778 : state.state = SBP2_detect_state::GET_LEN;
399 : }
400 125556 : break;
401 :
402 62778 : case SBP2_detect_state::GET_LEN:
403 62778 : state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
404 62778 : state.msg_len = data;
405 62778 : state.n_read = 0;
406 62778 : state.state = SBP2_detect_state::GET_MSG;
407 62778 : break;
408 :
409 683624 : case SBP2_detect_state::GET_MSG:
410 683624 : if (state.msg_type == SBP_HEARTBEAT_MSGTYPE && state.n_read < 4) {
411 28 : *((uint8_t*)&(state.heartbeat_buff) + state.n_read) = data;
412 : }
413 683624 : state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
414 683624 : state.n_read += 1;
415 683624 : if (state.n_read >= state.msg_len) {
416 62767 : state.n_read = 0;
417 62767 : state.state = SBP2_detect_state::GET_CRC;
418 : }
419 683624 : break;
420 :
421 125534 : case SBP2_detect_state::GET_CRC:
422 125534 : *((uint8_t*)&(state.crc) + state.n_read) = data;
423 125534 : state.n_read += 1;
424 125534 : if (state.n_read >= 2) {
425 62767 : state.state = SBP2_detect_state::WAITING;
426 62767 : if (state.crc == state.crc_so_far
427 46 : && state.msg_type == SBP_HEARTBEAT_MSGTYPE) {
428 2 : struct sbp_heartbeat_t* heartbeat = ((struct sbp_heartbeat_t*)state.heartbeat_buff);
429 2 : return heartbeat->protocol_major == 2;
430 : }
431 62765 : return false;
432 : }
433 62767 : break;
434 :
435 0 : default:
436 0 : state.state = SBP2_detect_state::WAITING;
437 0 : break;
438 : }
439 22975751 : return false;
440 : }
441 :
442 : #if HAL_LOGGING_ENABLED
443 : void
444 0 : AP_GPS_SBP2::logging_log_full_update()
445 : {
446 0 : if (!should_log()) {
447 0 : return;
448 : }
449 :
450 : //TODO: Expand with heartbeat info.
451 : //TODO: Get rid of IAR NUM HYPO
452 :
453 0 : struct log_SbpHealth pkt = {
454 : LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH),
455 0 : time_us : AP_HAL::micros64(),
456 0 : crc_error_counter : crc_error_counter,
457 0 : last_injected_data_ms : last_injected_data_ms,
458 : last_iar_num_hypotheses : 0,
459 0 : };
460 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
461 : };
462 : void
463 0 : AP_GPS_SBP2::logging_ext_event() {
464 0 : if (!should_log()) {
465 0 : return;
466 : }
467 :
468 0 : struct log_SbpEvent pkt = {
469 : LOG_PACKET_HEADER_INIT(LOG_MSG_SBPEVENT),
470 0 : time_us : AP_HAL::micros64(),
471 0 : wn : last_event.wn,
472 0 : tow : last_event.tow,
473 0 : ns_residual : last_event.ns_residual,
474 0 : level : last_event.flags.level,
475 0 : quality : last_event.flags.quality,
476 0 : };
477 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
478 : };
479 : #endif // HAL_LOGGING_ENABLED
480 : #endif //AP_GPS_SBP2_ENABLED
|
