forked from MalcolmRobb/dump1090
-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathdump1090.c
4088 lines (3616 loc) · 161 KB
/
dump1090.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/* dump1090, a Mode S messages decoder for RTLSDR devices.
*
* Copyright (C) 2012 by Salvatore Sanfilippo <[email protected]>
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are
* met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "dump1090.h"
/* ============================= Utility functions ========================== */
static uint64_t mstime(void) {
struct timeval tv;
uint64_t mst;
gettimeofday(&tv, NULL);
mst = ((uint64_t)tv.tv_sec)*1000;
mst += tv.tv_usec/1000;
return mst;
}
void sigintHandler(int dummy) {
MODES_NOTUSED(dummy);
signal(SIGINT, SIG_DFL); // reset signal handler - bit extra safety
Modes.exit = 1; // Signal to threads that we are done
}
/* =============================== Initialization =========================== */
void modesInitConfig(void) {
// Default everything to zero/NULL
memset(&Modes, 0, sizeof(Modes));
// Now initialise things that should not be 0/NULL to their defaults
Modes.gain = MODES_MAX_GAIN;
Modes.freq = MODES_DEFAULT_FREQ;
Modes.check_crc = 1;
Modes.net_output_sbs_port = MODES_NET_OUTPUT_SBS_PORT;
Modes.net_output_raw_port = MODES_NET_OUTPUT_RAW_PORT;
Modes.net_input_raw_port = MODES_NET_INPUT_RAW_PORT;
Modes.net_output_beast_port = MODES_NET_OUTPUT_BEAST_PORT;
Modes.net_input_beast_port = MODES_NET_INPUT_BEAST_PORT;
Modes.net_http_port = MODES_NET_HTTP_PORT;
Modes.interactive_rows = MODES_INTERACTIVE_ROWS;
Modes.interactive_ttl = MODES_INTERACTIVE_TTL;
Modes.fUserLat = MODES_USER_LATITUDE_DFLT;
Modes.fUserLon = MODES_USER_LONGITUDE_DFLT;
}
void modesInit(void) {
int i, q;
pthread_mutex_init(&Modes.data_mutex,NULL);
pthread_cond_init(&Modes.data_cond,NULL);
// Allocate the various buffers used by Modes
if ( ((Modes.icao_cache = (uint32_t *) malloc(sizeof(uint32_t) * MODES_ICAO_CACHE_LEN * 2) ) == NULL) ||
((Modes.data = (uint16_t *) malloc(MODES_ASYNC_BUF_SIZE) ) == NULL) ||
((Modes.magnitude = (uint16_t *) malloc(MODES_ASYNC_BUF_SIZE+MODES_PREAMBLE_SIZE+MODES_LONG_MSG_SIZE) ) == NULL) ||
((Modes.maglut = (uint16_t *) malloc(sizeof(uint16_t) * 256 * 256) ) == NULL) ||
((Modes.beastOut = (char *) malloc(MODES_RAWOUT_BUF_SIZE) ) == NULL) ||
((Modes.rawOut = (char *) malloc(MODES_RAWOUT_BUF_SIZE) ) == NULL) )
{
fprintf(stderr, "Out of memory allocating data buffer.\n");
exit(1);
}
// Clear the buffers that have just been allocated, just in-case
memset(Modes.icao_cache, 0, sizeof(uint32_t) * MODES_ICAO_CACHE_LEN * 2);
memset(Modes.data, 127, MODES_ASYNC_BUF_SIZE);
memset(Modes.magnitude, 0, MODES_ASYNC_BUF_SIZE+MODES_PREAMBLE_SIZE+MODES_LONG_MSG_SIZE);
// Validate the users Lat/Lon home location inputs
if ( (Modes.fUserLat > 90.0) // Latitude must be -90 to +90
|| (Modes.fUserLat < -90.0) // and
|| (Modes.fUserLon > 360.0) // Longitude must be -180 to +360
|| (Modes.fUserLon < -180.0) ) {
Modes.fUserLat = Modes.fUserLon = 0.0;
} else if (Modes.fUserLon > 180.0) { // If Longitude is +180 to +360, make it -180 to 0
Modes.fUserLon -= 360.0;
}
// If both Lat and Lon are 0.0 then the users location is either invalid/not-set, or (s)he's in the
// Atlantic ocean off the west coast of Africa. This is unlikely to be correct.
// Set the user LatLon valid flag only if either Lat or Lon are non zero. Note the Greenwich meridian
// is at 0.0 Lon,so we must check for either fLat or fLon being non zero not both.
// Testing the flag at runtime will be much quicker than ((fLon != 0.0) || (fLat != 0.0))
Modes.bUserFlags &= ~MODES_USER_LATLON_VALID;
if ((Modes.fUserLat != 0.0) || (Modes.fUserLon != 0.0)) {
Modes.bUserFlags |= MODES_USER_LATLON_VALID;
}
// Limit the maximum requested raw output size to less than one Ethernet Block
if (Modes.net_output_raw_size > (MODES_RAWOUT_BUF_FLUSH))
{Modes.net_output_raw_size = MODES_RAWOUT_BUF_FLUSH;}
if (Modes.net_output_raw_rate > (MODES_RAWOUT_BUF_RATE))
{Modes.net_output_raw_rate = MODES_RAWOUT_BUF_RATE;}
// Initialise the Block Timers to something half sensible
ftime(&Modes.stSystemTimeRTL);
Modes.stSystemTimeBlk = Modes.stSystemTimeRTL;
// Each I and Q value varies from 0 to 255, which represents a range from -1 to +1. To get from the
// unsigned (0-255) range you therefore subtract 127 (or 128 or 127.5) from each I and Q, giving you
// a range from -127 to +128 (or -128 to +127, or -127.5 to +127.5)..
//
// To decode the AM signal, you need the magnitude of the waveform, which is given by sqrt((I^2)+(Q^2))
// The most this could be is if I&Q are both 128 (or 127 or 127.5), so you could end up with a magnitude
// of 181.019 (or 179.605, or 180.312)
//
// However, in reality the magnitude of the signal should never exceed the range -1 to +1, because the
// values are I = rCos(w) and Q = rSin(w). Therefore the integer computed magnitude should (can?) never
// exceed 128 (or 127, or 127.5 or whatever)
//
// If we scale up the results so that they range from 0 to 65535 (16 bits) then we need to multiply
// by 511.99, (or 516.02 or 514). antirez's original code multiplies by 360, presumably because he's
// assuming the maximim calculated amplitude is 181.019, and (181.019 * 360) = 65166.
//
// So lets see if we can improve things by subtracting 127.5, Well in integer arithmatic we can't
// subtract half, so, we'll double everything up and subtract one, and then compensate for the doubling
// in the multiplier at the end.
//
// If we do this we can never have I or Q equal to 0 - they can only be as small as +/- 1.
// This gives us a minimum magnitude of root 2 (0.707), so the dynamic range becomes (1.414-255). This
// also affects our scaling value, which is now 65535/(255 - 1.414), or 258.433254
//
// The sums then become mag = 258.433254 * (sqrt((I*2-255)^2 + (Q*2-255)^2) - 1.414)
// or mag = (258.433254 * sqrt((I*2-255)^2 + (Q*2-255)^2)) - 365.4798
//
// We also need to clip mag just incaes any rogue I/Q values somehow do have a magnitude greater than 255.
//
for (i = 0; i <= 255; i++) {
for (q = 0; q <= 255; q++) {
int mag, mag_i, mag_q;
mag_i = (i * 2) - 255;
mag_q = (q * 2) - 255;
mag = (int) round((sqrt((mag_i*mag_i)+(mag_q*mag_q)) * 258.433254) - 365.4798);
Modes.maglut[(i*256)+q] = (uint16_t) ((mag < 65535) ? mag : 65535);
}
}
// Prepare error correction tables
modesInitErrorInfo();
}
/* =============================== RTLSDR handling ========================== */
void modesInitRTLSDR(void) {
int j;
int device_count;
char vendor[256], product[256], serial[256];
device_count = rtlsdr_get_device_count();
if (!device_count) {
fprintf(stderr, "No supported RTLSDR devices found.\n");
exit(1);
}
fprintf(stderr, "Found %d device(s):\n", device_count);
for (j = 0; j < device_count; j++) {
rtlsdr_get_device_usb_strings(j, vendor, product, serial);
fprintf(stderr, "%d: %s, %s, SN: %s %s\n", j, vendor, product, serial,
(j == Modes.dev_index) ? "(currently selected)" : "");
}
if (rtlsdr_open(&Modes.dev, Modes.dev_index) < 0) {
fprintf(stderr, "Error opening the RTLSDR device: %s\n",
strerror(errno));
exit(1);
}
/* Set gain, frequency, sample rate, and reset the device. */
rtlsdr_set_tuner_gain_mode(Modes.dev,
(Modes.gain == MODES_AUTO_GAIN) ? 0 : 1);
if (Modes.gain != MODES_AUTO_GAIN) {
if (Modes.gain == MODES_MAX_GAIN) {
/* Find the maximum gain available. */
int numgains;
int gains[100];
numgains = rtlsdr_get_tuner_gains(Modes.dev, gains);
Modes.gain = gains[numgains-1];
fprintf(stderr, "Max available gain is: %.2f\n", Modes.gain/10.0);
}
rtlsdr_set_tuner_gain(Modes.dev, Modes.gain);
fprintf(stderr, "Setting gain to: %.2f\n", Modes.gain/10.0);
} else {
fprintf(stderr, "Using automatic gain control.\n");
}
rtlsdr_set_freq_correction(Modes.dev, Modes.ppm_error);
if (Modes.enable_agc) rtlsdr_set_agc_mode(Modes.dev, 1);
rtlsdr_set_center_freq(Modes.dev, Modes.freq);
rtlsdr_set_sample_rate(Modes.dev, MODES_DEFAULT_RATE);
rtlsdr_reset_buffer(Modes.dev);
fprintf(stderr, "Gain reported by device: %.2f\n",
rtlsdr_get_tuner_gain(Modes.dev)/10.0);
}
/* We use a thread reading data in background, while the main thread
* handles decoding and visualization of data to the user.
*
* The reading thread calls the RTLSDR API to read data asynchronously, and
* uses a callback to populate the data buffer.
* A Mutex is used to avoid races with the decoding thread. */
void rtlsdrCallback(unsigned char *buf, uint32_t len, void *ctx) {
MODES_NOTUSED(ctx);
pthread_mutex_lock(&Modes.data_mutex);
ftime(&Modes.stSystemTimeRTL);
if (len > MODES_ASYNC_BUF_SIZE) len = MODES_ASYNC_BUF_SIZE;
/* Read the new data. */
memcpy(Modes.data, buf, len);
Modes.data_ready = 1;
/* Signal to the other thread that new data is ready */
pthread_cond_signal(&Modes.data_cond);
pthread_mutex_unlock(&Modes.data_mutex);
}
/* This is used when --ifile is specified in order to read data from file
* instead of using an RTLSDR device. */
void readDataFromFile(void) {
pthread_mutex_lock(&Modes.data_mutex);
while(1) {
ssize_t nread, toread;
unsigned char *p;
if (Modes.exit == 1) break;
if (Modes.data_ready) {
pthread_cond_wait(&Modes.data_cond,&Modes.data_mutex);
continue;
}
if (Modes.interactive) {
/* When --ifile and --interactive are used together, slow down
* playing at the natural rate of the RTLSDR received. */
pthread_mutex_unlock(&Modes.data_mutex);
usleep(64000);
pthread_mutex_lock(&Modes.data_mutex);
}
toread = MODES_ASYNC_BUF_SIZE;
p = (unsigned char *) Modes.data;
while(toread) {
nread = read(Modes.fd, p, toread);
if (nread <= 0) {
Modes.exit = 1; /* Signal the other thread to exit. */
break;
}
p += nread;
toread -= nread;
}
if (toread) {
/* Not enough data on file to fill the buffer? Pad with
* no signal. */
memset(p,127,toread);
}
Modes.data_ready = 1;
/* Signal to the other thread that new data is ready */
pthread_cond_signal(&Modes.data_cond);
}
}
/* We read data using a thread, so the main thread only handles decoding
* without caring about data acquisition. */
void *readerThreadEntryPoint(void *arg) {
MODES_NOTUSED(arg);
if (Modes.filename == NULL) {
rtlsdr_read_async(Modes.dev, rtlsdrCallback, NULL,
MODES_ASYNC_BUF_NUMBER,
MODES_ASYNC_BUF_SIZE);
} else {
readDataFromFile();
}
/* Signal to the other thread that new data is ready - dummy really so threads don't mutually lock */
Modes.data_ready = 1;
pthread_cond_signal(&Modes.data_cond);
pthread_mutex_unlock(&Modes.data_mutex);
pthread_exit(NULL);
}
/* ============================== Debugging ================================= */
/* Helper function for dumpMagnitudeVector().
* It prints a single bar used to display raw signals.
*
* Since every magnitude sample is between 0-255, the function uses
* up to 63 characters for every bar. Every character represents
* a length of 4, 3, 2, 1, specifically:
*
* "O" is 4
* "o" is 3
* "-" is 2
* "." is 1
*/
void dumpMagnitudeBar(int index, int magnitude) {
char *set = " .-o";
char buf[256];
int div = magnitude / 256 / 4;
int rem = magnitude / 256 % 4;
memset(buf,'O',div);
buf[div] = set[rem];
buf[div+1] = '\0';
if (index >= 0)
printf("[%.3d] |%-66s %d\n", index, buf, magnitude);
else
printf("[%.2d] |%-66s %d\n", index, buf, magnitude);
}
/* Display an ASCII-art alike graphical representation of the undecoded
* message as a magnitude signal.
*
* The message starts at the specified offset in the "m" buffer.
* The function will display enough data to cover a short 56 bit message.
*
* If possible a few samples before the start of the messsage are included
* for context. */
void dumpMagnitudeVector(uint16_t *m, uint32_t offset) {
uint32_t padding = 5; /* Show a few samples before the actual start. */
uint32_t start = (offset < padding) ? 0 : offset-padding;
uint32_t end = offset + (MODES_PREAMBLE_SAMPLES)+(MODES_SHORT_MSG_SAMPLES) - 1;
uint32_t j;
for (j = start; j <= end; j++) {
dumpMagnitudeBar(j-offset, m[j]);
}
}
/* Produce a raw representation of the message as a Javascript file
* loadable by debug.html. */
void dumpRawMessageJS(char *descr, unsigned char *msg,
uint16_t *m, uint32_t offset, int fixable, char *bitpos)
{
int padding = 5; /* Show a few samples before the actual start. */
int start = offset - padding;
int end = offset + (MODES_PREAMBLE_SAMPLES)+(MODES_LONG_MSG_SAMPLES) - 1;
FILE *fp;
int j;
MODES_NOTUSED(fixable);
if ((fp = fopen("frames.js","a")) == NULL) {
fprintf(stderr, "Error opening frames.js: %s\n", strerror(errno));
exit(1);
}
fprintf(fp,"frames.push({\"descr\": \"%s\", \"mag\": [", descr);
for (j = start; j <= end; j++) {
fprintf(fp,"%d", j < 0 ? 0 : m[j]);
if (j != end) fprintf(fp,",");
}
fprintf(fp,"], \"fix1\": %d, \"fix2\": %d, \"bits\": %d, \"hex\": \"",
bitpos[0], bitpos[1] , modesMessageLenByType(msg[0]>>3));
for (j = 0; j < MODES_LONG_MSG_BYTES; j++)
fprintf(fp,"\\x%02x",msg[j]);
fprintf(fp,"\"});\n");
fclose(fp);
}
/* This is a wrapper for dumpMagnitudeVector() that also show the message
* in hex format with an additional description.
*
* descr is the additional message to show to describe the dump.
* msg points to the decoded message
* m is the original magnitude vector
* offset is the offset where the message starts
*
* The function also produces the Javascript file used by debug.html to
* display packets in a graphical format if the Javascript output was
* enabled.
*/
void dumpRawMessage(char *descr, unsigned char *msg,
uint16_t *m, uint32_t offset)
{
int j;
int msgtype = msg[0] >> 3;
int fixable = 0;
char bitpos[MODES_MAX_BITERRORS];
for (j = 0; j < MODES_MAX_BITERRORS; j++) {
bitpos[j] = -1;
}
if (msgtype == 17) {
fixable = fixBitErrors(msg, MODES_LONG_MSG_BITS, MODES_MAX_BITERRORS, bitpos);
}
if (Modes.debug & MODES_DEBUG_JS) {
dumpRawMessageJS(descr, msg, m, offset, fixable, bitpos);
return;
}
printf("\n--- %s\n ", descr);
for (j = 0; j < MODES_LONG_MSG_BYTES; j++) {
printf("%02x",msg[j]);
if (j == MODES_SHORT_MSG_BYTES-1) printf(" ... ");
}
printf(" (DF %d, Fixable: %d)\n", msgtype, fixable);
dumpMagnitudeVector(m,offset);
printf("---\n\n");
}
/* ===================== Mode A/C detection and decoding =================== */
//
// This table is used to build the Mode A/C variable called ModeABits.Each
// bit period is inspected, and if it's value exceeds the threshold limit,
// then the value in this table is or-ed into ModeABits.
//
// At the end of message processing, ModeABits will be the decoded ModeA value.
//
// We can also flag noise in bits that should be zeros - the xx bits. Noise in
// these bits cause bits (31-16) in ModeABits to be set. Then at the end of message
// processing we can test for errors by looking at these bits.
//
uint32_t ModeABitTable[24] = {
0x00000000, // F1 = 1
0x00000010, // C1
0x00001000, // A1
0x00000020, // C2
0x00002000, // A2
0x00000040, // C4
0x00004000, // A4
0x40000000, // xx = 0 Set bit 30 if we see this high
0x00000100, // B1
0x00000001, // D1
0x00000200, // B2
0x00000002, // D2
0x00000400, // B4
0x00000004, // D4
0x00000000, // F2 = 1
0x08000000, // xx = 0 Set bit 27 if we see this high
0x04000000, // xx = 0 Set bit 26 if we see this high
0x00000080, // SPI
0x02000000, // xx = 0 Set bit 25 if we see this high
0x01000000, // xx = 0 Set bit 24 if we see this high
0x00800000, // xx = 0 Set bit 23 if we see this high
0x00400000, // xx = 0 Set bit 22 if we see this high
0x00200000, // xx = 0 Set bit 21 if we see this high
0x00100000, // xx = 0 Set bit 20 if we see this high
};
//
// This table is used to produce an error variable called ModeAErrs.Each
// inter-bit period is inspected, and if it's value falls outside of the
// expected range, then the value in this table is or-ed into ModeAErrs.
//
// At the end of message processing, ModeAErrs will indicate if we saw
// any inter-bit anomolies, and the bits that are set will show which
// bits had them.
//
uint32_t ModeAMidTable[24] = {
0x80000000, // F1 = 1 Set bit 31 if we see F1_C1 error
0x00000010, // C1 Set bit 4 if we see C1_A1 error
0x00001000, // A1 Set bit 12 if we see A1_C2 error
0x00000020, // C2 Set bit 5 if we see C2_A2 error
0x00002000, // A2 Set bit 13 if we see A2_C4 error
0x00000040, // C4 Set bit 6 if we see C3_A4 error
0x00004000, // A4 Set bit 14 if we see A4_xx error
0x40000000, // xx = 0 Set bit 30 if we see xx_B1 error
0x00000100, // B1 Set bit 8 if we see B1_D1 error
0x00000001, // D1 Set bit 0 if we see D1_B2 error
0x00000200, // B2 Set bit 9 if we see B2_D2 error
0x00000002, // D2 Set bit 1 if we see D2_B4 error
0x00000400, // B4 Set bit 10 if we see B4_D4 error
0x00000004, // D4 Set bit 2 if we see D4_F2 error
0x20000000, // F2 = 1 Set bit 29 if we see F2_xx error
0x08000000, // xx = 0 Set bit 27 if we see xx_xx error
0x04000000, // xx = 0 Set bit 26 if we see xx_SPI error
0x00000080, // SPI Set bit 15 if we see SPI_xx error
0x02000000, // xx = 0 Set bit 25 if we see xx_xx error
0x01000000, // xx = 0 Set bit 24 if we see xx_xx error
0x00800000, // xx = 0 Set bit 23 if we see xx_xx error
0x00400000, // xx = 0 Set bit 22 if we see xx_xx error
0x00200000, // xx = 0 Set bit 21 if we see xx_xx error
0x00100000, // xx = 0 Set bit 20 if we see xx_xx error
};
//
// The "off air" format is,,
// _F1_C1_A1_C2_A2_C4_A4_xx_B1_D1_B2_D2_B4_D4_F2_xx_xx_SPI_
//
// Bit spacing is 1.45uS, with 0.45uS high, and 1.00us low. This is a problem
// because we ase sampling at 2Mhz (500nS) so we are below Nyquist.
//
// The bit spacings are..
// F1 : 0.00,
// 1.45, 2.90, 4.35, 5.80, 7.25, 8.70,
// X : 10.15,
// : 11.60, 13.05, 14.50, 15.95, 17.40, 18.85,
// F2 : 20.30,
// X : 21.75, 23.20, 24.65
//
// This equates to the following sample point centers at 2Mhz.
// [ 0.0],
// [ 2.9], [ 5.8], [ 8.7], [11.6], [14.5], [17.4],
// [20.3],
// [23.2], [26.1], [29.0], [31.9], [34.8], [37.7]
// [40.6]
// [43.5], [46.4], [49.3]
//
// We know that this is a supposed to be a binary stream, so the signal
// should either be a 1 or a 0. Therefore, any energy above the noise level
// in two adjacent samples must be from the same pulse, so we can simply
// add the values together..
//
int detectModeA(uint16_t *m, struct modesMessage *mm)
{
int j, lastBitWasOne;
int ModeABits = 0;
int ModeAErrs = 0;
int byte, bit;
int thisSample, lastBit, lastSpace = 0;
int m0, m1, m2, m3, mPhase;
int n0, n1, n2 ,n3;
int F1_sig, F1_noise;
int F2_sig, F2_noise;
int fSig, fNoise, fLevel, fLoLo;
// m[0] contains the energy from 0 -> 499 nS
// m[1] contains the energy from 500 -> 999 nS
// m[2] contains the energy from 1000 -> 1499 nS
// m[3] contains the energy from 1500 -> 1999 nS
//
// We are looking for a Frame bit (F1) whose width is 450nS, followed by
// 1000nS of quiet.
//
// The width of the frame bit is 450nS, which is 90% of our sample rate.
// Therefore, in an ideal world, all the energy for the frame bit will be
// in a single sample, preceeded by (at least) one zero, and followed by
// two zeros, Best case we can look for ...
//
// 0 - 1 - 0 - 0
//
// However, our samples are not phase aligned, so some of the energy from
// each bit could be spread over two consecutive samples. Worst case is
// that we sample half in one bit, and half in the next. In that case,
// we're looking for
//
// 0 - 0.5 - 0.5 - 0.
m0 = m[0]; m1 = m[1];
if (m0 >= m1) // m1 *must* be bigger than m0 for this to be F1
{return (0);}
m2 = m[2]; m3 = m[3];
//
// if (m2 <= m0), then assume the sample bob on (Phase == 0), so don't look at m3
if ((m2 <= m0) || (m2 < m3))
{m3 = m2; m2 = m0;}
if ( (m3 >= m1) // m1 must be bigger than m3
|| (m0 > m2) // m2 can be equal to m0 if ( 0,1,0,0 )
|| (m3 > m2) ) // m2 can be equal to m3 if ( 0,1,0,0 )
{return (0);}
// m0 = noise
// m1 = noise + (signal * X))
// m2 = noise + (signal * (1-X))
// m3 = noise
//
// Hence, assuming all 4 samples have similar amounts of noise in them
// signal = (m1 + m2) - ((m0 + m3) * 2)
// noise = (m0 + m3) / 2
//
F1_sig = (m1 + m2) - ((m0 + m3) << 1);
F1_noise = (m0 + m3) >> 1;
if ( (F1_sig < MODEAC_MSG_SQUELCH_LEVEL) // minimum required F1 signal amplitude
|| (F1_sig < (F1_noise << 2)) ) // minimum allowable Sig/Noise ratio 4:1
{return (0);}
// If we get here then we have a potential F1, so look for an equally valid F2 20.3uS later
//
// Our F1 is centered somewhere between samples m[1] and m[2]. We can guestimate where F2 is
// by comparing the ratio of m1 and m2, and adding on 20.3 uS (40.6 samples)
//
mPhase = ((m2 * 20) / (m1 + m2));
byte = (mPhase + 812) / 20;
n0 = m[byte++]; n1 = m[byte++];
if (n0 >= n1) // n1 *must* be bigger than n0 for this to be F2
{return (0);}
n2 = m[byte++];
//
// if the sample bob on (Phase == 0), don't look at n3
//
if ((mPhase + 812) % 20)
{n3 = m[byte++];}
else
{n3 = n2; n2 = n0;}
if ( (n3 >= n1) // n1 must be bigger than n3
|| (n0 > n2) // n2 can be equal to n0 ( 0,1,0,0 )
|| (n3 > n2) ) // n2 can be equal to n3 ( 0,1,0,0 )
{return (0);}
F2_sig = (n1 + n2) - ((n0 + n3) << 1);
F2_noise = (n0 + n3) >> 1;
if ( (F2_sig < MODEAC_MSG_SQUELCH_LEVEL) // minimum required F2 signal amplitude
|| (F2_sig < (F2_noise << 2)) ) // maximum allowable Sig/Noise ratio 4:1
{return (0);}
fSig = (F1_sig + F2_sig) >> 1;
fNoise = (F1_noise + F2_noise) >> 1;
fLoLo = fNoise + (fSig >> 2); // 1/2
fLevel = fNoise + (fSig >> 1);
lastBitWasOne = 1;
lastBit = F1_sig;
//
// Now step by a half ModeA bit, 0.725nS, which is 1.45 samples, which is 29/20
// No need to do bit 0 because we've already selected it as a valid F1
// Do several bits past the SPI to increase error rejection
//
for (j = 1, mPhase += 29; j < 48; mPhase += 29, j ++)
{
byte = 1 + (mPhase / 20);
thisSample = m[byte] - fNoise;
if (mPhase % 20) // If the bit is split over two samples...
{thisSample += (m[byte+1] - fNoise);} // add in the second sample's energy
// If we're calculating a space value
if (j & 1)
{lastSpace = thisSample;}
else
{// We're calculating a new bit value
bit = j >> 1;
if (thisSample >= fLevel)
{// We're calculating a new bit value, and its a one
ModeABits |= ModeABitTable[bit--]; // or in the correct bit
if (lastBitWasOne)
{ // This bit is one, last bit was one, so check the last space is somewhere less than one
if ( (lastSpace >= (thisSample>>1)) || (lastSpace >= lastBit) )
{ModeAErrs |= ModeAMidTable[bit];}
}
else
{// This bit,is one, last bit was zero, so check the last space is somewhere less than one
if (lastSpace >= (thisSample >> 1))
{ModeAErrs |= ModeAMidTable[bit];}
}
lastBitWasOne = 1;
}
else
{// We're calculating a new bit value, and its a zero
if (lastBitWasOne)
{ // This bit is zero, last bit was one, so check the last space is somewhere in between
if (lastSpace >= lastBit)
{ModeAErrs |= ModeAMidTable[bit];}
}
else
{// This bit,is zero, last bit was zero, so check the last space is zero too
if (lastSpace >= fLoLo)
{ModeAErrs |= ModeAMidTable[bit];}
}
lastBitWasOne = 0;
}
lastBit = (thisSample >> 1);
}
}
//
// Output format is : 00:A4:A2:A1:00:B4:B2:B1:00:C4:C2:C1:00:D4:D2:D1
//
if ((ModeABits < 3) || (ModeABits & 0xFFFF8808) || (ModeAErrs) )
{return (ModeABits = 0);}
fSig = (fSig + 0x7F) >> 8;
mm->signalLevel = ((fSig < 255) ? fSig : 255);
return ModeABits;
}
// Input format is : 00:A4:A2:A1:00:B4:B2:B1:00:C4:C2:C1:00:D4:D2:D1
int ModeAToModeC(unsigned int ModeA)
{
unsigned int FiveHundreds = 0;
unsigned int OneHundreds = 0;
if ( (ModeA & 0xFFFF888B) // D1 set is illegal. D2 set is > 62700ft which is unlikely
|| ((ModeA & 0x000000F0) == 0) ) // C1,,C4 cannot be Zero
{return -9999;}
if (ModeA & 0x0010) {OneHundreds ^= 0x007;} // C1
if (ModeA & 0x0020) {OneHundreds ^= 0x003;} // C2
if (ModeA & 0x0040) {OneHundreds ^= 0x001;} // C4
// Remove 7s from OneHundreds (Make 7->5, snd 5->7).
if ((OneHundreds & 5) == 5) {OneHundreds ^= 2;}
// Check for invalid codes, only 1 to 5 are valid
if (OneHundreds > 5)
{return -9999;}
//if (ModeA & 0x0001) {FiveHundreds ^= 0x1FF;} // D1 never used for altitude
if (ModeA & 0x0002) {FiveHundreds ^= 0x0FF;} // D2
if (ModeA & 0x0004) {FiveHundreds ^= 0x07F;} // D4
if (ModeA & 0x1000) {FiveHundreds ^= 0x03F;} // A1
if (ModeA & 0x2000) {FiveHundreds ^= 0x01F;} // A2
if (ModeA & 0x4000) {FiveHundreds ^= 0x00F;} // A4
if (ModeA & 0x0100) {FiveHundreds ^= 0x007;} // B1
if (ModeA & 0x0200) {FiveHundreds ^= 0x003;} // B2
if (ModeA & 0x0400) {FiveHundreds ^= 0x001;} // B4
// Correct order of OneHundreds.
if (FiveHundreds & 1) {OneHundreds = 6 - OneHundreds;}
return ((FiveHundreds * 5) + OneHundreds - 13);
}
void decodeModeAMessage(struct modesMessage *mm, int ModeA)
{
mm->msgtype = 32; // Valid Mode S DF's are DF-00 to DF-31.
// so use 32 to indicate Mode A/C
mm->msgbits = 16; // Fudge up a Mode S style data stream
mm->msg[0] = (ModeA >> 8);
mm->msg[1] = (ModeA);
// Fudge an ICAO address based on Mode A (remove the Ident bit)
// Use an upper address byte of FF, since this is ICAO unallocated
mm->addr = 0x00FF0000 | (ModeA & 0x0000FF7F);
// Set the Identity field to ModeA
mm->modeA = ModeA & 0x7777;
mm->bFlags |= MODES_ACFLAGS_SQUAWK_VALID;
// Flag ident in flight status
mm->fs = ModeA & 0x0080;
// Not much else we can tell from a Mode A/C reply.
// Just fudge up a few bits to keep other code happy
mm->crcok = 1;
mm->correctedbits = 0;
}
/* ===================== Mode S detection and decoding ===================== */
/* Parity table for MODE S Messages.
* The table contains 112 elements, every element corresponds to a bit set
* in the message, starting from the first bit of actual data after the
* preamble.
*
* For messages of 112 bit, the whole table is used.
* For messages of 56 bits only the last 56 elements are used.
*
* The algorithm is as simple as xoring all the elements in this table
* for which the corresponding bit on the message is set to 1.
*
* The latest 24 elements in this table are set to 0 as the checksum at the
* end of the message should not affect the computation.
*
* Note: this function can be used with DF11 and DF17, other modes have
* the CRC xored with the sender address as they are reply to interrogations,
* but a casual listener can't split the address from the checksum.
*/
uint32_t modes_checksum_table[112] = {
0x3935ea, 0x1c9af5, 0xf1b77e, 0x78dbbf, 0xc397db, 0x9e31e9, 0xb0e2f0, 0x587178,
0x2c38bc, 0x161c5e, 0x0b0e2f, 0xfa7d13, 0x82c48d, 0xbe9842, 0x5f4c21, 0xd05c14,
0x682e0a, 0x341705, 0xe5f186, 0x72f8c3, 0xc68665, 0x9cb936, 0x4e5c9b, 0xd8d449,
0x939020, 0x49c810, 0x24e408, 0x127204, 0x093902, 0x049c81, 0xfdb444, 0x7eda22,
0x3f6d11, 0xe04c8c, 0x702646, 0x381323, 0xe3f395, 0x8e03ce, 0x4701e7, 0xdc7af7,
0x91c77f, 0xb719bb, 0xa476d9, 0xadc168, 0x56e0b4, 0x2b705a, 0x15b82d, 0xf52612,
0x7a9309, 0xc2b380, 0x6159c0, 0x30ace0, 0x185670, 0x0c2b38, 0x06159c, 0x030ace,
0x018567, 0xff38b7, 0x80665f, 0xbfc92b, 0xa01e91, 0xaff54c, 0x57faa6, 0x2bfd53,
0xea04ad, 0x8af852, 0x457c29, 0xdd4410, 0x6ea208, 0x375104, 0x1ba882, 0x0dd441,
0xf91024, 0x7c8812, 0x3e4409, 0xe0d800, 0x706c00, 0x383600, 0x1c1b00, 0x0e0d80,
0x0706c0, 0x038360, 0x01c1b0, 0x00e0d8, 0x00706c, 0x003836, 0x001c1b, 0xfff409,
0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000,
0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000,
0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000
};
uint32_t modesChecksum(unsigned char *msg, int bits) {
uint32_t crc = 0;
uint32_t rem = 0;
int offset = (bits == 112) ? 0 : (112-56);
uint8_t theByte = *msg;
uint32_t * pCRCTable = &modes_checksum_table[offset];
int j;
// We don't really need to include the checksum itself
bits -= 24;
for(j = 0; j < bits; j++) {
if ((j & 7) == 0)
theByte = *msg++;
// If bit is set, xor with corresponding table entry.
if (theByte & 0x80) {crc ^= *pCRCTable;}
pCRCTable++;
theByte = theByte << 1;
}
rem = (msg[0] << 16) | (msg[1] << 8) | msg[2]; // message checksum
return ((crc ^ rem) & 0x00FFFFFF); // 24 bit checksum syndrome.
}
//
// Given the Downlink Format (DF) of the message, return the message length in bits.
//
// All known DF's 16 or greater are long. All known DF's 15 or less are short.
// There are lots of unused codes in both category, so we can assume ICAO will stick to
// these rules, meaning that the most significant bit of the DF indicates the length.
//
int modesMessageLenByType(int type) {
return (type & 0x10) ? MODES_LONG_MSG_BITS : MODES_SHORT_MSG_BITS ;
}
//
// Try to fix single bit errors using the checksum. On success modifies
// the original buffer with the fixed version, and returns the position
// of the error bit. Otherwise if fixing failed -1 is returned.
//
int fixSingleBitErrors(unsigned char *msg, int bits) {
int j;
unsigned char aux[MODES_LONG_MSG_BYTES];
memcpy(aux, msg, bits/8);
// Do not attempt to error correct Bits 0-4. These contain the DF, and must
// be correct because we can only error correct DF17
for (j = 5; j < bits; j++) {
int byte = j/8;
int bitmask = 1 << (7 - (j & 7));
aux[byte] ^= bitmask; // Flip j-th bit
if (0 == modesChecksum(aux, bits)) {
// The error is fixed. Overwrite the original buffer with the
// corrected sequence, and returns the error bit position
msg[byte] = aux[byte];
return (j);
}
aux[byte] ^= bitmask; // Flip j-th bit back again
}
return (-1);
}
//
// Similar to fixSingleBitErrors() but try every possible two bit combination.
// This is very slow and should be tried only against DF17 messages that
// don't pass the checksum, and only in Aggressive Mode.
/*
int fixTwoBitsErrors(unsigned char *msg, int bits) {
int j, i;
unsigned char aux[MODES_LONG_MSG_BYTES];
memcpy(aux, msg, bits/8);
// Do not attempt to error correct Bits 0-4. These contain the DF, and must
// be correct because we can only error correct DF17
for (j = 5; j < bits; j++) {
int byte1 = j/8;
int bitmask1 = 1 << (7 - (j & 7));
aux[byte1] ^= bitmask1; // Flip j-th bit
// Don't check the same pairs multiple times, so i starts from j+1
for (i = j+1; i < bits; i++) {
int byte2 = i/8;
int bitmask2 = 1 << (7 - (i & 7));
aux[byte2] ^= bitmask2; // Flip i-th bit
if (0 == modesChecksum(aux, bits)) {
// The error is fixed. Overwrite the original buffer with
// the corrected sequence, and returns the error bit position
msg[byte1] = aux[byte1];
msg[byte2] = aux[byte2];
// We return the two bits as a 16 bit integer by shifting
// 'i' on the left. This is possible since 'i' will always
// be non-zero because i starts from j+1
return (j | (i << 8));
aux[byte2] ^= bitmask2; // Flip i-th bit back
}
aux[byte1] ^= bitmask1; // Flip j-th bit back
}
}
return (-1);
}
*/
/* Code for introducing a less CPU-intensive method of correcting
* single bit errors.
*
* Makes use of the fact that the crc checksum is linear with respect to
* the bitwise xor operation, i.e.
* crc(m^e) = (crc(m)^crc(e)
* where m and e are the message resp. error bit vectors.
*
* Call crc(e) the syndrome.
*
* The code below works by precomputing a table of (crc(e), e) for all
* possible error vectors e (here only single bit and double bit errors),
* search for the syndrome in the table, and correct the then known error.
* The error vector e is represented by one or two bit positions that are
* changed. If a second bit position is not used, it is -1.
*
* Run-time is binary search in a sorted table, plus some constant overhead,
* instead of running through all possible bit positions (resp. pairs of
* bit positions).
*
*
*
*/
struct errorinfo {
uint32_t syndrome; // CRC syndrome
int bits; // Number of bit positions to fix
int pos[MODES_MAX_BITERRORS]; // Bit positions corrected by this syndrome
};
#define NERRORINFO \
(MODES_LONG_MSG_BITS+MODES_LONG_MSG_BITS*(MODES_LONG_MSG_BITS-1)/2)
struct errorinfo bitErrorTable[NERRORINFO];
/* Compare function as needed for stdlib's qsort and bsearch functions */
int cmpErrorInfo(const void *p0, const void *p1) {
struct errorinfo *e0 = (struct errorinfo*)p0;
struct errorinfo *e1 = (struct errorinfo*)p1;
if (e0->syndrome == e1->syndrome) {
return 0;
} else if (e0->syndrome < e1->syndrome) {
return -1;
} else {
return 1;
}
}
/* Compute the table of all syndromes for 1-bit and 2-bit error vectors */
void modesInitErrorInfo() {
unsigned char msg[MODES_LONG_MSG_BYTES];
int i, j, n;
uint32_t crc;
n = 0;
memset(bitErrorTable, 0, sizeof(bitErrorTable));
memset(msg, 0, MODES_LONG_MSG_BYTES);
// Add all possible single and double bit errors
// don't include errors in first 5 bits (DF type)
for (i = 5; i < MODES_LONG_MSG_BITS; i++) {
int bytepos0 = (i >> 3);
int mask0 = 1 << (7 - (i & 7));
msg[bytepos0] ^= mask0; // create error0
crc = modesChecksum(msg, MODES_LONG_MSG_BITS);
bitErrorTable[n].syndrome = crc; // single bit error case
bitErrorTable[n].bits = 1;
bitErrorTable[n].pos[0] = i;
bitErrorTable[n].pos[1] = -1;
n += 1;
if (Modes.nfix_crc > 1) {
for (j = i+1; j < MODES_LONG_MSG_BITS; j++) {
int bytepos1 = (j >> 3);
int mask1 = 1 << (7 - (j & 7));
msg[bytepos1] ^= mask1; // create error1
crc = modesChecksum(msg, MODES_LONG_MSG_BITS);
if (n >= NERRORINFO) {
//fprintf(stderr, "Internal error, too many entries, fix NERRORINFO\n");
break;
}
bitErrorTable[n].syndrome = crc; // two bit error case
bitErrorTable[n].bits = 2;