def setup():
senMPU()
global ori
ori = Fusion()
Timing = True
Calibrate = False
def getmag():
senMPU()
return mag
if Calibrate:
print("Calibrant. Presionar KEY un cop acabat.")
ori.calibrate(getmag, sw, lambda: pyb.delay(100))
print(ori.magbias)
if Timing:
'''
mag = mpu.magnetic # Don't include blocking read in time
accel = mpu.acceleration # or i2c
gyro = mpu.gyro
'''
senMPU()
start = time.ticks_us() # Measure computation time only
ori.update(accel, gyro, mag, 0.005) # 1.97mS on Pyboard
t = time.ticks_diff(time.ticks_us(), start)
print("Temps de mostreig (uS):", t)
class OrientationSensor(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.Fusion = Fusion(timediff)
self.MPU = mpu9250()
def run(self):
while (True):
#print(time.time())
self.Fusion.update(self.MPU.accel, self.MPU.gyro, self.MPU.mag,
datetime.datetime.now())
#self.Fusion.heading/pitch/roll for the data
#print(time.time())
time.sleep(0.01)
from fusion import Fusion
import pandas as pd
ts = 0.01
fuse = Fusion(lambda x, y: y - x)
data = pd.read_table("data/sensor.txt")
rows = data.iterrows()
df = pd.DataFrame(columns=['heading', 'pitch', 'roll'])
for count, row in rows:
fuse.update(row[14:17], row[17:20], row[20:23], ts=ts * count)
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(
fuse.heading, fuse.pitch, fuse.roll))
df = df.append(
{
'heading': fuse.heading,
'pitch': fuse.pitch,
'roll': fuse.roll
},
ignore_index=True)
df.to_csv("data/output.txt")
class TCCompass:
def __init__(self, imu, timeout=1000):
# load configuration file
with open('calibration.conf', mode='r') as f:
mpu_conf = f.readline()
mcnf = pickle.loads(mpu_conf)
self.MPU_Centre = mcnf[0][0]
self.MPU_TR = mcnf[1]
self.counter = pyb.millis()
self.timeout = timeout
# setup MPU9250
self.imu = imu
self.gyrobias = (-1.394046, 1.743511, 0.4735878)
# setup fusions
self.fuse = Fusion()
self.update()
self.hrp = [self.fuse.heading, self.fuse.roll, self.fuse.pitch]
def process(self):
self.update()
if pyb.elapsed_millis(self.counter) >= self.timeout:
self.hrp = [self.fuse.heading, self.fuse.roll, self.fuse.pitch]
self.counter = pyb.millis()
return True
return False
def update(self):
accel = self.imu.accel.xyz
gyroraw = self.imu.gyro.xyz
gyro = [
gyroraw[0] - self.gyrobias[0], gyroraw[1] - self.gyrobias[1],
gyroraw[2] - self.gyrobias[2]
]
self.mag = self.imu.mag.xyz
self.fuse.update(
accel, gyro, self.adjust_mag(self.mag, self.MPU_Centre,
self.MPU_TR))
def getmag(self):
return self.imu.mag.xyz
@staticmethod
def adjust_mag(mag, centre, TR):
mx_raw = mag[0] - centre[0]
my_raw = mag[1] - centre[1]
mz_raw = mag[2] - centre[2]
mx = mx_raw * TR[0][0] + my_raw * TR[0][1] + mz_raw * TR[0][2]
my = mx_raw * TR[1][0] + my_raw * TR[1][1] + mz_raw * TR[1][2]
mz = mx_raw * TR[2][0] + my_raw * TR[2][1] + mz_raw * TR[2][2]
return (mx, my, mz)
def Calibrate(self):
print("Calibrating. Press switch when done.")
sw = pyb.Switch()
self.fuse.calibrate(self.getmag, sw, lambda: pyb.delay(100))
print(self.fuse.magbias)
def gyrocal(self):
xa = 0
ya = 0
za = 0
for x in range(0, 100):
xyz = self.imu.gyro.xyz
xa += xyz[0]
ya += xyz[1]
za += xyz[2]
pyb.delay(1000)
print(xa / 100, ya / 100, za / 100)
@property
def heading(self):
a = self.hrp[0]
if a < 0:
a += 360
return a
@property
def roll(self):
return self.hrp[1]
@property
def pitch(self):
return self.hrp[2]
@property
def output(self):
outstring = [
CMP("1,{},{}".format(self.mag, self.hrp[0])).msg,
HDG(self.hrp[0]).msg,
XDR(self.hrp[2], self.hrp[1])
]
for x in range(0, 2):
outstring[x] = outstring[x].replace('(', '')
outstring[x] = outstring[x].replace(')', '')
return outstring
pi.set_servo_pulsewidth(SERVO_2, 1500)
pi.set_servo_pulsewidth(SERVO_3, 1500)
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
while True:
start = time.time()
accel = mpu9250.readAccel()
gyro = mpu9250.readGyro()
mag = mpu9250.readMagnet()
accel_ali = (accel['x'], accel['y'], accel['z'])
gyro_ali = (gyro['x'], gyro['y'], gyro['z'])
mag_ali = (mag['x'], mag['y'], mag['z'])
fuse.update(accel_ali, gyro_ali, mag_ali) # Note blocking mag read
magnitude = math.sqrt((map_angle_pitch(fuse.pitch) *
map_angle_pitch(fuse.pitch)) +
(map_angle_roll(fuse.roll) *
(map_angle_roll(fuse.roll))))
theta = math.atan2(map_angle_pitch(fuse.pitch), map_angle_roll(fuse.roll))
feedback_angle_magnitude = magnitude
pid_angle_magnitude.update(feedback_angle_magnitude)
pid_gyro_magnitude.SetPoint = pid_angle_magnitude.output
feedback_gyro_magnitude = math.sqrt(gyro['x'] * gyro['x'] +
gyro['y'] * gyro['y'])
pid_gyro_magnitude.update(feedback_gyro_magnitude)
output_magnitude = (pid_gyro_magnitude.output)
Vx = -(math.cos(theta) * output_magnitude)
Vy = -(math.sin(theta) * output_magnitude)
speed_wheel_1 = map((-Vx))
# Choose test to run
Calibrate = True
Timing = False
def getmag(): # Return (x, y, z) tuple (blocking read)
return imu.mag.xyz
if Calibrate:
print("Calibrating. Press switch when done.")
sw = pyb.Switch()
fuse.calibrate(getmag, sw, lambda : pyb.delay(100))
print(fuse.magbias)
if Timing:
mag = imu.mag.xyz # Don't include blocking read in time
accel = imu.accel.xyz # or i2c
gyro = imu.gyro.xyz
start = pyb.micros()
fuse.update(accel, gyro, mag) # 1.65mS on Pyboard
t = pyb.elapsed_micros(start)
print("Update time (uS):", t)
count = 0
while True:
fuse.update(imu.accel.xyz, imu.gyro.xyz, imu.mag.xyz) # Note blocking mag read
if count % 50 == 0:
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(fuse.heading, fuse.pitch, fuse.roll))
pyb.delay(20)
count += 1
float(datapts[START_COL + 5]) * DEGREES_PER_RADIAN)
mag = (float(datapts[START_COL + 6]), float(datapts[START_COL + 7]),
float(datapts[START_COL + 8]))
curr_time = curr_time + (1.0 / 100) # Assume 100 hz as reported
timeval = curr_time
accel_tuples.append(accel)
gyro_tuples.append(gyro)
mag_tuples.append(mag)
time_stamps.append(timeval)
print("Accel tuples length: ", len(accel_tuples))
count = 0
quat_list = []
while count < len(accel_tuples):
# fuse.update(imu.accel.xyz, imu.gyro.xyz, imu.mag.xyz) # Note blocking mag read
fuse.update(accel_tuples[count], gyro_tuples[count], mag_tuples[count],
time_stamps[count]) # Note blocking mag read
quat_list.append(fuse.q)
count += 1
count = 0
with open(args.output_file, "w") as f:
while count < len(quat_list):
f.write("{:.3f} {:.3f} {:.3f} {:.3f}\n".format(quat_list[count][0],
quat_list[count][1],
quat_list[count][2],
quat_list[count][3]))
if count % 50 == 0:
# print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(fuse.heading, fuse.pitch, fuse.roll))
print(
"Quaternion orientation: w={:7.3f} x={:7.3f} y={:7.3f} z={:7.3f}"
.format(quat_list[count][0], quat_list[count][1],
subset = data[0:50]
def get_AGM(row, key): #takes one time point and one IMU name
acc = (row[key + ' accX'], row[key + ' accY'], row[key + ' accZ'])
gyro = (row[key + ' gyroX'], row[key + ' gyroY'], row[key + ' gyroZ'])
mag = (row[key + ' magneticX'], row[key + ' magneticY'],
row[key + ' magneticZ'])
return acc, gyro, mag
#y = [] #used for testing/looking at plots
for t in subset:
back = get_AGM(t, 'BACK')
RUA = get_AGM(t, 'RUA')
RLA = get_AGM(t, 'RLA')
LUA = get_AGM(t, 'LUA')
LLA = get_AGM(t, 'LLA')
fuse_back.update(back[0], back[1], back[2], t['MILLISEC'])
fuse_RUA.update(RUA[0], RUA[1], RUA[2], t['MILLISEC'])
#y.append(fuse_RUA.roll)
fuse_RLA.update(RLA[0], RLA[1], RLA[2], t['MILLISEC'])
fuse_LUA.update(LUA[0], LUA[1], LUA[2], t['MILLISEC'])
fuse_LLA.update(LLA[0], LLA[1], LLA[2], t['MILLISEC'])
#for reference
#import matplotlib.pyplot as plt
#plt.plot([s['MILLISEC'] for s in subset], y)
#plt.show()
accel = (float(imu.ax), float(imu.ay), float(imu.az))
gyro = (float(imu.gx), float(imu.gy), float(imu.gz))
mag = (float(imu.mx), float(imu.my), float(imu.mz))
# Print the results
print "Accel: " + str(imu.ax) + ", " + str(imu.ay) + ", " + str(imu.az)
print "Mag: " + str(imu.mx) + ", " + str(imu.my) + ", " + str(imu.mz)
print "Gyro: " + str(imu.gx) + ", " + str(imu.gy) + ", " + str(imu.gz)
print "Temperature: " + str(imu.temp)
outFile_accel.write("{:7.3f},{:7.3f},{:7.3f},{:d}\n".format(imu.ax, imu.ay, imu.az,is_swinging()))
data = {"AX":str(imu.ax),"AY":str(imu.ay),"AZ":str(imu.az)}
#passes the data to the fusion data for yaw pitch and roll data
fuse.update(accel,gyro, mag)
# print("I am printing the fuse data", fuse.roll)
print("\n")
callibrate_count += 1
#initial callibration
if (not callibrated or callibrate_count < 50) and (abs(_heading - fuse.heading) > 1.0 or abs(_pitch - fuse.pitch) > 1.0 or abs(_roll - fuse.roll) > 1.0):
#_heading = (_heading * callibrate_count + fuse.heading) / (callibrate_count + 1)
#_pitch = (_pitch * callibrate_count + fuse.pitch) / (callibrate_count + 1)
#_roll = (_roll * callibrate_count + fuse.roll) / (callibrate_count + 1)
_heading = fuse.heading
_pitch = fuse.pitch
_roll = fuse.roll
else:
callibrated = True
heading = fuse.heading - _heading
#gather the accel results for the fusion algorithm
accel = (float(imu.ax), float(imu.ay), float(imu.az))
gyro = (float(imu.gx), float(imu.gy), float(imu.gz))
mag = (float(imu.mx), float(imu.my), float(imu.mz))
#Print the results
print "Accel: " + str(imu.ax) + ", " + str(imu.ay) + ", " + str(imu.az)
print "Mag: " + str(imu.mx) + ", " + str(imu.my) + ", " + str(imu.mz)
print "Gyro: " + str(imu.gx) + ", " + str(imu.gy) + ", " + str(imu.gz)
outFile_accel.write("{:7.3f},{:7.3f},{:7.3f},{:d}\n".format(
imu.ax, imu.ay, imu.az, is_swinging()))
data = {"AX": str(imu.ax), "AY": str(imu.ay), "AZ": str(imu.az)}
#passes the data to the fusion data for yaw pitch and roll data
fuse.update(accel, gyro, mag)
# print("I am printing the fuse data", fuse.roll)
print("\n")
callibrate_count += 1
#initial callibration
if (not callibrated or
callibrate_count < 50) and (abs(_heading - fuse.heading) > 1.0
or abs(_pitch - fuse.pitch) > 1.0
or abs(_roll - fuse.roll) > 1.0):
#_heading = (_heading * callibrate_count + fuse.heading) / (callibrate_count + 1)
#_pitch = (_pitch * callibrate_count + fuse.pitch) / (callibrate_count + 1)
#_roll = (_roll * callibrate_count + fuse.roll) / (callibrate_count + 1)
_heading = fuse.heading
_pitch = fuse.pitch
_roll = fuse.roll
# def test(s):
# """ displays a scrolling text with temperature, pressure, humidity information"""
# while (1):
# s.matrix.write("{0:.1f}deg / {1:.1f}%rH / {2:.0f}hPa".format(
# s.temperature, s.humidity,s.pressure))
# pyb.delay(2000)
#
# test(s)
import pyb
from pyb import I2C
from balancing import Balance
from fusion import Fusion
i2c = I2C(1, I2C.MASTER)
i2c.deinit()
i2c.init(I2C.MASTER)
from sensehat import uSenseHAT
s = uSenseHAT(i2c)
fuse = Fusion()
balance = Balance(s.matrix)
while (1):
for g,a in s.lsm.iter_accel_gyro():
fuse.update(a, g, s.lsm.read_magnet())
balance.update(fuse.heading, fuse.pitch, fuse.roll)
pyb.delay(5)
if is_micropython:
def TimeDiff(start, end):
return time.ticks_diff(start, end)/1000000
else: # Cpython cheat: test data does not roll over
def TimeDiff(start, end):
return (start - end)/1000000
# Expect a timestamp. Use supplied differencing function.
fuse = Fusion(TimeDiff)
def getmag(): # Return (x, y, z) magnetometer vector.
imudata = next(get_data)
return imudata[2]
print(intro)
fuse.calibrate(getmag, lambda : not calibrate, lambda : time.sleep(0.01))
print('Cal done. Magnetometer bias vector:', fuse.magbias)
print('Heading Pitch Roll')
count = 0
while True:
try:
data = next(get_data)
except StopIteration: # A file is finite.
break # A real app would probably run forever.
fuse.update(*data)
if count % 25 == 0:
print("{:8.3f} {:8.3f} {:8.3f}".format(fuse.heading, fuse.pitch, fuse.roll))
time.sleep(0.02)
count += 1
# SPI connection:
#spi = busio.SPI(board.SCK, MOSI=board.MOSI, MISO=board.MISO)
#xgcs = digitalio.DigitalInOut(board.D5) # Pin connected to XGCS (accel/gyro chip select).
#mcs = digitalio.DigitalInOut(board.D6) # Pin connected to MCS (magnetometer chip select).
#sensor = adafruit_lsm9ds1.LSM9DS1_SPI(spi, xgcs, mcs)
# Main loop will read the acceleration, magnetometer, gyroscope, Temperature
# values every second and print them out.
while True:
# Read acceleration, magnetometer, gyroscope, temperature from sensor
accel_x, accel_y, accel_z = sensor.accelerometer
mag_x, mag_y, mag_z = sensor.magnetometer
gyro_x, gyro_y, gyro_z = sensor.gyroscope
temp = sensor.temperature
# update sensor fusion
fuse.update(sensor.accelerometer, sensor.gyroscope, sensor.magnetometer)
# Print values.
print('Acceleration (m/s^2): ({0:0.3f},{1:0.3f},{2:0.3f})'.format(accel_x, accel_y, accel_z))
print('Magnetometer (gauss): ({0:0.3f},{1:0.3f},{2:0.3f})'.format(mag_x, mag_y, mag_z))
print('Gyroscope (degrees/sec): ({0:0.3f},{1:0.3f},{2:0.3f})'.format(gyro_x, gyro_y, gyro_z))
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(fuse.heading, fuse.pitch, fuse.roll))
print('Temperature: {0:0.3f}C'.format(temp))
# Delay for a second.
time.sleep(1.0)
imu_data = np.loadtxt(dir_name + 'imu.txt', delimiter=',')
mag_cali_data = np.loadtxt('/home/steve/Data/II/21/imu.txt', delimiter=',')
fuse = Fusion()
fuse.own_calibrate(mag_cali_data[:, 7:10])
attitude = np.zeros([imu_data.shape[0], 3])
for i in range(imu_data.shape[0]):
bb_times = 5
if i is 0:
bb_times = 100
for j in range(bb_times):
fuse.update(imu_data[i, 1:4], imu_data[i, 4:7], imu_data[i, 7:10],
0.005 / float(bb_times))
# fuse.update(imu_data[i, 1:4], imu_data[i, 4:7], imu_data[i, 7:10],0.001)
# time.sleep(0.005)
if i % 200 == 0:
print("attitude:{:7.3f} {:7.3f} {:7.3f}".format(
fuse.heading, fuse.pitch, fuse.roll))
attitude[i, 0] = fuse.heading
attitude[i, 1] = fuse.pitch
attitude[i, 2] = fuse.roll
attitude[:200, :] = attitude[300, :]
imu_data[:, 7:10] = attitude / 180.0 * np.pi
np.savetxt(dir_name + 'imu_att.txt', imu_data, delimiter=',')
print('mag bias :', fuse.magbias)
plt.figure()
def getmag(): # Return (x, y, z) tuple (blocking read)
return imu.mag.xyz
if buttonA.isPressed():
print("Calibrating. Press switch when done.")
fuse.calibrate(getmag, BtnB.press, lambda : time.sleep(0.1))
print(fuse.magbias)
if False:
mag = imu.magnetic # Don't include blocking read in time
accel = imu.acceleration # or i2c
gyro = imu.gyro
start = time.ticks_us() # Measure computation time only
fuse.update(accel, gyro, mag) # 1.97mS on Pyboard
t = time.ticks_diff(time.ticks_us(), start)
print("Update time (uS):", t)
lcd.clear()
lcd.font(lcd.FONT_Small)
lcd.setTextColor(lcd.WHITE, lcd.BLACK)
count = 0
while not buttonA.isPressed():
accel = imu.acceleration
gyro = imu.gyro
mag = imu.magnetic
fuse.update(accel, gyro, mag) # Note blocking mag read
Timing = True
def getmag(): # Return (x, y, z) tuple (blocking read)
return imu.mag.xyz
if Calibrate:
print("Calibrating. Press switch when done.")
fuse.calibrate(getmag, sw, lambda: pyb.delay(100))
print(fuse.magbias)
if Timing:
mag = imu.mag.xyz # Don't include blocking read in time
accel = imu.accel.xyz # or i2c
gyro = imu.gyro.xyz
start = time.ticks_us() # Measure computation time only
fuse.update(accel, gyro, mag) # 1.97mS on Pyboard
t = time.ticks_diff(time.ticks_us(), start)
print("Update time (uS):", t)
count = 0
while True:
fuse.update(imu.accel.xyz, imu.gyro.xyz,
imu.mag.xyz) # Note blocking mag read
if count % 50 == 0:
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(
fuse.heading, fuse.pitch, fuse.roll))
time.sleep_ms(20)
count += 1
def TimeDiff(start, end):
return (start - end) / 1000000
# Expect a timestamp. Use supplied differencing function.
fuse = Fusion(TimeDiff)
def getmag(): # Return (x, y, z) magnetometer vector.
imudata = next(get_data)
return imudata[2]
print(intro)
fuse.calibrate(getmag, lambda: not calibrate, lambda: time.sleep(0.01))
print('Cal done. Magnetometer bias vector:', fuse.magbias)
print('Heading Pitch Roll')
count = 0
while True:
try:
data = next(get_data)
except StopIteration: # A file is finite.
break # A real app would probably run forever.
fuse.update(*data)
if count % 25 == 0:
print("{:8.3f} {:8.3f} {:8.3f}".format(fuse.heading, fuse.pitch,
fuse.roll))
time.sleep(0.02)
count += 1