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
# Try to seed the initial values
# Find the first TPV record
for i in range(0, len(data)):
if data[i]['class'] == "TPV":
y_speed = data[i]['speed']
z_bearing = data[i]['track']
longitude = data[i]['lon']
latitude = data[i]['lat']
break
# Find the first ATT record
for i in range(0, len(data)):
if data[i]['class'] == "ATT":
last_time = pirail.parse_time(data[i]['time'])
x_last_acc = data[i]['acc_x']
x_speed = 0
y_last_acc = data[i]['acc_y']
z_last_acc = data[i]['gyro_z']
break
y_speed = None
z_bearing = None
for obj in bad_data:
if obj['class'] == "TPV":
draw_gps_fix(obj, COLOR_RED)
data.append(att)
#data.append((items[0],
#obj['acc_x'],
#obj['acc_y'],
#obj['acc_z'],
#obj['gyro_x'],
#obj['gyro_y'],
#obj['gyro_z'],
#))
gyroXangle = gyroYangle = gyroZangle = CFangleX = CFangleY = CFangleZ = 0
print("Time", "Lat", "Long", "AngleX", "AngleY", "AngleZ")
last_time = data[0]['time']
for obj in data:
DT = (pirail.parse_time(obj['time']) -
pirail.parse_time(last_time)).total_seconds()
gyroXangle += obj['gyro_x'] * DT
gyroYangle += obj['gyro_y'] * DT
gyroZangle += obj['gyro_z'] * DT
AccXangle = (float)(math.atan2(obj['acc_y'], obj['acc_z']) +
math.pi) * RAD_TO_DEG
AccYangle = (float)(math.atan2(obj['acc_z'], obj['acc_x']) +
math.pi) * RAD_TO_DEG
AccZangle = (float)(math.atan2(obj['acc_y'], obj['acc_x']) +
math.pi) * RAD_TO_DEG
# Complementary Filter
CFangleX = AA * (CFangleX + obj['gyro_x'] * DT) + (1 - AA) * AccXangle
def convert_csv_to_json(data_fil, convert=False, sortby="time"):
""" Convert CSV to JSON """
output_file = data_file.replace('.csv', '.json')
data = []
with open(data_file) as f:
for line in f:
if convert:
# JSON requires quotes
line = line.replace("'", '"')
# JSON boolean values
line = line.replace("True", "true")
line = line.replace("False", "false")
# JSON null value
line = line.replace("None", "null")
items = line.split()
if items[-1] != "*":
continue
try:
obj = json.loads(" ".join(items[2:-1]))
except json.decoder.JSONDecodeError:
# Ignore data that fails to parse JSON
continue
# Add fields if not present
if not 'class' in obj:
obj['class'] = 'DEBUG'
if not 'time' in obj:
obj['time'] = items[0]
data.append(obj)
# Resort data
if sortby != 'time':
data = sorted(data, key=lambda k: k[sortby], reverse=False)
# Add num_sat and num_used
num_sat = num_used = 0
for obj in data:
if obj['class'] == "SKY":
num_sat = num_used = 0
for s in obj['satellites']:
num_sat += 1
if s['used']:
num_used += 1
elif obj['class'] == "TPV" and not 'num_used' in obj:
obj['num_sat'] = num_sat
obj['num_used'] = num_used
# Calculate Yaw/Pitch/Roll
# Based on:
# https://github.com/ozzmaker/BerryIMU/blob/master/python-BerryIMU-gryo-accel-compass/berryIMU-simple.py
CFangleX = CFangleY = CFangleZ = 0
gyroXangle = gyroYangle = gyroZangle = 0
last_time = None
for obj in data:
# Skip over non-IMU data
if obj['class'] != "ATT":
continue
# Time Delta
if last_time is not None:
DT = (pirail.parse_time(obj['time']) - pirail.parse_time(last_time)).total_seconds()
else:
DT = 0
last_time = obj['time']
# Calculate the angles from the gyro
gyroXangle+=obj['gyro_x']*DT
gyroYangle+=obj['gyro_y']*DT
gyroZangle+=obj['gyro_z']*DT
gyroXangle = gyroXangle % 360
gyroYangle = gyroYangle % 360
gyroZangle = gyroZangle % 360
# Complementary Filter
AccXangle = math.degrees(math.atan2(obj['acc_y'], obj['acc_z']) + math.pi)
AccYangle = math.degrees(math.atan2(obj['acc_z'], obj['acc_x']) + math.pi)
AccZangle = math.degrees(math.atan2(obj['acc_y'], obj['acc_x']) + math.pi)
CFangleX = AA * (CFangleX + obj['gyro_x'] * DT) + (1 - AA) * AccXangle
CFangleY = AA * (CFangleY + obj['gyro_y'] * DT) + (1 - AA) * AccYangle
CFangleZ = AA * (CFangleZ + obj['gyro_z'] * DT) + (1 - AA) * AccZangle
if 'roll' in obj:
CFangleY = obj['roll']
else:
obj['roll'] = CFangleY
if 'pitch' in obj:
CFangleX = obj['pitch']
else:
obj['pitch'] = CFangleX
if 'yaw' in obj:
CFangleZ = obj['yaw']
else:
obj['yaw'] = CFangleZ
obj['gyro_x_angle'] = gyroXangle
obj['gyro_y_angle'] = gyroYangle
obj['gyro_z_angle'] = gyroZangle
# Normalize yaw/pitch/roll
#count = sum_roll = sum_pitch = sum_yaw = 0
#for obj in data:
# if 'roll' in obj:
# sum_roll += obj['roll']
# sum_pitch += obj['pitch']
# sum_yaw += obj['yaw']
# count += 1
#avg_roll = sum_roll / count
#avg_pitch = sum_pitch / count
#avg_yaw = sum_yaw / count
#for obj in data:
# if 'roll' in obj:
# obj['roll'] -= avg_roll
# obj['pitch'] -= avg_pitch
# obj['yaw'] -= avg_yaw
if len(data) > 0:
with open(output_file, "w") as f:
for obj in data:
timestamp = obj['time']
f.write(json.dumps(obj)+"\n")
atime = datetime.datetime.utcnow()
mtime = parse(timestamp, fuzzy=True)
os.utime(output_file, (atime.timestamp(), mtime.timestamp()))
except IndexError:
print("USAGE: %s [args] data_file.json" % sys.argv[0])
sys.exit(1)
last_speed = None
print("Latitude Longitude Seconds")
for line_no, obj in pirail.read(filename, classes=['TPV']):
if 'speed' not in obj or 'eps' not in obj or 'time' not in obj:
continue
speed = obj['speed']
eps = obj['eps']
fix_time = pirail.parse_time(obj['time'])
if last_speed is None:
last_speed = speed
last_time = fix_time
time_delta = (fix_time - last_time).total_seconds()
if speed < eps and last_speed != 0 and time_delta > TIME_THRESHOLD:
# we stopped
last_speed = 0
#print("Stopped: %s" % obj)
print("%f %f %d" % (obj['lat'], obj['lon'], time_delta))
elif speed > eps and last_speed == 0:
# started to move again
last_speed = speed
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())
for acc in acclist:
acc_time = pirail.parse_time(acc['time'])
millsec = (acc_time - time_start
).total_seconds() / time_delta.total_seconds()
acc['lat'] = last_tpv['lat'] + millsec * delta_lat
acc['lon'] = last_tpv['lon'] + millsec * delta_lon
def read_data(tc):
tc['D'] = []
queue = []
first = last = None
try:
with open(tc['data_file']+".pickle","rb") as f:
tc['D'] = pickle.load(f)
return
except:
pass
if tc['data_file'] is None:
return
with open(tc['data_file']) as f:
used = current = 0
for line in csv.reader(f, delimiter=' ', quotechar="'"):
print(line)
if line[0][0] == "#" or line[-1] != '*':
continue
if line[1] == "SKY":
obj = json.loads(" ".join(line[2:-1]))
used = count = 0
for s in obj['satellites']:
count += 1
if s['used']:
used += 1
elif line[1] == "TPV":
obj = json.loads(" ".join(line[2:-1]))
if used < 10:
continue
if not('lat' in obj and 'lon' in obj and 'time' in obj):
continue
if first is None:
first = obj
else:
last = obj
start_time = dp.parse(first['time']).timestamp()
start_lat = first['lat']
start_lon = first['lon']
end_time = dp.parse(last['time']).timestamp()
end_lat = last['lat']
end_lon = last['lon']
for q in queue:
q_time = dp.parse(q[0]).timestamp()
# time ratio
ratio = (q_time - start_time) / (end_time - start_time)
# latitiude
q_lat = (end_lat - start_lat) * ratio + start_lat
# longitude
q_lon = (end_lon - start_lon) * ratio + start_lon
#print(ratio, q_lat, q_lon, q)
obj = parse_line(q)
if obj is not None:
obj['lat'] = q_lat
obj['lon'] = q_lon
tc['D'].append(obj)
queue = []
first = last
tc['D'].append(parse_line(line))
else:
queue.append(line)
print ("entries", len(tc['D']))
# Calculate Yaw/Pitch/Roll
# Based on:
# https://github.com/ozzmaker/BerryIMU/blob/master/python-BerryIMU-gryo-accel-compass/berryIMU-simple.py
gyroXangle = gyroYangle = gyroZangle = CFangleX = CFangleY = CFangleZ = 0
last_time = None
for obj in tc['D']:
if obj['class'] != "ATT":
continue
if 'roll' in obj:
continue
if last_time is not None:
DT = (pirail.parse_time(obj['time']) - pirail.parse_time(last_time)).total_seconds()
last_time = obj['time']
else:
DT = 0
gyroXangle+=obj['gyro_x']*DT;
gyroYangle+=obj['gyro_y']*DT;
gyroZangle+=obj['gyro_z']*DT;
AccXangle = math.degrees((float) (math.atan2(obj['acc_y'],obj['acc_z'])+math.pi));
AccYangle = math.degrees((float) (math.atan2(obj['acc_z'],obj['acc_x'])+math.pi));
AccZangle = math.degrees((float) (math.atan2(obj['acc_y'],obj['acc_x'])+math.pi));
# Complementary Filter
CFangleX=AA*(CFangleX+obj['gyro_x']*DT) +(1 - AA) * AccXangle;
CFangleY=AA*(CFangleY+obj['gyro_y']*DT) +(1 - AA) * AccYangle;
CFangleZ=AA*(CFangleZ+obj['gyro_z']*DT) +(1 - AA) * AccZangle;
obj['roll'] = CFangleY
obj['pitch'] = CFangleX
obj['yaw'] = CFangleZ
# Calculate Mileage
for obj in tc['D']:
if 'mileage' not in obj:
obj['mileage'], obj['certainty'] = tc['G'].find_mileage(obj['lat'], obj['lon'])
tc['D'] = sorted(tc['D'], key=lambda k: k['mileage'], reverse=False)
with open(tc['data_file'] + ".pickle", "wb" ) as f:
pickle.dump(tc['D'], f, pickle.HIGHEST_PROTOCOL)