kopia lustrzana https://github.com/cyoung/stratux
Formatting.
Christopher Young
rodzic
229df30ff4
commit
a8e8562e5f
|
|
@ -36,7 +36,8 @@ static void read_from_stdin(void);
|
|||
static int process_buffer(uint16_t *phi, int len, uint64_t offset);
|
||||
static int demod_adsb_frame(uint16_t *phi, uint8_t *to, int *rs_errors);
|
||||
static int demod_uplink_frame(uint16_t *phi, uint8_t *to, int *rs_errors);
|
||||
static void demod_frame(uint16_t *phi, uint8_t *frame, int bytes, int16_t center_dphi);
|
||||
static void demod_frame(uint16_t *phi, uint8_t *frame, int bytes,
|
||||
int16_t center_dphi);
|
||||
static void handle_adsb_frame(uint64_t timestamp, uint8_t *frame, int rs);
|
||||
static void handle_uplink_frame(uint64_t timestamp, uint8_t *frame, int rs);
|
||||
|
||||
|
|
@ -47,10 +48,9 @@ static void handle_uplink_frame(uint64_t timestamp, uint8_t *frame, int rs);
|
|||
// relying on signed overflow is theoretically bad. Let's do it properly.
|
||||
|
||||
#ifdef USE_SIGNED_OVERFLOW
|
||||
#define phi_difference(from,to) ((int16_t)((to) - (from)))
|
||||
#define phi_difference(from, to) ((int16_t)((to) - (from)))
|
||||
#else
|
||||
inline int16_t phi_difference(uint16_t from, uint16_t to)
|
||||
{
|
||||
inline int16_t phi_difference(uint16_t from, uint16_t to) {
|
||||
int32_t difference = to - from; // lies in the range -65535 .. +65535
|
||||
if (difference >= 32768) // +32768..+65535
|
||||
return difference - 65536; // -> -32768..-1: always in range
|
||||
|
|
@ -62,8 +62,7 @@ inline int16_t phi_difference(uint16_t from, uint16_t to)
|
|||
#endif
|
||||
|
||||
#ifndef BUILD_LIB
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
int main(int argc, char **argv) {
|
||||
make_atan2_table();
|
||||
init_fec();
|
||||
read_from_stdin();
|
||||
|
|
@ -71,16 +70,15 @@ int main(int argc, char **argv)
|
|||
}
|
||||
#else
|
||||
static CallBack userCB = NULL;
|
||||
void Dump978Init(CallBack cb)
|
||||
{
|
||||
void Dump978Init(CallBack cb) {
|
||||
make_atan2_table();
|
||||
init_fec();
|
||||
userCB = cb;
|
||||
}
|
||||
#endif
|
||||
|
||||
static void dump_raw_message(char updown, uint8_t *data, int len, int rs_errors)
|
||||
{
|
||||
static void dump_raw_message(char updown, uint8_t *data, int len,
|
||||
int rs_errors) {
|
||||
#ifndef BUILD_LIB
|
||||
int i;
|
||||
|
||||
|
|
@ -97,23 +95,22 @@ static void dump_raw_message(char updown, uint8_t *data, int len, int rs_errors)
|
|||
#endif
|
||||
}
|
||||
|
||||
static void handle_adsb_frame(uint64_t timestamp, uint8_t *frame, int rs)
|
||||
{
|
||||
dump_raw_message('-', frame, (frame[0]>>3) == 0 ? SHORT_FRAME_DATA_BYTES : LONG_FRAME_DATA_BYTES, rs);
|
||||
static void handle_adsb_frame(uint64_t timestamp, uint8_t *frame, int rs) {
|
||||
dump_raw_message('-', frame, (frame[0] >> 3) == 0 ? SHORT_FRAME_DATA_BYTES
|
||||
: LONG_FRAME_DATA_BYTES,
|
||||
rs);
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
static void handle_uplink_frame(uint64_t timestamp, uint8_t *frame, int rs)
|
||||
{
|
||||
static void handle_uplink_frame(uint64_t timestamp, uint8_t *frame, int rs) {
|
||||
dump_raw_message('+', frame, UPLINK_FRAME_DATA_BYTES, rs);
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
uint16_t iqphase[65536]; // contains value [0..65536) -> [0, 2*pi)
|
||||
|
||||
void make_atan2_table(void)
|
||||
{
|
||||
unsigned i,q;
|
||||
void make_atan2_table(void) {
|
||||
unsigned i, q;
|
||||
union {
|
||||
uint8_t iq[2];
|
||||
uint16_t iq16;
|
||||
|
|
@ -123,18 +120,20 @@ void make_atan2_table(void)
|
|||
for (q = 0; q < 256; ++q) {
|
||||
double d_i = (i - 127.5);
|
||||
double d_q = (q - 127.5);
|
||||
double ang = atan2(d_q, d_i) + M_PI; // atan2 returns [-pi..pi], normalize to [0..2*pi]
|
||||
double ang = atan2(d_q, d_i) +
|
||||
M_PI; // atan2 returns [-pi..pi], normalize to [0..2*pi]
|
||||
double scaled_ang = round(32768 * ang / M_PI);
|
||||
|
||||
u.iq[0] = i;
|
||||
u.iq[1] = q;
|
||||
iqphase[u.iq16] = (scaled_ang < 0 ? 0 : scaled_ang > 65535 ? 65535 : (uint16_t)scaled_ang);
|
||||
iqphase[u.iq16] =
|
||||
(scaled_ang < 0 ? 0 : scaled_ang > 65535 ? 65535
|
||||
: (uint16_t)scaled_ang);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void convert_to_phi(uint16_t *buffer, int n)
|
||||
{
|
||||
static void convert_to_phi(uint16_t *buffer, int n) {
|
||||
int i;
|
||||
|
||||
for (i = 0; i < n; ++i)
|
||||
|
|
@ -142,33 +141,31 @@ static void convert_to_phi(uint16_t *buffer, int n)
|
|||
}
|
||||
|
||||
#ifndef BUILD_LIB
|
||||
void read_from_stdin(void)
|
||||
{
|
||||
char buffer[65536*2];
|
||||
void read_from_stdin(void) {
|
||||
char buffer[65536 * 2];
|
||||
int n;
|
||||
int used = 0;
|
||||
uint64_t offset = 0;
|
||||
|
||||
while ( (n = read(0, buffer+used, sizeof(buffer)-used)) > 0 ) {
|
||||
while ((n = read(0, buffer + used, sizeof(buffer) - used)) > 0) {
|
||||
int processed;
|
||||
|
||||
convert_to_phi((uint16_t*) (buffer+(used&~1)), ((used&1)+n)/2);
|
||||
convert_to_phi((uint16_t *)(buffer + (used & ~1)), ((used & 1) + n) / 2);
|
||||
|
||||
used += n;
|
||||
processed = process_buffer((uint16_t*) buffer, used/2, offset);
|
||||
processed = process_buffer((uint16_t *)buffer, used / 2, offset);
|
||||
used -= processed * 2;
|
||||
offset += processed;
|
||||
if (used > 0) {
|
||||
memmove(buffer, buffer+processed*2, used);
|
||||
memmove(buffer, buffer + processed * 2, used);
|
||||
}
|
||||
}
|
||||
}
|
||||
#else
|
||||
// #define DEFAULT_SAMPLE_RATE 2048000
|
||||
// #define DEFAULT_BUF_LENGTH (262144) 16*16384
|
||||
static char buffer[65536*2]; // 131072, max received should be 113120
|
||||
int process_data(char *data, int dlen)
|
||||
{
|
||||
static char buffer[65536 * 2]; // 131072, max received should be 113120
|
||||
int process_data(char *data, int dlen) {
|
||||
int n;
|
||||
int processed;
|
||||
int doffset = 0;
|
||||
|
|
@ -176,17 +173,17 @@ int process_data(char *data, int dlen)
|
|||
static uint64_t offset = 0;
|
||||
|
||||
while (dlen > 0) {
|
||||
n = (sizeof(buffer)-used) >= dlen ? dlen : (sizeof(buffer)-used);
|
||||
memcpy(buffer+used, data+doffset, n);
|
||||
n = (sizeof(buffer) - used) >= dlen ? dlen : (sizeof(buffer) - used);
|
||||
memcpy(buffer + used, data + doffset, n);
|
||||
|
||||
convert_to_phi((uint16_t*) (buffer+(used&~1)), ((used&1)+n)/2);
|
||||
convert_to_phi((uint16_t *)(buffer + (used & ~1)), ((used & 1) + n) / 2);
|
||||
|
||||
used += n;
|
||||
processed = process_buffer((uint16_t*)buffer, used/2, offset);
|
||||
processed = process_buffer((uint16_t *)buffer, used / 2, offset);
|
||||
used -= processed * 2;
|
||||
offset += processed;
|
||||
if (used > 0) {
|
||||
memmove(buffer, buffer+processed*2, used);
|
||||
memmove(buffer, buffer + processed * 2, used);
|
||||
}
|
||||
|
||||
doffset += n;
|
||||
|
|
@ -196,10 +193,8 @@ int process_data(char *data, int dlen)
|
|||
}
|
||||
#endif
|
||||
|
||||
|
||||
// Return 1 if word is "equal enough" to expected
|
||||
static inline int sync_word_fuzzy_compare(uint64_t word, uint64_t expected)
|
||||
{
|
||||
static inline int sync_word_fuzzy_compare(uint64_t word, uint64_t expected) {
|
||||
uint64_t diff;
|
||||
|
||||
if (word == expected)
|
||||
|
|
@ -233,22 +228,22 @@ static inline int sync_word_fuzzy_compare(uint64_t word, uint64_t expected)
|
|||
// or we have seen too many set bits.
|
||||
|
||||
// >= 1 bit
|
||||
diff &= (diff-1); // clear lowest set bit
|
||||
diff &= (diff - 1); // clear lowest set bit
|
||||
if (!diff)
|
||||
return 1; // 1 bit error
|
||||
|
||||
// >= 2 bits
|
||||
diff &= (diff-1); // clear lowest set bit
|
||||
diff &= (diff - 1); // clear lowest set bit
|
||||
if (!diff)
|
||||
return 1; // 2 bits error
|
||||
|
||||
// >= 3 bits
|
||||
diff &= (diff-1); // clear lowest set bit
|
||||
diff &= (diff - 1); // clear lowest set bit
|
||||
if (!diff)
|
||||
return 1; // 3 bits error
|
||||
|
||||
// >= 4 bits
|
||||
diff &= (diff-1); // clear lowest set bit
|
||||
diff &= (diff - 1); // clear lowest set bit
|
||||
if (!diff)
|
||||
return 1; // 4 bits error
|
||||
|
||||
|
|
@ -262,8 +257,7 @@ static inline int sync_word_fuzzy_compare(uint64_t word, uint64_t expected)
|
|||
// that matches the sync word 'pattern'. Place the dphi
|
||||
// threshold to use for bit slicing in '*center'. Return 1
|
||||
// if the sync word is OK, 0 on failure
|
||||
int check_sync_word(uint16_t *phi, uint64_t pattern, int16_t *center)
|
||||
{
|
||||
int check_sync_word(uint16_t *phi, uint64_t pattern, int16_t *center) {
|
||||
int i;
|
||||
int32_t dphi_zero_total = 0;
|
||||
int zero_bits = 0;
|
||||
|
|
@ -275,9 +269,9 @@ int check_sync_word(uint16_t *phi, uint64_t pattern, int16_t *center)
|
|||
// take the mean of the two as our central value
|
||||
|
||||
for (i = 0; i < SYNC_BITS; ++i) {
|
||||
int16_t dphi = phi_difference(phi[i*2], phi[i*2+1]);
|
||||
int16_t dphi = phi_difference(phi[i * 2], phi[i * 2 + 1]);
|
||||
|
||||
if (pattern & (1UL << (35-i))) {
|
||||
if (pattern & (1UL << (35 - i))) {
|
||||
++one_bits;
|
||||
dphi_one_total += dphi;
|
||||
} else {
|
||||
|
|
@ -294,9 +288,9 @@ int check_sync_word(uint16_t *phi, uint64_t pattern, int16_t *center)
|
|||
// recheck sync word using our center value
|
||||
error_bits = 0;
|
||||
for (i = 0; i < SYNC_BITS; ++i) {
|
||||
int16_t dphi = phi_difference(phi[i*2], phi[i*2+1]);
|
||||
int16_t dphi = phi_difference(phi[i * 2], phi[i * 2 + 1]);
|
||||
|
||||
if (pattern & (1UL << (35-i))) {
|
||||
if (pattern & (1UL << (35 - i))) {
|
||||
if (dphi < *center)
|
||||
++error_bits;
|
||||
} else {
|
||||
|
|
@ -305,15 +299,15 @@ int check_sync_word(uint16_t *phi, uint64_t pattern, int16_t *center)
|
|||
}
|
||||
}
|
||||
|
||||
//fprintf(stdout, "check_sync_word: center=%.0fkHz, errors=%d\n", *center * 2083334.0 / 65536 / 1000, error_bits);
|
||||
// fprintf(stdout, "check_sync_word: center=%.0fkHz, errors=%d\n", *center *
|
||||
// 2083334.0 / 65536 / 1000, error_bits);
|
||||
|
||||
return (error_bits <= MAX_SYNC_ERRORS);
|
||||
}
|
||||
|
||||
#define SYNC_MASK ((((uint64_t)1)<<SYNC_BITS)-1)
|
||||
#define SYNC_MASK ((((uint64_t)1) << SYNC_BITS) - 1)
|
||||
|
||||
int process_buffer(uint16_t *phi, int len, uint64_t offset)
|
||||
{
|
||||
int process_buffer(uint16_t *phi, int len, uint64_t offset) {
|
||||
uint64_t sync0 = 0, sync1 = 0;
|
||||
int lenbits;
|
||||
int bit;
|
||||
|
|
@ -339,10 +333,10 @@ int process_buffer(uint16_t *phi, int len, uint64_t offset)
|
|||
// ensure we don't consume any partial sync word we might be part-way
|
||||
// through. This means we don't need to maintain state between calls.
|
||||
|
||||
lenbits = len/2 - (SYNC_BITS + UPLINK_FRAME_BITS);
|
||||
lenbits = len / 2 - (SYNC_BITS + UPLINK_FRAME_BITS);
|
||||
for (bit = 0; bit < lenbits; ++bit) {
|
||||
int16_t dphi0 = phi_difference(phi[bit*2], phi[bit*2+1]);
|
||||
int16_t dphi1 = phi_difference(phi[bit*2+1], phi[bit*2+2]);
|
||||
int16_t dphi0 = phi_difference(phi[bit * 2], phi[bit * 2 + 1]);
|
||||
int16_t dphi1 = phi_difference(phi[bit * 2 + 1], phi[bit * 2 + 2]);
|
||||
|
||||
sync0 = ((sync0 << 1) | (dphi0 > 0 ? 1 : 0)) & SYNC_MASK;
|
||||
sync1 = ((sync1 << 1) | (dphi1 > 0 ? 1 : 0)) & SYNC_MASK;
|
||||
|
|
@ -360,22 +354,23 @@ int process_buffer(uint16_t *phi, int len, uint64_t offset)
|
|||
// errors.
|
||||
|
||||
// check for downlink frames:
|
||||
if (sync_word_fuzzy_compare(sync0, ADSB_SYNC_WORD) || sync_word_fuzzy_compare(sync1, ADSB_SYNC_WORD)) {
|
||||
int startbit = (bit-SYNC_BITS+1);
|
||||
if (sync_word_fuzzy_compare(sync0, ADSB_SYNC_WORD) ||
|
||||
sync_word_fuzzy_compare(sync1, ADSB_SYNC_WORD)) {
|
||||
int startbit = (bit - SYNC_BITS + 1);
|
||||
int shift = (sync_word_fuzzy_compare(sync0, ADSB_SYNC_WORD) ? 0 : 1);
|
||||
int index = startbit*2+shift;
|
||||
int index = startbit * 2 + shift;
|
||||
|
||||
int skip_0, skip_1;
|
||||
int rs_0 = -1, rs_1 = -1;
|
||||
|
||||
skip_0 = demod_adsb_frame(phi+index, demod_buf_a, &rs_0);
|
||||
skip_1 = demod_adsb_frame(phi+index+1, demod_buf_b, &rs_1);
|
||||
skip_0 = demod_adsb_frame(phi + index, demod_buf_a, &rs_0);
|
||||
skip_1 = demod_adsb_frame(phi + index + 1, demod_buf_b, &rs_1);
|
||||
if (skip_0 && rs_0 <= rs_1) {
|
||||
handle_adsb_frame(offset+index, demod_buf_a, rs_0);
|
||||
handle_adsb_frame(offset + index, demod_buf_a, rs_0);
|
||||
bit = startbit + skip_0;
|
||||
continue;
|
||||
} else if (skip_1 && rs_1 <= rs_0) {
|
||||
handle_adsb_frame(offset+index+1, demod_buf_b, rs_1);
|
||||
handle_adsb_frame(offset + index + 1, demod_buf_b, rs_1);
|
||||
bit = startbit + skip_1;
|
||||
continue;
|
||||
} else {
|
||||
|
|
@ -384,22 +379,23 @@ int process_buffer(uint16_t *phi, int len, uint64_t offset)
|
|||
}
|
||||
|
||||
// check for uplink frames:
|
||||
else if (sync_word_fuzzy_compare(sync0, UPLINK_SYNC_WORD) || sync_word_fuzzy_compare(sync1, UPLINK_SYNC_WORD)) {
|
||||
int startbit = (bit-SYNC_BITS+1);
|
||||
else if (sync_word_fuzzy_compare(sync0, UPLINK_SYNC_WORD) ||
|
||||
sync_word_fuzzy_compare(sync1, UPLINK_SYNC_WORD)) {
|
||||
int startbit = (bit - SYNC_BITS + 1);
|
||||
int shift = (sync_word_fuzzy_compare(sync0, UPLINK_SYNC_WORD) ? 0 : 1);
|
||||
int index = startbit*2+shift;
|
||||
int index = startbit * 2 + shift;
|
||||
|
||||
int skip_0, skip_1;
|
||||
int rs_0 = -1, rs_1 = -1;
|
||||
|
||||
skip_0 = demod_uplink_frame(phi+index, demod_buf_a, &rs_0);
|
||||
skip_1 = demod_uplink_frame(phi+index+1, demod_buf_b, &rs_1);
|
||||
skip_0 = demod_uplink_frame(phi + index, demod_buf_a, &rs_0);
|
||||
skip_1 = demod_uplink_frame(phi + index + 1, demod_buf_b, &rs_1);
|
||||
if (skip_0 && rs_0 <= rs_1) {
|
||||
handle_uplink_frame(offset+index, demod_buf_a, rs_0);
|
||||
handle_uplink_frame(offset + index, demod_buf_a, rs_0);
|
||||
bit = startbit + skip_0;
|
||||
continue;
|
||||
} else if (skip_1 && rs_1 <= rs_0) {
|
||||
handle_uplink_frame(offset+index+1, demod_buf_b, rs_1);
|
||||
handle_uplink_frame(offset + index + 1, demod_buf_b, rs_1);
|
||||
bit = startbit + skip_1;
|
||||
continue;
|
||||
} else {
|
||||
|
|
@ -408,23 +404,31 @@ int process_buffer(uint16_t *phi, int len, uint64_t offset)
|
|||
}
|
||||
}
|
||||
|
||||
return (bit - SYNC_BITS)*2;
|
||||
return (bit - SYNC_BITS) * 2;
|
||||
}
|
||||
|
||||
// demodulate 'bytes' bytes from samples at 'phi' into 'frame',
|
||||
// using 'center_dphi' as the bit slicing threshold
|
||||
static void demod_frame(uint16_t *phi, uint8_t *frame, int bytes, int16_t center_dphi)
|
||||
{
|
||||
static void demod_frame(uint16_t *phi, uint8_t *frame, int bytes,
|
||||
int16_t center_dphi) {
|
||||
while (--bytes >= 0) {
|
||||
uint8_t b = 0;
|
||||
if (phi_difference(phi[0], phi[1]) > center_dphi) b |= 0x80;
|
||||
if (phi_difference(phi[2], phi[3]) > center_dphi) b |= 0x40;
|
||||
if (phi_difference(phi[4], phi[5]) > center_dphi) b |= 0x20;
|
||||
if (phi_difference(phi[6], phi[7]) > center_dphi) b |= 0x10;
|
||||
if (phi_difference(phi[8], phi[9]) > center_dphi) b |= 0x08;
|
||||
if (phi_difference(phi[10], phi[11]) > center_dphi) b |= 0x04;
|
||||
if (phi_difference(phi[12], phi[13]) > center_dphi) b |= 0x02;
|
||||
if (phi_difference(phi[14], phi[15]) > center_dphi) b |= 0x01;
|
||||
if (phi_difference(phi[0], phi[1]) > center_dphi)
|
||||
b |= 0x80;
|
||||
if (phi_difference(phi[2], phi[3]) > center_dphi)
|
||||
b |= 0x40;
|
||||
if (phi_difference(phi[4], phi[5]) > center_dphi)
|
||||
b |= 0x20;
|
||||
if (phi_difference(phi[6], phi[7]) > center_dphi)
|
||||
b |= 0x10;
|
||||
if (phi_difference(phi[8], phi[9]) > center_dphi)
|
||||
b |= 0x08;
|
||||
if (phi_difference(phi[10], phi[11]) > center_dphi)
|
||||
b |= 0x04;
|
||||
if (phi_difference(phi[12], phi[13]) > center_dphi)
|
||||
b |= 0x02;
|
||||
if (phi_difference(phi[14], phi[15]) > center_dphi)
|
||||
b |= 0x01;
|
||||
*frame++ = b;
|
||||
phi += 16;
|
||||
}
|
||||
|
|
@ -436,8 +440,7 @@ static void demod_frame(uint16_t *phi, uint8_t *frame, int bytes, int16_t center
|
|||
// number of corrected errors, or 9999 if demodulation failed.
|
||||
// Return 0 if demodulation failed, or the number of bits (not
|
||||
// samples) consumed if demodulation was OK.
|
||||
static int demod_adsb_frame(uint16_t *phi, uint8_t *to, int *rs_errors)
|
||||
{
|
||||
static int demod_adsb_frame(uint16_t *phi, uint8_t *to, int *rs_errors) {
|
||||
int16_t center_dphi;
|
||||
int frametype;
|
||||
|
||||
|
|
@ -446,7 +449,7 @@ static int demod_adsb_frame(uint16_t *phi, uint8_t *to, int *rs_errors)
|
|||
return 0;
|
||||
}
|
||||
|
||||
demod_frame(phi + SYNC_BITS*2, to, LONG_FRAME_BYTES, center_dphi);
|
||||
demod_frame(phi + SYNC_BITS * 2, to, LONG_FRAME_BYTES, center_dphi);
|
||||
frametype = correct_adsb_frame(to, rs_errors);
|
||||
if (frametype == 1)
|
||||
return (SYNC_BITS + SHORT_FRAME_BITS);
|
||||
|
|
@ -462,8 +465,7 @@ static int demod_adsb_frame(uint16_t *phi, uint8_t *to, int *rs_errors)
|
|||
// number of corrected errors, or 9999 if demodulation failed.
|
||||
// Return 0 if demodulation failed, or the number of bits (not
|
||||
// samples) consumed if demodulation was OK.
|
||||
static int demod_uplink_frame(uint16_t *phi, uint8_t *to, int *rs_errors)
|
||||
{
|
||||
static int demod_uplink_frame(uint16_t *phi, uint8_t *to, int *rs_errors) {
|
||||
int16_t center_dphi;
|
||||
uint8_t interleaved[UPLINK_FRAME_BYTES];
|
||||
|
||||
|
|
@ -472,11 +474,12 @@ static int demod_uplink_frame(uint16_t *phi, uint8_t *to, int *rs_errors)
|
|||
return 0;
|
||||
}
|
||||
|
||||
demod_frame(phi + SYNC_BITS*2, interleaved, UPLINK_FRAME_BYTES, center_dphi);
|
||||
demod_frame(phi + SYNC_BITS * 2, interleaved, UPLINK_FRAME_BYTES,
|
||||
center_dphi);
|
||||
|
||||
// deinterleave and correct
|
||||
if (correct_uplink_frame(interleaved, to, rs_errors) == 1)
|
||||
return (UPLINK_FRAME_BITS+SYNC_BITS);
|
||||
return (UPLINK_FRAME_BITS + SYNC_BITS);
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue