CoCalc -- AP_ADSB_uAvionix

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_ADSB/AP_ADSB_uAvionix_UCP.cpp
Views: ¹⁷⁹⁸
1
/*
2
   Copyright (C) 2021  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
   Author: Tom Pittenger
18
 */
19

20
#include "AP_ADSB_uAvionix_UCP.h"
21

22
// This driver implements the UCP protocol from uAvionix which is a variant of the GDL90 protocol by Garmin
23
// https://uavionix.com/downloads/ping200X/uAvionix-UCP-Transponder-ICD-Rev-Q.pdf
24

25
#if HAL_ADSB_UCP_ENABLED
26

27
#include <AP_SerialManager/AP_SerialManager.h>
28
#include <GCS_MAVLink/GCS.h>
29
#include <AP_HAL/AP_HAL.h>
30
#include <AP_Math/AP_Math.h>
31
#include <AP_Math/crc.h>
32
#include <ctype.h>
33
#include <AP_Notify/AP_Notify.h>
34

35
#include <AP_GPS/AP_GPS.h>
36

37
extern const AP_HAL::HAL &hal;
38

39
#define AP_ADSB_UAVIONIX_HEALTH_TIMEOUT_MS                     (5000UL)
40

41
#define AP_ADSB_UAVIONIX_GCS_LOST_COMMS_LONG_TIMEOUT_MINUTES    (15UL)
42
#define AP_ADSB_UAVIONIX_GCS_LOST_COMMS_LONG_TIMEOUT_MS         (1000UL * 60UL * AP_ADSB_UAVIONIX_GCS_LOST_COMMS_LONG_TIMEOUT_MINUTES)
43

44
#define AP_ADSB_UAVIONIX_DETECT_GROUNDSTATE                     0
45
#define AP_ADSB_UAVIONIX_EMERGENCY_STATUS_ON_LOST_LINK          0
46

47
// detect if any port is configured as uAvionix_UCP
48
bool AP_ADSB_uAvionix_UCP::detect()
49
{
50
    return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
51
}
52

53

54
// Init, called once after class is constructed
55
bool AP_ADSB_uAvionix_UCP::init()
56
{
57
    _port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
58
    if (_port == nullptr) {
59
        return false;
60
    }
61

62
    _frontend.out_state.ctrl.squawkCode = 1200;
63
    _frontend.out_state.tx_status.squawk = 1200;
64
    _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
65

66
    return true;
67
}
68

69

70
void AP_ADSB_uAvionix_UCP::update()
71
{
72
    if (_port == nullptr) {
73
        return;
74
    }
75

76
    const uint32_t now_ms = AP_HAL::millis();
77

78
    // -----------------------------
79
    // read any available data on serial port
80
    // -----------------------------
81
    uint32_t nbytes = MIN(_port->available(), 10UL * GDL90_RX_MAX_PACKET_LENGTH);
82
    while (nbytes-- > 0) {
83
        uint8_t data;
84
        if (!_port->read(data)) {
85
            break;
86
        }
87
        if (parseByte(data, rx.msg, rx.status)) {
88
            rx.last_msg_ms = now_ms;
89
            handle_msg(rx.msg);
90
        }
91
    } // while nbytes
92

93

94
    if (run_state.last_packet_Transponder_Id_ms == 0 && run_state.request_Transponder_Id_tries < 5) {
95
        if (now_ms - run_state.last_packet_Request_Transponder_Id_ms >= 1000)
96
        {
97
            request_msg(GDL90_ID_IDENTIFICATION);
98
            run_state.request_Transponder_Id_tries++;
99
        }
100
    }
101

102
    if (run_state.last_packet_Transponder_Config_ms == 0 && run_state.request_Transponder_Config_tries < 5) {
103
        if (now_ms - run_state.last_packet_Request_Transponder_Config_ms >= 1000)
104
        {
105
            request_msg(GDL90_ID_TRANSPONDER_CONFIG);
106
            run_state.request_Transponder_Config_tries++;
107
        }
108
    }
109

110
   if (now_ms - run_state.last_packet_Transponder_Control_ms >= 1000) {
111
        run_state.last_packet_Transponder_Control_ms = now_ms;
112

113
        // We want to use the defaults stored on the ping200X, if possible.
114
        // Until we get the config message (or we've tried requesting it several times),
115
        // don't send the control message.
116
        if (run_state.last_packet_Transponder_Config_ms != 0 || run_state.request_Transponder_Config_tries >= 5) {
117
            send_Transponder_Control();
118
        }
119
    }
120

121
    if ((now_ms - run_state.last_packet_GPS_ms >= 200) && (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Ping200X_Send_GPS)) != 0) {
122
        run_state.last_packet_GPS_ms = now_ms;
123
        send_GPS_Data();
124
    }
125

126
    // if the transponder has stopped giving us the data needed to
127
    // fill the transponder status mavlink message, indicate status unavailable.
128
    if ((now_ms - run_state.last_packet_Transponder_Status_ms >= 10000)
129
        && (now_ms - run_state.last_packet_Transponder_Heartbeat_ms >= 10000)
130
        && (now_ms - run_state.last_packet_Transponder_Ownship_ms >= 10000)) {
131
        _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
132
    }
133
}
134

135

136
void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg)
137
{
138
    switch(msg.messageId) {
139
    case GDL90_ID_HEARTBEAT: {
140
        // The Heartbeat message provides real-time indications of the status and operation of the
141
        // transponder. The message will be transmitted with a period of one second for the UCP
142
        // protocol.
143
        memcpy(&rx.decoded.heartbeat, msg.raw, sizeof(rx.decoded.heartbeat));
144
        run_state.last_packet_Transponder_Heartbeat_ms = AP_HAL::millis();
145
        _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
146

147
        if (rx.decoded.heartbeat.status.one.maintenanceRequired) {
148
            _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_MAINT_REQ;
149
        } else {
150
            _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_MAINT_REQ;
151
        }
152

153
        if (rx.decoded.heartbeat.status.two.functionFailureGnssUnavailable) {
154
            _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_UNAVAIL;
155
        } else {
156
            _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_UNAVAIL;
157
        }
158

159
        if (rx.decoded.heartbeat.status.two.functionFailureGnssNo3dFix) {
160
            _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_NO_POS;
161
        } else {
162
            _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_NO_POS;
163
        }
164

165
        if (rx.decoded.heartbeat.status.two.functionFailureTransmitSystem) {
166
            _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;
167
        } else {
168
            _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;
169
        }
170
        }
171
        break;
172

173
    case GDL90_ID_IDENTIFICATION:
174
        run_state.last_packet_Transponder_Id_ms = AP_HAL::millis();
175
        // The Identification message contains information used to identify the connected device. The
176
        // Identification message will be transmitted with a period of one second regardless of data status
177
        // or update for the UCP protocol and will be transmitted upon request for the UCP-HD protocol.
178
        if (memcmp(&rx.decoded.identification, msg.raw, sizeof(rx.decoded.identification)) != 0) {
179
            memcpy(&rx.decoded.identification, msg.raw, sizeof(rx.decoded.identification));
180

181
            // Firmware Part Number (not null terminated, but null padded if part number is less than 15 characters).
182
            // Copy into a temporary string that is 1 char longer so we ensure it's null terminated
183
            const uint8_t str_len = sizeof(rx.decoded.identification.primaryFwPartNumber);
184
            char primaryFwPartNumber[str_len+1];
185
            memcpy(&primaryFwPartNumber, rx.decoded.identification.primaryFwPartNumber, str_len);
186
            primaryFwPartNumber[str_len] = 0;
187

188
            GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"ADSB:Detected %s v%u.%u.%u SN:%u %s",
189
                get_hardware_name(rx.decoded.identification.primary.hwId),
190
                (unsigned)rx.decoded.identification.primary.fwMajorVersion,
191
                (unsigned)rx.decoded.identification.primary.fwMinorVersion,
192
                (unsigned)rx.decoded.identification.primary.fwBuildVersion,
193
                (unsigned)rx.decoded.identification.primary.serialNumber,
194
                primaryFwPartNumber);
195
        }
196
        break;
197

198
    case GDL90_ID_TRANSPONDER_CONFIG:
199
        run_state.last_packet_Transponder_Config_ms = AP_HAL::millis();
200
        memcpy(&rx.decoded.transponder_config, msg.raw, sizeof(rx.decoded.transponder_config));
201
        break;
202

203
#if AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS
204
    case GDL90_ID_OWNSHIP_REPORT:
205
        _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
206
        // The Ownship message contains information on the GNSS position. If the Ownship GNSS
207
        // position fix is invalid, the Latitude, Longitude, and NIC fields will all have the ZERO value. The
208
        // Ownship message will be transmitted with a period of one second regardless of data status or
209
        // update for the UCP protocol. All fields in the ownship message are transmitted MSB first
210
        memcpy(&rx.decoded.ownship_report, msg.raw, sizeof(rx.decoded.ownship_report));
211
        run_state.last_packet_Transponder_Ownship_ms = AP_HAL::millis();
212
        _frontend.out_state.tx_status.NIC_NACp = rx.decoded.ownship_report.report.NIC | (rx.decoded.ownship_report.report.NACp << 4);
213
        memcpy(_frontend.out_state.tx_status.flight_id, rx.decoded.ownship_report.report.callsign, sizeof(_frontend.out_state.tx_status.flight_id));
214
        break;
215

216
    case GDL90_ID_OWNSHIP_GEOMETRIC_ALTITUDE:
217
        // An Ownship Geometric Altitude message will be transmitted with a period of one second when
218
        // the GNSS fix is valid for the UCP protocol. All fields in the Geometric Ownship Altitude
219
        // message are transmitted MSB first.
220
        memcpy(&rx.decoded.ownship_geometric_altitude, msg.raw, sizeof(rx.decoded.ownship_geometric_altitude));
221
        break;
222

223
    case GDL90_ID_SENSOR_MESSAGE:
224
        memcpy(&rx.decoded.sensor_message, msg.raw, sizeof(rx.decoded.sensor_message));
225
        break;
226

227
    case GDL90_ID_TRANSPONDER_STATUS:
228
    {
229
        _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
230
        switch (msg.payload[0]) {
231
        case 1: {
232
            // version 1 of the transponder status message is sent at 1 Hz (if UCP protocol out is enabled on the transponder)
233
            memcpy(&rx.decoded.transponder_status, msg.raw, sizeof(rx.decoded.transponder_status));
234
            if (rx.decoded.transponder_status.identActive) {
235
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
236
            } else {
237
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
238
            }
239

240
            if (rx.decoded.transponder_status.modeAEnabled) {
241
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED;
242
            } else {
243
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED;
244
            }
245

246
            if (rx.decoded.transponder_status.modeCEnabled) {
247
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED;
248
            } else {
249
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED;
250
            }
251

252
            if (rx.decoded.transponder_status.modeSEnabled) {
253
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED;
254
            } else {
255
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED;
256
            }
257

258
            if (rx.decoded.transponder_status.es1090TxEnabled) {
259
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED;
260
            } else {
261
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED;
262
            }
263

264
            if (rx.decoded.transponder_status.x_bit) {
265
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED;
266
            } else {
267
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED;
268
            }
269

270
            _frontend.out_state.tx_status.squawk = rx.decoded.transponder_status.squawkCode;
271

272
            if (run_state.last_packet_Transponder_Status_ms == 0 && run_state.last_packet_Transponder_Config_ms == 0) {
273
                // If this is the first time we've seen a status message,
274
                // and we haven't initialized the control message from the config message,
275
                // set initial control message contents to match transponder's current behavior.
276
                _frontend.out_state.ctrl.modeAEnabled = rx.decoded.transponder_status.modeAEnabled;
277
                _frontend.out_state.ctrl.modeCEnabled = rx.decoded.transponder_status.modeCEnabled;
278
                _frontend.out_state.ctrl.modeSEnabled = rx.decoded.transponder_status.modeSEnabled;
279
                _frontend.out_state.ctrl.es1090TxEnabled = rx.decoded.transponder_status.es1090TxEnabled;
280
                _frontend.out_state.ctrl.squawkCode = rx.decoded.transponder_status.squawkCode;
281
                _frontend.out_state.ctrl.x_bit = rx.decoded.transponder_status.x_bit;
282
            }
283
            run_state.last_packet_Transponder_Status_ms = AP_HAL::millis();
284
#if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED
285
            GCS_SEND_MESSAGE(MSG_UAVIONIX_ADSB_OUT_STATUS);
286
            run_state.last_gcs_send_message_Transponder_Status_ms = AP_HAL::millis();
287
#endif
288
            break;
289
        }
290
        case 2:
291
            // deprecated
292
            break;
293
        case 3: {
294
            // Version 3 of the transponder status message is sent in response to the transponder control message (if UCP-HD protocol out is enabled on the transponder)
295
            memcpy(&rx.decoded.transponder_status_v3, msg.raw, sizeof(rx.decoded.transponder_status_v3));
296

297
            if (rx.decoded.transponder_status_v3.indicatingOnGround) {
298
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_ON_GROUND;
299
            } else {
300
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_ON_GROUND;
301
            }
302

303
            if (rx.decoded.transponder_status_v3.fault) {
304
                // unsure what fault is indicated, query heartbeat for more info
305
                request_msg(GDL90_ID_HEARTBEAT);
306
            }
307

308
            if (rx.decoded.transponder_status_v3.identActive) {
309
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
310
            } else {
311
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
312
            }
313

314
            if (rx.decoded.transponder_status_v3.modeAEnabled) {
315
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED;
316
            } else {
317
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED;
318
            }
319

320
            if (rx.decoded.transponder_status_v3.modeCEnabled) {
321
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED;
322
            } else {
323
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED;
324
            }
325

326
            if (rx.decoded.transponder_status_v3.modeSEnabled) {
327
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED;
328
            } else {
329
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED;
330
            }
331

332
            if (rx.decoded.transponder_status_v3.es1090TxEnabled) {
333
                _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED;
334
            } else {
335
                _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED;
336
            }
337

338
            _frontend.out_state.tx_status.squawk = rx.decoded.transponder_status_v3.squawkCode;
339
            _frontend.out_state.tx_status.NIC_NACp = rx.decoded.transponder_status_v3.NIC | (rx.decoded.transponder_status_v3.NACp << 4);
340
            _frontend.out_state.tx_status.boardTemp = rx.decoded.transponder_status_v3.temperature;
341

342
            if (run_state.last_packet_Transponder_Status_ms == 0 && run_state.last_packet_Transponder_Config_ms == 0) {
343
                // If this is the first time we've seen a status message,
344
                // and we haven't initialized the control message from the config message,
345
                // set initial control message contents to match transponder's current behavior.
346
                _frontend.out_state.ctrl.modeAEnabled = rx.decoded.transponder_status_v3.modeAEnabled;
347
                _frontend.out_state.ctrl.modeCEnabled = rx.decoded.transponder_status_v3.modeCEnabled;
348
                _frontend.out_state.ctrl.modeSEnabled = rx.decoded.transponder_status_v3.modeSEnabled;
349
                _frontend.out_state.ctrl.es1090TxEnabled = rx.decoded.transponder_status_v3.es1090TxEnabled;
350
                _frontend.out_state.ctrl.squawkCode = rx.decoded.transponder_status_v3.squawkCode;
351
            }
352
            run_state.last_packet_Transponder_Status_ms = AP_HAL::millis();
353
#if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED
354
            GCS_SEND_MESSAGE(MSG_UAVIONIX_ADSB_OUT_STATUS);
355
            run_state.last_gcs_send_message_Transponder_Status_ms = AP_HAL::millis();
356
#endif
357
            break;
358
        }
359
        default:
360
            break;
361
        }
362
        break;
363
    }
364
#endif // AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS
365

366
    case GDL90_ID_TRANSPONDER_CONTROL:
367
    case GDL90_ID_GPS_DATA:
368
    case GDL90_ID_MESSAGE_REQUEST:
369
        // not handled, outbound only
370
        break;
371
    default:
372
        //GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"ADSB:Unknown msg %d", (int)msg.messageId);
373
        break;
374
    }
375
}
376

377

378
const char* AP_ADSB_uAvionix_UCP::get_hardware_name(const uint8_t hwId)
379
{
380
    switch(hwId) {
381
        case 0x09: return "Ping200s";
382
        case 0x0A: return "Ping20s";
383
        case 0x18: return "Ping200C";
384
        case 0x27: return "Ping20Z";
385
        case 0x2D: return "SkyBeaconX";             // (certified)
386
        case 0x26: //return "Ping200Z/Ping200X";    // (uncertified). Let's fallthrough and use Ping200X
387
        case 0x2F: return "Ping200X";               // (certified)
388
        case 0x30: return "TailBeaconX";            // (certified)
389
    } // switch hwId
390
    return "Unknown HW";
391
}
392

393
void AP_ADSB_uAvionix_UCP::send_Transponder_Control()
394
{
395
    GDL90_TRANSPONDER_CONTROL_MSG msg {};
396
    msg.messageId = GDL90_ID_TRANSPONDER_CONTROL;
397
    msg.version = GDL90_TRANSPONDER_CONTROL_VERSION;
398

399
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
400
    // when using the simulator, always declare we're on the ground to help
401
    // inhibit chaos if this ias actually being broadcasted on real hardware
402
    msg.airGroundState =  ADSB_ON_GROUND;
403
#elif AP_ADSB_UAVIONIX_DETECT_GROUNDSTATE
404
    msg.airGroundState =  _frontend.out_state.is_flying ? ADSB_AIRBORNE_SUBSONIC : ADSB_ON_GROUND;
405
#else
406
    msg.airGroundState = ADSB_AIRBORNE_SUBSONIC;
407
#endif
408

409
    msg.baroCrossChecked = ADSB_NIC_BARO_UNVERIFIED;
410
    msg.identActive = _frontend.out_state.ctrl.identActive;
411
    _frontend.out_state.ctrl.identActive = false; // only send identButtonActive once per request
412
    msg.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled;
413
    msg.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled;
414
    msg.modeSEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.modeSEnabled;
415
    msg.es1090TxEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.es1090TxEnabled;
416

417
    // if enabled via param ADSB_OPTIONS, use squawk 7400 while in any Loss-Comms related failsafe
418
    // https://www.faa.gov/documentLibrary/media/Notice/N_JO_7110.724_5-2-9_UAS_Lost_Link_2.pdf
419
    const AP_Notify& notify = AP::notify();
420
    if (((_frontend._options & uint32_t(AP_ADSB::AdsbOption::Squawk_7400_FS_RC)) && notify.flags.failsafe_radio) ||
421
        ((_frontend._options & uint32_t(AP_ADSB::AdsbOption::Squawk_7400_FS_GCS)) && notify.flags.failsafe_gcs)) {
422
        msg.squawkCode = 7400;
423
    } else {
424
        msg.squawkCode = _frontend.out_state.ctrl.squawkCode;
425
    }
426

427
#if AP_ADSB_UAVIONIX_EMERGENCY_STATUS_ON_LOST_LINK
428
    const uint32_t last_gcs_ms = gcs().sysid_myggcs_last_seen_time_ms();
429
    const bool gcs_lost_comms = (last_gcs_ms != 0) && (AP_HAL::millis() - last_gcs_ms > AP_ADSB_UAVIONIX_GCS_LOST_COMMS_LONG_TIMEOUT_MS);
430
    msg.emergencyState = gcs_lost_comms ? ADSB_EMERGENCY_STATUS::ADSB_EMERGENCY_UAS_LOST_LINK : ADSB_EMERGENCY_STATUS::ADSB_EMERGENCY_NONE;
431
#else
432
    msg.emergencyState = ADSB_EMERGENCY_STATUS::ADSB_EMERGENCY_NONE;
433
#endif
434

435
#if GDL90_TRANSPONDER_CONTROL_VERSION == 2
436
    msg.x_bit = 0;
437
#endif
438

439
    memcpy(msg.callsign, _frontend.out_state.ctrl.callsign, sizeof(msg.callsign));
440

441
    gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg));
442
}
443

444
void AP_ADSB_uAvionix_UCP::send_GPS_Data()
445
{
446
    GDL90_GPS_DATA_V2 msg {};
447
    msg.messageId = GDL90_ID_GPS_DATA;
448
    msg.version = 2;
449

450
    const AP_ADSB::Loc &gps { _frontend._my_loc };
451

452
    const GPS_FIX fix = (GPS_FIX)gps.status();
453
    const bool fix_is_good = (fix >= GPS_FIX_3D);
454
    const Vector3f velocity = fix_is_good ? gps.velocity() : Vector3f();
455

456
    msg.utcTime_s = gps.time_epoch_usec() * 1E-6;
457
    msg.latitude_ddE7 = fix_is_good ? _frontend._my_loc.lat : INT32_MAX;
458
    msg.longitude_ddE7 = fix_is_good ? _frontend._my_loc.lng : INT32_MAX;
459
    msg.altitudeGnss_mm = fix_is_good ? (_frontend._my_loc.alt * 10): INT32_MAX;
460

461
    // Protection Limits. FD or SBAS-based depending on state bits
462
    // Estimate HPL based on horizontal accuracy/HFOM to a probability of 10^-7:
463
    //  Using the central limit theorem for a Gaussian distribution,
464
    //  this is 5.326724*stdDev.
465
    //  Conservatively round up to 6 as a scaling factor,
466
    //  and asssume HFOM of 95% was calculated as 2*stdDev*HDOP.
467
    //  This yields a factor of 3 to estimate HPL from horizontal accuracy.
468
    float accHoriz;
469
    bool gotAccHoriz = gps.horizontal_accuracy(accHoriz);
470
    msg.HPL_mm = gotAccHoriz ? 3 * accHoriz * 1E3 : UINT32_MAX; // required to calculate NIC
471
    msg.VPL_cm = UINT32_MAX; // unused by ping200X
472

473
    // Figure of Merits
474
    msg.horizontalFOM_mm = gotAccHoriz ? accHoriz * 1E3 : UINT32_MAX;
475
    float accVert;
476
    msg.verticalFOM_cm = gps.vertical_accuracy(accVert) ? accVert * 1E2 : UINT16_MAX;
477
    float accVel;
478
    msg.horizontalVelocityFOM_mmps = gps.speed_accuracy(accVel) ? accVel * 1E3 : UINT16_MAX;
479
    msg.verticalVelocityFOM_mmps = msg.horizontalVelocityFOM_mmps;
480

481
    // Velocities
482
    msg.verticalVelocity_cmps = fix_is_good ? -1.0f * velocity.z * 1E2 : INT16_MAX;
483
    msg.northVelocity_mmps = fix_is_good ? velocity.x * 1E3 : INT32_MAX;
484
    msg.eastVelocity_mmps = fix_is_good ? velocity.y * 1E3 : INT32_MAX;
485

486
    // State
487
    msg.fixType = fix;
488

489
    GDL90_GPS_NAV_STATE nav_state {};
490
    nav_state.HPLfdeActive = 1;
491
    nav_state.fault = 0;
492
    nav_state.HrdMagNorth = 0;  // 1 means "north" is magnetic north
493

494
    msg.navState = nav_state;
495
    msg.satsUsed = gps.num_sats();
496

497
    gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg));
498
}
499

500

501
bool AP_ADSB_uAvionix_UCP::hostTransmit(uint8_t *buffer, uint16_t length)
502
{
503
    if (_port == nullptr || _port->txspace() < length) {
504
      return false;
505
    }
506
    _port->write(buffer, length);
507
    return true;
508
}
509

510

511
bool AP_ADSB_uAvionix_UCP::request_msg(const GDL90_MESSAGE_ID msg_id)
512
{
513
    const GDL90_TRANSPONDER_MESSAGE_REQUEST_V2 msg = {
514
      messageId : GDL90_ID_MESSAGE_REQUEST,
515
      version   : 2,
516
      reqMsgId  : msg_id
517
    };
518
    return gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg)) != 0;
519
}
520

521

522
uint16_t AP_ADSB_uAvionix_UCP::gdl90Transmit(GDL90_TX_MESSAGE &message, const uint16_t length)
523
{
524
    uint8_t gdl90FrameBuffer[GDL90_TX_MAX_FRAME_LENGTH] {};
525

526
    const uint16_t frameCrc = crc16_ccitt_GDL90((uint8_t*)&message.raw, length, 0);
527

528
    // Set flag byte in frame buffer
529
    gdl90FrameBuffer[0] = GDL90_FLAG_BYTE;
530
    uint16_t frameIndex = 1;
531

532
    // Copy and stuff all payload bytes into frame buffer
533
    for (uint16_t i = 0; i < length+2; i++) {
534
        // Check for overflow of frame buffer
535
        if (frameIndex >= GDL90_TX_MAX_FRAME_LENGTH) {
536
            return 0;
537
        }
538

539
        uint8_t data;
540
        // Append CRC to payload
541
        if (i == length) {
542
            data = LOWBYTE(frameCrc);
543
        } else if (i == length+1) {
544
            data = HIGHBYTE(frameCrc);
545
        } else {
546
            data = message.raw[i];
547
        }
548

549
        if (data == GDL90_FLAG_BYTE || data == GDL90_CONTROL_ESCAPE_BYTE) {
550
            // Check for frame buffer overflow on stuffed byte
551
            if (frameIndex + 2 > GDL90_TX_MAX_FRAME_LENGTH) {
552
              return 0;
553
            }
554

555
            // Set control break and stuff this byte
556
            gdl90FrameBuffer[frameIndex++] = GDL90_CONTROL_ESCAPE_BYTE;
557
            gdl90FrameBuffer[frameIndex++] = data ^ GDL90_STUFF_BYTE;
558
        } else {
559
            gdl90FrameBuffer[frameIndex++] = data;
560
        }
561
    }
562

563
    // Add end of frame indication
564
    gdl90FrameBuffer[frameIndex++] = GDL90_FLAG_BYTE;
565

566
    // Push packet to UART
567
    if (hostTransmit(gdl90FrameBuffer, frameIndex)) {
568
        return frameIndex;
569
    }
570

571
    return 0;
572
}
573

574

575
bool AP_ADSB_uAvionix_UCP::parseByte(const uint8_t data, GDL90_RX_MESSAGE &msg, GDL90_RX_STATUS &status)
576
{
577
    switch (status.state)
578
    {
579
    case GDL90_RX_IDLE:
580
        if (data == GDL90_FLAG_BYTE && status.prev_data == GDL90_FLAG_BYTE) {
581
            status.length = 0;
582
            status.state = GDL90_RX_IN_PACKET;
583
        }
584
        break;
585

586
    case GDL90_RX_IN_PACKET:
587
        if (data == GDL90_CONTROL_ESCAPE_BYTE) {
588
            status.state = GDL90_RX_UNSTUFF;
589

590
        } else if (data == GDL90_FLAG_BYTE) {
591
            // packet complete! Check CRC and restart packet cycle on all pass or fail scenarios
592
            status.state = GDL90_RX_IDLE;
593

594
            if (status.length < GDL90_OVERHEAD_LENGTH) {
595
                // something is wrong, there's no actual data
596
                return false;
597
            }
598

599
            const uint8_t crc_LSB = msg.raw[status.length - 2];
600
            const uint8_t crc_MSB = msg.raw[status.length - 1];
601

602
            // NOTE: status.length contains messageId, payload and CRC16. So status.length-3 is effective payload length
603
            msg.crc = (uint16_t)crc_LSB | ((uint16_t)crc_MSB << 8);
604
            const uint16_t crc = crc16_ccitt_GDL90((uint8_t*)&msg.raw, status.length-2, 0);
605
            if (crc == msg.crc) {
606
                status.prev_data = data;
607
                // NOTE: this is the only path that returns true
608
                return true;
609
            }
610

611
        } else if (status.length < GDL90_RX_MAX_PACKET_LENGTH) {
612
            msg.raw[status.length++] = data;
613

614
        } else {
615
            status.state = GDL90_RX_IDLE;
616
        }
617
        break;
618

619
    case GDL90_RX_UNSTUFF:
620
        msg.raw[status.length++] = data ^ GDL90_STUFF_BYTE;
621
        status.state = GDL90_RX_IN_PACKET;
622
        break;
623
    }
624
    status.prev_data = data;
625
    return false;
626
}
627

628
#endif // HAL_ADSB_UCP_ENABLED
629

630

631
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.
