def serialEvent(): global count global myFile ## read whole file and keep data in fileData if(count == 0): for i, lineF in enumerate(myFile): fileData.append(lineF) if(count >= len(fileData)): print "out of file" return ## get current line line = fileData[count] if(len(line) <> 0): print "data line:" line = line.split() print line; print "quaternion:"; AHRS.MadgwickAHRSupdate(float(line[1])*grad2rad,float(line[2])*grad2rad,float(line[3])*grad2rad,float(line[4]),float(line[5]),float(line[6]),float(line[7]),float(line[8]),float(line[9])) #AHRS.MahonyAHRSupdate(float(line[1])*grad2rad,float(line[2])*grad2rad,float(line[3])*grad2rad,float(line[4]),float(line[5]),float(line[6]),float(line[7]),float(line[8]),float(line[9])) q = [AHRS.q0,AHRS.q1,AHRS.q2,AHRS.q3]; print q; print "euler:" MadgwickQuaternionLibrary.quaternionToEuler(q[0],q[1],q[2],q[3]) euler[0] = MadgwickQuaternionLibrary.psi; # index 0 = psi = x euler[1] = MadgwickQuaternionLibrary.theta; # index 1 = theta = y euler[2] = MadgwickQuaternionLibrary.phi; # index 2 = phi = z print euler; count = count + 1
def read_from_port(): global myPort line = myPort.readline() line = line.split() print ("raw data: " + str(line)) line = calibrate_raw_measures(int(line[1]),int(line[2]),int(line[3]),int(line[4]),int(line[5]),int(line[6]),int(line[7]),int(line[8]),int(line[9])) #print "quaternion:"; #MadgwickAHRSupdate #raw data - without calibration #AHRS.MadgwickAHRSupdate(float(line[1])*grad2rad,float(line[2])*grad2rad,float(line[3])*grad2rad,float(line[4]),float(line[5]),float(line[6]),float(line[7]),float(line[8]),float(line[9])) #calibrated data AHRS.MadgwickAHRSupdate(float(line[0])*grad2rad,float(line[1])*grad2rad,float(line[2])*grad2rad,float(line[3]),float(line[4]),float(line[5]),float(line[6]),float(line[7]),float(line[8])) #MahonyAHRSupdate #raw data - without calibration #AHRS.MahonyAHRSupdate(float(line[1])*grad2rad,float(line[2])*grad2rad,float(line[3])*grad2rad,float(line[4]),float(line[5]),float(line[6]),float(line[7]),float(line[8]),float(line[9])) #calibrated data #AHRS.MahonyAHRSupdate(float(line[0])*grad2rad,float(line[1])*grad2rad,float(line[2])*grad2rad,float(line[3]),float(line[4]),float(line[5]),float(line[6]),float(line[7]),float(line[8])) q = [AHRS.q0,AHRS.q1,AHRS.q2,AHRS.q3]; #print q; #print "euler:" MadgwickQuaternionLibrary.quaternionToEuler(q[0],q[1],q[2],q[3]) euler[0] = MadgwickQuaternionLibrary.psi; # index 0 = psi = x euler[1] = MadgwickQuaternionLibrary.theta; # index 1 = theta = y euler[2] = MadgwickQuaternionLibrary.phi; # index 2 = phi = z