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 : // Septentrio GPS driver for ArduPilot.
18 : // Code by Michael Oborne
19 : //
20 :
21 : #include "AP_GPS.h"
22 : #include "AP_GPS_SBF.h"
23 : #include <GCS_MAVLink/GCS.h>
24 : #include <AP_InternalError/AP_InternalError.h>
25 : #include <stdio.h>
26 : #include <ctype.h>
27 :
28 : #if AP_GPS_SBF_ENABLED
29 : extern const AP_HAL::HAL& hal;
30 :
31 : #define SBF_DEBUGGING 0
32 :
33 : #if SBF_DEBUGGING
34 : // INFO rather than debug because MP filters DEBUG
35 : # define Debug(fmt, args ...) \
36 : do { \
37 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s:%d: " fmt, \
38 : __FUNCTION__, __LINE__, \
39 : ## args); \
40 : } while(0)
41 : #else
42 : # define Debug(fmt, args ...)
43 : #endif
44 :
45 : #ifndef GPS_SBF_STREAM_NUMBER
46 : #define GPS_SBF_STREAM_NUMBER 1
47 : #endif
48 :
49 : #define SBF_EXCESS_COMMAND_BYTES 5 // 2 start bytes + validity byte + space byte + endline byte
50 :
51 : #define RX_ERROR_MASK (CONGESTION | \
52 : MISSEDEVENT | \
53 : CPUOVERLOAD | \
54 : INVALIDCONFIG | \
55 : OUTOFGEOFENCE)
56 :
57 : constexpr const char *AP_GPS_SBF::portIdentifiers[];
58 : constexpr const char* AP_GPS_SBF::_initialisation_blob[];
59 : constexpr const char* AP_GPS_SBF::sbas_on_blob[];
60 :
61 1 : AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps,
62 : AP_GPS::Params &_params,
63 : AP_GPS::GPS_State &_state,
64 1 : AP_HAL::UARTDriver *_port) :
65 1 : AP_GPS_Backend(_gps, _params, _state, _port)
66 : {
67 1 : sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
68 :
69 1 : _config_last_ack_time = AP_HAL::millis();
70 :
71 : // if we ever parse RTK observations it will always be of type NED, so set it once
72 1 : state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED;
73 :
74 : // yaw available when option bit set or using dual antenna
75 2 : if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw) ||
76 1 : (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA)) {
77 0 : state.gps_yaw_configured = true;
78 : }
79 1 : }
80 :
81 0 : AP_GPS_SBF::~AP_GPS_SBF (void) {
82 0 : free(config_string);
83 0 : }
84 :
85 : // Process all bytes available from the stream
86 : //
87 : bool
88 693 : AP_GPS_SBF::read(void)
89 : {
90 693 : bool ret = false;
91 693 : uint32_t available_bytes = port->available();
92 8505 : for (uint32_t i = 0; i < available_bytes; i++) {
93 7812 : uint8_t temp = port->read();
94 : #if AP_GPS_DEBUG_LOGGING_ENABLED
95 : log_data(&temp, 1);
96 : #endif
97 7812 : ret |= parse(temp);
98 : }
99 :
100 693 : const uint32_t now = AP_HAL::millis();
101 693 : if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
102 693 : if (config_step != Config_State::Complete) {
103 693 : if (now > _init_blob_time) {
104 693 : if (now > _config_last_ack_time + 2000) {
105 6 : const size_t port_enable_len = strlen(_port_enable);
106 6 : if (port_enable_len <= port->txspace()) {
107 : // try to enable input on the GPS port if we have not made progress on configuring it
108 : Debug("SBF Sending port enable");
109 6 : port->write((const uint8_t*)_port_enable, port_enable_len);
110 6 : _config_last_ack_time = now;
111 : }
112 687 : } else if (readyForCommand) {
113 0 : if (config_string == nullptr) {
114 0 : switch (config_step) {
115 0 : case Config_State::Baud_Rate:
116 0 : if (asprintf(&config_string, "scs,COM%d,baud%d,bits8,No,bit1,%s\n",
117 0 : (int)params.com_port,
118 : 230400,
119 0 : port->get_flow_control() != AP_HAL::UARTDriver::flow_control::FLOW_CONTROL_ENABLE ? "none" : "RTS|CTS") == -1) {
120 0 : config_string = nullptr;
121 : }
122 0 : break;
123 0 : case Config_State::SSO:
124 : const char *extra_config;
125 0 : switch (get_type()) {
126 0 : case AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA:
127 0 : extra_config = "+AttCovEuler+AuxAntPositions";
128 0 : break;
129 0 : case AP_GPS::GPS_Type::GPS_TYPE_SBF:
130 : default:
131 0 : extra_config = "";
132 0 : break;
133 : }
134 0 : if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod%s,msec100\n",
135 : (int)GPS_SBF_STREAM_NUMBER,
136 0 : (int)params.com_port,
137 0 : extra_config) == -1) {
138 0 : config_string = nullptr;
139 : }
140 0 : break;
141 0 : case Config_State::Constellation:
142 0 : if ((params.gnss_mode&0x6F)!=0) {
143 : //IMES not taken into account by Septentrio receivers
144 0 : if (asprintf(&config_string, "sst, %s%s%s%s%s%s\n", (params.gnss_mode&(1U<<0))!=0 ? "GPS" : "",
145 0 : (params.gnss_mode&(1U<<1))!=0 ? ((params.gnss_mode&0x01)==0 ? "SBAS" : "+SBAS") : "",
146 0 : (params.gnss_mode&(1U<<2))!=0 ? ((params.gnss_mode&0x03)==0 ? "GALILEO" : "+GALILEO") : "",
147 0 : (params.gnss_mode&(1U<<3))!=0 ? ((params.gnss_mode&0x07)==0 ? "BEIDOU" : "+BEIDOU") : "",
148 0 : (params.gnss_mode&(1U<<5))!=0 ? ((params.gnss_mode&0x0F)==0 ? "QZSS" : "+QZSS") : "",
149 0 : (params.gnss_mode&(1U<<6))!=0 ? ((params.gnss_mode&0x2F)==0 ? "GLONASS" : "+GLONASS") : "") == -1) {
150 0 : config_string=nullptr;
151 : }
152 0 : break;
153 : }
154 0 : config_step = Config_State::Blob;
155 : FALLTHROUGH;
156 0 : case Config_State::Blob:
157 0 : if (asprintf(&config_string, "%s\n", _initialisation_blob[_init_blob_index]) == -1) {
158 0 : config_string = nullptr;
159 : }
160 0 : break;
161 0 : case Config_State::SBAS:
162 0 : switch ((AP_GPS::SBAS_Mode)gps._sbas_mode) {
163 0 : case AP_GPS::SBAS_Mode::Disabled:
164 0 : if (asprintf(&config_string, "%s\n", sbas_off) == -1) {
165 0 : config_string = nullptr;
166 : }
167 0 : break;
168 0 : case AP_GPS::SBAS_Mode::Enabled:
169 0 : if (asprintf(&config_string, "%s\n", sbas_on_blob[_init_blob_index]) == -1) {
170 0 : config_string = nullptr;
171 : }
172 0 : break;
173 0 : case AP_GPS::SBAS_Mode::DoNotChange:
174 0 : config_string = nullptr;
175 0 : config_step = Config_State::Complete;
176 0 : break;
177 : }
178 0 : break;
179 0 : case Config_State::SGA:
180 : {
181 0 : const char *targetGA = "none";
182 0 : if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) {
183 0 : targetGA = "MultiAntenna";
184 : }
185 0 : if (asprintf(&config_string, "sga, %s\n", targetGA)) {
186 0 : config_string = nullptr;
187 : }
188 0 : break;
189 : }
190 0 : case Config_State::Complete:
191 : // should never reach here, why search for a config if we have fully configured already
192 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
193 0 : break;
194 : }
195 : }
196 :
197 0 : if (config_string != nullptr) {
198 0 : const size_t config_length = strlen(config_string);
199 0 : if (config_length <= port->txspace()) {
200 : Debug("SBF sending init string: %s", config_string);
201 0 : port->write((const uint8_t*)config_string, config_length);
202 0 : readyForCommand = false;
203 : }
204 : }
205 : }
206 : }
207 0 : } else if (gps._raw_data == 2) { // only manage disarm/rearms when the user opts into it
208 0 : if (hal.util->get_soft_armed()) {
209 0 : _has_been_armed = true;
210 0 : } else if (_has_been_armed && (RxState & SBF_DISK_MOUNTED)) {
211 : // since init is done at this point and unmounting should be rate limited,
212 : // take over the _init_blob_time variable
213 0 : if (now > _init_blob_time) {
214 0 : unmount_disk();
215 0 : _init_blob_time = now + 1000;
216 : }
217 : }
218 : }
219 : }
220 :
221 : // yaw timeout after 300 milliseconds
222 693 : if ((now - state.gps_yaw_time_ms) > 300) {
223 693 : state.have_gps_yaw = false;
224 693 : state.have_gps_yaw_accuracy = false;
225 : }
226 :
227 693 : return ret;
228 : }
229 :
230 0 : bool AP_GPS_SBF::logging_healthy(void) const
231 : {
232 0 : switch (gps._raw_data) {
233 0 : case 1:
234 : default:
235 0 : return (RxState & SBF_DISK_MOUNTED) && (RxState & SBF_DISK_ACTIVITY);
236 0 : case 2:
237 0 : return ((RxState & SBF_DISK_MOUNTED) && (RxState & SBF_DISK_ACTIVITY)) || (!hal.util->get_soft_armed() && _has_been_armed);
238 : }
239 : }
240 :
241 : bool
242 7812 : AP_GPS_SBF::parse(uint8_t temp)
243 : {
244 7812 : switch (sbf_msg.sbf_state)
245 : {
246 126 : default:
247 : case sbf_msg_parser_t::PREAMBLE1:
248 126 : if (temp == SBF_PREAMBLE1) {
249 126 : sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE2;
250 126 : sbf_msg.read = 0;
251 : } else {
252 : // attempt to detect command prompt
253 0 : portIdentifier[portLength++] = (char)temp;
254 0 : bool foundPossiblePort = false;
255 0 : for (const char *portId : portIdentifiers) {
256 0 : if (strncmp(portId, portIdentifier, MIN(portLength, 3)) == 0) {
257 : // we found one of the COM/USB/IP related ports
258 0 : if (portLength == 4) {
259 : // validate that we have an ascii number
260 0 : if (isdigit((char)temp)) {
261 0 : foundPossiblePort = true;
262 0 : break;
263 : }
264 0 : } else if (portLength >= sizeof(portIdentifier)) {
265 0 : if ((char)temp == '>') {
266 0 : readyForCommand = true;
267 : Debug("SBF: Ready for command");
268 : }
269 : } else {
270 0 : foundPossiblePort = true;
271 : }
272 0 : break;
273 : }
274 : }
275 0 : if (!foundPossiblePort) {
276 0 : portLength = 0;
277 : }
278 : }
279 126 : break;
280 126 : case sbf_msg_parser_t::PREAMBLE2:
281 126 : if (temp == SBF_PREAMBLE2) {
282 126 : sbf_msg.sbf_state = sbf_msg_parser_t::CRC1;
283 0 : } else if (temp == 'R') {
284 : Debug("SBF got a response\n");
285 0 : sbf_msg.sbf_state = sbf_msg_parser_t::COMMAND_LINE;
286 : }
287 : else
288 : {
289 0 : sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
290 : }
291 126 : break;
292 126 : case sbf_msg_parser_t::CRC1:
293 126 : sbf_msg.crc = temp;
294 126 : sbf_msg.sbf_state = sbf_msg_parser_t::CRC2;
295 126 : break;
296 126 : case sbf_msg_parser_t::CRC2:
297 126 : sbf_msg.crc += (uint16_t)(temp << 8);
298 126 : sbf_msg.sbf_state = sbf_msg_parser_t::BLOCKID1;
299 126 : break;
300 126 : case sbf_msg_parser_t::BLOCKID1:
301 126 : sbf_msg.blockid = temp;
302 126 : sbf_msg.sbf_state = sbf_msg_parser_t::BLOCKID2;
303 126 : break;
304 126 : case sbf_msg_parser_t::BLOCKID2:
305 126 : sbf_msg.blockid += (uint16_t)(temp << 8);
306 126 : sbf_msg.sbf_state = sbf_msg_parser_t::LENGTH1;
307 126 : break;
308 126 : case sbf_msg_parser_t::LENGTH1:
309 126 : sbf_msg.length = temp;
310 126 : sbf_msg.sbf_state = sbf_msg_parser_t::LENGTH2;
311 126 : break;
312 126 : case sbf_msg_parser_t::LENGTH2:
313 126 : sbf_msg.length += (uint16_t)(temp << 8);
314 126 : sbf_msg.sbf_state = sbf_msg_parser_t::DATA;
315 126 : if (sbf_msg.length % 4 != 0) {
316 0 : sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
317 : Debug("bad packet length=%u\n", (unsigned)sbf_msg.length);
318 : }
319 126 : if (sbf_msg.length < 8) {
320 : Debug("bad packet length=%u\n", (unsigned)sbf_msg.length);
321 0 : sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
322 0 : crc_error_counter++; // this is a probable buffer overflow, but this
323 : // indicates not enough bytes to do a crc
324 0 : break;
325 : }
326 126 : if (sbf_msg.length > 256) {
327 : // no SBF packet is this big! serial corruption may
328 : // cause the length to get very large; 24320 has been
329 : // seen (0x5F00). Discard and go back to looking for
330 : // preamble.
331 0 : sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
332 0 : crc_error_counter++; // this is a probable serial corruption
333 : }
334 126 : break;
335 6804 : case sbf_msg_parser_t::DATA:
336 6804 : if (sbf_msg.read < sizeof(sbf_msg.data)) {
337 6804 : sbf_msg.data.bytes[sbf_msg.read] = temp;
338 : }
339 6804 : sbf_msg.read++;
340 6804 : if (sbf_msg.read >= (sbf_msg.length - 8)) {
341 126 : if (sbf_msg.read > sizeof(sbf_msg.data)) {
342 : // not interested in these large messages
343 0 : sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
344 0 : break;
345 : }
346 126 : uint16_t crc = crc16_ccitt((uint8_t*)&sbf_msg.blockid, 2, 0);
347 126 : crc = crc16_ccitt((uint8_t*)&sbf_msg.length, 2, crc);
348 126 : crc = crc16_ccitt((uint8_t*)&sbf_msg.data, sbf_msg.length - 8, crc);
349 :
350 126 : sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
351 :
352 126 : if (sbf_msg.crc == crc) {
353 126 : return process_message();
354 : } else {
355 : Debug("crc fail\n");
356 0 : crc_error_counter++;
357 : }
358 : }
359 6678 : break;
360 0 : case sbf_msg_parser_t::COMMAND_LINE:
361 0 : if (sbf_msg.read < (sizeof(sbf_msg.data) - 1)) {
362 0 : sbf_msg.data.bytes[sbf_msg.read] = temp;
363 : } else {
364 : // we don't have enough buffer to compare the commands
365 : // most probable cause is that a user injected a longer command then
366 : // we have buffer for, or it could be a corruption, either way we
367 : // simply ignore the result
368 0 : sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
369 0 : break;
370 : }
371 0 : sbf_msg.read++;
372 0 : if (temp == '\n') {
373 0 : sbf_msg.data.bytes[sbf_msg.read] = 0;
374 :
375 : // received the result, lets assess it
376 0 : if (sbf_msg.data.bytes[0] == ':') {
377 : // valid command, determine if it was the one we were trying
378 : // to send in the configuration sequence
379 0 : if (config_string != nullptr) {
380 0 : if (!strncmp(config_string, (char *)(sbf_msg.data.bytes + 2),
381 0 : sbf_msg.read - SBF_EXCESS_COMMAND_BYTES)) {
382 : Debug("SBF Ack Command: %s\n", sbf_msg.data.bytes);
383 0 : free(config_string);
384 0 : config_string = nullptr;
385 0 : switch (config_step) {
386 0 : case Config_State::Baud_Rate:
387 0 : config_step = Config_State::SSO;
388 0 : break;
389 0 : case Config_State::SSO:
390 0 : config_step = Config_State::Constellation;
391 0 : break;
392 0 : case Config_State::Constellation:
393 : // we can also move to
394 : // Config_State::Blob if we choose
395 : // not to update the GPS's
396 : // constellation configuration
397 : // (above).
398 0 : config_step = Config_State::Blob;
399 0 : break;
400 0 : case Config_State::Blob:
401 0 : _init_blob_index++;
402 0 : if (_init_blob_index >= ARRAY_SIZE(_initialisation_blob)) {
403 0 : config_step = Config_State::SBAS;
404 0 : _init_blob_index = 0;
405 : }
406 0 : break;
407 0 : case Config_State::SBAS:
408 0 : _init_blob_index++;
409 0 : if ((gps._sbas_mode == AP_GPS::SBAS_Mode::Disabled)
410 0 : ||_init_blob_index >= ARRAY_SIZE(sbas_on_blob)) {
411 0 : config_step = Config_State::SGA;
412 : }
413 0 : break;
414 0 : case Config_State::SGA:
415 0 : config_step = Config_State::Complete;
416 0 : break;
417 0 : case Config_State::Complete:
418 : // should never reach here, this implies that we validated a config string when we hadn't sent any
419 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
420 0 : break;
421 : }
422 0 : _config_last_ack_time = AP_HAL::millis();
423 : } else {
424 : Debug("SBF Ack command (unexpected): %s\n", sbf_msg.data.bytes);
425 : }
426 : }
427 : } else {
428 : // rejected command, send it out as a debug
429 : Debug("SBF NACK Command: %s\n", sbf_msg.data.bytes);
430 : }
431 : // resume normal parsing
432 0 : sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
433 0 : break;
434 : }
435 0 : break;
436 : }
437 :
438 7686 : return false;
439 : }
440 :
441 63 : static bool is_DNU(double value)
442 : {
443 63 : constexpr double DNU = -2e10f;
444 : #pragma GCC diagnostic push
445 : #pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as it's false positive when testing for DNU values
446 63 : return value == DNU;
447 : #pragma GCC diagnostic pop
448 : }
449 :
450 : bool
451 126 : AP_GPS_SBF::process_message(void)
452 : {
453 126 : uint16_t blockid = (sbf_msg.blockid & 8191u);
454 :
455 : Debug("BlockID %d", blockid);
456 :
457 126 : switch (blockid) {
458 63 : case PVTGeodetic:
459 : {
460 63 : const msg4007 &temp = sbf_msg.data.msg4007u;
461 :
462 : // Update time state
463 63 : if (temp.WNc != 65535) {
464 63 : state.time_week = temp.WNc;
465 63 : state.time_week_ms = (uint32_t)(temp.TOW);
466 : }
467 :
468 63 : check_new_itow(temp.TOW, sbf_msg.length);
469 63 : state.last_gps_time_ms = AP_HAL::millis();
470 :
471 : // Update velocity state (don't use −2·10^10)
472 63 : if (temp.Vn > -200000) {
473 63 : state.velocity.x = (float)(temp.Vn);
474 63 : state.velocity.y = (float)(temp.Ve);
475 63 : state.velocity.z = (float)(-temp.Vu);
476 :
477 63 : state.have_vertical_velocity = true;
478 :
479 63 : velocity_to_speed_course(state);
480 63 : state.rtk_age_ms = temp.MeanCorrAge * 10;
481 :
482 : // value is expressed as twice the rms error = int16 * 0.01/2
483 63 : state.horizontal_accuracy = (float)temp.HAccuracy * 0.005f;
484 63 : state.vertical_accuracy = (float)temp.VAccuracy * 0.005f;
485 63 : state.have_horizontal_accuracy = true;
486 63 : state.have_vertical_accuracy = true;
487 : }
488 :
489 : // Update position state (don't use -2·10^10)
490 63 : if (temp.Latitude > -200000) {
491 63 : state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * (double)1e7);
492 63 : state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * (double)1e7);
493 63 : state.have_undulation = !is_DNU(temp.Undulation);
494 63 : double height = temp.Height; // in metres
495 63 : if (state.have_undulation) {
496 0 : height -= temp.Undulation;
497 0 : state.undulation = -temp.Undulation;
498 : }
499 63 : set_alt_amsl_cm(state, (float)height * 1e2f); // m -> cm
500 : }
501 :
502 63 : state.num_sats = temp.NrSV;
503 63 : if (temp.NrSV == 255) {
504 : // Do-Not-Use value for NrSV field in PVTGeodetic message
505 0 : state.num_sats = 0;
506 : }
507 :
508 : Debug("temp.Mode=0x%02x\n", (unsigned)temp.Mode);
509 63 : switch (temp.Mode & 15) {
510 0 : case 0: // no pvt
511 0 : state.status = AP_GPS_FixType::NONE;
512 0 : break;
513 0 : case 1: // standalone
514 0 : state.status = AP_GPS_FixType::FIX_3D;
515 0 : break;
516 0 : case 2: // dgps
517 0 : state.status = AP_GPS_FixType::DGPS;
518 0 : break;
519 0 : case 3: // fixed location
520 0 : state.status = AP_GPS_FixType::FIX_3D;
521 0 : break;
522 63 : case 4: // rtk fixed
523 63 : state.status = AP_GPS_FixType::RTK_FIXED;
524 63 : break;
525 0 : case 5: // rtk float
526 0 : state.status = AP_GPS_FixType::RTK_FLOAT;
527 0 : break;
528 0 : case 6: // sbas
529 0 : state.status = AP_GPS_FixType::DGPS;
530 0 : break;
531 0 : case 7: // moving rtk fixed
532 0 : state.status = AP_GPS_FixType::RTK_FIXED;
533 0 : break;
534 0 : case 8: // moving rtk float
535 0 : state.status = AP_GPS_FixType::RTK_FLOAT;
536 0 : break;
537 : }
538 :
539 63 : if ((temp.Mode & 64) > 0) { // gps is in base mode
540 0 : state.status = AP_GPS_FixType::NONE;
541 63 : } else if ((temp.Mode & 128) > 0) { // gps only has 2d fix
542 0 : state.status = AP_GPS_FixType::FIX_2D;
543 : }
544 :
545 63 : return true;
546 : }
547 63 : case DOP:
548 : {
549 63 : const msg4001 &temp = sbf_msg.data.msg4001u;
550 63 : check_new_itow(temp.TOW, sbf_msg.length);
551 :
552 63 : state.hdop = temp.HDOP;
553 63 : state.vdop = temp.VDOP;
554 63 : break;
555 : }
556 0 : case ReceiverStatus:
557 : {
558 0 : const msg4014 &temp = sbf_msg.data.msg4014u;
559 0 : check_new_itow(temp.TOW, sbf_msg.length);
560 0 : RxState = temp.RxState;
561 0 : if ((RxError & RX_ERROR_MASK) != (temp.RxError & RX_ERROR_MASK)) {
562 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF error changed (0x%08x/0x%08x)", (unsigned int)(state.instance + 1),
563 : (unsigned int)(RxError & RX_ERROR_MASK), (unsigned int)(temp.RxError & RX_ERROR_MASK));
564 : }
565 0 : RxError = temp.RxError;
566 0 : break;
567 : }
568 0 : case VelCovGeodetic:
569 : {
570 0 : const msg5908 &temp = sbf_msg.data.msg5908u;
571 :
572 0 : check_new_itow(temp.TOW, sbf_msg.length);
573 : // select the maximum variance, as the EKF will apply it to all the columns in it's estimate
574 : // FIXME: Support returning the covariance matrix to the EKF
575 0 : float max_variance_squared = MAX(temp.Cov_VnVn, MAX(temp.Cov_VeVe, temp.Cov_VuVu));
576 0 : if (is_positive(max_variance_squared)) {
577 0 : state.have_speed_accuracy = true;
578 0 : state.speed_accuracy = sqrt(max_variance_squared);
579 : } else {
580 0 : state.have_speed_accuracy = false;
581 : }
582 0 : break;
583 : }
584 0 : case AttEulerCov:
585 : {
586 : // yaw accuracy is taken from this message even though we actually calculate the yaw ourself (see AuxAntPositions below)
587 : // this is OK based on the assumption that the calculation methods are similar and that inaccuracy arises from the sensor readings
588 0 : if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) {
589 0 : const msg5939 &temp = sbf_msg.data.msg5939u;
590 :
591 0 : check_new_itow(temp.TOW, sbf_msg.length);
592 :
593 0 : constexpr uint8_t errorBits = 0x8F; // Bits 0-1 are aux 1 baseline
594 : // Bits 2-3 are aux 2 baseline
595 : // Bit 7 is attitude not requested
596 0 : if (((temp.Error & errorBits) == 0)
597 0 : && !is_DNU(temp.Cov_HeadHead)) {
598 0 : state.gps_yaw_accuracy = sqrtf(temp.Cov_HeadHead);
599 0 : state.have_gps_yaw_accuracy = true;
600 : } else {
601 0 : state.have_gps_yaw_accuracy = false;
602 : }
603 : }
604 0 : break;
605 : }
606 0 : case AuxAntPositions:
607 : {
608 : #if GPS_MOVING_BASELINE
609 0 : if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) {
610 : // calculate yaw using reported antenna positions in earth-frame
611 : // note that this calculation does not correct for the vehicle's roll and pitch meaning it is inaccurate at very high lean angles
612 0 : const msg5942 &temp = sbf_msg.data.msg5942u;
613 0 : check_new_itow(temp.TOW, sbf_msg.length);
614 0 : if (temp.N > 0 && temp.ant1.Error == 0 && temp.ant1.AmbiguityType == 0) {
615 : // valid RTK integer fix
616 0 : const float rel_heading_deg = degrees(atan2f(temp.ant1.DeltaEast, temp.ant1.DeltaNorth));
617 0 : calculate_moving_base_yaw(rel_heading_deg,
618 0 : Vector3f(temp.ant1.DeltaNorth, temp.ant1.DeltaEast, temp.ant1.DeltaUp).length(),
619 0 : -temp.ant1.DeltaUp);
620 : }
621 : }
622 : #endif
623 0 : break;
624 : }
625 0 : case BaseVectorGeod:
626 : {
627 0 : const msg4028 &temp = sbf_msg.data.msg4028u;
628 :
629 : // just breakout any consts we need for Do Not Use (DNU) reasons
630 0 : constexpr uint16_t uint16DNU = 65535;
631 :
632 0 : check_new_itow(temp.TOW, sbf_msg.length);
633 :
634 0 : if (temp.N == 0) { // no sub blocks so just bail, we can't do anything useful here
635 0 : state.rtk_num_sats = 0;
636 0 : state.rtk_age_ms = 0;
637 0 : state.rtk_baseline_y_mm = 0;
638 0 : state.rtk_baseline_x_mm = 0;
639 0 : state.rtk_baseline_z_mm = 0;
640 0 : break;
641 : }
642 :
643 0 : state.rtk_num_sats = temp.info.NrSV;
644 :
645 0 : state.rtk_age_ms = (temp.info.CorrAge != 65535) ? ((uint32_t)temp.info.CorrAge) * 10 : 0;
646 :
647 : // copy the position as long as the data isn't DNU, we require NED, and heading before accepting any of it
648 0 : if (!is_DNU(temp.info.DeltaEast) && !is_DNU(temp.info.DeltaNorth) && !is_DNU(temp.info.DeltaUp) &&
649 0 : (temp.info.Azimuth != uint16DNU)) {
650 :
651 0 : state.rtk_baseline_y_mm = temp.info.DeltaEast * 1e3;
652 0 : state.rtk_baseline_x_mm = temp.info.DeltaNorth * 1e3;
653 0 : state.rtk_baseline_z_mm = temp.info.DeltaUp * -1e3;
654 :
655 : #if GPS_MOVING_BASELINE
656 : // copy the baseline data as a yaw source
657 0 : if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) {
658 0 : calculate_moving_base_yaw(temp.info.Azimuth * 0.01f + 180.0f,
659 0 : Vector3f(temp.info.DeltaNorth, temp.info.DeltaEast, temp.info.DeltaUp).length(),
660 0 : -temp.info.DeltaUp);
661 : }
662 : #endif // GPS_MOVING_BASELINE
663 :
664 0 : } else if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) {
665 0 : state.rtk_baseline_y_mm = 0;
666 0 : state.rtk_baseline_x_mm = 0;
667 0 : state.rtk_baseline_z_mm = 0;
668 0 : state.have_gps_yaw = false;
669 : }
670 :
671 0 : break;
672 : }
673 : }
674 :
675 63 : return false;
676 : }
677 :
678 0 : void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
679 : {
680 0 : if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE &&
681 0 : config_step != Config_State::Complete) {
682 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF is not fully configured (%u/%u/%u/%u)",
683 : state.instance + 1,
684 : (unsigned)config_step,
685 : _init_blob_index,
686 : (unsigned)ARRAY_SIZE(_initialisation_blob),
687 : (unsigned)ARRAY_SIZE(sbas_on_blob));
688 : }
689 0 : }
690 :
691 5157 : bool AP_GPS_SBF::is_configured (void) const {
692 10314 : return ((gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) ||
693 10314 : (config_step == Config_State::Complete) ||AP_SIM_GPS_SBF_ENABLED);
694 : }
695 :
696 65 : bool AP_GPS_SBF::is_healthy (void) const {
697 65 : return (RxError & RX_ERROR_MASK) == 0;
698 : }
699 :
700 0 : void AP_GPS_SBF::mount_disk (void) const {
701 0 : const char* command = "emd, DSK1, Mount\n";
702 : Debug("Mounting disk");
703 0 : port->write((const uint8_t*)command, strlen(command));
704 0 : }
705 :
706 0 : void AP_GPS_SBF::unmount_disk (void) const {
707 0 : const char* command = "emd, DSK1, Unmount\n";
708 0 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "SBF unmounting disk");
709 0 : port->write((const uint8_t*)command, strlen(command));
710 0 : }
711 :
712 0 : bool AP_GPS_SBF::prepare_for_arming(void) {
713 0 : bool is_logging = true; // assume that its logging until proven otherwise
714 0 : if (gps._raw_data) {
715 0 : if (!(RxState & SBF_DISK_MOUNTED)){
716 0 : is_logging = false;
717 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: SBF disk is not mounted", state.instance + 1);
718 :
719 : // simply attempt to mount the disk, no need to check if the command was
720 : // ACK/NACK'd as we don't continuously attempt to remount the disk
721 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: Attempting to mount disk", state.instance + 1);
722 0 : mount_disk();
723 : // reset the flag to indicate if we should be logging
724 0 : _has_been_armed = false;
725 : }
726 0 : else if (RxState & SBF_DISK_FULL) {
727 0 : is_logging = false;
728 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: SBF disk is full", state.instance + 1);
729 : }
730 0 : else if (!(RxState & SBF_DISK_ACTIVITY)) {
731 0 : is_logging = false;
732 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: SBF is not currently logging", state.instance + 1);
733 : }
734 : }
735 :
736 0 : return is_logging;
737 : }
738 :
739 : #endif
|
