def initSensors(): print("start init sensors") for ch in range(len(list_ch)): devices = [0x68,0x69] res = [] bus.write_byte_data(mux_address,0x04,list_ch[ch]) #print("test ch "+str(ch)) for dev in range(len(devices)): #print(devices[dev]) try: sensor = mpu9250.MPU9250(devices[dev],'test') time.sleep(0.1) s =sensor.readRow() print(s) res.append(sensor) print('Initialized on line {0} sensor {1}'.format(ch,dev+1)) except Exception as e: #print(e) pass # #if e.errno != errno.EREMOTEIO: # # print("Error: {0} on address {1} {2}".format(e, ch,dev)) # except Exception as e: # exception if read_byte fails # print("Error unk: {0} on address {1} {2}".format(e, ch,dev)) # pass sensors.append(res) #except Exception as e: # exception if read_byte fails # print("Error multiplexer: {0} on address {1}".format(e,mux_address)) print(sensors)
def main(max_time, altitude_dif, delay_sample): formatted_time = datetime.datetime.utcnow().isoformat() print('[{}][main] Starting main'.format(formatted_time)) bmp280_0 = bmp280.BMP280(delay_sample - 0.01) dht11_0 = dht11.DHT11(pin=4) mpu9250_0 = mpu9250.MPU9250() servo_0 = servo.Servo(0) deployer_0 = parachute_deployer.Deployer(bmp280_0, servo_0, altitude_dif, delay_sample) logger_0 = data_logger.Logger(bmp280_0, dht11_0, mpu9250_0) camera_0 = camera.Camera() deployer_thread = threading.Thread(target=deployer_0.main) logging_thread = threading.Thread(target=logger_0.log) deployer_thread.start() logging_thread.start() camera_0.start_recording() finisher_input_thread = threading.Thread(target=finisher_input, args=(deployer_0, logger_0, camera_0), daemon=True) finisher_time_thread = threading.Thread(target=finisher_time, args=(deployer_0, logger_0, camera_0, max_time), daemon=True) finisher_input_thread.start() finisher_time_thread.start()
def main(): i2c = busio.I2C(board.SCL, board.SDA) mpu = mpu9250.MPU9250(i2c) while True: blink_color(board.LED_G) _acceleration = mpu.acceleration _gyro = mpu.gyro _temperature = mpu.temperature # Assemble all the data to capture # counter, accX, accY, accZ, gyroX, gyroY, gyroZ, temp # TODO Nicer to use a dictionary here and use that to print a header row payload = merge_tuples(str(time.monotonic()), _acceleration, _gyro, _temperature) write_data(payload)
def mpu9250_example(): mpu9250_0 = mpu9250.MPU9250() print('MPU9250') accel = mpu9250_0.readAccel() print("ax =", (accel['x'])) print("ay =", (accel['y'])) print("az =", (accel['z'])) gyro = mpu9250_0.readGyro() print("gx =", (gyro['x'])) print("gy =", (gyro['y'])) print("gz =", (gyro['z'])) mag = mpu9250_0.readMagnet() print("mx =", (mag['x'])) print("my =", (mag['y'])) print("mz =", (mag['z'])) print()
def senMPU(): global accel, acX, acY, acZ, gyro, gX, gY, gZ, mag, mX, mY, mZ bypass = MPU9250(i2c_gy91) if MPUCalibrat: ak8963 = AK8963(i2c_gy91, offset=offsetMag, scale=scaleMag) #Pendent d'actualització de la llibreria a la versio 4.0 #mpu6500 = MPU6500(i2c_gy91, offset=offsetGyro, gyro_sf=SF_DEG_S) mpu6500 = MPU6500(i2c_gy91, gyro_sf=SF_DEG_S) else: ak8963 = AK8963(i2c_gy91) mpu6500 = MPU6500(i2c_gy91, gyro_sf=SF_DEG_S) mpu = mpu9250.MPU9250(i2c_gy91, ak8963=ak8963, mpu6500=mpu6500) #Reorientacio dels eixos del accelerometre i el giroscopi per a que tinguin la mateixa disposicio que el magnetometre #Veure eixos al datasheet MPU9250 accel, gyro = orientate((1, 0, 2), (False, False, True), mpu.acceleration, mpu.gyro) acX, acY, acZ = accel gX, gY, gZ = gyro mag = mpu.magnetic mX, mY, mZ = mag
def testIMU(stdscr): IMU1 = mpu9250.MPU9250() IMU1.calGyro() time.sleep(2) curses.init_pair(1, curses.COLOR_RED, curses.COLOR_WHITE) curses.init_pair(2, curses.COLOR_BLUE, curses.COLOR_WHITE) curses.init_pair(3, curses.COLOR_GREEN, curses.COLOR_WHITE) color = 1 while True: gyro = IMU1.readGyro() heading = IMU1.curHeading() color += 1 if color == 4: color = 1 stdscr.erase() stdscr.addstr(0,0,"GX: " + str(gyro['x'])) stdscr.addstr(1,0,"GY: " + str(gyro['y'])) stdscr.addstr(2,0,"GZ: " + str(gyro['z'])) stdscr.addstr(3,0,"ROLL: " + str(heading['roll'])) stdscr.addstr(4,0,"PITCH: " + str(heading['pitch'])) stdscr.addstr(5,0,"YAW: " + str(heading['yaw'])) stdscr.addstr(3,50,"geoff gay", curses.color_pair(color)) stdscr.addstr(4,50,"geoff gay", curses.color_pair(color)) stdscr.addstr(5,50,"geoff gay", curses.color_pair(color)) stdscr.addstr(6,50,"geoff gay", curses.color_pair(color)) stdscr.addstr(7,50,"geoff gay", curses.color_pair(color)) stdscr.addstr(8,50,"geoff gay", curses.color_pair(color)) stdscr.addstr(9,50,"geoff gay", curses.color_pair(color)) stdscr.addstr(6,0,'TIME ELAPSED: ' + str(IMU1.timeElapsed(time.time()))) stdscr.refresh() time.sleep(0.1)
import mpu9250 import time import sys IMU1 = mpu9250.MPU9250() try: IMU1.calGyro() time.sleep(2) IMU1.timeElapsed(time.time()) while True: accel = IMU1.readAccel() print('') print("ax = " + str(accel['x'])) print("ay = " + str(accel['y'])) print("az = " + str(accel['z'])) #t = IMU1.timeElapsed(time.time()) #print('TIME ELAPSED: ' + str(t) gyro = IMU1.readGyro() heading = IMU1.curHeading() magnet = IMU1.readMagnet() counter = 0 if counter % 100 == 0: print("MagnetX: " + str(magnet['x'])) print("MagnetY: " + str(magnet['y'])) print("MagnetZ: " + str(magnet['z'])) print('') #print " [PRE] gx = " , ( gyro['xPre'] )
def AHRS_process(processEXIT, output_array): # output_array[0]=AHRS roll angle (deg) # output_array[1]=ARHS pitch angle (deg) # output_array[2]=AHRS yaw angle (deg) # output_array[3]=AHRS psi dot (deg/s) # output_array[4]=AHRS phi dot (deg/s) # output_array[5]=AHRS theta dot (deg/s) # output_array[6]=hard iron offest x # output_array[7]=hard iron offset y # output_array[8]=hard iron offset z # output_array[9]=magnetometer function check # output_array[10]=roll orientation offset due to installation # output_array[11]=pitch orientation offset due to installation # output_array[12]=ax # output_array[13]=ay # output_array[14]=az # output_array[15]=p # output_array[16]=q # output_array[17]=r # output_array[18]=orientation print('Starting AHRS process.') att = Complementary_Filter2.comp_filt(output_array[6], output_array[7], output_array[8]) imu = mpu9250.MPU9250() imu.initialize() time.sleep(1) g_offset = [0, 0, 0] # Low pass filter setup #fc=15; #Hz #dt_f=0.0011 #measured average (sec) #pie=3.14159265 #a_f=2*pie*dt_f*fc/(2*pie*dt_f*fc+1) #b_f=1-a_f #first_time=1 # Loop to determine gyroscope offsets for x in range(0, 100): m9a, m9g, m9m = imu.getMotion9() g_offset[0] = g_offset[0] + m9g[1] / 100 g_offset[1] = g_offset[1] + m9g[0] / 100 g_offset[2] = g_offset[2] + m9g[2] / 100 if (m9m[0] == 0.0 and m9m[1] == 0.0 and m9m[2] == 0.0): output_array[9] = 1.0 # Main AHRS loop while processEXIT.value == 0: m9a, m9g, m9m = imu.getMotion9() if output_array[18] == 1: gx = (m9g[1] - g_offset[0]) * 57.2958 #Convert to deg/s gy = (m9g[0] - g_offset[1]) * 57.2958 gz = -(m9g[2] - g_offset[2]) * 57.2958 ax = m9a[1] * 0.10197 #Convert to g-force (1/9.81) ay = m9a[0] * 0.10197 az = -m9a[2] * 0.10197 elif output_array[18] == 5: gx = (m9g[1] - g_offset[0]) * 57.2958 #Convert to deg/s gy = -(m9g[0] - g_offset[1]) * 57.2958 gz = (m9g[2] - g_offset[2]) * 57.2958 ax = m9a[1] * 0.10197 #Convert to g-force (1/9.81) ay = -m9a[0] * 0.10197 az = m9a[2] * 0.10197 att.attitude3(ax, ay, az, gx, gy, gz, m9m[0], m9m[1], m9m[2]) # Apply LPF (only for ouput data) #if first_time==1: # theta_dot_d=att.thetad_d # phi_dot_d=att.phid_d # psi_dot_d=att.psid_d # first_time=0 #else: # theta_dot_d=a_f*att.thetad_d+b_f*theta_dot_d # phi_dot_d=a_f*att.phid_d+b_f*phi_dot_d # psi_dot_d=a_f*att.psid_d+b_f*psi_dot_d # Set outputs output_array[0] = att.roll_d - output_array[ 10] #Subtract offsets due to orientation error output_array[1] = att.pitch_d - output_array[ 11] #Subtract offsets due to orientation error output_array[2] = att.yaw_d # If using LPF #output_array[3]=psi_dot_d #output_array[4]=phi_dot_d #output_array[5]=theta_dot_d # If not using LPF output_array[3] = att.psid_d output_array[4] = att.phid_d output_array[5] = att.thetad_d output_array[12] = ax output_array[13] = ay output_array[14] = az output_array[15] = gx output_array[16] = gy output_array[17] = gz print('AHRS process stopped.')
def mpu9250_init(): global i2c_mpu9250 global sensor i2c_mpu9250 = machine.I2C(scl=machine.Pin(26), sda=machine.Pin(25)) sensor = mpu9250.MPU9250(i2c=i2c_mpu9250) sensor.calibrate_MPU9250()
import tcp_client as tcp import time import mpu9250 as mpu ''' Script to send all''' mpu = mpu.MPU9250() Client = tcp.tcpClient('192.168.1.36', 60000) Client.connect() while True: data = mpu.readAll() Client.sndPack(data) time.sleep(0.1)
#--------------------------------------------------------------- # HEADER STARTS HERE: #--------------------------------------------------------------- import mpu9250 import time import sys import RPi.GPIO as GPIO import math import quantizer as quant import numpy as np GPIO.setmode(GPIO.BOARD) # Sensors class initialization sens = mpu9250.MPU9250() # A few seconds for initializing the system, about 10 seconds time.sleep(3) # Getting gryo biases: def gyrobias(): xsum = ysum = zsum = 0 for i in range(0,1000,1): gyro = sens.readGyro() xsum = xsum + gyro['x'] ysum = ysum + gyro['y'] zsum = zsum + gyro['z'] time.sleep(0.01) gxb = xsum/float(1000) gyb = ysum/float(1000) gzb = zsum/float(1000)
pin12 = pins.Pins(2) pin13 = pins.Pins(18) pin14 = pins.Pins(19) pin15 = pins.Pins(23) pin16 = pins.Pins(5) pin19 = pins.Pins(22) pin20 = pins.Pins(21) import display Image = display.Image display = display.Display() import button button_a = button.Button(35) button_b = button.Button(27) import temperature __adc = machine.ADC(machine.Pin(34, machine.Pin.IN)) __adc.atten(machine.ADC.ATTN_11DB) temperature = temperature.Temperature(__adc) try: import mpu9250 __sensor = mpu9250.MPU9250( machine.I2C(scl=machine.Pin(22), sda=machine.Pin(21), freq=200000)) import compass compass = compass.Compass(__sensor) import accelerometer accelerometer = accelerometer.Accelerometer(__sensor) except Exception as e: print("MPU9250 ERROR")
import mpu9250 import time from socket import socket, AF_INET, SOCK_DGRAM PORT = 5000 ADDRESS = "192.168.1.36" s = socket(AF_INET, SOCK_DGRAM) sensor = mpu9250.MPU9250(0.1, 0x68, 0x0c) sensor.reset_register() sensor.power_wake_up() sensor.set_accel_range(8, False) sensor.set_gyro_range(1000, False) sensor.set_mag_register('100Hz', '16bit') while True: now = time.time() axis = sensor.get_axis() # -> pitch, yaw, roll data = str(axis[0]) + ',' + str(axis[1]) + ',' + str(axis[2]) # print(data) s.sendto(data.encode(), (ADDRESS, PORT)) sleep_time = sensor.sampling_time - (time.time() - now) if sleep_time < 0.0: continue time.sleep(sleep_time) s.close()
while not wifi.connect(WIFI_SSIDS): time.sleep(0.5) name = ("%02x" * 6) % struct.unpack("6B", machine.unique_id()) #topic = "%s/%s" % (MQTT_TOPIC, name) topic = MQTT_TOPIC mqtt = MQTTClient(name, MQTT_HOST) mqtt.connect() i2c = machine.I2C(freq=400000, scl=machine.Pin(19), sda=machine.Pin(23)) i2c.writeto_mem(104, 107, bytes([0])) bmp = bmp280.BMP280(i2c) mpu = mpu9250.MPU9250(i2c) queue = [] time.sleep(1) _, _, alt0 = bmp.read() t0 = time.ticks_ms() def read_sensors(_): if gc.mem_free() > 4096: (acc_x, acc_y, acc_z), _ = mpu.read() _, _, alt = bmp.read() t = time.ticks_ms() queue.insert(0, [t - t0, acc_z, alt - alt0])