c2e79d4555
* Better preamble detection to skip most of the messages we'll likely not be able to decode. * A Phase correction algorithm that improves the recognition compared to the previous algorithm used. * Javascript output in debug mode, and a debug.html file that can be used in order to see graphically undecoded samples. * Ability to detect cross-read messages, that are, messages that happen to start and end across two different reads from the device or file. * A few bugx fixed. * README improved.
2565 lines
94 KiB
C
2565 lines
94 KiB
C
/* Mode1090, a Mode S messages decoder for RTLSDR devices.
|
|
*
|
|
* Copyright (C) 2012 by Salvatore Sanfilippo <antirez@gmail.com>
|
|
*
|
|
* 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 <stdio.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <pthread.h>
|
|
#include <stdint.h>
|
|
#include <errno.h>
|
|
#include <unistd.h>
|
|
#include <math.h>
|
|
#include <sys/time.h>
|
|
#include <signal.h>
|
|
#include <fcntl.h>
|
|
#include <ctype.h>
|
|
#include <sys/stat.h>
|
|
#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<<0)
|
|
#define MODES_DEBUG_DEMODERR (1<<1)
|
|
#define MODES_DEBUG_BADCRC (1<<2)
|
|
#define MODES_DEBUG_GOODCRC (1<<3)
|
|
#define MODES_DEBUG_NOPREAMBLE (1<<4)
|
|
#define MODES_DEBUG_NET (1<<5)
|
|
#define MODES_DEBUG_JS (1<<6)
|
|
|
|
/* 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_SBS_PORT 30003
|
|
#define MODES_NET_OUTPUT_RAW_PORT 30002
|
|
#define MODES_NET_INPUT_RAW_PORT 30001
|
|
#define MODES_NET_HTTP_PORT 8080
|
|
#define MODES_CLIENT_BUF_SIZE 1024
|
|
#define MODES_NET_SNDBUF_SIZE (1024*64)
|
|
|
|
#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+1]; /* 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. */
|
|
long long 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 sbsos; /* SBS output listening socket. */
|
|
int ros; /* Raw output listening socket. */
|
|
int ris; /* Raw input listening socket. */
|
|
int https; /* HTTP 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_only; /* Enable just networking. */
|
|
int net_output_sbs_port; /* SBS output TCP port. */
|
|
int net_output_raw_port; /* Raw output TCP port. */
|
|
int net_input_raw_port; /* Raw input TCP port. */
|
|
int net_http_port; /* HTTP 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. */
|
|
int aggressive; /* Aggressive detection algorithm. */
|
|
|
|
/* 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;
|
|
long long stat_single_bit_fix;
|
|
long long stat_two_bits_fix;
|
|
long long stat_http_requests;
|
|
long long stat_sbs_connections;
|
|
long long stat_out_of_phase;
|
|
} 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 */
|
|
int phase_corrected; /* True if phase correction was applied. */
|
|
|
|
/* 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);
|
|
struct aircraft* interactiveReceiveData(struct modesMessage *mm);
|
|
void modesSendRawOutput(struct modesMessage *mm);
|
|
void modesSendSBSOutput(struct modesMessage *mm, struct aircraft *a);
|
|
void useModesMessage(struct modesMessage *mm);
|
|
int fixSingleBitErrors(unsigned char *msg, int bits);
|
|
int fixTwoBitsErrors(unsigned char *msg, int bits);
|
|
int modesMessageLenByType(int type);
|
|
|
|
/* ============================= 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_only = 0;
|
|
Modes.net_output_sbs_port = MODES_NET_OUTPUT_SBS_PORT;
|
|
Modes.net_output_raw_port = MODES_NET_OUTPUT_RAW_PORT;
|
|
Modes.net_input_raw_port = MODES_NET_INPUT_RAW_PORT;
|
|
Modes.net_http_port = MODES_NET_HTTP_PORT;
|
|
Modes.onlyaddr = 0;
|
|
Modes.debug = 0;
|
|
Modes.interactive = 0;
|
|
Modes.interactive_rows = MODES_INTERACTIVE_ROWS;
|
|
Modes.interactive_ttl = MODES_INTERACTIVE_TTL;
|
|
Modes.aggressive = 0;
|
|
}
|
|
|
|
void modesInit(void) {
|
|
int i, q;
|
|
|
|
pthread_mutex_init(&Modes.data_mutex,NULL);
|
|
pthread_cond_init(&Modes.data_cond,NULL);
|
|
/* We add a full message minus a final bit to the length, so that we
|
|
* can carry the remaining part of the buffer that we can't process
|
|
* in the message detection loop, back at the start of the next data
|
|
* to process. This way we are able to also detect messages crossing
|
|
* two reads. */
|
|
Modes.data_len = MODES_DATA_LEN + (MODES_FULL_LEN-1)*4;
|
|
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);
|
|
}
|
|
memset(Modes.data,127,Modes.data_len);
|
|
|
|
/* 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.stat_single_bit_fix = 0;
|
|
Modes.stat_two_bits_fix = 0;
|
|
Modes.stat_http_requests = 0;
|
|
Modes.stat_sbs_connections = 0;
|
|
Modes.stat_out_of_phase = 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;
|
|
/* Move the last part of the previous buffer, that was not processed,
|
|
* on the start of the new buffer. */
|
|
memcpy(Modes.data, Modes.data+MODES_DATA_LEN, (MODES_FULL_LEN-1)*4);
|
|
/* Read the new data. */
|
|
memcpy(Modes.data+(MODES_FULL_LEN-1)*4, 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);
|
|
}
|
|
|
|
/* Move the last part of the previous buffer, that was not processed,
|
|
* on the start of the new buffer. */
|
|
memcpy(Modes.data, Modes.data+MODES_DATA_LEN, (MODES_FULL_LEN-1)*4);
|
|
toread = MODES_DATA_LEN;
|
|
p = Modes.data+(MODES_FULL_LEN-1)*4;
|
|
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 a few 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]);
|
|
}
|
|
}
|
|
|
|
/* Produce a raw representation of the message as a Javascript file
|
|
* loadable by debug.html. */
|
|
void dumpRawMessageJS(char *descr, unsigned char *msg,
|
|
uint16_t *m, uint32_t offset, int fixable)
|
|
{
|
|
int padding = 5; /* Show a few samples before the actual start. */
|
|
int start = offset - padding;
|
|
int end = offset + (MODES_PREAMBLE_US*2)+(MODES_LONG_MSG_BITS*2) - 1;
|
|
FILE *fp;
|
|
int j, fix1 = -1, fix2 = -1;
|
|
|
|
if (fixable != -1) {
|
|
fix1 = fixable & 0xff;
|
|
if (fixable > 255) fix2 = fixable >> 8;
|
|
}
|
|
|
|
if ((fp = fopen("frames.js","a")) == NULL) {
|
|
fprintf(stderr, "Error opening frames.js: %s\n", strerror(errno));
|
|
exit(1);
|
|
}
|
|
|
|
fprintf(fp,"frames.push({\"descr\": \"%s\", \"mag\": [", descr);
|
|
for (j = start; j <= end; j++) {
|
|
fprintf(fp,"%d", j < 0 ? 0 : m[j]);
|
|
if (j != end) fprintf(fp,",");
|
|
}
|
|
fprintf(fp,"], \"fix1\": %d, \"fix2\": %d, \"bits\": %d, \"hex\": \"",
|
|
fix1, fix2, modesMessageLenByType(msg[0]>>3));
|
|
for (j = 0; j < MODES_LONG_MSG_BYTES; j++)
|
|
fprintf(fp,"\\x%02x",msg[j]);
|
|
fprintf(fp,"\"});\n");
|
|
fclose(fp);
|
|
}
|
|
|
|
/* This is a wrapper for dumpMagnitudeVector() that also show the message
|
|
* in hex format with an additional description.
|
|
*
|
|
* descr is the additional message to show to describe the dump.
|
|
* msg points to the decoded message
|
|
* m is the original magnitude vector
|
|
* offset is the offset where the message starts
|
|
*
|
|
* The function also produces the Javascript file used by debug.html to
|
|
* display packets in a graphical format if the Javascript output was
|
|
* enabled.
|
|
*/
|
|
void dumpRawMessage(char *descr, unsigned char *msg,
|
|
uint16_t *m, uint32_t offset)
|
|
{
|
|
int j;
|
|
int msgtype = msg[0]>>3;
|
|
int fixable = -1;
|
|
|
|
if (msgtype == 11 || msgtype == 17) {
|
|
int msgbits = (msgtype == 11) ? MODES_SHORT_MSG_BITS :
|
|
MODES_LONG_MSG_BITS;
|
|
fixable = fixSingleBitErrors(msg,msgbits);
|
|
if (fixable == -1)
|
|
fixable = fixTwoBitsErrors(msg,msgbits);
|
|
}
|
|
|
|
if (Modes.debug & MODES_DEBUG_JS) {
|
|
dumpRawMessageJS(descr, msg, m, offset, fixable);
|
|
return;
|
|
}
|
|
|
|
printf("\n--- %s\n ", descr);
|
|
for (j = 0; j < MODES_LONG_MSG_BYTES; j++) {
|
|
printf("%02x",msg[j]);
|
|
if (j == MODES_SHORT_MSG_BYTES-1) printf(" ... ");
|
|
}
|
|
printf(" (DF %d, Fixable: %d)\n", msgtype, fixable);
|
|
dumpMagnitudeVector(m,offset);
|
|
printf("---\n\n");
|
|
}
|
|
|
|
/* ===================== Mode 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;
|
|
}
|
|
|
|
/* Similar to fixSingleBitErrors() but try every possible two bit combination.
|
|
* This is very slow and should be tried only against DF17 messages that
|
|
* don't pass the checksum, and only in Aggressive Mode. */
|
|
int fixTwoBitsErrors(unsigned char *msg, int bits) {
|
|
int j, i;
|
|
unsigned char aux[MODES_LONG_MSG_BITS/8];
|
|
|
|
for (j = 0; j < bits; j++) {
|
|
int byte1 = j/8;
|
|
int bitmask1 = 1 << (7-(j%8));
|
|
|
|
/* Don't check the same pairs multiple times, so i starts from j+1 */
|
|
for (i = j+1; i < bits; i++) {
|
|
int byte2 = i/8;
|
|
int bitmask2 = 1 << (7-(i%8));
|
|
uint32_t crc1, crc2;
|
|
|
|
memcpy(aux,msg,bits/8);
|
|
|
|
aux[byte1] ^= bitmask1; /* Flip j-th bit. */
|
|
aux[byte2] ^= bitmask2; /* Flip i-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);
|
|
/* We return the two bits as a 16 bit integer by shifting
|
|
* 'i' on the left. This is possible since 'i' will always
|
|
* be non-zero because i starts from j+1. */
|
|
return j | (i<<8);
|
|
}
|
|
}
|
|
}
|
|
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.
|
|
*
|
|
* This function expects mm->msgtype and mm->msgbits to be correctly
|
|
* populated by the caller.
|
|
*
|
|
* On success the correct ICAO address is stored in the modesMessage
|
|
* structure in the aa3, aa2, and aa1 fiedls.
|
|
*
|
|
* If the function successfully recovers a message with a correct checksum
|
|
* it returns 1. Otherwise 0 is returned. */
|
|
int bruteForceAP(unsigned char *msg, struct modesMessage *mm) {
|
|
unsigned char aux[MODES_LONG_MSG_BYTES];
|
|
int msgtype = mm->msgtype;
|
|
int msgbits = mm->msgbits;
|
|
|
|
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)) {
|
|
mm->aa1 = aux[lastbyte-2];
|
|
mm->aa2 = aux[lastbyte-1];
|
|
mm->aa3 = aux[lastbyte];
|
|
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;
|
|
} else if (Modes.aggressive && mm->msgtype == 17 &&
|
|
(mm->errorbit = fixTwoBitsErrors(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;
|
|
|
|
/* In the squawk (identity) field bits are interleaved like that
|
|
* (message bit 20 to bit 32):
|
|
*
|
|
* C1-A1-C2-A2-C4-A4-ZERO-B1-D1-B2-D2-B4-D4
|
|
*
|
|
* So every group of three bits A, B, C, D represent an integer
|
|
* from 0 to 7.
|
|
*
|
|
* The actual meaning is just 4 octal numbers, but we convert it
|
|
* into a base ten number tha happens to represent the four
|
|
* octal numbers.
|
|
*
|
|
* For more info: http://en.wikipedia.org/wiki/Gillham_code */
|
|
{
|
|
int a,b,c,d;
|
|
|
|
a = ((msg[3] & 0x80) >> 5) |
|
|
((msg[2] & 0x02) >> 0) |
|
|
((msg[2] & 0x08) >> 3);
|
|
b = ((msg[3] & 0x02) << 1) |
|
|
((msg[3] & 0x08) >> 2) |
|
|
((msg[3] & 0x20) >> 5);
|
|
c = ((msg[2] & 0x01) << 2) |
|
|
((msg[2] & 0x04) >> 1) |
|
|
((msg[2] & 0x10) >> 4);
|
|
d = ((msg[3] & 0x01) << 2) |
|
|
((msg[3] & 0x04) >> 1) |
|
|
((msg[3] & 0x10) >> 4);
|
|
mm->identity = a*1000 + b*100 + c*10 + d;
|
|
}
|
|
|
|
/* DF 11 & 17: try to populate our ICAO addresses whitelist.
|
|
* DFs with an AP field (xored addr and crc), try to decode it. */
|
|
if (mm->msgtype != 11 && mm->msgtype != 17) {
|
|
/* 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 (bruteForceAP(msg,mm)) {
|
|
/* We recovered the message, mark the checksum as valid. */
|
|
mm->crcok = 1;
|
|
} else {
|
|
mm->crcok = 0;
|
|
}
|
|
} else {
|
|
/* 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));
|
|
}
|
|
}
|
|
}
|
|
mm->phase_corrected = 0; /* Set to 1 by the caller if needed. */
|
|
}
|
|
|
|
/* 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 : %d\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];
|
|
}
|
|
}
|
|
|
|
/* Return -1 if the message is out of fase left-side
|
|
* Return 1 if the message is out of fase right-size
|
|
* Return 0 if the message is not particularly out of phase. */
|
|
int detectOutOfPhase(uint16_t *m) {
|
|
m += 16; /* Skip preamble. */
|
|
if (m[3] > m[2]/2) return 1;
|
|
if (m[6] > m[7]/2) return -1;
|
|
return 0;
|
|
}
|
|
|
|
/* This function does not really correct the phase of the message, it just
|
|
* applies a transformation to the first sample representing a given bit:
|
|
*
|
|
* If the previous bit was one, we amplify it a bit.
|
|
* If the previous bit was zero, we decrease it a bit.
|
|
*
|
|
* This simple transformation makes the message a bit more likely to be
|
|
* correctly decoded for out of phase messages:
|
|
*
|
|
* When messages are out of phase there is more uncertainty in
|
|
* sequences of the same bit multiple times, since 11111 will be
|
|
* transmitted as continuously altering magnitude (high, low, high, low...)
|
|
*
|
|
* However because the message is out of phase some part of the high
|
|
* is mixed in the low part, so that it is hard to distinguish if it is
|
|
* a zero or a one.
|
|
*
|
|
* However when the message is out of phase passing from 0 to 1 or from
|
|
* 1 to 0 happens in a very recognizable way, for instance in the 0 -> 1
|
|
* transition, magnitude goes low, high, high, low, and one of of the
|
|
* two middle samples the high will be *very* high as part of the previous
|
|
* or next high signal will be mixed there.
|
|
*
|
|
* Applying our simple transformation we make more likely if the current
|
|
* bit is a zero, to detect another zero. Symmetrically if it is a one
|
|
* it will be more likely to detect a one because of the transformation.
|
|
* In this way similar levels will be interpreted more likely in the
|
|
* correct way. */
|
|
void applyPhaseCorrection(uint16_t *m) {
|
|
int j;
|
|
|
|
for (j = 0; j < (MODES_LONG_MSG_BITS-1)*2; j += 2) {
|
|
if (m[j] > m[j+1]) {
|
|
/* One */
|
|
m[j+2] = (m[j+2] * 5) / 4;
|
|
} else {
|
|
/* Zero */
|
|
m[j+2] = (m[j+2] * 4) / 5;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* 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];
|
|
uint16_t aux[MODES_LONG_MSG_BITS*2];
|
|
uint32_t j;
|
|
int use_correction = 0;
|
|
|
|
/* 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, delta, i, errors;
|
|
int good_message = 0, out_of_phase;
|
|
|
|
if (use_correction) goto good_preamble; /* We already checked it. */
|
|
|
|
/* 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;
|
|
}
|
|
|
|
if (!Modes.aggressive) {
|
|
/* The samples between the two spikes must be < than the average
|
|
* of the high spikes level. We don't test bits too near to
|
|
* the high levels as signals can be out of phase so part of the
|
|
* energy can be in the near samples. */
|
|
high = (m[j]+m[j+2]+m[j+7]+m[j+9])/6;
|
|
if (m[j+4] >= high ||
|
|
m[j+5] >= 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 11-14 must be low, as it is the
|
|
* space between the preamble and real data. Again we don't test
|
|
* bits too near to high levels, see above. */
|
|
if (m[j+11] >= high ||
|
|
m[j+12] >= high ||
|
|
m[j+13] >= high ||
|
|
m[j+14] >= 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++;
|
|
|
|
good_preamble:
|
|
out_of_phase = detectOutOfPhase(m+j);
|
|
|
|
/* If the previous attempt with this message failed, retry using
|
|
* magnitude correction. */
|
|
if (use_correction) {
|
|
memcpy(aux,m+j+MODES_PREAMBLE_US*2,sizeof(aux));
|
|
applyPhaseCorrection(m+j);
|
|
}
|
|
|
|
/* 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];
|
|
delta = low-high;
|
|
if (delta < 0) delta = -delta;
|
|
|
|
if (i > 0 && delta < 256) {
|
|
bits[i/2] = bits[i/2-1];
|
|
} else 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;
|
|
}
|
|
}
|
|
|
|
/* Restore the original message if we used magnitude correction. */
|
|
if (use_correction)
|
|
memcpy(m+j+MODES_PREAMBLE_US*2,aux,sizeof(aux));
|
|
|
|
/* 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? */
|
|
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) {
|
|
use_correction = 0;
|
|
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 || (Modes.aggressive && errors < 3)) {
|
|
struct modesMessage mm;
|
|
|
|
/* Decode the received message and update statistics */
|
|
decodeModesMessage(&mm,msg);
|
|
|
|
/* Update statistics. */
|
|
if (mm.crcok || (!out_of_phase || use_correction)) {
|
|
if (errors == 0) Modes.stat_demodulated++;
|
|
if (mm.errorbit == -1) {
|
|
if (mm.crcok)
|
|
Modes.stat_goodcrc++;
|
|
else
|
|
Modes.stat_badcrc++;
|
|
} else {
|
|
Modes.stat_badcrc++;
|
|
Modes.stat_fixed++;
|
|
if (mm.errorbit < MODES_LONG_MSG_BITS)
|
|
Modes.stat_single_bit_fix++;
|
|
else
|
|
Modes.stat_two_bits_fix++;
|
|
}
|
|
}
|
|
|
|
/* Output debug mode info if needed. */
|
|
if (!out_of_phase || use_correction) {
|
|
if (Modes.debug & MODES_DEBUG_DEMOD)
|
|
dumpRawMessage("Demodulated with 0 errors", msg, m, j);
|
|
else if (Modes.debug & MODES_DEBUG_BADCRC &&
|
|
mm.msgtype == 17 &&
|
|
(!mm.crcok || mm.errorbit != -1))
|
|
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);
|
|
}
|
|
|
|
/* Skip this message if we are sure it's fine. */
|
|
if (mm.crcok) {
|
|
j += (MODES_PREAMBLE_US+(msglen*8))*2;
|
|
good_message = 1;
|
|
if (use_correction)
|
|
mm.phase_corrected = 1;
|
|
}
|
|
|
|
/* Pass data to the next layer */
|
|
useModesMessage(&mm);
|
|
} else {
|
|
if (Modes.debug & MODES_DEBUG_DEMODERR &&
|
|
(!out_of_phase || use_correction))
|
|
{
|
|
printf("The following message has %d demod errors\n", errors);
|
|
dumpRawMessage("Demodulated with errors", msg, m, j);
|
|
}
|
|
}
|
|
|
|
/* Retry with phase correction if possible. */
|
|
if (!good_message && !use_correction && out_of_phase) {
|
|
j--;
|
|
use_correction = 1;
|
|
Modes.stat_out_of_phase++;
|
|
} else {
|
|
use_correction = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* 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)) {
|
|
/* Track aircrafts in interactive mode or if the HTTP
|
|
* interface is enabled. */
|
|
if (Modes.interactive || Modes.stat_http_requests > 0 || Modes.stat_sbs_connections > 0) {
|
|
struct aircraft *a = interactiveReceiveData(mm);
|
|
if (a && Modes.stat_sbs_connections > 0) modesSendSBSOutput(mm, a); /* Feed SBS output clients. */
|
|
}
|
|
/* In non-interactive way, display messages on standard output. */
|
|
if (!Modes.interactive) {
|
|
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(double lat, int isodd) {
|
|
int nl = cprNLFunction(lat) - isodd;
|
|
if (nl < 1) nl = 1;
|
|
return nl;
|
|
}
|
|
|
|
double cprDlonFunction(double lat, int isodd) {
|
|
return 360.0 / cprNFunction(lat, 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. */
|
|
struct aircraft *interactiveReceiveData(struct modesMessage *mm) {
|
|
uint32_t addr;
|
|
struct aircraft *a, *aux;
|
|
|
|
if (Modes.check_crc && mm->crcok == 0) return NULL;
|
|
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 = mstime();
|
|
} else {
|
|
a->even_cprlat = mm->raw_latitude;
|
|
a->even_cprlon = mm->raw_longitude;
|
|
a->even_cprtime = mstime();
|
|
}
|
|
/* If the two data is less than 10 seconds apart, compute
|
|
* the position. */
|
|
if (abs(a->even_cprtime - a->odd_cprtime) <= 10000) {
|
|
decodeCPR(a);
|
|
}
|
|
} else if (mm->metype == 19) {
|
|
if (mm->mesub == 1 || mm->mesub == 2) {
|
|
a->speed = mm->velocity;
|
|
a->track = mm->heading;
|
|
}
|
|
}
|
|
}
|
|
return a;
|
|
}
|
|
|
|
/* 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) {
|
|
struct {
|
|
char *descr;
|
|
int *socket;
|
|
int port;
|
|
} services[4] = {
|
|
{"Raw TCP output", &Modes.ros, Modes.net_output_raw_port},
|
|
{"Raw TCP input", &Modes.ris, Modes.net_input_raw_port},
|
|
{"HTTP server", &Modes.https, Modes.net_http_port},
|
|
{"Basestation TCP output", &Modes.sbsos, Modes.net_output_sbs_port}
|
|
};
|
|
int j;
|
|
|
|
memset(Modes.clients,0,sizeof(Modes.clients));
|
|
Modes.maxfd = -1;
|
|
|
|
for (j = 0; j < 4; j++) {
|
|
int s = anetTcpServer(Modes.aneterr, services[j].port, NULL);
|
|
if (s == -1) {
|
|
fprintf(stderr, "Error opening the listening port %d (%s): %s\n",
|
|
services[j].port, services[j].descr, strerror(errno));
|
|
exit(1);
|
|
}
|
|
anetNonBlock(Modes.aneterr, s);
|
|
*services[j].socket = s;
|
|
}
|
|
|
|
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[4];
|
|
|
|
services[0] = Modes.ros;
|
|
services[1] = Modes.ris;
|
|
services[2] = Modes.https;
|
|
services[3] = Modes.sbsos;
|
|
|
|
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. */
|
|
}
|
|
|
|
anetNonBlock(Modes.aneterr, fd);
|
|
c = malloc(sizeof(*c));
|
|
c->service = services[j];
|
|
c->fd = fd;
|
|
c->buflen = 0;
|
|
Modes.clients[fd] = c;
|
|
anetSetSendBuffer(Modes.aneterr,fd,MODES_NET_SNDBUF_SIZE);
|
|
|
|
if (Modes.maxfd < fd) Modes.maxfd = fd;
|
|
if (services[j] == Modes.sbsos) Modes.stat_sbs_connections++;
|
|
|
|
j--; /* Try again with the same listening port. */
|
|
|
|
if (Modes.debug & MODES_DEBUG_NET)
|
|
printf("Created new client %d\n", fd);
|
|
}
|
|
}
|
|
|
|
/* 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 (Modes.debug & MODES_DEBUG_NET)
|
|
printf("Closing client %d\n", fd);
|
|
|
|
/* 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);
|
|
}
|
|
|
|
|
|
/* Write SBS output to TCP clients. */
|
|
void modesSendSBSOutput(struct modesMessage *mm, struct aircraft *a) {
|
|
char msg[256], *p = msg;
|
|
int emergency = 0, ground = 0, alert = 0, spi = 0;
|
|
|
|
if (mm->msgtype == 4 || mm->msgtype == 5 || mm->msgtype == 21) {
|
|
/* Node: identity is calculated/kept in base10 but is actually
|
|
* octal (07500 is represented as 7500) */
|
|
if (mm->identity == 7500 || mm->identity == 7600 ||
|
|
mm->identity == 7700) emergency = -1;
|
|
if (mm->fs == 1 || mm->fs == 3) ground = -1;
|
|
if (mm->fs == 2 || mm->fs == 3 || mm->fs == 4) alert = -1;
|
|
if (mm->fs == 4 || mm->fs == 5) spi = -1;
|
|
}
|
|
|
|
if (mm->msgtype == 0) {
|
|
p += sprintf(p, "MSG,5,,,%02X%02X%02X,,,,,,,%d,,,,,,,,,,",
|
|
mm->aa1, mm->aa2, mm->aa3, mm->altitude);
|
|
} else if (mm->msgtype == 4) {
|
|
p += sprintf(p, "MSG,5,,,%02X%02X%02X,,,,,,,%d,,,,,,,%d,%d,%d,%d",
|
|
mm->aa1, mm->aa2, mm->aa3, mm->altitude, alert, emergency, spi, ground);
|
|
} else if (mm->msgtype == 5) {
|
|
p += sprintf(p, "MSG,6,,,%02X%02X%02X,,,,,,,,,,,,,%d,%d,%d,%d,%d",
|
|
mm->aa1, mm->aa2, mm->aa3, mm->identity, alert, emergency, spi, ground);
|
|
} else if (mm->msgtype == 11) {
|
|
p += sprintf(p, "MSG,8,,,%02X%02X%02X,,,,,,,,,,,,,,,,,",
|
|
mm->aa1, mm->aa2, mm->aa3);
|
|
} else if (mm->msgtype == 17 && mm->metype == 4) {
|
|
p += sprintf(p, "MSG,1,,,%02X%02X%02X,,,,,,%s,,,,,,,,0,0,0,0",
|
|
mm->aa1, mm->aa2, mm->aa3, mm->flight);
|
|
} else if (mm->msgtype == 17 && mm->metype >= 9 && mm->metype <= 18) {
|
|
if (a->lat == 0 && a->lon == 0)
|
|
p += sprintf(p, "MSG,3,,,%02X%02X%02X,,,,,,,%d,,,,,,,0,0,0,0",
|
|
mm->aa1, mm->aa2, mm->aa3, mm->altitude);
|
|
else
|
|
p += sprintf(p, "MSG,3,,,%02X%02X%02X,,,,,,,%d,,,%1.5f,%1.5f,,,"
|
|
"0,0,0,0",
|
|
mm->aa1, mm->aa2, mm->aa3, mm->altitude, a->lat, a->lon);
|
|
} else if (mm->msgtype == 17 && mm->metype == 19 && mm->mesub == 1) {
|
|
int vr = (mm->vert_rate_sign==0?1:-1) * (mm->vert_rate-1) * 64;
|
|
|
|
p += sprintf(p, "MSG,4,,,%02X%02X%02X,,,,,,,,%d,%d,,,%i,,0,0,0,0",
|
|
mm->aa1, mm->aa2, mm->aa3, a->speed, a->track, vr);
|
|
} else if (mm->msgtype == 21) {
|
|
p += sprintf(p, "MSG,6,,,%02X%02X%02X,,,,,,,,,,,,,%d,%d,%d,%d,%d",
|
|
mm->aa1, mm->aa2, mm->aa3, mm->identity, alert, emergency, spi, ground);
|
|
} else {
|
|
return;
|
|
}
|
|
|
|
*p++ = '\n';
|
|
modesSendAllClients(Modes.sbsos, 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 string is supposed to be at the start of the client buffer
|
|
* and null-terminated.
|
|
*
|
|
* 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.
|
|
*
|
|
* The function always returns 0 (success) to the caller as there is
|
|
* no case where we want broken messages here to close the client
|
|
* connection. */
|
|
int decodeHexMessage(struct client *c) {
|
|
char *hex = c->buf;
|
|
int l = strlen(hex), j;
|
|
unsigned char msg[MODES_LONG_MSG_BYTES];
|
|
struct modesMessage mm;
|
|
|
|
/* Remove spaces on the left and on the right. */
|
|
while(l && isspace(hex[l-1])) {
|
|
hex[l-1] = '\0';
|
|
l--;
|
|
}
|
|
while(isspace(*hex)) {
|
|
hex++;
|
|
l--;
|
|
}
|
|
|
|
/* Turn the message into binary. */
|
|
if (l < 2 || hex[0] != '*' || hex[l-1] != ';') return 0;
|
|
hex++; l-=2; /* Skip * and ; */
|
|
if (l > MODES_LONG_MSG_BYTES*2) return 0; /* 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 0;
|
|
msg[j/2] = (high<<4) | low;
|
|
}
|
|
decodeModesMessage(&mm,msg);
|
|
useModesMessage(&mm);
|
|
return 0;
|
|
}
|
|
|
|
/* Return a description of planes in json. */
|
|
char *aircraftsToJson(int *len) {
|
|
struct aircraft *a = Modes.aircrafts;
|
|
int buflen = 1024; /* The initial buffer is incremented as needed. */
|
|
char *buf = malloc(buflen), *p = buf;
|
|
int l;
|
|
|
|
l = snprintf(p,buflen,"[\n");
|
|
p += l; buflen -= l;
|
|
while(a) {
|
|
int altitude = a->altitude, speed = a->speed;
|
|
|
|
/* Convert units to metric if --metric was specified. */
|
|
if (Modes.metric) {
|
|
altitude /= 3.2828;
|
|
speed *= 1.852;
|
|
}
|
|
|
|
if (a->lat != 0 && a->lon != 0) {
|
|
l = snprintf(p,buflen,
|
|
"{\"hex\":\"%s\", \"flight\":\"%s\", \"lat\":%f, "
|
|
"\"lon\":%f, \"altitude\":%d, \"track\":%d},\n",
|
|
a->hexaddr, a->flight, a->lat, a->lon, a->altitude, a->track);
|
|
p += l; buflen -= l;
|
|
/* Resize if needed. */
|
|
if (buflen < 256) {
|
|
int used = p-buf;
|
|
buflen += 1024; /* Our increment. */
|
|
buf = realloc(buf,used+buflen);
|
|
p = buf+used;
|
|
}
|
|
}
|
|
a = a->next;
|
|
}
|
|
/* Remove the final comma if any, and closes the json array. */
|
|
if (*(p-2) == ',') {
|
|
*(p-2) = '\n';
|
|
p--;
|
|
buflen++;
|
|
}
|
|
l = snprintf(p,buflen,"]\n");
|
|
p += l; buflen -= l;
|
|
|
|
*len = p-buf;
|
|
return buf;
|
|
}
|
|
|
|
#define MODES_CONTENT_TYPE_HTML "text/html;charset=utf-8"
|
|
#define MODES_CONTENT_TYPE_JSON "application/json;charset=utf-8"
|
|
|
|
/* Get an HTTP request header and write the response to the client.
|
|
* Again here we assume that the socket buffer is enough without doing
|
|
* any kind of userspace buffering.
|
|
*
|
|
* Returns 1 on error to signal the caller the client connection should
|
|
* be closed. */
|
|
int handleHTTPRequest(struct client *c) {
|
|
char hdr[512];
|
|
int clen, hdrlen;
|
|
int httpver, keepalive;
|
|
char *p, *url, *content;
|
|
char *ctype;
|
|
|
|
if (Modes.debug & MODES_DEBUG_NET)
|
|
printf("\nHTTP request: %s\n", c->buf);
|
|
|
|
/* Minimally parse the request. */
|
|
httpver = (strstr(c->buf, "HTTP/1.1") != NULL) ? 11 : 10;
|
|
if (httpver == 10) {
|
|
/* HTTP 1.0 defaults to close, unless otherwise specified. */
|
|
keepalive = strstr(c->buf, "Connection: keep-alive") != NULL;
|
|
} else if (httpver == 11) {
|
|
/* HTTP 1.1 defaults to keep-alive, unless close is specified. */
|
|
keepalive = strstr(c->buf, "Connection: close") == NULL;
|
|
}
|
|
|
|
/* Identify he URL. */
|
|
p = strchr(c->buf,' ');
|
|
if (!p) return 1; /* There should be the method and a space... */
|
|
url = ++p; /* Now this should point to the requested URL. */
|
|
p = strchr(p, ' ');
|
|
if (!p) return 1; /* There should be a space before HTTP/... */
|
|
*p = '\0';
|
|
|
|
if (Modes.debug & MODES_DEBUG_NET) {
|
|
printf("\nHTTP keep alive: %d\n", keepalive);
|
|
printf("HTTP requested URL: %s\n\n", url);
|
|
}
|
|
|
|
/* Select the content to send, we have just two so far:
|
|
* "/" -> Our google map application.
|
|
* "/data.json" -> Our ajax request to update planes. */
|
|
if (strstr(url, "/data.json")) {
|
|
content = aircraftsToJson(&clen);
|
|
ctype = MODES_CONTENT_TYPE_JSON;
|
|
} else {
|
|
struct stat sbuf;
|
|
int fd = -1;
|
|
|
|
if (stat("gmap.html",&sbuf) != -1 &&
|
|
(fd = open("gmap.html",O_RDONLY)) != -1)
|
|
{
|
|
content = malloc(sbuf.st_size);
|
|
if (read(fd,content,sbuf.st_size) == -1) {
|
|
snprintf(content,sbuf.st_size,"Error reading from file: %s",
|
|
strerror(errno));
|
|
}
|
|
clen = sbuf.st_size;
|
|
} else {
|
|
char buf[128];
|
|
|
|
clen = snprintf(buf,sizeof(buf),"Error opening HTML file: %s",
|
|
strerror(errno));
|
|
content = strdup(buf);
|
|
}
|
|
if (fd != -1) close(fd);
|
|
ctype = MODES_CONTENT_TYPE_HTML;
|
|
}
|
|
|
|
/* Create the header and send the reply. */
|
|
hdrlen = snprintf(hdr, sizeof(hdr),
|
|
"HTTP/1.1 200 OK\r\n"
|
|
"Server: Dump1090\r\n"
|
|
"Content-Type: %s\r\n"
|
|
"Connection: %s\r\n"
|
|
"Content-Length: %d\r\n"
|
|
"\r\n",
|
|
ctype,
|
|
keepalive ? "keep-alive" : "close",
|
|
clen);
|
|
|
|
if (Modes.debug & MODES_DEBUG_NET)
|
|
printf("HTTP Reply header:\n%s", hdr);
|
|
|
|
/* Send header and content. */
|
|
if (write(c->fd, hdr, hdrlen) == -1 ||
|
|
write(c->fd, content, clen) == -1)
|
|
{
|
|
free(content);
|
|
return 1;
|
|
}
|
|
free(content);
|
|
Modes.stat_http_requests++;
|
|
return !keepalive;
|
|
}
|
|
|
|
/* This function polls the clients using read() in order to receive new
|
|
* messages from the net.
|
|
*
|
|
* The message is supposed to be separated by the next message by the
|
|
* separator 'sep', that is a null-terminated C string.
|
|
*
|
|
* Every full message received is decoded and passed to the higher layers
|
|
* calling the function 'handler'.
|
|
*
|
|
* The handelr returns 0 on success, or 1 to signal this function we
|
|
* should close the connection with the client in case of non-recoverable
|
|
* errors. */
|
|
void modesReadFromClient(struct client *c, char *sep,
|
|
int(*handler)(struct client *))
|
|
{
|
|
while(1) {
|
|
int left = MODES_CLIENT_BUF_SIZE - c->buflen;
|
|
int nread = read(c->fd, c->buf+c->buflen, left);
|
|
int fullmsg = 0;
|
|
int i;
|
|
char *p;
|
|
|
|
if (nread <= 0) {
|
|
if (nread == 0 || errno != EAGAIN) {
|
|
/* Error, or end of file. */
|
|
modesFreeClient(c->fd);
|
|
}
|
|
break; /* Serve next client. */
|
|
}
|
|
c->buflen += nread;
|
|
|
|
/* Always null-term so we are free to use strstr() */
|
|
c->buf[c->buflen] = '\0';
|
|
|
|
/* If there is a complete message there must be the separator 'sep'
|
|
* in the buffer, note that we full-scan the buffer at every read
|
|
* for simplicity. */
|
|
while ((p = strstr(c->buf, sep)) != NULL) {
|
|
i = p - c->buf; /* Turn it as an index inside the buffer. */
|
|
c->buf[i] = '\0'; /* Te handler expects null terminated strings. */
|
|
/* Call the function to process the message. It returns 1
|
|
* on error to signal we should close the client connection. */
|
|
if (handler(c)) {
|
|
modesFreeClient(c->fd);
|
|
return;
|
|
}
|
|
/* Move what's left at the start of the buffer. */
|
|
i += strlen(sep); /* The separator is part of the previous msg. */
|
|
memmove(c->buf,c->buf+i,c->buflen-i);
|
|
c->buflen -= i;
|
|
c->buf[c->buflen] = '\0';
|
|
/* Maybe there are more messages inside the buffer.
|
|
* Start looping from the start again. */
|
|
fullmsg = 1;
|
|
}
|
|
/* If our buffer is full discard it, this is some badly
|
|
* formatted shit. */
|
|
if (c->buflen == MODES_CLIENT_BUF_SIZE) {
|
|
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 (!fullmsg) break;
|
|
}
|
|
}
|
|
|
|
/* Read data from clients. This function actually delegates a lower-level
|
|
* function that depends on the kind of service (raw, http, ...). */
|
|
void modesReadFromClients(void) {
|
|
int j;
|
|
struct client *c;
|
|
|
|
for (j = 0; j <= Modes.maxfd; j++) {
|
|
if ((c = Modes.clients[j]) == NULL) continue;
|
|
if (c->service == Modes.ris)
|
|
modesReadFromClient(c,"\n",decodeHexMessage);
|
|
else if (c->service == Modes.https)
|
|
modesReadFromClient(c,"\r\n\r\n",handleHTTPRequest);
|
|
}
|
|
}
|
|
|
|
/* ================================ Main ==================================== */
|
|
|
|
void showHelp(void) {
|
|
printf(
|
|
"--device-index <index> Select RTL device (default: 0).\n"
|
|
"--gain <db> Set gain (default: max gain. Use -100 for auto-gain).\n"
|
|
"--enable-agc> Enable the Automatic Gain Control (default: off).\n"
|
|
"--freq <hz> Set frequency (default: 1090 Mhz).\n"
|
|
"--ifile <filename> Read data from file (use '-' for stdin).\n"
|
|
"--interactive Interactive mode refreshing data on screen.\n"
|
|
"--interactive-rows <num> Max number of rows in interactive mode (default: 15).\n"
|
|
"--interactive-ttl <sec> Remove from list if idle for <sec> (default: 60).\n"
|
|
"--raw Show only messages hex values.\n"
|
|
"--net Enable networking.\n"
|
|
"--net-only Enable just networking, no RTL device or file used.\n"
|
|
"--net-ro-port <port> TCP listening port for raw output (default: 30002).\n"
|
|
"--net-ri-port <port> TCP listening port for raw input (default: 30001).\n"
|
|
"--net-http-port <port> HTTP server port (default: 8080).\n"
|
|
"--net-sbs-port <port> TCP listening port for BaseStation format output (default: 30003).\n"
|
|
"--no-fix Disable single-bits error correction using CRC.\n"
|
|
"--no-crc-check Disable messages with broken CRC (discouraged).\n"
|
|
"--aggressive More CPU for more messages (two bits fixes, ...).\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 <level> Strip IQ file removing samples < level.\n"
|
|
"--debug <flags> Debug mode (verbose), see README for details.\n"
|
|
"--help Show this help.\n"
|
|
"\n"
|
|
"Debug mode flags: d = Log frames decoded with errors\n"
|
|
" D = Log frames decoded with zero errors\n"
|
|
" c = Log frames with bad CRC\n"
|
|
" C = Log frames with good CRC\n"
|
|
" p = Log frames with bad preamble\n"
|
|
" n = Log network debugging info\n"
|
|
" j = Log frames to frames.js, loadable by debug.html.\n"
|
|
);
|
|
}
|
|
|
|
/* This function is called a few times every second by main in order to
|
|
* perform tasks we need to do continuously, like accepting new clients
|
|
* from the net, refreshing the screen in interactive mode, and so forth. */
|
|
void backgroundTasks(void) {
|
|
if (Modes.net) {
|
|
modesAcceptClients();
|
|
modesReadFromClients();
|
|
}
|
|
|
|
/* 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();
|
|
}
|
|
}
|
|
|
|
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-only")) {
|
|
Modes.net = 1;
|
|
Modes.net_only = 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],"--net-http-port") && more) {
|
|
Modes.net_http_port = atoi(argv[++j]);
|
|
} else if (!strcmp(argv[j],"--net-sbs-port") && more) {
|
|
Modes.net_output_sbs_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],"--aggressive")) {
|
|
Modes.aggressive++;
|
|
} 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) {
|
|
char *f = argv[++j];
|
|
while(*f) {
|
|
switch(*f) {
|
|
case 'D': Modes.debug |= MODES_DEBUG_DEMOD; break;
|
|
case 'd': Modes.debug |= MODES_DEBUG_DEMODERR; break;
|
|
case 'C': Modes.debug |= MODES_DEBUG_GOODCRC; break;
|
|
case 'c': Modes.debug |= MODES_DEBUG_BADCRC; break;
|
|
case 'p': Modes.debug |= MODES_DEBUG_NOPREAMBLE; break;
|
|
case 'n': Modes.debug |= MODES_DEBUG_NET; break;
|
|
case 'j': Modes.debug |= MODES_DEBUG_JS; break;
|
|
default:
|
|
fprintf(stderr, "Unknown debugging flag: %c\n", *f);
|
|
exit(1);
|
|
break;
|
|
}
|
|
f++;
|
|
}
|
|
} 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.net_only) {
|
|
fprintf(stderr,"Net-only mode, no RTL device or file open.\n");
|
|
} else 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();
|
|
|
|
/* If the user specifies --net-only, just run in order to serve network
|
|
* clients without reading data from the RTL device. */
|
|
while (Modes.net_only) {
|
|
backgroundTasks();
|
|
usleep(100000);
|
|
}
|
|
|
|
/* 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);
|
|
backgroundTasks();
|
|
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 again after phase correction\n",
|
|
Modes.stat_out_of_phase);
|
|
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 errors corrected\n", Modes.stat_fixed);
|
|
printf("%lld single bit errors\n", Modes.stat_single_bit_fix);
|
|
printf("%lld two bits errors\n", Modes.stat_two_bits_fix);
|
|
printf("%lld total usable messages\n",
|
|
Modes.stat_goodcrc + Modes.stat_fixed);
|
|
}
|
|
|
|
rtlsdr_close(Modes.dev);
|
|
return 0;
|
|
}
|