dump1090/demod_2400.c
Oliver Jowett 4e177c2d64 Store computed reception time in the message struct so we don't rely on
the message being emitted immediately.

Fix computation of reception time so it's more sensible (the block timestamp
is some time after reception of the _end_ of the block, not the start) - this
means that message-emission times are always later than message-reception
times in SBS output, which is a bit more sensible.

Use clock_gettime in preference to ftime.
2015-02-08 17:46:01 +00:00

495 lines
20 KiB
C

// Part of dump1090, a Mode S message decoder for RTLSDR devices.
//
// demod_2400.c: 2.4MHz Mode S demodulator.
//
// Copyright (c) 2014,2015 Oliver Jowett <oliver@mutability.co.uk>
//
// This file is free software: you may copy, redistribute and/or modify it
// under the terms of the GNU General Public License as published by the
// Free Software Foundation, either version 2 of the License, or (at your
// option) any later version.
//
// This file is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "dump1090.h"
//
// Measuring the noise power is actually surprisingly expensive on an ARM -
// it increases the CPU use of the demodulator by 1/3. So it's off by default.
// You can turn it back on here:
#undef MEASURE_NOISE
// 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_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; }
return best;
}
//
// Given 'mlen' magnitude samples in 'm', sampled at 2.4MHz,
// try to demodulate some Mode S messages.
//
void demodulate2400(uint16_t *m, uint32_t mlen) {
struct modesMessage mm;
unsigned char msg1[MODES_LONG_MSG_BYTES], msg2[MODES_LONG_MSG_BYTES], *msg;
uint32_t j;
#ifdef MEASURE_NOISE
uint32_t last_message_end = 0;
#endif
unsigned char *bestmsg;
int bestscore, bestphase;
#ifdef MEASURE_NOISE
// noise floor:
uint32_t noise_power_count = 0;
uint64_t noise_power_sum = 0;
#endif
memset(&mm, 0, sizeof(mm));
msg = msg1;
for (j = 0; j < mlen; j++) {
uint16_t *preamble = &m[j];
int high;
uint32_t base_signal, base_noise;
int initial_phase, first_phase, last_phase, try_phase;
int msglen;
#ifdef MEASURE_NOISE
// update noise for all samples that aren't part of a message
// (we don't know if m[j] is or not, yet, so work one sample
// in arrears)
if (j > last_message_end+1) {
// There seems to be a weird compiler bug I'm hitting here..
// if you compute the square directly, it occasionally gets mangled.
uint64_t s = TRUE_AMPLITUDE(m[j-1]);
noise_power_sum += s * s;
noise_power_count++;
}
#endif
// 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;
base_signal = preamble[1] + preamble[3] + preamble[9];
base_noise = 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;
base_signal = preamble[1] + preamble[3] + preamble[9] + preamble[12];
base_noise = 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;
base_signal = preamble[1] + preamble[12];
base_noise = 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;
base_signal = preamble[1] + preamble[4] + preamble[10] + preamble[12];
base_noise = 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;
base_signal = preamble[4] + preamble[10] + preamble[12];
base_noise = preamble[6] + preamble[7] + preamble[8];
} else {
// no suitable peaks
continue;
}
// Check for enough signal
if (base_signal * 2 < 3 * base_noise) // 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) {
continue;
}
if (Modes.phase_enhance) {
first_phase = 4;
last_phase = 8; // try all phases
} else {
// Crosscorrelate against the first few bits to find a likely phase offset
initial_phase = best_phase(&preamble[19]);
if (initial_phase < 0) {
continue; // nothing satisfactory
}
first_phase = last_phase = initial_phase; // try only the phase we think it is
}
Modes.stats_current.demod_preambles++;
bestmsg = NULL; bestscore = -2; bestphase = -1;
for (try_phase = first_phase; try_phase <= last_phase; ++try_phase) {
uint16_t *pPtr;
int phase, i, score, bytelen;
// Decode all the next 112 bits, regardless of the actual message
// size. We'll check the actual message type later
pPtr = &m[j+19] + (try_phase/5);
phase = try_phase % 5;
bytelen = MODES_LONG_MSG_BYTES;
for (i = 0; i < bytelen; ++i) {
uint8_t theByte = 0;
switch (phase) {
case 0:
theByte =
(slice_phase0(pPtr) > 0 ? 0x80 : 0) |
(slice_phase2(pPtr+2) > 0 ? 0x40 : 0) |
(slice_phase4(pPtr+4) > 0 ? 0x20 : 0) |
(slice_phase1(pPtr+7) > 0 ? 0x10 : 0) |
(slice_phase3(pPtr+9) > 0 ? 0x08 : 0) |
(slice_phase0(pPtr+12) > 0 ? 0x04 : 0) |
(slice_phase2(pPtr+14) > 0 ? 0x02 : 0) |
(slice_phase4(pPtr+16) > 0 ? 0x01 : 0);
phase = 1;
pPtr += 19;
break;
case 1:
theByte =
(slice_phase1(pPtr) > 0 ? 0x80 : 0) |
(slice_phase3(pPtr+2) > 0 ? 0x40 : 0) |
(slice_phase0(pPtr+5) > 0 ? 0x20 : 0) |
(slice_phase2(pPtr+7) > 0 ? 0x10 : 0) |
(slice_phase4(pPtr+9) > 0 ? 0x08 : 0) |
(slice_phase1(pPtr+12) > 0 ? 0x04 : 0) |
(slice_phase3(pPtr+14) > 0 ? 0x02 : 0) |
(slice_phase0(pPtr+17) > 0 ? 0x01 : 0);
phase = 2;
pPtr += 19;
break;
case 2:
theByte =
(slice_phase2(pPtr) > 0 ? 0x80 : 0) |
(slice_phase4(pPtr+2) > 0 ? 0x40 : 0) |
(slice_phase1(pPtr+5) > 0 ? 0x20 : 0) |
(slice_phase3(pPtr+7) > 0 ? 0x10 : 0) |
(slice_phase0(pPtr+10) > 0 ? 0x08 : 0) |
(slice_phase2(pPtr+12) > 0 ? 0x04 : 0) |
(slice_phase4(pPtr+14) > 0 ? 0x02 : 0) |
(slice_phase1(pPtr+17) > 0 ? 0x01 : 0);
phase = 3;
pPtr += 19;
break;
case 3:
theByte =
(slice_phase3(pPtr) > 0 ? 0x80 : 0) |
(slice_phase0(pPtr+3) > 0 ? 0x40 : 0) |
(slice_phase2(pPtr+5) > 0 ? 0x20 : 0) |
(slice_phase4(pPtr+7) > 0 ? 0x10 : 0) |
(slice_phase1(pPtr+10) > 0 ? 0x08 : 0) |
(slice_phase3(pPtr+12) > 0 ? 0x04 : 0) |
(slice_phase0(pPtr+15) > 0 ? 0x02 : 0) |
(slice_phase2(pPtr+17) > 0 ? 0x01 : 0);
phase = 4;
pPtr += 19;
break;
case 4:
theByte =
(slice_phase4(pPtr) > 0 ? 0x80 : 0) |
(slice_phase1(pPtr+3) > 0 ? 0x40 : 0) |
(slice_phase3(pPtr+5) > 0 ? 0x20 : 0) |
(slice_phase0(pPtr+8) > 0 ? 0x10 : 0) |
(slice_phase2(pPtr+10) > 0 ? 0x08 : 0) |
(slice_phase4(pPtr+12) > 0 ? 0x04 : 0) |
(slice_phase1(pPtr+15) > 0 ? 0x02 : 0) |
(slice_phase3(pPtr+17) > 0 ? 0x01 : 0);
phase = 0;
pPtr += 20;
break;
}
msg[i] = theByte;
if (i == 0) {
switch (msg[0] >> 3) {
case 0: case 4: case 5: case 11:
bytelen = MODES_SHORT_MSG_BYTES; break;
case 16: case 17: case 18: case 20: case 21: case 24:
break;
default:
bytelen = 1; // unknown DF, give up immediately
break;
}
}
}
// Score the mode S message and see if it's any good.
score = scoreModesMessage(msg, i*8);
if (score > bestscore) {
// new high score!
bestmsg = msg;
bestscore = score;
bestphase = try_phase;
// swap to using the other buffer so we don't clobber our demodulated data
// (if we find a better result then we'll swap back, but that's OK because
// we no longer need this copy if we found a better one)
msg = (msg == msg1) ? msg2 : msg1;
}
}
// Do we have a candidate?
if (bestscore < 0) {
if (bestscore == -1)
Modes.stats_current.demod_rejected_unknown_icao++;
else
Modes.stats_current.demod_rejected_bad++;
continue; // nope.
}
msglen = modesMessageLenByType(bestmsg[0] >> 3);
// Set initial mm structure details
mm.timestampMsg = Modes.timestampBlk + (j*5) + bestphase;
// compute message receive time as block-start-time + difference in the 12MHz clock
mm.sysTimestampMsg = Modes.stSystemTimeBlk; // end of block time
mm.sysTimestampMsg.tv_nsec -= receiveclock_ns_elapsed(mm.timestampMsg, Modes.timestampBlk + MODES_ASYNC_BUF_SAMPLES * 5); // time until end of block
normalize_timespec(&mm.sysTimestampMsg);
mm.score = bestscore;
mm.bFlags = mm.correctedbits = 0;
// measure signal power
{
uint64_t signal_power_sum = 0;
double signal_power;
int signal_len = msglen*12/5 + 1;
int k;
for (k = 0; k < signal_len; ++k) {
uint64_t s = TRUE_AMPLITUDE(m[j+19+k]);
signal_power_sum += s * s;
}
mm.signalLevel = signal_power = signal_power_sum / MAX_POWER / signal_len;
Modes.stats_current.signal_power_sum += signal_power;
Modes.stats_current.signal_power_count ++;
if (signal_power > Modes.stats_current.peak_signal_power)
Modes.stats_current.peak_signal_power = signal_power;
if (signal_power > 0.50119)
Modes.stats_current.strong_signal_count++; // signal power above -3dBFS
}
// Decode the received message
{
int result = decodeModesMessage(&mm, bestmsg);
if (result < 0) {
if (result == -1)
Modes.stats_current.demod_rejected_unknown_icao++;
else
Modes.stats_current.demod_rejected_bad++;
continue;
} else {
Modes.stats_current.demod_accepted[mm.correctedbits]++;
}
}
// Skip over the message:
// (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)
#ifdef MEASURE_NOISE
last_message_end = j + (8 + msglen)*12/5;
#endif
j += (8 + msglen - 8)*12/5 - 1;
// Pass data to the next layer
useModesMessage(&mm);
}
#ifdef MEASURE_NOISE
Modes.stats_current.noise_power_sum += (noise_power_sum / MAX_POWER / noise_power_count);
Modes.stats_current.noise_power_count ++;
#endif
}