Merge remote-tracking branch 'upstream/master' into dev

This commit is contained in:
Oliver Jowett 2016-10-01 15:18:26 +01:00
commit 812e4486a4
2 changed files with 152 additions and 5 deletions

View file

@ -1787,6 +1787,17 @@ typedef enum {
TISB_CATEGORY = 2048
} tisb_flags;
static inline unsigned unsigned_difference(unsigned v1, unsigned v2)
{
return (v1 > v2) ? (v1 - v2) : (v2 - v1);
}
static inline unsigned heading_difference(unsigned h1, unsigned h2)
{
unsigned d = unsigned_difference(h1, h2);
return (d < 180) ? d : (360 - d);
}
static void writeFATSV()
{
struct aircraft *a;
@ -1860,19 +1871,19 @@ static void writeFATSV()
if (altGNSSValid && abs(a->altitude_gnss - a->fatsv_emitted_altitude_gnss) >= 50) {
changed = 1;
}
if (headingValid && abs(a->heading - a->fatsv_emitted_heading) >= 2) {
if (headingValid && heading_difference(a->heading, a->fatsv_emitted_heading) >= 2) {
changed = 1;
}
if (headingMagValid && abs(a->heading_magnetic - a->fatsv_emitted_heading_magnetic) >= 2) {
if (headingMagValid && heading_difference(a->heading_magnetic, a->fatsv_emitted_heading_magnetic) >= 2) {
changed = 1;
}
if (speedValid && abs(a->speed - a->fatsv_emitted_speed) >= 25) {
if (speedValid && unsigned_difference(a->speed, a->fatsv_emitted_speed) >= 25) {
changed = 1;
}
if (speedIASValid && abs(a->speed_ias - a->fatsv_emitted_speed_ias) >= 25) {
if (speedIASValid && unsigned_difference(a->speed_ias, a->fatsv_emitted_speed_ias) >= 25) {
changed = 1;
}
if (speedTASValid && abs(a->speed_tas - a->fatsv_emitted_speed_tas) >= 25) {
if (speedTASValid && unsigned_difference(a->speed_tas, a->fatsv_emitted_speed_tas) >= 25) {
changed = 1;
}

136
tools/replay-beast.py Executable file
View file

@ -0,0 +1,136 @@
#!/usr/bin/env python3
#
# Reads Beast-format input files and replays them to stdout
# (either in their original form or in a human-readable form)
# while maintaining the correct inter-message delays according
# to the timestamps contained in the input file.
#
from contextlib import closing
import time
MODE_AC = 'MODE_AC'
MODE_S_SHORT = 'MODE_S_SHORT'
MODE_S_LONG = 'MODE_S_LONG'
RADARCAPE_STATUS = 'RADARCAPE_STATUS'
def parse(buf):
i = 0
messages = []
msglen = -1
start = 0
while i < len(buf):
if buf[i] != 0x1a:
i += 1
continue
i += 1
if i >= len(buf):
break
msglen = 1 + 6
if buf[i] == 0x31:
msglen += 2
msgtype = MODE_AC
elif buf[i] == 0x32:
msglen += 7
msgtype = MODE_S_SHORT
elif buf[i] == 0x33:
msglen += 14
msgtype = MODE_S_LONG
elif buf[i] == 0x34:
msglen += 14
msgtype = RADARCAPE_STATUS
else:
continue
i += 1
msgbytes = bytearray()
while i < len(buf) and len(msgbytes) < msglen:
if buf[i] == 0x1a:
i += 1
if i >= len(buf) or buf[i] != 0x1a:
break
msgbytes.append(buf[i])
i += 1
if len(msgbytes) == msglen:
timestamp = (msgbytes[0] << 40) | (msgbytes[1] << 32) | (msgbytes[2] << 24) | (msgbytes[3] << 16) | (msgbytes[4] << 8) | (msgbytes[5])
signal = msgbytes[6]
data = msgbytes[7:]
raw = buf[start:i]
messages.append( (msgtype, timestamp, signal, data, raw) )
start = i
return (buf[start:], messages)
def replay(filename, radarcape_mode, show_mode, delay_mode):
with closing(open(filename, 'rb')) as f:
buf = b''
last_timestamp = None
last_time = None
while True:
more = f.read(1024)
buf = buf + more
buf, messages = parse(buf)
if not messages and not more:
break
for msgtype, timestamp, signal, data, raw in messages:
if delay_mode:
if radarcape_mode:
secs = timestamp >> 30
nanos = timestamp & 0x00003FFFFFFF
adj_timestamp = nanos + secs * 1000000000
freq = 1e9
else:
adj_timestamp = timestamp
freq = 12e6
if last_timestamp is None:
last_timestamp = adj_timestamp
last_time = time.time()
elif adj_timestamp > last_timestamp:
now = time.time()
sched_delta = (adj_timestamp - last_timestamp) / freq
delay = last_time + sched_delta - now
if delay > 0.010:
time.sleep(delay)
last_timestamp = adj_timestamp
last_time += sched_delta
if show_mode:
h = ''
for b in data:
h += '{0:02X}'.format(b)
print("Type: {0:16s} Time: {1:06X} Signal: {2:3d} Data: {3}".format(msgtype, timestamp, signal, h))
else:
sys.stdout.buffer.write(raw)
sys.stdout.buffer.flush()
if __name__ == '__main__':
import sys
radarcape_mode = False
show_mode = False
delay_mode = True
for filename in sys.argv[1:]:
if filename == '--radarcape':
radarcape_mode = True
elif filename == '--beast':
radarcape_mode = False
elif filename == '--show':
show_mode = True
elif filename == '--raw':
show_mode = False
elif filename == '--delay':
delay_mode = True
elif filename == '--no-delay':
delay_mode = False
else:
replay(filename, radarcape_mode=radarcape_mode, show_mode=show_mode, delay_mode=delay_mode)