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 InertialLabs INS
17 : */
18 :
19 : #include "AP_ExternalAHRS_config.h"
20 :
21 : #if AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
22 :
23 : #include "AP_ExternalAHRS_InertialLabs.h"
24 : #include <AP_Math/AP_Math.h>
25 : #include <AP_Math/crc.h>
26 : #include <AP_Baro/AP_Baro.h>
27 : #include <AP_Compass/AP_Compass.h>
28 : #include <AP_GPS/AP_GPS.h>
29 : #include <AP_Airspeed/AP_Airspeed.h>
30 : #include <AP_InertialSensor/AP_InertialSensor.h>
31 : #include <GCS_MAVLink/GCS.h>
32 : #include <AP_Logger/AP_Logger.h>
33 : #include <AP_SerialManager/AP_SerialManager.h>
34 : #include <AP_HAL/utility/sparse-endian.h>
35 : #include <AP_Common/Bitmask.h>
36 : #include <AP_Vehicle/AP_Vehicle_Type.h>
37 :
38 : extern const AP_HAL::HAL &hal;
39 :
40 : // unit status bits
41 : #define ILABS_UNIT_STATUS_ALIGNMENT_FAIL 0x0001
42 : #define ILABS_UNIT_STATUS_OPERATION_FAIL 0x0002
43 : #define ILABS_UNIT_STATUS_GYRO_FAIL 0x0004
44 : #define ILABS_UNIT_STATUS_ACCEL_FAIL 0x0008
45 : #define ILABS_UNIT_STATUS_MAG_FAIL 0x0010
46 : #define ILABS_UNIT_STATUS_ELECTRONICS_FAIL 0x0020
47 : #define ILABS_UNIT_STATUS_GNSS_FAIL 0x0040
48 : #define ILABS_UNIT_STATUS_RUNTIME_CAL 0x0080
49 : #define ILABS_UNIT_STATUS_VOLTAGE_LOW 0x0100
50 : #define ILABS_UNIT_STATUS_VOLTAGE_HIGH 0x0200
51 : #define ILABS_UNIT_STATUS_X_RATE_HIGH 0x0400
52 : #define ILABS_UNIT_STATUS_Y_RATE_HIGH 0x0800
53 : #define ILABS_UNIT_STATUS_Z_RATE_HIGH 0x1000
54 : #define ILABS_UNIT_STATUS_MAG_FIELD_HIGH 0x2000
55 : #define ILABS_UNIT_STATUS_TEMP_RANGE_ERR 0x4000
56 : #define ILABS_UNIT_STATUS_RUNTIME_CAL2 0x8000
57 :
58 : // unit status2 bits
59 : #define ILABS_UNIT_STATUS2_ACCEL_X_HIGH 0x0001
60 : #define ILABS_UNIT_STATUS2_ACCEL_Y_HIGH 0x0002
61 : #define ILABS_UNIT_STATUS2_ACCEL_Z_HIGH 0x0004
62 : #define ILABS_UNIT_STATUS2_BARO_FAIL 0x0008
63 : #define ILABS_UNIT_STATUS2_DIFF_PRESS_FAIL 0x0010
64 : #define ILABS_UNIT_STATUS2_MAGCAL_2D_ACT 0x0020
65 : #define ILABS_UNIT_STATUS2_MAGCAL_3D_ACT 0x0040
66 : #define ILABS_UNIT_STATUS2_GNSS_FUSION_OFF 0x0080
67 : #define ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF 0x0100
68 : #define ILABS_UNIT_STATUS2_MAG_FUSION_OFF 0x0200
69 : #define ILABS_UNIT_STATUS2_GNSS_POS_VALID 0x0400
70 :
71 : // air data status bits
72 : #define ILABS_AIRDATA_INIT_FAIL 0x0001
73 : #define ILABS_AIRDATA_DIFF_PRESS_INIT_FAIL 0x0002
74 : #define ILABS_AIRDATA_STATIC_PRESS_FAIL 0x0004
75 : #define ILABS_AIRDATA_DIFF_PRESS_FAIL 0x0008
76 : #define ILABS_AIRDATA_STATIC_PRESS_RANGE_ERR 0x0010
77 : #define ILABS_AIRDATA_DIFF_PRESS_RANGE_ERR 0x0020
78 : #define ILABS_AIRDATA_PRESS_ALT_FAIL 0x0100
79 : #define ILABS_AIRDATA_AIRSPEED_FAIL 0x0200
80 : #define ILABS_AIRDATA_BELOW_THRESHOLD 0x0400
81 :
82 :
83 : // constructor
84 11 : AP_ExternalAHRS_InertialLabs::AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *_frontend,
85 11 : AP_ExternalAHRS::state_t &_state) :
86 11 : AP_ExternalAHRS_backend(_frontend, _state)
87 : {
88 11 : auto &sm = AP::serialmanager();
89 11 : uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
90 11 : if (!uart) {
91 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "InertialLabs ExternalAHRS no UART");
92 0 : return;
93 : }
94 11 : baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
95 11 : port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
96 :
97 : // don't offer IMU by default, at 200Hz it is too slow for many aircraft
98 11 : set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) |
99 : uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) |
100 : uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS));
101 :
102 11 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_InertialLabs::update_thread, void), "ILabs", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
103 0 : AP_HAL::panic("InertialLabs Failed to start ExternalAHRS update thread");
104 : }
105 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs ExternalAHRS initialised");
106 : }
107 :
108 : /*
109 : re-sync buffer on parse failure
110 : */
111 0 : void AP_ExternalAHRS_InertialLabs::re_sync(void)
112 : {
113 0 : if (buffer_ofs > 3) {
114 : /*
115 : look for the 2 byte header and try to sync to that
116 : */
117 0 : const uint16_t header = 0x55AA;
118 0 : const uint8_t *p = (const uint8_t *)memmem(&buffer[1], buffer_ofs-3, &header, sizeof(header));
119 0 : if (p != nullptr) {
120 0 : const uint16_t n = p - &buffer[0];
121 0 : memmove(&buffer[0], p, buffer_ofs - n);
122 0 : buffer_ofs -= n;
123 : } else {
124 0 : buffer_ofs = 0;
125 : }
126 : } else {
127 0 : buffer_ofs = 0;
128 : }
129 0 : }
130 :
131 : // macro for checking we don't run past end of message buffer
132 : #define CHECK_SIZE(d) need_re_sync = (message_ofs + (msg_len=sizeof(d)) > buffer_end); if (need_re_sync) break
133 :
134 : // lookup a message in the msg_types bitmask to see if we received it in this packet
135 : #define GOT_MSG(mtype) msg_types.get(unsigned(MessageType::mtype))
136 :
137 : /*
138 : check header is valid
139 : */
140 35334 : bool AP_ExternalAHRS_InertialLabs::check_header(const ILabsHeader *h) const
141 : {
142 70668 : return h->magic == 0x55AA &&
143 35334 : h->msg_type == 1 &&
144 106002 : h->msg_id == 0x95 &&
145 70668 : h->msg_len <= sizeof(buffer)-2;
146 : }
147 :
148 : /*
149 : check the UART for more data
150 : returns true if we have consumed potentially valid bytes
151 : */
152 85088 : bool AP_ExternalAHRS_InertialLabs::check_uart()
153 : {
154 85088 : WITH_SEMAPHORE(state.sem);
155 :
156 85089 : if (!setup_complete) {
157 0 : return false;
158 : }
159 : // ensure we own the uart
160 85089 : uart->begin(0);
161 85089 : uint32_t n = uart->available();
162 85089 : if (n == 0) {
163 49755 : return false;
164 : }
165 35334 : if (n + buffer_ofs > sizeof(buffer)) {
166 265 : n = sizeof(buffer) - buffer_ofs;
167 : }
168 35334 : const ILabsHeader *h = (ILabsHeader *)&buffer[0];
169 :
170 35334 : if (buffer_ofs < sizeof(ILabsHeader)) {
171 35334 : n = MIN(n, sizeof(ILabsHeader)-buffer_ofs);
172 : } else {
173 0 : if (!check_header(h)) {
174 0 : re_sync();
175 0 : return false;
176 : }
177 0 : if (buffer_ofs > h->msg_len+8) {
178 0 : re_sync();
179 0 : return false;
180 : }
181 0 : n = MIN(n, uint32_t(h->msg_len + 2 - buffer_ofs));
182 : }
183 :
184 35334 : const ssize_t nread = uart->read(&buffer[buffer_ofs], n);
185 35334 : if (nread != ssize_t(n)) {
186 0 : re_sync();
187 0 : return false;
188 : }
189 35334 : buffer_ofs += n;
190 :
191 35334 : if (buffer_ofs < sizeof(ILabsHeader)) {
192 0 : return true;
193 : }
194 :
195 35334 : if (!check_header(h)) {
196 0 : re_sync();
197 0 : return false;
198 : }
199 :
200 35334 : if (buffer_ofs < h->msg_len+2) {
201 : /*
202 : see if we can read the rest immediately
203 : */
204 35334 : const uint16_t needed = h->msg_len+2 - buffer_ofs;
205 35334 : if (uart->available() < needed) {
206 : // need more data
207 0 : return true;
208 : }
209 35334 : const ssize_t nread2 = uart->read(&buffer[buffer_ofs], needed);
210 35334 : if (nread2 != needed) {
211 0 : re_sync();
212 0 : return false;
213 : }
214 35334 : buffer_ofs += nread2;
215 : }
216 :
217 : // check checksum
218 35334 : const uint16_t crc1 = crc_sum_of_bytes_16(&buffer[2], buffer_ofs-4);
219 35334 : const uint16_t crc2 = le16toh_ptr(&buffer[buffer_ofs-2]);
220 35334 : if (crc1 != crc2) {
221 0 : re_sync();
222 0 : return false;
223 : }
224 :
225 35334 : const uint8_t *buffer_end = &buffer[buffer_ofs];
226 35334 : const uint16_t payload_size = h->msg_len - 6;
227 35334 : const uint8_t *payload = &buffer[6];
228 35334 : if (payload_size < 3) {
229 0 : re_sync();
230 0 : return false;
231 : }
232 35334 : const uint8_t num_messages = payload[0];
233 35334 : if (num_messages == 0 ||
234 35334 : num_messages > payload_size-1) {
235 0 : re_sync();
236 0 : return false;
237 : }
238 35334 : const uint8_t *message_ofs = &payload[num_messages+1];
239 35334 : bool need_re_sync = false;
240 :
241 : // bitmask for what types we get
242 35334 : Bitmask<256> msg_types;
243 35334 : uint32_t now_ms = AP_HAL::millis();
244 :
245 1166022 : for (uint8_t i=0; i<num_messages; i++) {
246 1130688 : if (message_ofs >= buffer_end) {
247 0 : re_sync();
248 0 : return false;
249 : }
250 1130688 : MessageType mtype = (MessageType)payload[1+i];
251 1130688 : ILabsData &u = *(ILabsData*)message_ofs;
252 1130688 : uint8_t msg_len = 0;
253 :
254 1130688 : msg_types.set(unsigned(mtype));
255 :
256 1130688 : switch (mtype) {
257 35334 : case MessageType::GPS_INS_TIME_MS: {
258 : // this is the GPS tow timestamp in ms for when the IMU data was sampled
259 35334 : CHECK_SIZE(u.gnss_time_ms);
260 35334 : gps_data.ms_tow = u.gnss_time_ms;
261 35334 : break;
262 : }
263 35334 : case MessageType::GPS_WEEK: {
264 35334 : CHECK_SIZE(u.gnss_week);
265 35334 : gps_data.gps_week = u.gnss_week;
266 35334 : break;
267 : }
268 35334 : case MessageType::ACCEL_DATA_HR: {
269 35334 : CHECK_SIZE(u.accel_data_hr);
270 : // should use 9.8106 instead of GRAVITY_MSS-constant in accordance with the device-documentation
271 35334 : ins_data.accel = u.accel_data_hr.tofloat().rfu_to_frd()*9.8106f*1.0e-6; // m/s^2
272 35334 : break;
273 : }
274 35334 : case MessageType::GYRO_DATA_HR: {
275 35334 : CHECK_SIZE(u.gyro_data_hr);
276 35334 : ins_data.gyro = u.gyro_data_hr.tofloat().rfu_to_frd()*DEG_TO_RAD*1.0e-5; // rad/s
277 35334 : break;
278 : }
279 35334 : case MessageType::BARO_DATA: {
280 35334 : CHECK_SIZE(u.baro_data);
281 35334 : baro_data.pressure_pa = u.baro_data.pressure_pa2*2; // Pa
282 35334 : state2.baro_alt = u.baro_data.baro_alt*0.01; // m
283 35334 : break;
284 : }
285 35334 : case MessageType::MAG_DATA: {
286 35334 : CHECK_SIZE(u.mag_data);
287 35334 : mag_data.field = u.mag_data.tofloat().rfu_to_frd()*(10*NTESLA_TO_MGAUSS); // milligauss
288 35334 : break;
289 : }
290 35334 : case MessageType::ORIENTATION_ANGLES: {
291 35334 : CHECK_SIZE(u.orientation_angles);
292 35334 : state.quat.from_euler(radians(u.orientation_angles.roll*0.01),
293 35334 : radians(u.orientation_angles.pitch*0.01),
294 35334 : radians(u.orientation_angles.yaw*0.01));
295 35334 : state.have_quaternion = true;
296 35334 : if (last_att_ms == 0) {
297 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got link");
298 : }
299 35334 : last_att_ms = now_ms;
300 35334 : break;
301 : }
302 35334 : case MessageType::VELOCITIES: {
303 35334 : CHECK_SIZE(u.velocity);
304 35334 : state.velocity = u.velocity.tofloat().rfu_to_frd()*0.01;
305 35334 : gps_data.ned_vel_north = state.velocity.x; // m/s
306 35334 : gps_data.ned_vel_east = state.velocity.y; // m/s
307 35334 : gps_data.ned_vel_down = state.velocity.z; // m/s
308 35334 : state.have_velocity = true;
309 35334 : last_vel_ms = now_ms;
310 35334 : break;
311 : }
312 35334 : case MessageType::POSITION: {
313 35334 : CHECK_SIZE(u.position);
314 35334 : state.location.lat = u.position.lat; // deg*1.0e7
315 35334 : state.location.lng = u.position.lon; // deg*1.0e7
316 35334 : state.location.alt = u.position.alt; // m*100
317 :
318 35334 : gps_data.latitude = u.position.lat; // deg*1.0e7
319 35334 : gps_data.longitude = u.position.lon; // deg*1.0e7
320 35334 : gps_data.msl_altitude = u.position.alt; // m*100
321 :
322 35334 : state.have_location = true;
323 35334 : state.last_location_update_us = AP_HAL::micros();
324 35334 : last_pos_ms = now_ms;
325 35334 : break;
326 : }
327 35334 : case MessageType::KF_VEL_COVARIANCE: {
328 35334 : CHECK_SIZE(u.kf_vel_covariance);
329 35334 : state2.kf_vel_covariance = u.kf_vel_covariance.tofloat().rfu_to_frd(); // mm/s
330 35334 : break;
331 : }
332 35334 : case MessageType::KF_POS_COVARIANCE: {
333 35334 : CHECK_SIZE(u.kf_pos_covariance);
334 35334 : state2.kf_pos_covariance = u.kf_pos_covariance.tofloat(); // mm
335 35334 : break;
336 : }
337 35334 : case MessageType::UNIT_STATUS: {
338 35334 : CHECK_SIZE(u.unit_status);
339 35334 : state2.unit_status = u.unit_status;
340 35334 : break;
341 : }
342 35334 : case MessageType::GNSS_EXTENDED_INFO: {
343 35334 : CHECK_SIZE(u.gnss_extended_info);
344 35334 : gps_data.fix_type = AP_GPS_FixType(u.gnss_extended_info.fix_type+1);
345 35334 : gnss_data.spoof_status = u.gnss_extended_info.spoofing_status;
346 35334 : break;
347 : }
348 35334 : case MessageType::NUM_SATS: {
349 35334 : CHECK_SIZE(u.num_sats);
350 35334 : gps_data.satellites_in_view = u.num_sats;
351 35334 : break;
352 : }
353 35334 : case MessageType::GNSS_POSITION: {
354 35334 : CHECK_SIZE(u.gnss_position);
355 35334 : gnss_data.lat = u.gnss_position.lat; // deg*1.0e7
356 35334 : gnss_data.lng = u.gnss_position.lon; // deg*1.0e7
357 35334 : gnss_data.alt = u.gnss_position.alt; // mm
358 35334 : break;
359 : }
360 35334 : case MessageType::GNSS_VEL_TRACK: {
361 35334 : CHECK_SIZE(u.gnss_vel_track);
362 35334 : gnss_data.hor_speed = u.gnss_vel_track.hor_speed*0.01; // m/s
363 35334 : gnss_data.ver_speed = u.gnss_vel_track.ver_speed*0.01; // m/s
364 35334 : gnss_data.track_over_ground = u.gnss_vel_track.track_over_ground*0.01; // deg
365 35334 : break;
366 : }
367 35334 : case MessageType::GNSS_POS_TIMESTAMP: {
368 35334 : CHECK_SIZE(u.gnss_pos_timestamp);
369 35334 : gnss_data.pos_timestamp = u.gnss_pos_timestamp;
370 35334 : break;
371 : }
372 35334 : case MessageType::GNSS_INFO_SHORT: {
373 35334 : CHECK_SIZE(u.gnss_info_short);
374 35334 : gnss_data.info_short = u.gnss_info_short;
375 35334 : break;
376 : }
377 35334 : case MessageType::GNSS_NEW_DATA: {
378 35334 : CHECK_SIZE(u.gnss_new_data);
379 35334 : gnss_data.new_data = u.gnss_new_data;
380 35334 : break;
381 : }
382 35334 : case MessageType::GNSS_JAM_STATUS: {
383 35334 : CHECK_SIZE(u.gnss_jam_status);
384 35334 : gnss_data.jam_status = u.gnss_jam_status;
385 35334 : break;
386 : }
387 35334 : case MessageType::DIFFERENTIAL_PRESSURE: {
388 35334 : CHECK_SIZE(u.differential_pressure);
389 35334 : airspeed_data.differential_pressure = u.differential_pressure*1.0e-4*100; // 100: mbar to Pa
390 35334 : break;
391 : }
392 35334 : case MessageType::TRUE_AIRSPEED: {
393 35334 : CHECK_SIZE(u.true_airspeed);
394 35334 : state2.true_airspeed = u.true_airspeed*0.01; // m/s
395 35334 : break;
396 : }
397 35334 : case MessageType::WIND_SPEED: {
398 35334 : CHECK_SIZE(u.wind_speed);
399 35334 : state2.wind_speed = u.wind_speed.tofloat().rfu_to_frd()*0.01; // m/s
400 35334 : break;
401 : }
402 35334 : case MessageType::AIR_DATA_STATUS: {
403 35334 : CHECK_SIZE(u.air_data_status);
404 35334 : state2.air_data_status = u.air_data_status;
405 35334 : break;
406 : }
407 35334 : case MessageType::SUPPLY_VOLTAGE: {
408 35334 : CHECK_SIZE(u.supply_voltage);
409 35334 : state2.supply_voltage = u.supply_voltage*0.01; // V
410 35334 : break;
411 : }
412 35334 : case MessageType::TEMPERATURE: {
413 35334 : CHECK_SIZE(u.temperature);
414 : // assume same temperature for baro and airspeed
415 35334 : baro_data.temperature = u.temperature*0.1; // degC
416 35334 : airspeed_data.temperature = u.temperature*0.1; // degC
417 35334 : ins_data.temperature = u.temperature*0.1;
418 35334 : break;
419 : }
420 35334 : case MessageType::UNIT_STATUS2: {
421 35334 : CHECK_SIZE(u.unit_status2);
422 35334 : state2.unit_status2 = u.unit_status2;
423 35334 : break;
424 : }
425 35334 : case MessageType::GNSS_ANGLES: {
426 35334 : CHECK_SIZE(u.gnss_angles);
427 35334 : gnss_data.heading = u.gnss_angles.heading*0.01; // deg
428 35334 : gnss_data.pitch = u.gnss_angles.pitch*0.01; // deg
429 35334 : break;
430 : }
431 35334 : case MessageType::GNSS_ANGLE_POS_TYPE: {
432 35334 : CHECK_SIZE(u.gnss_angle_pos_type);
433 35334 : gnss_data.angle_pos_type = u.gnss_angle_pos_type;
434 35334 : break;
435 : }
436 35334 : case MessageType::GNSS_HEADING_TIMESTAMP: {
437 35334 : CHECK_SIZE(u.gnss_heading_timestamp);
438 35334 : gnss_data.heading_timestamp = u.gnss_heading_timestamp;
439 35334 : break;
440 : }
441 35334 : case MessageType::GNSS_DOP: {
442 35334 : CHECK_SIZE(u.gnss_dop);
443 35334 : gnss_data.gdop = u.gnss_dop.gdop*0.1;
444 35334 : gnss_data.pdop = u.gnss_dop.pdop*0.1;
445 35334 : gnss_data.tdop = u.gnss_dop.tdop*0.1;
446 :
447 35334 : gps_data.hdop = u.gnss_dop.hdop*0.1;
448 35334 : gps_data.vdop = u.gnss_dop.vdop*0.1;
449 35334 : break;
450 : }
451 35334 : case MessageType::INS_SOLUTION_STATUS: {
452 35334 : CHECK_SIZE(u.ins_sol_status);
453 35334 : state2.ins_sol_status = u.ins_sol_status;
454 35334 : break;
455 : }
456 : }
457 :
458 1130688 : if (msg_len == 0) {
459 : // got an unknown message
460 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: unknown msg 0x%02x", unsigned(mtype));
461 0 : buffer_ofs = 0;
462 0 : return false;
463 : }
464 1130688 : message_ofs += msg_len;
465 :
466 1130688 : if (msg_len == 0 || need_re_sync) {
467 0 : re_sync();
468 0 : return false;
469 : }
470 : }
471 :
472 35334 : if (h->msg_len != message_ofs-buffer) {
473 : // we had stray bytes at the end of the message
474 0 : re_sync();
475 0 : return false;
476 : }
477 :
478 70668 : if (GOT_MSG(ACCEL_DATA_HR) &&
479 35334 : GOT_MSG(GYRO_DATA_HR)) {
480 35334 : AP::ins().handle_external(ins_data);
481 35334 : state.accel = ins_data.accel;
482 35334 : state.gyro = ins_data.gyro;
483 :
484 : #if HAL_LOGGING_ENABLED
485 35334 : uint64_t now_us = AP_HAL::micros64();
486 :
487 : // @LoggerMessage: ILB1
488 : // @Description: InertialLabs AHRS data1
489 : // @Field: TimeUS: Time since system startup
490 : // @Field: GMS: GPS INS time (round)
491 : // @Field: GyrX: Gyro X
492 : // @Field: GyrY: Gyro Y
493 : // @Field: GyrZ: Gyro z
494 : // @Field: AccX: Accelerometer X
495 : // @Field: AccY: Accelerometer Y
496 : // @Field: AccZ: Accelerometer Z
497 :
498 35334 : AP::logger().WriteStreaming("ILB1", "TimeUS,GMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ",
499 : "s-kkkooo",
500 : "F-------",
501 : "QIffffff",
502 : now_us, gps_data.ms_tow,
503 35334 : ins_data.gyro.x, ins_data.gyro.y, ins_data.gyro.z,
504 35334 : ins_data.accel.x, ins_data.accel.y, ins_data.accel.z);
505 : #endif // HAL_LOGGING_ENABLED
506 : }
507 :
508 70668 : if (GOT_MSG(GPS_INS_TIME_MS) &&
509 70668 : GOT_MSG(NUM_SATS) &&
510 70668 : GOT_MSG(GNSS_POSITION) &&
511 70668 : GOT_MSG(GNSS_NEW_DATA) &&
512 106002 : GOT_MSG(GNSS_EXTENDED_INFO) &&
513 35334 : gnss_data.new_data != 0) {
514 : uint8_t instance;
515 1763 : if (AP::gps().get_first_external_instance(instance)) {
516 1458 : AP::gps().handle_external(gps_data, instance);
517 : }
518 1763 : if (gps_data.satellites_in_view > 3) {
519 1763 : if (last_gps_ms == 0) {
520 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got GPS lock");
521 11 : if (!state.have_origin) {
522 11 : state.origin = Location{
523 : gps_data.latitude,
524 : gps_data.longitude,
525 : gps_data.msl_altitude,
526 : Location::AltFrame::ABSOLUTE};
527 11 : state.have_origin = true;
528 : }
529 : }
530 1763 : last_gps_ms = now_ms;
531 : }
532 :
533 : #if HAL_LOGGING_ENABLED
534 1763 : uint64_t now_us = AP_HAL::micros64();
535 :
536 : // @LoggerMessage: ILB4
537 : // @Description: InertialLabs AHRS data4
538 : // @Field: TimeUS: Time since system startup
539 : // @Field: GMS: GNSS Position timestamp
540 : // @Field: GWk: GPS Week
541 : // @Field: NSat: Number of satellites
542 : // @Field: NewGPS: Indicator of new update of GPS data
543 : // @Field: Lat: GNSS Latitude
544 : // @Field: Lng: GNSS Longitude
545 : // @Field: Alt: GNSS Altitude
546 : // @Field: GCrs: GNSS Track over ground
547 : // @Field: Spd: GNSS Horizontal speed
548 : // @Field: VZ: GNSS Vertical speed
549 :
550 1763 : AP::logger().WriteStreaming("ILB4", "TimeUS,GMS,GWk,NSat,NewGPS,Lat,Lng,Alt,GCrs,Spd,VZ",
551 : "s----DUmhnn",
552 : "F----------",
553 : "QIHBBffffff",
554 1763 : now_us, gnss_data.pos_timestamp, gps_data.gps_week,
555 1763 : gps_data.satellites_in_view, gnss_data.new_data,
556 1763 : gnss_data.lat*1.0e-7, gnss_data.lng*1.0e-7, gnss_data.alt*0.01,
557 1763 : gnss_data.track_over_ground, gnss_data.hor_speed, gnss_data.ver_speed
558 : );
559 :
560 : // @LoggerMessage: ILB5
561 : // @Description: InertialLabs AHRS data5
562 : // @Field: TimeUS: Time since system startup
563 : // @Field: GMS: GNSS Position timestamp
564 : // @Field: FType: fix type
565 : // @Field: GSS: GNSS spoofing status
566 : // @Field: GJS: GNSS jamming status
567 : // @Field: GI1: GNSS Info1
568 : // @Field: GI2: GNSS Info2
569 : // @Field: GAPS: GNSS Angles position type
570 :
571 1763 : AP::logger().WriteStreaming("ILB5", "TimeUS,GMS,FType,GSS,GJS,GI1,GI2,GAPS",
572 : "s-------",
573 : "F-------",
574 : "QIBBBBBB",
575 : now_us, gnss_data.pos_timestamp, gps_data.fix_type,
576 1763 : gnss_data.spoof_status, gnss_data.jam_status,
577 1763 : gnss_data.info_short.info1, gnss_data.info_short.info2,
578 1763 : gnss_data.angle_pos_type);
579 :
580 : // @LoggerMessage: ILB6
581 : // @Description: InertialLabs AHRS data6
582 : // @Field: TimeUS: Time since system startup
583 : // @Field: GMS: GNSS Position timestamp
584 : // @Field: GpsHTS: GNSS Heading timestamp
585 : // @Field: GpsYaw: GNSS Heading
586 : // @Field: GpsPitch: GNSS Pitch
587 : // @Field: GDOP: GNSS GDOP
588 : // @Field: PDOP: GNSS PDOP
589 : // @Field: HDOP: GNSS HDOP
590 : // @Field: VDOP: GNSS VDOP
591 : // @Field: TDOP: GNSS TDOP
592 :
593 1763 : AP::logger().WriteStreaming("ILB6", "TimeUS,GMS,GpsHTS,GpsYaw,GpsPitch,GDOP,PDOP,HDOP,VDOP,TDOP",
594 : "s--hd-----",
595 : "F---------",
596 : "QIIfffffff",
597 : now_us, gnss_data.pos_timestamp, gnss_data.heading_timestamp,
598 1763 : gnss_data.heading, gnss_data.pitch, gnss_data.gdop, gnss_data.pdop,
599 1763 : gps_data.hdop, gps_data.vdop, gnss_data.tdop);
600 : #endif // HAL_LOGGING_ENABLED
601 : }
602 :
603 : #if AP_BARO_EXTERNALAHRS_ENABLED
604 70668 : if (GOT_MSG(BARO_DATA) &&
605 35334 : GOT_MSG(TEMPERATURE)) {
606 35334 : AP::baro().handle_external(baro_data);
607 :
608 : #if HAL_LOGGING_ENABLED
609 35334 : uint64_t now_us = AP_HAL::micros64();
610 :
611 : // @LoggerMessage: ILB3
612 : // @Description: InertialLabs AHRS data3
613 : // @Field: TimeUS: Time since system startup
614 : // @Field: GMS: GPS INS time (round)
615 : // @Field: Press: Static pressure
616 : // @Field: Diff: Differential pressure
617 : // @Field: Temp: Temperature
618 : // @Field: Alt: Baro altitude
619 : // @Field: TAS: true airspeed
620 : // @Field: VWN: Wind velocity north
621 : // @Field: VWE: Wind velocity east
622 : // @Field: VWD: Wind velocity down
623 : // @Field: ADU: Air Data Unit status
624 :
625 35334 : AP::logger().WriteStreaming("ILB3", "TimeUS,GMS,Press,Diff,Temp,Alt,TAS,VWN,VWE,VWD,ADU",
626 : "s-PPOmnnnn-",
627 : "F----------",
628 : "QIffffffffH",
629 : now_us, gps_data.ms_tow,
630 35334 : baro_data.pressure_pa, airspeed_data.differential_pressure, baro_data.temperature,
631 35334 : state2.baro_alt, state2.true_airspeed,
632 35334 : state2.wind_speed.x, state2.wind_speed.y, state2.wind_speed.z,
633 35334 : state2.air_data_status);
634 : #endif // HAL_LOGGING_ENABLED
635 : }
636 : #endif // AP_BARO_EXTERNALAHRS_ENABLED
637 :
638 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
639 35334 : if (GOT_MSG(MAG_DATA)) {
640 35334 : AP::compass().handle_external(mag_data);
641 :
642 : #if HAL_LOGGING_ENABLED
643 35334 : uint64_t now_us = AP_HAL::micros64();
644 :
645 : // @LoggerMessage: ILB2
646 : // @Description: InertialLabs AHRS data2
647 : // @Field: TimeUS: Time since system startup
648 : // @Field: GMS: GPS INS time (round)
649 : // @Field: MagX: Magnetometer X
650 : // @Field: MagY: Magnetometer Y
651 : // @Field: MagZ: Magnetometer Z
652 :
653 35334 : AP::logger().WriteStreaming("ILB2", "TimeUS,GMS,MagX,MagY,MagZ",
654 : "s----",
655 : "F----",
656 : "QIfff",
657 : now_us, gps_data.ms_tow,
658 35334 : mag_data.field.x, mag_data.field.y, mag_data.field.z);
659 : #endif // HAL_LOGGING_ENABLED
660 : }
661 : #endif // AP_COMPASS_EXTERNALAHRS_ENABLED
662 :
663 : #if AP_AIRSPEED_EXTERNAL_ENABLED && (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane))
664 : // only on plane and copter as others do not link AP_Airspeed
665 70668 : if (GOT_MSG(DIFFERENTIAL_PRESSURE) &&
666 35334 : GOT_MSG(TEMPERATURE)) {
667 35334 : auto *arsp = AP::airspeed();
668 35334 : if (arsp != nullptr) {
669 35334 : arsp->handle_external(airspeed_data);
670 : }
671 : }
672 :
673 : #endif // AP_AIRSPEED_EXTERNAL_ENABLED
674 :
675 35334 : buffer_ofs = 0;
676 :
677 70668 : if (GOT_MSG(POSITION) &&
678 70668 : GOT_MSG(ORIENTATION_ANGLES) &&
679 35334 : GOT_MSG(VELOCITIES)) {
680 :
681 : float roll, pitch, yaw_deg;
682 35334 : state.quat.to_euler(roll, pitch, yaw_deg);
683 :
684 35334 : yaw_deg = fmodf(degrees(yaw_deg), 360.0f);
685 35334 : if (yaw_deg < 0.0f) {
686 30912 : yaw_deg += 360.0f;
687 : }
688 :
689 : #if HAL_LOGGING_ENABLED
690 35334 : uint64_t now_us = AP_HAL::micros64();
691 :
692 : // @LoggerMessage: ILB7
693 : // @Description: InertialLabs AHRS data7
694 : // @Field: TimeUS: Time since system startup
695 : // @Field: GMS: GPS INS time (round)
696 : // @Field: Roll: euler roll
697 : // @Field: Pitch: euler pitch
698 : // @Field: Yaw: euler yaw
699 : // @Field: VN: velocity north
700 : // @Field: VE: velocity east
701 : // @Field: VD: velocity down
702 : // @Field: Lat: latitude
703 : // @Field: Lng: longitude
704 : // @Field: Alt: altitude MSL
705 : // @Field: USW: USW1
706 : // @Field: USW2: USW2
707 : // @Field: Vdc: Supply voltage
708 :
709 70668 : AP::logger().WriteStreaming("ILB7", "TimeUS,GMS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lng,Alt,USW,USW2,Vdc",
710 : "s-dddnnnDUm--v",
711 : "F-------------",
712 : "QIfffffffffHHf",
713 : now_us, gps_data.ms_tow,
714 35334 : degrees(roll), degrees(pitch), yaw_deg,
715 35334 : state.velocity.x, state.velocity.y, state.velocity.z,
716 35334 : state.location.lat*1.0e-7, state.location.lng*1.0e-7, state.location.alt*0.01,
717 35334 : state2.unit_status, state2.unit_status2,
718 35334 : state2.supply_voltage);
719 :
720 : // @LoggerMessage: ILB8
721 : // @Description: InertialLabs AHRS data8
722 : // @Field: TimeUS: Time since system startup
723 : // @Field: GMS: GPS INS time (round)
724 : // @Field: PVN: position variance north
725 : // @Field: PVE: position variance east
726 : // @Field: PVD: position variance down
727 : // @Field: VVN: velocity variance north
728 : // @Field: VVE: velocity variance east
729 : // @Field: VVD: velocity variance down
730 :
731 35334 : AP::logger().WriteStreaming("ILB8", "TimeUS,GMS,PVN,PVE,PVD,VVN,VVE,VVD",
732 : "s-mmmnnn",
733 : "F-------",
734 : "QIffffff",
735 : now_us, gps_data.ms_tow,
736 35334 : state2.kf_pos_covariance.x, state2.kf_pos_covariance.y, state2.kf_pos_covariance.z,
737 35334 : state2.kf_vel_covariance.x, state2.kf_vel_covariance.y, state2.kf_vel_covariance.z);
738 : #endif // HAL_LOGGING_ENABLED
739 : }
740 :
741 35334 : const uint32_t dt_critical_usw = 10000;
742 35334 : uint32_t now_usw = AP_HAL::millis();
743 :
744 : // InertialLabs critical messages to GCS (sending messages once every 10 seconds)
745 35334 : if ((last_unit_status != state2.unit_status) ||
746 35334 : (last_unit_status2 != state2.unit_status2) ||
747 35334 : (last_air_data_status != state2.air_data_status) ||
748 35334 : (now_usw - last_critical_msg_ms > dt_critical_usw)) {
749 :
750 : // Critical USW message
751 14 : if (state2.unit_status & ILABS_UNIT_STATUS_ALIGNMENT_FAIL) {
752 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Unsuccessful initial alignment");
753 : }
754 14 : if (state2.unit_status & ILABS_UNIT_STATUS_OPERATION_FAIL) {
755 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: IMU data are incorrect");
756 : }
757 :
758 14 : if (state2.unit_status & ILABS_UNIT_STATUS_GYRO_FAIL) {
759 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Gyros failure");
760 : }
761 :
762 14 : if (state2.unit_status & ILABS_UNIT_STATUS_ACCEL_FAIL) {
763 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Accelerometers failure");
764 : }
765 :
766 14 : if (state2.unit_status & ILABS_UNIT_STATUS_MAG_FAIL) {
767 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Magnetometers failure");
768 : }
769 :
770 14 : if (state2.unit_status & ILABS_UNIT_STATUS_ELECTRONICS_FAIL) {
771 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Electronics failure");
772 : }
773 :
774 14 : if (state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) {
775 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: GNSS receiver failure");
776 : }
777 :
778 : // Critical USW2 message
779 14 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_BARO_FAIL) {
780 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Baro altimeter failure");
781 : }
782 :
783 14 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_DIFF_PRESS_FAIL) {
784 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Diff. pressure sensor failure");
785 : }
786 :
787 : // Critical ADU message
788 14 : if (state2.air_data_status & ILABS_AIRDATA_INIT_FAIL) {
789 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Static pressure sensor unsuccessful initialization");
790 : }
791 :
792 14 : if (state2.air_data_status & ILABS_AIRDATA_DIFF_PRESS_INIT_FAIL) {
793 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Diff. pressure sensor unsuccessful initialization");
794 : }
795 :
796 14 : if (state2.air_data_status & ILABS_AIRDATA_STATIC_PRESS_FAIL) {
797 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ILAB: Static pressure sensor failure is detect");
798 : }
799 :
800 14 : if (state2.air_data_status & ILABS_AIRDATA_DIFF_PRESS_FAIL) {
801 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ILAB: Diff. pressure sensor failure is detect");
802 : }
803 :
804 14 : last_critical_msg_ms = AP_HAL::millis();
805 : }
806 :
807 35334 : if (last_unit_status != state2.unit_status) {
808 0 : if (state2.unit_status & ILABS_UNIT_STATUS_RUNTIME_CAL) {
809 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: On-the-fly calibration is in progress");
810 : }
811 :
812 0 : if (state2.unit_status & ILABS_UNIT_STATUS_VOLTAGE_LOW) {
813 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Low input voltage");
814 : } else {
815 0 : if (last_unit_status & ILABS_UNIT_STATUS_VOLTAGE_LOW) {
816 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Input voltage is in range");
817 : }
818 : }
819 :
820 0 : if (state2.unit_status & ILABS_UNIT_STATUS_VOLTAGE_HIGH) {
821 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: High input voltage");
822 : } else {
823 0 : if (last_unit_status & ILABS_UNIT_STATUS_VOLTAGE_HIGH) {
824 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Input voltage is in range");
825 : }
826 : }
827 :
828 0 : if (state2.unit_status & ILABS_UNIT_STATUS_X_RATE_HIGH) {
829 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Y-axis angular rate is exceeded");
830 : } else {
831 0 : if (last_unit_status & ILABS_UNIT_STATUS_X_RATE_HIGH) {
832 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Y-axis angular rate is in range");
833 : }
834 : }
835 :
836 0 : if (state2.unit_status & ILABS_UNIT_STATUS_Y_RATE_HIGH) {
837 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: X-axis angular rate is exceeded");
838 : } else {
839 0 : if (last_unit_status & ILABS_UNIT_STATUS_Y_RATE_HIGH) {
840 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: X-axis angular rate is in range");
841 : }
842 : }
843 :
844 0 : if (state2.unit_status & ILABS_UNIT_STATUS_Z_RATE_HIGH) {
845 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Z-axis angular rate is exceeded");
846 : } else {
847 0 : if (last_unit_status & ILABS_UNIT_STATUS_Z_RATE_HIGH) {
848 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Z-axis angular rate is in range");
849 : }
850 : }
851 :
852 0 : if (state2.unit_status & ILABS_UNIT_STATUS_MAG_FIELD_HIGH) {
853 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Large total magnetic field");
854 : } else {
855 0 : if (last_unit_status & ILABS_UNIT_STATUS_MAG_FIELD_HIGH) {
856 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Total magnetic field is in range");
857 : }
858 : }
859 :
860 0 : if (state2.unit_status & ILABS_UNIT_STATUS_TEMP_RANGE_ERR) {
861 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Temperature is out of range");
862 : } else {
863 0 : if (last_unit_status & ILABS_UNIT_STATUS_TEMP_RANGE_ERR) {
864 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Temperature is in range");
865 : }
866 : }
867 :
868 0 : if (state2.unit_status & ILABS_UNIT_STATUS_RUNTIME_CAL2) {
869 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: On-the-fly calibration successful");
870 : }
871 :
872 0 : last_unit_status = state2.unit_status;
873 : }
874 :
875 : // InertialLabs INS Unit Status Word 2 (USW2) messages to GCS
876 35334 : if (last_unit_status2 != state2.unit_status2) {
877 0 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_ACCEL_X_HIGH) {
878 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Y-acceleration is out of range");
879 : } else {
880 0 : if (last_unit_status2 & ILABS_UNIT_STATUS2_ACCEL_X_HIGH) {
881 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Y-acceleration is in range");
882 : }
883 : }
884 :
885 0 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Y_HIGH) {
886 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: X-acceleration is out of range");
887 : } else {
888 0 : if (last_unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Y_HIGH) {
889 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: X-acceleration is in range");
890 : }
891 : }
892 :
893 0 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Z_HIGH) {
894 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Z-acceleration is out of range");
895 : } else {
896 0 : if (last_unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Z_HIGH) {
897 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Z-acceleration is in range");
898 : }
899 : }
900 :
901 0 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_MAGCAL_2D_ACT) {
902 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Automatic 2D calibration is in progress");
903 : }
904 :
905 0 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_MAGCAL_3D_ACT) {
906 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Automatic 3D calibration is in progress");
907 : }
908 :
909 0 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_FUSION_OFF) {
910 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS input switched off");
911 : } else {
912 0 : if (last_unit_status2 & ILABS_UNIT_STATUS2_GNSS_FUSION_OFF) {
913 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS input switched on");
914 : }
915 : }
916 :
917 0 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF) {
918 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Diff. pressure input switched off");
919 : } else {
920 0 : if (last_unit_status2 & ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF) {
921 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Diff. pressure input switched on");
922 : }
923 : }
924 :
925 0 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_MAG_FUSION_OFF) {
926 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Magnetometer input switched off");
927 : } else {
928 0 : if (last_unit_status2 & ILABS_UNIT_STATUS2_MAG_FUSION_OFF) {
929 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Magnetometer input switched on");
930 : }
931 : }
932 :
933 0 : if (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) {
934 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Incorrect GNSS position");
935 : } else {
936 0 : if (last_unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) {
937 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS position is correct");
938 : }
939 : }
940 :
941 0 : last_unit_status2 = state2.unit_status2;
942 : }
943 :
944 : // InertialLabs INS Air Data Unit (ADU) status messages to GCS
945 35334 : if (last_air_data_status != state2.air_data_status) {
946 0 : if (state2.air_data_status & ILABS_AIRDATA_STATIC_PRESS_RANGE_ERR) {
947 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Static pressure is out of range");
948 : } else {
949 0 : if (last_air_data_status & ILABS_AIRDATA_STATIC_PRESS_RANGE_ERR) {
950 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Static pressure is in range");
951 : }
952 : }
953 :
954 0 : if (state2.air_data_status & ILABS_AIRDATA_DIFF_PRESS_RANGE_ERR) {
955 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Diff. pressure is out of range");
956 : } else {
957 0 : if (last_air_data_status & ILABS_AIRDATA_DIFF_PRESS_RANGE_ERR) {
958 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Diff. pressure is in range");
959 : }
960 : }
961 :
962 0 : if (state2.air_data_status & ILABS_AIRDATA_PRESS_ALT_FAIL) {
963 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Pressure altitude is incorrect");
964 : } else {
965 0 : if (last_air_data_status & ILABS_AIRDATA_PRESS_ALT_FAIL) {
966 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Pressure altitude is correct");
967 : }
968 : }
969 :
970 0 : if (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) {
971 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Air speed is incorrect");
972 : } else {
973 0 : if (last_air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) {
974 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Air speed is correct");
975 : }
976 : }
977 :
978 0 : if (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) {
979 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Air speed is below the threshold");
980 : } else {
981 0 : if (last_air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) {
982 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Air speed is above the threshold");
983 : }
984 : }
985 :
986 0 : last_air_data_status = state2.air_data_status;
987 : }
988 :
989 : // InertialLabs INS spoofing detection messages to GCS
990 35334 : if (last_spoof_status != gnss_data.spoof_status) {
991 11 : if ((last_spoof_status == 2 || last_spoof_status == 3) && (gnss_data.spoof_status == 1)) {
992 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS no spoofing");
993 : }
994 :
995 11 : if (last_spoof_status == 2) {
996 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: GNSS spoofing indicated");
997 : }
998 :
999 11 : if (last_spoof_status == 3) {
1000 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: GNSS multiple spoofing indicated");
1001 : }
1002 :
1003 11 : last_spoof_status = gnss_data.spoof_status;
1004 : }
1005 :
1006 : // InertialLabs INS jamming detection messages to GCS
1007 35334 : if (last_jam_status != gnss_data.jam_status) {
1008 11 : if ((last_jam_status == 2 || last_jam_status == 3) && (gnss_data.jam_status == 1)) {
1009 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS no jamming");
1010 : }
1011 :
1012 11 : if (gnss_data.jam_status == 3) {
1013 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: GNSS jamming indicated and no fix");
1014 : }
1015 :
1016 11 : last_jam_status = gnss_data.jam_status;
1017 : }
1018 :
1019 35334 : return true;
1020 85089 : }
1021 :
1022 11 : void AP_ExternalAHRS_InertialLabs::update_thread()
1023 : {
1024 : // Open port in the thread
1025 11 : uart->begin(baudrate, 1024, 512);
1026 :
1027 : /*
1028 : we assume the user has already configured the device
1029 : */
1030 :
1031 11 : setup_complete = true;
1032 : while (true) {
1033 76926 : if (!check_uart()) {
1034 45538 : hal.scheduler->delay_microseconds(250);
1035 : }
1036 : }
1037 : }
1038 :
1039 : // get serial port number for the uart
1040 33 : int8_t AP_ExternalAHRS_InertialLabs::get_port(void) const
1041 : {
1042 33 : if (!uart) {
1043 0 : return -1;
1044 : }
1045 33 : return port_num;
1046 : };
1047 :
1048 : // accessors for AP_AHRS
1049 8209 : bool AP_ExternalAHRS_InertialLabs::healthy(void) const
1050 : {
1051 8209 : WITH_SEMAPHORE(state.sem);
1052 8209 : return AP_HAL::millis() - last_att_ms < 100;
1053 8209 : }
1054 :
1055 8163 : bool AP_ExternalAHRS_InertialLabs::initialised(void) const
1056 : {
1057 8163 : if (!setup_complete) {
1058 0 : return false;
1059 : }
1060 8163 : return true;
1061 : }
1062 :
1063 46 : bool AP_ExternalAHRS_InertialLabs::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
1064 : {
1065 46 : if (!setup_complete) {
1066 0 : hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs setup failed");
1067 0 : return false;
1068 : }
1069 46 : if (!healthy()) {
1070 0 : hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs unhealthy");
1071 0 : return false;
1072 : }
1073 46 : WITH_SEMAPHORE(state.sem);
1074 46 : uint32_t now = AP_HAL::millis();
1075 46 : if (now - last_att_ms > 10 ||
1076 46 : now - last_pos_ms > 10 ||
1077 46 : now - last_vel_ms > 10) {
1078 0 : hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs not up to date");
1079 0 : return false;
1080 : }
1081 46 : return true;
1082 46 : }
1083 :
1084 : /*
1085 : get filter status. We don't know the meaning of the status bits yet,
1086 : so assume all OK if we have GPS lock
1087 : */
1088 9759 : void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status) const
1089 : {
1090 9759 : WITH_SEMAPHORE(state.sem);
1091 9759 : uint32_t now = AP_HAL::millis();
1092 9759 : const uint32_t dt_limit = 200;
1093 9759 : const uint32_t dt_limit_gps = 500;
1094 9759 : memset(&status, 0, sizeof(status));
1095 9759 : const bool init_ok = (state2.unit_status & (ILABS_UNIT_STATUS_ALIGNMENT_FAIL|ILABS_UNIT_STATUS_OPERATION_FAIL))==0;
1096 9759 : status.flags.initalized = init_ok;
1097 9759 : status.flags.attitude = init_ok && (now - last_att_ms < dt_limit) && init_ok;
1098 9759 : status.flags.vert_vel = init_ok && (now - last_vel_ms < dt_limit);
1099 9759 : status.flags.vert_pos = init_ok && (now - last_pos_ms < dt_limit);
1100 9759 : status.flags.horiz_vel = status.flags.vert_vel;
1101 9759 : status.flags.horiz_pos_abs = status.flags.vert_pos;
1102 9759 : status.flags.horiz_pos_rel = status.flags.horiz_pos_abs;
1103 9759 : status.flags.pred_horiz_pos_rel = status.flags.horiz_pos_abs;
1104 9759 : status.flags.pred_horiz_pos_abs = status.flags.horiz_pos_abs;
1105 19518 : status.flags.using_gps = (now - last_gps_ms < dt_limit_gps) &&
1106 9759 : ((state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) | (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_FUSION_OFF)) == 0;
1107 29277 : status.flags.gps_quality_good = (now - last_gps_ms < dt_limit_gps) &&
1108 9759 : (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) != 0 &&
1109 0 : (state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) == 0;
1110 9759 : status.flags.rejecting_airspeed = (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL);
1111 9759 : }
1112 :
1113 : // get variances
1114 9759 : bool AP_ExternalAHRS_InertialLabs::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
1115 : {
1116 9759 : velVar = state2.kf_vel_covariance.length() * vel_gate_scale;
1117 9759 : posVar = state2.kf_pos_covariance.xy().length() * pos_gate_scale;
1118 9759 : hgtVar = state2.kf_pos_covariance.z * hgt_gate_scale;
1119 9759 : tasVar = 0;
1120 9759 : return true;
1121 : }
1122 :
1123 : #endif // AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
1124 :
|
