CoCalc -- AP_ExternalAHRS_InertialLabs.cpp

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp
Views: ¹⁷⁹⁸
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
AP_ExternalAHRS_InertialLabs::AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *_frontend,
85
                                                           AP_ExternalAHRS::state_t &_state) :
86
    AP_ExternalAHRS_backend(_frontend, _state)
87
{
88
    auto &sm = AP::serialmanager();
89
    uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
90
    if (!uart) {
91
        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "InertialLabs ExternalAHRS no UART");
92
        return;
93
    }
94
    baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
95
    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
    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
    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_InertialLabs::update_thread, void), "ILabs", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
103
        AP_HAL::panic("InertialLabs Failed to start ExternalAHRS update thread");
104
    }
105
    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs ExternalAHRS initialised");
106
}
107

108
/*
109
  re-sync buffer on parse failure
110
 */
111
void AP_ExternalAHRS_InertialLabs::re_sync(void)
112
{
113
    if (buffer_ofs > 3) {
114
        /*
115
          look for the 2 byte header and try to sync to that
116
         */
117
        const uint16_t header = 0x55AA;
118
        const uint8_t *p = (const uint8_t *)memmem(&buffer[1], buffer_ofs-3, &header, sizeof(header));
119
        if (p != nullptr) {
120
            const uint16_t n = p - &buffer[0];
121
            memmove(&buffer[0], p, buffer_ofs - n);
122
            buffer_ofs -= n;
123
        } else {
124
            buffer_ofs = 0;
125
        }
126
    } else {
127
        buffer_ofs = 0;
128
    }
129
}
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
bool AP_ExternalAHRS_InertialLabs::check_header(const ILabsHeader *h) const
141
{
142
    return h->magic == 0x55AA &&
143
        h->msg_type == 1 &&
144
        h->msg_id == 0x95 &&
145
        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
bool AP_ExternalAHRS_InertialLabs::check_uart()
153
{
154
    WITH_SEMAPHORE(state.sem);
155

156
    if (!setup_complete) {
157
        return false;
158
    }
159
    // ensure we own the uart
160
    uart->begin(0);
161
    uint32_t n = uart->available();
162
    if (n == 0) {
163
        return false;
164
    }
165
    if (n + buffer_ofs > sizeof(buffer)) {
166
        n = sizeof(buffer) - buffer_ofs;
167
    }
168
    const ILabsHeader *h = (ILabsHeader *)&buffer[0];
169

170
    if (buffer_ofs < sizeof(ILabsHeader)) {
171
        n = MIN(n, sizeof(ILabsHeader)-buffer_ofs);
172
    } else {
173
        if (!check_header(h)) {
174
            re_sync();
175
            return false;
176
        }
177
        if (buffer_ofs > h->msg_len+8) {
178
            re_sync();
179
            return false;
180
        }
181
        n = MIN(n, uint32_t(h->msg_len + 2 - buffer_ofs));
182
    }
183

184
    const ssize_t nread = uart->read(&buffer[buffer_ofs], n);
185
    if (nread != ssize_t(n)) {
186
        re_sync();
187
        return false;
188
    }
189
    buffer_ofs += n;
190

191
    if (buffer_ofs < sizeof(ILabsHeader)) {
192
        return true;
193
    }
194

195
    if (!check_header(h)) {
196
        re_sync();
197
        return false;
198
    }
199

200
    if (buffer_ofs < h->msg_len+2) {
201
        /*
202
          see if we can read the rest immediately
203
         */
204
        const uint16_t needed = h->msg_len+2 - buffer_ofs;
205
        if (uart->available() < needed) {
206
            // need more data
207
            return true;
208
        }
209
        const ssize_t nread2 = uart->read(&buffer[buffer_ofs], needed);
210
        if (nread2 != needed) {
211
            re_sync();
212
            return false;
213
        }
214
        buffer_ofs += nread2;
215
    }
216

217
    // check checksum
218
    const uint16_t crc1 = crc_sum_of_bytes_16(&buffer[2], buffer_ofs-4);
219
    const uint16_t crc2 = le16toh_ptr(&buffer[buffer_ofs-2]);
220
    if (crc1 != crc2) {
221
        re_sync();
222
        return false;
223
    }
224

225
    const uint8_t *buffer_end = &buffer[buffer_ofs];
226
    const uint16_t payload_size = h->msg_len - 6;
227
    const uint8_t *payload = &buffer[6];
228
    if (payload_size < 3) {
229
        re_sync();
230
        return false;
231
    }
232
    const uint8_t num_messages = payload[0];
233
    if (num_messages == 0 ||
234
        num_messages > payload_size-1) {
235
        re_sync();
236
        return false;
237
    }
238
    const uint8_t *message_ofs = &payload[num_messages+1];
239
    bool need_re_sync = false;
240

241
    // bitmask for what types we get
242
    Bitmask<256> msg_types;
243
    uint32_t now_ms = AP_HAL::millis();
244

245
    for (uint8_t i=0; i<num_messages; i++) {
246
        if (message_ofs >= buffer_end) {
247
            re_sync();
248
            return false;
249
        }
250
        MessageType mtype = (MessageType)payload[1+i];
251
        ILabsData &u = *(ILabsData*)message_ofs;
252
        uint8_t msg_len = 0;
253

254
        msg_types.set(unsigned(mtype));
255

256
        switch (mtype) {
257
        case MessageType::GPS_INS_TIME_MS: {
258
            // this is the GPS tow timestamp in ms for when the IMU data was sampled
259
            CHECK_SIZE(u.gnss_time_ms);
260
            gps_data.ms_tow = u.gnss_time_ms;
261
            break;
262
        }
263
        case MessageType::GPS_WEEK: {
264
            CHECK_SIZE(u.gnss_week);
265
            gps_data.gps_week = u.gnss_week;
266
            break;
267
        }
268
        case MessageType::ACCEL_DATA_HR: {
269
            CHECK_SIZE(u.accel_data_hr);
270
            // should use 9.8106 instead of GRAVITY_MSS-constant in accordance with the device-documentation
271
            ins_data.accel = u.accel_data_hr.tofloat().rfu_to_frd()*9.8106f*1.0e-6; // m/s^2
272
            break;
273
        }
274
        case MessageType::GYRO_DATA_HR: {
275
            CHECK_SIZE(u.gyro_data_hr);
276
            ins_data.gyro = u.gyro_data_hr.tofloat().rfu_to_frd()*DEG_TO_RAD*1.0e-5; // rad/s
277
            break;
278
        }
279
        case MessageType::BARO_DATA: {
280
            CHECK_SIZE(u.baro_data);
281
            baro_data.pressure_pa = u.baro_data.pressure_pa2*2; // Pa
282
            state2.baro_alt = u.baro_data.baro_alt*0.01; // m
283
            break;
284
        }
285
        case MessageType::MAG_DATA: {
286
            CHECK_SIZE(u.mag_data);
287
            mag_data.field = u.mag_data.tofloat().rfu_to_frd()*(10*NTESLA_TO_MGAUSS); // milligauss
288
            break;
289
        }
290
        case MessageType::ORIENTATION_ANGLES: {
291
            CHECK_SIZE(u.orientation_angles);
292
            state.quat.from_euler(radians(u.orientation_angles.roll*0.01),
293
                                radians(u.orientation_angles.pitch*0.01),
294
                                radians(u.orientation_angles.yaw*0.01));
295
            state.have_quaternion = true;
296
            if (last_att_ms == 0) {
297
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got link");
298
            }
299
            last_att_ms = now_ms;
300
            break;
301
        }
302
        case MessageType::VELOCITIES: {
303
            CHECK_SIZE(u.velocity);
304
            state.velocity = u.velocity.tofloat().rfu_to_frd()*0.01;
305
            gps_data.ned_vel_north = state.velocity.x; // m/s
306
            gps_data.ned_vel_east = state.velocity.y; // m/s
307
            gps_data.ned_vel_down = state.velocity.z; // m/s
308
            state.have_velocity = true;
309
            last_vel_ms = now_ms;
310
            break;
311
        }
312
        case MessageType::POSITION: {
313
            CHECK_SIZE(u.position);
314
            state.location.lat = u.position.lat; // deg*1.0e7
315
            state.location.lng = u.position.lon; // deg*1.0e7
316
            state.location.alt = u.position.alt; // m*100
317

318
            gps_data.latitude = u.position.lat; // deg*1.0e7
319
            gps_data.longitude = u.position.lon; // deg*1.0e7
320
            gps_data.msl_altitude = u.position.alt; // m*100
321

322
            state.have_location = true;
323
            state.last_location_update_us = AP_HAL::micros();
324
            last_pos_ms = now_ms;
325
            break;
326
        }
327
        case MessageType::KF_VEL_COVARIANCE: {
328
            CHECK_SIZE(u.kf_vel_covariance);
329
            state2.kf_vel_covariance = u.kf_vel_covariance.tofloat().rfu_to_frd(); // mm/s
330
            break;
331
        }
332
        case MessageType::KF_POS_COVARIANCE: {
333
            CHECK_SIZE(u.kf_pos_covariance);
334
            state2.kf_pos_covariance = u.kf_pos_covariance.tofloat(); // mm
335
            break;
336
        }
337
        case MessageType::UNIT_STATUS: {
338
            CHECK_SIZE(u.unit_status);
339
            state2.unit_status = u.unit_status;
340
            break;
341
        }
342
        case MessageType::GNSS_EXTENDED_INFO: {
343
            CHECK_SIZE(u.gnss_extended_info);
344
            gps_data.fix_type = AP_GPS_FixType(u.gnss_extended_info.fix_type+1);
345
            gnss_data.spoof_status = u.gnss_extended_info.spoofing_status;
346
            break;
347
        }
348
        case MessageType::NUM_SATS: {
349
            CHECK_SIZE(u.num_sats);
350
            gps_data.satellites_in_view = u.num_sats;
351
            break;
352
        }
353
        case MessageType::GNSS_POSITION: {
354
            CHECK_SIZE(u.gnss_position);
355
            gnss_data.lat = u.gnss_position.lat; // deg*1.0e7
356
            gnss_data.lng = u.gnss_position.lon; // deg*1.0e7
357
            gnss_data.alt = u.gnss_position.alt; // mm
358
            break;
359
        }
360
        case MessageType::GNSS_VEL_TRACK: {
361
            CHECK_SIZE(u.gnss_vel_track);
362
            gnss_data.hor_speed = u.gnss_vel_track.hor_speed*0.01; // m/s
363
            gnss_data.ver_speed = u.gnss_vel_track.ver_speed*0.01; // m/s
364
            gnss_data.track_over_ground = u.gnss_vel_track.track_over_ground*0.01; // deg
365
            break;
366
        }
367
        case MessageType::GNSS_POS_TIMESTAMP: {
368
            CHECK_SIZE(u.gnss_pos_timestamp);
369
            gnss_data.pos_timestamp = u.gnss_pos_timestamp;
370
            break;
371
        }
372
        case MessageType::GNSS_INFO_SHORT: {
373
            CHECK_SIZE(u.gnss_info_short);
374
            gnss_data.info_short = u.gnss_info_short;
375
            break;
376
        }
377
        case MessageType::GNSS_NEW_DATA: {
378
            CHECK_SIZE(u.gnss_new_data);
379
            gnss_data.new_data = u.gnss_new_data;
380
            break;
381
        }
382
        case MessageType::GNSS_JAM_STATUS: {
383
            CHECK_SIZE(u.gnss_jam_status);
384
            gnss_data.jam_status = u.gnss_jam_status;
385
            break;
386
        }
387
        case MessageType::DIFFERENTIAL_PRESSURE: {
388
            CHECK_SIZE(u.differential_pressure);
389
            airspeed_data.differential_pressure = u.differential_pressure*1.0e-4*100; // 100: mbar to Pa
390
            break;
391
        }
392
        case MessageType::TRUE_AIRSPEED: {
393
            CHECK_SIZE(u.true_airspeed);
394
            state2.true_airspeed = u.true_airspeed*0.01; // m/s
395
            break;
396
        }
397
        case MessageType::WIND_SPEED: {
398
            CHECK_SIZE(u.wind_speed);
399
            state2.wind_speed = u.wind_speed.tofloat().rfu_to_frd()*0.01; // m/s
400
            break;
401
        }
402
        case MessageType::AIR_DATA_STATUS: {
403
            CHECK_SIZE(u.air_data_status);
404
            state2.air_data_status = u.air_data_status;
405
            break;
406
        }
407
        case MessageType::SUPPLY_VOLTAGE: {
408
            CHECK_SIZE(u.supply_voltage);
409
            state2.supply_voltage = u.supply_voltage*0.01; // V
410
            break;
411
        }
412
        case MessageType::TEMPERATURE: {
413
            CHECK_SIZE(u.temperature);
414
            // assume same temperature for baro and airspeed
415
            baro_data.temperature = u.temperature*0.1; // degC
416
            airspeed_data.temperature = u.temperature*0.1; // degC
417
            ins_data.temperature = u.temperature*0.1;
418
            break;
419
        }
420
        case MessageType::UNIT_STATUS2: {
421
            CHECK_SIZE(u.unit_status2);
422
            state2.unit_status2 = u.unit_status2;
423
            break;
424
        }
425
        case MessageType::GNSS_ANGLES: {
426
            CHECK_SIZE(u.gnss_angles);
427
            gnss_data.heading = u.gnss_angles.heading*0.01; // deg
428
            gnss_data.pitch = u.gnss_angles.pitch*0.01; // deg
429
            break;
430
        }
431
        case MessageType::GNSS_ANGLE_POS_TYPE: {
432
            CHECK_SIZE(u.gnss_angle_pos_type);
433
            gnss_data.angle_pos_type = u.gnss_angle_pos_type;
434
            break;
435
        }
436
        case MessageType::GNSS_HEADING_TIMESTAMP: {
437
            CHECK_SIZE(u.gnss_heading_timestamp);
438
            gnss_data.heading_timestamp = u.gnss_heading_timestamp;
439
            break;
440
        }
441
        case MessageType::GNSS_DOP: {
442
            CHECK_SIZE(u.gnss_dop);
443
            gnss_data.gdop = u.gnss_dop.gdop*0.1;
444
            gnss_data.pdop = u.gnss_dop.pdop*0.1;
445
            gnss_data.tdop = u.gnss_dop.tdop*0.1;
446

447
            gps_data.hdop = u.gnss_dop.hdop*0.1;
448
            gps_data.vdop = u.gnss_dop.vdop*0.1;
449
            break;
450
        }
451
        case MessageType::INS_SOLUTION_STATUS: {
452
            CHECK_SIZE(u.ins_sol_status);
453
            state2.ins_sol_status = u.ins_sol_status;
454
            break;
455
        }
456
        }
457

458
        if (msg_len == 0) {
459
            // got an unknown message
460
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: unknown msg 0x%02x", unsigned(mtype));
461
            buffer_ofs = 0;
462
            return false;
463
        }
464
        message_ofs += msg_len;
465

466
        if (msg_len == 0 || need_re_sync) {
467
            re_sync();
468
            return false;
469
        }
470
    }
471

472
    if (h->msg_len != message_ofs-buffer) {
473
        // we had stray bytes at the end of the message
474
        re_sync();
475
        return false;
476
    }
477

478
    if (GOT_MSG(ACCEL_DATA_HR) &&
479
        GOT_MSG(GYRO_DATA_HR)) {
480
        AP::ins().handle_external(ins_data);
481
        state.accel = ins_data.accel;
482
        state.gyro = ins_data.gyro;
483

484
#if HAL_LOGGING_ENABLED
485
        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
        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
                                    ins_data.gyro.x, ins_data.gyro.y, ins_data.gyro.z,
504
                                    ins_data.accel.x, ins_data.accel.y, ins_data.accel.z);
505
#endif // HAL_LOGGING_ENABLED
506
    }
507

508
    if (GOT_MSG(GPS_INS_TIME_MS) &&
509
        GOT_MSG(NUM_SATS) &&
510
        GOT_MSG(GNSS_POSITION) &&
511
        GOT_MSG(GNSS_NEW_DATA) &&
512
        GOT_MSG(GNSS_EXTENDED_INFO) &&
513
        gnss_data.new_data != 0) {
514
        uint8_t instance;
515
        if (AP::gps().get_first_external_instance(instance)) {
516
            AP::gps().handle_external(gps_data, instance);
517
        }
518
        if (gps_data.satellites_in_view > 3) {
519
            if (last_gps_ms == 0) {
520
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got GPS lock");
521
                if (!state.have_origin) {
522
                    state.origin = Location{
523
                        gps_data.latitude,
524
                        gps_data.longitude,
525
                        gps_data.msl_altitude,
526
                        Location::AltFrame::ABSOLUTE};
527
                    state.have_origin = true;
528
                }
529
            }
530
            last_gps_ms = now_ms;
531
        }
532

533
#if HAL_LOGGING_ENABLED
534
        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
        AP::logger().WriteStreaming("ILB4", "TimeUS,GMS,GWk,NSat,NewGPS,Lat,Lng,Alt,GCrs,Spd,VZ",
551
                                    "s----DUmhnn",
552
                                    "F----------",
553
                                    "QIHBBffffff",
554
                                    now_us, gnss_data.pos_timestamp, gps_data.gps_week,
555
                                    gps_data.satellites_in_view, gnss_data.new_data,
556
                                    gnss_data.lat*1.0e-7, gnss_data.lng*1.0e-7, gnss_data.alt*0.01,
557
                                    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
        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
                                    gnss_data.spoof_status, gnss_data.jam_status,
577
                                    gnss_data.info_short.info1, gnss_data.info_short.info2,
578
                                    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
        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
                                    gnss_data.heading, gnss_data.pitch, gnss_data.gdop, gnss_data.pdop,
599
                                    gps_data.hdop, gps_data.vdop, gnss_data.tdop);
600
#endif // HAL_LOGGING_ENABLED
601
    }
602

603
#if AP_BARO_EXTERNALAHRS_ENABLED
604
    if (GOT_MSG(BARO_DATA) &&
605
        GOT_MSG(TEMPERATURE)) {
606
        AP::baro().handle_external(baro_data);
607

608
#if HAL_LOGGING_ENABLED
609
        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
        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
                                    baro_data.pressure_pa, airspeed_data.differential_pressure, baro_data.temperature,
631
                                    state2.baro_alt, state2.true_airspeed,
632
                                    state2.wind_speed.x, state2.wind_speed.y, state2.wind_speed.z,
633
                                    state2.air_data_status);
634
#endif // HAL_LOGGING_ENABLED
635
    }
636
#endif // AP_BARO_EXTERNALAHRS_ENABLED
637

638
#if AP_COMPASS_EXTERNALAHRS_ENABLED
639
    if (GOT_MSG(MAG_DATA)) {
640
        AP::compass().handle_external(mag_data);
641

642
#if HAL_LOGGING_ENABLED
643
        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
        AP::logger().WriteStreaming("ILB2", "TimeUS,GMS,MagX,MagY,MagZ",
654
                                    "s----",
655
                                    "F----",
656
                                    "QIfff",
657
                                    now_us, gps_data.ms_tow,
658
                                    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
    if (GOT_MSG(DIFFERENTIAL_PRESSURE) &&
666
        GOT_MSG(TEMPERATURE)) {
667
        auto *arsp = AP::airspeed();
668
        if (arsp != nullptr) {
669
            arsp->handle_external(airspeed_data);
670
        }
671
    }
672

673
#endif // AP_AIRSPEED_EXTERNAL_ENABLED
674

675
    buffer_ofs = 0;
676

677
    if (GOT_MSG(POSITION) &&
678
        GOT_MSG(ORIENTATION_ANGLES) &&
679
        GOT_MSG(VELOCITIES)) {
680

681
        float roll, pitch, yaw_deg;
682
        state.quat.to_euler(roll, pitch, yaw_deg);
683

684
        yaw_deg = fmodf(degrees(yaw_deg), 360.0f);
685
        if (yaw_deg < 0.0f) {
686
            yaw_deg += 360.0f;
687
        }
688

689
#if HAL_LOGGING_ENABLED
690
        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
        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
                                    degrees(roll), degrees(pitch), yaw_deg,
715
                                    state.velocity.x, state.velocity.y, state.velocity.z,
716
                                    state.location.lat*1.0e-7, state.location.lng*1.0e-7, state.location.alt*0.01,
717
                                    state2.unit_status, state2.unit_status2,
718
                                    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
        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
                                    state2.kf_pos_covariance.x, state2.kf_pos_covariance.y, state2.kf_pos_covariance.z,
737
                                    state2.kf_vel_covariance.x, state2.kf_vel_covariance.y, state2.kf_vel_covariance.z);
738
#endif  // HAL_LOGGING_ENABLED
739
    }
740

741
    const uint32_t dt_critical_usw = 10000;
742
    uint32_t now_usw = AP_HAL::millis();
743

744
    // InertialLabs critical messages to GCS (sending messages once every 10 seconds)
745
    if ((last_unit_status != state2.unit_status) ||
746
        (last_unit_status2 != state2.unit_status2) ||
747
        (last_air_data_status != state2.air_data_status) ||
748
        (now_usw - last_critical_msg_ms > dt_critical_usw)) {
749

750
        // Critical USW message
751
        if (state2.unit_status & ILABS_UNIT_STATUS_ALIGNMENT_FAIL) {
752
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Unsuccessful initial alignment");
753
        }
754
        if (state2.unit_status & ILABS_UNIT_STATUS_OPERATION_FAIL) {
755
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: IMU data are incorrect");
756
        }
757

758
        if (state2.unit_status & ILABS_UNIT_STATUS_GYRO_FAIL) {
759
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Gyros failure");
760
        }
761

762
        if (state2.unit_status & ILABS_UNIT_STATUS_ACCEL_FAIL) {
763
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Accelerometers failure");
764
        }
765

766
        if (state2.unit_status & ILABS_UNIT_STATUS_MAG_FAIL) {
767
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Magnetometers failure");
768
        }
769

770
        if (state2.unit_status & ILABS_UNIT_STATUS_ELECTRONICS_FAIL) {
771
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Electronics failure");
772
        }
773

774
        if (state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) {
775
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: GNSS receiver failure");
776
        }
777

778
        // Critical USW2 message
779
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_BARO_FAIL) {
780
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Baro altimeter failure");
781
        }
782

783
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_DIFF_PRESS_FAIL) {
784
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Diff. pressure sensor failure");
785
        }
786

787
        // Critical ADU message
788
        if (state2.air_data_status & ILABS_AIRDATA_INIT_FAIL) {
789
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Static pressure sensor unsuccessful initialization");
790
        }
791

792
        if (state2.air_data_status & ILABS_AIRDATA_DIFF_PRESS_INIT_FAIL) {
793
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ILAB: Diff. pressure sensor unsuccessful initialization");
794
        }
795

796
        if (state2.air_data_status & ILABS_AIRDATA_STATIC_PRESS_FAIL) {
797
            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ILAB: Static pressure sensor failure is detect");
798
        }
799

800
        if (state2.air_data_status & ILABS_AIRDATA_DIFF_PRESS_FAIL) {
801
            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ILAB: Diff. pressure sensor failure is detect");
802
        }
803

804
        last_critical_msg_ms = AP_HAL::millis();
805
    }
806

807
    if (last_unit_status != state2.unit_status) {
808
        if (state2.unit_status & ILABS_UNIT_STATUS_RUNTIME_CAL) {
809
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: On-the-fly calibration is in progress");
810
        }
811

812
        if (state2.unit_status & ILABS_UNIT_STATUS_VOLTAGE_LOW) {
813
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Low input voltage");
814
        } else {
815
            if (last_unit_status & ILABS_UNIT_STATUS_VOLTAGE_LOW) {
816
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Input voltage is in range");
817
            }
818
        }
819

820
        if (state2.unit_status & ILABS_UNIT_STATUS_VOLTAGE_HIGH) {
821
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: High input voltage");
822
        } else {
823
            if (last_unit_status & ILABS_UNIT_STATUS_VOLTAGE_HIGH) {
824
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Input voltage is in range");
825
            }
826
        }
827

828
        if (state2.unit_status & ILABS_UNIT_STATUS_X_RATE_HIGH) {
829
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Y-axis angular rate is exceeded");
830
        } else {
831
            if (last_unit_status & ILABS_UNIT_STATUS_X_RATE_HIGH) {
832
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Y-axis angular rate is in range");
833
            }
834
        }
835

836
        if (state2.unit_status & ILABS_UNIT_STATUS_Y_RATE_HIGH) {
837
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: X-axis angular rate is exceeded");
838
        } else {
839
            if (last_unit_status & ILABS_UNIT_STATUS_Y_RATE_HIGH) {
840
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: X-axis angular rate is in range");
841
            }
842
        }
843

844
        if (state2.unit_status & ILABS_UNIT_STATUS_Z_RATE_HIGH) {
845
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Z-axis angular rate is exceeded");
846
        } else {
847
            if (last_unit_status & ILABS_UNIT_STATUS_Z_RATE_HIGH) {
848
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Z-axis angular rate is in range");
849
            }
850
        }
851

852
        if (state2.unit_status & ILABS_UNIT_STATUS_MAG_FIELD_HIGH) {
853
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Large total magnetic field");
854
        } else {
855
            if (last_unit_status & ILABS_UNIT_STATUS_MAG_FIELD_HIGH) {
856
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Total magnetic field is in range");
857
            }
858
        }
859

860
        if (state2.unit_status & ILABS_UNIT_STATUS_TEMP_RANGE_ERR) {
861
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Temperature is out of range");
862
        } else {
863
            if (last_unit_status & ILABS_UNIT_STATUS_TEMP_RANGE_ERR) {
864
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Temperature is in range");
865
            }
866
        }
867

868
        if (state2.unit_status & ILABS_UNIT_STATUS_RUNTIME_CAL2) {
869
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: On-the-fly calibration successful");
870
        }
871

872
        last_unit_status = state2.unit_status;
873
    }
874

875
    // InertialLabs INS Unit Status Word 2 (USW2) messages to GCS
876
    if (last_unit_status2 != state2.unit_status2) {
877
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_ACCEL_X_HIGH) {
878
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Y-acceleration is out of range");
879
        } else {
880
            if (last_unit_status2 & ILABS_UNIT_STATUS2_ACCEL_X_HIGH) {
881
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Y-acceleration is in range");
882
            }
883
        }
884

885
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Y_HIGH) {
886
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: X-acceleration is out of range");
887
        } else {
888
            if (last_unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Y_HIGH) {
889
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: X-acceleration is in range");
890
            }
891
        }
892

893
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Z_HIGH) {
894
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Z-acceleration is out of range");
895
        } else {
896
            if (last_unit_status2 & ILABS_UNIT_STATUS2_ACCEL_Z_HIGH) {
897
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Z-acceleration is in range");
898
            }
899
        }
900

901
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_MAGCAL_2D_ACT) {
902
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Automatic 2D calibration is in progress");
903
        }
904

905
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_MAGCAL_3D_ACT) {
906
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Automatic 3D calibration is in progress");
907
        }
908

909
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_FUSION_OFF) {
910
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS input switched off");
911
        } else {
912
            if (last_unit_status2 & ILABS_UNIT_STATUS2_GNSS_FUSION_OFF) {
913
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS input switched on");
914
            }
915
        }
916

917
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF) {
918
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Diff. pressure input switched off");
919
        } else {
920
            if (last_unit_status2 & ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF) {
921
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Diff. pressure input switched on");
922
            }
923
        }
924

925
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_MAG_FUSION_OFF) {
926
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Magnetometer input switched off");
927
        } else {
928
            if (last_unit_status2 & ILABS_UNIT_STATUS2_MAG_FUSION_OFF) {
929
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Magnetometer input switched on");
930
            }
931
        }
932

933
        if (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) {
934
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Incorrect GNSS position");
935
        } else {
936
            if (last_unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) {
937
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS position is correct");
938
            }
939
        }
940

941
        last_unit_status2 = state2.unit_status2;
942
    }
943

944
    // InertialLabs INS Air Data Unit (ADU) status messages to GCS
945
    if (last_air_data_status != state2.air_data_status) {
946
        if (state2.air_data_status & ILABS_AIRDATA_STATIC_PRESS_RANGE_ERR) {
947
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Static pressure is out of range");
948
        } else {
949
            if (last_air_data_status & ILABS_AIRDATA_STATIC_PRESS_RANGE_ERR) {
950
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Static pressure is in range");
951
            }
952
        }
953

954
        if (state2.air_data_status & ILABS_AIRDATA_DIFF_PRESS_RANGE_ERR) {
955
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Diff. pressure is out of range");
956
        } else {
957
            if (last_air_data_status & ILABS_AIRDATA_DIFF_PRESS_RANGE_ERR) {
958
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Diff. pressure is in range");
959
            }
960
        }
961

962
        if (state2.air_data_status & ILABS_AIRDATA_PRESS_ALT_FAIL) {
963
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Pressure altitude is incorrect");
964
        } else {
965
            if (last_air_data_status & ILABS_AIRDATA_PRESS_ALT_FAIL) {
966
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Pressure altitude is correct");
967
            }
968
        }
969

970
        if (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) {
971
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Air speed is incorrect");
972
        } else {
973
            if (last_air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) {
974
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Air speed is correct");
975
            }
976
        }
977

978
        if (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) {
979
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Air speed is below the threshold");
980
        } else {
981
            if (last_air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL) {
982
                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: Air speed is above the threshold");
983
            }
984
        }
985

986
        last_air_data_status = state2.air_data_status;
987
    }
988

989
    // InertialLabs INS spoofing detection messages to GCS
990
    if (last_spoof_status != gnss_data.spoof_status) {
991
        if ((last_spoof_status == 2 || last_spoof_status == 3) && (gnss_data.spoof_status == 1)) {
992
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS no spoofing");
993
        }
994

995
        if (last_spoof_status == 2) {
996
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: GNSS spoofing indicated");
997
        }
998

999
        if (last_spoof_status == 3) {
1000
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: GNSS multiple spoofing indicated");
1001
        }
1002

1003
        last_spoof_status = gnss_data.spoof_status;
1004
    }
1005

1006
    // InertialLabs INS jamming detection messages to GCS
1007
    if (last_jam_status != gnss_data.jam_status) {
1008
        if ((last_jam_status == 2 || last_jam_status == 3) && (gnss_data.jam_status == 1)) {
1009
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ILAB: GNSS no jamming");
1010
        }
1011

1012
        if (gnss_data.jam_status == 3) {
1013
            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: GNSS jamming indicated and no fix");
1014
        }
1015

1016
        last_jam_status = gnss_data.jam_status;
1017
    }
1018

1019
    return true;
1020
}
1021

1022
void AP_ExternalAHRS_InertialLabs::update_thread()
1023
{
1024
    // Open port in the thread
1025
    uart->begin(baudrate, 1024, 512);
1026

1027
    /*
1028
      we assume the user has already configured the device
1029
     */
1030

1031
    setup_complete = true;
1032
    while (true) {
1033
        if (!check_uart()) {
1034
            hal.scheduler->delay_microseconds(250);
1035
        }
1036
    }
1037
}
1038

1039
// get serial port number for the uart
1040
int8_t AP_ExternalAHRS_InertialLabs::get_port(void) const
1041
{
1042
    if (!uart) {
1043
        return -1;
1044
    }
1045
    return port_num;
1046
};
1047

1048
// accessors for AP_AHRS
1049
bool AP_ExternalAHRS_InertialLabs::healthy(void) const
1050
{
1051
    WITH_SEMAPHORE(state.sem);
1052
    return AP_HAL::millis() - last_att_ms < 100;
1053
}
1054

1055
bool AP_ExternalAHRS_InertialLabs::initialised(void) const
1056
{
1057
    if (!setup_complete) {
1058
        return false;
1059
    }
1060
    return true;
1061
}
1062

1063
bool AP_ExternalAHRS_InertialLabs::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
1064
{
1065
    if (!setup_complete) {
1066
        hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs setup failed");
1067
        return false;
1068
    }
1069
    if (!healthy()) {
1070
        hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs unhealthy");
1071
        return false;
1072
    }
1073
    WITH_SEMAPHORE(state.sem);
1074
    uint32_t now = AP_HAL::millis();
1075
    if (now - last_att_ms > 10 ||
1076
        now - last_pos_ms > 10 ||
1077
        now - last_vel_ms > 10) {
1078
        hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs not up to date");
1079
        return false;
1080
    }
1081
    return true;
1082
}
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
void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status) const
1089
{
1090
    WITH_SEMAPHORE(state.sem);
1091
    uint32_t now = AP_HAL::millis();
1092
    const uint32_t dt_limit = 200;
1093
    const uint32_t dt_limit_gps = 500;
1094
    memset(&status, 0, sizeof(status));
1095
    const bool init_ok = (state2.unit_status & (ILABS_UNIT_STATUS_ALIGNMENT_FAIL|ILABS_UNIT_STATUS_OPERATION_FAIL))==0;
1096
    status.flags.initalized = init_ok;
1097
    status.flags.attitude = init_ok && (now - last_att_ms < dt_limit) && init_ok;
1098
    status.flags.vert_vel = init_ok && (now - last_vel_ms < dt_limit);
1099
    status.flags.vert_pos = init_ok && (now - last_pos_ms < dt_limit);
1100
    status.flags.horiz_vel = status.flags.vert_vel;
1101
    status.flags.horiz_pos_abs = status.flags.vert_pos;
1102
    status.flags.horiz_pos_rel = status.flags.horiz_pos_abs;
1103
    status.flags.pred_horiz_pos_rel = status.flags.horiz_pos_abs;
1104
    status.flags.pred_horiz_pos_abs = status.flags.horiz_pos_abs;
1105
    status.flags.using_gps = (now - last_gps_ms < dt_limit_gps) &&
1106
        ((state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) | (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_FUSION_OFF)) == 0;
1107
    status.flags.gps_quality_good = (now - last_gps_ms < dt_limit_gps) &&
1108
        (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) != 0 &&
1109
        (state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) == 0;
1110
    status.flags.rejecting_airspeed = (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL);
1111
}
1112

1113
// get variances
1114
bool AP_ExternalAHRS_InertialLabs::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
1115
{
1116
    velVar = state2.kf_vel_covariance.length() * vel_gate_scale;
1117
    posVar = state2.kf_pos_covariance.xy().length() * pos_gate_scale;
1118
    hgtVar = state2.kf_pos_covariance.z * hgt_gate_scale;
1119
    tasVar = 0;
1120
    return true;
1121
}
1122

1123
#endif  // AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
1124

1125

1126
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
