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)
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)
