From: Olivier Matz <zer0@droids-corp.org>
Date: Tue, 25 Nov 2014 19:44:56 +0000 (+0100)
Subject: IMU_Razor9DOF.py: adapt to new log format
X-Git-Url: http://git.droids-corp.org/?p=fpv.git;a=commitdiff_plain;h=78085e4d2c0b4c7c2bc762929311b3ce6572375f

IMU_Razor9DOF.py: adapt to new log format
---

diff --git a/imuboard/IMU_Razor9DOF.py b/imuboard/IMU_Razor9DOF.py
index 91351ae..97ae27f 100644
--- a/imuboard/IMU_Razor9DOF.py
+++ b/imuboard/IMU_Razor9DOF.py
@@ -12,6 +12,7 @@ import serial
 import string
 import math
 import sys
+import re
 
 from time import time
 
@@ -78,6 +79,7 @@ platform = box(length=1, height=0.05, width=1, color=color.red)
 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]*)"
 
 f = open("Serial"+str(time())+".txt", 'w')
 
@@ -88,33 +90,13 @@ cpt = 0
 t_start = time()
 while 1:
     line = ser.readline()
-    line = line.replace("!ANG:","")   # Delete "!ANG:"
-    #print line
-    f.write(line)                     # Write to the output log file
-    words = string.split(line,"\t")    # Fields split
-    #print words
-    print words
-    if len(words) != 3:
-        continue
-
-    """
-    a = float(words[0])#*grad2rad
-    b = float(words[1])#*grad2rad
-    c = float(words[2])#*grad2rad
-    d = float(words[3])#*grad2rad
-
-    platform.axis = (1, 1, 1)
-    #print a, b, c, d
-    platform.rotate(angle=a, axis = (b, c, d))
-    continue
-    """
-    try:
-        roll = float(words[0])#*grad2rad
-        pitch = float(words[1])#*grad2rad
-        yaw = float(words[2])#*grad2rad
-    except:
-        print "Invalid line"
+    #line = line.replace("!ANG:","")   # Delete "!ANG:"
+    m = re.match(".*IMU received %s %s %s"%(INT, INT, INT), line)
+    if m == None:
+        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:
@@ -139,8 +121,8 @@ while 1:
     cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0)
     cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0)
     arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0)
-    L1.text = str(float(words[0]))
-    L2.text = str(float(words[1]))
-    L3.text = str(float(words[2]))
+    L1.text = str(roll)
+    L2.text = str(pitch)
+    L3.text = str(yaw)
 ser.close()
 f.close()