import math
|
from collections import defaultdict
|
|
def analyze_speeds(filepath):
|
print(f"Analyzing speeds in {filepath}...")
|
|
state_data = defaultdict(list)
|
|
prev_x = None
|
prev_y = None
|
prev_time = None
|
prev_heading = None
|
|
with open(filepath, 'r', encoding='utf-8', errors='ignore') as f:
|
lines = f.readlines()
|
|
for line in lines:
|
line = line.strip()
|
if not line.startswith('$CAL'):
|
continue
|
|
parts = line.split(',')
|
if len(parts) < 10:
|
continue
|
|
try:
|
time_ms = int(parts[2])
|
state = parts[3]
|
pwm_thr = int(parts[4])
|
pwm_str = int(parts[5])
|
x = float(parts[6])
|
y = float(parts[7])
|
heading = float(parts[9])
|
|
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
|
|
# Calculate yaw rate
|
d_head = heading - prev_heading
|
if d_head > 180: d_head -= 360
|
if d_head < -180: d_head += 360
|
yaw_rate = d_head / dt # deg/s
|
|
if 0 < dt < 0.5:
|
state_data[state].append({
|
'speed': speed,
|
'yaw_rate': yaw_rate,
|
'pwm_thr': pwm_thr,
|
'pwm_str': pwm_str,
|
'heading': heading
|
})
|
|
prev_x = x
|
prev_y = y
|
prev_time = time_ms
|
prev_heading = heading
|
|
except ValueError:
|
continue
|
|
print("\n=== Average Speeds per State ===")
|
sorted_states = sorted(state_data.keys())
|
|
for state in sorted_states:
|
if 'REST' in state or 'IDLE' in state or 'BRAKE' in state or 'WARMUP' in state:
|
continue
|
|
speeds = [d['speed'] for d in state_data[state]]
|
yaw_rates = [d['yaw_rate'] for d in state_data[state]]
|
pwms_t = [d['pwm_thr'] for d in state_data[state]]
|
# pwm_str is 6th column in log, but 4th in parts (0-based index: 0=$CAL, 1=cnt, 2=time, 3=state, 4=thr, 5=str)
|
# Wait, in the code: pwm_thr = int(parts[4]), pwm_str = int(parts[5])
|
pwms_s = [d['pwm_str'] for d in state_data[state]]
|
|
if not speeds:
|
continue
|
|
avg_speed = sum(speeds) / len(speeds)
|
avg_yaw = sum(yaw_rates) / len(yaw_rates)
|
avg_pwm_t = sum(pwms_t) / len(pwms_t)
|
avg_pwm_s = sum(pwms_s) / len(pwms_s)
|
count = len(speeds)
|
|
print(f"{state:<15} | Thr: {avg_pwm_t:.0f} | Str: {avg_pwm_s:.0f} | Speed: {avg_speed:.3f} m/s | Yaw: {avg_yaw:.3f} deg/s | N: {count}")
|
|
if __name__ == "__main__":
|
analyze_speeds("python/calibration_20251205_105312.log")
|
