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)