diff --git a/Makefile b/Makefile index 205ff0f..9265ac5 100644 --- a/Makefile +++ b/Makefile @@ -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 diff --git a/demod_2400.c b/demod_2400.c new file mode 100644 index 0000000..20c4896 --- /dev/null +++ b/demod_2400.c @@ -0,0 +1,532 @@ +// dump1090, a Mode S messages decoder for RTLSDR devices. +// +// Copyright (C) 2014,2015 Oliver Jowett +// +// 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; + } + } + } +} + diff --git a/demod_2400.h b/demod_2400.h new file mode 100644 index 0000000..4de16a7 --- /dev/null +++ b/demod_2400.h @@ -0,0 +1,38 @@ +// dump1090, a Mode S messages decoder for RTLSDR devices. +// +// Copyright (C) 2014,2015 Oliver Jowett +// +// 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 + +void demodulate2400(uint16_t *m, uint32_t mlen); + +#endif diff --git a/dump1090.c b/dump1090.c index 8ac527e..bf98468 100644 --- a/dump1090.c +++ b/dump1090.c @@ -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); diff --git a/dump1090.h b/dump1090.h index 7892d98..84cc5d5 100644 --- a/dump1090.h +++ b/dump1090.h @@ -73,6 +73,7 @@ #include "anet.h" #include "crc.h" #include "demod_2000.h" +#include "demod_2400.h" // ============================= #defines =============================== // diff --git a/mode_s.c b/mode_s.c index 7e0da53..bbd596d 100644 --- a/mode_s.c +++ b/mode_s.c @@ -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; - } - } - } -} - // //========================================================================= //