adsblib 1.0
Portable C99 library for encoding ADS-B Out DF17 messages
 
Loading...
Searching...
No Matches
adsblib.c
1#include "adsblib.h"
2
3#include <math.h>
4#include <stddef.h>
5#include <stdlib.h>
6#include <string.h>
7
8/* ============================================================
9 * Library Version
10 * ============================================================ */
11
12#define ADSBLIB_STR(x) ADSBLIB_STR_(x)
13#define ADSBLIB_STR_(x) #x
14
15const char *adsb_version_string(void)
16{
17 return ADSBLIB_STR(ADSBLIB_VERSION_MAJOR) "."
18 ADSBLIB_STR(ADSBLIB_VERSION_MINOR) "."
19 ADSBLIB_STR(ADSBLIB_VERSION_PATCH);
20}
21
22/* ============================================================
23 * Internal Constants
24 * ============================================================ */
25
26#define ADSB_DF17 17U
27#define ADSB_CA 5U
28
29#define ADSB_CPR_BITS 17U
30#define ADSB_CPR_MOD (1U << ADSB_CPR_BITS)
31
32#define ADSB_NZ 15.0
33
34/* ============================================================
35 * Internal Helpers
36 * ============================================================ */
37
38static double adsb_pi(void)
39{
40 return acos(-1.0);
41}
42
43static bool is_valid_icao(uint32_t icao)
44{
45 return (icao <= ADSB_ICAO_MAX);
46}
47
48static bool is_valid_latitude(double latitude_deg)
49{
50 return isfinite(latitude_deg) && (latitude_deg >= -90.0) && (latitude_deg <= 90.0);
51}
52
53static bool is_valid_longitude(double longitude_deg)
54{
55 return isfinite(longitude_deg) && (longitude_deg >= -180.0) && (longitude_deg <= 180.0);
56}
57
58static bool is_valid_altitude(int32_t altitude_ft)
59{
60 return (altitude_ft >= -1000) && (altitude_ft <= 60000);
61}
62
63static bool is_valid_speed(double speed_kt)
64{
65 return isfinite(speed_kt) && (speed_kt >= 0.0);
66}
67
68static bool is_valid_track(double track_deg)
69{
70 return isfinite(track_deg) && (track_deg >= 0.0) && (track_deg < 360.0);
71}
72
73static bool is_valid_vertical_rate(int32_t vr_fpm)
74{
75 return (vr_fpm >= -32640) && (vr_fpm <= 32640);
76}
77
78static bool is_valid_emergency_state(adsb_emergency_state_t state)
79{
80 return (state >= ADSB_EMERGENCY_NONE) &&
81 (state <= ADSB_EMERGENCY_DOWNED_AIRCRAFT);
82}
83
84static bool is_valid_mode_a_code(uint16_t mode_a_code)
85{
86 return mode_a_code <= 07777U;
87}
88
89static bool is_valid_altitude_source(adsb_altitude_source_t source)
90{
91 return (source == ADSB_ALTITUDE_SOURCE_MCP) ||
92 (source == ADSB_ALTITUDE_SOURCE_FMS);
93}
94
95static enc_status_t encode_target_altitude(int32_t altitude_ft, uint32_t *altitude_field)
96{
97 long raw;
98
99 if (altitude_field == NULL)
100 {
101 return ENC_INVALID_SELECTED_ALTITUDE;
102 }
103 if (altitude_ft == -1)
104 {
105 *altitude_field = 0U;
106 return ENC_OK;
107 }
108 if ((altitude_ft < 0) || (altitude_ft > 65472))
109 {
110 return ENC_INVALID_SELECTED_ALTITUDE;
111 }
112
113 raw = lround((double)altitude_ft / 32.0) + 1L;
114 if ((raw < 1L) || (raw > 2047L))
115 {
116 return ENC_INVALID_SELECTED_ALTITUDE;
117 }
118
119 *altitude_field = (uint32_t)raw;
120 return ENC_OK;
121}
122
123static enc_status_t encode_barometric_pressure(double pressure_mbar, uint32_t *pressure_field)
124{
125 long raw;
126
127 if (pressure_field == NULL)
128 {
129 return ENC_INVALID_BARO_PRESSURE;
130 }
131 if (pressure_mbar == -1.0)
132 {
133 *pressure_field = 0U;
134 return ENC_OK;
135 }
136 if (!isfinite(pressure_mbar) || (pressure_mbar < 800.0) || (pressure_mbar > 1208.0))
137 {
138 return ENC_INVALID_BARO_PRESSURE;
139 }
140
141 raw = lround((pressure_mbar - 800.0) / 0.8) + 1L;
142 if ((raw < 1L) || (raw > 511L))
143 {
144 return ENC_INVALID_BARO_PRESSURE;
145 }
146
147 *pressure_field = (uint32_t)raw;
148 return ENC_OK;
149}
150
151static uint32_t encode_mode_a_code(uint16_t mode_a_code)
152{
153 uint32_t a = (mode_a_code >> 9U) & 0x7U;
154 uint32_t b = (mode_a_code >> 6U) & 0x7U;
155 uint32_t c = (mode_a_code >> 3U) & 0x7U;
156 uint32_t d = mode_a_code & 0x7U;
157
158 /* Gillham bit order: C1 A1 C2 A2 C4 A4 X B1 D1 B2 D2 B4 D4. */
159 return ((a & 4U) << 5U) |
160 ((a & 2U) << 8U) |
161 ((a & 1U) << 11U) |
162 ((b & 4U) >> 1U) |
163 ((b & 2U) << 2U) |
164 ((b & 1U) << 5U) |
165 ((c & 4U) << 6U) |
166 ((c & 2U) << 9U) |
167 ((c & 1U) << 12U) |
168 ((d & 4U) >> 2U) |
169 ((d & 2U) >> 1U) |
170 ((d & 1U) << 4U);
171}
172
173/*
174 * Encode a ground speed (kt) into the 7-bit surface-position movement
175 * field. The field is non-linear: 6 bins of increasing step size, plus
176 * code 1 for "stopped" and code 124 for the open-ended "175 kt or more".
177 * Table per ICAO Annex 10 / DO-260B (movement field encoding).
178 */
179static uint32_t encode_movement(double speed_kt)
180{
181 static const uint32_t bin_lb_code[6] = { 2U, 9U, 13U, 39U, 94U, 109U };
182 static const double bin_lb_kt[6] = { 0.125, 1.0, 2.0, 15.0, 70.0, 100.0 };
183 static const double bin_step_kt[6] = { 0.125, 0.25, 0.5, 1.0, 2.0, 5.0 };
184 if (speed_kt < 0.125)
185 {
186 return 1U; /* Aircraft stopped */
187 }
188
189 if (speed_kt >= 175.0)
190 {
191 return 124U; /* 175 kt or more (open-ended) */
192 }
193
194 for (int i = 5; i >= 0; --i)
195 {
196 if (speed_kt >= bin_lb_kt[i])
197 {
198 return bin_lb_code[i] + (uint32_t)floor((speed_kt - bin_lb_kt[i]) / bin_step_kt[i]);
199 }
200 }
201
202 return 1U; /* Unreachable: speed_kt >= 0.125 is handled by the loop above. */
203}
204
205static double pos_mod(double value, double modulus)
206{
207 double r;
208
209 if (modulus <= 0.0)
210 {
211 return 0.0;
212 }
213
214 r = fmod(value, modulus);
215 if (r < 0.0)
216 {
217 r += modulus;
218 }
219
220 return r;
221}
222
223static void frame_set_bits(uint8_t frame[ADSB_FRAME_BYTES], uint32_t start_bit, uint32_t num_bits, uint32_t value)
224{
225 uint32_t i;
226
227 for (i = 0U; i < num_bits; ++i)
228 {
229 uint32_t dst_bit = start_bit + i;
230 uint32_t dst_byte = dst_bit / 8U;
231 uint32_t dst_bit_in_byte = 7U - (dst_bit % 8U);
232 uint32_t src_bit = num_bits - 1U - i;
233 uint8_t bit_value = (uint8_t)((value >> src_bit) & 0x1U);
234
235 if (bit_value != 0U)
236 {
237 frame[dst_byte] = (uint8_t)(frame[dst_byte] | (uint8_t)(1U << dst_bit_in_byte));
238 }
239 else
240 {
241 frame[dst_byte] = (uint8_t)(frame[dst_byte] & (uint8_t)~(uint8_t)(1U << dst_bit_in_byte));
242 }
243 }
244}
245
246static size_t callsign_len8(const char callsign[ADSB_CALLSIGN_LEN + 1])
247{
248 size_t i;
249
250 for (i = 0U; i < (ADSB_CALLSIGN_LEN + 1U); ++i)
251 {
252 if (callsign[i] == '\0')
253 {
254 return i;
255 }
256 }
257
258 return ADSB_CALLSIGN_LEN + 1U;
259}
260
261static int callsign_char_to_6bit(char c)
262{
263 if ((c >= 'A') && (c <= 'Z'))
264 {
265 return (int)(c - 'A') + 1;
266 }
267
268 if ((c >= '0') && (c <= '9'))
269 {
270 return (int)(c - '0') + 48;
271 }
272
273 if (c == ' ')
274 {
275 return 32;
276 }
277
278 return -1;
279}
280
281static enc_status_t encode_callsign(const char in_callsign[ADSB_CALLSIGN_LEN + 1], uint8_t out_codes[ADSB_CALLSIGN_LEN])
282{
283 size_t len;
284 size_t i;
285
286 if (in_callsign == NULL)
287 {
288 return ENC_INVALID_ARGUMENT;
289 }
290
291 len = callsign_len8(in_callsign);
292 if (len > ADSB_CALLSIGN_LEN)
293 {
294 return ENC_INVALID_CALLSIGN;
295 }
296
297 for (i = 0U; i < ADSB_CALLSIGN_LEN; ++i)
298 {
299 char c = (i < len) ? in_callsign[i] : ' ';
300 int code = callsign_char_to_6bit(c);
301
302 if (code < 0)
303 {
304 return ENC_INVALID_CALLSIGN;
305 }
306
307 out_codes[i] = (uint8_t)code;
308 }
309
310 return ENC_OK;
311}
312
313static enc_status_t encode_altitude_12bit(int32_t altitude_ft, uint32_t *alt_code)
314{
315 int32_t n;
316 uint32_t high;
317 uint32_t low;
318
319 if ((alt_code == NULL) || !is_valid_altitude(altitude_ft))
320 {
321 return ENC_INVALID_ALTITUDE;
322 }
323
324 /*
325 * Q=1 uses 25 ft increments. This is the common ADS-B airborne format.
326 * Encodable range with this representation is -1000..50175 ft.
327 */
328 if (altitude_ft > 50175)
329 {
330 return ENC_INVALID_ALTITUDE;
331 }
332
333 n = (altitude_ft + 1000) / 25;
334 if ((n < 0) || (n > 2047))
335 {
336 return ENC_INVALID_ALTITUDE;
337 }
338
339 high = (uint32_t)((n >> 4) & 0x7FU);
340 low = (uint32_t)(n & 0x0FU);
341
342 *alt_code = (high << 5) | (1U << 4) | low;
343 return ENC_OK;
344}
345
346/* ============================================================
347 * Public Utility Functions
348 * ============================================================ */
349
350void adsb_frame_clear(uint8_t frame[ADSB_FRAME_BYTES])
351{
352 if (frame != NULL)
353 {
354 (void)memset(frame, 0, ADSB_FRAME_BYTES);
355 }
356}
357
358void adsb_frame_to_hex(const uint8_t frame[ADSB_FRAME_BYTES], char hex_string[29])
359{
360 static const char hex_lut[] = "0123456789ABCDEF";
361 uint32_t i;
362
363 if ((frame == NULL) || (hex_string == NULL))
364 {
365 return;
366 }
367
368 for (i = 0U; i < ADSB_FRAME_BYTES; ++i)
369 {
370 hex_string[(2U * i)] = hex_lut[(frame[i] >> 4) & 0x0FU];
371 hex_string[(2U * i) + 1U] = hex_lut[frame[i] & 0x0FU];
372 }
373
374 hex_string[28] = '\0';
375}
376
377uint32_t adsb_crc24(const uint8_t *data, uint32_t num_bits)
378{
379 uint32_t i;
380 uint32_t crc = 0U;
381 const uint32_t poly = 0xFFF409U;
382
383 if (data == NULL)
384 {
385 return 0U;
386 }
387
388 for (i = 0U; i < num_bits; ++i)
389 {
390 uint32_t byte_idx = i / 8U;
391 uint32_t bit_idx = 7U - (i % 8U);
392 uint32_t bit = (data[byte_idx] >> bit_idx) & 0x1U;
393 uint32_t top = (crc >> 23U) & 0x1U;
394
395 crc = (crc << 1U) & 0xFFFFFFU;
396 if ((top ^ bit) != 0U)
397 {
398 crc ^= poly;
399 }
400 }
401
402 return crc & 0xFFFFFFU;
403}
404
405void adsb_apply_crc(uint8_t frame[ADSB_FRAME_BYTES])
406{
407 uint32_t crc;
408
409 if (frame == NULL)
410 {
411 return;
412 }
413
414 crc = adsb_crc24(frame, 88U);
415
416 frame[11] = (uint8_t)((crc >> 16U) & 0xFFU);
417 frame[12] = (uint8_t)((crc >> 8U) & 0xFFU);
418 frame[13] = (uint8_t)(crc & 0xFFU);
419}
420
421bool adsb_verify_crc(const uint8_t frame[ADSB_FRAME_BYTES])
422{
423 uint32_t expected_crc;
424 uint32_t frame_crc;
425
426 if (frame == NULL)
427 {
428 return false;
429 }
430
431 expected_crc = adsb_crc24(frame, 88U);
432 frame_crc = ((uint32_t)frame[11] << 16U) |
433 ((uint32_t)frame[12] << 8U) |
434 (uint32_t)frame[13];
435
436 return (expected_crc == frame_crc);
437}
438
439/* ============================================================
440 * CPR Functions
441 * ============================================================ */
442
443int adsb_cpr_nl(double latitude_deg)
444{
445 double lat;
446 double a;
447 double b;
448 double num;
449 double den;
450 double angle;
451
452 if (!isfinite(latitude_deg))
453 {
454 return 1;
455 }
456
457 lat = fabs(latitude_deg);
458
459 if (lat < 1e-12)
460 {
461 return 59;
462 }
463
464 if (lat >= 87.0)
465 {
466 return 1;
467 }
468
469 a = 1.0 - cos(adsb_pi() / (2.0 * ADSB_NZ));
470 b = cos((adsb_pi() / 180.0) * lat);
471 b *= b;
472
473 if (b <= 0.0)
474 {
475 return 1;
476 }
477
478 angle = 1.0 - (a / b);
479 if (angle < -1.0)
480 {
481 angle = -1.0;
482 }
483 if (angle > 1.0)
484 {
485 angle = 1.0;
486 }
487
488 num = 2.0 * adsb_pi();
489 den = acos(angle);
490
491 if (den <= 0.0)
492 {
493 return 1;
494 }
495
496 return (int)floor(num / den);
497}
498
499uint32_t adsb_cpr_encode_latitude(double latitude_deg, cpr_format_t format)
500{
501 double dlat;
502 double yz;
503
504 if (!is_valid_latitude(latitude_deg))
505 {
506 return 0U;
507 }
508
509 if (format == CPR_EVEN)
510 {
511 dlat = 360.0 / (4.0 * ADSB_NZ);
512 }
513 else
514 {
515 dlat = 360.0 / (4.0 * ADSB_NZ - 1.0);
516 }
517
518 yz = floor(((double)ADSB_CPR_MOD * pos_mod(latitude_deg, dlat) / dlat) + 0.5);
519
520 return ((uint32_t)yz) & 0x1FFFFU;
521}
522
523uint32_t adsb_cpr_encode_longitude(double latitude_deg, double longitude_deg, cpr_format_t format)
524{
525 int nl;
526 int ni;
527 double dlon;
528 double xz;
529
530 if (!is_valid_latitude(latitude_deg) || !is_valid_longitude(longitude_deg))
531 {
532 return 0U;
533 }
534
535 nl = adsb_cpr_nl(latitude_deg);
536 if (format == CPR_EVEN)
537 {
538 ni = nl;
539 }
540 else
541 {
542 ni = nl - 1;
543 }
544
545 if (ni < 1)
546 {
547 ni = 1;
548 }
549
550 dlon = 360.0 / (double)ni;
551 xz = floor(((double)ADSB_CPR_MOD * pos_mod(longitude_deg, dlon) / dlon) + 0.5);
552
553 return ((uint32_t)xz) & 0x1FFFFU;
554}
555
556/* ============================================================
557 * Public Encoding Functions
558 * ============================================================ */
559
560enc_status_t adsb_encode_identification(const adsb_identification_t *msg, uint8_t frame[ADSB_FRAME_BYTES])
561{
562 uint8_t callsign_codes[ADSB_CALLSIGN_LEN];
563 enc_status_t st;
564 uint32_t i;
565
566 if ((msg == NULL) || (frame == NULL))
567 {
568 return ENC_INVALID_ARGUMENT;
569 }
570
571 if (!is_valid_icao(msg->icao))
572 {
573 return ENC_INVALID_ICAO;
574 }
575
576 st = encode_callsign(msg->callsign, callsign_codes);
577 if (st != ENC_OK)
578 {
579 return st;
580 }
581
582 adsb_frame_clear(frame);
583
584 frame_set_bits(frame, 0U, 5U, ADSB_DF17);
585 frame_set_bits(frame, 5U, 3U, ADSB_CA);
586 frame_set_bits(frame, 8U, 24U, msg->icao & ADSB_ICAO_MAX);
587
588 frame_set_bits(frame, 32U, 5U, 4U); /* Type code 4: aircraft identification */
589 frame_set_bits(frame, 37U, 3U, 0U); /* Emitter category unknown */
590
591 for (i = 0U; i < ADSB_CALLSIGN_LEN; ++i)
592 {
593 frame_set_bits(frame, 40U + (6U * i), 6U, callsign_codes[i]);
594 }
595
596 adsb_apply_crc(frame);
597
598 return ENC_OK;
599}
600
601enc_status_t adsb_encode_position(const adsb_position_t *msg, uint8_t frame[ADSB_FRAME_BYTES])
602{
603 uint32_t alt_code;
604 uint32_t cpr_lat;
605 uint32_t cpr_lon;
606 enc_status_t st;
607
608 if ((msg == NULL) || (frame == NULL))
609 {
610 return ENC_INVALID_ARGUMENT;
611 }
612
613 if (!is_valid_icao(msg->icao))
614 {
615 return ENC_INVALID_ICAO;
616 }
617 if (!is_valid_latitude(msg->latitude_deg))
618 {
619 return ENC_INVALID_LATITUDE;
620 }
621 if (!is_valid_longitude(msg->longitude_deg))
622 {
623 return ENC_INVALID_LONGITUDE;
624 }
625 if (!is_valid_altitude(msg->altitude_ft))
626 {
627 return ENC_INVALID_ALTITUDE;
628 }
629 if ((msg->cpr_format != CPR_EVEN) && (msg->cpr_format != CPR_ODD))
630 {
631 return ENC_INVALID_ARGUMENT;
632 }
633
634 st = encode_altitude_12bit(msg->altitude_ft, &alt_code);
635 if (st != ENC_OK)
636 {
637 return st;
638 }
639
640 cpr_lat = adsb_cpr_encode_latitude(msg->latitude_deg, msg->cpr_format);
641 cpr_lon = adsb_cpr_encode_longitude(msg->latitude_deg, msg->longitude_deg, msg->cpr_format);
642
643 adsb_frame_clear(frame);
644
645 frame_set_bits(frame, 0U, 5U, ADSB_DF17);
646 frame_set_bits(frame, 5U, 3U, ADSB_CA);
647 frame_set_bits(frame, 8U, 24U, msg->icao & ADSB_ICAO_MAX);
648
649 frame_set_bits(frame, 32U, 5U, 11U); /* Type code 11 */
650 frame_set_bits(frame, 37U, 2U, 0U); /* Surveillance status */
651 frame_set_bits(frame, 39U, 1U, 0U); /* NIC supplement-B */
652 frame_set_bits(frame, 40U, 12U, alt_code & 0x0FFFU); /* Altitude */
653 frame_set_bits(frame, 52U, 1U, 0U); /* UTC sync time flag */
654 frame_set_bits(frame, 53U, 1U, (uint32_t)msg->cpr_format & 0x01U); /* CPR format */
655 frame_set_bits(frame, 54U, 17U, cpr_lat & 0x1FFFFU);
656 frame_set_bits(frame, 71U, 17U, cpr_lon & 0x1FFFFU);
657
658 adsb_apply_crc(frame);
659
660 return ENC_OK;
661}
662
663enc_status_t adsb_encode_surface_position(const adsb_surface_position_t *msg, uint8_t frame[ADSB_FRAME_BYTES])
664{
665 uint32_t cpr_lat;
666 uint32_t cpr_lon;
667 uint32_t movement;
668 uint32_t track_code;
669
670 if ((msg == NULL) || (frame == NULL))
671 {
672 return ENC_INVALID_ARGUMENT;
673 }
674
675 if (!is_valid_icao(msg->icao))
676 {
677 return ENC_INVALID_ICAO;
678 }
679 if (!is_valid_latitude(msg->latitude_deg))
680 {
681 return ENC_INVALID_LATITUDE;
682 }
683 if (!is_valid_longitude(msg->longitude_deg))
684 {
685 return ENC_INVALID_LONGITUDE;
686 }
687 if (!is_valid_speed(msg->ground_speed_kt))
688 {
689 return ENC_INVALID_SPEED;
690 }
691 if (!is_valid_track(msg->ground_track_deg))
692 {
693 return ENC_INVALID_TRACK;
694 }
695 if ((msg->cpr_format != CPR_EVEN) && (msg->cpr_format != CPR_ODD))
696 {
697 return ENC_INVALID_ARGUMENT;
698 }
699
700 movement = encode_movement(msg->ground_speed_kt);
701 track_code = ((uint32_t)lround(msg->ground_track_deg / (360.0 / 128.0))) & 0x7FU;
702
703 cpr_lat = adsb_cpr_encode_latitude(msg->latitude_deg, msg->cpr_format);
704 cpr_lon = adsb_cpr_encode_longitude(msg->latitude_deg, msg->longitude_deg, msg->cpr_format);
705
706 adsb_frame_clear(frame);
707
708 frame_set_bits(frame, 0U, 5U, ADSB_DF17);
709 frame_set_bits(frame, 5U, 3U, ADSB_CA);
710 frame_set_bits(frame, 8U, 24U, msg->icao & ADSB_ICAO_MAX);
711
712 /*
713 * Type code 8: surface position, navigation accuracy unknown. NIC
714 * selection isn't exposed on the public API yet, mirroring
715 * adsb_encode_position, which similarly fixes TC 11 rather than
716 * exposing NIC.
717 */
718 frame_set_bits(frame, 32U, 5U, 8U);
719 frame_set_bits(frame, 37U, 7U, movement & 0x7FU);
720 frame_set_bits(frame, 44U, 1U, 1U); /* Ground track status: valid */
721 frame_set_bits(frame, 45U, 7U, track_code);
722 frame_set_bits(frame, 52U, 1U, 0U); /* UTC sync time flag */
723 frame_set_bits(frame, 53U, 1U, (uint32_t)msg->cpr_format & 0x01U);
724 frame_set_bits(frame, 54U, 17U, cpr_lat & 0x1FFFFU);
725 frame_set_bits(frame, 71U, 17U, cpr_lon & 0x1FFFFU);
726
727 adsb_apply_crc(frame);
728
729 return ENC_OK;
730}
731
732enc_status_t adsb_encode_velocity(const adsb_velocity_t *msg, uint8_t frame[ADSB_FRAME_BYTES])
733{
734 double track_rad;
735 double v_east;
736 double v_north;
737 uint32_t subtype;
738 int32_t ew_speed;
739 int32_t ns_speed;
740 uint32_t ew_dir;
741 uint32_t ns_dir;
742 uint32_t ew_field;
743 uint32_t ns_field;
744 int32_t vr_q;
745 uint32_t vr_sign;
746 uint32_t vr_field;
747
748 if ((msg == NULL) || (frame == NULL))
749 {
750 return ENC_INVALID_ARGUMENT;
751 }
752
753 if (!is_valid_icao(msg->icao))
754 {
755 return ENC_INVALID_ICAO;
756 }
757 if (!is_valid_speed(msg->ground_speed_kt))
758 {
759 return ENC_INVALID_SPEED;
760 }
761 if (!is_valid_track(msg->track_deg))
762 {
763 return ENC_INVALID_TRACK;
764 }
765 if (!is_valid_vertical_rate(msg->vertical_rate_fpm))
766 {
767 return ENC_INVALID_VERTICAL_RATE;
768 }
769
770 track_rad = msg->track_deg * (adsb_pi() / 180.0);
771 v_east = msg->ground_speed_kt * sin(track_rad);
772 v_north = msg->ground_speed_kt * cos(track_rad);
773
774 /*
775 * Subtype 1 (subsonic, 1 kt/LSB) covers components up to 1022 kt.
776 * Subtype 2 (supersonic, 4 kt/LSB) covers components up to 4088 kt
777 * and is selected automatically when subtype 1's range is exceeded.
778 */
779 ew_speed = (int32_t)lround(fabs(v_east));
780 ns_speed = (int32_t)lround(fabs(v_north));
781
782 if ((ew_speed <= 1022) && (ns_speed <= 1022))
783 {
784 subtype = 1U;
785 }
786 else
787 {
788 subtype = 2U;
789 ew_speed = (int32_t)lround(fabs(v_east) / 4.0);
790 ns_speed = (int32_t)lround(fabs(v_north) / 4.0);
791 }
792
793 if ((ew_speed > 1022) || (ns_speed > 1022))
794 {
795 return ENC_INVALID_SPEED;
796 }
797
798 ew_dir = (v_east < 0.0) ? 1U : 0U;
799 ns_dir = (v_north < 0.0) ? 1U : 0U;
800
801 ew_field = (uint32_t)(ew_speed + 1);
802 ns_field = (uint32_t)(ns_speed + 1);
803
804 vr_q = (int32_t)lround((double)abs(msg->vertical_rate_fpm) / 64.0);
805 if (vr_q > 510)
806 {
807 return ENC_INVALID_VERTICAL_RATE;
808 }
809
810 vr_sign = (msg->vertical_rate_fpm < 0) ? 1U : 0U;
811 vr_field = (uint32_t)(vr_q + 1);
812
813 adsb_frame_clear(frame);
814
815 frame_set_bits(frame, 0U, 5U, ADSB_DF17);
816 frame_set_bits(frame, 5U, 3U, ADSB_CA);
817 frame_set_bits(frame, 8U, 24U, msg->icao & ADSB_ICAO_MAX);
818
819 frame_set_bits(frame, 32U, 5U, 19U); /* Type code 19: airborne velocity */
820 frame_set_bits(frame, 37U, 3U, subtype); /* Subtype 1: subsonic, 2: supersonic */
821 frame_set_bits(frame, 40U, 1U, 0U); /* Intent change flag */
822 frame_set_bits(frame, 41U, 1U, 0U); /* IFR capability / reserved */
823 frame_set_bits(frame, 42U, 3U, 0U); /* NACv unknown */
824 frame_set_bits(frame, 45U, 1U, ew_dir);
825 frame_set_bits(frame, 46U, 10U, ew_field & 0x03FFU);
826 frame_set_bits(frame, 56U, 1U, ns_dir);
827 frame_set_bits(frame, 57U, 10U, ns_field & 0x03FFU);
828 frame_set_bits(frame, 67U, 1U, 1U); /* Vertical rate source: barometric */
829 frame_set_bits(frame, 68U, 1U, vr_sign);
830 frame_set_bits(frame, 69U, 9U, vr_field & 0x01FFU);
831 frame_set_bits(frame, 78U, 2U, 0U); /* Reserved */
832 frame_set_bits(frame, 80U, 1U, 0U); /* GNSS/baro diff sign */
833 frame_set_bits(frame, 81U, 7U, 0U); /* GNSS/baro diff unavailable */
834
835 adsb_apply_crc(frame);
836
837 return ENC_OK;
838}
839
840enc_status_t adsb_encode_emergency(const adsb_emergency_t *msg, uint8_t frame[ADSB_FRAME_BYTES])
841{
842 uint32_t mode_a_field;
843
844 if ((msg == NULL) || (frame == NULL))
845 {
846 return ENC_INVALID_ARGUMENT;
847 }
848
849 if (!is_valid_icao(msg->icao))
850 {
851 return ENC_INVALID_ICAO;
852 }
853 if (!is_valid_emergency_state(msg->emergency_state))
854 {
855 return ENC_INVALID_EMERGENCY_STATE;
856 }
857 if (!is_valid_mode_a_code(msg->mode_a_code))
858 {
859 return ENC_INVALID_MODE_A_CODE;
860 }
861
862 mode_a_field = encode_mode_a_code(msg->mode_a_code);
863
864 adsb_frame_clear(frame);
865
866 frame_set_bits(frame, 0U, 5U, ADSB_DF17);
867 frame_set_bits(frame, 5U, 3U, ADSB_CA);
868 frame_set_bits(frame, 8U, 24U, msg->icao & ADSB_ICAO_MAX);
869
870 frame_set_bits(frame, 32U, 5U, 28U); /* Type code 28 */
871 frame_set_bits(frame, 37U, 3U, 1U); /* Subtype 1 */
872 frame_set_bits(frame, 40U, 3U, (uint32_t)msg->emergency_state);
873 frame_set_bits(frame, 43U, 13U, mode_a_field & 0x1FFFU);
874 frame_set_bits(frame, 56U, 32U, 0U); /* Reserved */
875
876 adsb_apply_crc(frame);
877
878 return ENC_OK;
879}
880
881enc_status_t adsb_encode_target_state(const adsb_target_state_t *msg, uint8_t frame[ADSB_FRAME_BYTES])
882{
883 uint32_t altitude_field;
884 uint32_t pressure_field;
885 uint32_t heading_field = 0U;
886 enc_status_t st;
887
888 if ((msg == NULL) || (frame == NULL))
889 {
890 return ENC_INVALID_ARGUMENT;
891 }
892 if (!is_valid_icao(msg->icao))
893 {
894 return ENC_INVALID_ICAO;
895 }
896 if (!is_valid_altitude_source(msg->selected_altitude_source))
897 {
898 return ENC_INVALID_ARGUMENT;
899 }
900 st = encode_target_altitude(msg->selected_altitude_ft, &altitude_field);
901 if (st != ENC_OK)
902 {
903 return st;
904 }
905 st = encode_barometric_pressure(msg->barometric_pressure_mbar, &pressure_field);
906 if (st != ENC_OK)
907 {
908 return st;
909 }
910 if (msg->nac_p > 15U)
911 {
912 return ENC_INVALID_NAC_P;
913 }
914 if (msg->sil > 3U)
915 {
916 return ENC_INVALID_SIL;
917 }
918 if (msg->selected_heading_valid)
919 {
920 if (!is_valid_track(msg->selected_heading_deg))
921 {
922 return ENC_INVALID_HEADING;
923 }
924 heading_field = (uint32_t)lround(msg->selected_heading_deg * (512.0 / 360.0)) & 0x1FFU;
925 }
926
927 adsb_frame_clear(frame);
928
929 frame_set_bits(frame, 0U, 5U, ADSB_DF17);
930 frame_set_bits(frame, 5U, 3U, ADSB_CA);
931 frame_set_bits(frame, 8U, 24U, msg->icao & ADSB_ICAO_MAX);
932
933 frame_set_bits(frame, 32U, 5U, 29U); /* Type code 29 */
934 frame_set_bits(frame, 37U, 2U, 1U); /* Subtype 1 */
935 frame_set_bits(frame, 39U, 1U, 0U); /* SIL supplement, reserved */
936 frame_set_bits(frame, 40U, 1U, (uint32_t)msg->selected_altitude_source);
937 frame_set_bits(frame, 41U, 11U, altitude_field);
938 frame_set_bits(frame, 52U, 9U, pressure_field);
939 frame_set_bits(frame, 61U, 1U, msg->selected_heading_valid ? 1U : 0U);
940 frame_set_bits(frame, 62U, 9U, heading_field);
941 frame_set_bits(frame, 71U, 4U, msg->nac_p);
942 frame_set_bits(frame, 75U, 1U, msg->nic_baro ? 1U : 0U);
943 frame_set_bits(frame, 76U, 2U, msg->sil);
944 frame_set_bits(frame, 78U, 1U, msg->mode_status ? 1U : 0U);
945 frame_set_bits(frame, 79U, 1U, msg->mode_status && msg->autopilot_engaged ? 1U : 0U);
946 frame_set_bits(frame, 80U, 1U, msg->mode_status && msg->vnav_mode ? 1U : 0U);
947 frame_set_bits(frame, 81U, 1U, msg->mode_status && msg->altitude_hold_mode ? 1U : 0U);
948 frame_set_bits(frame, 82U, 1U, 0U); /* IMF / ADS-R, reserved */
949 frame_set_bits(frame, 83U, 1U, msg->mode_status && msg->approach_mode ? 1U : 0U);
950 frame_set_bits(frame, 84U, 1U, msg->tcas_operational ? 1U : 0U);
951 frame_set_bits(frame, 85U, 1U, msg->mode_status && msg->lnav_mode ? 1U : 0U);
952 frame_set_bits(frame, 86U, 2U, 0U); /* Reserved */
953
954 adsb_apply_crc(frame);
955
956 return ENC_OK;
957}
958
959enc_status_t adsb_encode_operational_status(const adsb_operational_status_t *msg, uint8_t frame[ADSB_FRAME_BYTES])
960{
961 if ((msg == NULL) || (frame == NULL))
962 {
963 return ENC_INVALID_ARGUMENT;
964 }
965 if (!is_valid_icao(msg->icao))
966 {
967 return ENC_INVALID_ICAO;
968 }
969 if (msg->subtype > 1U)
970 {
971 return ENC_INVALID_OPERATIONAL_SUBTYPE;
972 }
973 if (msg->adsb_version > 7U)
974 {
975 return ENC_INVALID_ADSB_VERSION;
976 }
977 if (msg->nac_p > 15U)
978 {
979 return ENC_INVALID_NAC_P;
980 }
981 if (msg->sil > 3U)
982 {
983 return ENC_INVALID_SIL;
984 }
985
986 adsb_frame_clear(frame);
987
988 frame_set_bits(frame, 0U, 5U, ADSB_DF17);
989 frame_set_bits(frame, 5U, 3U, ADSB_CA);
990 frame_set_bits(frame, 8U, 24U, msg->icao & ADSB_ICAO_MAX);
991
992 frame_set_bits(frame, 32U, 5U, 31U); /* Type code 31 */
993 frame_set_bits(frame, 37U, 3U, msg->subtype);
994 frame_set_bits(frame, 40U, 16U, msg->capability_class);
995 frame_set_bits(frame, 56U, 16U, msg->operational_mode);
996 frame_set_bits(frame, 72U, 3U, msg->adsb_version);
997 frame_set_bits(frame, 75U, 1U, msg->nic_supplement_a ? 1U : 0U);
998 frame_set_bits(frame, 76U, 4U, msg->nac_p);
999 frame_set_bits(frame, 80U, 2U, 0U); /* GVA / reserved */
1000 frame_set_bits(frame, 82U, 2U, msg->sil);
1001 frame_set_bits(frame, 84U, 1U, msg->nic_baro ? 1U : 0U);
1002 frame_set_bits(frame, 85U, 1U, msg->heading_reference_magnetic ? 1U : 0U);
1003 frame_set_bits(frame, 86U, 1U, msg->sil_supplement ? 1U : 0U);
1004 frame_set_bits(frame, 87U, 1U, 0U); /* Reserved */
1005
1006 adsb_apply_crc(frame);
1007
1008 return ENC_OK;
1009}
adsb_emergency_t
Definition adsblib.h:167
adsb_identification_t
Definition adsblib.h:82
adsb_operational_status_t
Definition adsblib.h:221
adsb_position_t
Definition adsblib.h:93
adsb_surface_position_t
Definition adsblib.h:110
adsb_target_state_t
Definition adsblib.h:189
adsb_velocity_t
Definition adsblib.h:135