import math
|
from collections import defaultdict
|
import re
|
|
def analyze_turn_rates(filepath):
|
print(f"Analyzing turn rates in {filepath} using Z-gyro...")
|
|
# Format: $CAL,count,time,state,pwm_t,pwm_s,x,y,z,head,pitch,roll,gx,gy,gz,ax,ay,az
|
|
state_data = defaultdict(list)
|
|
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) < 15:
|
continue
|
|
try:
|
state = parts[3]
|
pwm_thr = int(parts[4])
|
pwm_str = int(parts[5])
|
|
# Get gyro Z (15th element, index 14)
|
# Unit check: Usually LSB or deg/s or rad/s. User said "Z轴角速度挺大的"
|
# If raw LSB, we need scale. If processed, maybe deg/s.
|
# Let's look at values: e.g. -1000, -1200.
|
# Assuming raw LSB for now, let's see the range.
|
gz_raw = int(parts[14])
|
|
# Also check heading change rate for verification if GPS heading is valid
|
heading = float(parts[9])
|
|
state_data[state].append({
|
'gz': gz_raw,
|
'pwm_thr': pwm_thr,
|
'pwm_str': pwm_str,
|
'heading': heading
|
})
|
|
except ValueError:
|
continue
|
|
print("\n=== Analysis of Turning Speeds ===")
|
print(f"{'State':<15} | {'PWM_Str':<8} | {'Avg Gyro Z':<10} | {'Est. Deg/s':<10}")
|
print("-" * 60)
|
|
results = []
|
|
sorted_states = sorted(state_data.keys())
|
for state in sorted_states:
|
if 'TURN' not in state:
|
continue
|
|
data = state_data[state]
|
gz_values = [d['gz'] for d in data]
|
pwm_str = data[0]['pwm_str']
|
|
if not gz_values:
|
continue
|
|
# Calculate average
|
avg_gz = sum(gz_values) / len(gz_values)
|
|
# Try to estimate actual deg/s from Gyro LSB
|
# Typical IMU scale (e.g. BMI088, MPU6050) at +/- 2000dps is ~16.4 LSB/(deg/s)
|
# at +/- 250dps is ~131 LSB/(deg/s)
|
# Let's guess scale based on user feedback or typical values.
|
# If raw values are around 1000, and it's turning visibly...
|
# 1000 / 16.4 = 60 deg/s.
|
# 1000 / 131 = 7.6 deg/s.
|
# Let's just report raw first.
|
|
results.append((state, pwm_str, avg_gz))
|
print(f"{state:<15} | {pwm_str:<8} | {avg_gz:<10.1f} | {avg_gz/16.4:<10.1f}?")
|
|
print("\n=== Proposed Mapping Table ===")
|
# Separate Left and Right turns
|
left_turns = []
|
right_turns = []
|
|
for state, pwm, gz in results:
|
if 'TURN_L' in state:
|
left_turns.append((pwm, gz))
|
elif 'TURN_R' in state:
|
right_turns.append((pwm, gz))
|
|
# Sort by PWM deviation from 1500
|
left_turns.sort(key=lambda x: x[0]) # 1000, 1100...
|
right_turns.sort(key=lambda x: x[0]) # 1600, 1700...
|
|
print("PWM | Gyro Z (Avg) | Direction")
|
for pwm, gz in left_turns:
|
print(f"{pwm:<4} | {gz:<12.1f} | Left")
|
for pwm, gz in right_turns:
|
print(f"{pwm:<4} | {gz:<12.1f} | Right")
|
|
if __name__ == "__main__":
|
analyze_turn_rates("python/calibration_20251205_105312.log")
|
