├── .gitignore ├── .travis.yml ├── LICENSE ├── Makefile ├── README.md ├── include └── mode-s.h ├── src └── mode-s.c └── tests └── test.c /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | tests/fixtures 3 | tests/test 4 | tests/results 5 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: c 2 | 3 | before_install: 4 | - eval "${MATRIX_EVAL}" 5 | 6 | script: make && make test 7 | 8 | matrix: 9 | include: 10 | - os: linux 11 | addons: 12 | apt: 13 | sources: 14 | - ubuntu-toolchain-r-test 15 | packages: 16 | - gcc-4.9 17 | env: 18 | - MATRIX_EVAL="CC=gcc-4.9" 19 | 20 | - os: linux 21 | addons: 22 | apt: 23 | sources: 24 | - ubuntu-toolchain-r-test 25 | packages: 26 | - gcc-5 27 | env: 28 | - MATRIX_EVAL="CC=gcc-5" 29 | 30 | - os: linux 31 | addons: 32 | apt: 33 | sources: 34 | - ubuntu-toolchain-r-test 35 | packages: 36 | - gcc-6 37 | env: 38 | - MATRIX_EVAL="CC=gcc-6" 39 | 40 | - os: linux 41 | addons: 42 | apt: 43 | sources: 44 | - ubuntu-toolchain-r-test 45 | packages: 46 | - gcc-7 47 | env: 48 | - MATRIX_EVAL="CC=gcc-7" 49 | 50 | - os: linux 51 | addons: 52 | apt: 53 | sources: 54 | - ubuntu-toolchain-r-test 55 | - llvm-toolchain-precise-3.6 56 | packages: 57 | - clang-3.6 58 | env: 59 | - MATRIX_EVAL="CC=clang-3.6" 60 | 61 | - os: linux 62 | addons: 63 | apt: 64 | sources: 65 | - ubuntu-toolchain-r-test 66 | - llvm-toolchain-precise-3.7 67 | packages: 68 | - clang-3.7 69 | env: 70 | - MATRIX_EVAL="CC=clang-3.7" 71 | 72 | - os: linux 73 | addons: 74 | apt: 75 | sources: 76 | - ubuntu-toolchain-r-test 77 | - llvm-toolchain-precise-3.8 78 | packages: 79 | - clang-3.8 80 | env: 81 | - MATRIX_EVAL="CC=clang-3.8" 82 | 83 | - os: linux 84 | addons: 85 | apt: 86 | sources: 87 | - llvm-toolchain-trusty-3.9 88 | packages: 89 | - clang-3.9 90 | env: 91 | - MATRIX_EVAL="CC=clang-3.9" 92 | 93 | - os: linux 94 | addons: 95 | apt: 96 | sources: 97 | - llvm-toolchain-trusty-4.0 98 | packages: 99 | - clang-4.0 100 | env: 101 | - MATRIX_EVAL="CC=clang-4.0" 102 | 103 | - os: linux 104 | addons: 105 | apt: 106 | sources: 107 | - llvm-toolchain-trusty-5.0 108 | packages: 109 | - clang-5.0 110 | env: 111 | - MATRIX_EVAL="CC=clang-5.0" 112 | 113 | - os: osx 114 | osx_image: xcode8 115 | env: 116 | - MATRIX_EVAL="CC=gcc-4.9" 117 | 118 | - os: osx 119 | osx_image: xcode8 120 | env: 121 | - MATRIX_EVAL="brew update && brew install gcc5 && CC=gcc-5" 122 | 123 | - os: osx 124 | osx_image: xcode8 125 | env: 126 | - MATRIX_EVAL="brew update && brew install gcc6 && CC=gcc-6" 127 | 128 | - os: osx 129 | osx_image: xcode8 130 | env: 131 | - MATRIX_EVAL="brew update && brew install gcc && CC=gcc-7" 132 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2012, Salvatore Sanfilippo 4 | Copyright (c) 2017, Thomas Watson 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | INCLUDE = ./include 2 | CFLAGS ?= -O2 -g -Wall -W 3 | LDFLAGS += -lpthread -lm 4 | CC ?= gcc 5 | 6 | test_file := tests/test 7 | test_fixtires_dir := tests/fixtures 8 | test_results := tests/results 9 | 10 | .PHONY: all test clean 11 | .DELETE_ON_ERROR: 12 | 13 | all: $(test_file) 14 | 15 | %.o: %.c 16 | $(CC) -c $(CFLAGS) -I${INCLUDE} $^ -o $@ 17 | 18 | $(test_file): tests/test.o src/mode-s.o 19 | $(CC) ${CFLAGS} $^ ${LDFLAGS} -o $@ 20 | 21 | test: $(test_results) 22 | 23 | $(test_results): $(test_file) 24 | if [ ! -d "$(test_fixtires_dir)" ]; then \ 25 | git clone --depth=1 https://github.com/watson/libmodes-test-fixtures.git $(test_fixtires_dir); \ 26 | else \ 27 | (cd $(test_fixtires_dir) && git pull --depth=1 origin master); \ 28 | fi 29 | $(test_file) $(test_fixtires_dir)/dump.bin | tee $@ 30 | 31 | clean: 32 | rm -fr **/*.o $(test_file) $(test_fixtires_dir) $(test_results) 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # libmodes 2 | 3 | [![Build status](https://travis-ci.org/watson/libmodes.svg?branch=master)](https://travis-ci.org/watson/libmodes) 4 | 5 | This is a C library for decoding Mode S messages from aviation 6 | aircrafts. It supports both standard Mode S Acquisition Squitter 7 | messages (56 bits) and Mode S Extended Squitter messages (112 bits) that 8 | also carry ADS-B information. 9 | 10 | This project is a refactoring of the popular 11 | [dump1090](https://github.com/antirez/dump1090) project by Salvatore 12 | Sanfilippo. It modularizes the code into separate functions and removes 13 | all non-essentials, so that only the decoding logic is left. 14 | 15 | ## Usage 16 | 17 | ```c 18 | #include "mode-s.h" 19 | #include 20 | 21 | void on_msg(mode_s_t *self, struct mode_s_msg *mm) { 22 | printf("Got message from flight %s at altitude %d\n", mm->flight, mm->altitude); 23 | } 24 | 25 | int main(int argc, char **argv) { 26 | mode_s_t state; 27 | uint32_t data_len = 262620; 28 | unsigned char data[data_len]; 29 | uint16_t mag[data_len / 2]; 30 | 31 | // initialize the decoder state 32 | mode_s_init(&state); 33 | 34 | // get some raw IQ data somehow 35 | get_samples(&data); 36 | 37 | // compute the magnitude of the signal 38 | mode_s_compute_magnitude_vector(&data, &mag, data_len); 39 | 40 | // detect Mode S messages in the signal and call on_msg with each message 41 | mode_s_detect(&state, &mag, data_len/2, on_msg); 42 | } 43 | ``` 44 | 45 | Check out 46 | [`tests/test.c`](https://github.com/watson/libmodes/blob/master/tests/test.c) 47 | for a complete example. 48 | 49 | ## Message Format 50 | 51 | The provided callback to `mode_s_detect` will be called with a 52 | `mode_s_msg` struct. This struct contain the following fields: 53 | 54 | ### Generic fields 55 | 56 | - `unsigned char msg[14]` - Binary message 57 | - `int msgbits` - Number of bits in message 58 | - `int msgtype` - Downlink format # 59 | - `int crcok` - True if CRC was valid 60 | - `uint32_t crc` - Message CRC 61 | - `int errorbit` - Bit corrected. `-1` if no bit corrected 62 | - `int aa1` - ICAO Address byte 1 63 | - `int aa2` - ICAO Address byte 2 64 | - `int aa3` - ICAO Address byte 3 65 | - `int phase_corrected` - True if phase correction was applied 66 | 67 | ### DF 11 68 | 69 | - `int ca` - Responder capabilities 70 | 71 | ### DF 17 72 | 73 | - `int metype` - Extended squitter message type 74 | - `int mesub` - Extended squitter message subtype 75 | - `int heading_is_valid` 76 | - `int heading` 77 | - `int aircraft_type` 78 | - `int fflag` - `1` = Odd, `0` = Even CPR message 79 | - `int tflag` - UTC synchronized? 80 | - `int raw_latitude` - Non decoded latitude 81 | - `int raw_longitude` - Non decoded longitude 82 | - `char flight[9]` - 8 chars flight number 83 | - `int ew_dir` - `0` = East, `1` = West 84 | - `int ew_velocity` - E/W velocity 85 | - `int ns_dir` - `0` = North, `1` = South 86 | - `int ns_velocity` - N/S velocity. 87 | - `int vert_rate_source` - Vertical rate source 88 | - `int vert_rate_sign` - Vertical rate sign 89 | - `int vert_rate` - Vertical rate 90 | - `int velocity` - Computed from EW and NS velocity 91 | 92 | ### DF4, DF5, DF20, DF21 93 | 94 | - `int fs` - Flight status for DF4, 5, 20, 21 95 | - `int dr` - Request extraction of downlink request 96 | - `int um` - Request extraction of downlink request 97 | - `int identity` - 13 bits identity (Squawk) 98 | 99 | ### Fields used by multiple message types 100 | 101 | - `int altitude` 102 | - `int unit` 103 | 104 | ## Testing 105 | 106 | To test the library, simply run: 107 | 108 | ``` 109 | make && make test 110 | ``` 111 | 112 | Note that the first time you run `make test`, a large (ca. 50MB) test 113 | fixture will be downloaded to `tests/fixtures`. You can delete this 114 | folder at any time if you wish. 115 | 116 | ## License 117 | 118 | BSD-2-Clause 119 | -------------------------------------------------------------------------------- /include/mode-s.h: -------------------------------------------------------------------------------- 1 | #ifndef __MODE_S_DECODER_H 2 | #define __MODE_S_DECODER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define MODE_S_ICAO_CACHE_LEN 1024 // Power of two required 12 | #define MODE_S_LONG_MSG_BYTES (112/8) 13 | #define MODE_S_UNIT_FEET 0 14 | #define MODE_S_UNIT_METERS 1 15 | 16 | // Program state 17 | typedef struct { 18 | // Internal state 19 | uint32_t icao_cache[sizeof(uint32_t)*MODE_S_ICAO_CACHE_LEN*2]; // Recently seen ICAO addresses cache 20 | 21 | // Configuration 22 | int fix_errors; // Single bit error correction if true 23 | int aggressive; // Aggressive detection algorithm 24 | int check_crc; // Only display messages with good CRC 25 | } mode_s_t; 26 | 27 | // The struct we use to store information about a decoded message 28 | struct mode_s_msg { 29 | // Generic fields 30 | unsigned char msg[MODE_S_LONG_MSG_BYTES]; // Binary message 31 | int msgbits; // Number of bits in message 32 | int msgtype; // Downlink format # 33 | int crcok; // True if CRC was valid 34 | uint32_t crc; // Message CRC 35 | int errorbit; // Bit corrected. -1 if no bit corrected. 36 | int aa1, aa2, aa3; // ICAO Address bytes 1 2 and 3 37 | int phase_corrected; // True if phase correction was applied. 38 | 39 | // DF 11 40 | int ca; // Responder capabilities. 41 | 42 | // DF 17 43 | int metype; // Extended squitter message type. 44 | int mesub; // Extended squitter message subtype. 45 | int heading_is_valid; 46 | int heading; 47 | int aircraft_type; 48 | int fflag; // 1 = Odd, 0 = Even CPR message. 49 | int tflag; // UTC synchronized? 50 | int raw_latitude; // Non decoded latitude 51 | int raw_longitude; // Non decoded longitude 52 | char flight[9]; // 8 chars flight number. 53 | int ew_dir; // 0 = East, 1 = West. 54 | int ew_velocity; // E/W velocity. 55 | int ns_dir; // 0 = North, 1 = South. 56 | int ns_velocity; // N/S velocity. 57 | int vert_rate_source; // Vertical rate source. 58 | int vert_rate_sign; // Vertical rate sign. 59 | int vert_rate; // Vertical rate. 60 | int velocity; // Computed from EW and NS velocity. 61 | 62 | // DF4, DF5, DF20, DF21 63 | int fs; // Flight status for DF4,5,20,21 64 | int dr; // Request extraction of downlink request. 65 | int um; // Request extraction of downlink request. 66 | int identity; // 13 bits identity (Squawk). 67 | 68 | // Fields used by multiple message types. 69 | int altitude, unit; 70 | }; 71 | 72 | typedef void (*mode_s_callback_t)(mode_s_t *self, struct mode_s_msg *mm); 73 | 74 | void mode_s_init(mode_s_t *self); 75 | void mode_s_compute_magnitude_vector(unsigned char *data, uint16_t *mag, uint32_t size); 76 | void mode_s_detect(mode_s_t *self, uint16_t *mag, uint32_t maglen, mode_s_callback_t); 77 | void mode_s_decode(mode_s_t *self, struct mode_s_msg *mm, unsigned char *msg); 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /src/mode-s.c: -------------------------------------------------------------------------------- 1 | #include "mode-s.h" 2 | 3 | #define MODE_S_PREAMBLE_US 8 // microseconds 4 | #define MODE_S_LONG_MSG_BITS 112 5 | #define MODE_S_SHORT_MSG_BITS 56 6 | #define MODE_S_FULL_LEN (MODE_S_PREAMBLE_US+MODE_S_LONG_MSG_BITS) 7 | 8 | #define MODE_S_ICAO_CACHE_TTL 60 // Time to live of cached addresses. 9 | 10 | static uint16_t maglut[129*129*2]; 11 | static int maglut_initialized = 0; 12 | 13 | // =============================== Initialization =========================== 14 | 15 | void mode_s_init(mode_s_t *self) { 16 | int i, q; 17 | 18 | self->fix_errors = 1; 19 | self->check_crc = 1; 20 | self->aggressive = 0; 21 | 22 | // Allocate the ICAO address cache. We use two uint32_t for every entry 23 | // because it's a addr / timestamp pair for every entry 24 | memset(&self->icao_cache, 0, sizeof(self->icao_cache)); 25 | 26 | // Populate the I/Q -> Magnitude lookup table. It is used because sqrt or 27 | // round may be expensive and may vary a lot depending on the libc used. 28 | // 29 | // We scale to 0-255 range multiplying by 1.4 in order to ensure that every 30 | // different I/Q pair will result in a different magnitude value, not losing 31 | // any resolution. 32 | if (!maglut_initialized) { 33 | for (i = 0; i <= 128; i++) { 34 | for (q = 0; q <= 128; q++) { 35 | maglut[i*129+q] = round(sqrt(i*i+q*q)*360); 36 | } 37 | } 38 | maglut_initialized = 1; 39 | } 40 | } 41 | 42 | // ===================== Mode S detection and decoding ===================== 43 | 44 | // Parity table for MODE S Messages. 45 | // 46 | // The table contains 112 elements, every element corresponds to a bit set in 47 | // the message, starting from the first bit of actual data after the preamble. 48 | // 49 | // For messages of 112 bit, the whole table is used. For messages of 56 bits 50 | // only the last 56 elements are used. 51 | // 52 | // The algorithm is as simple as xoring all the elements in this table for 53 | // which the corresponding bit on the message is set to 1. 54 | // 55 | // The latest 24 elements in this table are set to 0 as the checksum at the end 56 | // of the message should not affect the computation. 57 | // 58 | // Note: this function can be used with DF11 and DF17, other modes have the CRC 59 | // xored with the sender address as they are reply to interrogations, but a 60 | // casual listener can't split the address from the checksum. 61 | uint32_t mode_s_checksum_table[] = { 62 | 0x3935ea, 0x1c9af5, 0xf1b77e, 0x78dbbf, 0xc397db, 0x9e31e9, 0xb0e2f0, 0x587178, 63 | 0x2c38bc, 0x161c5e, 0x0b0e2f, 0xfa7d13, 0x82c48d, 0xbe9842, 0x5f4c21, 0xd05c14, 64 | 0x682e0a, 0x341705, 0xe5f186, 0x72f8c3, 0xc68665, 0x9cb936, 0x4e5c9b, 0xd8d449, 65 | 0x939020, 0x49c810, 0x24e408, 0x127204, 0x093902, 0x049c81, 0xfdb444, 0x7eda22, 66 | 0x3f6d11, 0xe04c8c, 0x702646, 0x381323, 0xe3f395, 0x8e03ce, 0x4701e7, 0xdc7af7, 67 | 0x91c77f, 0xb719bb, 0xa476d9, 0xadc168, 0x56e0b4, 0x2b705a, 0x15b82d, 0xf52612, 68 | 0x7a9309, 0xc2b380, 0x6159c0, 0x30ace0, 0x185670, 0x0c2b38, 0x06159c, 0x030ace, 69 | 0x018567, 0xff38b7, 0x80665f, 0xbfc92b, 0xa01e91, 0xaff54c, 0x57faa6, 0x2bfd53, 70 | 0xea04ad, 0x8af852, 0x457c29, 0xdd4410, 0x6ea208, 0x375104, 0x1ba882, 0x0dd441, 71 | 0xf91024, 0x7c8812, 0x3e4409, 0xe0d800, 0x706c00, 0x383600, 0x1c1b00, 0x0e0d80, 72 | 0x0706c0, 0x038360, 0x01c1b0, 0x00e0d8, 0x00706c, 0x003836, 0x001c1b, 0xfff409, 73 | 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 74 | 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 75 | 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000 76 | }; 77 | 78 | uint32_t mode_s_checksum(unsigned char *msg, int bits) { 79 | uint32_t crc = 0; 80 | int offset = (bits == 112) ? 0 : (112-56); 81 | int j; 82 | 83 | for(j = 0; j < bits; j++) { 84 | int byte = j/8; 85 | int bit = j%8; 86 | int bitmask = 1 << (7-bit); 87 | 88 | // If bit is set, xor with corresponding table entry. 89 | if (msg[byte] & bitmask) 90 | crc ^= mode_s_checksum_table[j+offset]; 91 | } 92 | return crc; // 24 bit checksum. 93 | } 94 | 95 | // Given the Downlink Format (DF) of the message, return the message length in 96 | // bits. 97 | int mode_s_msg_len_by_type(int type) { 98 | if (type == 16 || type == 17 || 99 | type == 19 || type == 20 || 100 | type == 21) 101 | return MODE_S_LONG_MSG_BITS; 102 | else 103 | return MODE_S_SHORT_MSG_BITS; 104 | } 105 | 106 | // Try to fix single bit errors using the checksum. On success modifies the 107 | // original buffer with the fixed version, and returns the position of the 108 | // error bit. Otherwise if fixing failed -1 is returned. 109 | int fix_single_bit_errors(unsigned char *msg, int bits) { 110 | int j; 111 | unsigned char aux[MODE_S_LONG_MSG_BITS/8]; 112 | 113 | for (j = 0; j < bits; j++) { 114 | int byte = j/8; 115 | int bitmask = 1 << (7-(j%8)); 116 | uint32_t crc1, crc2; 117 | 118 | memcpy(aux, msg, bits/8); 119 | aux[byte] ^= bitmask; // Flip j-th bit. 120 | 121 | crc1 = ((uint32_t)aux[(bits/8)-3] << 16) | 122 | ((uint32_t)aux[(bits/8)-2] << 8) | 123 | (uint32_t)aux[(bits/8)-1]; 124 | crc2 = mode_s_checksum(aux, bits); 125 | 126 | if (crc1 == crc2) { 127 | // The error is fixed. Overwrite the original buffer with the 128 | // corrected sequence, and returns the error bit position. 129 | memcpy(msg, aux, bits/8); 130 | return j; 131 | } 132 | } 133 | return -1; 134 | } 135 | 136 | // Similar to fix_single_bit_errors() but try every possible two bit 137 | // combination. This is very slow and should be tried only against DF17 138 | // messages that don't pass the checksum, and only in Aggressive Mode. 139 | int fix_two_bits_errors(unsigned char *msg, int bits) { 140 | int j, i; 141 | unsigned char aux[MODE_S_LONG_MSG_BITS/8]; 142 | 143 | for (j = 0; j < bits; j++) { 144 | int byte1 = j/8; 145 | int bitmask1 = 1 << (7-(j%8)); 146 | 147 | // Don't check the same pairs multiple times, so i starts from j+1 148 | for (i = j+1; i < bits; i++) { 149 | int byte2 = i/8; 150 | int bitmask2 = 1 << (7-(i%8)); 151 | uint32_t crc1, crc2; 152 | 153 | memcpy(aux, msg, bits/8); 154 | 155 | aux[byte1] ^= bitmask1; // Flip j-th bit. 156 | aux[byte2] ^= bitmask2; // Flip i-th bit. 157 | 158 | crc1 = ((uint32_t)aux[(bits/8)-3] << 16) | 159 | ((uint32_t)aux[(bits/8)-2] << 8) | 160 | (uint32_t)aux[(bits/8)-1]; 161 | crc2 = mode_s_checksum(aux, bits); 162 | 163 | if (crc1 == crc2) { 164 | // The error is fixed. Overwrite the original buffer with the 165 | // corrected sequence, and returns the error bit position. 166 | memcpy(msg, aux, bits/8); 167 | // We return the two bits as a 16 bit integer by shifting 'i' 168 | // on the left. This is possible since 'i' will always be 169 | // non-zero because i starts from j+1. 170 | return j | (i<<8); 171 | } 172 | } 173 | } 174 | return -1; 175 | } 176 | 177 | // Hash the ICAO address to index our cache of MODE_S_ICAO_CACHE_LEN elements, 178 | // that is assumed to be a power of two. 179 | uint32_t icao_cache_has_addr(uint32_t a) { 180 | // The following three rounds wil make sure that every bit affects every 181 | // output bit with ~ 50% of probability. 182 | a = ((a >> 16) ^ a) * 0x45d9f3b; 183 | a = ((a >> 16) ^ a) * 0x45d9f3b; 184 | a = ((a >> 16) ^ a); 185 | return a & (MODE_S_ICAO_CACHE_LEN-1); 186 | } 187 | 188 | // Add the specified entry to the cache of recently seen ICAO addresses. Note 189 | // that we also add a timestamp so that we can make sure that the entry is only 190 | // valid for MODE_S_ICAO_CACHE_TTL seconds. 191 | void add_recently_seen_icao_addr(mode_s_t *self, uint32_t addr) { 192 | uint32_t h = icao_cache_has_addr(addr); 193 | self->icao_cache[h*2] = addr; 194 | self->icao_cache[h*2+1] = (uint32_t) time(NULL); 195 | } 196 | 197 | // Returns 1 if the specified ICAO address was seen in a DF format with proper 198 | // checksum (not xored with address) no more than * MODE_S_ICAO_CACHE_TTL 199 | // seconds ago. Otherwise returns 0. 200 | int icao_addr_was_recently_seen(mode_s_t *self, uint32_t addr) { 201 | uint32_t h = icao_cache_has_addr(addr); 202 | uint32_t a = self->icao_cache[h*2]; 203 | int32_t t = self->icao_cache[h*2+1]; 204 | 205 | return a && a == addr && time(NULL)-t <= MODE_S_ICAO_CACHE_TTL; 206 | } 207 | 208 | // If the message type has the checksum xored with the ICAO address, try to 209 | // brute force it using a list of recently seen ICAO addresses. 210 | // 211 | // Do this in a brute-force fashion by xoring the predicted CRC with the 212 | // address XOR checksum field in the message. This will recover the address: if 213 | // we found it in our cache, we can assume the message is ok. 214 | // 215 | // This function expects mm->msgtype and mm->msgbits to be correctly populated 216 | // by the caller. 217 | // 218 | // On success the correct ICAO address is stored in the mode_s_msg structure in 219 | // the aa3, aa2, and aa1 fiedls. 220 | // 221 | // If the function successfully recovers a message with a correct checksum it 222 | // returns 1. Otherwise 0 is returned. 223 | int brute_force_ap(mode_s_t *self, unsigned char *msg, struct mode_s_msg *mm) { 224 | unsigned char aux[MODE_S_LONG_MSG_BYTES]; 225 | int msgtype = mm->msgtype; 226 | int msgbits = mm->msgbits; 227 | 228 | if (msgtype == 0 || // Short air surveillance 229 | msgtype == 4 || // Surveillance, altitude reply 230 | msgtype == 5 || // Surveillance, identity reply 231 | msgtype == 16 || // Long Air-Air survillance 232 | msgtype == 20 || // Comm-A, altitude request 233 | msgtype == 21 || // Comm-A, identity request 234 | msgtype == 24) // Comm-C ELM 235 | { 236 | uint32_t addr; 237 | uint32_t crc; 238 | int lastbyte = (msgbits/8)-1; 239 | 240 | // Work on a copy. 241 | memcpy(aux, msg, msgbits/8); 242 | 243 | // Compute the CRC of the message and XOR it with the AP field so that 244 | // we recover the address, because: 245 | // 246 | // (ADDR xor CRC) xor CRC = ADDR. 247 | crc = mode_s_checksum(aux, msgbits); 248 | aux[lastbyte] ^= crc & 0xff; 249 | aux[lastbyte-1] ^= (crc >> 8) & 0xff; 250 | aux[lastbyte-2] ^= (crc >> 16) & 0xff; 251 | 252 | // If the obtained address exists in our cache we consider the message 253 | // valid. 254 | addr = aux[lastbyte] | (aux[lastbyte-1] << 8) | (aux[lastbyte-2] << 16); 255 | if (icao_addr_was_recently_seen(self, addr)) { 256 | mm->aa1 = aux[lastbyte-2]; 257 | mm->aa2 = aux[lastbyte-1]; 258 | mm->aa3 = aux[lastbyte]; 259 | return 1; 260 | } 261 | } 262 | return 0; 263 | } 264 | 265 | // Decode the 13 bit AC altitude field (in DF 20 and others). Returns the 266 | // altitude, and set 'unit' to either MODE_S_UNIT_METERS or MDOES_UNIT_FEETS. 267 | int decode_ac13_field(unsigned char *msg, int *unit) { 268 | int m_bit = msg[3] & (1<<6); 269 | int q_bit = msg[3] & (1<<4); 270 | 271 | if (!m_bit) { 272 | *unit = MODE_S_UNIT_FEET; 273 | if (q_bit) { 274 | // N is the 11 bit integer resulting from the removal of bit Q and M 275 | int n = ((msg[2]&31)<<6) | 276 | ((msg[3]&0x80)>>2) | 277 | ((msg[3]&0x20)>>1) | 278 | (msg[3]&15); 279 | // The final altitude is due to the resulting number multiplied by 280 | // 25, minus 1000. 281 | return n*25-1000; 282 | } else { 283 | // TODO: Implement altitude where Q=0 and M=0 284 | } 285 | } else { 286 | *unit = MODE_S_UNIT_METERS; 287 | // TODO: Implement altitude when meter unit is selected. 288 | } 289 | return 0; 290 | } 291 | 292 | // Decode the 12 bit AC altitude field (in DF 17 and others). Returns the 293 | // altitude or 0 if it can't be decoded. 294 | int decode_ac12_field(unsigned char *msg, int *unit) { 295 | int q_bit = msg[5] & 1; 296 | 297 | if (q_bit) { 298 | // N is the 11 bit integer resulting from the removal of bit Q 299 | *unit = MODE_S_UNIT_FEET; 300 | int n = ((msg[5]>>1)<<4) | ((msg[6]&0xF0) >> 4); 301 | // The final altitude is due to the resulting number multiplied by 25, 302 | // minus 1000. 303 | return n*25-1000; 304 | } else { 305 | return 0; 306 | } 307 | } 308 | 309 | static const char *ais_charset = "?ABCDEFGHIJKLMNOPQRSTUVWXYZ????? ???????????????0123456789??????"; 310 | 311 | // Decode a raw Mode S message demodulated as a stream of bytes by 312 | // mode_s_detect(), and split it into fields populating a mode_s_msg structure. 313 | void mode_s_decode(mode_s_t *self, struct mode_s_msg *mm, unsigned char *msg) { 314 | uint32_t crc2; // Computed CRC, used to verify the message CRC. 315 | 316 | // Work on our local copy 317 | memcpy(mm->msg, msg, MODE_S_LONG_MSG_BYTES); 318 | msg = mm->msg; 319 | 320 | // Get the message type ASAP as other operations depend on this 321 | mm->msgtype = msg[0]>>3; // Downlink Format 322 | mm->msgbits = mode_s_msg_len_by_type(mm->msgtype); 323 | 324 | // CRC is always the last three bytes. 325 | mm->crc = ((uint32_t)msg[(mm->msgbits/8)-3] << 16) | 326 | ((uint32_t)msg[(mm->msgbits/8)-2] << 8) | 327 | (uint32_t)msg[(mm->msgbits/8)-1]; 328 | crc2 = mode_s_checksum(msg, mm->msgbits); 329 | 330 | // Check CRC and fix single bit errors using the CRC when possible (DF 11 and 17). 331 | mm->errorbit = -1; // No error 332 | mm->crcok = (mm->crc == crc2); 333 | 334 | if (!mm->crcok && self->fix_errors && (mm->msgtype == 11 || mm->msgtype == 17)) { 335 | if ((mm->errorbit = fix_single_bit_errors(msg, mm->msgbits)) != -1) { 336 | mm->crc = mode_s_checksum(msg, mm->msgbits); 337 | mm->crcok = 1; 338 | } else if (self->aggressive && mm->msgtype == 17 && 339 | (mm->errorbit = fix_two_bits_errors(msg, mm->msgbits)) != -1) { 340 | mm->crc = mode_s_checksum(msg, mm->msgbits); 341 | mm->crcok = 1; 342 | } 343 | } 344 | 345 | // Note that most of the other computation happens *after* we fix the 346 | // single bit errors, otherwise we would need to recompute the fields 347 | // again. 348 | mm->ca = msg[0] & 7; // Responder capabilities. 349 | 350 | // ICAO address 351 | mm->aa1 = msg[1]; 352 | mm->aa2 = msg[2]; 353 | mm->aa3 = msg[3]; 354 | 355 | // DF 17 type (assuming this is a DF17, otherwise not used) 356 | mm->metype = msg[4] >> 3; // Extended squitter message type. 357 | mm->mesub = msg[4] & 7; // Extended squitter message subtype. 358 | 359 | // Fields for DF4,5,20,21 360 | mm->fs = msg[0] & 7; // Flight status for DF4,5,20,21 361 | mm->dr = msg[1] >> 3 & 31; // Request extraction of downlink request. 362 | mm->um = ((msg[1] & 7)<<3)| // Request extraction of downlink request. 363 | msg[2]>>5; 364 | 365 | // In the squawk (identity) field bits are interleaved like that (message 366 | // bit 20 to bit 32): 367 | // 368 | // C1-A1-C2-A2-C4-A4-ZERO-B1-D1-B2-D2-B4-D4 369 | // 370 | // So every group of three bits A, B, C, D represent an integer from 0 to 371 | // 7. 372 | // 373 | // The actual meaning is just 4 octal numbers, but we convert it into a 374 | // base ten number tha happens to represent the four octal numbers. 375 | // 376 | // For more info: http://en.wikipedia.org/wiki/Gillham_code 377 | { 378 | int a, b, c, d; 379 | 380 | a = ((msg[3] & 0x80) >> 5) | 381 | ((msg[2] & 0x02) >> 0) | 382 | ((msg[2] & 0x08) >> 3); 383 | b = ((msg[3] & 0x02) << 1) | 384 | ((msg[3] & 0x08) >> 2) | 385 | ((msg[3] & 0x20) >> 5); 386 | c = ((msg[2] & 0x01) << 2) | 387 | ((msg[2] & 0x04) >> 1) | 388 | ((msg[2] & 0x10) >> 4); 389 | d = ((msg[3] & 0x01) << 2) | 390 | ((msg[3] & 0x04) >> 1) | 391 | ((msg[3] & 0x10) >> 4); 392 | mm->identity = a*1000 + b*100 + c*10 + d; 393 | } 394 | 395 | // DF 11 & 17: try to populate our ICAO addresses whitelist. DFs with an AP 396 | // field (xored addr and crc), try to decode it. 397 | if (mm->msgtype != 11 && mm->msgtype != 17) { 398 | // Check if we can check the checksum for the Downlink Formats where 399 | // the checksum is xored with the aircraft ICAO address. We try to 400 | // brute force it using a list of recently seen aircraft addresses. 401 | if (brute_force_ap(self, msg, mm)) { 402 | // We recovered the message, mark the checksum as valid. 403 | mm->crcok = 1; 404 | } else { 405 | mm->crcok = 0; 406 | } 407 | } else { 408 | // If this is DF 11 or DF 17 and the checksum was ok, we can add this 409 | // address to the list of recently seen addresses. 410 | if (mm->crcok && mm->errorbit == -1) { 411 | uint32_t addr = (mm->aa1 << 16) | (mm->aa2 << 8) | mm->aa3; 412 | add_recently_seen_icao_addr(self, addr); 413 | } 414 | } 415 | 416 | // Decode 13 bit altitude for DF0, DF4, DF16, DF20 417 | if (mm->msgtype == 0 || mm->msgtype == 4 || 418 | mm->msgtype == 16 || mm->msgtype == 20) { 419 | mm->altitude = decode_ac13_field(msg, &mm->unit); 420 | } 421 | 422 | // Decode extended squitter specific stuff. 423 | if (mm->msgtype == 17) { 424 | // Decode the extended squitter message. 425 | 426 | if (mm->metype >= 1 && mm->metype <= 4) { 427 | // Aircraft Identification and Category 428 | mm->aircraft_type = mm->metype-1; 429 | mm->flight[0] = (ais_charset)[msg[5]>>2]; 430 | mm->flight[1] = ais_charset[((msg[5]&3)<<4)|(msg[6]>>4)]; 431 | mm->flight[2] = ais_charset[((msg[6]&15)<<2)|(msg[7]>>6)]; 432 | mm->flight[3] = ais_charset[msg[7]&63]; 433 | mm->flight[4] = ais_charset[msg[8]>>2]; 434 | mm->flight[5] = ais_charset[((msg[8]&3)<<4)|(msg[9]>>4)]; 435 | mm->flight[6] = ais_charset[((msg[9]&15)<<2)|(msg[10]>>6)]; 436 | mm->flight[7] = ais_charset[msg[10]&63]; 437 | mm->flight[8] = '\0'; 438 | } else if (mm->metype >= 9 && mm->metype <= 18) { 439 | // Airborne position Message 440 | mm->fflag = msg[6] & (1<<2); 441 | mm->tflag = msg[6] & (1<<3); 442 | mm->altitude = decode_ac12_field(msg, &mm->unit); 443 | mm->raw_latitude = ((msg[6] & 3) << 15) | 444 | (msg[7] << 7) | 445 | (msg[8] >> 1); 446 | mm->raw_longitude = ((msg[8]&1) << 16) | 447 | (msg[9] << 8) | 448 | msg[10]; 449 | } else if (mm->metype == 19 && mm->mesub >= 1 && mm->mesub <= 4) { 450 | // Airborne Velocity Message 451 | if (mm->mesub == 1 || mm->mesub == 2) { 452 | mm->ew_dir = (msg[5]&4) >> 2; 453 | mm->ew_velocity = ((msg[5]&3) << 8) | msg[6]; 454 | mm->ns_dir = (msg[7]&0x80) >> 7; 455 | mm->ns_velocity = ((msg[7]&0x7f) << 3) | ((msg[8]&0xe0) >> 5); 456 | mm->vert_rate_source = (msg[8]&0x10) >> 4; 457 | mm->vert_rate_sign = (msg[8]&0x8) >> 3; 458 | mm->vert_rate = ((msg[8]&7) << 6) | ((msg[9]&0xfc) >> 2); 459 | // Compute velocity and angle from the two speed components 460 | mm->velocity = sqrt(mm->ns_velocity*mm->ns_velocity+ 461 | mm->ew_velocity*mm->ew_velocity); 462 | if (mm->velocity) { 463 | int ewv = mm->ew_velocity; 464 | int nsv = mm->ns_velocity; 465 | double heading; 466 | 467 | if (mm->ew_dir) ewv *= -1; 468 | if (mm->ns_dir) nsv *= -1; 469 | heading = atan2(ewv, nsv); 470 | 471 | // Convert to degrees. 472 | mm->heading = heading * 360 / (M_PI*2); 473 | // We don't want negative values but a 0-360 scale. 474 | if (mm->heading < 0) mm->heading += 360; 475 | } else { 476 | mm->heading = 0; 477 | } 478 | } else if (mm->mesub == 3 || mm->mesub == 4) { 479 | mm->heading_is_valid = msg[5] & (1<<2); 480 | mm->heading = (360.0/128) * (((msg[5] & 3) << 5) | 481 | (msg[6] >> 3)); 482 | } 483 | } 484 | } 485 | mm->phase_corrected = 0; // Set to 1 by the caller if needed. 486 | } 487 | 488 | // Turn I/Q samples pointed by `data` into the magnitude vector pointed by `mag` 489 | void mode_s_compute_magnitude_vector(unsigned char *data, uint16_t *mag, uint32_t size) { 490 | uint32_t j; 491 | 492 | // Compute the magnitude vector. It's just SQRT(I^2 + Q^2), but we rescale 493 | // to the 0-255 range to exploit the full resolution. 494 | for (j = 0; j < size; j += 2) { 495 | int i = data[j]-127; 496 | int q = data[j+1]-127; 497 | 498 | if (i < 0) i = -i; 499 | if (q < 0) q = -q; 500 | mag[j/2] = maglut[i*129+q]; 501 | } 502 | } 503 | 504 | // Return -1 if the message is out of fase left-side 505 | // Return 1 if the message is out of fase right-size 506 | // Return 0 if the message is not particularly out of phase. 507 | // 508 | // Note: this function will access mag[-1], so the caller should make sure to 509 | // call it only if we are not at the start of the current buffer. 510 | int detect_out_of_phase(uint16_t *mag) { 511 | if (mag[3] > mag[2]/3) return 1; 512 | if (mag[10] > mag[9]/3) return 1; 513 | if (mag[6] > mag[7]/3) return -1; 514 | if (mag[-1] > mag[1]/3) return -1; 515 | return 0; 516 | } 517 | 518 | // This function does not really correct the phase of the message, it just 519 | // applies a transformation to the first sample representing a given bit: 520 | // 521 | // If the previous bit was one, we amplify it a bit. 522 | // If the previous bit was zero, we decrease it a bit. 523 | // 524 | // This simple transformation makes the message a bit more likely to be 525 | // correctly decoded for out of phase messages: 526 | // 527 | // When messages are out of phase there is more uncertainty in sequences of the 528 | // same bit multiple times, since 11111 will be transmitted as continuously 529 | // altering magnitude (high, low, high, low...) 530 | // 531 | // However because the message is out of phase some part of the high is mixed 532 | // in the low part, so that it is hard to distinguish if it is a zero or a one. 533 | // 534 | // However when the message is out of phase passing from 0 to 1 or from 1 to 0 535 | // happens in a very recognizable way, for instance in the 0 -> 1 transition, 536 | // magnitude goes low, high, high, low, and one of of the two middle samples 537 | // the high will be *very* high as part of the previous or next high signal 538 | // will be mixed there. 539 | // 540 | // Applying our simple transformation we make more likely if the current bit is 541 | // a zero, to detect another zero. Symmetrically if it is a one it will be more 542 | // likely to detect a one because of the transformation. In this way similar 543 | // levels will be interpreted more likely in the correct way. 544 | void apply_phase_correction(uint16_t *mag) { 545 | int j; 546 | 547 | mag += 16; // Skip preamble. 548 | for (j = 0; j < (MODE_S_LONG_MSG_BITS-1)*2; j += 2) { 549 | if (mag[j] > mag[j+1]) { 550 | // One 551 | mag[j+2] = (mag[j+2] * 5) / 4; 552 | } else { 553 | // Zero 554 | mag[j+2] = (mag[j+2] * 4) / 5; 555 | } 556 | } 557 | } 558 | 559 | // Detect a Mode S messages inside the magnitude buffer pointed by 'mag' and of 560 | // size 'maglen' bytes. Every detected Mode S message is convert it into a 561 | // stream of bits and passed to the function to display it. 562 | void mode_s_detect(mode_s_t *self, uint16_t *mag, uint32_t maglen, mode_s_callback_t cb) { 563 | unsigned char bits[MODE_S_LONG_MSG_BITS]; 564 | unsigned char msg[MODE_S_LONG_MSG_BITS/2]; 565 | uint16_t aux[MODE_S_LONG_MSG_BITS*2]; 566 | uint32_t j; 567 | int use_correction = 0; 568 | 569 | // The Mode S preamble is made of impulses of 0.5 microseconds at the 570 | // following time offsets: 571 | // 572 | // 0 - 0.5 usec: first impulse. 573 | // 1.0 - 1.5 usec: second impulse. 574 | // 3.5 - 4 usec: third impulse. 575 | // 4.5 - 5 usec: last impulse. 576 | // 577 | // Since we are sampling at 2 Mhz every sample in our magnitude vector is 578 | // 0.5 usec, so the preamble will look like this, assuming there is an 579 | // impulse at offset 0 in the array: 580 | // 581 | // 0 ----------------- 582 | // 1 - 583 | // 2 ------------------ 584 | // 3 -- 585 | // 4 - 586 | // 5 -- 587 | // 6 - 588 | // 7 ------------------ 589 | // 8 -- 590 | // 9 ------------------- 591 | for (j = 0; j < maglen - MODE_S_FULL_LEN*2; j++) { 592 | int low, high, delta, i, errors; 593 | int good_message = 0; 594 | 595 | if (use_correction) goto good_preamble; // We already checked it. 596 | 597 | // First check of relations between the first 10 samples representing a 598 | // valid preamble. We don't even investigate further if this simple 599 | // test is not passed. 600 | if (!(mag[j] > mag[j+1] && 601 | mag[j+1] < mag[j+2] && 602 | mag[j+2] > mag[j+3] && 603 | mag[j+3] < mag[j] && 604 | mag[j+4] < mag[j] && 605 | mag[j+5] < mag[j] && 606 | mag[j+6] < mag[j] && 607 | mag[j+7] > mag[j+8] && 608 | mag[j+8] < mag[j+9] && 609 | mag[j+9] > mag[j+6])) 610 | { 611 | continue; 612 | } 613 | 614 | // The samples between the two spikes must be < than the average of the 615 | // high spikes level. We don't test bits too near to the high levels as 616 | // signals can be out of phase so part of the energy can be in the near 617 | // samples. 618 | high = (mag[j]+mag[j+2]+mag[j+7]+mag[j+9])/6; 619 | if (mag[j+4] >= high || 620 | mag[j+5] >= high) 621 | { 622 | continue; 623 | } 624 | 625 | // Similarly samples in the range 11-14 must be low, as it is the space 626 | // between the preamble and real data. Again we don't test bits too 627 | // near to high levels, see above. 628 | if (mag[j+11] >= high || 629 | mag[j+12] >= high || 630 | mag[j+13] >= high || 631 | mag[j+14] >= high) 632 | { 633 | continue; 634 | } 635 | 636 | good_preamble: 637 | // If the previous attempt with this message failed, retry using 638 | // magnitude correction. 639 | if (use_correction) { 640 | memcpy(aux, mag+j+MODE_S_PREAMBLE_US*2, sizeof(aux)); 641 | if (j && detect_out_of_phase(mag+j)) { 642 | apply_phase_correction(mag+j); 643 | } 644 | // TODO ... apply other kind of corrections. 645 | } 646 | 647 | // Decode all the next 112 bits, regardless of the actual message size. 648 | // We'll check the actual message type later. 649 | errors = 0; 650 | for (i = 0; i < MODE_S_LONG_MSG_BITS*2; i += 2) { 651 | low = mag[j+i+MODE_S_PREAMBLE_US*2]; 652 | high = mag[j+i+MODE_S_PREAMBLE_US*2+1]; 653 | delta = low-high; 654 | if (delta < 0) delta = -delta; 655 | 656 | if (i > 0 && delta < 256) { 657 | bits[i/2] = bits[i/2-1]; 658 | } else if (low == high) { 659 | // Checking if two adiacent samples have the same magnitude is 660 | // an effective way to detect if it's just random noise that 661 | // was detected as a valid preamble. 662 | bits[i/2] = 2; // error 663 | if (i < MODE_S_SHORT_MSG_BITS*2) errors++; 664 | } else if (low > high) { 665 | bits[i/2] = 1; 666 | } else { 667 | // (low < high) for exclusion 668 | bits[i/2] = 0; 669 | } 670 | } 671 | 672 | // Restore the original message if we used magnitude correction. 673 | if (use_correction) 674 | memcpy(mag+j+MODE_S_PREAMBLE_US*2, aux, sizeof(aux)); 675 | 676 | // Pack bits into bytes 677 | for (i = 0; i < MODE_S_LONG_MSG_BITS; i += 8) { 678 | msg[i/8] = 679 | bits[i]<<7 | 680 | bits[i+1]<<6 | 681 | bits[i+2]<<5 | 682 | bits[i+3]<<4 | 683 | bits[i+4]<<3 | 684 | bits[i+5]<<2 | 685 | bits[i+6]<<1 | 686 | bits[i+7]; 687 | } 688 | 689 | int msgtype = msg[0]>>3; 690 | int msglen = mode_s_msg_len_by_type(msgtype)/8; 691 | 692 | // Last check, high and low bits are different enough in magnitude to 693 | // mark this as real message and not just noise? 694 | delta = 0; 695 | for (i = 0; i < msglen*8*2; i += 2) { 696 | delta += abs(mag[j+i+MODE_S_PREAMBLE_US*2]- 697 | mag[j+i+MODE_S_PREAMBLE_US*2+1]); 698 | } 699 | delta /= msglen*4; 700 | 701 | // Filter for an average delta of three is small enough to let almost 702 | // every kind of message to pass, but high enough to filter some random 703 | // noise. 704 | if (delta < 10*255) { 705 | use_correction = 0; 706 | continue; 707 | } 708 | 709 | // If we reached this point, and error is zero, we are very likely with 710 | // a Mode S message in our hands, but it may still be broken and CRC 711 | // may not be correct. This is handled by the next layer. 712 | if (errors == 0 || (self->aggressive && errors < 3)) { 713 | struct mode_s_msg mm; 714 | 715 | // Decode the received message 716 | mode_s_decode(self, &mm, msg); 717 | 718 | // Skip this message if we are sure it's fine. 719 | if (mm.crcok) { 720 | j += (MODE_S_PREAMBLE_US+(msglen*8))*2; 721 | good_message = 1; 722 | if (use_correction) 723 | mm.phase_corrected = 1; 724 | } 725 | 726 | // Pass data to the next layer 727 | if (self->check_crc == 0 || mm.crcok) { 728 | cb(self, &mm); 729 | } 730 | } 731 | 732 | // Retry with phase correction if possible. 733 | if (!good_message && !use_correction) { 734 | j--; 735 | use_correction = 1; 736 | } else { 737 | use_correction = 0; 738 | } 739 | } 740 | } 741 | -------------------------------------------------------------------------------- /tests/test.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "mode-s.h" 6 | 7 | #define MODE_S_DATA_LEN (16*16384) // 256k 8 | #define MODE_S_PREAMBLE_US 8 // microseconds 9 | #define MODE_S_LONG_MSG_BITS 112 10 | #define MODE_S_FULL_LEN (MODE_S_PREAMBLE_US+MODE_S_LONG_MSG_BITS) 11 | 12 | #define MODE_S_NOTUSED(V) ((void) V) 13 | 14 | pthread_t reader_thread; 15 | pthread_mutex_t data_mutex; // Mutex to synchronize buffer access. 16 | pthread_cond_t data_cond; // Conditional variable associated. 17 | unsigned char *data; // Raw IQ samples buffer 18 | int fd; // file descriptor. 19 | uint32_t data_len; // Buffer length. 20 | int data_ready = 0; // Data ready to be processed. 21 | int should_exit = 0; // Exit from the main loop when true. 22 | 23 | void read_data_from_file(void) { 24 | pthread_mutex_lock(&data_mutex); 25 | while(1) { 26 | ssize_t nread, toread; 27 | unsigned char *p; 28 | 29 | if (data_ready) { 30 | pthread_cond_wait(&data_cond, &data_mutex); 31 | continue; 32 | } 33 | 34 | // Move the last part of the previous buffer, that was not processed, on 35 | // the start of the new buffer. 36 | memcpy(data, data+MODE_S_DATA_LEN, (MODE_S_FULL_LEN-1)*4); 37 | toread = MODE_S_DATA_LEN; 38 | p = data+(MODE_S_FULL_LEN-1)*4; 39 | while(toread) { 40 | nread = read(fd, p, toread); 41 | if (nread <= 0) { 42 | should_exit = 1; // Signal the other thread to exit. 43 | break; 44 | } 45 | p += nread; 46 | toread -= nread; 47 | } 48 | if (toread) { 49 | // Not enough data on file to fill the buffer? Pad with no signal. 50 | memset(p, 127, toread); 51 | } 52 | data_ready = 1; 53 | // Signal to the other thread that new data is ready 54 | pthread_cond_signal(&data_cond); 55 | } 56 | } 57 | 58 | // We read data using a thread, so the main thread only handles decoding 59 | // without caring about data acquisition. 60 | void *reader_thread_entry_point(void *arg) { 61 | MODE_S_NOTUSED(arg); 62 | read_data_from_file(); 63 | return NULL; 64 | } 65 | 66 | int msgNo = 0; 67 | char *messages[8] = { 68 | "8d45ac2d9904d910613f94ba81b5", 69 | "5d45ac2da5e9cb", 70 | "8d45ac2d583561285c4fa686fcdc", 71 | "a00006979b580030400000df4221", 72 | "5d45ac2da5e9cb", 73 | "5d45ac2da5e9cb", 74 | "02a186b39408d0", 75 | "200006b31828c8" 76 | }; 77 | 78 | void test(mode_s_t *self, struct mode_s_msg *mm) { 79 | MODE_S_NOTUSED(self); 80 | int j; 81 | 82 | char *msg = (char*)malloc(mm->msgbits/4 * sizeof(char)); 83 | for (j = 0; j < mm->msgbits/8; j++) sprintf(&msg[j*2], "%02x", mm->msg[j]); 84 | printf("validating message #%d\n", msgNo + 1); 85 | 86 | assert(strcmp(msg, messages[msgNo++]) == 0); 87 | } 88 | 89 | int main(int argc, char **argv) { 90 | mode_s_t state; 91 | uint16_t *mag; 92 | 93 | if (argc != 2) { 94 | fprintf(stderr, "Provide data filename as first argument\n"); 95 | exit(1); 96 | } 97 | 98 | char *filename = strdup(argv[1]); 99 | if ((fd = open(filename, O_RDONLY)) == -1) { 100 | perror("Opening data file"); 101 | exit(1); 102 | } 103 | 104 | data_len = MODE_S_DATA_LEN + (MODE_S_FULL_LEN-1)*4; 105 | if ((data = malloc(data_len)) == NULL || 106 | (mag = malloc(sizeof(uint16_t) * (data_len / 2))) == NULL) { 107 | fprintf(stderr, "Out of memory allocating data buffer.\n"); 108 | exit(1); 109 | } 110 | 111 | mode_s_init(&state); 112 | 113 | pthread_create(&reader_thread, NULL, reader_thread_entry_point, NULL); 114 | 115 | pthread_mutex_lock(&data_mutex); 116 | while(1) { 117 | if (!data_ready) { 118 | pthread_cond_wait(&data_cond, &data_mutex); 119 | continue; 120 | } 121 | mode_s_compute_magnitude_vector(data, mag, data_len); 122 | 123 | // Signal to the other thread that we processed the available data and we 124 | // want more. 125 | data_ready = 0; 126 | pthread_cond_signal(&data_cond); 127 | 128 | // Process data after releasing the lock, so that the capturing thread can 129 | // read data while we perform computationally expensive stuff * at the same 130 | // time. (This should only be useful with very slow processors). 131 | pthread_mutex_unlock(&data_mutex); 132 | mode_s_detect(&state, mag, data_len/2, test); 133 | pthread_mutex_lock(&data_mutex); 134 | if (should_exit) break; 135 | } 136 | 137 | printf("all ok\n"); 138 | return 0; 139 | } 140 | --------------------------------------------------------------------------------