def plot_value(tc, field="acc_z", scale=1):
"""
Draw Random Data
"""
im = tc['image']
margin = tc['margin']
(first, last, pixel_per_mile) = tc['mileposts']
draw = ImageDraw.Draw(im)
yz = int((im.size[1]-margin)*0.26)
data = [None] * im.size[0]
draw.text((margin, yz), field.upper(), fill=COLORS['black'])
# Read from file
data_sum = data_count = 0
for line_no, obj in pirail.read(tc['data_file'], classes=["ATT"]):
data_sum += obj[field]
data_count += 1
# Normalize data by subtracting the average
data_avg = data_sum / data_count
speed = eps = 0
# Read from file again
for line_no, obj in pirail.read(tc['data_file'], classes=["TPV", "ATT"], args={
'start-mileage': first,
'end-mileage': last,
}):
mileage = obj['mileage']
x = mile_to_pixel(tc, mileage-first)
if obj['class'] == "TPV":
speed = obj['speed']
eps = obj['eps']
elif obj['class'] == "ATT":
if speed < eps:
pass
else:
data_point = obj[field] - data_avg
# Look for maximum magnitude
if data[x] is None or abs(data_point) > abs(data[x]):
data[x] = data_point
# Plot the data
last_x = None
for x in range(margin, im.size[0]-2*margin):
draw.point((x,yz), fill=COLORS['black'])
if data[x] is None:
continue
y = yz-scale*data[x]
if last_x is None:
draw.point((x,y), fill=COLORS['red'])
else:
draw.line((x,y,last_x,last_y), fill=COLORS['red'])
last_x = x
last_y = y
del draw
def write_average_json_file(path_to_read_file):
average_acc_x = 0.0
average_acc_y = 0.0
average_acc_z = 0.0
average_roll = 0.0
average_pitch = 0.0
average_yaw = 0.0
N = 0.0
for line_no, obj in pirail.read('../web_server_flask/' +
path_to_read_file):
N += 1
average_acc_x += obj["acc_x"]
average_acc_y += obj["acc_y"]
average_acc_z += obj["acc_z"]
average_roll += obj["roll"]
average_pitch += obj["pitch"]
average_yaw += obj["yaw"]
loc_average_acc_x = average_acc_x / N
loc_average_acc_y = average_acc_y / N
average_acc_z = average_acc_z / N
average_roll = average_roll / N
average_pitch = average_pitch / N
average_yaw = average_yaw / N
return loc_average_acc_x, loc_average_acc_y, average_acc_z, average_roll, average_pitch, average_yaw
def write_sd(read_file, json_name):
list = []
for num, obj in pirail.read('../web_server_flask/' + read_file):
list.append(obj[json_name])
sd = statistics.stdev(list)
return sd
def write_max(read_file, json_name):
list = []
for num, obj in pirail.read('../web_server_flask/' + read_file):
list.append(obj[json_name])
#print(list)
maxList = max(list)
return maxList
def main(filename):
last_lat = last_lon = speed = 0
slice_count = 0
data = [0] * 360
output = filename.replace("json", "kml")
with open(output, "w") as k:
k.write('<?xml version="1.0" encoding="UTF-8"?>\n')
k.write('<kml xmlns="http://www.opengis.net/kml/2.2">\n')
k.write('<Document>\n')
for line_no, obj in pirail.read(filename, classes=['TPV', 'LIDAR']):
count = 0
if obj['class'] == "TPV" and 'speed' in obj:
speed = obj['speed']
elif obj['class'] == "LIDAR" and speed >= MIN_SPEED:
data = [0] * 360
for angle, distance in obj['scan']:
distance = lidar_util.estimate_from_lidar(float(distance))
i = round(float(angle)) % 360
data[i] = distance
report = plot(data, obj['time'], obj['lat'], obj['lon'],
obj['mileage'], speed, slice_count)
if report['gauge_error'] or report['plate_error']:
count += 1
if OUTPUT:
if (last_lat != obj['lat']
or last_lon != obj['lon']) and count > 10:
print("%d %0.6f %0.6f %0.6f %0.2f %d" %
(slice_count, obj['lat'], obj['lon'],
obj['mileage'], report['gauge'],
report['plate_score']))
k.write('<Placemark>\n')
k.write('<name>Point %d</name>\n' % slice_count)
k.write('<description>\n')
k.write('Mileage = %0.2f\n' % obj['mileage'])
if report['gauge_error']:
k.write('Gage = %0.f in\n' % report['gauge'])
if report['plate_error']:
k.write(aar_plate.full_name + ' obstruction')
k.write('</description>\n')
k.write('<Point>\n')
k.write(
'<coordinates>%0.6f,%0.6f,%0.6f</coordinates>\n'
% (obj['lon'], obj['lat'], obj['alt']))
k.write('</Point>\n')
k.write('</Placemark>\n')
last_lat = obj['lat']
last_lon = obj['lon']
else:
count = 0
slice_count += 1
k.write('</Document>\n')
k.write('</kml>\n')
def write_json_data_to_range(pathToRange, locationPath, minRange, maxRange):
data = []
for line_no, obj in pirail.read('../web_server_flask/' + locationPath):
if obj["mileage"] >= minRange and obj["mileage"] <= maxRange:
data.append(obj)
data = sorted(data, key=lambda k: k["mileage"], reverse=False)
#print(os.path.isfile(pathToRange))
f = open(pathToRange, 'x')
for obj in data:
j = re.sub(r"'", '"', str(obj))
f.write(j + '\n')
f.close()
def main(filename):
x = 0
data = [0]*360
for line_no,obj in pirail.read(filename, classes=['TPV', 'LIDAR']):
count = 0
if obj['class'] == "TPV" and 'speed' in obj:
speed = obj['speed']
elif obj['class'] == "LIDAR" and speed >= MIN_SPEED:
for angle, distance in obj['scan']:
distance = lidar_util.estimate_from_lidar(float(distance))
i = round(float(angle)) % 360
data[i] = distance
plot(x, data)
x += 1
if x == WIDTH:
break
def clearance(filename):
data = [99999] * 360
print(data)
latitude = longitude = mileage = speed = 0
for line_no, obj in pirail.read(filename, classes=['TPV', 'LIDAR']):
if obj['class'] == "LIDAR":
timestamp = obj['time']
for angle, distance in obj['scan']:
distance = lidar_util.estimate_from_lidar(float(distance))
if distance > 1000 and speed > .5:
i = round(float(angle)) % 360
data[i] = min(data[i], float(distance))
elif obj['class'] == "TPV":
if 'speed' in obj:
speed = to_mph(obj['speed'])
report = plot(data, timestamp, latitude, longitude, mileage, speed, 0)
print(report)
def gage(tc):
"""
Draw Gage
"""
im = tc['image']
margin = tc['margin']
(first, last, pixel_per_mile) = tc['mileposts']
draw = ImageDraw.Draw(im)
y = int((im.size[1]-margin)*0.34)
draw.text((margin, y), aar.full_name, fill=COLORS['black'])
data = [0] * 360
ghost = [0] * 360
total_slope = total_slope_count = 0
# Read from file
for line_no, obj in pirail.read(tc['data_file']):
if obj['class'] not in ["LIDAR", "L"]:
continue
mileage = obj['mileage']
if not(first <= mileage <= last):
continue
x = mile_to_pixel(tc, mileage-first)
lidar_util.process_scan(obj['lidar'], data, ghost)
new_data = lidar_util.convert_to_xy(data, offset=2.1528056371157285)
gage,slope,p1,p2 = lidar_util.calc_gage(new_data)
if not(aar.min_gauge <= gage <= aar.max_gauge):
#print(mileage, gage)
draw.point((x,y+(gage-aar.standard_gauge)*2),fill=COLORS['red'])
else:
#draw.point((x,y), fill=COLORS['black'])
pass
total_slope += slope
total_slope_count += 1
if total_slope_count > 0:
#print(total_slope/total_slope_count)
pass
del draw
def main(filename):
used=count=0
print("Time Latitude Longitude Used Count")
for line_no, obj in pirail.read(filename, classes=['SKY', 'TPV']):
if obj['class'] == "SKY":
used=count=0
for s in obj['satellites']:
count += 1
if s['used']:
used += 1
elif obj['class'] == "TPV":
if 'num_sat' in obj:
count = obj['num_sat']
if 'num_used' in obj:
used = obj['num_used']
if 'lat' in obj and 'lon' in obj:
if used >= pirail.GPS_THRESHOLD:
print("%s %f %f %d %d" % (obj['time'], obj['lat'], obj['lon'], used, count))
def main():
try:
filename = sys.argv[-1]
except:
print("USAGE: %s [args] data_file.json" % sys.argv[0])
sys.exit(1)
data = []
for line_no, obj in pirail.read(filename, classes=['LIDAR']):
data.append(obj)
x_data = []
y_data = []
z_data = []
count = 1
base_mileage = data[0]['mileage']
for scan in data:
count += 1
if count % 10 == 0:
for angle, distance in scan['scan']:
distance = lidar_util.estimate_from_lidar(float(distance))
if distance > 0.0:
x, y = pirail.vector_to_coordinates(
angle + OFFSET, distance)
x_data.append(x)
y_data.append(y)
z_data.append(scan['mileage'] - base_mileage)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(x_data, z_data, y_data, s=1, c='black', marker='o')
ax.set_xlabel('X Label')
ax.set_ylabel('Z Label')
ax.set_zlabel('Y Label')
plt.show()
def main(filename):
sums = {
'acc_x': 0,
'acc_y': 0,
'acc_z': 0,
'gyro_x': 0,
'gyro_y': 0,
'gyro_z': 0,
}
cnt = 0
print("Time ax ay az gx gy gz")
for line_no, obj in pirail.read(filename, classes=['ATT']):
print("%s %f %f %f %f %f %f" % (
obj['time'],
obj['acc_x'] - aax,
obj['acc_y'] - aay,
obj['acc_z'] - aaz,
obj['gyro_x'] - agx,
obj['gyro_y'] - agy,
obj['gyro_z'] - agz,
))
sums['acc_x'] += obj['acc_x']
sums['acc_y'] += obj['acc_y']
sums['acc_z'] += obj['acc_z']
sums['gyro_x'] += obj['gyro_x']
sums['gyro_y'] += obj['gyro_y']
sums['gyro_z'] += obj['gyro_z']
cnt += 1
print("Average %f %f %f %f %f %f" % (
sums['acc_x'] / cnt,
sums['acc_y'] / cnt,
sums['acc_z'] / cnt,
sums['gyro_x'] / cnt,
sums['gyro_y'] / cnt,
sums['gyro_z'] / cnt,
))
"""
Slice a datafile based on line number
"""
import sys
import json
import pirail
try:
filename = sys.argv[-1]
split_line_no = int(sys.argv[-2])
except IndexError:
print("USAGE: %s [args] line_no data_file.json" % sys.argv[0])
sys.exit(1)
outfile_name = filename.replace(".json", "_%d.json")
outfile = None
for line_no, obj in pirail.read(filename):
if outfile is None or line_no == split_line_no:
if outfile is not None:
outfile.close()
outfile = open(outfile_name % line_no, "w")
print("Writing %s" % (outfile_name % line_no))
outfile.write(json.dumps(obj) + "\n")
outfile.close()
def main(filename):
s = [0] * RANGE
data = []
for a in range(RANGE):
data.append([])
with open("clearance.csv", "w") as clearances:
clearances.write("Date Time Mileage Clearance\n")
for line_no, obj in pirail.read(filename, classes=['LIDAR']):
for angle, distance in obj['scan']:
distance = lidar_util.estimate_from_lidar(distance)
a = round(angle + OFFSET) % RANGE
d = float(distance)
if d > 0:
(x, y) = pirail.vector_to_coordinates(a, d)
s[a] = y
data[a].append(d)
# Calculate ground clearance
sample = s[150:210]
clearance = -sum(sample) / len(sample)
if (0 < clearance < 625):
clearances.write("%s %f %f\n" % (
obj['time'].replace('T', ' ').replace('Z', ''),
obj['mileage'],
clearance * MM_TO_INCH,
))
avgd = [0] * RANGE
with open("average_lidar.csv", "w") as f:
f.write("Angle Distance Trend X Y\n")
for a in range(RANGE):
if len(data[a]) > 0:
d = statistics.mean(data[a])
avgd[a] = d
else:
d = 0
(x, y) = pirail.vector_to_coordinates(a, d)
if a == 0:
trend = 0
elif d > avgd[a - 1]:
trend = 1
elif d < avgd[a - 1]:
trend = -1
else:
trend = 0
f.write("%d %f %d %f %f\n" % (a, d, trend, x, y))
# find right rail
a = 180 - 60
while avgd[a] > avgd[a + 1]:
a += 1
print("Right: %d" % a)
# find left rail
a = 180 + 60
while avgd[a] > avgd[a - 1]:
a -= 1
print("Left: %d" % a)
with open("min_lidar.csv", "w") as f:
f.write("Angle Distance X Y\n")
for a in range(RANGE):
if len(data[a]) > 0:
d = my_min(data[a], THRESHOLD)
else:
d = 0
(x, y) = pirail.vector_to_coordinates(a, d)
f.write("%d %f %f %f\n" % (a, d, x, y))
min_lat = 90
max_lat = -90
min_lon = 180
max_lon = -180
# Average Errors
aax = -0.76994928407068
aay = -0.12089123215864023
agz = -0.5585602094240765
data=[]
bad_data=[]
acc_count = 0
for line_no, obj in pirail.read(filename, classes=['ATT', 'TPV']):
if obj['class'] == 'ATT':
# Error compensation
obj['acc_x'] -= aax
obj['acc_y'] -= aay
obj['gyro_z'] -= agz
data.append(obj)
acc_count += 1
elif obj['class'] == 'TPV':
if obj['num_used'] >= pirail.GPS_THRESHOLD:
min_lat = min(min_lat, obj['lat'])
max_lat = max(max_lat, obj['lat'])
min_lon = min(min_lon, obj['lon'])
max_lon = max(max_lon, obj['lon'])
data.append(obj)
else:
def get_file(self, groups, qsdict):
"""
Data Fetching API
GET pirail_data_fetch.php
"""
# Parse the query string and
# Build the Args Dictionary
args = {}
try:
if self.headers['accept'] == "application/json":
stream = False
elif self.headers['accept'] == "text/event-stream":
stream = True
else:
stream = qsdict.get("stream", ["true"])[0].lower() == "true"
filename = qsdict.get("file", [None])[0]
if filename is None:
self.send_error(HTTPStatus.NOT_FOUND,
HTTPStatus.NOT_FOUND.description)
return
else:
pathname = os.path.normpath(DATAROOT + "/" + filename)
if not os.path.isfile(pathname):
self.send_error(HTTPStatus.NOT_FOUND,
HTTPStatus.NOT_FOUND.description)
return
xform = qsdict.get("xform", ["default"])[0].lower()
if xform in DATA_XFORM:
xform_function = DATA_XFORM[xform]
else:
xform_function = DATA_XFORM['default']
value = qsdict.get("start-mileage", [None])[0]
if value is not None:
args['start-mileage'] = float(value)
value = qsdict.get("end-mileage", [None])[0]
if value is not None:
args['end-mileage'] = float(value)
value = qsdict.get("start-time", [None])[0]
if value is not None:
args['start-time'] = value
value = qsdict.get("end-time", [None])[0]
if value is not None:
args['end-time'] = float(value)
value = qsdict.get("start-latitude", [None])[0]
if value is not None:
args['start-latitude'] = float(value)
value = qsdict.get("end-latitude", [None])[0]
if value is not None:
args['end-latitude'] = float(value)
value = qsdict.get("start-longitude", [None])[0]
if value is not None:
args['start-longitude'] = float(value)
value = qsdict.get("end-longitude", [None])[0]
if value is not None:
args['end-longitude'] = float(value)
classes = qsdict.get("classes", None)
except ValueError as ex:
self.send_error(HTTPStatus.BAD_REQUEST, str(ex))
return
data = []
self.send_response(HTTPStatus.OK)
if stream:
self.send_header("Content-type", "text/event-stream")
self.end_headers()
else:
self.send_header("Content-type", "application/json")
for line_no, obj in pirail.read(pathname, classes=classes, args=args):
if stream:
output = "event: pirail\ndata: %s\n\n" % json.dumps(
xform_function(obj, qsdict))
self.wfile.write(output.encode('utf-8'))
else:
data.append(xform_function(obj, qsdict))
if not stream:
# Sort the data
if SORTBY is not None:
data = sorted(data, key=lambda k: k[SORTBY], reverse=False)
output = json.dumps(data, indent=4) + "\n"
self.send_header("Content-length", str(len(output)))
self.end_headers()
self.wfile.write(output.encode('utf-8'))
else:
output = "event: pirail\ndata: %s\n\n" % json.dumps({"done": True})
self.wfile.write(output.encode('utf-8'))
#!/usr/bin/env python3
"""
GPS Smoothing
"""
import sys
import json
import math
import pirail
data = []
for line_no, obj in pirail.read(sys.argv[-1], classes=['SKY', 'TPV']):
if obj['class'] == "TPV":
if obj['num_used'] >= pirail.GPS_THRESHOLD and 'lon' in obj and 'lat' in obj:
data.append(obj)
elif obj['class'] == "SKY":
used = count = 0
count = len(obj['satellites'])
for i in range(0, count):
if obj['satellites'][i]['used']:
used += 1
print("Longitude Latitude dx epx dy epy used count")
for i in range(1, len(data)):
if 'exp' not in data[i] or 'epy' not in data[i]:
continue
dx = abs((data[i]['lon'] - data[i - 1]['lon']) * 111120 *
math.cos(math.radians(data[i]['lat'])))
dy = abs(
(data[i]['lat'] - data[i - 1]['lat']) * 111128) # degrees to meters
def accel(tc, scale=1):
"""
Draw Acceleration Data
"""
im = tc['image']
margin = tc['margin']
(first, last, pixel_per_mile) = tc['mileposts']
draw = ImageDraw.Draw(im)
yx = int((im.size[1]-margin)*0.10)
yy = int((im.size[1]-margin)*0.18)
yz = int((im.size[1]-margin)*0.26)
ygx = int((im.size[1]-margin)*0.34)
ygy = int((im.size[1]-margin)*0.42)
ygz = int((im.size[1]-margin)*0.50)
ys = int((im.size[1]-margin)*0.58)
ACCxp = [0] * im.size[0]
ACCyp = [0] * im.size[0]
ACCzp = [0] * im.size[0]
GYRxp = [0] * im.size[0]
GYRyp = [0] * im.size[0]
GYRzp = [0] * im.size[0]
draw.text((margin, yx), "AX", fill=COLORS['red'])
draw.text((margin, yy), "AY", fill=COLORS['blue'])
draw.text((margin, yz), "AZ", fill=COLORS['green'])
draw.text((margin, ygx), "GX", fill=COLORS['red'])
draw.text((margin, ygy), "GY", fill=COLORS['blue'])
draw.text((margin, ygz), "GZ", fill=COLORS['green'])
#draw.text((margin, ys), "S", fill=COLORS['black'])
accel_threshold = 0.00
mileage = None
speed = 0
last_x = None
accel_file = open("accel.csv", "w")
accel_file.write("mileage acc_x acc_y acc_z gyro_x gyro_y gyro_z\n")
# Read from file
for line_no, obj in pirail.read(tc['data_file'], args={
'start-mileage': first,
'end-mileage': last,
}):
#print(obj)
mileage = obj['mileage']
x = mile_to_pixel(tc, mileage-first)
if obj['class'] == "G" or obj['class'] == "TPV":
# Speed
speed = obj['speed']
#draw.point((x, ys-speed), fill=COLORS['black'])
elif obj['class'] in ["A", "ATT"]:
if speed == 100:
#draw.point((x, yx), fill=COLORS['black'])
#draw.point((x, yy), fill=COLORS['black'])
#draw.point((x, yz), fill=COLORS['black'])
pass
else:
ACCx = (obj['acc_x'])
if abs(ACCx) > ACCxp[x]:
ACCxp[x] = ACCx
ACCy = (obj['acc_y'])
if abs(ACCy) > ACCyp[x]:
ACCyp[x] = ACCy
ACCz = (obj['acc_z'] - 9.80665)
if abs(ACCz) > ACCzp[x]:
ACCzp[x] = ACCz
GYRx = (obj['gyro_x'])
if abs(GYRx) > GYRxp[x]:
GYRxp[x] = GYRx
GYRy = (obj['gyro_y'])
if abs(GYRy) > GYRyp[x]:
GYRyp[x] = GYRy
GYRz = (obj['gyro_z'])
if abs(GYRz) > GYRzp[x]:
GYRzp[x] = GYRz
accel_file.write("%f %f %f %f %f %f %f %f %f\n" %( obj['mileage'], obj['lat'], obj['lon'], obj['acc_x'], obj['acc_y'], obj['acc_z'], obj['gyro_x'], obj['gyro_y'], obj['gyro_z']))
for x in range(margin, len(ACCxp)-2*margin):
draw.line((x,yx-scale*ACCxp[x],x-1,yx-scale*ACCxp[x-1]),fill=COLORS['red'])
draw.line((x,yy-scale*ACCyp[x],x-1,yy-scale*ACCyp[x-1]),fill=COLORS['blue'])
draw.line((x,yz-scale*ACCzp[x],x-1,yz-scale*ACCzp[x-1]),fill=COLORS['green'])
draw.line((x,ygx-scale*GYRxp[x],x-1,ygx-scale*GYRxp[x-1]),fill=COLORS['red'])
draw.line((x,ygy-scale*GYRyp[x],x-1,ygy-scale*GYRyp[x-1]),fill=COLORS['blue'])
draw.line((x,ygz-scale*GYRzp[x],x-1,ygz-scale*GYRzp[x-1]),fill=COLORS['green'])
accel_file.close()
del draw
#!/usr/bin/env python3
import sys
import json
import pirail
print("Time Latitude Longitude Yaw Pitch Roll")
for line_no, obj in pirail.read(sys.argv[-1], classes=['ATT']):
if 'roll' in obj:
print("%s %f %f %f %f %f" % (
obj['time'],
obj['lat'],
obj['lon'],
obj['yaw'],
obj['pitch'],
obj['roll'],
)
)
def string_chart_by_time(tc):
"""
Draw a string chart
x = mileage
y = time
"""
im = tc['image']
margin = tc['margin']
(first, last, pixel_per_mile) = tc['mileposts']
draw = ImageDraw.Draw(im)
mintime = None
maxtime = None
timedata = []
skip = False
lastm = None
speed = 0
for line_no, obj in pirail.read(tc['data_file'], classes=['TPV'], args={
'start-mileage': first,
'end-mileage': last,
}):
if obj['num_used'] < pirail.GPS_THRESHOLD:
continue
#if obj['speed'] < obj['eps']:
# if skip:
# continue
# skip = True
#else:
# skip = False
mileage = obj['mileage']
objtime = pirail.parse_time(obj['time'])
speed = obj['speed']
if mintime is None or objtime < mintime:
mintime = objtime
if maxtime is None or objtime > maxtime:
maxtime = objtime
if lastm is None or abs(mileage - lastm) >= MILEAGE_THRESHOLD:
timedata.append({
'time': objtime,
'mileage': mileage,
'speed': speed,
})
lastm = mileage
timedata = sorted(timedata, key=lambda k: k['time'], reverse=False)
lastx = lasty = lasttime = None
for obj in timedata:
mileage = obj['mileage']
objtime = obj['time']
speed = obj['speed'] * MS_TO_MPH
x = mile_to_pixel(tc, mileage-first)
y = (im.size[1]-2*margin) * (objtime - mintime).total_seconds() / (maxtime-mintime).total_seconds() + margin
if lasttime is None:
timediff = 0
else:
timediff = (objtime - lasttime).total_seconds()
if speed > 25:
color=COLORS['red']
elif speed > 15:
color=COLORS['orange']
elif speed > 5:
color=COLORS['green']
else:
color=COLORS['blue']
if lastx is None or timediff > TIME_THRESHOLD:
draw.point((x, y), fill=color)
else:
draw.line((lastx, lasty, x, y), fill=color, width=STRING_WIDTH)
lastx = x
lasty = y
lasttime = objtime
lastm = mileage
if mintime is not None:
for hour in range(mintime.hour, maxtime.hour+1):
objtime = datetime.datetime(mintime.year, mintime.month, mintime.day, hour, 0, 0)
x = 10
y = (im.size[1]-2*margin) * (objtime - mintime).total_seconds() / (maxtime-mintime).total_seconds() + margin
draw.text((x, y), "%d:00Z" % hour, fill=COLORS['blue'])
del draw
def smooth_data(tc, input_data=None, mileage_threshold=0.01, track_threshold=45, write_file=True):
(first, last, pixel_per_mile) = tc['mileposts']
data = []
used = 0
for line_no, obj in pirail.read(tc['data_file'], classes=['TPV'], args={
'start-mileage': first,
'end-mileage': last,
}):
if obj['num_used'] < pirail.GPS_THRESHOLD:
continue
try:
data.append(obj)
except KeyError as ex:
print("skipped", ex, obj)
# Ensure the list is sorted by mileage
data = sorted(data, key=lambda k: k['mileage'], reverse=False)
# smooth, looking at all measurements +/-
smooth_data = []
for i in range(0, len(data)):
lat = [data[i]['lat']]
lon = [data[i]['lon']]
alt = [data[i]['alt']]
mileage = data[i]['mileage']
# work backwards
j = i-1
while j >= 0 and data[j]['mileage'] >= mileage-mileage_threshold/2:
lat.append(data[j]['lat'])
lon.append(data[j]['lon'])
alt.append(data[j]['alt'])
j -= 1
# work forwards
j = i + 1
while j < len(data) and data[j]['mileage'] <= mileage+mileage_threshold/2:
lat.append(data[j]['lat'])
lon.append(data[j]['lon'])
alt.append(data[j]['alt'])
j += 1
# Average
new_lat = avg_3_of_5(lat)
new_lon = avg_3_of_5(lon)
new_alt = avg_3_of_5(alt)
mileage, certainty = tc['G'].find_mileage(new_lat, new_lon)
obj = {
'class': data[i]['class'],
'speed': data[i]['speed'],
'eps': data[i]['eps'],
'lat': new_lat,
'lon': new_lon,
'alt': new_alt,
'mileage': mileage,
'certainty': certainty,
'track': data[i]['track']
}
if i == 0:
smooth_data.append(obj)
last_obj = obj
else:
d = geo.great_circle(last_obj['lat'], last_obj['lon'],
obj['lat'], obj['lon'])
b = geo.bearing(last_obj['lat'], last_obj['lon'],
obj['lat'], obj['lon'])
if d > mileage_threshold:
#print("Inserted: %f @ %d" % (d,b))
smooth_data.append(obj)
last_obj = obj
# Ensure we're still sorted
smooth_data = sorted(smooth_data, key=lambda k: k['mileage'], reverse=False)
for i in range(1,len(smooth_data)):
b = geo.bearing(smooth_data[i-1]['lat'], smooth_data[i-1]['lon'],
smooth_data[i]['lat'], smooth_data[i]['lon'])
smooth_data[i]['track'] = b
if len(smooth_data) > 2:
smooth_data[0]['track'] = smooth_data[1]['track']
# save result
if write_file:
with open("smoothdata_tmp.csv", "w") as f:
f.write("Mileage,Latitude,Longitude,Altitude,Track\n")
for obj in smooth_data:
f.write("%f %f %f %f %f\n" % (
obj['mileage'],
obj['lat'],
obj['lon'],
obj['alt'],
obj['track'],
))
#print("Smooth=%d" % len(smooth_data))
return smooth_data
if len(sys.argv) < 3:
print("USAGE: %s [args] json_file known_file" % sys.argv[0])
sys.exit(1)
json_file = os.path.abspath(sys.argv[-2])
known_file = sys.argv[-1]
GPS_MIN_MODE = 3
GPS = gps_to_mileage.Gps2Miles(known_file)
acclist = []
data = []
last_tpv = None
for line_no, obj in pirail.read(json_file):
if obj['class'] in ["SKY", "ATT", "LIDAR", "LPCM"]:
acclist.append(obj)
elif obj['class'] == "TPV":
if obj['num_used'] < pirail.GPS_THRESHOLD or obj['mode'] < GPS_MIN_MODE:
continue
if last_tpv is not None:
if len(acclist) > 0:
time_start = pirail.parse_time(obj['time'])
time_delta = time_start - pirail.parse_time(last_tpv['time'])
delta_lat = (obj['lat'] - last_tpv['lat'])
delta_lon = (obj['lon'] - last_tpv['lon'])
delta_alt = (obj['alt'] - last_tpv['alt'])
#print ("delta_lat", delta_lat)
#print ("delta_lon", delta_lon)
#print ("time_delta", time_delta.total_seconds())
