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])
