AFG/dump1090
3
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

Separate 2.4MHz demodulator into its own file.

Browse source
This commit is contained in:
Oliver Jowett 2015-01-19 23:50:25 +00:00
parent a6542b505b
commit f753c2d9fe
6 changed files with 573 additions and 501 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
Makefile
View file

@ -21,7 +21,7 @@ all: dump1090 view1090
%.o: %.c dump1090.h
$(CC) $(CPPFLAGS) $(CFLAGS) $(EXTRACFLAGS) -c $<
dump1090: dump1090.o anet.o interactive.o mode_ac.o mode_s.o net_io.o crc.o demod_2000.o
dump1090: dump1090.o anet.o interactive.o mode_ac.o mode_s.o net_io.o crc.o demod_2000.o demod_2400.o
$(CC) -g -o dump1090 $^ $(LIBS) $(LIBS_RTL) $(LDFLAGS)
view1090: view1090.o anet.o interactive.o mode_ac.o mode_s.o net_io.o crc.o

532
demod_2400.c Normal file
View file

@ -0,0 +1,532 @@
// dump1090, a Mode S messages decoder for RTLSDR devices.
//
// Copyright (C) 2014,2015 Oliver Jowett <oliver@mutability.co.uk>
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#include "dump1090.h"
// 2.4MHz sampling rate version
//
// When sampling at 2.4MHz we have exactly 6 samples per 5 symbols.
// Each symbol is 500ns wide, each sample is 416.7ns wide
//
// We maintain a phase offset that is expressed in units of 1/5 of a sample i.e. 1/6 of a symbol, 83.333ns
// Each symbol we process advances the phase offset by 6 i.e. 6/5 of a sample, 500ns
//
// The correlation functions below correlate a 1-0 pair of symbols (i.e. manchester encoded 1 bit)
// starting at the given sample, and assuming that the symbol starts at a fixed 0-5 phase offset within
// m[0]. They return a correlation value, generally interpreted as >0 = 1 bit, <0 = 0 bit
// TODO check if there are better (or more balanced) correlation functions to use here
// nb: the correlation functions sum to zero, so we do not need to adjust for the DC offset in the input signal
// (adding any constant value to all of m[0..3] does not change the result)
static inline int slice_phase0(uint16_t *m) {
return 5 * m[0] - 3 * m[1] - 2 * m[2];
}
static inline int slice_phase1(uint16_t *m) {
return 4 * m[0] - m[1] - 3 * m[2];
}
static inline int slice_phase2(uint16_t *m) {
return 3 * m[0] + m[1] - 4 * m[2];
}
static inline int slice_phase3(uint16_t *m) {
return 2 * m[0] + 3 * m[1] - 5 * m[2];
}
static inline int slice_phase4(uint16_t *m) {
return m[0] + 5 * m[1] - 5 * m[2] - m[3];
}
static inline int correlate_phase0(uint16_t *m) {
return slice_phase0(m) * 26;
}
static inline int correlate_phase1(uint16_t *m) {
return slice_phase1(m) * 38;
}
static inline int correlate_phase2(uint16_t *m) {
return slice_phase2(m) * 38;
}
static inline int correlate_phase3(uint16_t *m) {
return slice_phase3(m) * 26;
}
static inline int correlate_phase4(uint16_t *m) {
return slice_phase4(m) * 19;
}
//
// These functions work out the correlation quality for the 10 symbols (5 bits) starting at m[0] + given phase offset.
// This is used to find the right phase offset to use for decoding.
//
static inline int correlate_check_0(uint16_t *m) {
return
abs(correlate_phase0(&m[0])) +
abs(correlate_phase2(&m[2])) +
abs(correlate_phase4(&m[4])) +
abs(correlate_phase1(&m[7])) +
abs(correlate_phase3(&m[9]));
}
static inline int correlate_check_1(uint16_t *m) {
return
abs(correlate_phase1(&m[0])) +
abs(correlate_phase3(&m[2])) +
abs(correlate_phase0(&m[5])) +
abs(correlate_phase2(&m[7])) +
abs(correlate_phase4(&m[9]));
}
static inline int correlate_check_2(uint16_t *m) {
return
abs(correlate_phase2(&m[0])) +
abs(correlate_phase4(&m[2])) +
abs(correlate_phase1(&m[5])) +
abs(correlate_phase3(&m[7])) +
abs(correlate_phase0(&m[10]));
}
static inline int correlate_check_3(uint16_t *m) {
return
abs(correlate_phase3(&m[0])) +
abs(correlate_phase0(&m[3])) +
abs(correlate_phase2(&m[5])) +
abs(correlate_phase4(&m[7])) +
abs(correlate_phase1(&m[10]));
}
static inline int correlate_check_4(uint16_t *m) {
return
abs(correlate_phase4(&m[0])) +
abs(correlate_phase1(&m[3])) +
abs(correlate_phase3(&m[5])) +
abs(correlate_phase0(&m[8])) +
abs(correlate_phase2(&m[10]));
}
// Work out the best phase offset to use for the given message.
static int best_phase(uint16_t *m) {
int test;
int best = -1;
int bestval = (m[0] + m[1] + m[2] + m[3] + m[4] + m[5]); // minimum correlation quality we will accept
// empirical testing suggests that 4..8 is the best range to test for here
// (testing a wider range runs the danger of picking the wrong phase for
// a message that would otherwise be successfully decoded - the correlation
// functions can match well with a one symbol / half bit offset)
// this is consistent with the peak detection which should produce
// the first data symbol with phase offset 4..8
//test = correlate_check_2(&m[0]);
//if (test > bestval) { bestval = test; best = 2; }
//test = correlate_check_3(&m[0]);
//if (test > bestval) { bestval = test; best = 3; }
test = correlate_check_4(&m[0]);
if (test > bestval) { bestval = test; best = 4; }
test = correlate_check_0(&m[1]);
if (test > bestval) { bestval = test; best = 5; }
test = correlate_check_1(&m[1]);
if (test > bestval) { bestval = test; best = 6; }
test = correlate_check_2(&m[1]);
if (test > bestval) { bestval = test; best = 7; }
test = correlate_check_3(&m[1]);
if (test > bestval) { bestval = test; best = 8; }
//test = correlate_check_4(&m[1]);
//if (test > bestval) { bestval = test; best = 9; }
return best;
}
//
//=========================================================================
//
// 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 demodulate2400(uint16_t *m, uint32_t mlen)
{
struct modesMessage mm;
unsigned char msg[MODES_LONG_MSG_BYTES], *pMsg;
uint32_t j;
memset(&mm, 0, sizeof(mm));
for (j = 0; j < mlen; j++) {
uint16_t *preamble = &m[j];
int high, i, initial_phase, phase, errors, errors56, errorsTy;
int msglen, scanlen;
uint16_t *pPtr;
uint8_t theByte, theErrs;
uint32_t sigLevel, noiseLevel;
uint16_t snr;
int try_phase;
// Look for a message starting at around sample 0 with phase offset 3..7
// Ideal sample values for preambles with different phase
// Xn is the first data symbol with phase offset N
//
// sample#: 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0
// phase 3: 2/4\0/5\1 0 0 0 0/5\1/3 3\0 0 0 0 0 0 X4
// phase 4: 1/5\0/4\2 0 0 0 0/4\2 2/4\0 0 0 0 0 0 0 X0
// phase 5: 0/5\1/3 3\0 0 0 0/3 3\1/5\0 0 0 0 0 0 0 X1
// phase 6: 0/4\2 2/4\0 0 0 0 2/4\0/5\1 0 0 0 0 0 0 X2
// phase 7: 0/3 3\1/5\0 0 0 0 1/5\0/4\2 0 0 0 0 0 0 X3
//
// quick check: we must have a rising edge 0->1 and a falling edge 12->13
if (! (preamble[0] < preamble[1] && preamble[12] > preamble[13]) )
continue;
if (preamble[1] > preamble[2] && // 1
preamble[2] < preamble[3] && preamble[3] > preamble[4] && // 3
preamble[8] < preamble[9] && preamble[9] > preamble[10] && // 9
preamble[10] < preamble[11]) { // 11-12
// peaks at 1,3,9,11-12: phase 3
high = (preamble[1] + preamble[3] + preamble[9] + preamble[11] + preamble[12]) / 4;
sigLevel = preamble[1] + preamble[3] + preamble[9];
noiseLevel = preamble[5] + preamble[6] + preamble[7];
} else if (preamble[1] > preamble[2] && // 1
preamble[2] < preamble[3] && preamble[3] > preamble[4] && // 3
preamble[8] < preamble[9] && preamble[9] > preamble[10] && // 9
preamble[11] < preamble[12]) { // 12
// peaks at 1,3,9,12: phase 4
high = (preamble[1] + preamble[3] + preamble[9] + preamble[12]) / 4;
sigLevel = preamble[1] + preamble[3] + preamble[9] + preamble[12];
noiseLevel = preamble[5] + preamble[6] + preamble[7] + preamble[8];
} else if (preamble[1] > preamble[2] && // 1
preamble[2] < preamble[3] && preamble[4] > preamble[5] && // 3-4
preamble[8] < preamble[9] && preamble[10] > preamble[11] && // 9-10
preamble[11] < preamble[12]) { // 12
// peaks at 1,3-4,9-10,12: phase 5
high = (preamble[1] + preamble[3] + preamble[4] + preamble[9] + preamble[10] + preamble[12]) / 4;
sigLevel = preamble[1] + preamble[12];
noiseLevel = preamble[6] + preamble[7];
} else if (preamble[1] > preamble[2] && // 1
preamble[3] < preamble[4] && preamble[4] > preamble[5] && // 4
preamble[9] < preamble[10] && preamble[10] > preamble[11] && // 10
preamble[11] < preamble[12]) { // 12
// peaks at 1,4,10,12: phase 6
high = (preamble[1] + preamble[4] + preamble[10] + preamble[12]) / 4;
sigLevel = preamble[1] + preamble[4] + preamble[10] + preamble[12];
noiseLevel = preamble[5] + preamble[6] + preamble[7] + preamble[8];
} else if (preamble[2] > preamble[3] && // 1-2
preamble[3] < preamble[4] && preamble[4] > preamble[5] && // 4
preamble[9] < preamble[10] && preamble[10] > preamble[11] && // 10
preamble[11] < preamble[12]) { // 12
// peaks at 1-2,4,10,12: phase 7
high = (preamble[1] + preamble[2] + preamble[4] + preamble[10] + preamble[12]) / 4;
sigLevel = preamble[4] + preamble[10] + preamble[12];
noiseLevel = preamble[6] + preamble[7] + preamble[8];
} else {
// no suitable peaks
continue;
}
// Check for enough signal
if (sigLevel * 2 < 3 * noiseLevel) // about 3.5dB SNR
continue;
// Check that the "quiet" bits 6,7,15,16,17 are actually quiet
if (preamble[5] >= high ||
preamble[6] >= high ||
preamble[7] >= high ||
preamble[8] >= high ||
preamble[14] >= high ||
preamble[15] >= high ||
preamble[16] >= high ||
preamble[17] >= high ||
preamble[18] >= high) {
++Modes.stat_preamble_not_quiet;
continue;
}
// Crosscorrelate against the first few bits to find a likely phase offset
initial_phase = best_phase(&preamble[19]);
if (initial_phase < 0) {
++Modes.stat_preamble_no_correlation;
continue; // nothing satisfactory
}
Modes.stat_valid_preamble++;
Modes.stat_preamble_phase[initial_phase%MODES_MAX_PHASE_STATS]++;
try_phase = initial_phase;
retry:
// Rather than clear the whole mm structure, just clear the parts which are required. The clear
// is required for every possible preamble, and we don't want to be memset-ing the whole
// modesMessage structure if we don't have to..
mm.bFlags =
mm.crcok =
mm.correctedbits = 0;
// Decode all the next 112 bits, regardless of the actual message
// size. We'll check the actual message type later
pMsg = &msg[0];
pPtr = &m[j+19] + (try_phase/5);
phase = try_phase % 5;
theByte = 0;
theErrs = 0; errorsTy = 0;
errors = 0; errors56 = 0;
msglen = scanlen = MODES_LONG_MSG_BITS;
for (i = 0; i < scanlen; i++) {
int test;
switch (phase) {
case 0:
test = slice_phase0(pPtr);
phase = 2;
pPtr += 2;
break;
case 1:
test = slice_phase1(pPtr);
phase = 3;
pPtr += 2;
break;
case 2:
test = slice_phase2(pPtr);
phase = 4;
pPtr += 2;
break;
case 3:
test = slice_phase3(pPtr);
phase = 0;
pPtr += 3;
break;
case 4:
test = slice_phase4(pPtr);
// A phase-4 bit exactly straddles a sample boundary.
// Here's what a 1-0 bit with phase 4 looks like:
//
// |SYM 1|
// xxx| | |xxx
// |SYM 2|
//
// 012340123401234012340 <-- sample phase
// | 0 | 1 | 2 | 3 | <-- sample boundaries
//
// Samples 1 and 2 only have power from symbols 1 and 2.
// So we can use this to extract signal/noise values
// as one of the two symbols is high (signal) and the
// other is low (noise)
//
// This also gives us an equal number of signal and noise
// samples, which is convenient. Using the first half of
// a phase 0 bit, or the second half of a phase 3 bit, would
// also work, but we have no guarantees about how many signal
// or noise bits we'd see in those phases.
if (test < 0) { // 0 1
noiseLevel += pPtr[1];
sigLevel += pPtr[2];
} else { // 1 0
sigLevel += pPtr[1];
noiseLevel += pPtr[2];
}
phase = 1;
pPtr += 3;
break;
default:
test = 0;
break;
}
if (test > 0)
theByte |= 1;
/* else if (test < 0) theByte |= 0; */
else if (test == 0) {
if (i >= MODES_SHORT_MSG_BITS) { // poor correlation, and we're in the long part of a frame
errors++;
} else if (i >= 5) { // poor correlation, and we're in the short part of a frame
scanlen = MODES_LONG_MSG_BITS;
errors56 = ++errors;
} else if (i) { // poor correlation, and we're in the message type part of a frame
errorsTy = errors56 = ++errors;
theErrs |= 1;
} else { // poor correlation, and we're in the first bit of the message type part of a frame
errorsTy = errors56 = ++errors;
theErrs |= 1;
}
}
if ((i & 7) == 7)
*pMsg++ = theByte;
theByte = theByte << 1;
if (i < 7)
{theErrs = theErrs << 1;}
// If we've exceeded the permissible number of encoding errors, abandon ship now
if (errors > MODES_MSG_ENCODER_ERRS) {
if (i < MODES_SHORT_MSG_BITS) {
msglen = 0;
} else if ((errorsTy == 1) && (theErrs == 0x80)) {
// If we only saw one error in the first bit of the byte of the frame, then it's possible
// we guessed wrongly about the value of the bit. We may be able to correct it by guessing
// the other way.
//
// We guessed a '1' at bit 7, which is the DF length bit == 112 Bits.
// Inverting bit 7 will change the message type from a long to a short.
// Invert the bit, cross your fingers and carry on.
msglen = MODES_SHORT_MSG_BITS;
msg[0] ^= theErrs; errorsTy = 0;
errors = errors56; // revert to the number of errors prior to bit 56
Modes.stat_DF_Len_Corrected++;
} else if (i < MODES_LONG_MSG_BITS) {
msglen = MODES_SHORT_MSG_BITS;
errors = errors56;
} else {
msglen = MODES_LONG_MSG_BITS;
}
break;
}
}
// Ensure msglen is consistent with the DF type
if (msglen > 0) {
i = modesMessageLenByType(msg[0] >> 3);
if (msglen > i) {msglen = i;}
else if (msglen < i) {msglen = 0;}
}
//
// If we guessed at any of the bits in the DF type field, then look to see if our guess was sensible.
// Do this by looking to see if the original guess results in the DF type being one of the ICAO defined
// message types. If it isn't then toggle the guessed bit and see if this new value is ICAO defined.
// if the new value is ICAO defined, then update it in our message.
if ((msglen) && (errorsTy == 1) && (theErrs & 0x78)) {
// We guessed at one (and only one) of the message type bits. See if our guess is "likely"
// to be correct by comparing the DF against a list of known good DF's
int thisDF = ((theByte = msg[0]) >> 3) & 0x1f;
uint32_t validDFbits = 0x017F0831; // One bit per 32 possible DF's. Set bits 0,4,5,11,16.17.18.19,20,21,22,24
uint32_t thisDFbit = (1 << thisDF);
if (0 == (validDFbits & thisDFbit)) {
// The current DF is not ICAO defined, so is probably an errors.
// Toggle the bit we guessed at and see if the resultant DF is more likely
theByte ^= theErrs;
thisDF = (theByte >> 3) & 0x1f;
thisDFbit = (1 << thisDF);
// if this DF any more likely?
if (validDFbits & thisDFbit) {
// Yep, more likely, so update the main message
msg[0] = theByte;
Modes.stat_DF_Type_Corrected++;
errors--; // decrease the error count so we attempt to use the modified DF.
}
}
}
// snr = 5 * 20log10(sigLevel / noiseLevel) (in units of 0.2dB)
// = 100log10(sigLevel) - 100log10(noiseLevel)
while (sigLevel > 65535 || noiseLevel > 65535) {
sigLevel >>= 1;
noiseLevel >>= 1;
}
snr = Modes.log10lut[sigLevel] - Modes.log10lut[noiseLevel];
// When we reach this point, if error is small, and the signal strength is large enough
// we may have a Mode S message on our hands. It may still be broken and the CRC may not
// be correct, but this can be handled by the next layer.
if ( (msglen)
// && ((2 * snr) > (int) (MODES_MSG_SQUELCH_DB * 10))
&& (errors <= MODES_MSG_ENCODER_ERRS) ) {
// Set initial mm structure details
mm.timestampMsg = Modes.timestampBlk + (j*5) + try_phase;
mm.signalLevel = (snr > 255 ? 255 : (uint8_t)snr);
mm.phase_corrected = (initial_phase != try_phase);
// Decode the received message
decodeModesMessage(&mm, msg);
// Update statistics
if (Modes.stats) {
struct demod_stats *dstats = (mm.phase_corrected ? &Modes.stat_demod_phasecorrected : &Modes.stat_demod);
switch (errors) {
case 0: dstats->demodulated0++; break;
case 1: dstats->demodulated1++; break;
case 2: dstats->demodulated2++; break;
default: dstats->demodulated3++; break;
}
if (mm.crcok) {
dstats->goodcrc++;
dstats->goodcrc_byphase[try_phase%MODES_MAX_PHASE_STATS]++;
} else if (mm.correctedbits > 0) {
dstats->badcrc++;
dstats->fixed++;
if (mm.correctedbits <= MODES_MAX_BITERRORS)
dstats->bit_fix[mm.correctedbits-1] += 1;
} else {
dstats->badcrc++;
}
}
// Skip this message if we are sure it's fine
// (we actually skip to 8 bits before the end of the message,
// because we can often decode two messages that *almost* collide,
// where the preamble of the second message clobbered the last
// few bits of the first message, but the message bits didn't
// overlap)
if (mm.crcok || mm.correctedbits) {
j += (8 + msglen - 8)*12/5 - 1;
}
// Pass data to the next layer
useModesMessage(&mm);
// Only try with different phases if we mostly demodulated OK,
// but the CRC failed. This seems to catch most of the cases
// where trying different phases actually helps, and is much
// cheaper than trying it on every single candidate that passes
// peak detection
if (Modes.phase_enhance && !mm.crcok && !mm.correctedbits) {
if (try_phase == initial_phase)
++Modes.stat_out_of_phase;
try_phase++;
if (try_phase == 9)
try_phase = 4;
if (try_phase != initial_phase)
goto retry;
}
}
}
}

38
demod_2400.h Normal file
View file

@ -0,0 +1,38 @@
// dump1090, a Mode S messages decoder for RTLSDR devices.
//
// Copyright (C) 2014,2015 Oliver Jowett <oliver@mutability.co.uk>
//
// 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.
//
#ifndef DUMP1090_DEMOD_2400_H
#define DUMP1090_DEMOD_2400_H
#include <stdint.h>
void demodulate2400(uint16_t *m, uint32_t mlen);
#endif

2
dump1090.c
View file

@ -1039,7 +1039,7 @@ int main(int argc, char **argv) {
clock_gettime(CLOCK_THREAD_CPUTIME_ID, &cpu_start_time);
if (Modes.oversample)
detectModeS_oversample(Modes.magnitude, MODES_ASYNC_BUF_SAMPLES);
demodulate2400(Modes.magnitude, MODES_ASYNC_BUF_SAMPLES);
else
demodulate2000(Modes.magnitude, MODES_ASYNC_BUF_SAMPLES);

1
dump1090.h
View file

@ -73,6 +73,7 @@
#include "anet.h"
#include "crc.h"
#include "demod_2000.h"
#include "demod_2400.h"
// ============================= #defines ===============================
//

499
mode_s.c
View file

@ -903,505 +903,6 @@ void computeMagnitudeVector(uint16_t *p) {
}
}
// 2.4MHz sampling rate version
//
// When sampling at 2.4MHz we have exactly 6 samples per 5 symbols.
// Each symbol is 500ns wide, each sample is 416.7ns wide
//
// We maintain a phase offset that is expressed in units of 1/5 of a sample i.e. 1/6 of a symbol, 83.333ns
// Each symbol we process advances the phase offset by 6 i.e. 6/5 of a sample, 500ns
//
// The correlation functions below correlate a 1-0 pair of symbols (i.e. manchester encoded 1 bit)
// starting at the given sample, and assuming that the symbol starts at a fixed 0-5 phase offset within
// m[0]. They return a correlation value, generally interpreted as >0 = 1 bit, <0 = 0 bit
// TODO check if there are better (or more balanced) correlation functions to use here
// nb: the correlation functions sum to zero, so we do not need to adjust for the DC offset in the input signal
// (adding any constant value to all of m[0..3] does not change the result)
static inline int slice_phase0(uint16_t *m) {
return 5 * m[0] - 3 * m[1] - 2 * m[2];
}
static inline int slice_phase1(uint16_t *m) {
return 4 * m[0] - m[1] - 3 * m[2];
}
static inline int slice_phase2(uint16_t *m) {
return 3 * m[0] + m[1] - 4 * m[2];
}
static inline int slice_phase3(uint16_t *m) {
return 2 * m[0] + 3 * m[1] - 5 * m[2];
}
static inline int slice_phase4(uint16_t *m) {
return m[0] + 5 * m[1] - 5 * m[2] - m[3];
}
static inline int correlate_phase0(uint16_t *m) {
return slice_phase0(m) * 26;
}
static inline int correlate_phase1(uint16_t *m) {
return slice_phase1(m) * 38;
}
static inline int correlate_phase2(uint16_t *m) {
return slice_phase2(m) * 38;
}
static inline int correlate_phase3(uint16_t *m) {
return slice_phase3(m) * 26;
}
static inline int correlate_phase4(uint16_t *m) {
return slice_phase4(m) * 19;
}
//
// These functions work out the correlation quality for the 10 symbols (5 bits) starting at m[0] + given phase offset.
// This is used to find the right phase offset to use for decoding.
//
static inline int correlate_check_0(uint16_t *m) {
return
abs(correlate_phase0(&m[0])) +
abs(correlate_phase2(&m[2])) +
abs(correlate_phase4(&m[4])) +
abs(correlate_phase1(&m[7])) +
abs(correlate_phase3(&m[9]));
}
static inline int correlate_check_1(uint16_t *m) {
return
abs(correlate_phase1(&m[0])) +
abs(correlate_phase3(&m[2])) +
abs(correlate_phase0(&m[5])) +
abs(correlate_phase2(&m[7])) +
abs(correlate_phase4(&m[9]));
}
static inline int correlate_check_2(uint16_t *m) {
return
abs(correlate_phase2(&m[0])) +
abs(correlate_phase4(&m[2])) +
abs(correlate_phase1(&m[5])) +
abs(correlate_phase3(&m[7])) +
abs(correlate_phase0(&m[10]));
}
static inline int correlate_check_3(uint16_t *m) {
return
abs(correlate_phase3(&m[0])) +
abs(correlate_phase0(&m[3])) +
abs(correlate_phase2(&m[5])) +
abs(correlate_phase4(&m[7])) +
abs(correlate_phase1(&m[10]));
}
static inline int correlate_check_4(uint16_t *m) {
return
abs(correlate_phase4(&m[0])) +
abs(correlate_phase1(&m[3])) +
abs(correlate_phase3(&m[5])) +
abs(correlate_phase0(&m[8])) +
abs(correlate_phase2(&m[10]));
}
// Work out the best phase offset to use for the given message.
static int best_phase(uint16_t *m) {
int test;
int best = -1;
int bestval = (m[0] + m[1] + m[2] + m[3] + m[4] + m[5]); // minimum correlation quality we will accept
// empirical testing suggests that 4..8 is the best range to test for here
// (testing a wider range runs the danger of picking the wrong phase for
// a message that would otherwise be successfully decoded - the correlation
// functions can match well with a one symbol / half bit offset)
// this is consistent with the peak detection which should produce
// the first data symbol with phase offset 4..8
//test = correlate_check_2(&m[0]);
//if (test > bestval) { bestval = test; best = 2; }
//test = correlate_check_3(&m[0]);
//if (test > bestval) { bestval = test; best = 3; }
test = correlate_check_4(&m[0]);
if (test > bestval) { bestval = test; best = 4; }
test = correlate_check_0(&m[1]);
if (test > bestval) { bestval = test; best = 5; }
test = correlate_check_1(&m[1]);
if (test > bestval) { bestval = test; best = 6; }
test = correlate_check_2(&m[1]);
if (test > bestval) { bestval = test; best = 7; }
test = correlate_check_3(&m[1]);
if (test > bestval) { bestval = test; best = 8; }
//test = correlate_check_4(&m[1]);
//if (test > bestval) { bestval = test; best = 9; }
return best;
}
//
//=========================================================================
//
// 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_oversample(uint16_t *m, uint32_t mlen) {
struct modesMessage mm;
unsigned char msg[MODES_LONG_MSG_BYTES], *pMsg;
uint32_t j;
memset(&mm, 0, sizeof(mm));
for (j = 0; j < mlen; j++) {
uint16_t *preamble = &m[j];
int high, i, initial_phase, phase, errors, errors56, errorsTy;
int msglen, scanlen;
uint16_t *pPtr;
uint8_t theByte, theErrs;
uint32_t sigLevel, noiseLevel;
uint16_t snr;
int try_phase;
// Look for a message starting at around sample 0 with phase offset 3..7
// Ideal sample values for preambles with different phase
// Xn is the first data symbol with phase offset N
//
// sample#: 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0
// phase 3: 2/4\0/5\1 0 0 0 0/5\1/3 3\0 0 0 0 0 0 X4
// phase 4: 1/5\0/4\2 0 0 0 0/4\2 2/4\0 0 0 0 0 0 0 X0
// phase 5: 0/5\1/3 3\0 0 0 0/3 3\1/5\0 0 0 0 0 0 0 X1
// phase 6: 0/4\2 2/4\0 0 0 0 2/4\0/5\1 0 0 0 0 0 0 X2
// phase 7: 0/3 3\1/5\0 0 0 0 1/5\0/4\2 0 0 0 0 0 0 X3
//
// quick check: we must have a rising edge 0->1 and a falling edge 12->13
if (! (preamble[0] < preamble[1] && preamble[12] > preamble[13]) )
continue;
if (preamble[1] > preamble[2] && // 1
preamble[2] < preamble[3] && preamble[3] > preamble[4] && // 3
preamble[8] < preamble[9] && preamble[9] > preamble[10] && // 9
preamble[10] < preamble[11]) { // 11-12
// peaks at 1,3,9,11-12: phase 3
high = (preamble[1] + preamble[3] + preamble[9] + preamble[11] + preamble[12]) / 4;
sigLevel = preamble[1] + preamble[3] + preamble[9];
noiseLevel = preamble[5] + preamble[6] + preamble[7];
} else if (preamble[1] > preamble[2] && // 1
preamble[2] < preamble[3] && preamble[3] > preamble[4] && // 3
preamble[8] < preamble[9] && preamble[9] > preamble[10] && // 9
preamble[11] < preamble[12]) { // 12
// peaks at 1,3,9,12: phase 4
high = (preamble[1] + preamble[3] + preamble[9] + preamble[12]) / 4;
sigLevel = preamble[1] + preamble[3] + preamble[9] + preamble[12];
noiseLevel = preamble[5] + preamble[6] + preamble[7] + preamble[8];
} else if (preamble[1] > preamble[2] && // 1
preamble[2] < preamble[3] && preamble[4] > preamble[5] && // 3-4
preamble[8] < preamble[9] && preamble[10] > preamble[11] && // 9-10
preamble[11] < preamble[12]) { // 12
// peaks at 1,3-4,9-10,12: phase 5
high = (preamble[1] + preamble[3] + preamble[4] + preamble[9] + preamble[10] + preamble[12]) / 4;
sigLevel = preamble[1] + preamble[12];
noiseLevel = preamble[6] + preamble[7];
} else if (preamble[1] > preamble[2] && // 1
preamble[3] < preamble[4] && preamble[4] > preamble[5] && // 4
preamble[9] < preamble[10] && preamble[10] > preamble[11] && // 10
preamble[11] < preamble[12]) { // 12
// peaks at 1,4,10,12: phase 6
high = (preamble[1] + preamble[4] + preamble[10] + preamble[12]) / 4;
sigLevel = preamble[1] + preamble[4] + preamble[10] + preamble[12];
noiseLevel = preamble[5] + preamble[6] + preamble[7] + preamble[8];
} else if (preamble[2] > preamble[3] && // 1-2
preamble[3] < preamble[4] && preamble[4] > preamble[5] && // 4
preamble[9] < preamble[10] && preamble[10] > preamble[11] && // 10
preamble[11] < preamble[12]) { // 12
// peaks at 1-2,4,10,12: phase 7
high = (preamble[1] + preamble[2] + preamble[4] + preamble[10] + preamble[12]) / 4;
sigLevel = preamble[4] + preamble[10] + preamble[12];
noiseLevel = preamble[6] + preamble[7] + preamble[8];
} else {
// no suitable peaks
continue;
}
// Check for enough signal
if (sigLevel * 2 < 3 * noiseLevel) // about 3.5dB SNR
continue;
// Check that the "quiet" bits 6,7,15,16,17 are actually quiet
if (preamble[5] >= high ||
preamble[6] >= high ||
preamble[7] >= high ||
preamble[8] >= high ||
preamble[14] >= high ||
preamble[15] >= high ||
preamble[16] >= high ||
preamble[17] >= high ||
preamble[18] >= high) {
++Modes.stat_preamble_not_quiet;
continue;
}
// Crosscorrelate against the first few bits to find a likely phase offset
initial_phase = best_phase(&preamble[19]);
if (initial_phase < 0) {
++Modes.stat_preamble_no_correlation;
continue; // nothing satisfactory
}
Modes.stat_valid_preamble++;
Modes.stat_preamble_phase[initial_phase%MODES_MAX_PHASE_STATS]++;
try_phase = initial_phase;
retry:
// Rather than clear the whole mm structure, just clear the parts which are required. The clear
// is required for every possible preamble, and we don't want to be memset-ing the whole
// modesMessage structure if we don't have to..
mm.bFlags =
mm.crcok =
mm.correctedbits = 0;
// Decode all the next 112 bits, regardless of the actual message
// size. We'll check the actual message type later
pMsg = &msg[0];
pPtr = &m[j+19] + (try_phase/5);
phase = try_phase % 5;
theByte = 0;
theErrs = 0; errorsTy = 0;
errors = 0; errors56 = 0;
msglen = scanlen = MODES_LONG_MSG_BITS;
for (i = 0; i < scanlen; i++) {
int test;
switch (phase) {
case 0:
test = slice_phase0(pPtr);
phase = 2;
pPtr += 2;
break;
case 1:
test = slice_phase1(pPtr);
phase = 3;
pPtr += 2;
break;
case 2:
test = slice_phase2(pPtr);
phase = 4;
pPtr += 2;
break;
case 3:
test = slice_phase3(pPtr);
phase = 0;
pPtr += 3;
break;
case 4:
test = slice_phase4(pPtr);
// A phase-4 bit exactly straddles a sample boundary.
// Here's what a 1-0 bit with phase 4 looks like:
//
// |SYM 1|
// xxx| | |xxx
// |SYM 2|
//
// 012340123401234012340 <-- sample phase
// | 0 | 1 | 2 | 3 | <-- sample boundaries
//
// Samples 1 and 2 only have power from symbols 1 and 2.
// So we can use this to extract signal/noise values
// as one of the two symbols is high (signal) and the
// other is low (noise)
//
// This also gives us an equal number of signal and noise
// samples, which is convenient. Using the first half of
// a phase 0 bit, or the second half of a phase 3 bit, would
// also work, but we have no guarantees about how many signal
// or noise bits we'd see in those phases.
if (test < 0) { // 0 1
noiseLevel += pPtr[1];
sigLevel += pPtr[2];
} else { // 1 0
sigLevel += pPtr[1];
noiseLevel += pPtr[2];
}
phase = 1;
pPtr += 3;
break;
default:
test = 0;
break;
}
if (test > 0)
theByte |= 1;
/* else if (test < 0) theByte |= 0; */
else if (test == 0) {
if (i >= MODES_SHORT_MSG_BITS) { // poor correlation, and we're in the long part of a frame
errors++;
} else if (i >= 5) { // poor correlation, and we're in the short part of a frame
scanlen = MODES_LONG_MSG_BITS;
errors56 = ++errors;
} else if (i) { // poor correlation, and we're in the message type part of a frame
errorsTy = errors56 = ++errors;
theErrs |= 1;
} else { // poor correlation, and we're in the first bit of the message type part of a frame
errorsTy = errors56 = ++errors;
theErrs |= 1;
}
}
if ((i & 7) == 7)
*pMsg++ = theByte;
theByte = theByte << 1;
if (i < 7)
{theErrs = theErrs << 1;}
// If we've exceeded the permissible number of encoding errors, abandon ship now
if (errors > MODES_MSG_ENCODER_ERRS) {
if (i < MODES_SHORT_MSG_BITS) {
msglen = 0;
} else if ((errorsTy == 1) && (theErrs == 0x80)) {
// If we only saw one error in the first bit of the byte of the frame, then it's possible
// we guessed wrongly about the value of the bit. We may be able to correct it by guessing
// the other way.
//
// We guessed a '1' at bit 7, which is the DF length bit == 112 Bits.
// Inverting bit 7 will change the message type from a long to a short.
// Invert the bit, cross your fingers and carry on.
msglen = MODES_SHORT_MSG_BITS;
msg[0] ^= theErrs; errorsTy = 0;
errors = errors56; // revert to the number of errors prior to bit 56
Modes.stat_DF_Len_Corrected++;
} else if (i < MODES_LONG_MSG_BITS) {
msglen = MODES_SHORT_MSG_BITS;
errors = errors56;
} else {
msglen = MODES_LONG_MSG_BITS;
}
break;
}
}
// Ensure msglen is consistent with the DF type
if (msglen > 0) {
i = modesMessageLenByType(msg[0] >> 3);
if (msglen > i) {msglen = i;}
else if (msglen < i) {msglen = 0;}
}
//
// If we guessed at any of the bits in the DF type field, then look to see if our guess was sensible.
// Do this by looking to see if the original guess results in the DF type being one of the ICAO defined
// message types. If it isn't then toggle the guessed bit and see if this new value is ICAO defined.
// if the new value is ICAO defined, then update it in our message.
if ((msglen) && (errorsTy == 1) && (theErrs & 0x78)) {
// We guessed at one (and only one) of the message type bits. See if our guess is "likely"
// to be correct by comparing the DF against a list of known good DF's
int thisDF = ((theByte = msg[0]) >> 3) & 0x1f;
uint32_t validDFbits = 0x017F0831; // One bit per 32 possible DF's. Set bits 0,4,5,11,16.17.18.19,20,21,22,24
uint32_t thisDFbit = (1 << thisDF);
if (0 == (validDFbits & thisDFbit)) {
// The current DF is not ICAO defined, so is probably an errors.
// Toggle the bit we guessed at and see if the resultant DF is more likely
theByte ^= theErrs;
thisDF = (theByte >> 3) & 0x1f;
thisDFbit = (1 << thisDF);
// if this DF any more likely?
if (validDFbits & thisDFbit) {
// Yep, more likely, so update the main message
msg[0] = theByte;
Modes.stat_DF_Type_Corrected++;
errors--; // decrease the error count so we attempt to use the modified DF.
}
}
}
// snr = 5 * 20log10(sigLevel / noiseLevel) (in units of 0.2dB)
// = 100log10(sigLevel) - 100log10(noiseLevel)
while (sigLevel > 65535 || noiseLevel > 65535) {
sigLevel >>= 1;
noiseLevel >>= 1;
}
snr = Modes.log10lut[sigLevel] - Modes.log10lut[noiseLevel];
// When we reach this point, if error is small, and the signal strength is large enough
// we may have a Mode S message on our hands. It may still be broken and the CRC may not
// be correct, but this can be handled by the next layer.
if ( (msglen)
// && ((2 * snr) > (int) (MODES_MSG_SQUELCH_DB * 10))
&& (errors <= MODES_MSG_ENCODER_ERRS) ) {
// Set initial mm structure details
mm.timestampMsg = Modes.timestampBlk + (j*5) + try_phase;
mm.signalLevel = (snr > 255 ? 255 : (uint8_t)snr);
mm.phase_corrected = (initial_phase != try_phase);
// Decode the received message
decodeModesMessage(&mm, msg);
// Update statistics
if (Modes.stats) {
struct demod_stats *dstats = (mm.phase_corrected ? &Modes.stat_demod_phasecorrected : &Modes.stat_demod);
switch (errors) {
case 0: dstats->demodulated0++; break;
case 1: dstats->demodulated1++; break;
case 2: dstats->demodulated2++; break;
default: dstats->demodulated3++; break;
}
if (mm.crcok) {
dstats->goodcrc++;
dstats->goodcrc_byphase[try_phase%MODES_MAX_PHASE_STATS]++;
} else if (mm.correctedbits > 0) {
dstats->badcrc++;
dstats->fixed++;
if (mm.correctedbits <= MODES_MAX_BITERRORS)
dstats->bit_fix[mm.correctedbits-1] += 1;
} else {
dstats->badcrc++;
}
}
// Skip this message if we are sure it's fine
// (we actually skip to 8 bits before the end of the message,
// because we can often decode two messages that *almost* collide,
// where the preamble of the second message clobbered the last
// few bits of the first message, but the message bits didn't
// overlap)
if (mm.crcok || mm.correctedbits) {
j += (8 + msglen - 8)*12/5 - 1;
}
// Pass data to the next layer
useModesMessage(&mm);
// Only try with different phases if we mostly demodulated OK,
// but the CRC failed. This seems to catch most of the cases
// where trying different phases actually helps, and is much
// cheaper than trying it on every single candidate that passes
// peak detection
if (Modes.phase_enhance && !mm.crcok && !mm.correctedbits) {
if (try_phase == initial_phase)
++Modes.stat_out_of_phase;
try_phase++;
if (try_phase == 9)
try_phase = 4;
if (try_phase != initial_phase)
goto retry;
}
}
}
}
//
//=========================================================================
//