def readFromPort():
global myPort;
line = myPort.readline()
line = line.split()
if(need_calibrate == 1): #calibrated data
line = CalibrationManager.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]))
else:
line = (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]))
gx = float(line[0])*0.0175; gy = float(line[1])*0.0175; gz = float(line[2])*0.0175;
ax = float(line[3])/1000; ay = float(line[4])/1000; az = float(line[5])/1000;
mx = float(line[6]); my = float(line[7]); mz = float(line[8]);
#ax=0.0;ay=0.0;az=1.0;
#gx=0.0;gy=0.0;gz=gz*0.0175;
#mx=0.0;my=0.0;mz=0.0;
#print (ax, ay, az)
if(AHRS_Madgwick_algorithm == 1):
(q0, q1, q2, q3) = MadgwickAHRS.MadgwickAHRSupdate(-(gx * grad2rad), gy * grad2rad, -(gz * grad2rad), -ax, ay, az, mx, -my, -mz);
attitude_data = MadgwickAHRS.getMadAttitude();
else:
(q0, q1, q2, q3) = MahonyAHRS.MahonyAHRSupdate(-(gx * grad2rad), gy * grad2rad, -(gz * grad2rad), -ax, ay, az, mx, -my, -mz);
attitude_data = MahonyAHRS.getMahAttitude();
roll = attitude_data[0];
pitch = attitude_data[1];
yaw = attitude_data[2];
return [roll, pitch, yaw];
def readFromFile():
global lineNumberOfFile;
#global roll; global pitch; global yaw;
global myFile
## read whole file and keep data in fileData
if(lineNumberOfFile == 0):
for i, lineF in enumerate(myFile):
fileData.append(lineF)
if(lineNumberOfFile >= len(fileData)):
print "out of file"
return
## get current line
line = fileData[lineNumberOfFile]
if(len(line) <> 0):
lineNumberOfFile = lineNumberOfFile + 1
line = line.split()
if(need_calibrate == 1): #calibrated data
line = CalibrationManager.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]))
else:
line = (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]))
gx = float(line[0]); gy = float(line[1]); gz = float(line[2]);
ax = float(line[3])/1000; ay = float(line[4])/1000; az = float(line[5])/1000;
mx = float(line[6]); my = float(line[7]); mz = float(line[8]);
ax=0.0;ay=0.0;az=1.0;
gx=0.0;gy=0.0;gz=gz*0.0175;
mx=0.0;my=0.0;mz=0.0;
if(AHRS_Madgwick_algorithm == 1):
(q0, q1, q2, q3) = MadgwickAHRS.MadgwickAHRSupdate(-(gx * grad2rad), gy * grad2rad, -(gz * grad2rad), -ax, ay, az, mx, -my, -mz);
attitude_data = MadgwickAHRS.getMadAttitude();
else:
(q0, q1, q2, q3) = MahonyAHRS.MahonyAHRSupdate(-(gx * grad2rad), gy * grad2rad, -(gz * grad2rad), -ax, ay, az, mx, -my, -mz);
attitude_data = MahonyAHRS.getMahAttitude();
roll = attitude_data[0];
pitch = attitude_data[1];
yaw = attitude_data[2];
return [roll, pitch, yaw];
def add_imu_reading(self, imu_reading): (gx, gy, gz, ax, ay, az, dt_imu) = imu_reading dt = time.time() - self._last_imu_update new_quat = MahonyAHRS.MahonyAHRSupdateIMU(gx,gy,gz,ax,ay,az,dt, self.q[0],self.q[1],self.q[2],self.q[3], self.integralFB[0],self.integralFB[0],self.integralFB[0]) self._last_imu_update = time.time() self.q = new_quat[0:4] self.integralFB = new_quat[4:] try: self._last_rpy = quat2rpy(self.q) if self._listen_to_vicon: self._last_rpy[2] = self._vicon_yaw except ValueError: pass self._last_gyro = [gx,gy,gz] self._last_acc = [ax,ay,az]