/* Mode1090, a Mode S messages decoder for RTLSDR devices. * * Copyright (C) 2012 by Salvatore Sanfilippo * * 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 #include #include #include #include #include #include #include #include #include #include #include #include "rtl-sdr.h" #include "anet.h" #define MODES_DEFAULT_RATE 2000000 #define MODES_DEFAULT_FREQ 1090000000 #define MODES_DEFAULT_WIDTH 1000 #define MODES_DEFAULT_HEIGHT 700 #define MODES_ASYNC_BUF_NUMBER 12 #define MODES_DATA_LEN (16*16384) /* 256k */ #define MODES_AUTO_GAIN -100 /* Use automatic gain. */ #define MODES_MAX_GAIN 999999 /* Use max available gain. */ #define MODES_PREAMBLE_US 8 /* microseconds */ #define MODES_LONG_MSG_BITS 112 #define MODES_SHORT_MSG_BITS 56 #define MODES_FULL_LEN (MODES_PREAMBLE_US+MODES_LONG_MSG_BITS) #define MODES_LONG_MSG_BYTES (112/8) #define MODES_SHORT_MSG_BYTES (56/8) #define MODES_ICAO_CACHE_LEN 1024 /* Power of two required. */ #define MODES_ICAO_CACHE_TTL 60 /* Time to live of cached addresses. */ #define MODES_UNIT_FEET 0 #define MODES_UNIT_METERS 1 #define MODES_DEBUG_DEMOD 1 #define MODES_DEBUG_DEMODERR 2 #define MODES_DEBUG_BADCRC 3 #define MODES_DEBUG_GOODCRC 4 #define MODES_DEBUG_NOPREAMBLE 5 /* When debug is set to MODES_DEBUG_NOPREAMBLE, the first sample must be * at least greater than a given level for us to dump the signal. */ #define MODES_DEBUG_NOPREAMBLE_LEVEL 25 #define MODES_INTERACTIVE_REFRESH_TIME 250 /* Milliseconds */ #define MODES_INTERACTIVE_ROWS 15 /* Rows on screen */ #define MODES_INTERACTIVE_TTL 60 /* TTL before being removed */ #define MODES_NET_MAX_FD 1024 #define MODES_NET_OUTPUT_RAW_PORT 30002 #define MODES_NET_INPUT_RAW_PORT 30001 #define MODES_CLIENT_BUF_SIZE 256 #define MODES_NOTUSED(V) ((void) V) /* Structure used to describe a networking client. */ struct client { int fd; /* File descriptor. */ int service; /* TCP port the client is connected to. */ char buf[MODES_CLIENT_BUF_SIZE]; /* Read buffer. */ int buflen; /* Amount of data on buffer. */ }; /* Structure used to describe an aircraft in iteractive mode. */ struct aircraft { uint32_t addr; /* ICAO address */ char hexaddr[7]; /* Printable ICAO address */ char flight[9]; /* Flight number */ int altitude; /* Altitude */ int speed; /* Velocity computed from EW and NS components. */ int track; /* Angle of flight. */ time_t seen; /* Time at which the last packet was received. */ long messages; /* Number of Mode S messages received. */ /* Encoded latitude and longitude as extracted by odd and even * CPR encoded messages. */ int odd_cprlat; int odd_cprlon; int even_cprlat; int even_cprlon; double lat, lon; /* Coordinated obtained from CPR encoded data. */ time_t odd_cprtime, even_cprtime; struct aircraft *next; /* Next aircraft in our linked list. */ }; /* Program global state. */ struct { /* Internal state */ pthread_t reader_thread; pthread_mutex_t data_mutex; /* Mutex to synchronize buffer access. */ pthread_cond_t data_cond; /* Conditional variable associated. */ unsigned char *data; /* Raw IQ samples buffer */ uint16_t *magnitude; /* Magnitude vector */ uint32_t data_len; /* Buffer length. */ int fd; /* --ifile option file descriptor. */ int data_ready; /* Data ready to be processed. */ uint32_t *icao_cache; /* Recently seen ICAO addresses cache. */ uint16_t *maglut; /* I/Q -> Magnitude lookup table. */ int exit; /* Exit from the main loop when true. */ /* RTLSDR */ int dev_index; int gain; int enable_agc; rtlsdr_dev_t *dev; int freq; /* Networking */ char aneterr[ANET_ERR_LEN]; struct client *clients[MODES_NET_MAX_FD]; /* Our clients. */ int maxfd; /* Greatest fd currently active. */ int ros; /* Raw output listening socket. */ int ris; /* Raw input listening socket. */ /* Configuration */ char *filename; /* Input form file, --ifile option. */ int fix_errors; /* Single bit error correction if true. */ int check_crc; /* Only display messages with good CRC. */ int raw; /* Raw output format. */ int debug; /* Debugging mode. */ int net; /* Enable networking. */ int net_output_raw_port; /* Raw output TCP port. */ int net_input_raw_port; /* Raw input TCP port. */ int interactive; /* Interactive mode */ int interactive_rows; /* Interactive mode: max number of rows. */ int interactive_ttl; /* Interactive mode: TTL before deletion. */ int stats; /* Print stats at exit in --ifile mode. */ int onlyaddr; /* Print only ICAO addresses. */ int metric; /* Use metric units. */ /* Interactive mode */ struct aircraft *aircrafts; long long interactive_last_update; /* Last screen update in milliseconds */ /* Statistics */ long long stat_valid_preamble; long long stat_demodulated; long long stat_goodcrc; long long stat_badcrc; long long stat_fixed; } Modes; /* The struct we use to store information about a decoded message. */ struct modesMessage { /* Generic fields */ unsigned char msg[MODES_LONG_MSG_BYTES]; /* Binary message. */ int msgbits; /* Number of bits in message */ int msgtype; /* Downlink format # */ int crcok; /* True if CRC was valid */ uint32_t crc; /* Message CRC */ int errorbit; /* Bit corrected. -1 if no bit corrected. */ int aa1, aa2, aa3; /* ICAO Address bytes 1 2 and 3 */ /* DF 11 */ int ca; /* Responder capabilities. */ /* DF 17 */ int metype; /* Extended squitter message type. */ int mesub; /* Extended squitter message subtype. */ int heading_is_valid; int heading; int aircraft_type; int fflag; /* 1 = Odd, 0 = Even CPR message. */ int tflag; /* UTC synchronized? */ int raw_latitude; /* Non decoded latitude */ int raw_longitude; /* Non decoded longitude */ char flight[9]; /* 8 chars flight number. */ int ew_dir; /* 0 = East, 1 = West. */ int ew_velocity; /* E/W velocity. */ int ns_dir; /* 0 = North, 1 = South. */ int ns_velocity; /* N/S velocity. */ int vert_rate_source; /* Vertical rate source. */ int vert_rate_sign; /* Vertical rate sign. */ int vert_rate; /* Vertical rate. */ int velocity; /* Computed from EW and NS velocity. */ /* DF4, DF5, DF20, DF21 */ int fs; /* Flight status for DF4,5,20,21 */ int dr; /* Request extraction of downlink request. */ int um; /* Request extraction of downlink request. */ int identity; /* 13 bits identity (Squawk). */ /* Fields used by multiple message types. */ int altitude, unit; }; void interactiveShowData(void); void interactiveReceiveData(struct modesMessage *mm); void modesSendRawOutput(struct modesMessage *mm); void useModesMessage(struct modesMessage *mm); /* ============================= Utility functions ========================== */ static long long mstime(void) { struct timeval tv; long long mst; gettimeofday(&tv, NULL); mst = ((long long)tv.tv_sec)*1000; mst += tv.tv_usec/1000; return mst; } /* =============================== Initialization =========================== */ void modesInitConfig(void) { Modes.gain = MODES_MAX_GAIN; Modes.dev_index = 0; Modes.enable_agc = 0; Modes.freq = MODES_DEFAULT_FREQ; Modes.filename = NULL; Modes.fix_errors = 1; Modes.check_crc = 1; Modes.raw = 0; Modes.net = 0; Modes.net_output_raw_port = MODES_NET_OUTPUT_RAW_PORT; Modes.net_input_raw_port = MODES_NET_INPUT_RAW_PORT; Modes.onlyaddr = 0; Modes.debug = 0; Modes.interactive = 0; Modes.interactive_rows = MODES_INTERACTIVE_ROWS; Modes.interactive_ttl = MODES_INTERACTIVE_TTL; } void modesInit(void) { int i, q; pthread_mutex_init(&Modes.data_mutex,NULL); pthread_cond_init(&Modes.data_cond,NULL); Modes.data_len = MODES_DATA_LEN; Modes.data_ready = 0; /* Allocate the ICAO address cache. We use two uint32_t for every * entry because it's a addr / timestamp pair for every entry. */ Modes.icao_cache = malloc(sizeof(uint32_t)*MODES_ICAO_CACHE_LEN*2); memset(Modes.icao_cache,0,sizeof(Modes.icao_cache)); Modes.aircrafts = NULL; Modes.interactive_last_update = 0; if ((Modes.data = malloc(Modes.data_len)) == NULL || (Modes.magnitude = malloc(Modes.data_len*2)) == NULL) { fprintf(stderr, "Out of memory allocating data buffer.\n"); exit(1); } /* Populate the I/Q -> Magnitude lookup table. It is used because * sqrt or round may be expensive and may vary a lot depending on * the libc used. * * We scale to 0-255 range multiplying by 1.4 in order to ensure that * every different I/Q pair will result in a different magnitude value, * not losing any resolution. */ Modes.maglut = malloc(129*129*2); for (i = 0; i <= 128; i++) { for (q = 0; q <= 128; q++) { Modes.maglut[i*129+q] = round(sqrt(i*i+q*q)*360); } } /* Statistics */ Modes.stat_valid_preamble = 0; Modes.stat_demodulated = 0; Modes.stat_goodcrc = 0; Modes.stat_badcrc = 0; Modes.stat_fixed = 0; Modes.exit = 0; } /* =============================== RTLSDR handling ========================== */ void modesInitRTLSDR(void) { int j; int device_count; int ppm_error = 0; 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, 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); if (len > Modes.data_len) len = Modes.data_len; 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.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(5000); pthread_mutex_lock(&Modes.data_mutex); } toread = Modes.data_len; p = 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.data_len); } else { readDataFromFile(); } return 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 5 samples before the actual start. */ uint32_t start = (offset < padding) ? 0 : offset-padding; uint32_t end = offset + (MODES_PREAMBLE_US*2)+(MODES_SHORT_MSG_BITS*2) - 1; uint32_t j; for (j = start; j <= end; j++) { dumpMagnitudeBar(j-offset, m[j]); } } /* 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 */ void dumpRawMessage(char *descr, unsigned char *msg, uint16_t *m, uint32_t offset) { int j; 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("\n"); dumpMagnitudeVector(m,offset); printf("---\n\n"); } /* ===================== 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; int offset = (bits == 112) ? 0 : (112-56); int j; for(j = 0; j < bits; j++) { int byte = j/8; int bit = j%8; int bitmask = 1 << (7-bit); /* If bit is set, xor with corresponding table entry. */ if (msg[byte] & bitmask) crc ^= modes_checksum_table[j+offset]; } return crc; /* 24 bit checksum. */ } /* Given the Downlink Format (DF) of the message, return the message length * in bits. */ int modesMessageLenByType(int type) { if (type == 16 || type == 17 || type == 19 || type == 20 || type == 21) return MODES_LONG_MSG_BITS; else return 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_BITS/8]; for (j = 0; j < bits; j++) { int byte = j/8; int bitmask = 1 << (7-(j%8)); uint32_t crc1, crc2; memcpy(aux,msg,bits/8); aux[byte] ^= bitmask; /* Flip j-th bit. */ crc1 = ((uint32_t)aux[(bits/8)-3] << 16) | ((uint32_t)aux[(bits/8)-2] << 8) | (uint32_t)aux[(bits/8)-1]; crc2 = modesChecksum(aux,bits); if (crc1 == crc2) { /* The error is fixed. Overwrite the original buffer with * the corrected sequence, and returns the error bit * position. */ memcpy(msg,aux,bits/8); return j; } } return -1; } /* Hash the ICAO address to index our cache of MODES_ICAO_CACHE_LEN * elements, that is assumed to be a power of two. */ uint32_t ICAOCacheHashAddress(uint32_t a) { /* The following three rounds wil make sure that every bit affects * every output bit with ~ 50% of probability. */ a = ((a >> 16) ^ a) * 0x45d9f3b; a = ((a >> 16) ^ a) * 0x45d9f3b; a = ((a >> 16) ^ a); return a & (MODES_ICAO_CACHE_LEN-1); } /* Add the specified entry to the cache of recently seen ICAO addresses. * Note that we also add a timestamp so that we can make sure that the * entry is only valid for MODES_ICAO_CACHE_TTL seconds. */ void addRecentlySeenICAOAddr(uint32_t addr) { uint32_t h = ICAOCacheHashAddress(addr); Modes.icao_cache[h*2] = addr; Modes.icao_cache[h*2+1] = (uint32_t) time(NULL); } /* Returns 1 if the specified ICAO address was seen in a DF format with * proper checksum (not xored with address) no more than * MODES_ICAO_CACHE_TTL * seconds ago. Otherwise returns 0. */ int ICAOAddressWasRecentlySeen(uint32_t addr) { uint32_t h = ICAOCacheHashAddress(addr); uint32_t a = Modes.icao_cache[h*2]; uint32_t t = Modes.icao_cache[h*2+1]; return a && a == addr && time(NULL)-t <= MODES_ICAO_CACHE_TTL; } /* If the message type has the checksum xored with the ICAO address, try to * brute force it using a list of recently seen ICAO addresses. * * Do this in a brute-force fashion by xoring the predicted CRC with * the address XOR checksum field in the message. This will recover the * address: if we found it in our cache, we can assume the message is ok. * * On success the input buffer is modified to remove the xored checksum * from the packet, so that the last three bytes will contain the * plain ICAO address. * * If the function successfully recovers a message with a correct checksum * it returns 1. Otherwise 0 is returned. */ int bruteForceAP(unsigned char *msg, int msgbits) { unsigned char aux[MODES_LONG_MSG_BITS/8]; int msgtype = msg[0]>>3; if (msgtype == 0 || /* Short air surveillance */ msgtype == 4 || /* Surveillance, altitude reply */ msgtype == 5 || /* Surveillance, identity reply */ msgtype == 16 || /* Long Air-Air survillance */ msgtype == 20 || /* Comm-A, altitude request */ msgtype == 21 || /* Comm-A, identity request */ msgtype == 24) /* Comm-C ELM */ { uint32_t addr; uint32_t crc; int lastbyte = (msgbits/8)-1; /* Work on a copy. */ memcpy(aux,msg,msgbits/8); /* Compute the CRC of the message and XOR it with the AP field * so that we recover the address, because: * * (ADDR xor CRC) xor CRC = ADDR. */ crc = modesChecksum(aux,msgbits); aux[lastbyte] ^= crc & 0xff; aux[lastbyte-1] ^= (crc >> 8) & 0xff; aux[lastbyte-2] ^= (crc >> 16) & 0xff; /* If the obtained address exists in our cache we consider * the message valid. */ addr = aux[lastbyte] | (aux[lastbyte-1] << 8) | (aux[lastbyte-2] << 16); if (ICAOAddressWasRecentlySeen(addr)) { msg[lastbyte] = aux[lastbyte]; msg[lastbyte-1] = aux[lastbyte-1]; msg[lastbyte-2] = aux[lastbyte-2]; return 1; } } return 0; } /* Decode the 13 bit AC altitude field (in DF 20 and others). * Returns the altitude, and set 'unit' to either MODES_UNIT_METERS * or MDOES_UNIT_FEETS. */ int decodeAC13Field(unsigned char *msg, int *unit) { int m_bit = msg[3] & (1<<6); int q_bit = msg[3] & (1<<4); if (!m_bit) { *unit = MODES_UNIT_FEET; if (q_bit) { /* N is the 11 bit integer resulting from the removal of bit * Q and M */ int n = ((msg[2]&31)<<6) | ((msg[3]&0x80)>>2) | ((msg[3]&0x20)>>1) | (msg[3]&15); /* The final altitude is due to the resulting number multiplied * by 25, minus 1000. */ return n*25-1000; } else { /* TODO: Implement altitude where Q=0 and M=0 */ } } else { *unit = MODES_UNIT_METERS; /* TODO: Implement altitude when meter unit is selected. */ } return 0; } /* Decode the 12 bit AC altitude field (in DF 17 and others). * Returns the altitude or 0 if it can't be decoded. */ int decodeAC12Field(unsigned char *msg, int *unit) { int q_bit = msg[5] & 1; if (q_bit) { /* N is the 11 bit integer resulting from the removal of bit * Q */ *unit = MODES_UNIT_FEET; int n = ((msg[5]>>1)<<4) | ((msg[6]&0xF0) >> 4); /* The final altitude is due to the resulting number multiplied * by 25, minus 1000. */ return n*25-1000; } else { return 0; } } /* Capability table. */ char *ca_str[8] = { /* 0 */ "Level 1 (Survillance Only)", /* 1 */ "Level 2 (DF0,4,5,11)", /* 2 */ "Level 3 (DF0,4,5,11,20,21)", /* 3 */ "Level 4 (DF0,4,5,11,20,21,24)", /* 4 */ "Level 2+3+4 (DF0,4,5,11,20,21,24,code7 - is on ground)", /* 5 */ "Level 2+3+4 (DF0,4,5,11,20,21,24,code7 - is on airborne)", /* 6 */ "Level 2+3+4 (DF0,4,5,11,20,21,24,code7)", /* 7 */ "Level 7 ???" }; /* Flight status table. */ char *fs_str[8] = { /* 0 */ "Normal, Airborne", /* 1 */ "Normal, On the ground", /* 2 */ "ALERT, Airborne", /* 3 */ "ALERT, On the ground", /* 4 */ "ALERT & Special Position Identification. Airborne or Ground", /* 5 */ "Special Position Identification. Airborne or Ground", /* 6 */ "Value 6 is not assigned", /* 7 */ "Value 7 is not assigned" }; /* ME message type to description table. */ char *me_str[] = { }; char *getMEDescription(int metype, int mesub) { char *mename = "Unknown"; if (metype >= 1 && metype <= 4) mename = "Aircraft Identification and Category"; else if (metype >= 5 && metype <= 8) mename = "Surface Position"; else if (metype >= 9 && metype <= 18) mename = "Airborne Position (Baro Altitude)"; else if (metype == 19 && mesub >=1 && mesub <= 4) mename = "Airborne Velocity"; else if (metype >= 20 && metype <= 22) mename = "Airborne Position (GNSS Height)"; else if (metype == 23 && mesub == 0) mename = "Test Message"; else if (metype == 24 && mesub == 1) mename = "Surface System Status"; else if (metype == 28 && mesub == 1) mename = "Extended Squitter Aircraft Status (Emergency)"; else if (metype == 28 && mesub == 2) mename = "Extended Squitter Aircraft Status (1090ES TCAS RA)"; else if (metype == 29 && (mesub == 0 || mesub == 1)) mename = "Target State and Status Message"; else if (metype == 31 && (mesub == 0 || mesub == 1)) mename = "Aircraft Operational Status Message"; return mename; } /* Decode a raw Mode S message demodulated as a stream of bytes by * detectModeS(), and split it into fields populating a modesMessage * structure. */ void decodeModesMessage(struct modesMessage *mm, unsigned char *msg) { uint32_t crc2; /* Computed CRC, used to verify the message CRC. */ char *ais_charset = "?ABCDEFGHIJKLMNOPQRSTUVWXYZ????? ???????????????0123456789??????"; /* Work on our local copy */ memcpy(mm->msg,msg,MODES_LONG_MSG_BYTES); msg = mm->msg; /* Get the message type ASAP as other operations depend on this */ mm->msgtype = msg[0]>>3; /* Downlink Format */ mm->msgbits = modesMessageLenByType(mm->msgtype); /* CRC is always the last three bytes. */ mm->crc = ((uint32_t)msg[(mm->msgbits/8)-3] << 16) | ((uint32_t)msg[(mm->msgbits/8)-2] << 8) | (uint32_t)msg[(mm->msgbits/8)-1]; crc2 = modesChecksum(msg,mm->msgbits); /* Check CRC and fix single bit errors using the CRC when * possible (DF 11 and 17). */ mm->errorbit = -1; /* No error */ mm->crcok = (mm->crc == crc2); if (!mm->crcok && Modes.fix_errors && (mm->msgtype == 11 || mm->msgtype == 17)) { if ((mm->errorbit = fixSingleBitErrors(msg,mm->msgbits)) != -1) { mm->crc = modesChecksum(msg,mm->msgbits); mm->crcok = 1; } } /* Note that most of the other computation happens *after* we fix * the single bit errors, otherwise we would need to recompute the * fields again. */ mm->ca = msg[0] & 7; /* Responder capabilities. */ /* ICAO address */ mm->aa1 = msg[1]; mm->aa2 = msg[2]; mm->aa3 = msg[3]; /* DF 17 type (assuming this is a DF17, otherwise not used) */ mm->metype = msg[4] >> 3; /* Extended squitter message type. */ mm->mesub = msg[4] & 7; /* Extended squitter message subtype. */ /* Fields for DF4,5,20,21 */ mm->fs = msg[0] & 7; /* Flight status for DF4,5,20,21 */ mm->dr = msg[1] >> 3 & 31; /* Request extraction of downlink request. */ mm->um = ((msg[1] & 7)<<3)| /* Request extraction of downlink request. */ msg[2]>>5; mm->identity = (msg[2]&31 << 8) | msg[3]; /* 13 bits identity. */ /* Check if we can check the checksum for the Downlink Formats where * the checksum is xored with the aircraft ICAO address. We try to * brute force it using a list of recently seen aircraft addresses. */ if (mm->msgtype != 11 && mm->msgtype != 17) { /* Return to the caller now if we can't resolve the API field and * we need to check the checksum. */ if (bruteForceAP(msg,mm->msgbits)) { /* We recovered the message! * Populate the AA fields with the right information. */ mm->aa3 = msg[mm->msgbits/8-1]; mm->aa2 = msg[mm->msgbits/8-2]; mm->aa1 = msg[mm->msgbits/8-3]; mm->crcok = 1; } else { mm->crcok = 0; } } /* If this is DF 11 or DF 17 and the checksum was ok, * we can add this address to the list of recently seen * addresses. */ if (mm->crcok && mm->errorbit == -1) { uint32_t addr = (mm->aa1 << 16) | (mm->aa2 << 8) | mm->aa3; addRecentlySeenICAOAddr(addr); } /* Decode 13 bit altitude for DF0, DF4, DF16, DF20 */ if (mm->msgtype == 0 || mm->msgtype == 4 || mm->msgtype == 16 || mm->msgtype == 20) { mm->altitude = decodeAC13Field(msg, &mm->unit); } /* Decode extended squitter specific stuff. */ if (mm->msgtype == 17) { /* Decode the extended squitter message. */ if (mm->metype >= 1 && mm->metype <= 4) { /* Aircraft Identification and Category */ mm->aircraft_type = mm->metype-1; mm->flight[0] = ais_charset[msg[5]>>2]; mm->flight[1] = ais_charset[((msg[5]&3)<<4)|(msg[6]>>4)]; mm->flight[2] = ais_charset[((msg[6]&15)<<2)|(msg[7]>>6)]; mm->flight[3] = ais_charset[msg[7]&63]; mm->flight[4] = ais_charset[msg[8]>>2]; mm->flight[5] = ais_charset[((msg[8]&3)<<4)|(msg[9]>>4)]; mm->flight[6] = ais_charset[((msg[9]&15)<<2)|(msg[10]>>6)]; mm->flight[7] = ais_charset[msg[10]&63]; mm->flight[8] = '\0'; } else if (mm->metype >= 9 && mm->metype <= 18) { /* Airborne position Message */ mm->fflag = msg[6] & (1<<2); mm->tflag = msg[6] & (1<<3); mm->altitude = decodeAC12Field(msg,&mm->unit); mm->raw_latitude = ((msg[6] & 3) << 15) | (msg[7] << 7) | (msg[8] >> 1); mm->raw_longitude = ((msg[8]&1) << 16) | (msg[9] << 8) | msg[10]; } else if (mm->metype == 19 && mm->mesub >= 1 && mm->mesub <= 4) { /* Airborne Velocity Message */ if (mm->mesub == 1 || mm->mesub == 2) { mm->ew_dir = (msg[5]&4) >> 2; mm->ew_velocity = ((msg[5]&3) << 8) | msg[6]; mm->ns_dir = (msg[7]&0x80) >> 7; mm->ns_velocity = ((msg[7]&0x7f) << 3) | ((msg[8]&0xe0) >> 5); mm->vert_rate_source = (msg[8]&0x10) >> 4; mm->vert_rate_sign = (msg[8]&0x8) >> 5; mm->vert_rate = ((msg[8]&7) << 6) | ((msg[9]&0xfc) >> 2); /* Compute velocity and angle from the two speed * components. */ mm->velocity = sqrt(mm->ns_velocity*mm->ns_velocity+ mm->ew_velocity*mm->ew_velocity); if (mm->velocity) { int ewv = mm->ew_velocity; int nsv = mm->ns_velocity; double heading; if (mm->ew_dir) ewv *= -1; if (mm->ns_dir) nsv *= -1; heading = atan2(ewv,nsv); /* Convert to degrees. */ mm->heading = heading * 360 / (M_PI*2); /* We don't want negative values but a 0-360 scale. */ if (mm->heading < 0) mm->heading += 360; } else { mm->heading = 0; } } else if (mm->mesub == 3 || mm->mesub == 4) { mm->heading_is_valid = msg[5] & (1<<2); mm->heading = (360.0/128) * (((msg[5] & 3) << 5) | (msg[6] >> 3)); } } } } /* This function gets a decoded Mode S Message and prints it on the screen * in a human readable format. */ void displayModesMessage(struct modesMessage *mm) { int j; /* Handle only addresses mode first. */ if (Modes.onlyaddr) { printf("%02x%02x%02x\n", mm->aa1, mm->aa2, mm->aa3); return; } /* Show the raw message. */ printf("*"); for (j = 0; j < mm->msgbits/8; j++) printf("%02x", mm->msg[j]); printf(";\n"); if (Modes.raw) { fflush(stdout); /* Provide data to the reader ASAP. */ return; /* Enough for --raw mode */ } printf("CRC: %06x (%s)\n", (int)mm->crc, mm->crcok ? "ok" : "wrong"); if (mm->errorbit != -1) printf("Single bit error fixed, bit %d\n", mm->errorbit); if (mm->msgtype == 0) { /* DF 0 */ printf("DF 0: Short Air-Air Surveillance.\n"); printf(" Altitude : %d %s\n", mm->altitude, (mm->unit == MODES_UNIT_METERS) ? "meters" : "feet"); printf(" ICAO Address : %02x%02x%02x\n", mm->aa1, mm->aa2, mm->aa3); } else if (mm->msgtype == 4 || mm->msgtype == 20) { printf("DF %d: %s, Altitude Reply.\n", mm->msgtype, (mm->msgtype == 4) ? "Surveillance" : "Comm-B"); printf(" Flight Status : %s\n", fs_str[mm->fs]); printf(" DR : %d\n", mm->dr); printf(" UM : %d\n", mm->um); printf(" Altitude : %d %s\n", mm->altitude, (mm->unit == MODES_UNIT_METERS) ? "meters" : "feet"); printf(" ICAO Address : %02x%02x%02x\n", mm->aa1, mm->aa2, mm->aa3); if (mm->msgtype == 20) { /* TODO: 56 bits DF20 MB additional field. */ } } else if (mm->msgtype == 5 || mm->msgtype == 21) { printf("DF %d: %s, Identity Reply.\n", mm->msgtype, (mm->msgtype == 5) ? "Surveillance" : "Comm-B"); printf(" Flight Status : %s\n", fs_str[mm->fs]); printf(" DR : %d\n", mm->dr); printf(" UM : %d\n", mm->um); printf(" Squawk : %o (octal)\n", mm->identity); printf(" ICAO Address : %02x%02x%02x\n", mm->aa1, mm->aa2, mm->aa3); if (mm->msgtype == 21) { /* TODO: 56 bits DF21 MB additional field. */ } } else if (mm->msgtype == 11) { /* DF 11 */ printf("DF 11: All Call Reply.\n"); printf(" Capability : %s\n", ca_str[mm->ca]); printf(" ICAO Address: %02x%02x%02x\n", mm->aa1, mm->aa2, mm->aa3); } else if (mm->msgtype == 17) { /* DF 17 */ printf("DF 17: ADS-B message.\n"); printf(" Capability : %d (%s)\n", mm->ca, ca_str[mm->ca]); printf(" ICAO Address : %02x%02x%02x\n", mm->aa1, mm->aa2, mm->aa3); printf(" Extended Squitter Type: %d\n", mm->metype); printf(" Extended Squitter Sub : %d\n", mm->mesub); printf(" Extended Squitter Name: %s\n", getMEDescription(mm->metype,mm->mesub)); /* Decode the extended squitter message. */ if (mm->metype >= 1 && mm->metype <= 4) { /* Aircraft identification. */ char *ac_type_str[4] = { "Aircraft Type D", "Aircraft Type C", "Aircraft Type B", "Aircraft Type A" }; printf(" Aircraft Type : %s\n", ac_type_str[mm->aircraft_type]); printf(" Identification : %s\n", mm->flight); } else if (mm->metype >= 9 && mm->metype <= 18) { printf(" F flag : %s\n", mm->fflag ? "odd" : "even"); printf(" T flag : %s\n", mm->tflag ? "UTC" : "non-UTC"); printf(" Altitude : %d feet\n", mm->altitude); printf(" Latitude : %d (not decoded)\n", mm->raw_latitude); printf(" Longitude: %d (not decoded)\n", mm->raw_longitude); } else if (mm->metype == 19 && mm->mesub >= 1 && mm->mesub <= 4) { if (mm->mesub == 1 || mm->mesub == 2) { /* Velocity */ printf(" EW direction : %d\n", mm->ew_dir); printf(" EW velocity : %d\n", mm->ew_velocity); printf(" NS direction : %d\n", mm->ns_dir); printf(" NS velocity : %d\n", mm->ns_velocity); printf(" Vertical rate src : %d\n", mm->vert_rate_source); printf(" Vertical rate sign: %d\n", mm->vert_rate_sign); printf(" Vertical rate : %d\n", mm->vert_rate); } else if (mm->mesub == 3 || mm->mesub == 4) { printf(" Heading status: %d", mm->heading_is_valid); printf(" Heading: %d", mm->heading); } } else { printf(" Unrecognized ME type: %d subtype: %d\n", mm->metype, mm->mesub); } } else { if (Modes.check_crc) printf("DF %d with good CRC received " "(decoding still not implemented).\n", mm->msgtype); } } /* Turn I/Q samples pointed by Modes.data into the magnitude vector * pointed by Modes.magnitude. */ void computeMagnitudeVector(void) { uint16_t *m = Modes.magnitude; unsigned char *p = Modes.data; uint32_t j; /* Compute the magnitudo vector. It's just SQRT(I^2 + Q^2), but * we rescale to the 0-255 range to exploit the full resolution. */ for (j = 0; j < Modes.data_len; j += 2) { int i = p[j]-127; int q = p[j+1]-127; if (i < 0) i = -i; if (q < 0) q = -q; m[j/2] = Modes.maglut[i*129+q]; } } /* Detect a Mode S messages inside the magnitude buffer pointed by 'm' and of * size 'mlen' bytes. Every detected Mode S message is convert it into a * stream of bits and passed to the function to display it. */ void detectModeS(uint16_t *m, uint32_t mlen) { unsigned char bits[MODES_LONG_MSG_BITS]; unsigned char msg[MODES_LONG_MSG_BITS/2]; uint32_t j; /* The Mode S preamble is made of impulses of 0.5 microseconds at * the following time offsets: * * 0 - 0.5 usec: first impulse. * 1.0 - 1.5 usec: second impulse. * 3.5 - 4 usec: third impulse. * 4.5 - 5 usec: last impulse. * * Since we are sampling at 2 Mhz every sample in our magnitude vector * is 0.5 usec, so the preamble will look like this, assuming there is * an impulse at offset 0 in the array: * * 0 ----------------- * 1 - * 2 ------------------ * 3 -- * 4 - * 5 -- * 6 - * 7 ------------------ * 8 -- * 9 ------------------- */ for (j = 0; j < mlen - MODES_FULL_LEN*2; j++) { int low, high, i, errors; /* First check of relations between the first 10 samples * representing a valid preamble. We don't even investigate further * if this simple test is not passed. */ if (!(m[j] > m[j+1] && m[j+1] < m[j+2] && m[j+2] > m[j+3] && m[j+3] < m[j] && m[j+4] < m[j] && m[j+5] < m[j] && m[j+6] < m[j] && m[j+7] > m[j+8] && m[j+8] < m[j+9] && m[j+9] > m[j+6])) { if (Modes.debug == MODES_DEBUG_NOPREAMBLE && m[j] > MODES_DEBUG_NOPREAMBLE_LEVEL) dumpRawMessage("Unexpected ratio among first 10 samples", msg, m, j); continue; } /* The samples between the two spikes must be < than the average * of the high spikes level. */ high = (m[j]+m[j+2]+m[j+7]+m[j+9])/4; if (m[j+3] >= high || m[j+4] >= high || m[j+5] >= high || m[j+6] >= high) { if (Modes.debug == MODES_DEBUG_NOPREAMBLE && m[j] > MODES_DEBUG_NOPREAMBLE_LEVEL) dumpRawMessage("Too high level in samples between 3 and 6", msg, m, j); continue; } /* Similarly samples in the range 10-15 must be low, as it is the * space between the preamble and real data. */ if (m[j+10] >= high || m[j+11] >= high || m[j+12] >= high || m[j+13] >= high || m[j+14] >= high || m[j+15] >= high) { if (Modes.debug == MODES_DEBUG_NOPREAMBLE && m[j] > MODES_DEBUG_NOPREAMBLE_LEVEL) dumpRawMessage("Too high level in samples between 10 and 15", msg, m, j); continue; } Modes.stat_valid_preamble++; /* Decode all the next 112 bits, regardless of the actual message * size. We'll check the actual message type later. */ errors = 0; for (i = 0; i < MODES_LONG_MSG_BITS*2; i += 2) { low = m[j+i+MODES_PREAMBLE_US*2]; high = m[j+i+MODES_PREAMBLE_US*2+1]; if (low == high) { /* Checking if two adiacent samples have the same magnitude * is an effective way to detect if it's just random noise * that was detected as a valid preamble. */ bits[i/2] = 2; /* error */ if (i < MODES_SHORT_MSG_BITS*2) errors++; } else if (low > high) { bits[i/2] = 1; } else { /* (low < high) for exclusion */ bits[i/2] = 0; } } /* Pack bits into bytes */ for (i = 0; i < MODES_LONG_MSG_BITS; i += 8) { msg[i/8] = bits[i]<<7 | bits[i+1]<<6 | bits[i+2]<<5 | bits[i+3]<<4 | bits[i+4]<<3 | bits[i+5]<<2 | bits[i+6]<<1 | bits[i+7]; } int msgtype = msg[0]>>3; int msglen = modesMessageLenByType(msgtype)/8; /* Last check, high and low bits are different enough in magnitude * to mark this as real message and not just noise? */ int delta = 0; for (i = 0; i < msglen*8*2; i += 2) { delta += abs(m[j+i+MODES_PREAMBLE_US*2]- m[j+i+MODES_PREAMBLE_US*2+1]); } delta /= msglen*4; /* Filter for an average delta of three is small enough to let almost * every kind of message to pass, but high enough to filter some * random noise. */ if (delta < 10*255) continue; /* If we reached this point, and error is zero, we are very likely * with a Mode S message in our hands, but it may still be broken * and CRC may not be correct. This is handled by the next layer. */ if (errors == 0) { struct modesMessage mm; /* Decode the received message and update statistics */ decodeModesMessage(&mm,msg); Modes.stat_demodulated++; if (mm.errorbit == -1) { if (mm.crcok) Modes.stat_goodcrc++; else Modes.stat_badcrc++; } else { Modes.stat_badcrc++; Modes.stat_fixed++; } /* Output debug mode info if needed. */ if (Modes.debug == MODES_DEBUG_DEMOD) dumpRawMessage("Demodulated with 0 errors", msg, m, j); else if (Modes.debug == MODES_DEBUG_BADCRC && !mm.crcok) dumpRawMessage("Decoded with bad CRC", msg, m, j); else if (Modes.debug == MODES_DEBUG_GOODCRC && mm.crcok && mm.errorbit == -1) dumpRawMessage("Decoded with good CRC", msg, m, j); /* Pass data to the next layer */ useModesMessage(&mm); /* Skip this message if we are sure it's fine. */ if (mm.crcok) j += (MODES_PREAMBLE_US+(msglen*8))*2; } else { if (Modes.debug == MODES_DEBUG_DEMODERR) { printf("The following message has %d demod errors\n", errors); dumpRawMessage("Demodulated with errors", msg, m, j); } } } } /* When a new message is available, because it was decoded from the * RTL device, file, or received in the TCP input port, or any other * way we can receive a decoded message, we call this function in order * to use the message. * * Basically this function passes a raw message to the upper layers for * further processing and visualization. */ void useModesMessage(struct modesMessage *mm) { if (!Modes.stats && (Modes.check_crc == 0 || mm->crcok)) { if (Modes.interactive) { interactiveReceiveData(mm); } else { displayModesMessage(mm); if (!Modes.raw && !Modes.onlyaddr) printf("\n"); } /* Send data to connected clients. */ if (Modes.net) { modesSendRawOutput(mm); /* Feed raw output clients. */ } } } /* ========================= Interactive mode =============================== */ /* Return a new aircraft structure for the interactive mode linked list * of aircrafts. */ struct aircraft *interactiveCreateAircraft(uint32_t addr) { struct aircraft *a = malloc(sizeof(*a)); a->addr = addr; snprintf(a->hexaddr,sizeof(a->hexaddr),"%06x",(int)addr); a->flight[0] = '\0'; a->altitude = 0; a->speed = 0; a->track = 0; a->odd_cprlat = 0; a->odd_cprlon = 0; a->odd_cprtime = 0; a->even_cprlat = 0; a->even_cprlon = 0; a->even_cprtime = 0; a->lat = 0; a->lon = 0; a->seen = time(NULL); a->messages = 0; a->next = NULL; return a; } /* Return the aircraft with the specified address, or NULL if no aircraft * exists with this address. */ struct aircraft *interactiveFindAircraft(uint32_t addr) { struct aircraft *a = Modes.aircrafts; while(a) { if (a->addr == addr) return a; a = a->next; } return NULL; } /* Always positive MOD operation, used for CPR decoding. */ int cprModFunction(int a, int b) { int res = a % b; if (res < 0) res += b; return res; } /* The NL function uses the precomputed table from 1090-WP-9-14 */ int cprNLFunction(double lat) { if (lat < 10.47047130) return 59; if (lat < 14.82817437) return 58; if (lat < 18.18626357) return 57; if (lat < 21.02939493) return 56; if (lat < 23.54504487) return 55; if (lat < 25.82924707) return 54; if (lat < 27.93898710) return 53; if (lat < 29.91135686) return 52; if (lat < 31.77209708) return 51; if (lat < 33.53993436) return 50; if (lat < 35.22899598) return 49; if (lat < 36.85025108) return 48; if (lat < 38.41241892) return 47; if (lat < 39.92256684) return 46; if (lat < 41.38651832) return 45; if (lat < 42.80914012) return 44; if (lat < 44.19454951) return 43; if (lat < 45.54626723) return 42; if (lat < 46.86733252) return 41; if (lat < 48.16039128) return 40; if (lat < 49.42776439) return 39; if (lat < 50.67150166) return 38; if (lat < 51.89342469) return 37; if (lat < 53.09516153) return 36; if (lat < 54.27817472) return 35; if (lat < 55.44378444) return 34; if (lat < 56.59318756) return 33; if (lat < 57.72747354) return 32; if (lat < 58.84763776) return 31; if (lat < 59.95459277) return 30; if (lat < 61.04917774) return 29; if (lat < 62.13216659) return 28; if (lat < 63.20427479) return 27; if (lat < 64.26616523) return 26; if (lat < 65.31845310) return 25; if (lat < 66.36171008) return 24; if (lat < 67.39646774) return 23; if (lat < 68.42322022) return 22; if (lat < 69.44242631) return 21; if (lat < 70.45451075) return 20; if (lat < 71.45986473) return 19; if (lat < 72.45884545) return 18; if (lat < 73.45177442) return 17; if (lat < 74.43893416) return 16; if (lat < 75.42056257) return 15; if (lat < 76.39684391) return 14; if (lat < 77.36789461) return 13; if (lat < 78.33374083) return 12; if (lat < 79.29428225) return 11; if (lat < 80.24923213) return 10; if (lat < 81.19801349) return 9; if (lat < 82.13956981) return 8; if (lat < 83.07199445) return 7; if (lat < 83.99173563) return 6; if (lat < 84.89166191) return 5; if (lat < 85.75541621) return 4; if (lat < 86.53536998) return 3; if (lat < 87.00000000) return 2; else return 1; } int cprNFunction(int i, int isodd) { int nl = cprNLFunction(i) - isodd; if (nl < 1) nl = 1; return nl; } double cprDlonFunction(int i, int isodd) { return 360.0 / cprNFunction(i, isodd); } /* This algorithm comes from: * http://www.lll.lu/~edward/edward/adsb/DecodingADSBposition.html. * * * A few remarks: * 1) 131072 is 2^17 since CPR latitude and longitude are encoded in 17 bits. * 2) We assume that we always received the odd packet as last packet for * simplicity. This may provide a position that is less fresh of a few * seconds. */ void decodeCPR(struct aircraft *a) { const double AirDlat0 = 360.0 / 60; const double AirDlat1 = 360.0 / 59; double lat0 = a->even_cprlat; double lat1 = a->odd_cprlat; double lon0 = a->even_cprlon; double lon1 = a->odd_cprlon; /* Compute the Latitude Index "j" */ int j = floor(((59*lat0 - 60*lat1) / 131072) + 0.5); double rlat0 = AirDlat0 * (cprModFunction(j,60) + lat0 / 131072); double rlat1 = AirDlat1 * (cprModFunction(j,59) + lat1 / 131072); if (rlat0 >= 90 && rlat0 <= 270) rlat0 -= 360; if (rlat1 >= 90 && rlat1 <= 270) rlat1 -= 360; /* Check that both are in the same latitude zone, or abort. */ if (cprNLFunction(rlat0) != cprNLFunction(rlat1)) return; /* Compute ni and the longitude index m */ if (a->even_cprtime > a->odd_cprtime) { /* Use even packet. */ int ni = cprNFunction(rlat0,0); int m = floor((((lon0 * (cprNLFunction(rlat0)-1)) - (lon1 * cprNLFunction(rlat0))) / 131072) + 0.5); a->lon = cprDlonFunction(rlat0,0) * (cprModFunction(m,ni)+lon0/131072); a->lat = rlat0; } else { /* Use odd packet. */ int ni = cprNFunction(rlat1,1); int m = floor((((lon0 * (cprNLFunction(rlat1)-1)) - (lon1 * cprNLFunction(rlat1))) / 131072.0) + 0.5); a->lon = cprDlonFunction(rlat1,1) * (cprModFunction(m,ni)+lon1/131072); a->lat = rlat1; } if (a->lon > 180) a->lon -= 360; } /* Receive new messages and populate the interactive mode with more info. */ void interactiveReceiveData(struct modesMessage *mm) { uint32_t addr; struct aircraft *a, *aux; if (Modes.check_crc && mm->crcok == 0) return; addr = (mm->aa1 << 16) | (mm->aa2 << 8) | mm->aa3; /* Loookup our aircraft or create a new one. */ a = interactiveFindAircraft(addr); if (!a) { a = interactiveCreateAircraft(addr); a->next = Modes.aircrafts; Modes.aircrafts = a; } else { /* If it is an already known aircraft, move it on head * so we keep aircrafts ordered by received message time. * * However move it on head only if at least one second elapsed * since the aircraft that is currently on head sent a message, * othewise with multiple aircrafts at the same time we have an * useless shuffle of positions on the screen. */ if (0 && Modes.aircrafts != a && (time(NULL) - a->seen) >= 1) { aux = Modes.aircrafts; while(aux->next != a) aux = aux->next; /* Now we are a node before the aircraft to remove. */ aux->next = aux->next->next; /* removed. */ /* Add on head */ a->next = Modes.aircrafts; Modes.aircrafts = a; } } a->seen = time(NULL); a->messages++; if (mm->msgtype == 0 || mm->msgtype == 4 || mm->msgtype == 20) { a->altitude = mm->altitude; } else if (mm->msgtype == 17) { if (mm->metype >= 1 && mm->metype <= 4) { memcpy(a->flight, mm->flight, sizeof(a->flight)); } else if (mm->metype >= 9 && mm->metype <= 18) { a->altitude = mm->altitude; if (mm->fflag) { a->odd_cprlat = mm->raw_latitude; a->odd_cprlon = mm->raw_longitude; a->odd_cprtime = a->seen; /* Current time. */ } else { a->even_cprlat = mm->raw_latitude; a->even_cprlon = mm->raw_longitude; a->even_cprtime = a->seen; /* Current time. */ } /* If the two data is less than 10 seconds apart, compute * the position. */ if (abs(a->even_cprtime - a->odd_cprtime) <= 10) decodeCPR(a); } else if (mm->metype == 19) { if (mm->mesub == 1 || mm->mesub == 2) { a->speed = mm->velocity; a->track = mm->heading; } } } } /* Show the currently captured interactive data on screen. */ void interactiveShowData(void) { struct aircraft *a = Modes.aircrafts; time_t now = time(NULL); char progress[4]; int count = 0; memset(progress,' ',3); progress[time(NULL)%3] = '.'; progress[3] = '\0'; printf("\x1b[H\x1b[2J"); /* Clear the screen */ printf( "Hex Flight Altitude Speed Lat Lon Track Messages Seen %s\n" "--------------------------------------------------------------------------------\n", progress); while(a && count < Modes.interactive_rows) { int altitude = a->altitude, speed = a->speed; /* Convert units to metric if --metric was specified. */ if (Modes.metric) { altitude /= 3.2828; speed *= 1.852; } printf("%-6s %-8s %-9d %-7d %-7.03f %-7.03f %-3d %-9ld %d sec\n", a->hexaddr, a->flight, altitude, speed, a->lat, a->lon, a->track, a->messages, (int)(now - a->seen)); a = a->next; count++; } } /* When in interactive mode If we don't receive new nessages within * MODES_INTERACTIVE_TTL seconds we remove the aircraft from the list. */ void interactiveRemoveStaleAircrafts(void) { struct aircraft *a = Modes.aircrafts; struct aircraft *prev = NULL; time_t now = time(NULL); while(a) { if ((now - a->seen) > Modes.interactive_ttl) { struct aircraft *next = a->next; /* Remove the element from the linked list, with care * if we are removing the first element. */ free(a); if (!prev) Modes.aircrafts = next; else prev->next = next; a = next; } else { prev = a; a = a->next; } } } /* ============================== Snip mode ================================= */ /* Get raw IQ samples and filter everything is < than the specified level * for more than 256 samples in order to reduce example file size. */ void snipMode(int level) { int i, q; long long c = 0; while ((i = getchar()) != EOF && (q = getchar()) != EOF) { if (abs(i-127) < level && abs(q-127) < level) { c++; if (c > MODES_PREAMBLE_US*4) continue; } else { c = 0; } putchar(i); putchar(q); } } /* ============================= Networking ================================= * Note: here we risregard any kind of good coding practice in favor of * extreme simplicity, that is: * * 1) We only rely on the kernel buffers for our I/O without any kind of * user space buffering. * 2) We don't register any kind of event handler, from time to time a * function gets called and we accept new connections. All the rest is * handled via non-blocking I/O and manually pullign clients to see if * they have something new to share with us when reading is needed. */ /* Networking "stack" initialization. */ void modesInitNet(void) { memset(Modes.clients,0,sizeof(Modes.clients)); Modes.maxfd = -1; /* Raw output port */ Modes.ros = anetTcpServer(Modes.aneterr, Modes.net_output_raw_port, NULL); if (Modes.ros == -1) { fprintf(stderr, "Error opening raw TCP output port %d: %s\n", Modes.net_output_raw_port, Modes.aneterr); exit(1); } /* Raw input port */ Modes.ris = anetTcpServer(Modes.aneterr, Modes.net_input_raw_port, NULL); if (Modes.ris == -1) { fprintf(stderr, "Error opening raw TCP input port %d: %s\n", Modes.net_input_raw_port, Modes.aneterr); exit(1); } anetNonBlock(Modes.aneterr, Modes.ros); anetNonBlock(Modes.aneterr, Modes.ris); signal(SIGPIPE, SIG_IGN); } /* This function gets called from time to time when the decoding thread is * awakened by new data arriving. This usually happens a few times every * second. */ void modesAcceptClients(void) { int fd, port; unsigned int j; struct client *c; int services[2]; services[0] = Modes.ros; services[1] = Modes.ris; for (j = 0; j < sizeof(services)/sizeof(int); j++) { fd = anetTcpAccept(Modes.aneterr, services[j], NULL, &port); if (fd == -1) continue; if (fd >= MODES_NET_MAX_FD) { close(fd); return; /* Max number of clients reached. */ } c = malloc(sizeof(*c)); c->service = services[j]; c->fd = fd; c->buflen = 0; Modes.clients[fd] = c; if (Modes.maxfd < fd) Modes.maxfd = fd; j--; /* Try again with the same listening port. */ } } /* On error free the client, collect the structure, adjust maxfd if needed. */ void modesFreeClient(int fd) { close(fd); free(Modes.clients[fd]); Modes.clients[fd] = NULL; /* If this was our maxfd, rescan the full clients array to check what's * the new max. */ if (Modes.maxfd == fd) { int j; Modes.maxfd = -1; for (j = 0; j < MODES_NET_MAX_FD; j++) { if (Modes.clients[j]) Modes.maxfd = j; } } } /* Send the specified message to all clients listening for a given service. */ void modesSendAllClients(int service, void *msg, int len) { int j; struct client *c; for (j = 0; j <= Modes.maxfd; j++) { c = Modes.clients[j]; if (c && c->service == service) { int nwritten = write(j, msg, len); if (nwritten != len) { modesFreeClient(j); } } } } /* Write raw output to TCP clients. */ void modesSendRawOutput(struct modesMessage *mm) { char msg[128], *p = msg; int j; *p++ = '*'; for (j = 0; j < mm->msgbits/8; j++) { sprintf(p, "%02X", mm->msg[j]); p += 2; } *p++ = ';'; *p++ = '\n'; modesSendAllClients(Modes.ros, msg, p-msg); } /* Turn an hex digit into its 4 bit decimal value. * Returns -1 if the digit is not in the 0-F range. */ int hexDigitVal(int c) { c = tolower(c); if (c >= '0' && c <= '9') return c-'0'; else if (c >= 'a' && c <= 'f') return c-'a'+10; else return -1; } /* This function decodes a string representing a Mode S message in * raw hex format like: *8D4B969699155600E87406F5B69F; * * The message is passed to the higher level layers, so it feeds * the selected screen output, the network output and so forth. * * If the message looks invalid is silently discarded. */ void decodeHexMessage(char *hex) { int l = strlen(hex), j; unsigned char msg[MODES_LONG_MSG_BYTES]; struct modesMessage mm; if (l < 2 || hex[0] != '*' || hex[l-1] != ';') return; hex++; l-=2; /* Skip * and ; */ if (l > MODES_LONG_MSG_BYTES*2) return; /* Too long message... broken. */ for (j = 0; j < l; j += 2) { int high = hexDigitVal(hex[j]); int low = hexDigitVal(hex[j+1]); if (high == -1 || low == -1) return; msg[j/2] = (high<<4) | low; } decodeModesMessage(&mm,msg); useModesMessage(&mm); } /* This function polls all the clients using read() in order to receive new * messages from the net. * * Every full message received is decoded and passed to the higher layers. */ void modesReceiveRawInput(void) { int j; struct client *c; for (j = 0; j <= Modes.maxfd; j++) { c = Modes.clients[j]; if (c && c->service == Modes.ris) { while(1) { int left = sizeof(c->buf) - c->buflen; int nread = read(j, c->buf+c->buflen, left); int decoded = 0; int oldpos = c->buflen; int i; if (nread < 0) { if (nread == 0 || errno != EAGAIN) { /* Error, or end of file. */ modesFreeClient(j); } break; /* Serve next client. */ } c->buflen += nread; /* If there is a complete message there must be a newline * in the buffer. The iteration starts from 'oldpos' as * we need to check only the chars we read in this interaction * as we are sure there is no newline in the pre-existing * buffer. */ for (i = oldpos; i < c->buflen; i++) { if (c->buf[i] == '\n') { c->buf[i] = '\0'; if (i && c->buf[i-1] == '\r') c->buf[i-1] = '\0'; decodeHexMessage(c->buf); /* Move what's left at the start of the buffer. */ i++; memmove(c->buf,c->buf+i,c->buflen-i); c->buflen -= i; /* Maybe there are more messages inside the buffer. * Start looping from the start again. */ i = -1; decoded = 1; } } /* If our buffer is full discard it, this is some badly * formatted shit. */ if (c->buflen == sizeof(c->buf)) { c->buflen = 0; /* If there is garbage, read more to discard it ASAP. */ continue; } /* If no message was decoded process the next client, otherwise * read more data from the same client. */ if (!decoded) break; } } } } /* ================================ Main ==================================== */ void showHelp(void) { printf( "--device-index Select RTL device (default: 0).\n" "--gain Set gain (default: max gain. Use -100 for auto-gain).\n" "--enable-agc> Enable the Automatic Gain Control (default: off).\n" "--freq Set frequency (default: 1090 Mhz).\n" "--ifile Read data from file (use '-' for stdin).\n" "--interactive Interactive mode refreshing data on screen.\n" "--interactive-rows Max number of rows in interactive mode (default: 15).\n" "--interactive-ttl Remove from list if idle for (default: 60).\n" "--raw Show only messages hex values.\n" "--net Enable networking.\n" "--net-ro-port TCP listening port for raw output (default: 30002).\n" "--net-ri-port TCP listening port for raw input (default: 30001).\n" "--no-fix Disable single-bits error correction using CRC.\n" "--no-crc-check Disable messages with broken CRC (discouraged).\n" "--stats With --ifile print stats at exit. No other output.\n" "--onlyaddr Show only ICAO addresses (testing purposes).\n" "--metric Use metric units (meters, km/h, ...).\n" "--snip Strip IQ file removing samples < level.\n" "--debug Debug mode, see README for more information.\n" "--help Show this help.\n" ); } int main(int argc, char **argv) { int j; /* Set sane defaults. */ modesInitConfig(); /* Parse the command line options */ for (j = 1; j < argc; j++) { int more = j+1 < argc; /* There are more arguments. */ if (!strcmp(argv[j],"--device-index") && more) { Modes.dev_index = atoi(argv[++j]); } else if (!strcmp(argv[j],"--gain") && more) { Modes.gain = atof(argv[++j])*10; /* Gain is in tens of DBs */ } else if (!strcmp(argv[j],"--enable-agc")) { Modes.enable_agc++; } else if (!strcmp(argv[j],"--freq") && more) { Modes.freq = strtoll(argv[++j],NULL,10); } else if (!strcmp(argv[j],"--ifile") && more) { Modes.filename = strdup(argv[++j]); } else if (!strcmp(argv[j],"--no-fix")) { Modes.fix_errors = 0; } else if (!strcmp(argv[j],"--no-crc-check")) { Modes.check_crc = 0; } else if (!strcmp(argv[j],"--raw")) { Modes.raw = 1; } else if (!strcmp(argv[j],"--net")) { Modes.net = 1; } else if (!strcmp(argv[j],"--net-ro-port") && more) { Modes.net_output_raw_port = atoi(argv[++j]); } else if (!strcmp(argv[j],"--net-ri-port") && more) { Modes.net_input_raw_port = atoi(argv[++j]); } else if (!strcmp(argv[j],"--onlyaddr")) { Modes.onlyaddr = 1; } else if (!strcmp(argv[j],"--metric")) { Modes.metric = 1; } else if (!strcmp(argv[j],"--interactive")) { Modes.interactive = 1; } else if (!strcmp(argv[j],"--interactive-rows")) { Modes.interactive_rows = atoi(argv[++j]); } else if (!strcmp(argv[j],"--interactive-ttl")) { Modes.interactive_ttl = atoi(argv[++j]); } else if (!strcmp(argv[j],"--debug") && more) { Modes.debug = atoi(argv[++j]); } else if (!strcmp(argv[j],"--stats")) { Modes.stats = 1; } else if (!strcmp(argv[j],"--snip") && more) { snipMode(atoi(argv[++j])); exit(0); } else if (!strcmp(argv[j],"--help")) { showHelp(); exit(0); } else { fprintf(stderr, "Unknown or not enough arguments for option '%s'.\n\n", argv[j]); showHelp(); exit(1); } } /* Initialization */ modesInit(); if (Modes.filename == NULL) { modesInitRTLSDR(); } else { if (Modes.filename[0] == '-' && Modes.filename[1] == '\0') { Modes.fd = STDIN_FILENO; } else if ((Modes.fd = open(Modes.filename,O_RDONLY)) == -1) { perror("Opening data file"); exit(1); } } if (Modes.net) modesInitNet(); /* Create the thread that will read the data from the device. */ pthread_create(&Modes.reader_thread, NULL, readerThreadEntryPoint, NULL); pthread_mutex_lock(&Modes.data_mutex); while(1) { if (!Modes.data_ready) { pthread_cond_wait(&Modes.data_cond,&Modes.data_mutex); continue; } computeMagnitudeVector(); /* Signal to the other thread that we processed the available data * and we want more (useful for --ifile). */ Modes.data_ready = 0; pthread_cond_signal(&Modes.data_cond); /* Process data after releasing the lock, so that the capturing * thread can read data while we perform computationally expensive * stuff * at the same time. (This should only be useful with very * slow processors). */ pthread_mutex_unlock(&Modes.data_mutex); detectModeS(Modes.magnitude, Modes.data_len/2); if (Modes.net) { modesAcceptClients(); modesReceiveRawInput(); } /* Refresh screen when in interactive mode. */ if (Modes.interactive && (mstime() - Modes.interactive_last_update) > MODES_INTERACTIVE_REFRESH_TIME) { interactiveRemoveStaleAircrafts(); interactiveShowData(); Modes.interactive_last_update = mstime(); } pthread_mutex_lock(&Modes.data_mutex); if (Modes.exit) break; } /* If --ifile and --stats were given, print statistics. */ if (Modes.stats && Modes.filename) { printf("%lld valid preambles\n", Modes.stat_valid_preamble); printf("%lld demodulated with zero errors\n", Modes.stat_demodulated); printf("%lld with good crc\n", Modes.stat_goodcrc); printf("%lld with bad crc\n", Modes.stat_badcrc); printf("%lld single bit errors corrected\n", Modes.stat_fixed); } rtlsdr_close(Modes.dev); return 0; }