From 08c6f54f5988fb8b52187cc183f479208532480d Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Wed, 4 Mar 2015 21:31:28 +0100 Subject: [PATCH] IMU_Razor9DOF: allow to match other formats (logs) --- imuboard/IMU_Razor9DOF.py | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/imuboard/IMU_Razor9DOF.py b/imuboard/IMU_Razor9DOF.py index 97ae27f..6b1c168 100644 --- a/imuboard/IMU_Razor9DOF.py +++ b/imuboard/IMU_Razor9DOF.py @@ -80,6 +80,7 @@ p_line = box(length=1,height=0.08,width=0.1,color=color.yellow) plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1) INT = "([-+]?[0-9][0-9]*)" +FLOAT = "([-+]?[0-9]*\.?[0-9]+)" f = open("Serial"+str(time())+".txt", 'w') @@ -92,17 +93,31 @@ while 1: line = ser.readline() #line = line.replace("!ANG:","") # Delete "!ANG:" m = re.match(".*IMU received %s %s %s"%(INT, INT, INT), line) - if m == None: + if m: + roll, pitch, yaw = map(lambda x: float(x)*(math.pi/18000), m.groups()) + print roll, pitch, yaw + if not m: + m = re.match("%s \| gyro %s %s %s \| accel %s %s %s \| magnet %s %s %s \| angles %s %s %s"%( + INT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT), line) + if m: + #print m.groups() + roll, pitch, yaw = map(lambda x: float(x), m.groups()[10:13]) + #print roll, pitch, yaw + mx, my, mz = map(lambda x: float(x), m.groups()[7:10]) + norm = mx * mx + my * my + mz * mz + mx /= norm + my /= norm + mz /= norm + print ("%4.4f %4.4f %4.4f"%(mx * 1000, my * 1000, mz * 1000)) + if not m: print line continue - roll, pitch, yaw = map(lambda x: float(x)*(math.pi/18000), m.groups()) - print roll, pitch, yaw cpt += 1 if cpt > 10: t = time() - t_start t_start = time() - print "XXX", cpt/t + #print "XXX", cpt/t cpt = 0 axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch)) -- 2.39.5