import math
|
|
def check_jumps(filepath):
|
print(f"Checking {filepath} for GPS jumps...")
|
|
prev_x = None
|
prev_y = None
|
prev_time = None
|
|
jumps = []
|
|
with open(filepath, 'r', encoding='utf-8', errors='ignore') as f:
|
lines = f.readlines()
|
|
for i, line in enumerate(lines):
|
line = line.strip()
|
if not line.startswith('$CAL'):
|
continue
|
|
parts = line.split(',')
|
# Expected format has about 18 fields
|
if len(parts) < 10:
|
continue
|
|
try:
|
# Extract time and coordinates
|
# $CAL,count,time,state,pwm_t,pwm_s,x,y,z,...
|
time_ms = int(parts[2])
|
x = float(parts[6])
|
y = float(parts[7])
|
|
if prev_x is not None and prev_y is not None:
|
dx = x - prev_x
|
dy = y - prev_y
|
dist = math.sqrt(dx*dx + dy*dy)
|
dt = (time_ms - prev_time) / 1000.0
|
|
if dt > 0:
|
speed = dist / dt
|
# Threshold: e.g., speed > 3.0 m/s (10.8 km/h) is suspicious for a mower
|
# or just absolute jump > 0.3m in one frame (approx 50ms) -> 6m/s
|
if speed > 5.0 or dist > 0.5:
|
jumps.append({
|
'line': i+1,
|
'time': time_ms,
|
'prev_time': prev_time,
|
'dist': dist,
|
'speed': speed,
|
'pos': (x, y),
|
'prev_pos': (prev_x, prev_y)
|
})
|
|
prev_x = x
|
prev_y = y
|
prev_time = time_ms
|
|
except ValueError:
|
continue
|
|
if not jumps:
|
print("No significant GPS jumps detected.")
|
else:
|
print(f"Detected {len(jumps)} potential GPS jumps:")
|
for jump in jumps[:20]: # Show first 20
|
print(f" Line {jump['line']}: Time {jump['time']}ms, Jump {jump['dist']:.3f}m, Speed {jump['speed']:.2f}m/s, Pos {jump['prev_pos']} -> {jump['pos']}")
|
if len(jumps) > 20:
|
print(f" ... and {len(jumps)-20} more.")
|
|
if __name__ == "__main__":
|
check_jumps("python/calibration_20251205_105312.log")
|
