IMU_Razor9DOF: allow to match other formats (logs)

diff --git a/imuboard/IMU_Razor9DOF.py b/imuboard/IMU_Razor9DOF.py
index 97ae27f5d0c6774b30b8f742dd15d52ce5401931..6b1c1687f50786fd379beb0a31b20342c2e3825d 100644 (file)
--- 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)\r
 \r
 INT = "([-+]?[0-9][0-9]*)"\r
+FLOAT = "([-+]?[0-9]*\.?[0-9]+)"\r
 \r
 f = open("Serial"+str(time())+".txt", 'w')\r
 \r
@@ -92,17 +93,31 @@ while 1:
     line = ser.readline()\r
     #line = line.replace("!ANG:","")   # Delete "!ANG:"\r
     m = re.match(".*IMU received %s %s %s"%(INT, INT, INT), line)\r
-    if m == None:\r
+    if m:\r
+        roll, pitch, yaw = map(lambda x: float(x)*(math.pi/18000), m.groups())\r
+        print roll, pitch, yaw\r
+    if not m:\r
+        m = re.match("%s \| gyro %s %s %s \| accel %s %s %s \| magnet %s %s %s \| angles %s %s %s"%(\r
+            INT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT), line)\r
+    if m:\r
+        #print m.groups()\r
+        roll, pitch, yaw = map(lambda x: float(x), m.groups()[10:13])\r
+        #print roll, pitch, yaw\r
+        mx, my, mz = map(lambda x: float(x), m.groups()[7:10])\r
+        norm = mx * mx + my * my + mz * mz\r
+        mx /= norm\r
+        my /= norm\r
+        mz /= norm\r
+        print ("%4.4f %4.4f %4.4f"%(mx * 1000, my * 1000, mz * 1000))\r
+    if not m:\r
         print line\r
         continue\r
-    roll, pitch, yaw = map(lambda x: float(x)*(math.pi/18000), m.groups())\r
-    print roll, pitch, yaw\r
 \r
     cpt += 1\r
     if cpt > 10:\r
         t = time() - t_start\r
         t_start = time()\r
-        print "XXX", cpt/t\r
+        #print "XXX", cpt/t\r
         cpt = 0\r
 \r
     axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch))\r