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 : support for serial connected AHRS systems
17 : */
18 :
19 : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
20 :
21 : #include "AP_ExternalAHRS_config.h"
22 :
23 : #if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
24 :
25 : #include "AP_ExternalAHRS_VectorNav.h"
26 : #include <AP_Math/AP_Math.h>
27 : #include <AP_Math/crc.h>
28 : #include <AP_Baro/AP_Baro.h>
29 : #include <AP_Compass/AP_Compass.h>
30 : #include <AP_GPS/AP_GPS.h>
31 : #include <AP_InertialSensor/AP_InertialSensor.h>
32 : #include <GCS_MAVLink/GCS.h>
33 : #include <AP_Logger/AP_Logger.h>
34 : #include <AP_SerialManager/AP_SerialManager.h>
35 : #include <AP_Common/NMEA.h>
36 : #include <stdio.h>
37 : #include <AP_BoardConfig/AP_BoardConfig.h>
38 :
39 : extern const AP_HAL::HAL &hal;
40 :
41 :
42 : /*
43 : TYPE::VN_AHRS configures 2 packets: high-rate IMU and mid-rate EKF
44 : Header for IMU packet
45 : $VNWRG,75,3,16,01,0721*D415
46 : Common group (Group 1)
47 : TimeStartup
48 : AngularRate
49 : Accel
50 : Imu
51 : MagPres
52 : Header for EKF packet
53 : $VNWRG,76,3,16,11,0001,0106*B36B
54 : Common group (Group 1)
55 : TimeStartup
56 : Attitude group (Group 4)
57 : Ypr
58 : Quaternion
59 : YprU
60 : */
61 :
62 : struct PACKED VN_IMU_packet {
63 : static constexpr uint8_t header[]{0x01, 0x21, 0x07};
64 : uint64_t timeStartup;
65 : float gyro[3];
66 : float accel[3];
67 : float uncompAccel[3];
68 : float uncompAngRate[3];
69 : float mag[3];
70 : float temp;
71 : float pressure;
72 : };
73 : constexpr uint8_t VN_IMU_packet::header[];
74 : constexpr uint8_t VN_IMU_LENGTH = sizeof(VN_IMU_packet) + sizeof(VN_IMU_packet::header) + 1 + 2; // Includes sync byte and CRC
75 :
76 : struct PACKED VN_AHRS_ekf_packet {
77 : static constexpr uint8_t header[]{0x11, 0x01, 0x00, 0x06, 0x01};
78 : uint64_t timeStartup;
79 : float ypr[3];
80 : float quaternion[4];
81 : float yprU[3];
82 : };
83 : constexpr uint8_t VN_AHRS_ekf_packet::header[];
84 : constexpr uint8_t VN_AHRS_EKF_LENGTH = sizeof(VN_AHRS_ekf_packet) + sizeof(VN_AHRS_ekf_packet::header) + 1 + 2; // Includes sync byte and CRC
85 :
86 : /*
87 : TYPE::VN_INS configures 3 packets: high-rate IMU, mid-rate EKF, and 5Hz GNSS
88 : Header for IMU packet
89 : $VNWRG,75,3,16,01,0721*D415
90 : Common group (Group 1)
91 : TimeStartup
92 : AngularRate
93 : Accel
94 : Imu
95 : MagPres
96 : Header for EKF packet
97 : $VNWRG,76,3,16,31,0001,0106,0613*097A
98 : Common group (Group 1)
99 : TimeStartup
100 : Attitude group (Group 4)
101 : Ypr
102 : Quaternion
103 : YprU
104 : Ins group (Group 5)
105 : InsStatus
106 : PosLla
107 : VelNed
108 : PosU
109 : VelU
110 : Header for GNSS packet
111 : $VNWRG,77,1,160,49,0003,26B8,0018*4FD9
112 : Common group (Group 1)
113 : TimeStartup
114 : TimeGps
115 : Gnss1 group (Group 3)
116 : NumSats
117 : GnssFix
118 : GnssPosLla
119 : GnssVelNed
120 : PosU1
121 : VelU1
122 : GnssDop
123 : Gnss2 group (Group 6)
124 : NumSats
125 : GnssFix
126 : */
127 :
128 : union Ins_Status {
129 : uint16_t _value;
130 : struct {
131 : uint16_t mode : 2;
132 : uint16_t gnssFix : 1;
133 : uint16_t resv1 : 2;
134 : uint16_t imuErr : 1;
135 : uint16_t magPresErr : 1;
136 : uint16_t gnssErr : 1;
137 : uint16_t resv2 : 1;
138 : uint16_t gnssHeadingIns : 2;
139 : };
140 : };
141 :
142 : struct PACKED VN_INS_ekf_packet {
143 : static constexpr uint8_t header[]{0x31, 0x01, 0x00, 0x06, 0x01, 0x13, 0x06};
144 : uint64_t timeStartup;
145 : float ypr[3];
146 : float quaternion[4];
147 : float yprU[3];
148 : uint16_t insStatus;
149 : double posLla[3];
150 : float velNed[3];
151 : float posU;
152 : float velU;
153 : };
154 : constexpr uint8_t VN_INS_ekf_packet::header[];
155 : constexpr uint8_t VN_INS_EKF_LENGTH = sizeof(VN_INS_ekf_packet) + sizeof(VN_INS_ekf_packet::header) + 1 + 2; // Includes sync byte and CRC
156 :
157 : struct PACKED VN_INS_gnss_packet {
158 : static constexpr uint8_t header[]{0x49, 0x03, 0x00, 0xB8, 0x26, 0x18, 0x00};
159 : uint64_t timeStartup;
160 : uint64_t timeGps;
161 : uint8_t numSats1;
162 : uint8_t fix1;
163 : double posLla1[3];
164 : float velNed1[3];
165 : float posU1[3];
166 : float velU1;
167 : float dop1[7];
168 : uint8_t numSats2;
169 : uint8_t fix2;
170 : };
171 : constexpr uint8_t VN_INS_gnss_packet::header[];
172 : constexpr uint8_t VN_INS_GNSS_LENGTH = sizeof(VN_INS_gnss_packet) + sizeof(VN_INS_gnss_packet::header) + 1 + 2; // Includes sync byte and CRC
173 :
174 : // constructor
175 12 : AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *_frontend,
176 12 : AP_ExternalAHRS::state_t &_state) :
177 12 : AP_ExternalAHRS_backend(_frontend, _state)
178 : {
179 12 : auto &sm = AP::serialmanager();
180 12 : uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
181 12 : if (!uart) {
182 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VectorNav ExternalAHRS no UART");
183 0 : return;
184 : }
185 12 : baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
186 12 : port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
187 :
188 12 : bufsize = MAX(MAX(MAX(VN_IMU_LENGTH, VN_INS_EKF_LENGTH), VN_INS_GNSS_LENGTH), VN_AHRS_EKF_LENGTH);
189 :
190 12 : pktbuf = NEW_NOTHROW uint8_t[bufsize];
191 12 : latest_ins_ekf_packet = NEW_NOTHROW VN_INS_ekf_packet;
192 12 : latest_ins_gnss_packet = NEW_NOTHROW VN_INS_gnss_packet;
193 :
194 12 : if (!pktbuf || !latest_ins_ekf_packet) {
195 0 : AP_BoardConfig::allocation_error("VectorNav ExternalAHRS");
196 : }
197 :
198 12 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_VectorNav::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
199 0 : AP_HAL::panic("VectorNav Failed to start ExternalAHRS update thread");
200 : }
201 12 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VectorNav ExternalAHRS initialised");
202 : }
203 :
204 : /*
205 : check the UART for more data
206 : returns true if the function should be called again straight away
207 : */
208 : #define SYNC_BYTE 0xFA
209 421838 : bool AP_ExternalAHRS_VectorNav::check_uart()
210 : {
211 421838 : if (!setup_complete) {
212 194 : return false;
213 : }
214 421644 : WITH_SEMAPHORE(state.sem);
215 : // ensure we own the uart
216 421718 : uart->begin(0);
217 421718 : uint32_t n = uart->available();
218 421718 : if (n == 0) {
219 383850 : return false;
220 : }
221 37868 : if (pktoffset < bufsize) {
222 37868 : ssize_t nread = uart->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset)));
223 37868 : if (nread <= 0) {
224 0 : return false;
225 : }
226 37868 : pktoffset += nread;
227 : }
228 :
229 37868 : bool match_header1 = false;
230 37868 : bool match_header2 = false;
231 37868 : bool match_header3 = false;
232 37868 : bool match_header4 = false;
233 :
234 37868 : if (pktbuf[0] != SYNC_BYTE) {
235 12 : goto reset;
236 : }
237 :
238 :
239 37856 : match_header1 = (0 == memcmp(&pktbuf[1], VN_IMU_packet::header, MIN(sizeof(VN_IMU_packet::header), unsigned(pktoffset - 1))));
240 37856 : if (type == TYPE::VN_AHRS) {
241 0 : match_header2 = (0 == memcmp(&pktbuf[1], VN_AHRS_ekf_packet::header, MIN(sizeof(VN_AHRS_ekf_packet::header), unsigned(pktoffset - 1))));
242 : } else {
243 37856 : match_header3 = (0 == memcmp(&pktbuf[1], VN_INS_ekf_packet::header, MIN(sizeof(VN_INS_ekf_packet::header), unsigned(pktoffset - 1))));
244 37856 : match_header4 = (0 == memcmp(&pktbuf[1], VN_INS_gnss_packet::header, MIN(sizeof(VN_INS_gnss_packet::header), unsigned(pktoffset - 1))));
245 : }
246 37856 : if (!match_header1 && !match_header2 && !match_header3 && !match_header4) {
247 0 : goto reset;
248 : }
249 :
250 37856 : if (match_header1 && pktoffset >= VN_IMU_LENGTH) {
251 17993 : uint16_t crc = crc16_ccitt(&pktbuf[1], VN_IMU_LENGTH - 1, 0);
252 17993 : if (crc == 0) {
253 17993 : process_imu_packet(&pktbuf[sizeof(VN_IMU_packet::header) + 1]);
254 17993 : memmove(&pktbuf[0], &pktbuf[VN_IMU_LENGTH], pktoffset - VN_IMU_LENGTH);
255 17993 : pktoffset -= VN_IMU_LENGTH;
256 : } else {
257 0 : goto reset;
258 : }
259 37856 : } else if (match_header2 && pktoffset >= VN_AHRS_EKF_LENGTH) {
260 0 : uint16_t crc = crc16_ccitt(&pktbuf[1], VN_AHRS_EKF_LENGTH - 1, 0);
261 0 : if (crc == 0) {
262 0 : process_ahrs_ekf_packet(&pktbuf[sizeof(VN_AHRS_ekf_packet::header) + 1]);
263 0 : memmove(&pktbuf[0], &pktbuf[VN_AHRS_EKF_LENGTH], pktoffset - VN_AHRS_EKF_LENGTH);
264 0 : pktoffset -= VN_AHRS_EKF_LENGTH;
265 : } else {
266 0 : goto reset;
267 : }
268 19863 : } else if (match_header3 && pktoffset >= VN_INS_EKF_LENGTH) {
269 17993 : uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_EKF_LENGTH - 1, 0);
270 17993 : if (crc == 0) {
271 17993 : process_ins_ekf_packet(&pktbuf[sizeof(VN_INS_ekf_packet::header) + 1]);
272 17993 : memmove(&pktbuf[0], &pktbuf[VN_INS_EKF_LENGTH], pktoffset - VN_INS_EKF_LENGTH);
273 17993 : pktoffset -= VN_INS_EKF_LENGTH;
274 : } else {
275 0 : goto reset;
276 : }
277 19863 : } else if (match_header4 && pktoffset >= VN_INS_GNSS_LENGTH) {
278 1870 : uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_GNSS_LENGTH - 1, 0);
279 1870 : if (crc == 0) {
280 1870 : process_ins_gnss_packet(&pktbuf[sizeof(VN_INS_gnss_packet::header) + 1]);
281 1870 : memmove(&pktbuf[0], &pktbuf[VN_INS_GNSS_LENGTH], pktoffset - VN_INS_GNSS_LENGTH);
282 1870 : pktoffset -= VN_INS_GNSS_LENGTH;
283 : } else {
284 0 : goto reset;
285 : }
286 : }
287 37856 : return true;
288 :
289 12 : reset:
290 12 : uint8_t *p = (uint8_t *)memchr(&pktbuf[1], SYNC_BYTE, pktoffset-1);
291 12 : if (p) {
292 4 : uint8_t newlen = pktoffset - (p - pktbuf);
293 4 : memmove(&pktbuf[0], p, newlen);
294 4 : pktoffset = newlen;
295 : } else {
296 8 : pktoffset = 0;
297 : }
298 12 : return true;
299 421718 : }
300 :
301 : // Send command and wait for response
302 : // Only run from thread! This blocks and retries until a non-error response is received
303 : #define READ_REQUEST_RETRY_MS 500
304 96 : void AP_ExternalAHRS_VectorNav::run_command(const char * fmt, ...)
305 : {
306 : va_list ap;
307 :
308 96 : va_start(ap, fmt);
309 96 : hal.util->vsnprintf(message_to_send, sizeof(message_to_send), fmt, ap);
310 96 : va_end(ap);
311 :
312 96 : uint32_t request_sent = 0;
313 : while (true) {
314 1242 : hal.scheduler->delay(1);
315 :
316 1242 : const uint32_t now = AP_HAL::millis();
317 1242 : if (now - request_sent > READ_REQUEST_RETRY_MS) {
318 96 : nmea_printf(uart, "$%s", message_to_send);
319 96 : request_sent = now;
320 : }
321 :
322 1242 : int16_t nbytes = uart->available();
323 67086 : while (nbytes-- > 0) {
324 65940 : char c = uart->read();
325 65940 : if (decode(c)) {
326 96 : if (nmea.error_response && nmea.sentence_done) {
327 : // Received a valid VNERR. Try to resend after the timeout length
328 0 : break;
329 : }
330 96 : return;
331 : }
332 : }
333 1146 : }
334 : }
335 :
336 : // add a single character to the buffer and attempt to decode
337 : // returns true if a complete sentence was successfully decoded
338 65940 : bool AP_ExternalAHRS_VectorNav::decode(char c)
339 : {
340 65940 : switch (c) {
341 398 : case ',':
342 : // end of a term, add to checksum
343 398 : nmea.checksum ^= c;
344 : FALLTHROUGH;
345 948 : case '\r':
346 : case '\n':
347 : case '*':
348 : {
349 948 : if (nmea.sentence_done) {
350 190 : return false;
351 : }
352 758 : if (nmea.term_is_checksum) {
353 138 : nmea.sentence_done = true;
354 : uint8_t checksum;
355 138 : if (!hex_twochars_to_uint8(nmea.term, checksum)) {
356 42 : return false;
357 : }
358 96 : return ((checksum == nmea.checksum) && nmea.sentence_valid);
359 : }
360 :
361 : // null terminate and decode latest term
362 620 : nmea.term[nmea.term_offset] = 0;
363 620 : if (nmea.sentence_valid) {
364 488 : nmea.sentence_valid = decode_latest_term();
365 : }
366 :
367 : // move onto next term
368 620 : nmea.term_number++;
369 620 : nmea.term_offset = 0;
370 620 : nmea.term_is_checksum = (c == '*');
371 620 : return false;
372 : }
373 :
374 164 : case '$': // sentence begin
375 164 : nmea.sentence_valid = true;
376 164 : nmea.term_number = 0;
377 164 : nmea.term_offset = 0;
378 164 : nmea.checksum = 0;
379 164 : nmea.term_is_checksum = false;
380 164 : nmea.sentence_done = false;
381 164 : nmea.error_response = false;
382 164 : return false;
383 : }
384 :
385 : // ordinary characters are added to term
386 64828 : if (nmea.term_offset < sizeof(nmea.term) - 1) {
387 6736 : nmea.term[nmea.term_offset++] = c;
388 : }
389 64828 : if (!nmea.term_is_checksum) {
390 46510 : nmea.checksum ^= c;
391 : }
392 :
393 64828 : return false;
394 : }
395 :
396 : // decode the most recently consumed term
397 : // returns true if new term is valid
398 488 : bool AP_ExternalAHRS_VectorNav::decode_latest_term()
399 : {
400 : // Check the first two terms (In most cases header + reg number) that they match the sent
401 : // message. If not, the response is invalid.
402 488 : switch (nmea.term_number) {
403 140 : case 0:
404 140 : if (strncmp(nmea.term, "VNERR", nmea.term_offset) == 0) {
405 0 : nmea.error_response = true; // Message will be printed on next term
406 140 : } else if (strncmp(nmea.term, message_to_send, nmea.term_offset) != 0) {
407 44 : return false;
408 : }
409 96 : return true;
410 96 : case 1:
411 96 : if (nmea.error_response) {
412 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "VectorNav received VNERR code: %s", nmea.term);
413 96 : } else if (strlen(message_to_send) > 6 &&
414 96 : strncmp(nmea.term, &message_to_send[6], nmea.term_offset) != 0) { // Start after "VNXXX,"
415 0 : return false;
416 : }
417 96 : return true;
418 72 : case 2:
419 72 : if (strncmp(nmea.term, "VN-", 3) == 0) {
420 : // This term is the model number
421 12 : strncpy(model_name, nmea.term, sizeof(model_name));
422 : }
423 72 : return true;
424 180 : default:
425 180 : return true;
426 : }
427 : }
428 :
429 12 : void AP_ExternalAHRS_VectorNav::initialize() {
430 : // Open port in the thread
431 12 : uart->begin(baudrate, 1024, 512);
432 :
433 : // Pause asynchronous communications to simplify packet finding
434 12 : run_command("VNASY,0");
435 :
436 : // Stop ASCII async outputs for both UARTs. If only active UART is disabled, we get a baudrate
437 : // overflow on the other UART when configuring binary outputs (reg 75 and 76) to both UARTs
438 12 : run_command("VNWRG,06,0,1");
439 12 : run_command("VNWRG,06,0,2");
440 :
441 : // Read Model Number Register, ID 1
442 12 : run_command("VNRRG,01");
443 :
444 : // Setup for messages respective model types (on both UARTs)
445 12 : if (strncmp(model_name, "VN-1", 4) == 0) {
446 : // VN-1X0
447 0 : type = TYPE::VN_AHRS;
448 :
449 : // These assumes unit is still configured at its default rate of 800hz
450 0 : run_command("VNWRG,75,3,%u,01,0721", unsigned(800 / get_rate()));
451 0 : run_command("VNWRG,76,3,16,11,0001,0106");
452 : } else {
453 : // Default to setup for sensors other than VN-100 or VN-110
454 : // This assumes unit is still configured at its default IMU rate of 400hz for VN-300, 800hz for others
455 12 : uint16_t imu_rate = 800; // Default for everything but VN-300
456 12 : if (strncmp(model_name, "VN-300", 6) == 0) {
457 12 : imu_rate = 400;
458 : }
459 12 : if (strncmp(model_name, "VN-3", 4) == 0) {
460 12 : has_dual_gnss = true;
461 : }
462 12 : run_command("VNWRG,75,3,%u,01,0721", unsigned(imu_rate / get_rate()));
463 12 : run_command("VNWRG,76,3,%u,31,0001,0106,0613", unsigned(imu_rate / 50));
464 12 : run_command("VNWRG,77,3,%u,49,0003,26B8,0018", unsigned(imu_rate / 5));
465 : }
466 :
467 : // Resume asynchronous communications
468 12 : run_command("VNASY,1");
469 12 : setup_complete = true;
470 12 : }
471 :
472 12 : void AP_ExternalAHRS_VectorNav::update_thread() {
473 12 : initialize();
474 : while (true) {
475 88607 : if (!check_uart()) {
476 75719 : hal.scheduler->delay(1);
477 : }
478 : }
479 : }
480 :
481 0 : const char* AP_ExternalAHRS_VectorNav::get_name() const
482 : {
483 0 : if (setup_complete) {
484 0 : return model_name;
485 : }
486 0 : return nullptr;
487 : }
488 :
489 : // Input data struct for EAHA logging message, used by both AHRS mode and INS mode
490 : struct AP_ExternalAHRS_VectorNav::VNAT {
491 : uint64_t timeUs;
492 : float quat[4];
493 : float ypr[3];
494 : float yprU[3];
495 : };
496 :
497 :
498 17993 : void AP_ExternalAHRS_VectorNav::write_vnat(const VNAT& data_to_log) const {
499 :
500 : #if HAL_LOGGING_ENABLED
501 : // @LoggerMessage: VNAT
502 : // @Description: VectorNav Attitude data
503 : // @Field: TimeUS: Time since system startup
504 : // @Field: Q1: Attitude quaternion 1
505 : // @Field: Q2: Attitude quaternion 2
506 : // @Field: Q3: Attitude quaternion 3
507 : // @Field: Q4: Attitude quaternion 4
508 : // @Field: Yaw: Yaw
509 : // @Field: Pitch: Pitch
510 : // @Field: Roll: Roll
511 : // @Field: YU: Yaw unceratainty
512 : // @Field: PU: Pitch uncertainty
513 : // @Field: RU: Roll uncertainty
514 :
515 17993 : AP::logger().WriteStreaming("VNAT", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU",
516 : "s----dddddd", "F0000000000",
517 : "Qffffffffff",
518 17993 : data_to_log.timeUs,
519 17993 : data_to_log.quat[0], data_to_log.quat[1], data_to_log.quat[2], data_to_log.quat[3],
520 17993 : data_to_log.ypr[0], data_to_log.ypr[1], data_to_log.ypr[2],
521 17993 : data_to_log.yprU[0], data_to_log.yprU[1], data_to_log.yprU[2]);
522 : #endif
523 17993 : }
524 :
525 :
526 :
527 : // process INS mode INS packet
528 17993 : void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b)
529 : {
530 17993 : const struct VN_IMU_packet &pkt = *(struct VN_IMU_packet *)b;
531 :
532 17993 : last_pkt1_ms = AP_HAL::millis();
533 :
534 17993 : const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU);
535 :
536 : {
537 17993 : WITH_SEMAPHORE(state.sem);
538 17993 : if (use_uncomp) {
539 0 : state.accel = Vector3f{pkt.uncompAccel[0], pkt.uncompAccel[1], pkt.uncompAccel[2]};
540 0 : state.gyro = Vector3f{pkt.uncompAngRate[0], pkt.uncompAngRate[1], pkt.uncompAngRate[2]};
541 : } else {
542 17993 : state.accel = Vector3f{pkt.accel[0], pkt.accel[1], pkt.accel[2]};
543 17993 : state.gyro = Vector3f{pkt.gyro[0], pkt.gyro[1], pkt.gyro[2]};
544 : }
545 17993 : }
546 :
547 : #if AP_BARO_EXTERNALAHRS_ENABLED
548 : {
549 : AP_ExternalAHRS::baro_data_message_t baro;
550 17993 : baro.instance = 0;
551 17993 : baro.pressure_pa = pkt.pressure * 1e3;
552 17993 : baro.temperature = pkt.temp;
553 :
554 17993 : AP::baro().handle_external(baro);
555 : }
556 : #endif
557 :
558 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
559 : {
560 17993 : AP_ExternalAHRS::mag_data_message_t mag;
561 17993 : mag.field = Vector3f{pkt.mag[0], pkt.mag[1], pkt.mag[2]};
562 17993 : mag.field *= 1000; // to mGauss
563 :
564 17993 : AP::compass().handle_external(mag);
565 : }
566 : #endif
567 :
568 : {
569 17993 : AP_ExternalAHRS::ins_data_message_t ins;
570 :
571 17993 : ins.accel = state.accel;
572 17993 : ins.gyro = state.gyro;
573 17993 : ins.temperature = pkt.temp;
574 :
575 17993 : AP::ins().handle_external(ins);
576 : }
577 :
578 : #if HAL_LOGGING_ENABLED
579 : // @LoggerMessage: VNIM
580 : // @Description: VectorNav IMU data
581 : // @Field: TimeUS: Time since system startup
582 : // @Field: Temp: Temprature
583 : // @Field: Pres: Pressure
584 : // @Field: MX: Magnetic feild X-axis
585 : // @Field: MY: Magnetic feild Y-axis
586 : // @Field: MZ: Magnetic feild Z-axis
587 : // @Field: AX: Acceleration X-axis
588 : // @Field: AY: Acceleration Y-axis
589 : // @Field: AZ: Acceleration Z-axis
590 : // @Field: GX: Rotation rate X-axis
591 : // @Field: GY: Rotation rate Y-axis
592 : // @Field: GZ: Rotation rate Z-axis
593 :
594 35986 : AP::logger().WriteStreaming("VNIM", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ",
595 : "sdPGGGoooEEE", "F00000000000",
596 : "Qfffffffffff",
597 : AP_HAL::micros64(),
598 17993 : pkt.temp, pkt.pressure*1e3,
599 17993 : pkt.mag[0], pkt.mag[1], pkt.mag[2],
600 17993 : state.accel[0], state.accel[1], state.accel[2],
601 17993 : state.gyro[0], state.gyro[1], state.gyro[2],
602 17993 : state.quat[0], state.quat[1], state.quat[2], state.quat[3]);
603 : #endif // HAL_LOGGING_ENABLED
604 17993 : }
605 :
606 : // process AHRS mode AHRS packet
607 0 : void AP_ExternalAHRS_VectorNav::process_ahrs_ekf_packet(const uint8_t *b) {
608 0 : const struct VN_AHRS_ekf_packet &pkt = *(struct VN_AHRS_ekf_packet *)b;
609 :
610 0 : last_pkt2_ms = AP_HAL::millis();
611 :
612 0 : state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]};
613 0 : state.have_quaternion = true;
614 :
615 : #if HAL_LOGGING_ENABLED
616 : VNAT data_to_log;
617 0 : data_to_log.timeUs = AP_HAL::micros64();
618 0 : memcpy(data_to_log.quat, pkt.quaternion, sizeof(pkt.quaternion));
619 0 : memcpy(data_to_log.ypr, pkt.ypr, sizeof(pkt.ypr));
620 0 : memcpy(data_to_log.yprU, pkt.yprU, sizeof(pkt.yprU));
621 :
622 0 : write_vnat(data_to_log);
623 : #endif // HAL_LOGGING_ENABLED
624 0 : }
625 :
626 : // process INS mode EKF packet
627 17993 : void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) {
628 17993 : const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)b;
629 :
630 17993 : last_pkt2_ms = AP_HAL::millis();
631 17993 : *latest_ins_ekf_packet = pkt;
632 :
633 17993 : state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]};
634 17993 : state.have_quaternion = true;
635 :
636 17993 : state.velocity = Vector3f{pkt.velNed[0], pkt.velNed[1], pkt.velNed[2]};
637 17993 : state.have_velocity = true;
638 :
639 17993 : state.location = Location{int32_t(pkt.posLla[0] * 1.0e7), int32_t(pkt.posLla[1] * 1.0e7), int32_t(pkt.posLla[2] * 1.0e2), Location::AltFrame::ABSOLUTE};
640 17993 : state.last_location_update_us = AP_HAL::micros();
641 17993 : state.have_location = true;
642 :
643 : #if HAL_LOGGING_ENABLED
644 : VNAT data_to_log;
645 17993 : auto now = AP_HAL::micros64();
646 17993 : data_to_log.timeUs = now;
647 17993 : memcpy(data_to_log.quat, pkt.quaternion, sizeof(pkt.quaternion));
648 17993 : memcpy(data_to_log.ypr, pkt.ypr, sizeof(pkt.ypr));
649 17993 : memcpy(data_to_log.yprU, pkt.yprU, sizeof(pkt.yprU));
650 17993 : write_vnat(data_to_log);
651 :
652 : // @LoggerMessage: VNKF
653 : // @Description: VectorNav INS Kalman Filter data
654 : // @Field: TimeUS: Time since system startup
655 : // @Field: InsStatus: VectorNav INS health status
656 : // @Field: Lat: Latitude
657 : // @Field: Lon: Longitude
658 : // @Field: Alt: Altitude
659 : // @Field: VelN: Velocity Northing
660 : // @Field: VelE: Velocity Easting
661 : // @Field: VelD: Velocity Downing
662 : // @Field: PosU: Filter estimated position uncertainty
663 : // @Field: VelU: Filter estimated Velocity uncertainty
664 :
665 17993 : AP::logger().WriteStreaming("VNKF", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU",
666 : "s-ddmnnndn", "F000000000",
667 : "QHdddfffff",
668 : now,
669 17993 : pkt.insStatus,
670 17993 : pkt.posLla[0], pkt.posLla[1], pkt.posLla[2],
671 17993 : pkt.velNed[0], pkt.velNed[1], pkt.velNed[2],
672 17993 : pkt.posU, pkt.velU);
673 : #endif // HAL_LOGGING_ENABLED
674 :
675 17993 : }
676 :
677 : // process INS mode GNSS packet
678 1870 : void AP_ExternalAHRS_VectorNav::process_ins_gnss_packet(const uint8_t *b) {
679 1870 : const struct VN_INS_gnss_packet &pkt = *(struct VN_INS_gnss_packet *)b;
680 : AP_ExternalAHRS::gps_data_message_t gps;
681 :
682 :
683 1870 : last_pkt3_ms = AP_HAL::millis();
684 1870 : *latest_ins_gnss_packet = pkt;
685 :
686 : // get ToW in milliseconds
687 1870 : gps.gps_week = pkt.timeGps / (AP_MSEC_PER_WEEK * 1000000ULL);
688 1870 : gps.ms_tow = (pkt.timeGps / 1000000ULL) % (60 * 60 * 24 * 7 * 1000ULL);
689 1870 : gps.fix_type = AP_GPS_FixType(pkt.fix1);
690 1870 : gps.satellites_in_view = pkt.numSats1;
691 :
692 1870 : gps.horizontal_pos_accuracy = pkt.posU1[0];
693 1870 : gps.vertical_pos_accuracy = pkt.posU1[2];
694 1870 : gps.horizontal_vel_accuracy = pkt.velU1;
695 :
696 1870 : gps.hdop = pkt.dop1[4];
697 1870 : gps.vdop = pkt.dop1[3];
698 :
699 1870 : gps.latitude = pkt.posLla1[0] * 1.0e7;
700 1870 : gps.longitude = pkt.posLla1[1] * 1.0e7;
701 1870 : gps.msl_altitude = pkt.posLla1[2] * 1.0e2;
702 :
703 1870 : gps.ned_vel_north = pkt.velNed1[0];
704 1870 : gps.ned_vel_east = pkt.velNed1[1];
705 1870 : gps.ned_vel_down = pkt.velNed1[2];
706 :
707 1870 : if (!state.have_origin && gps.fix_type >= AP_GPS_FixType::FIX_3D) {
708 12 : WITH_SEMAPHORE(state.sem);
709 24 : state.origin = Location{int32_t(pkt.posLla1[0] * 1.0e7), int32_t(pkt.posLla1[1] * 1.0e7),
710 12 : int32_t(pkt.posLla1[2] * 1.0e2), Location::AltFrame::ABSOLUTE};
711 12 : state.have_origin = true;
712 12 : }
713 : uint8_t instance;
714 1870 : if (AP::gps().get_first_external_instance(instance)) {
715 1564 : AP::gps().handle_external(gps, instance);
716 : }
717 1870 : }
718 :
719 : // get serial port number for the uart
720 38 : int8_t AP_ExternalAHRS_VectorNav::get_port(void) const
721 : {
722 38 : if (!uart) {
723 0 : return -1;
724 : }
725 38 : return port_num;
726 : };
727 :
728 : // accessors for AP_AHRS
729 33068 : bool AP_ExternalAHRS_VectorNav::healthy(void) const
730 : {
731 33068 : const uint32_t now = AP_HAL::millis();
732 33068 : return (now - last_pkt1_ms < 40) && (now - last_pkt2_ms < 500) && (type == TYPE::VN_AHRS ? true: now - last_pkt3_ms < 1000);
733 : }
734 :
735 33008 : bool AP_ExternalAHRS_VectorNav::initialised(void) const
736 : {
737 33008 : if (!setup_complete) {
738 10 : return false;
739 : }
740 32998 : return last_pkt1_ms != UINT32_MAX && last_pkt2_ms != UINT32_MAX && (type == TYPE::VN_AHRS ? true : last_pkt3_ms != UINT32_MAX);
741 : }
742 :
743 60 : bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
744 : {
745 60 : if (!setup_complete) {
746 0 : hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav setup failed");
747 0 : return false;
748 : }
749 60 : if (!healthy()) {
750 0 : hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav unhealthy");
751 0 : return false;
752 : }
753 60 : if (type == TYPE::VN_INS) {
754 60 : if (latest_ins_gnss_packet->fix1 < 3) {
755 0 : hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock");
756 0 : return false;
757 : }
758 60 : if (has_dual_gnss && (latest_ins_gnss_packet->fix2 < 3)) {
759 0 : hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS2 lock");
760 0 : return false;
761 : }
762 : }
763 60 : return true;
764 : }
765 :
766 : /*
767 : get filter status. We don't know the meaning of the status bits yet,
768 : so assume all OK if we have GPS lock
769 : */
770 18182 : void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) const
771 : {
772 18182 : memset(&status, 0, sizeof(status));
773 18182 : status.flags.initalized = initialised();
774 18182 : if (healthy()) {
775 18172 : if (type == TYPE::VN_AHRS) {
776 0 : status.flags.attitude = true;
777 : } else {
778 18172 : status.flags.attitude = true;
779 18172 : if (latest_ins_ekf_packet) {
780 18172 : status.flags.vert_vel = true;
781 18172 : status.flags.vert_pos = true;
782 :
783 18172 : status.flags.horiz_vel = true;
784 18172 : status.flags.horiz_pos_rel = true;
785 18172 : status.flags.horiz_pos_abs = true;
786 18172 : status.flags.pred_horiz_pos_rel = true;
787 18172 : status.flags.pred_horiz_pos_abs = true;
788 18172 : status.flags.using_gps = true;
789 : }
790 : }
791 : }
792 18182 : }
793 :
794 : // get variances
795 18182 : bool AP_ExternalAHRS_VectorNav::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
796 : {
797 18182 : const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)latest_ins_ekf_packet;
798 18182 : velVar = pkt.velU * vel_gate_scale;
799 18182 : posVar = pkt.posU * pos_gate_scale;
800 18182 : hgtVar = pkt.posU * hgt_gate_scale;
801 18182 : tasVar = 0;
802 18182 : return true;
803 : }
804 :
805 : #endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
|
