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)
def __init__(self):
super().__init__()
self.bert = BertModel.from_pretrained('bert_large/')
if args.bert_freeze:
for param in self.bert.parameters():
param.requires_grad = False
self.context_dropout = nn.Dropout(args.context_dropout)
self.mention_dropout = nn.Dropout(args.mention_dropout)
self.layer_norm = nn.LayerNorm(args.bert_hidden_size)
self.multi_head_atten = MultiHeadAttention(args.bert_hidden_size,
num_heads=8,
dropout=0.1)
self.mention_char_atten = MultiHeadAttention(args.bert_hidden_size,
num_heads=8,
dropout=0.1)
self.context_lstm = BiLSTM(input_size=args.bert_hidden_size,
hidden_size=args.rnn_hidden_size,
num_layers=args.rnn_num_layers,
dropout=args.rnn_dropout,
num_dirs=args.rnn_num_dirs)
self.mention_lstm = BiLSTM(input_size=args.bert_hidden_size,
hidden_size=args.rnn_hidden_size,
num_layers=args.rnn_num_layers,
dropout=args.rnn_dropout,
num_dirs=args.rnn_num_dirs)
self.context_attn_sum = SelfAttentiveSum(args.bert_hidden_size, 100)
self.mention_attn_sum = SelfAttentiveSum(args.bert_hidden_size, 1)
self.char_cnn = CharCNN(embedding_num=len(char_vocab),
embedding_dim=args.cnn_embedding_dim,
filters=eval(args.cnn_filters),
output_dim=args.cnn_output_dim)
self.linear = nn.Linear(in_features=2 * args.bert_hidden_size +
args.cnn_output_dim,
out_features=len(labels),
bias=True)
if args.interaction:
self.mention_linear = nn.Linear(in_features=args.bert_hidden_size +
args.cnn_output_dim,
out_features=args.bert_hidden_size,
bias=True)
self.affinity_matrix = nn.Linear(args.bert_hidden_size,
args.bert_hidden_size)
self.fusion = Fusion(args.bert_hidden_size)
self.normalize = Normalize()
self.fusion_linear = nn.Linear(in_features=2 *
args.bert_hidden_size,
out_features=len(labels),
bias=True)
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 __init__(self, dataQueue, runMarker): """ Purpose: Initialize 9-axis IMU. Passed: Queue for data transfer between processes. """ self.Fusion = Fusion(timeDiff) self.offset = np.zeros(12) self.dataQueue = dataQueue self.runMarker = runMarker self.loop = asyncio.get_event_loop() self.sensor = mpu9250(self.loop)
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)
def __init__(self, classifier_config):
super(ModifiedClassifier, self).__init__()
self.dropout = nn.Dropout(classifier_config.dropout_prob)
self.relu = nn.ReLU()
self.fusion_op = Fusion()
self.wv = nn.Linear(classifier_config.input_dim_v,
classifier_config.mid_dim)
self.wq = nn.Linear(classifier_config.input_dim_q,
classifier_config.mid_dim)
self.w = nn.Linear(classifier_config.mid_dim,
classifier_config.output_dim)
self.wc = nn.Linear(classifier_config.input_dim_c,
classifier_config.mid_dim)
self.bnc = nn.BatchNorm1d(classifier_config.mid_dim)
self.bn2 = nn.BatchNorm1d(classifier_config.mid_dim)
def init():
global uart
# Initialising UART6 (Y1, Y2)
uart = UART(6, baudrate=38400, timeout=10, read_buf_len=200)
pyb.repl_uart(uart)
print('RPi UART setup')
global uart_gps
global gps_device
global gps_planner
uart_gps = UART(4, baudrate=9600, timeout=10, read_buf_len=600)
gps_device = gps.GPS(uart_gps)
gps_planner = planner.PLANNER(gps_device)
print("GPS set up")
global pid_test
pid_test = pid.PID(0.3, 0, 0.1, gps_planner.dist_bearing, 1)
# If True, uart messages are sent back normally,
# if false, only log messages are sent back.
global UART_comms_master
UART_comms_master = False
global imu, fuse, i2c
global imu_acc, imu_gyr, imu_mag
i2c = I2C(scl=Pin('Y9'), sda=Pin('Y10'))
try:
imu = MPU9250(i2c, scaling=(-1, -1, -1))
imu._mag.cal = (20.915039062, 11.880468750, 23.293359375)
imu.gyro_filter_range = 4
imu.accel_filter_range = 4
print(imu.mag.cal)
print(imu.accel.cal)
print(imu.gyro.cal)
mpu_addr = "MPU9250 id: {:X}".format(imu.mpu_addr)
cmd.send_uart(mpu_addr, True)
fuse = Fusion()
except:
cmd.send_uart("No imu detected", True)
sensors.ina_init(i2c)
lsmsensor.accelerometer_range = AccelRange.RANGE_8G
print("Accelerometer range set to: %d G" %
AccelRange.string[lsmsensor.accelerometer_range])
lsmsensor.gyro_range = GyroRange.RANGE_2000_DPS
print("Gyro range set to: %d DPS" % GyroRange.string[lsmsensor.gyro_range])
lsmsensor.accelerometer_data_rate = Rate.RATE_1_66K_HZ
# sensor.accelerometer_data_rate = Rate.RATE_12_5_HZ
print("Accelerometer rate set to: %d HZ" %
Rate.string[lsmsensor.accelerometer_data_rate])
lsmsensor.gyro_data_rate = Rate.RATE_1_66K_HZ
print("Gyro rate set to: %d HZ" % Rate.string[lsmsensor.gyro_data_rate])
fuse = Fusion()
uart = UART(4)
uart.init(115200, bits=8, parity=None, stop=1)
count = 0
while True:
imu_accel_xyz = lsmsensor.acceleration
tmpgyro = lsmsensor.gyro
tmpgyro = (math.degrees(tmpgyro[0]), math.degrees(tmpgyro[1]),
math.degrees(tmpgyro[2]))
imu_gyro_xyz = tmpgyro
imu_mag_xyz = magsensor.magnetic
#fuse.update(imu_accel_xyz, imu_gyro_xyz, imu_mag_xyz) # Note blocking mag read
fuse.update_nomag(imu_accel_xyz, imu_gyro_xyz)
# Simple test program for sensor fusion on Pyboard
# Author Peter Hinch
# V0.7 25th June 2015 Adapted for new MPU9x50 interface
import pyb
from mpu9150 import MPU9150
from fusion import Fusion
imu = MPU9150('X')
fuse = Fusion()
# 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
import micropython
from machine import I2C, Pin, Timer
from lis2hh12 import LIS2HH12, FS_2G, ODR_OFF, ODR_50HZ
from fusion import Fusion
micropython.alloc_emergency_exception_buf(100)
i2c = I2C(scl=Pin(26), sda=Pin(25))
sensor = LIS2HH12(i2c, fs=FS_2G, odr=ODR_50HZ)
imu = Fusion()
def update_imu(timer):
imu.update_nomag(sensor.read(), (0, 0, 0))
def read_imu(timer):
imu.update_nomag(sensor.read(), (0, 0, 0))
print("{:7.3f} {:7.3f} {:7.3f}".format(imu.heading, imu.pitch, imu.roll))
timer_0 = Timer(0)
timer_0.init(period=100, mode=Timer.PERIODIC, callback=update_imu)
timer_1 = Timer(1)
timer_1.init(period=1000, mode=Timer.PERIODIC, callback=read_imu)
logger_exit = TraceExit("TraceExit")
ast_ops = ASTOps("ASTOps")
HandlerRegistry.register_init_handler(ast_ops)
HandlerRegistry.register_pre_handler(logger_entry)
HandlerRegistry.register_pre_handler(prof_entry)
HandlerRegistry.register_post_handler(prof_exit)
HandlerRegistry.register_post_handler(logger_exit)
# Setup graph IR passes
preprocess = PreProcess("Graph Preprocess")
type_check = TypeCheck("Type Check")
transform = Transform("Data Type Transformation")
stage = Stage("Stage Data")
fusion = Fusion("Fuse Tasks")
dot_before = DotGraphGenerator("Dot Graph Generation", "before")
dot_after = DotGraphGenerator("Dot Graph Generation", "after")
postprocess = PostProcess("Graph Postprocess")
PassManager.register_pass(preprocess)
PassManager.register_pass(dot_before)
PassManager.register_pass(type_check)
PassManager.register_pass(transform)
PassManager.register_pass(stage)
PassManager.register_pass(fusion)
PassManager.register_pass(dot_after)
PassManager.register_pass(postprocess)
params = {'split': None}
_graph = TaskGraph()
import machine, time
if not 'i2c' in dir():
i2c = machine.I2C(0, sda=21, scl=22)
if 104 in i2c.scan():
print('motion sensor detected on i2cbus 0')
from mpu9250 import MPU9250
imu = MPU9250(i2c)
print(imu.acceleration)
print(imu.gyro)
print(imu.magnetic)
from fusion import Fusion
fuse = Fusion()
# Choose test to run
Timing = True
if Timing:
accel = imu.acceleration
gyro = imu.gyro
start = time.ticks_us() # Measure computation time only
fuse.update_nomag(accel, gyro) # 979μs on Pyboard
t = time.ticks_diff(time.ticks_us(), start)
print("Update time (uS):", t)
count = 0
while True:
import tty
import termios
#Initialization code
# Create IMU object
imu = IMU() # To select a specific I2C port, use IMU(n). Default is 1.
# Initialize IMU
imu.initialize()
# Enable accel, mag, gyro, and temperature
imu.enable_accel()
imu.enable_mag()
imu.enable_gyro()
imu.enable_temp()
fuse = Fusion()
callibrate_count = 0
#swing detection
swing = SwingDetector()
# Set range on accel, mag, and gyro
# Specify Options: "2G", "4G", "6G", "8G", "16G"
imu.accel_range("2G") # leave blank for default of "2G"
# Specify Options: "2GAUSS", "4GAUSS", "8GAUSS", "12GAUSS"
imu.mag_range("2GAUSS") # leave blank for default of "2GAUSS"
# Specify Options: "245DPS", "500DPS", "2000DPS"
import timeit
import hebi
from time import sleep
import math as m
# IMU Fusion import
rate = 5 #Hz
sys.path.append('/usr/local/home/jebruce/Projects/Python/micropython-fusion/')
from fusion import Fusion
def timeDiff(end, start):
return end - start
fuse = Fusion(timeDiff)
# Module Names on SUPERball V2
numModules = 24
SBModuleNames = (['M' + str(i + 1).zfill(2) for i in xrange(numModules)])
# Need to look into XML formatting for Hebi Gains
# sio.loadmat('defaultGains.mat')
lookup = hebi.Lookup() # Get table of all Hebi motors
sleep(2) # gives the Lookup process time to discover modules
# Displays the Hebi modules found on the network
print('Modules found on the network:')
for entry in lookup.entrylist:
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
get_data = gdata()
# Test of supplying a timediff
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:
import machine
import time
from imu import MPU6050
import deltat
from fusion import Fusion
i2c = machine.I2C(scl=machine.Pin(22), sda=machine.Pin(19))
i2c.scan()
imu = MPU6050(i2c)
fuse = Fusion()
print(imu.accel.xyz)
while (True):
time.sleep_ms(500)
print(imu.accel.xyz)
while True:
fuse.update_nomag(imu.accel.xyz, imu.gyro.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
from machine import TouchPad, Pin
t = TouchPad(Pin(4))
t.config(500)
t.read()
def touch():
if t.read() < 300:
import paho.mqtt.client as mqtt
import json
import time, pygame
import numpy as np
from operator import itemgetter
from fusion import Fusion
import quaternion
black = (0, 0, 0)
red = (255, 0, 0)
green = (0, 170, 0)
sensor_mac = "B0B448C92601"
fuse = Fusion(lambda start, end: start - end)
oldpitch = [0]
oldroll = [0]
oldheading = [0]
samples = 10
j = 1
k = 0
sumpitch = 0
sumroll = 0
sumheading = 0
c = 10
def gdata():
# Return [[ax, ay, az], [gx, gy, gz], [mx, my, mz], timestamp]
# from whatever the device supplies (in this case JSON)
with open('mpudata', 'r') as f:
line = f.readline() # An app would do a blocking read of remote data
while line:
if args.panel == '':
print('\n请注明panel名称！\n')
sys.exit(1)
bed_path = '/home/longzhao/panelSel/bed'
bed_list = os.listdir(bed_path)
bed = [os.path.join(bed_path, i) for i in bed_list if args.panel in i][0]
t0 = time.time()
print('\n' + '#' * 60)
print('python ' + ' '.join(sys.argv))
if args.S_num == 'Single': #单样本分析
if args.S_type == 'tumor': #tumor样本
QC(args.T_prefix, args.threads)
basicAnalyze(args.T_prefix, args.threads, bed)
if args.fusion: #是否分析fusion
Fusion(args.T_prefix, bed)
SingleTumorVarient(args.T_prefix, bed, args.panel, args.threads)
# 样本类型是normal时，文件名称加somatic；否则文件名称加germline
base = '.somatic'
vcf = VcfAnnotation(args.T_prefix + base, args.threads)
vcf.tumor_annovar()
vcf.hgmd_annovar()
vcf.snpEff_Anno()
if 'brca' in args.panel: # 是否用brca数据库注释
vcf.brca_annovar()
BrcaResult(args.T_prefix + base)
Hgmd_pipe(args.T_prefix + base)
Multi2Raw(args.T_prefix + base, args.S_num,
args.S_type).multi2raw()
MultiDrug(args.T_prefix + base, args.cancer,
args.S_num).multi_drug_pipe()
import termios
import math
import json
import socket
# Create IMU object
imu = IMU() # To select a specific I2C port, use IMU(n). Default is 1.
# Initialize IMU
imu.initialize()
# Enable accel, mag, gyro, and temperature
imu.enable_accel()
imu.enable_mag()
imu.enable_gyro()
imu.enable_temp()
fuse = Fusion()
callibrate_count = 0
#swing detection
swing = SwingDetector()
# Set range on accel, mag, and gyro
# Specify Options: "2G", "4G", "6G", "8G", "16G"
imu.accel_range("2G") # leave blank for default of "2G"
# Specify Options: "2GAUSS", "4GAUSS", "8GAUSS", "12GAUSS"
imu.mag_range("2GAUSS") # leave blank for default of "2GAUSS"
# Specify Options: "245DPS", "500DPS", "2000DPS"
import tty
import termios
#Initialization code
# Create IMU object
imu = IMU() # To select a specific I2C port, use IMU(n). Default is 1.
# Initialize IMU
imu.initialize()
# Enable accel, mag, gyro, and temperature
imu.enable_accel()
imu.enable_mag()
imu.enable_gyro()
imu.enable_temp()
fuse = Fusion()
callibrate_count = 0
#swing detection
swing = SwingDetector()
# Set range on accel, mag, and gyro
# Specify Options: "2G", "4G", "6G", "8G", "16G"
imu.accel_range("2G") # leave blank for default of "2G"
# Specify Options: "2GAUSS", "4GAUSS", "8GAUSS", "12GAUSS"
imu.mag_range("2GAUSS") # leave blank for default of "2GAUSS"
# Specify Options: "245DPS", "500DPS", "2000DPS"
def main():
import sys
import argparse
ts = TimeStamp()
parser = argparse.ArgumentParser()
# parser.add_argument('host', action='store',help='MAC of BT device')
parser.add_argument('-n',
action='store',
dest='count',
default=0,
type=int,
help="Number of times to loop data")
parser.add_argument('-t',
action='store',
type=float,
default=5.0,
help='time between polling')
parser.add_argument('-T',
'--temperature',
action="store_true",
default=False)
parser.add_argument('-A',
'--accelerometer',
action='store_true',
default=False)
parser.add_argument('-H', '--humidity', action='store_true', default=False)
parser.add_argument('-M',
'--magnetometer',
action='store_true',
default=False)
parser.add_argument('-B',
'--barometer',
action='store_true',
default=False)
parser.add_argument('-G',
'--gyroscope',
action='store_true',
default=False)
parser.add_argument('-K', '--keypress', action='store_true', default=False)
parser.add_argument('-L', '--light', action='store_true', default=False)
parser.add_argument('-P', '--battery', action='store_true', default=False)
parser.add_argument('-F', '--fusion', action='store_true', default=False)
parser.add_argument('--all', action='store_true', default=True)
arg = parser.parse_args(sys.argv[1:])
filename = ("logfile-" + str("%10d" % time.time()) + ".txt")
f = open(filename, "w+")
#hosts = ['A4:34:F1:F3:7F:38', 'A4:34:F1:F3:5E:73', 'B0:91:22:F6:A4:86']
#hosts = ['A4:34:F1:F3:7F:38']
#hosts = ['B0:91:22:F6:A4:86']
hosts = ['B0:91:22:F6:EB:01']
tag = list(range(len(hosts)))
fuse = list(range(len(hosts)))
for i, host in enumerate(hosts):
print('Connecting to ' + host)
tag[i] = SensorTag(host)
fuse[i] = Fusion(TimeDiff)
# Enabling selected sensors
if arg.temperature or arg.humidity or arg.all:
tag[i].humidity.enable()
if arg.barometer or arg.all:
tag[i].barometer.enable()
if arg.accelerometer or arg.all:
tag[i].accelerometer.enable()
if arg.magnetometer or arg.all:
tag[i].magnetometer.enable()
if arg.gyroscope or arg.all:
tag[i].gyroscope.enable()
if arg.battery or arg.all:
tag[i].battery.enable()
if arg.keypress or arg.all:
tag[i].keypress.enable()
tag[i].setDelegate(KeypressDelegate())
if arg.light and tag[i].lightmeter is None:
print("Warning: no lightmeter on this device")
if (arg.light or arg.all) and tag[i].lightmeter is not None:
tag[i].lightmeter.enable()
# Some sensors (e.g., temperature, accelerometer) need some time for initialization.
# Not waiting here after enabling a sensor, the first read value might be empty or incorrect.
time.sleep(1.0)
def getmag():
return tag[i].magnetometer.read()
def sw():
time.sleep(10.0)
return True
if arg.fusion:
print("Starting calibration")
fuse[i].calibrate(getmag, sw)
print("Calibration done. Magnetometer bias vector: ",
fuse[i].magbias)
counter = list(range(len(hosts)))
for i in range(len(hosts)):
counter[i] = 0
magn = list(range(len(hosts)))
acce = list(range(len(hosts)))
gyro = list(range(len(hosts)))
ligh = list(range(len(hosts)))
batt = list(range(len(hosts)))
humi = list(range(len(hosts)))
baro = list(range(len(hosts)))
line = "{:18s};{:18s};{:12s};".format("HOST", "TIME", "DELT")
if arg.magnetometer or arg.all:
line += "{:8s};{:8s};{:8s};".format("MAGX", "MAGY", "MAGZ")
if arg.accelerometer or arg.all:
line += "{:8s};{:8s};{:8s};".format("ACCX", "ACCY", "ACCZ")
if arg.gyroscope or arg.all:
line += "{:8s};{:8s};{:8s};".format("GYRX", "GYRY", "GYRZ")
if arg.light or arg.all:
line += "{:8s};".format("LIGHT")
if arg.battery or arg.all:
line += "{:8s};".format("BATT")
if arg.humidity or arg.all:
line += "{:8s};".format("TEMP")
line += "{:8s};".format("HUMI")
if arg.barometer or arg.all:
line += "{:8s};".format("BARO")
if arg.fusion:
line += "{:8s};{:8s};{:8s}".format("HEAD", "PITCH", "ROLL")
f.write(line + '\n')
t = t_prev = 0
while True:
for i in range(len(hosts)):
t = ts.now_us()
if t_prev == 0:
t_prev = t
line = "{:18s};{:18.0f};{:10.0f};".format(hosts[i], t, t - t_prev)
if arg.magnetometer or arg.all:
magn[i] = tag[i].magnetometer.read()
line += "{:8.2f};{:8.2f};{:7.2f};".format(
magn[i][0], magn[i][1], magn[i][2])
if arg.accelerometer or arg.all:
acce[i] = tag[i].accelerometer.read()
line += "{:8.2f};{:8.2f};{:8.2f};".format(
acce[i][0], acce[i][1], acce[i][2])
if arg.gyroscope or arg.all:
gyro[i] = tag[i].gyroscope.read()
line += "{:8.2f};{:8.2f};{:8.2f};".format(
gyro[i][0], gyro[i][1], gyro[i][2])
if arg.light or arg.all:
ligh[i] = tag[i].lightmeter.read()
line += "{:8.2f};".format(ligh[i])
if arg.battery or arg.all:
batt[i] = tag[i].battery.read()
line += "{:8.2f};".format(batt[i])
if arg.humidity or arg.all:
humi[i] = tag[i].humidity.read()
line += "{:8.2f};".format(humi[i][0]) # humidity
line += "{:8.2f};".format(humi[i][1]) # temperature
if arg.barometer or arg.all:
baro[i] = tag[i].barometer.read()
line += "{:10.2f};".format(baro[i][1])
if arg.fusion:
fuse[i].update(acce[i], gyro[i], magn[i], t)
counter[i] += 1
if (counter[i] > 50):
line += "{:8.2f};{:8.2f};{:8.2f}".format(
fuse[i].heading, fuse[i].pitch, fuse[i].roll)
counter[i] = 0
f.write(line + '\n')
t_prev = t
#if arg.temperature or arg.all:
# f.write(hosts[i]+";TEMP;"+str("%.3f"%t)+';'+str("%.2f"%humi[i][0])+"\n")
#if arg.humidity or arg.all:
# f.write(hosts[i]+";HUMI;"+str("%.3f"%t)+";"+str("%.2f"%humi[i][1])+"\n")
#if arg.barometer or arg.all:
# f.write(hosts[i]+";PRES;"+str("%.3f"%t)+";"+str("%.2f"%baro[i][0])+";"+str("%.2f"%baro[i][1])+"\n")
#if arg.accelerometer or arg.all:
# f.write(hosts[i]+";ACCE;"+str("%.3f"%t)+";"+str("%.2f"%acce[i][0])+";"+str("%.2f"%acce[i][1])+";"+str("%.2f"%acce[i][2])+"\n")
#if arg.magnetometer or arg.all:
# f.write(hosts[i]+";MAGN;"+str("%.3f"%t)+";"+str("%.2f"%magn[i][0])+";"+str("%.2f"%magn[i][1])+";"+str("%.2f"%magn[i][2])+"\n")
#if arg.gyroscope or arg.all:
# f.write(hosts[i]+";GYRO;"+str("%.3f"%t)+";"+str("%.2f"%gyro[i][0])+";"+str("%.2f"%gyro[i][1])+";"+str("%.2f"%gyro[i][2])+"\n")
#if (arg.light or arg.all) and ligh[i] is not None:
# f.write(hosts[i]+";LIGH;"+str("%.3f"%t)+";"+str("%.2f"%ligh[i])+"\n")
#if arg.battery or arg.all:
# f.write(hosts[i]+";BATT;"+str("%.3f"%t)+";"+str("%.2f"%batt[i])+"\n")
tag[i].waitForNotifications(1.0)
for i in range(len(hosts)):
tag[i].disconnect()
del tag[i]
f.close()
import numpy as np
#from scipy.interpolate import spline
import FaBo9Axis_MPU9250
import time
from fusion import Fusion
import pigpio
import math
SERVO_1 = 16
SERVO_2 = 20
SERVO_3 = 21
pi = pigpio.pi()
mpu9250 = FaBo9Axis_MPU9250.MPU9250()
fuse = Fusion()
# Code for external switch
# Code for Pyboard switch
#sw = pyb.Switch()
# Choose test to run
Calibrate = False
Timing = False
#pid = PID.PID(1, 0.2, 0.02)
def clamp(n, minn, maxn):
return max(min(maxn, n), minn)
# fusiontest.py Simple test program for sensor fusion on Pyboard
# Author Peter Hinch
# Released under the MIT License (MIT)
# Copyright (c) 2017 Peter Hinch
# V0.8 14th May 2017 Option for external switch for cal test. Make platform independent.
# V0.7 25th June 2015 Adapted for new MPU9x50 interface
from m5stack import lcd, buttonA, buttonB
from machine import Pin
import utime as time
from mpu9250 import MPU9250
from fusion import Fusion
from machine import I2C
i2c = I2C(sda = 21, scl = 22)
imu = MPU9250(i2c)
fuse = Fusion()
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
else:
yield json.loads(line) # Convert foreign data format.
line = f.readline() # Blocking read.
get_data = gdata()
# Test of supplying a timediff
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.
import board
import busio
# import digitalio # Use when connected over SPI
import adafruit_lsm9ds1
from fusion import Fusion
# You have a couple options for wiring this sensor, either I2C (recommended)
# or a SPI connection. Choose _one_ option below and uncomment it:
# I2C connection:
i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
fuse = Fusion()
# 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
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")
# 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)
MapRoot = args.salmap_root
test_loader = torch.utils.data.DataLoader(MyTestData(test_dataRoot,
transform=True),
batch_size=1,
shuffle=True,
num_workers=4,
pin_memory=True)
print('data already')
"""""" """"" ~~~nets~~~ """ """"""
start_epoch = 0
start_iteration = 0
model_rgb = RGBNet(cfg['nclass'])
model_depth = DepthNet(cfg['nclass'])
model_fusion = Fusion(cfg['nclass'])
if args.param is True:
model_rgb.load_state_dict(
torch.load(os.path.join(args.snapshot_root, '.pth')))
model_depth.load_state_dict(
torch.load(os.path.join(args.snapshot_root, '.pth')))
model_fusion.load_state_dict(
torch.load(os.path.join(args.snapshot_root, '.pth')))
else:
vgg19_bn = torchvision.models.vgg19_bn(pretrained=True)
model_rgb.copy_params_from_vgg19_bn(vgg19_bn)
model_depth.copy_params_from_vgg19_bn(vgg19_bn)
if cuda:
# fusiontest6.py Simple test program for 6DOF sensor fusion on Pyboard
# Author Peter Hinch
# Released under the MIT License (MIT)
# Copyright (c) 2017 Peter Hinch
# V0.8 14th May 2017 Option for external switch for cal test. Make platform independent.
# V0.7 25th June 2015 Adapted for new MPU9x50 interface
from machine import Pin
import utime as time
from mpu9150 import MPU9150
from fusion import Fusion
imu = MPU9150('X')
fuse = Fusion()
# Choose test to run
Timing = True
if Timing:
accel = imu.accel.xyz
gyro = imu.gyro.xyz
start = time.ticks_us() # Measure computation time only
fuse.update_nomag(accel, gyro) # 979μs on Pyboard
t = time.ticks_diff(time.ticks_us(), start)
print("Update time (uS):", t)
count = 0
while True:
fuse.update_nomag(imu.accel.xyz, imu.gyro.xyz)
if count % 50 == 0:
parser = argparse.ArgumentParser(
description="Integrate accel+gyro+magnet data for the human gait dataset.")
parser.add_argument('--start',
default=5,
type=int,
help='the column of the first accelerometer data')
parser.add_argument('target_file',
help='the relative filepath of the file to use')
parser.add_argument('--output_file',
help='the relative filepath at which to store the output',
default='fusion_output_gait_1.dat')
args = parser.parse_args()
timediff_func = lambda start, end: 33.0 / 1000
fuse = Fusion(timediff_func)
# Choose test to run
Timing = False
START_COL = args.start
# DEGREES_PER_RADIAN = 360 / (2*math.pi)
DEGREES_PER_RADIAN = 1.0 / (360 / (2 * math.pi))
accel_tuples = []
gyro_tuples = []
mag_tuples = []
time_stamps = []
# Starting at column 76 is LUA acc, gyro, mag
with open(args.target_file) as f:
def calibrate(mag):
fuse = Fusion()
print("Calibrating. Press switch when done.")
sw = switch
fuse.calibrate(mag, sw, lambda: time.sleep(0.1))
print(fuse.magbias)
# fusiontest.py Simple test program for sensor fusion on Pyboard
# Author Peter Hinch
# Released under the MIT License (MIT)
# Copyright (c) 2017 Peter Hinch
# V0.8 14th May 2017 Option for external switch for cal test. Make platform independent.
# V0.7 25th June 2015 Adapted for new MPU9x50 interface
from machine import Pin
import utime as time
from mpu9150 import MPU9150
from fusion import Fusion
imu = MPU9150('X')
fuse = Fusion()
# Code for external switch
switch = Pin('Y7', Pin.IN, pull=Pin.PULL_UP) # Switch to ground on Y7
def sw():
return not switch.value()
# Code for Pyboard switch
#sw = pyb.Switch()
# Choose test to run
Calibrate = True
Timing = True
'--output',
default='./results/test.png',
help='define output path of fused image')
# return an ArgumentParser object
return parser.parse_args()
def print_args(args):
print("Running with the following configuration")
# get the __dict__ attribute of args using vars() function
args_map = vars(args)
for key in args_map:
print('\t', key, '-->', args_map[key])
# add one more empty line for better output
print()
if __name__ == '__main__':
# Parse arguments
args = make_args_parser()
print_args(args)
# Read images
input_images = []
for image in args.images:
input_images.append(cv2.imread(image))
# Compute fusion image
FU = Fusion(input_images)
fusion_img = FU.fuse()
# Write fusion image
cv2.imwrite(args.output, fusion_img)