15ea5ba3da
Magnitude conversion now happens immediately when sample data is received, so there is no risk of newly received data clobbering old data under CPU overload.
610 lines
25 KiB
C
610 lines
25 KiB
C
// Part of dump1090, a Mode S message decoder for RTLSDR devices.
|
|
//
|
|
// demod_2000.c: 2MHz 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/>.
|
|
|
|
// This file incorporates work covered by the following copyright and
|
|
// permission notice:
|
|
//
|
|
// 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 "dump1090.h"
|
|
|
|
// Mode S 2.0MHz demodulator
|
|
|
|
// ============================== 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
|
|
//
|
|
static 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 0x%04X\n", index, buf, magnitude);
|
|
else
|
|
printf("[%.2d] |%-66s 0x%04X\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.
|
|
//
|
|
static 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_SAMPLES)+(MODES_SHORT_MSG_SAMPLES) - 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.
|
|
//
|
|
static void dumpRawMessageJS(char *descr, unsigned char *msg,
|
|
uint16_t *m, uint32_t offset, struct errorinfo *ei)
|
|
{
|
|
int padding = 5; // Show a few samples before the actual start.
|
|
int start = offset - padding;
|
|
int end = offset + (MODES_PREAMBLE_SAMPLES)+(MODES_LONG_MSG_SAMPLES) - 1;
|
|
FILE *fp;
|
|
int j;
|
|
|
|
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, "], ");
|
|
for (j = 0; j < MODES_MAX_BITERRORS; ++j)
|
|
fprintf(fp,"\"fix%d\": %d, ", j, ei->bit[j]);
|
|
fprintf(fp, "\"bits\": %d, \"hex\": \"", 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.
|
|
//
|
|
static void dumpRawMessage(char *descr, unsigned char *msg, uint16_t *m, uint32_t offset) {
|
|
int j;
|
|
int msgtype = msg[0] >> 3;
|
|
struct errorinfo *ei = NULL;
|
|
|
|
if (msgtype == 17) {
|
|
int len = modesMessageLenByType(msgtype);
|
|
uint32_t csum = modesChecksum(msg, len);
|
|
ei = modesChecksumDiagnose(csum, len);
|
|
}
|
|
|
|
if (Modes.debug & MODES_DEBUG_JS) {
|
|
dumpRawMessageJS(descr, msg, m, offset, ei);
|
|
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, ei ? ei->errors : 0);
|
|
dumpMagnitudeVector(m,offset);
|
|
printf("---\n\n");
|
|
}
|
|
|
|
//
|
|
//=========================================================================
|
|
//
|
|
// 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.
|
|
//
|
|
// Note: this function will access pPreamble[-1], so the caller should make sure to
|
|
// call it only if we are not at the start of the current buffer
|
|
//
|
|
static int detectOutOfPhase(uint16_t *pPreamble) {
|
|
if (pPreamble[ 3] > pPreamble[2]/3) return 1;
|
|
if (pPreamble[10] > pPreamble[9]/3) return 1;
|
|
if (pPreamble[ 6] > pPreamble[7]/3) return -1;
|
|
if (pPreamble[-1] > pPreamble[1]/3) return -1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
static uint16_t clamped_scale(uint16_t v, uint16_t scale) {
|
|
uint32_t scaled = (uint32_t)v * scale / 16384;
|
|
if (scaled > 65535) return 65535;
|
|
return (uint16_t) scaled;
|
|
}
|
|
// This function decides whether we are sampling early or late,
|
|
// and by approximately how much, by looking at the energy in
|
|
// preamble bits before and after the expected pulse locations.
|
|
//
|
|
// It then deals with one sample pair at a time, comparing samples
|
|
// to make a decision about the bit value. Based on this decision it
|
|
// modifies the sample value of the *adjacent* sample which will
|
|
// contain some of the energy from the bit we just inspected.
|
|
//
|
|
// pPayload[0] should be the start of the preamble,
|
|
// pPayload[-1 .. MODES_PREAMBLE_SAMPLES + MODES_LONG_MSG_SAMPLES - 1] should be accessible.
|
|
// pPayload[MODES_PREAMBLE_SAMPLES .. MODES_PREAMBLE_SAMPLES + MODES_LONG_MSG_SAMPLES - 1] will be updated.
|
|
static void applyPhaseCorrection(uint16_t *pPayload) {
|
|
int j;
|
|
|
|
// we expect 1 bits at 0, 2, 7, 9
|
|
// and 0 bits at -1, 1, 3, 4, 5, 6, 8, 10, 11, 12, 13, 14
|
|
// use bits -1,6 for early detection (bit 0/7 arrived a little early, our sample period starts after the bit phase so we include some of the next bit)
|
|
// use bits 3,10 for late detection (bit 2/9 arrived a little late, our sample period starts before the bit phase so we include some of the last bit)
|
|
|
|
uint32_t onTime = (pPayload[0] + pPayload[2] + pPayload[7] + pPayload[9]);
|
|
uint32_t early = (pPayload[-1] + pPayload[6]) << 1;
|
|
uint32_t late = (pPayload[3] + pPayload[10]) << 1;
|
|
|
|
if (onTime == 0 && early == 0 && late == 0) {
|
|
// Blah, can't do anything with this, avoid a divide-by-zero
|
|
return;
|
|
}
|
|
|
|
if (early > late) {
|
|
// Our sample period starts late and so includes some of the next bit.
|
|
|
|
uint16_t scaleUp = 16384 + 16384 * early / (early + onTime); // 1 + early / (early+onTime)
|
|
uint16_t scaleDown = 16384 - 16384 * early / (early + onTime); // 1 - early / (early+onTime)
|
|
|
|
// trailing bits are 0; final data sample will be a bit low.
|
|
pPayload[MODES_PREAMBLE_SAMPLES + MODES_LONG_MSG_SAMPLES - 1] =
|
|
clamped_scale(pPayload[MODES_PREAMBLE_SAMPLES + MODES_LONG_MSG_SAMPLES - 1], scaleUp);
|
|
for (j = MODES_PREAMBLE_SAMPLES + MODES_LONG_MSG_SAMPLES - 2; j > MODES_PREAMBLE_SAMPLES; j -= 2) {
|
|
if (pPayload[j] > pPayload[j+1]) {
|
|
// x [1 0] y
|
|
// x overlapped with the "1" bit and is slightly high
|
|
pPayload[j-1] = clamped_scale(pPayload[j-1], scaleDown);
|
|
} else {
|
|
// x [0 1] y
|
|
// x overlapped with the "0" bit and is slightly low
|
|
pPayload[j-1] = clamped_scale(pPayload[j-1], scaleUp);
|
|
}
|
|
}
|
|
} else {
|
|
// Our sample period starts early and so includes some of the previous bit.
|
|
|
|
uint16_t scaleUp = 16384 + 16384 * late / (late + onTime); // 1 + late / (late+onTime)
|
|
uint16_t scaleDown = 16384 - 16384 * late / (late + onTime); // 1 - late / (late+onTime)
|
|
|
|
// leading bits are 0; first data sample will be a bit low.
|
|
pPayload[MODES_PREAMBLE_SAMPLES] = clamped_scale(pPayload[MODES_PREAMBLE_SAMPLES], scaleUp);
|
|
for (j = MODES_PREAMBLE_SAMPLES; j < MODES_PREAMBLE_SAMPLES + MODES_LONG_MSG_SAMPLES - 2; j += 2) {
|
|
if (pPayload[j] > pPayload[j+1]) {
|
|
// x [1 0] y
|
|
// y overlapped with the "0" bit and is slightly low
|
|
pPayload[j+2] = clamped_scale(pPayload[j+2], scaleUp);
|
|
} else {
|
|
// x [0 1] y
|
|
// y overlapped with the "1" bit and is slightly high
|
|
pPayload[j+2] = clamped_scale(pPayload[j+2], scaleDown);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//
|
|
//=========================================================================
|
|
//
|
|
// 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 demodulate2000(struct mag_buf *mag) {
|
|
struct modesMessage mm;
|
|
unsigned char msg[MODES_LONG_MSG_BYTES], *pMsg;
|
|
uint16_t aux[MODES_PREAMBLE_SAMPLES+MODES_LONG_MSG_SAMPLES+1];
|
|
uint32_t j;
|
|
int use_correction = 0;
|
|
unsigned mlen = mag->length;
|
|
uint16_t *m = mag->data;
|
|
|
|
memset(&mm, 0, sizeof(mm));
|
|
|
|
// 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; j++) {
|
|
int high, i, errors, errors56, errorsTy;
|
|
uint16_t *pPreamble, *pPayload, *pPtr;
|
|
uint8_t theByte, theErrs;
|
|
int msglen, scanlen;
|
|
uint32_t sigLevel, noiseLevel;
|
|
uint16_t snr;
|
|
int message_ok;
|
|
|
|
pPreamble = &m[j];
|
|
pPayload = &m[j+MODES_PREAMBLE_SAMPLES];
|
|
|
|
// Rather than clear the whole mm structure, just clear the parts which are required. The clear
|
|
// is required for every bit of the input stream, and we don't want to be memset-ing the whole
|
|
// modesMessage structure two million times per second if we don't have to..
|
|
mm.bFlags =
|
|
mm.correctedbits = 0;
|
|
|
|
if (!use_correction) // This is not a re-try with phase correction
|
|
{ // so try to find a new preamble
|
|
|
|
if (Modes.mode_ac)
|
|
{
|
|
int ModeA = detectModeA(pPreamble, &mm);
|
|
|
|
if (ModeA) // We have found a valid ModeA/C in the data
|
|
{
|
|
mm.timestampMsg = mag->sampleTimestamp + ((j+1) * 6);
|
|
|
|
// compute message receive time as block-start-time + difference in the 12MHz clock
|
|
mm.sysTimestampMsg = mag->sysTimestamp; // start of block time
|
|
mm.sysTimestampMsg.tv_nsec += receiveclock_ns_elapsed(mag->sampleTimestamp, mm.timestampMsg);
|
|
normalize_timespec(&mm.sysTimestampMsg);
|
|
|
|
// Decode the received message
|
|
decodeModeAMessage(&mm, ModeA);
|
|
|
|
// Pass data to the next layer
|
|
useModesMessage(&mm);
|
|
|
|
j += MODEAC_MSG_SAMPLES;
|
|
Modes.stats_current.demod_modeac++;
|
|
continue;
|
|
}
|
|
}
|
|
|
|
// 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 (!(pPreamble[0] > pPreamble[1] &&
|
|
pPreamble[1] < pPreamble[2] &&
|
|
pPreamble[2] > pPreamble[3] &&
|
|
pPreamble[3] < pPreamble[0] &&
|
|
pPreamble[4] < pPreamble[0] &&
|
|
pPreamble[5] < pPreamble[0] &&
|
|
pPreamble[6] < pPreamble[0] &&
|
|
pPreamble[7] > pPreamble[8] &&
|
|
pPreamble[8] < pPreamble[9] &&
|
|
pPreamble[9] > pPreamble[6]))
|
|
{
|
|
if (Modes.debug & MODES_DEBUG_NOPREAMBLE &&
|
|
*pPreamble > MODES_DEBUG_NOPREAMBLE_LEVEL)
|
|
dumpRawMessage("Unexpected ratio among first 10 samples", msg, m, j);
|
|
continue;
|
|
}
|
|
|
|
// The samples between the two spikes must be < than the average
|
|
// of the high spikes level. 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 = (pPreamble[0] + pPreamble[2] + pPreamble[7] + pPreamble[9]) / 6;
|
|
if (pPreamble[4] >= high ||
|
|
pPreamble[5] >= high)
|
|
{
|
|
if (Modes.debug & MODES_DEBUG_NOPREAMBLE &&
|
|
*pPreamble > 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 (pPreamble[11] >= high ||
|
|
pPreamble[12] >= high ||
|
|
pPreamble[13] >= high ||
|
|
pPreamble[14] >= high)
|
|
{
|
|
if (Modes.debug & MODES_DEBUG_NOPREAMBLE &&
|
|
*pPreamble > MODES_DEBUG_NOPREAMBLE_LEVEL)
|
|
dumpRawMessage("Too high level in samples between 10 and 15", msg, m, j);
|
|
continue;
|
|
}
|
|
Modes.stats_current.demod_preambles++;
|
|
}
|
|
|
|
else {
|
|
// If the previous attempt with this message failed, retry using
|
|
// magnitude correction
|
|
// Make a copy of the Payload, and phase correct the copy
|
|
memcpy(aux, &pPreamble[-1], sizeof(aux));
|
|
applyPhaseCorrection(&aux[1]);
|
|
pPayload = &aux[1 + MODES_PREAMBLE_SAMPLES];
|
|
// TODO ... apply other kind of corrections
|
|
}
|
|
|
|
// Decode all the next 112 bits, regardless of the actual message
|
|
// size. We'll check the actual message type later
|
|
pMsg = &msg[0];
|
|
pPtr = pPayload;
|
|
theByte = 0;
|
|
theErrs = 0; errorsTy = 0;
|
|
errors = 0; errors56 = 0;
|
|
|
|
// We should have 4 'bits' of 0/1 and 1/0 samples in the preamble,
|
|
// so include these in the signal strength
|
|
sigLevel = pPreamble[0] + pPreamble[2] + pPreamble[7] + pPreamble[9];
|
|
noiseLevel = pPreamble[1] + pPreamble[3] + pPreamble[4] + pPreamble[6] + pPreamble[8];
|
|
|
|
msglen = scanlen = MODES_LONG_MSG_BITS;
|
|
for (i = 0; i < scanlen; i++) {
|
|
uint32_t a = *pPtr++;
|
|
uint32_t b = *pPtr++;
|
|
|
|
if (a > b)
|
|
{theByte |= 1; if (i < 56) { sigLevel += a; noiseLevel += b; }}
|
|
else if (a < b)
|
|
{/*theByte |= 0;*/ if (i < 56) { sigLevel += b; noiseLevel += a; }}
|
|
else {
|
|
if (i < 56) { sigLevel += a; noiseLevel += a; }
|
|
if (i >= MODES_SHORT_MSG_BITS) //(a == b), and we're in the long part of a frame
|
|
{errors++; /*theByte |= 0;*/}
|
|
else if (i >= 5) //(a == b), and we're in the short part of a frame
|
|
{scanlen = MODES_LONG_MSG_BITS; errors56 = ++errors;/*theByte |= 0;*/}
|
|
else if (i) //(a == b), and we're in the message type part of a frame
|
|
{errorsTy = errors56 = ++errors; theErrs |= 1; /*theByte |= 0;*/}
|
|
else //(a == b), and we're in the first bit of the message type part of a frame
|
|
{errorsTy = errors56 = ++errors; theErrs |= 1; theByte |= 1;}
|
|
}
|
|
|
|
if ((i & 7) == 7)
|
|
{*pMsg++ = theByte;}
|
|
else if (i == 4) {
|
|
msglen = modesMessageLenByType(theByte);
|
|
if (errors == 0)
|
|
{scanlen = msglen;}
|
|
}
|
|
|
|
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
|
|
|
|
} 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;
|
|
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) ) {
|
|
int result;
|
|
|
|
// Set initial mm structure details
|
|
mm.timestampMsg = mag->sampleTimestamp + (j*6);
|
|
|
|
// compute message receive time as block-start-time + difference in the 12MHz clock
|
|
mm.sysTimestampMsg = mag->sysTimestamp; // start of block time
|
|
mm.sysTimestampMsg.tv_nsec += receiveclock_ns_elapsed(mag->sampleTimestamp, mm.timestampMsg);
|
|
normalize_timespec(&mm.sysTimestampMsg);
|
|
|
|
mm.signalLevel = (365.0*60 + sigLevel + noiseLevel) * (365.0*60 + sigLevel + noiseLevel) / MAX_POWER / 60 / 60;
|
|
|
|
// Decode the received message
|
|
result = decodeModesMessage(&mm, msg);
|
|
if (result < 0) {
|
|
message_ok = 0;
|
|
if (result == -1)
|
|
Modes.stats_current.demod_rejected_unknown_icao++;
|
|
else
|
|
Modes.stats_current.demod_rejected_bad++;
|
|
} else {
|
|
message_ok = 1;
|
|
Modes.stats_current.demod_accepted[mm.correctedbits]++;
|
|
}
|
|
|
|
// Update statistics
|
|
|
|
// Output debug mode info if needed
|
|
if (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 &&
|
|
(!message_ok || mm.correctedbits > 0))
|
|
dumpRawMessage("Decoded with bad CRC", msg, m, j);
|
|
else if (Modes.debug & MODES_DEBUG_GOODCRC &&
|
|
message_ok &&
|
|
mm.correctedbits == 0)
|
|
dumpRawMessage("Decoded with good CRC", msg, m, j);
|
|
}
|
|
|
|
// Skip this message if we are sure it's fine
|
|
if (message_ok) {
|
|
j += (MODES_PREAMBLE_US+msglen)*2 - 1;
|
|
|
|
// Pass data to the next layer
|
|
useModesMessage(&mm);
|
|
}
|
|
} else {
|
|
message_ok = 0;
|
|
if (Modes.debug & MODES_DEBUG_DEMODERR && use_correction) {
|
|
printf("The following message has %d demod errors\n", errors);
|
|
dumpRawMessage("Demodulated with errors", msg, m, j);
|
|
}
|
|
}
|
|
|
|
// Retry with phase correction if enabled, necessary and possible.
|
|
if (Modes.phase_enhance && (!message_ok || mm.correctedbits > 0) && !use_correction && j && detectOutOfPhase(pPreamble)) {
|
|
use_correction = 1; j--;
|
|
} else {
|
|
use_correction = 0;
|
|
}
|
|
}
|
|
}
|
|
|