Line data Source code
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 0 : bool AP_ADSB_uAvionix_UCP::detect()
49 : {
50 0 : return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
51 : }
52 :
53 :
54 : // Init, called once after class is constructed
55 0 : bool AP_ADSB_uAvionix_UCP::init()
56 : {
57 0 : _port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
58 0 : if (_port == nullptr) {
59 0 : return false;
60 : }
61 :
62 0 : _frontend.out_state.ctrl.squawkCode = 1200;
63 0 : _frontend.out_state.tx_status.squawk = 1200;
64 0 : _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
65 :
66 0 : return true;
67 : }
68 :
69 :
70 0 : void AP_ADSB_uAvionix_UCP::update()
71 : {
72 0 : if (_port == nullptr) {
73 0 : return;
74 : }
75 :
76 0 : const uint32_t now_ms = AP_HAL::millis();
77 :
78 : // -----------------------------
79 : // read any available data on serial port
80 : // -----------------------------
81 0 : uint32_t nbytes = MIN(_port->available(), 10UL * GDL90_RX_MAX_PACKET_LENGTH);
82 0 : while (nbytes-- > 0) {
83 : uint8_t data;
84 0 : if (!_port->read(data)) {
85 0 : break;
86 : }
87 0 : if (parseByte(data, rx.msg, rx.status)) {
88 0 : rx.last_msg_ms = now_ms;
89 0 : handle_msg(rx.msg);
90 : }
91 : } // while nbytes
92 :
93 :
94 0 : if (run_state.last_packet_Transponder_Id_ms == 0 && run_state.request_Transponder_Id_tries < 5) {
95 0 : if (now_ms - run_state.last_packet_Request_Transponder_Id_ms >= 1000)
96 : {
97 0 : request_msg(GDL90_ID_IDENTIFICATION);
98 0 : run_state.request_Transponder_Id_tries++;
99 : }
100 : }
101 :
102 0 : if (run_state.last_packet_Transponder_Config_ms == 0 && run_state.request_Transponder_Config_tries < 5) {
103 0 : if (now_ms - run_state.last_packet_Request_Transponder_Config_ms >= 1000)
104 : {
105 0 : request_msg(GDL90_ID_TRANSPONDER_CONFIG);
106 0 : run_state.request_Transponder_Config_tries++;
107 : }
108 : }
109 :
110 0 : if (now_ms - run_state.last_packet_Transponder_Control_ms >= 1000) {
111 0 : 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 0 : if (run_state.last_packet_Transponder_Config_ms != 0 || run_state.request_Transponder_Config_tries >= 5) {
117 0 : send_Transponder_Control();
118 : }
119 : }
120 :
121 0 : if ((now_ms - run_state.last_packet_GPS_ms >= 200) && (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Ping200X_Send_GPS)) != 0) {
122 0 : run_state.last_packet_GPS_ms = now_ms;
123 0 : 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 0 : if ((now_ms - run_state.last_packet_Transponder_Status_ms >= 10000)
129 0 : && (now_ms - run_state.last_packet_Transponder_Heartbeat_ms >= 10000)
130 0 : && (now_ms - run_state.last_packet_Transponder_Ownship_ms >= 10000)) {
131 0 : _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
132 : }
133 : }
134 :
135 :
136 0 : void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg)
137 : {
138 0 : switch(msg.messageId) {
139 0 : 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 0 : memcpy(&rx.decoded.heartbeat, msg.raw, sizeof(rx.decoded.heartbeat));
144 0 : run_state.last_packet_Transponder_Heartbeat_ms = AP_HAL::millis();
145 0 : _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
146 :
147 0 : if (rx.decoded.heartbeat.status.one.maintenanceRequired) {
148 0 : _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_MAINT_REQ;
149 : } else {
150 0 : _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_MAINT_REQ;
151 : }
152 :
153 0 : if (rx.decoded.heartbeat.status.two.functionFailureGnssUnavailable) {
154 0 : _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_UNAVAIL;
155 : } else {
156 0 : _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_UNAVAIL;
157 : }
158 :
159 0 : if (rx.decoded.heartbeat.status.two.functionFailureGnssNo3dFix) {
160 0 : _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_NO_POS;
161 : } else {
162 0 : _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_NO_POS;
163 : }
164 :
165 0 : if (rx.decoded.heartbeat.status.two.functionFailureTransmitSystem) {
166 0 : _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;
167 : } else {
168 0 : _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;
169 : }
170 : }
171 0 : break;
172 :
173 0 : case GDL90_ID_IDENTIFICATION:
174 0 : 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 0 : if (memcmp(&rx.decoded.identification, msg.raw, sizeof(rx.decoded.identification)) != 0) {
179 0 : 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 0 : const uint8_t str_len = sizeof(rx.decoded.identification.primaryFwPartNumber);
184 : char primaryFwPartNumber[str_len+1];
185 0 : memcpy(&primaryFwPartNumber, rx.decoded.identification.primaryFwPartNumber, str_len);
186 0 : primaryFwPartNumber[str_len] = 0;
187 :
188 0 : 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 0 : break;
197 :
198 0 : case GDL90_ID_TRANSPONDER_CONFIG:
199 0 : run_state.last_packet_Transponder_Config_ms = AP_HAL::millis();
200 0 : memcpy(&rx.decoded.transponder_config, msg.raw, sizeof(rx.decoded.transponder_config));
201 0 : break;
202 :
203 : #if AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS
204 0 : case GDL90_ID_OWNSHIP_REPORT:
205 0 : _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 0 : memcpy(&rx.decoded.ownship_report, msg.raw, sizeof(rx.decoded.ownship_report));
211 0 : run_state.last_packet_Transponder_Ownship_ms = AP_HAL::millis();
212 0 : _frontend.out_state.tx_status.NIC_NACp = rx.decoded.ownship_report.report.NIC | (rx.decoded.ownship_report.report.NACp << 4);
213 0 : memcpy(_frontend.out_state.tx_status.flight_id, rx.decoded.ownship_report.report.callsign, sizeof(_frontend.out_state.tx_status.flight_id));
214 0 : break;
215 :
216 0 : 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 0 : memcpy(&rx.decoded.ownship_geometric_altitude, msg.raw, sizeof(rx.decoded.ownship_geometric_altitude));
221 0 : break;
222 :
223 0 : case GDL90_ID_SENSOR_MESSAGE:
224 0 : memcpy(&rx.decoded.sensor_message, msg.raw, sizeof(rx.decoded.sensor_message));
225 0 : break;
226 :
227 0 : case GDL90_ID_TRANSPONDER_STATUS:
228 : {
229 0 : _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL;
230 0 : switch (msg.payload[0]) {
231 0 : 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 0 : memcpy(&rx.decoded.transponder_status, msg.raw, sizeof(rx.decoded.transponder_status));
234 0 : if (rx.decoded.transponder_status.identActive) {
235 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
236 : } else {
237 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
238 : }
239 :
240 0 : if (rx.decoded.transponder_status.modeAEnabled) {
241 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED;
242 : } else {
243 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED;
244 : }
245 :
246 0 : if (rx.decoded.transponder_status.modeCEnabled) {
247 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED;
248 : } else {
249 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED;
250 : }
251 :
252 0 : if (rx.decoded.transponder_status.modeSEnabled) {
253 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED;
254 : } else {
255 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED;
256 : }
257 :
258 0 : if (rx.decoded.transponder_status.es1090TxEnabled) {
259 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED;
260 : } else {
261 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED;
262 : }
263 :
264 0 : if (rx.decoded.transponder_status.x_bit) {
265 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED;
266 : } else {
267 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED;
268 : }
269 :
270 0 : _frontend.out_state.tx_status.squawk = rx.decoded.transponder_status.squawkCode;
271 :
272 0 : 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 0 : _frontend.out_state.ctrl.modeAEnabled = rx.decoded.transponder_status.modeAEnabled;
277 0 : _frontend.out_state.ctrl.modeCEnabled = rx.decoded.transponder_status.modeCEnabled;
278 0 : _frontend.out_state.ctrl.modeSEnabled = rx.decoded.transponder_status.modeSEnabled;
279 0 : _frontend.out_state.ctrl.es1090TxEnabled = rx.decoded.transponder_status.es1090TxEnabled;
280 0 : _frontend.out_state.ctrl.squawkCode = rx.decoded.transponder_status.squawkCode;
281 0 : _frontend.out_state.ctrl.x_bit = rx.decoded.transponder_status.x_bit;
282 : }
283 0 : run_state.last_packet_Transponder_Status_ms = AP_HAL::millis();
284 : #if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED
285 0 : GCS_SEND_MESSAGE(MSG_UAVIONIX_ADSB_OUT_STATUS);
286 0 : run_state.last_gcs_send_message_Transponder_Status_ms = AP_HAL::millis();
287 : #endif
288 0 : break;
289 : }
290 0 : case 2:
291 : // deprecated
292 0 : break;
293 0 : 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 0 : memcpy(&rx.decoded.transponder_status_v3, msg.raw, sizeof(rx.decoded.transponder_status_v3));
296 :
297 0 : if (rx.decoded.transponder_status_v3.indicatingOnGround) {
298 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_ON_GROUND;
299 : } else {
300 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_ON_GROUND;
301 : }
302 :
303 0 : if (rx.decoded.transponder_status_v3.fault) {
304 : // unsure what fault is indicated, query heartbeat for more info
305 0 : request_msg(GDL90_ID_HEARTBEAT);
306 : }
307 :
308 0 : if (rx.decoded.transponder_status_v3.identActive) {
309 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
310 : } else {
311 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE;
312 : }
313 :
314 0 : if (rx.decoded.transponder_status_v3.modeAEnabled) {
315 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED;
316 : } else {
317 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED;
318 : }
319 :
320 0 : if (rx.decoded.transponder_status_v3.modeCEnabled) {
321 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED;
322 : } else {
323 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED;
324 : }
325 :
326 0 : if (rx.decoded.transponder_status_v3.modeSEnabled) {
327 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED;
328 : } else {
329 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED;
330 : }
331 :
332 0 : if (rx.decoded.transponder_status_v3.es1090TxEnabled) {
333 0 : _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED;
334 : } else {
335 0 : _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED;
336 : }
337 :
338 0 : _frontend.out_state.tx_status.squawk = rx.decoded.transponder_status_v3.squawkCode;
339 0 : _frontend.out_state.tx_status.NIC_NACp = rx.decoded.transponder_status_v3.NIC | (rx.decoded.transponder_status_v3.NACp << 4);
340 0 : _frontend.out_state.tx_status.boardTemp = rx.decoded.transponder_status_v3.temperature;
341 :
342 0 : 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 0 : _frontend.out_state.ctrl.modeAEnabled = rx.decoded.transponder_status_v3.modeAEnabled;
347 0 : _frontend.out_state.ctrl.modeCEnabled = rx.decoded.transponder_status_v3.modeCEnabled;
348 0 : _frontend.out_state.ctrl.modeSEnabled = rx.decoded.transponder_status_v3.modeSEnabled;
349 0 : _frontend.out_state.ctrl.es1090TxEnabled = rx.decoded.transponder_status_v3.es1090TxEnabled;
350 0 : _frontend.out_state.ctrl.squawkCode = rx.decoded.transponder_status_v3.squawkCode;
351 : }
352 0 : run_state.last_packet_Transponder_Status_ms = AP_HAL::millis();
353 : #if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED
354 0 : GCS_SEND_MESSAGE(MSG_UAVIONIX_ADSB_OUT_STATUS);
355 0 : run_state.last_gcs_send_message_Transponder_Status_ms = AP_HAL::millis();
356 : #endif
357 0 : break;
358 : }
359 0 : default:
360 0 : break;
361 : }
362 0 : break;
363 : }
364 : #endif // AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS
365 :
366 0 : case GDL90_ID_TRANSPONDER_CONTROL:
367 : case GDL90_ID_GPS_DATA:
368 : case GDL90_ID_MESSAGE_REQUEST:
369 : // not handled, outbound only
370 0 : break;
371 0 : default:
372 : //GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"ADSB:Unknown msg %d", (int)msg.messageId);
373 0 : break;
374 : }
375 0 : }
376 :
377 :
378 0 : const char* AP_ADSB_uAvionix_UCP::get_hardware_name(const uint8_t hwId)
379 : {
380 0 : switch(hwId) {
381 0 : case 0x09: return "Ping200s";
382 0 : case 0x0A: return "Ping20s";
383 0 : case 0x18: return "Ping200C";
384 0 : case 0x27: return "Ping20Z";
385 0 : case 0x2D: return "SkyBeaconX"; // (certified)
386 0 : case 0x26: //return "Ping200Z/Ping200X"; // (uncertified). Let's fallthrough and use Ping200X
387 0 : case 0x2F: return "Ping200X"; // (certified)
388 0 : case 0x30: return "TailBeaconX"; // (certified)
389 : } // switch hwId
390 0 : return "Unknown HW";
391 : }
392 :
393 0 : void AP_ADSB_uAvionix_UCP::send_Transponder_Control()
394 : {
395 0 : GDL90_TRANSPONDER_CONTROL_MSG msg {};
396 0 : msg.messageId = GDL90_ID_TRANSPONDER_CONTROL;
397 0 : 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 0 : 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 0 : msg.airGroundState = ADSB_AIRBORNE_SUBSONIC;
407 : #endif
408 :
409 0 : msg.baroCrossChecked = ADSB_NIC_BARO_UNVERIFIED;
410 0 : msg.identActive = _frontend.out_state.ctrl.identActive;
411 0 : _frontend.out_state.ctrl.identActive = false; // only send identButtonActive once per request
412 0 : msg.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled;
413 0 : msg.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled;
414 0 : msg.modeSEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.modeSEnabled;
415 0 : 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 0 : const AP_Notify& notify = AP::notify();
420 0 : if (((_frontend._options & uint32_t(AP_ADSB::AdsbOption::Squawk_7400_FS_RC)) && notify.flags.failsafe_radio) ||
421 0 : ((_frontend._options & uint32_t(AP_ADSB::AdsbOption::Squawk_7400_FS_GCS)) && notify.flags.failsafe_gcs)) {
422 0 : msg.squawkCode = 7400;
423 : } else {
424 0 : 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_mygcs_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 0 : msg.emergencyState = ADSB_EMERGENCY_STATUS::ADSB_EMERGENCY_NONE;
433 : #endif
434 :
435 : #if GDL90_TRANSPONDER_CONTROL_VERSION == 2
436 0 : msg.x_bit = 0;
437 : #endif
438 :
439 0 : memcpy(msg.callsign, _frontend.out_state.ctrl.callsign, sizeof(msg.callsign));
440 :
441 0 : gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg));
442 0 : }
443 :
444 0 : void AP_ADSB_uAvionix_UCP::send_GPS_Data()
445 : {
446 0 : GDL90_GPS_DATA_V2 msg {};
447 0 : msg.messageId = GDL90_ID_GPS_DATA;
448 0 : msg.version = 2;
449 :
450 0 : const AP_ADSB::Loc &gps { _frontend._my_loc };
451 :
452 0 : const GPS_FIX fix = (GPS_FIX)gps.status();
453 0 : const bool fix_is_good = (fix >= GPS_FIX_3D);
454 0 : const Vector3f velocity = fix_is_good ? gps.velocity() : Vector3f();
455 :
456 0 : msg.utcTime_s = gps.time_epoch_usec() * 1E-6;
457 0 : msg.latitude_ddE7 = fix_is_good ? _frontend._my_loc.lat : INT32_MAX;
458 0 : msg.longitude_ddE7 = fix_is_good ? _frontend._my_loc.lng : INT32_MAX;
459 0 : 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 0 : bool gotAccHoriz = gps.horizontal_accuracy(accHoriz);
470 0 : msg.HPL_mm = gotAccHoriz ? 3 * accHoriz * 1E3 : UINT32_MAX; // required to calculate NIC
471 0 : msg.VPL_cm = UINT32_MAX; // unused by ping200X
472 :
473 : // Figure of Merits
474 0 : msg.horizontalFOM_mm = gotAccHoriz ? accHoriz * 1E3 : UINT32_MAX;
475 : float accVert;
476 0 : msg.verticalFOM_cm = gps.vertical_accuracy(accVert) ? accVert * 1E2 : UINT16_MAX;
477 : float accVel;
478 0 : msg.horizontalVelocityFOM_mmps = gps.speed_accuracy(accVel) ? accVel * 1E3 : UINT16_MAX;
479 0 : msg.verticalVelocityFOM_mmps = msg.horizontalVelocityFOM_mmps;
480 :
481 : // Velocities
482 0 : msg.verticalVelocity_cmps = fix_is_good ? -1.0f * velocity.z * 1E2 : INT16_MAX;
483 0 : msg.northVelocity_mmps = fix_is_good ? velocity.x * 1E3 : INT32_MAX;
484 0 : msg.eastVelocity_mmps = fix_is_good ? velocity.y * 1E3 : INT32_MAX;
485 :
486 : // State
487 0 : msg.fixType = fix;
488 :
489 0 : GDL90_GPS_NAV_STATE nav_state {};
490 0 : nav_state.HPLfdeActive = 1;
491 0 : nav_state.fault = 0;
492 0 : nav_state.HrdMagNorth = 0; // 1 means "north" is magnetic north
493 :
494 0 : msg.navState = nav_state;
495 0 : msg.satsUsed = gps.num_sats();
496 :
497 0 : gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg));
498 0 : }
499 :
500 :
501 0 : bool AP_ADSB_uAvionix_UCP::hostTransmit(uint8_t *buffer, uint16_t length)
502 : {
503 0 : if (_port == nullptr || _port->txspace() < length) {
504 0 : return false;
505 : }
506 0 : _port->write(buffer, length);
507 0 : return true;
508 : }
509 :
510 :
511 0 : bool AP_ADSB_uAvionix_UCP::request_msg(const GDL90_MESSAGE_ID msg_id)
512 : {
513 0 : const GDL90_TRANSPONDER_MESSAGE_REQUEST_V2 msg = {
514 : messageId : GDL90_ID_MESSAGE_REQUEST,
515 : version : 2,
516 : reqMsgId : msg_id
517 0 : };
518 0 : return gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg)) != 0;
519 : }
520 :
521 :
522 0 : uint16_t AP_ADSB_uAvionix_UCP::gdl90Transmit(GDL90_TX_MESSAGE &message, const uint16_t length)
523 : {
524 0 : uint8_t gdl90FrameBuffer[GDL90_TX_MAX_FRAME_LENGTH] {};
525 :
526 0 : const uint16_t frameCrc = crc16_ccitt_GDL90((uint8_t*)&message.raw, length, 0);
527 :
528 : // Set flag byte in frame buffer
529 0 : gdl90FrameBuffer[0] = GDL90_FLAG_BYTE;
530 0 : uint16_t frameIndex = 1;
531 :
532 : // Copy and stuff all payload bytes into frame buffer
533 0 : for (uint16_t i = 0; i < length+2; i++) {
534 : // Check for overflow of frame buffer
535 0 : if (frameIndex >= GDL90_TX_MAX_FRAME_LENGTH) {
536 0 : return 0;
537 : }
538 :
539 : uint8_t data;
540 : // Append CRC to payload
541 0 : if (i == length) {
542 0 : data = LOWBYTE(frameCrc);
543 0 : } else if (i == length+1) {
544 0 : data = HIGHBYTE(frameCrc);
545 : } else {
546 0 : data = message.raw[i];
547 : }
548 :
549 0 : if (data == GDL90_FLAG_BYTE || data == GDL90_CONTROL_ESCAPE_BYTE) {
550 : // Check for frame buffer overflow on stuffed byte
551 0 : if (frameIndex + 2 > GDL90_TX_MAX_FRAME_LENGTH) {
552 0 : return 0;
553 : }
554 :
555 : // Set control break and stuff this byte
556 0 : gdl90FrameBuffer[frameIndex++] = GDL90_CONTROL_ESCAPE_BYTE;
557 0 : gdl90FrameBuffer[frameIndex++] = data ^ GDL90_STUFF_BYTE;
558 : } else {
559 0 : gdl90FrameBuffer[frameIndex++] = data;
560 : }
561 : }
562 :
563 : // Add end of frame indication
564 0 : gdl90FrameBuffer[frameIndex++] = GDL90_FLAG_BYTE;
565 :
566 : // Push packet to UART
567 0 : if (hostTransmit(gdl90FrameBuffer, frameIndex)) {
568 0 : return frameIndex;
569 : }
570 :
571 0 : return 0;
572 : }
573 :
574 :
575 0 : bool AP_ADSB_uAvionix_UCP::parseByte(const uint8_t data, GDL90_RX_MESSAGE &msg, GDL90_RX_STATUS &status)
576 : {
577 0 : switch (status.state)
578 : {
579 0 : case GDL90_RX_IDLE:
580 0 : if (data == GDL90_FLAG_BYTE && status.prev_data == GDL90_FLAG_BYTE) {
581 0 : status.length = 0;
582 0 : status.state = GDL90_RX_IN_PACKET;
583 : }
584 0 : break;
585 :
586 0 : case GDL90_RX_IN_PACKET:
587 0 : if (data == GDL90_CONTROL_ESCAPE_BYTE) {
588 0 : status.state = GDL90_RX_UNSTUFF;
589 :
590 0 : } else if (data == GDL90_FLAG_BYTE) {
591 : // packet complete! Check CRC and restart packet cycle on all pass or fail scenarios
592 0 : status.state = GDL90_RX_IDLE;
593 :
594 0 : if (status.length < GDL90_OVERHEAD_LENGTH) {
595 : // something is wrong, there's no actual data
596 0 : return false;
597 : }
598 :
599 0 : const uint8_t crc_LSB = msg.raw[status.length - 2];
600 0 : 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 0 : msg.crc = (uint16_t)crc_LSB | ((uint16_t)crc_MSB << 8);
604 0 : const uint16_t crc = crc16_ccitt_GDL90((uint8_t*)&msg.raw, status.length-2, 0);
605 0 : if (crc == msg.crc) {
606 0 : status.prev_data = data;
607 : // NOTE: this is the only path that returns true
608 0 : return true;
609 : }
610 :
611 0 : } else if (status.length < GDL90_RX_MAX_PACKET_LENGTH) {
612 0 : msg.raw[status.length++] = data;
613 :
614 : } else {
615 0 : status.state = GDL90_RX_IDLE;
616 : }
617 0 : break;
618 :
619 0 : case GDL90_RX_UNSTUFF:
620 0 : if (status.length < ARRAY_SIZE(msg.raw)) {
621 0 : msg.raw[status.length++] = data ^ GDL90_STUFF_BYTE;
622 0 : status.state = GDL90_RX_IN_PACKET;
623 : } else {
624 0 : status.state = GDL90_RX_IDLE;
625 : }
626 0 : break;
627 : }
628 0 : status.prev_data = data;
629 0 : return false;
630 : }
631 :
632 : #endif // HAL_ADSB_UCP_ENABLED
633 :
|
