예제 #1
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)
예제 #2
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)
예제 #3
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")
예제 #4
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
예제 #5
0
    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))
예제 #6
0
# 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
예제 #7
0
                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],
예제 #8
0
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()
예제 #9
0
        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
예제 #11
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)
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
예제 #13
0
 
# 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)
예제 #14
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()
예제 #15
0

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
예제 #17
0
    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