Line data Source code
1 : /*
2 : Copyright (C) 2025 Kraus Hamdani Aerospace Inc. All rights reserved.
3 :
4 : This program is free software: you can redistribute it and/or modify
5 : it under the terms of the GNU General Public License as published by
6 : the Free Software Foundation, either version 3 of the License, or
7 : (at your option) any later version.
8 :
9 : This program is distributed in the hope that it will be useful,
10 : but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : GNU General Public License for more details.
13 :
14 : You should have received a copy of the GNU General Public License
15 : along with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 : /*
18 : support for serial connected SBG INS
19 : */
20 :
21 : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
22 :
23 : #include "AP_ExternalAHRS_config.h"
24 :
25 : #if AP_EXTERNAL_AHRS_SBG_ENABLED
26 :
27 : #include "AP_ExternalAHRS_SBG.h"
28 : #include <AP_Math/AP_Math.h>
29 : #include <AP_Math/crc.h>
30 : #include <AP_Baro/AP_Baro.h>
31 : #include <AP_Common/time.h>
32 : #include <AP_Compass/AP_Compass.h>
33 : #include <AP_InertialSensor/AP_InertialSensor.h>
34 : #include <GCS_MAVLink/GCS.h>
35 : #include <AP_SerialManager/AP_SerialManager.h>
36 : #include <AP_RTC/AP_RTC.h>
37 : #include <AP_GPS/AP_GPS.h>
38 : #include <AP_Airspeed/AP_Airspeed.h>
39 : #include <AP_Vehicle/AP_Vehicle_Type.h>
40 :
41 : extern const AP_HAL::HAL &hal;
42 :
43 : // constructor
44 0 : AP_ExternalAHRS_SBG::AP_ExternalAHRS_SBG(AP_ExternalAHRS *_frontend,
45 0 : AP_ExternalAHRS::state_t &_state) :
46 0 : AP_ExternalAHRS_backend(_frontend, _state)
47 : {
48 0 : auto &sm = AP::serialmanager();
49 0 : uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
50 0 : if (uart == nullptr) {
51 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "SBG ExternalAHRS no UART");
52 0 : return;
53 : }
54 0 : baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
55 0 : port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
56 :
57 :
58 : // don't offer IMU by default, at 200Hz it is too slow for many aircraft
59 0 : set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) |
60 : uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) |
61 : uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS));
62 :
63 0 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_SBG::update_thread, void), "SBG", 4096, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
64 0 : AP_HAL::panic("SBG Failed to start ExternalAHRS update thread");
65 : }
66 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG ExternalAHRS initialised");
67 : }
68 :
69 0 : void AP_ExternalAHRS_SBG::update_thread()
70 : {
71 0 : hal.scheduler->delay(1000);
72 0 : while (!hal.scheduler->is_system_initialized()) {
73 0 : hal.scheduler->delay(100);
74 : }
75 0 : hal.scheduler->delay(1000);
76 :
77 0 : if (uart == nullptr) {
78 0 : return;
79 : }
80 :
81 0 : uart->begin(baudrate, 1024, 1024);
82 :
83 0 : setup_complete = true;
84 :
85 : while (true) {
86 :
87 0 : if (check_uart()) {
88 : // we've parsed something. There might be more so lets come back quickly
89 0 : hal.scheduler->delay_microseconds(100);
90 0 : continue;
91 : }
92 :
93 : // uart is idle, lets snooze a little more and then do some housekeeping
94 0 : hal.scheduler->delay_microseconds(250);
95 :
96 0 : const uint32_t now_ms = AP_HAL::millis();
97 0 : if (cached.sbg.deviceInfo.firmwareRev == 0 && now_ms - version_check_ms >= 5000) {
98 : // request Device Info every few seconds until we get a response
99 0 : version_check_ms = now_ms;
100 :
101 : // static uint32_t count = 1;
102 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG Requesting DeviceInfo %u", count++);
103 :
104 0 : const sbgMessage msg = sbgMessage(SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INFO);
105 0 : UNUSED_RESULT(send_sbgMessage(uart, msg)); // don't care about any error, just retry at 1Hz
106 :
107 : #if AP_COMPASS_ENABLED
108 0 : } else if (now_ms - send_MagData_ms >= 100) {
109 0 : send_MagData_ms = now_ms;
110 :
111 0 : if (!send_MagData(uart)) {
112 : // TODO: if it fails maybe we should figure out why and retry?
113 : // possible causes:
114 : // 1) uart == nullptr
115 : // 2) msg.len > sizeof(data)
116 : // 3) did not write all the bytes out the uart: zero/fail is treated same as packet_len != bytes_sent
117 0 : if (now_ms - send_mag_error_last_ms >= 5000) {
118 : // throttle the error to no faster than 5Hz because if you get one error you'll likely get a lot
119 0 : send_mag_error_last_ms = now_ms;
120 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: Error sending Mag data");
121 : }
122 : }
123 : #endif // AP_COMPASS_ENABLED
124 :
125 : #if AP_BARO_ENABLED || AP_AIRSPEED_ENABLED
126 0 : } else if (now_ms - send_AirData_ms >= 100) {
127 0 : send_AirData_ms = now_ms;
128 :
129 0 : if (!send_AirData(uart)) {
130 : // TODO: if it fails maybe we should figure out why and retry?
131 : // possible causes:
132 : // 1) uart == nullptr
133 : // 2) msg.len > sizeof(data)
134 : // 3) did not write all the bytes out the uart: zero/fail is treated same as packet_len != bytes_sent
135 0 : if (now_ms - send_air_error_last_ms >= 5000) {
136 : // throttle the error to no faster than 5Hz because if you get one error you'll likely get a lot
137 0 : send_air_error_last_ms = now_ms;
138 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: Error sending Air data");
139 : }
140 : }
141 : #endif // AP_BARO_ENABLED || AP_AIRSPEED_ENABLED
142 : }
143 0 : } // while
144 : }
145 :
146 : // Builds packets by looking at each individual byte, once a full packet has been read in it checks the checksum then handles the packet.
147 : // returns true if any data was found in the UART buffer which was then processed
148 0 : bool AP_ExternalAHRS_SBG::check_uart()
149 : {
150 0 : uint32_t nbytes = MIN(uart->available(), 512u);
151 0 : if (nbytes == 0) {
152 0 : return false;
153 : }
154 0 : while (nbytes--> 0) {
155 : uint8_t b;
156 0 : if (!uart->read(b)) {
157 0 : break;
158 : }
159 :
160 0 : if (parse_byte(b, _inbound_state.msg, _inbound_state)) {
161 0 : handle_msg(_inbound_state.msg);
162 : }
163 : }
164 0 : return true;
165 : }
166 :
167 :
168 0 : bool AP_ExternalAHRS_SBG::send_sbgMessage(AP_HAL::UARTDriver *_uart, const sbgMessage &msg)
169 : {
170 0 : if (_uart == nullptr || msg.len > ARRAY_SIZE(msg.data)) {
171 : // invalid _uart or msg.len is out of range
172 0 : return false;
173 : }
174 :
175 0 : const uint16_t buffer_len = (SBG_PACKET_OVERHEAD + msg.len);
176 0 : uint8_t buffer[buffer_len];
177 :
178 0 : buffer[0] = SBG_PACKET_SYNC1;
179 0 : buffer[1] = SBG_PACKET_SYNC2;
180 0 : buffer[2] = msg.msgid;
181 0 : buffer[3] = msg.msgclass;
182 0 : buffer[4] = msg.len & 0xFF; // LSB first
183 0 : buffer[5] = msg.len >> 8;
184 :
185 0 : for (uint16_t i=0; i<msg.len; i++) {
186 0 : buffer[6+i] = msg.data[i];
187 : }
188 :
189 0 : const uint16_t crc = crc16_ccitt_r((const uint8_t*)&msg, msg.len+4, 0, 0);
190 :
191 0 : buffer[buffer_len-3] = crc & 0xFF; // LSB First
192 0 : buffer[buffer_len-2] = crc >> 8;
193 0 : buffer[buffer_len-1] = SBG_PACKET_ETX;
194 :
195 0 : const uint32_t bytes_sent = _uart->write(buffer, buffer_len);
196 0 : _uart->flush();
197 0 : return (bytes_sent == buffer_len);
198 0 : }
199 :
200 0 : bool AP_ExternalAHRS_SBG::parse_byte(const uint8_t data, sbgMessage &msg, SBG_PACKET_INBOUND_STATE &inbound_state)
201 : {
202 0 : switch (inbound_state.parser) {
203 0 : case SBG_PACKET_PARSE_STATE::SYNC1:
204 0 : inbound_state.parser = (data == SBG_PACKET_SYNC1) ? SBG_PACKET_PARSE_STATE::SYNC2 : SBG_PACKET_PARSE_STATE::SYNC1;
205 0 : break;
206 :
207 0 : case SBG_PACKET_PARSE_STATE::SYNC2:
208 0 : inbound_state.parser = (data == SBG_PACKET_SYNC2) ? SBG_PACKET_PARSE_STATE::MSG : SBG_PACKET_PARSE_STATE::SYNC1;
209 0 : break;
210 :
211 0 : case SBG_PACKET_PARSE_STATE::MSG:
212 0 : msg.msgid = data;
213 0 : inbound_state.parser = SBG_PACKET_PARSE_STATE::CLASS;
214 0 : break;
215 :
216 0 : case SBG_PACKET_PARSE_STATE::CLASS:
217 0 : msg.msgclass = data;
218 0 : inbound_state.parser = SBG_PACKET_PARSE_STATE::LEN1;
219 0 : break;
220 :
221 0 : case SBG_PACKET_PARSE_STATE::LEN1:
222 0 : msg.len = data;
223 0 : inbound_state.parser = SBG_PACKET_PARSE_STATE::LEN2;
224 0 : break;
225 :
226 0 : case SBG_PACKET_PARSE_STATE::LEN2:
227 0 : msg.len |= uint16_t(data) << 8;
228 0 : if (msg.len > sizeof(msg.data)) {
229 : // we can't handle this packet, it's larger than the rx buffer which is larger than the largest packet we care about
230 0 : inbound_state.data_count_skip = msg.len;
231 0 : inbound_state.parser = SBG_PACKET_PARSE_STATE::DROP_THIS_PACKET;
232 : } else {
233 0 : inbound_state.data_count = 0;
234 0 : inbound_state.parser = (msg.len > 0) ? SBG_PACKET_PARSE_STATE::DATA : SBG_PACKET_PARSE_STATE::CRC1;
235 : }
236 0 : break;
237 :
238 0 : case SBG_PACKET_PARSE_STATE::DATA:
239 0 : msg.data[inbound_state.data_count++] = data;
240 0 : if (inbound_state.data_count >= sizeof(msg.data)) {
241 0 : inbound_state.parser = SBG_PACKET_PARSE_STATE::SYNC1;
242 0 : } else if (inbound_state.data_count >= msg.len) {
243 0 : inbound_state.parser = SBG_PACKET_PARSE_STATE::CRC1;
244 : }
245 0 : break;
246 :
247 0 : case SBG_PACKET_PARSE_STATE::CRC1:
248 0 : inbound_state.crc = data;
249 0 : inbound_state.parser = SBG_PACKET_PARSE_STATE::CRC2;
250 0 : break;
251 :
252 0 : case SBG_PACKET_PARSE_STATE::CRC2:
253 0 : inbound_state.crc |= uint16_t(data) << 8;
254 0 : inbound_state.parser = SBG_PACKET_PARSE_STATE::SYNC1; // skip ETX and go directly to SYNC1. Do not pass Go.
255 : {
256 : // CRC field is computed on [MSG(1), CLASS(1), LEN(2), DATA(msg.len)] fields
257 0 : const uint16_t crc = crc16_ccitt_r((const uint8_t*)&msg, msg.len+4, 0, 0);
258 0 : if (crc == inbound_state.crc) {
259 0 : return true;
260 : }
261 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: Failed CRC. Received 0x%04X, Expected 0x%04X", (unsigned)inbound_state.crc, (unsigned)crc);
262 : }
263 0 : break;
264 :
265 0 : case SBG_PACKET_PARSE_STATE::ETX:
266 : // we can just skip this state and let SYNC1 fail once when it gets the ETX byte every time... same amount of work
267 0 : inbound_state.parser = SBG_PACKET_PARSE_STATE::SYNC1;
268 0 : break;
269 :
270 0 : default:
271 : case SBG_PACKET_PARSE_STATE::DROP_THIS_PACKET:
272 : // we're currently parsing a packet that is very large and is not one we care
273 : // about. This is not the packet you're looking for... drop all those bytes
274 0 : if (inbound_state.data_count_skip > 0) {
275 0 : inbound_state.data_count_skip--;
276 : }
277 0 : if (inbound_state.data_count_skip == 0) {
278 0 : inbound_state.parser = SBG_PACKET_PARSE_STATE::SYNC1;
279 : }
280 0 : break;
281 : }
282 :
283 0 : return false;
284 : }
285 :
286 0 : uint16_t AP_ExternalAHRS_SBG::make_gps_week(const SbgEComLogUtc *utc_data)
287 : {
288 0 : const struct tm tm {
289 0 : .tm_sec = utc_data->second,
290 0 : .tm_min = utc_data->minute,
291 0 : .tm_hour = utc_data->hour,
292 0 : .tm_mday = utc_data->day,
293 0 : .tm_mon = utc_data->month - 1,
294 0 : .tm_year = utc_data->year - 1900,
295 0 : };
296 :
297 :
298 : // convert from time structure to unix time
299 0 : const time_t unix_time = ap_mktime(&tm);
300 :
301 : // convert to time since GPS epoch
302 0 : const uint32_t gps_time = unix_time - ((utc_data->gpsTimeOfWeek + UNIX_OFFSET_MSEC) / 1000);
303 :
304 : // get GPS week
305 0 : const uint16_t gps_week = gps_time / AP_SEC_PER_WEEK;
306 :
307 0 : return gps_week;
308 : }
309 :
310 0 : void AP_ExternalAHRS_SBG::handle_msg(const sbgMessage &msg)
311 : {
312 0 : const uint32_t now_ms = AP_HAL::millis();
313 0 : const bool valid_class1_msg = ((SbgEComClass)msg.msgclass == SBG_ECOM_CLASS_LOG_ECOM_1 && (_SbgEComLog1MsgId)msg.msgid == SBG_ECOM_LOG_FAST_IMU_DATA);
314 :
315 0 : if ((SbgEComClass)msg.msgclass == SBG_ECOM_CLASS_LOG_CMD_0) {
316 0 : switch ((SbgEComCmd)msg.msgid) {
317 0 : case SBG_ECOM_CMD_ACK: // 0
318 0 : break;
319 :
320 0 : case SBG_ECOM_CMD_INFO: // 4
321 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.deviceInfo, sizeof(cached.sbg.deviceInfo), msg.data, msg.len);
322 :
323 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: %s", cached.sbg.deviceInfo.productCode);
324 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: %u, %u, %u, %u, %u, %u, %u: %u.%u.%u",
325 : // (unsigned)cached.sbg.deviceInfo.serialNumber,
326 : // (unsigned)cached.sbg.deviceInfo.calibationRev,
327 : // (unsigned)cached.sbg.deviceInfo.calibrationYear,
328 : // (unsigned)cached.sbg.deviceInfo.calibrationMonth,
329 : // (unsigned)cached.sbg.deviceInfo.calibrationDay,
330 : // (unsigned)cached.sbg.deviceInfo.hardwareRev,
331 : // (unsigned)cached.sbg.deviceInfo.firmwareRev,
332 : // (cached.sbg.deviceInfo.firmwareRev >> 22) & 0x3F,
333 : // (cached.sbg.deviceInfo.firmwareRev >> 16) & 0x3F,
334 : // cached.sbg.deviceInfo.firmwareRev & 0xFFFF
335 :
336 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: Serial Number: %u",(unsigned)cached.sbg.deviceInfo.serialNumber);
337 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: Version HW v%u.%u FW v%u.%u.%u",
338 : (unsigned)(cached.sbg.deviceInfo.hardwareRev >> 24) & 0x00FF,
339 : (unsigned)(cached.sbg.deviceInfo.hardwareRev >> 16) & 0x00FF,
340 :
341 : (unsigned) (cached.sbg.deviceInfo.firmwareRev >> 22) & 0x003F,
342 : (unsigned)(cached.sbg.deviceInfo.firmwareRev >> 16) & 0x003F,
343 : (unsigned)cached.sbg.deviceInfo.firmwareRev & 0xFFFF);
344 0 : break;
345 :
346 0 : case SBG_ECOM_CMD_COMPUTE_MAG_CALIB: // 15
347 0 : break;
348 :
349 0 : default:
350 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: Unknown ID=%u, CLASS=%u, LEN=%u", (unsigned)msg.msgid, (unsigned)msg.msgclass, (unsigned)msg.len);
351 0 : break;
352 : }
353 0 : return;
354 : }
355 :
356 :
357 :
358 0 : if (((SbgEComClass)msg.msgclass != SBG_ECOM_CLASS_LOG_ECOM_0) && !valid_class1_msg) {
359 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: Unknown ID=%u, CLASS=%u, LEN=%u", (unsigned)msg.msgid, (unsigned)msg.msgclass, (unsigned)msg.len);
360 0 : return;
361 : }
362 :
363 0 : const bool use_ekf_as_gnss = option_is_set(AP_ExternalAHRS::OPTIONS::SBG_EKF_AS_GNSS);
364 :
365 0 : bool updated_gps = false;
366 0 : bool updated_baro = false;
367 0 : bool updated_ins = false;
368 0 : bool updated_mag = false;
369 0 : bool updated_airspeed = false;
370 :
371 : {
372 0 : WITH_SEMAPHORE(state.sem);
373 :
374 0 : switch (msg.msgid) { // (_SbgEComLog)
375 0 : case SBG_ECOM_LOG_FAST_IMU_DATA: // 0
376 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.imuFastLegacy, sizeof(cached.sbg.imuFastLegacy), msg.data, msg.len);
377 :
378 0 : state.accel = Vector3f(cached.sbg.imuFastLegacy.accelerometers[0], cached.sbg.imuFastLegacy.accelerometers[1], cached.sbg.imuFastLegacy.accelerometers[2]);
379 0 : state.gyro = Vector3f(cached.sbg.imuFastLegacy.gyroscopes[0], cached.sbg.imuFastLegacy.gyroscopes[1], cached.sbg.imuFastLegacy.gyroscopes[2]);
380 0 : updated_ins = true;
381 0 : break;
382 :
383 0 : case SBG_ECOM_LOG_UTC_TIME: // 2
384 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.utc, sizeof(cached.sbg.utc), msg.data, msg.len);
385 :
386 0 : if (sbgEComLogUtcGetClockStatus(cached.sbg.utc.status) != SBG_ECOM_CLOCK_VALID ||
387 0 : sbgEComLogUtcGetClockUtcStatus(cached.sbg.utc.status) != SBG_ECOM_UTC_VALID)
388 : {
389 : // Data is not valid, ignore it.
390 : // This will happen even with a GPS fix = 3. A 3D lock is not enough, a valid
391 : // clock has a higher threshold of quiality needed. It also needs time to sync
392 : // its internal timing and PPS outputs before valid.
393 0 : break;
394 : }
395 :
396 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "------------------");
397 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: %u-%u-%u %02u:%02u:%02u",
398 : // cached.sbg.utc.year,
399 : // (unsigned)cached.sbg.utc.month,
400 : // (unsigned)cached.sbg.utc.day,
401 : // (unsigned)cached.sbg.utc.hour,
402 : // (unsigned)cached.sbg.utc.minute,
403 : // (unsigned)cached.sbg.utc.second);
404 :
405 : #if AP_RTC_ENABLED
406 : {
407 0 : const uint32_t utc_epoch_sec = AP::rtc().date_fields_to_clock_s(
408 0 : cached.sbg.utc.year,
409 0 : cached.sbg.utc.month - 1,
410 0 : cached.sbg.utc.day,
411 0 : cached.sbg.utc.hour,
412 0 : cached.sbg.utc.minute,
413 0 : cached.sbg.utc.second);
414 :
415 0 : const uint64_t utc_epoch_usec = ((uint64_t)utc_epoch_sec) * 1E6 + (cached.sbg.utc.nanoSecond * 1E-3);
416 0 : AP::rtc().set_utc_usec(utc_epoch_usec, AP_RTC::SOURCE_GPS);
417 :
418 : }
419 : #endif // AP_RTC_ENABLED
420 :
421 0 : cached.sensors.gps_data.ms_tow = cached.sbg.utc.gpsTimeOfWeek;
422 0 : cached.sensors.gps_data.gps_week = make_gps_week(&cached.sbg.utc);
423 0 : updated_gps = true;
424 0 : break;
425 :
426 0 : case SBG_ECOM_LOG_IMU_SHORT: // 44
427 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.imuShort, sizeof(cached.sbg.imuShort), msg.data, msg.len);
428 :
429 : {
430 0 : Vector3f temp;
431 : /*!< X, Y, Z delta velocity. Unit is 1048576 LSB for 1 m.s^-2. */
432 0 : temp = Vector3f(cached.sbg.imuShort.deltaVelocity[0], cached.sbg.imuShort.deltaVelocity[1], cached.sbg.imuShort.deltaVelocity[2]);
433 0 : state.accel = temp / SBG_ECOM_LOG_IMU_ACCEL_SCALE_STD;
434 :
435 : /*!< X, Y, Z delta angle. Unit is either 67108864 LSB for 1 rad.s^-1 (standard) or 12304174 LSB for 1 rad.s^-1 (high range), managed automatically. */
436 0 : temp = Vector3f(cached.sbg.imuShort.deltaAngle[0], cached.sbg.imuShort.deltaAngle[1], cached.sbg.imuShort.deltaAngle[2]);
437 0 : const float scaleFactor = (cached.sbg.imuShort.status & SBG_ECOM_IMU_GYROS_USE_HIGH_SCALE) ? SBG_ECOM_LOG_IMU_GYRO_SCALE_HIGH : SBG_ECOM_LOG_IMU_GYRO_SCALE_STD;
438 0 : state.gyro = temp / scaleFactor;
439 : }
440 0 : cached.sensors.ins_data.temperature = (cached.sbg.imuShort.temperature / SBG_ECOM_LOG_IMU_TEMP_SCALE_STD);
441 0 : updated_ins = true;
442 0 : break;
443 :
444 0 : case SBG_ECOM_LOG_IMU_DATA: // 3
445 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.imuLegacy, sizeof(cached.sbg.imuLegacy), msg.data, msg.len);
446 :
447 0 : state.accel = Vector3f(cached.sbg.imuLegacy.accelerometers[0], cached.sbg.imuLegacy.accelerometers[1], cached.sbg.imuLegacy.accelerometers[2]);
448 0 : state.gyro = Vector3f(cached.sbg.imuLegacy.gyroscopes[0], cached.sbg.imuLegacy.gyroscopes[1], cached.sbg.imuLegacy.gyroscopes[2]);
449 0 : cached.sensors.ins_data.temperature = cached.sbg.imuLegacy.temperature;
450 0 : updated_ins = true;
451 0 : break;
452 :
453 0 : case SBG_ECOM_LOG_MAG: // 4
454 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.mag, sizeof(cached.sbg.mag), msg.data, msg.len);
455 :
456 0 : state.accel = Vector3f(cached.sbg.mag.accelerometers[0], cached.sbg.mag.accelerometers[1], cached.sbg.mag.accelerometers[2]);
457 0 : updated_ins = true;
458 :
459 0 : cached.sensors.mag_data.field = Vector3f(cached.sbg.mag.magnetometers[0], cached.sbg.mag.magnetometers[1], cached.sbg.mag.magnetometers[2]);
460 0 : updated_mag = true;
461 0 : break;
462 :
463 0 : case SBG_ECOM_LOG_EKF_EULER: // 6
464 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.ekfEuler, sizeof(cached.sbg.ekfEuler), msg.data, msg.len);
465 :
466 : // float euler[3]; /*!< Roll, Pitch and Yaw angles in rad. */
467 : // float eulerStdDev[3]; /*!< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad. */
468 0 : state.quat.from_euler(cached.sbg.ekfEuler.euler[0], cached.sbg.ekfEuler.euler[1], cached.sbg.ekfEuler.euler[2]);
469 0 : state.have_quaternion = true;
470 0 : break;
471 :
472 0 : case SBG_ECOM_LOG_EKF_QUAT: // 7
473 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.ekfQuat, sizeof(cached.sbg.ekfQuat), msg.data, msg.len);
474 :
475 : // float quaternion[4]; /*!< Orientation quaternion stored in W, X, Y, Z form. */
476 : // float eulerStdDev[3]; /*!< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad. */
477 0 : memcpy(&state.quat, cached.sbg.ekfQuat.quaternion, sizeof(state.quat));
478 0 : state.have_quaternion = true;
479 0 : break;
480 :
481 0 : case SBG_ECOM_LOG_EKF_NAV: // 8
482 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.ekfNav, sizeof(cached.sbg.ekfNav), msg.data, msg.len);
483 :
484 0 : state.velocity = Vector3f(cached.sbg.ekfNav.velocity[0], cached.sbg.ekfNav.velocity[1], cached.sbg.ekfNav.velocity[2]);
485 0 : state.have_velocity = true;
486 :
487 0 : state.location = Location(cached.sbg.ekfNav.position[0]*1e7, cached.sbg.ekfNav.position[1]*1e7, cached.sbg.ekfNav.position[2]*1e2, Location::AltFrame::ABSOLUTE);
488 0 : state.last_location_update_us = AP_HAL::micros();
489 :
490 0 : ekf_is_full_nav = SbgEkfStatus_is_fullNav(cached.sbg.ekfNav.status);
491 :
492 0 : if (!state.have_location && ekf_is_full_nav) {
493 0 : state.have_location = true;
494 0 : } else if (!state.have_origin && cached.sensors.gps_data.fix_type >= AP_GPS_FixType::FIX_3D && ekf_is_full_nav) {
495 : // this is in an else so that origin doesn't get set on the very very first sample, do it on the second one just to give us a tiny bit more chance of a better origin
496 0 : state.origin = state.location;
497 0 : state.have_origin = true;
498 : }
499 :
500 0 : if (ekf_is_full_nav && use_ekf_as_gnss) {
501 0 : cached.sensors.gps_data.latitude = cached.sbg.ekfNav.position[0] * 1E7;
502 0 : cached.sensors.gps_data.longitude = cached.sbg.ekfNav.position[1] * 1E7;
503 0 : cached.sensors.gps_data.msl_altitude = cached.sbg.ekfNav.position[2] * 100;
504 :
505 0 : cached.sensors.gps_data.horizontal_pos_accuracy = Vector2f(cached.sbg.ekfNav.positionStdDev[0], cached.sbg.ekfNav.positionStdDev[1]).length();
506 0 : cached.sensors.gps_data.hdop = cached.sensors.gps_data.horizontal_pos_accuracy;
507 0 : cached.sensors.gps_data.vertical_pos_accuracy = cached.sbg.ekfNav.positionStdDev[2];
508 0 : cached.sensors.gps_data.vdop = cached.sensors.gps_data.vertical_pos_accuracy;
509 :
510 0 : cached.sensors.gps_data.fix_type = AP_GPS_FixType::FIX_3D;
511 :
512 0 : cached.sensors.gps_data.ned_vel_north = cached.sbg.ekfNav.velocity[0];
513 0 : cached.sensors.gps_data.ned_vel_east = cached.sbg.ekfNav.velocity[1];
514 0 : cached.sensors.gps_data.ned_vel_down = cached.sbg.ekfNav.velocity[2];
515 0 : cached.sensors.gps_data.horizontal_vel_accuracy = Vector2f(cached.sbg.ekfNav.velocityStdDev[0], cached.sbg.ekfNav.velocityStdDev[1]).length();
516 :
517 0 : updated_gps = true;
518 : }
519 0 : break;
520 :
521 0 : case SBG_ECOM_LOG_GPS1_VEL: // 13
522 : case SBG_ECOM_LOG_GPS2_VEL: // 16
523 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.gnssVel, sizeof(cached.sbg.gnssVel), msg.data, msg.len);
524 :
525 0 : if ((!use_ekf_as_gnss) || (use_ekf_as_gnss && !ekf_is_full_nav)) {
526 0 : cached.sensors.gps_data.ms_tow = cached.sbg.gnssVel.timeOfWeek;
527 0 : cached.sensors.gps_data.ned_vel_north = cached.sbg.gnssVel.velocity[0];
528 0 : cached.sensors.gps_data.ned_vel_east = cached.sbg.gnssVel.velocity[1];
529 0 : cached.sensors.gps_data.ned_vel_down = cached.sbg.gnssVel.velocity[2];
530 0 : cached.sensors.gps_data.horizontal_vel_accuracy = Vector2f(cached.sbg.gnssVel.velocityAcc[0], cached.sbg.gnssVel.velocityAcc[1]).length();
531 : // unused - cached.sbg.gnssVel.course
532 : // unused - cached.sbg.gnssVel.courseAcc
533 0 : updated_gps = true;
534 : }
535 0 : break;
536 :
537 0 : case SBG_ECOM_LOG_GPS1_POS: // 14
538 : case SBG_ECOM_LOG_GPS2_POS: // 17
539 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.gnssPos, sizeof(cached.sbg.gnssPos), msg.data, msg.len);
540 :
541 0 : if ((!use_ekf_as_gnss) || (use_ekf_as_gnss && !ekf_is_full_nav)) {
542 0 : cached.sensors.gps_data.ms_tow = cached.sbg.gnssPos.timeOfWeek;
543 0 : cached.sensors.gps_data.latitude = cached.sbg.gnssPos.latitude * 1E7;
544 0 : cached.sensors.gps_data.longitude = cached.sbg.gnssPos.longitude * 1E7;
545 0 : cached.sensors.gps_data.msl_altitude = cached.sbg.gnssPos.altitude * 100;
546 : // unused - cached.sbg.gnssPos.undulation
547 0 : cached.sensors.gps_data.horizontal_pos_accuracy = Vector2f(cached.sbg.gnssPos.latitudeAccuracy, cached.sbg.gnssPos.longitudeAccuracy).length();
548 0 : cached.sensors.gps_data.hdop = cached.sensors.gps_data.horizontal_pos_accuracy;
549 0 : cached.sensors.gps_data.vertical_pos_accuracy = cached.sbg.gnssPos.altitudeAccuracy;
550 0 : cached.sensors.gps_data.vdop = cached.sensors.gps_data.vertical_pos_accuracy;
551 0 : cached.sensors.gps_data.satellites_in_view = cached.sbg.gnssPos.numSvUsed;
552 : // unused - cached.sbg.gnssPos.baseStationId
553 : // unused - cached.sbg.gnssPos.differentialAge
554 0 : cached.sensors.gps_data.fix_type = SbgGpsPosStatus_to_GpsFixType(cached.sbg.gnssPos.status);
555 0 : updated_gps = true;
556 : }
557 0 : break;
558 :
559 0 : case SBG_ECOM_LOG_AIR_DATA: // 36
560 0 : safe_copy_msg_to_object((uint8_t*)&cached.sbg.airData, sizeof(cached.sbg.airData), msg.data, msg.len);
561 :
562 0 : if (cached.sbg.airData.status & SBG_ECOM_AIR_DATA_PRESSURE_ABS_VALID) {
563 0 : cached.sensors.baro_data.pressure_pa = cached.sbg.airData.pressureAbs;
564 0 : updated_baro = true;
565 : }
566 0 : if (cached.sbg.airData.status & SBG_ECOM_AIR_DATA_ALTITUDE_VALID) {
567 0 : cached.sensors.baro_height = cached.sbg.airData.altitude;
568 0 : updated_baro = true;
569 : }
570 0 : if (cached.sbg.airData.status & SBG_ECOM_AIR_DATA_PRESSURE_DIFF_VALID) {
571 0 : cached.sensors.airspeed_data.differential_pressure = cached.sbg.airData.pressureDiff;
572 0 : updated_airspeed = true;
573 : }
574 : // if (cached.sbg.airData.status & SBG_ECOM_AIR_DATA_AIRPSEED_VALID) {
575 : // // we don't accept airspeed directly, we compute it ourselves in AP_Airspeed via diff pressure
576 : // }
577 :
578 0 : if ((cached.sbg.airData.status & SBG_ECOM_AIR_DATA_TEMPERATURE_VALID) && (updated_baro || updated_airspeed)) {
579 0 : cached.sensors.airspeed_data.temperature = cached.sbg.airData.airTemperature;
580 0 : cached.sensors.baro_data.temperature = cached.sbg.airData.airTemperature;
581 : }
582 0 : break;
583 :
584 0 : default:
585 : // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBG: unhandled ID=%u, CLASS=%u, LEN=%u", (unsigned)msg.msgid, (unsigned)msg.msgclass, (unsigned)msg.len);
586 0 : return;
587 : } // switch msgid
588 0 : } // semaphore
589 :
590 : #if AP_GPS_ENABLED
591 0 : if (updated_gps) {
592 0 : cached.sensors.gps_ms = now_ms;
593 : uint8_t instance;
594 0 : if (AP::gps().get_first_external_instance(instance)) {
595 0 : AP::gps().handle_external(cached.sensors.gps_data, instance);
596 : }
597 : }
598 : #else
599 : (void)updated_gps;
600 : #endif
601 :
602 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
603 0 : if (updated_mag) {
604 0 : cached.sensors.mag_ms = now_ms;
605 0 : AP::compass().handle_external(cached.sensors.mag_data);
606 : }
607 : #else
608 : (void)updated_mag;
609 : #endif
610 :
611 : #if AP_BARO_EXTERNALAHRS_ENABLED
612 0 : if (updated_baro) {
613 0 : cached.sensors.baro_ms = now_ms;
614 0 : cached.sensors.baro_data.instance = 0;
615 0 : AP::baro().handle_external(cached.sensors.baro_data);
616 : }
617 : #else
618 : (void)updated_baro;
619 : #endif
620 :
621 : #if AP_AIRSPEED_EXTERNAL_ENABLED && (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane))
622 0 : if (updated_airspeed && AP::airspeed() != nullptr) {
623 0 : cached.sensors.airspeed_ms = now_ms;
624 0 : AP::airspeed()->handle_external(cached.sensors.airspeed_data);
625 : }
626 : #else
627 : (void)updated_airspeed;
628 : #endif
629 :
630 :
631 0 : if (updated_ins) {
632 0 : cached.sensors.ins_data.accel = state.accel;
633 0 : cached.sensors.ins_data.gyro = state.gyro;
634 0 : cached.sensors.ins_ms = now_ms;
635 0 : AP::ins().handle_external(cached.sensors.ins_data);
636 : }
637 :
638 0 : last_received_ms = now_ms;
639 : }
640 :
641 0 : void AP_ExternalAHRS_SBG::safe_copy_msg_to_object(uint8_t* dest, const uint16_t dest_len, const uint8_t* src, const uint16_t src_len)
642 : {
643 : // To allow for future changes of the SBG protocol the protocol allows extending messages
644 : // which can be detected by a length mismatch, usually longer. To allow for this you assume unused data is zero
645 : // but instead of zeroing the packet every time before populating it it's best to only zero when necessary.
646 0 : if (dest_len != src_len) {
647 0 : memset(dest, 0, dest_len);
648 : }
649 0 : memcpy(dest, src, MIN(dest_len,src_len));
650 0 : }
651 :
652 0 : bool AP_ExternalAHRS_SBG::SbgEkfStatus_is_fullNav(const uint32_t ekfStatus)
653 : {
654 0 : SbgEComSolutionMode solutionMode = (SbgEComSolutionMode)(ekfStatus & SBG_ECOM_LOG_EKF_SOLUTION_MODE_MASK);
655 :
656 0 : return (solutionMode == SBG_ECOM_SOL_MODE_NAV_POSITION);
657 : }
658 :
659 0 : AP_GPS_FixType AP_ExternalAHRS_SBG::SbgGpsPosStatus_to_GpsFixType(const uint32_t gpsPosStatus)
660 : {
661 0 : const uint32_t fix = (gpsPosStatus >> SBG_ECOM_GPS_POS_TYPE_SHIFT) & SBG_ECOM_GPS_POS_TYPE_MASK;
662 0 : switch ((SbgEComGpsPosType)fix) {
663 0 : case SBG_ECOM_POS_NO_SOLUTION: /*!< No valid solution available. */
664 : case SBG_ECOM_POS_UNKNOWN_TYPE: /*!< An unknown solution type has been computed. */
665 0 : return AP_GPS_FixType::NONE;
666 :
667 0 : case SBG_ECOM_POS_SINGLE: /*!< Single point solution position. */
668 : case SBG_ECOM_POS_FIXED: /*!< Fixed location solution position. */
669 0 : return AP_GPS_FixType::FIX_3D;
670 :
671 0 : case SBG_ECOM_POS_PSRDIFF: /*!< Standard Pseudorange Differential Solution (DGPS). */
672 : case SBG_ECOM_POS_SBAS: /*!< SBAS satellite used for differential corrections. */
673 0 : return AP_GPS_FixType::DGPS;
674 :
675 0 : case SBG_ECOM_POS_RTK_FLOAT: /*!< Floating RTK ambiguity solution (20 cms RTK). */
676 : case SBG_ECOM_POS_PPP_FLOAT: /*!< Precise Point Positioning with float ambiguities. */
677 : case SBG_ECOM_POS_OMNISTAR: /*!< Omnistar VBS Position (L1 sub-meter). */
678 0 : return AP_GPS_FixType::RTK_FLOAT;
679 :
680 0 : case SBG_ECOM_POS_RTK_INT: /*!< Integer RTK ambiguity solution (2 cms RTK). */
681 : case SBG_ECOM_POS_PPP_INT: /*!< Precise Point Positioning with fixed ambiguities. */
682 0 : return AP_GPS_FixType::RTK_FIXED;
683 : }
684 0 : return AP_GPS_FixType::NONE;
685 : }
686 :
687 0 : void AP_ExternalAHRS_SBG::get_filter_status(nav_filter_status &status) const
688 : {
689 0 : WITH_SEMAPHORE(state.sem);
690 0 : memset(&status, 0, sizeof(status));
691 :
692 0 : if (cached.sensors.ins_ms != 0 && cached.sensors.gps_ms != 0) {
693 0 : status.flags.initalized = true;
694 : }
695 0 : if (!healthy()) {
696 0 : return;
697 : }
698 :
699 0 : if (state.have_location) {
700 0 : status.flags.vert_pos = true;
701 0 : status.flags.horiz_pos_rel = true;
702 0 : status.flags.horiz_pos_abs = true;
703 0 : status.flags.pred_horiz_pos_rel = true;
704 0 : status.flags.pred_horiz_pos_abs = true;
705 0 : status.flags.using_gps = true;
706 : }
707 :
708 0 : if (state.have_quaternion) {
709 0 : status.flags.attitude = true;
710 : }
711 :
712 0 : if (state.have_velocity) {
713 0 : status.flags.vert_vel = true;
714 0 : status.flags.horiz_vel = true;
715 : }
716 0 : }
717 :
718 0 : bool AP_ExternalAHRS_SBG::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
719 : {
720 0 : if (!setup_complete) {
721 0 : hal.util->snprintf(failure_msg, failure_msg_len, "SBG setup failed");
722 0 : return false;
723 : }
724 0 : if (!healthy()) {
725 0 : hal.util->snprintf(failure_msg, failure_msg_len, "SBG unhealthy");
726 0 : return false;
727 : }
728 0 : return true;
729 : }
730 :
731 : #if AP_COMPASS_ENABLED
732 0 : bool AP_ExternalAHRS_SBG::send_MagData(AP_HAL::UARTDriver *_uart)
733 : {
734 0 : SbgEComLogMag mag_data_log {};
735 0 : mag_data_log.timeStamp = AP_HAL::micros();
736 :
737 0 : const auto &compass = AP::compass();
738 0 : if (compass.available() || compass.healthy()) {
739 : // TODO: consider also checking compass.last_update_usec() to only send when we have new data
740 :
741 0 : const Vector3f mag_field = compass.get_field();
742 0 : mag_data_log.magnetometers[0] = mag_field[0];
743 0 : mag_data_log.magnetometers[1] = mag_field[1];
744 0 : mag_data_log.magnetometers[2] = mag_field[2];
745 :
746 0 : mag_data_log.status |= (SBG_ECOM_MAG_MAG_X_BIT | SBG_ECOM_MAG_MAG_Y_BIT | SBG_ECOM_MAG_MAG_Z_BIT | SBG_ECOM_MAG_MAGS_IN_RANGE | SBG_ECOM_MAG_CALIBRATION_OK);
747 : }
748 :
749 0 : const sbgMessage msg = sbgMessage(SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_MAG, (uint8_t*)&mag_data_log, sizeof(mag_data_log));
750 0 : return send_sbgMessage(_uart, msg);
751 : }
752 : #endif // AP_COMPASS_ENABLED
753 :
754 0 : bool AP_ExternalAHRS_SBG::send_AirData(AP_HAL::UARTDriver *_uart)
755 : {
756 0 : SbgEComLogAirData air_data_log {};
757 0 : air_data_log.timeStamp = 0;
758 0 : air_data_log.status |= SBG_ECOM_AIR_DATA_TIME_IS_DELAY;
759 :
760 : #if AP_BARO_ENABLED
761 0 : const auto &baro = AP::baro();
762 0 : if (baro.healthy()) {
763 0 : air_data_log.pressureAbs = baro.get_pressure();
764 0 : air_data_log.altitude = baro.get_altitude();
765 0 : air_data_log.airTemperature = baro.get_temperature();
766 :
767 0 : air_data_log.status |= (SBG_ECOM_AIR_DATA_PRESSURE_ABS_VALID | SBG_ECOM_AIR_DATA_ALTITUDE_VALID | SBG_ECOM_AIR_DATA_TEMPERATURE_VALID);
768 : }
769 : #endif // AP_BARO_ENABLED
770 :
771 : #if AP_AIRSPEED_ENABLED
772 0 : auto *airspeed = AP::airspeed();
773 0 : if (airspeed != nullptr && airspeed->healthy()) {
774 : float airTemperature;
775 0 : if (airspeed->get_temperature(airTemperature)) {
776 0 : air_data_log.airTemperature = airTemperature;
777 0 : air_data_log.status |= SBG_ECOM_AIR_DATA_TEMPERATURE_VALID;
778 : }
779 :
780 0 : air_data_log.pressureDiff = airspeed->get_differential_pressure();
781 0 : air_data_log.trueAirspeed = airspeed->get_airspeed();
782 0 : air_data_log.status |= (SBG_ECOM_AIR_DATA_PRESSURE_DIFF_VALID | SBG_ECOM_AIR_DATA_AIRPSEED_VALID);
783 : }
784 : #endif // AP_AIRSPEED_ENABLED
785 :
786 0 : const sbgMessage msg = sbgMessage(SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_AIR_DATA, (uint8_t*)&air_data_log, sizeof(air_data_log));
787 0 : return send_sbgMessage(_uart, msg);
788 : }
789 :
790 : // get variances
791 0 : bool AP_ExternalAHRS_SBG::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
792 : {
793 0 : velVar = cached.sensors.gps_data.horizontal_vel_accuracy * vel_gate_scale;
794 0 : posVar = cached.sensors.gps_data.horizontal_pos_accuracy * pos_gate_scale;
795 0 : hgtVar = cached.sensors.gps_data.vertical_pos_accuracy * hgt_gate_scale;
796 0 : magVar.zero(); // Not provided, set to 0.
797 0 : tasVar = 0;
798 0 : return true;
799 : }
800 :
801 : #endif // AP_EXTERNAL_AHRS_SBG_ENABLED
802 :
|
