示例#1
0
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)
示例#2
0
    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)
示例#3
0
    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) 
示例#5
0
    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)
示例#7
0
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)
示例#8
0
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
示例#10
0
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)
示例#11
0
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()
示例#12
0
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:
示例#15
0
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
示例#16
0
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:
示例#17
0
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()
示例#20
0
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"
示例#21
0
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"
示例#22
0
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)
示例#24
0
# 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.
示例#26
0
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
示例#27
0
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")
示例#28
0
文件: main.py 项目: hoihu/projects
# 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)
示例#29
0
    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:
示例#30
0
# 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:
示例#31
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:
示例#32
0
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)