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 calc_bimu_orientation(sample): #uses bIMU.py module, appends heading, x, y, time to or_pos, subtracting initial orientation, and compensating for heading- standardizing along x and y vectors
global init_position
global or_pos
bIMU_data = bIMU.get_orientation(sample)
#data = round(bIMU_data[0]-init_position[0],dec_places),round(bIMU_data[1]-init_position[0],dec_places),round(bIMU_data[2]-init_position[0],dec_places),time()
data = round(bIMU_data[0],dec_places),round(bIMU_data[1],dec_places),round(bIMU_data[2],dec_places),time()
or_pos.append(data)
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];
from __future__ import print_function import MadgwickAHRS ax = 0. ay = 2. az = 5. gx = -6.2 gy = 7.8 gz = 0. mx = 0.0 my = 0. mz = 24. MadgwickAHRS.MadgwickAHRSupdate(gx, gy, gz, ax, ay, az, mx, my, mz) cv = MadgwickAHRS.cvar q0, q1, q2, q3 = cv.q0, cv.q1, cv.q2, cv.q3 print(q0, q1, q2, q3)
import MadgwickAHRS as ahrs
import time
sample_rate = 100
while True:
global sample_rate
t1 = time.time()
data = ahrs.get_orientation(sample_rate)
print "Orientation:",
print data[0],
print data[1],
print data[2]
t2 = time.time()
sample_rate = 1.0 / (t2 - t1)
# -*- coding: utf-8 -*-
import time
import math
import MadgwickAHRS
import pandas as pd
q = MadgwickAHRS.MadgwickAHRS()
data = pd.read_csv('sample.csv',
names=('gx', 'gy', 'gz', 'ax', 'ay', 'az', 'mx', 'my',
'mz'))
for i in range(len(data.index)):
start = time.time()
acc = (data['ax'][i], data['ay'][i], data['az'][i])
gyro = (data['gx'][i], data['gy'][i], data['gz'][i])
meg = (data['mx'][i], data['my'][i], data['mz'][i])
time.sleep(0.1)
print(acc)
print(gyro)
print(meg)
q.MadgwickAHRSupdate(gx=gyro[0] * math.pi / 180.0,
gy=gyro[1] * math.pi / 180.0,
gz=gyro[2] * math.pi / 180.0,
ax=acc[0],
ay=acc[1],
az=acc[2],
mx=meg[0] + 40,
my=meg[1] + 40,
mz=meg[2] + 40)
