def initializeSensors():
# Initialize the Altitude/Pressure Sensor (MPL3115A2)
global altitudePressureSensor
try:
altitudePressureSensor = adafruit_mpl3115a2.MPL3115A2(i2c,
address=0x60)
# You can configure the pressure at sealevel to get better altitude estimates.
# This value has to be looked up from your local weather forecast or meteorlogical
# reports. It will change day by day and even hour by hour with weather
# changes. Remember altitude estimation from barometric pressure is not exact!
# Set this to a value in pascals:
altitudePressureSensor.sealevel_pressure = 101760
except (OSError, ValueError):
print("Altitude sensor not detected")
#Initialize the Acceleration Sensor (lsm9ds1)
global accelerationSensor
try:
accelerationSensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
except (OSError, ValueError):
print("Acceleration sensor not detected")
#Initializes the Geiger Counter
global radiationSensor
try:
radiationSensor = RadiationWatch(24, 23)
radiationSensor.setup()
except (OSError, ValueError):
print("Radiation sensor not detected")
def data_thread(conn):
while True:
global sensor
try:
sensor_instance = sensor
accel_x, accel_y, accel_z = sensor.acceleration
mag_x, mag_y, mag_z = sensor.magnetic
gyro_x, gyro_y, gyro_z = sensor.gyro
temp = sensor.temperature
except OSError as e:
print("server disconnected")
reconnected = False
while not reconnected:
try:
i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
reconnected = True
print("server reconnected")
break
except ValueError as e:
reconnected = False
print("server reconnecting...")
time.sleep(1)
# Orientation Calculation (can be later used if a player's orientation can be set, instead of being controlled by a joystick or mouse of which orientation cannot be set to specific values.)
# pitch = math.atan2(accel_y, accel_z) * 180/math.pi
# roll = math.atan2(accel_x, accel_z) * 180/math.pi
# x_flat = mag_x*math.cos(pitch) + mag_y*math.sin(pitch)*math.sin(roll) + mag_z*math.sin(pitch)*math.cos(roll)
# y_flat = mag_y*math.cos(roll) + mag_z*math.sin(roll)
# yaw = math.atan2(-y_flat, x_flat) * 180/math.pi
conn.send(struct.pack('<3f', gyro_x, gyro_y, gyro_z)) # Little Endian Byte Order ("<")
def initializeSensors():
#Initialize the Acceleration Sensor (lsm9ds1)
global accelerationSensor
try:
accelerationSensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
except(OSError, ValueError):
print("Acceleration sensor not detected")
def __init__(self, bus, angle_bias=-1.34):
self.bus = bus
self.sensor = adafruit_lsm9ds1.LSM9DS1_I2C(bus)
self.angle = 0
self.raw_angle = 0
self.gyro_x = 0
self.angle_bias = angle_bias
self.last_time = time.monotonic()
def initializeSensor():
#Initialize the Acceleration Sensor (lsm9ds1)
global accelerationSensor
try:
accelerationSensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
#Preventing errors when the sensor is unplugged
except(OSError, ValueError):
print("Acceleration sensor not detected")
def __init__(self, rp_flip=True, r_neg=False, p_neg=False, y_neg=True):
# I2C connection:
# SPI connection:
# from digitalio import DigitalInOut, Direction
# spi = busio.SPI(board.SCK, board.MOSI, board.MISO)
# csag = DigitalInOut(board.D5)
# csag.direction = Direction.OUTPUT
# csag.value = True
# csm = DigitalInOut(board.D6)
# csm.direction = Direction.OUTPUT
# csm.value = True
# sensor = adafruit_lsm9ds1.LSM9DS1_SPI(spi, csag, csm)
self.i2c = busio.I2C(board.SCL, board.SDA)
self.sensor = adafruit_lsm9ds1.LSM9DS1_I2C(self.i2c)
# Calibration Parameters
self.x_gyro_calibration = 0
self.y_gyro_calibration = 0
self.z_gyro_calibration = 0
self.roll_calibration = 0
self.pitch_calibration = 0
self.yaw_calibration = 0
# IMU Parameters: acc (x,y,z), gyro(x,y,z)
self.imu_data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# Time in seconds
self.prev_time = time.time()
# IMU timer
self.imu_diff = 0
# Gyroscope integrals for filtering
self.roll_int = 0
self.pitch_int = 0
self.yaw_int = 0
# Complementary Filter Coefficient
self.comp_filter = 0.02
# Filtered RPY
self.roll = 0
self.pitch = 0
self.yaw = 0
# Used to turn the IMU into right-hand coordinate system
self.rp_flip = rp_flip
self.r_neg = r_neg
self.p_neg = p_neg
self.y_neg = y_neg
# Magnemometer Calibration Values
self.scale_x = 1
self.scale_y = 1
self.scale_z = 1
self.mag_x_bias = 0
self.mag_y_bias = 0
self.mag_z_bias = 0
self.yaw_bias = 0
# CALIBRATE
self.calibrate_imu()
print("IMU Calibrated!")
def initializeSensors():
#Initialize the Acceleration Sensor (lsm9ds1)
global accelerationSensor
global startTime
startTime = int(round(time.time() * 1000))
try:
accelerationSensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
except (OSError, ValueError):
print("Acceleration sensor not detected")
def detect_motion_lsm9ds1(self):
try:
import board
import busio
import adafruit_lsm9ds1
self.sensor_lsm9ds1 = adafruit_lsm9ds1.LSM9DS1_I2C(
busio.I2C(board.SCL, board.SDA))
return True
except:
return False
def __init__(self, test=False, givenM=(0, 0, 0), givenA=(0, 0, 0)):
if not test:
self.test = False
# pulling the drivers for the chip
i2c = busio.I2C(board.SCL, board.SDA)
self.sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
else:
self.test = True
self.mag_reading = givenM
self.accel_reading = givenA
def __init__(self, i2c=busio.I2C(board.SCL, board.SDA)):
# i2c = busio.I2C(board.SCL, board.SDA)
self._sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
# self._speed = None
self._degree = None
self._init_degree = None
self._direction = None
self._init_gyro = None
self._running = False
self._thread = None
self._gyro_lock = Lock()
def __init__(self, update_interval_ms=50):
# Thread parameters
self.thread_name = "Drive"
threading.Thread.__init__(self, name=self.thread_name)
self._stop_event = threading.Event()
self.enabled = False
self.update_interval_ms = update_interval_ms
self.conn = busio.I2C(board.SCL, board.SDA)
self.imu = adafruit_lsm9ds1.LSM9DS1_I2C(self.conn)
self.acceleration = 0
def initializeSensors():
#Initialize the Acceleration Sensor (lsm9ds1)
global accelerationSensor
try:
accelerationSensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
except(OSError, ValueError):
print("Acceleration sensor not detected")
#Initializes the Geiger Counter
global radiationSensor
try:
radiationSensor = RadiationWatch(24, 23)
radiationSensor.setup()
except(OSError, ValueError):
print("Radiation sensor not detected")
def calibrate():
"""
Calibrates a magnometer or gyroscope
Gyroscope values are in rads/s
"""
i2c = busio.I2C(board.A3, board.A2)
sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
MAG_MIN = [1000, 1000, 1000]
MAG_MAX = [-1000, -1000, -1000]
lastDisplayTime = time.monotonic()
xavg = 0
yavg = 0
zavg = 0
countavg = 0
while True:
x, y, z = sensor.magnetic
mag_vals = [x, y, z]
for i in range(3):
MAG_MIN[i] = min(MAG_MIN[i], mag_vals[i])
MAG_MAX[i] = max(MAG_MAX[i], mag_vals[i])
gx, gy, gz = sensor.gyro
gx = gx * 3.14159 / 180
xavg += gx
gy = gy * 3.14159 / 180
yavg += gy
gz = gz * 3.14159 / 180
zavg += gz
countavg += 1
if time.monotonic() - lastDisplayTime >= 3:
print("Uncalibrated:", x, y, z)
cal_x = map_range(x, MAG_MIN[0], MAG_MAX[0], -1, 1)
cal_y = map_range(y, MAG_MIN[1], MAG_MAX[1], -1, 1)
cal_z = map_range(z, MAG_MIN[2], MAG_MAX[2], -1, 1)
print("Calibrated: ", cal_x, cal_y, cal_z)
print("MAG_MIN =", MAG_MIN)
print("MAG_MAX =", MAG_MAX)
print("Uncalibrated gyro: ", (gx, gy, gz))
print("Gyro: ",
(xavg / countavg, yavg / countavg, zavg / countavg))
lastDisplayTime = time.monotonic()
def execute():
# ROS initialize
rospy.init_node('adrv_imu', anonymous=False)
rate = rospy.Rate(100)
data_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
mag_pub = rospy.Publisher('imu/mag', MagneticField, queue_size=10)
temp_pub = rospy.Publisher('imu/temperature', Float64, queue_size=10)
# Initial Interface
i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
data = Imu()
mag = MagneticField()
temp = Float64()
while not rospy.is_shutdown():
ax, ay, az = sensor.acceleration
mx, my, mz = sensor.magnetic
gx, gy, gz = sensor.gyro
t = sensor.temperature
data.header.frame_id = 'imu_link'
data.header.stamp = rospy.Time().now()
data.linear_acceleration.x = ax
data.linear_acceleration.y = ay
data.linear_acceleration.z = az
data.angular_velocity.x = gx / 180 * math.pi
data.angular_velocity.y = gy / 180 * math.pi
data.angular_velocity.z = gz / 180 * math.pi
mag.header.stamp = rospy.Time().now()
mag.magnetic_field.x = mx
mag.magnetic_field.y = my
mag.magnetic_field.z = mz
temp.data = t
data_pub.publish(data)
mag_pub.publish(mag)
temp_pub.publish(temp)
rate.sleep()
def get_accel_mag_gyro_temp_vals():
# Main loop will read the acceleration
i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
# connection = True
while True:
# Read acceleration
accel_x, accel_y, accel_z = sensor.acceleration
OTHER_LOCK.acquire()
readings = [accel_x, accel_y, accel_z]
if accel_data.full():
accel_data.get()
accel_data.put(readings)
OTHER_LOCK.release()
def main():
signal.signal(signal.SIGINT, signal_handler)
_log = Logger("orient_test", Level.INFO)
_log.info('start...')
_message_factory = MessageFactory(Level.INFO)
_queue = MessageQueue(_message_factory, Level.INFO)
try:
i2c = busio.I2C(board.SCL, board.SDA)
lsm9ds1 = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
success = 0
_log.info(Fore.RED + Style.BRIGHT +
'to calibrate, wave robot in air until it beeps...')
while True:
# read magnetometer...
mag_x, mag_y, mag_z = lsm9ds1.magnetic
accel_x, accel_y, accel_z = lsm9ds1.acceleration
degrees = convert_to_degrees(mag_x, mag_y, mag_z)
direction = Cardinal.get_heading_from_degrees(degrees)
lsm9ds1_heading, filtered_lsm9ds1_heading, roll, pitch, yaw = convert_to_degrees_with_accelerometer(
mag_x, mag_y, mag_z, accel_x, accel_y, accel_z)
_log.info(Fore.BLACK + 'LSM9DS1 heading: {:0.3f}°;\tdirection: {} (no accel);'.format(degrees, direction) + Fore.BLACK + Style.DIM \
+ ' magnetometer (gauss):\t({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z))
_log.info(Fore.BLUE + 'LSM9DS1 heading: {:>5.2f}°;\traw heading: {:>5.2f}°; (with accel)'.format(filtered_lsm9ds1_heading, lsm9ds1_heading) + Fore.BLACK + Style.DIM \
+ '\tpitch: {:>5.2f}; roll: {:>5.2f}; yaw: {:>5.2f}.'.format(pitch, roll, yaw) + Style.RESET_ALL)
print(Fore.BLACK + '.' + Style.RESET_ALL)
time.sleep(0.5)
# .......................................
except KeyboardInterrupt:
_log.info(Fore.RED + Style.BRIGHT +
'Ctrl-C caught: keyboard interrupt.')
_log.info('done.')
sys.exit(0)
def initializeSensors():
# Test initializing the I2C
try:
i2c = busio.I2C(board.SCL, board.SDA)
except:
print("I2C bus could not be initialized")
# Initialize the Altitude/Pressure Sensor (MPL3115A2)
# Alternatively you can specify a different I2C address for the device:
#sensor = adafruit_mpl3115a2.MPL3115A2(i2c, address=0x10)
global altitudePressureSensor
try:
altitudePressureSensor = adafruit_mpl3115a2.MPL3115A2(i2c,
address=0x60)
# You can configure the pressure at sealevel to get better altitude estimates.
# This value has to be looked up from your local weather forecast or meteorlogical
# reports. It will change day by day and even hour by hour with weather
# changes. Remember altitude estimation from barometric pressure is not exact!
# Set this to a value in pascals:
altitudePressureSensor.sealevel_pressure = 101760
except (OSError, ValueError, NameError):
print(
"Altitude sensor not detected. Please check the connection to the sensor.\n"
)
#Initialize the Acceleration Sensor (LSM9DS1)
global accelerationSensor
try:
accelerationSensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
except (OSError, ValueError, NameError):
print(
"Acceleration sensor not detected. Please check the connection to the sensor.\n"
)
global radiationSensor
try:
radiationSensor = RadiationWatch(24, 23)
radiationSensor.setup()
except (OSError, ValueError, NameError):
print(
"Radiation sensor not detected. Please check the connection to the sensor.\n"
)
def __init__(self, raw=False, processed=False, maj=False, i2c=None):
if i2c == None:
self.i2c = busio.I2C(board.SCL, board.SDA)
else:
self.i2c = i2c
self.sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
self.queue = queue.Queue()
self.old_accel = [0, 0, 0]
self.old_mag = [0, 0, 0]
self.old_gyro = [0, 0, 0]
self.raw = raw
self.processed = processed
self.ahrs = MadgwickAHRS(self.SAMPLE_TIME / 5)
self.maj = maj
self.thread = threading.Thread(None, self.mainloop)
self.thread.setDaemon(True)
self.thread.start()
def DOF():
import time
import board
import busio
import adafruit_lsm9ds1
# I2C connection:
i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
# SPI connection:
# from digitalio import DigitalInOut, Direction
# spi = busio.SPI(board.SCK, board.MOSI, board.MISO)
# csag = DigitalInOut(board.D5)
# csag.direction = Direction.OUTPUT
# csag.value = True
# csm = DigitalInOut(board.D6)
# csm.direction = Direction.OUTPUT
# csm.value = True
# sensor = adafruit_lsm9ds1.LSM9DS1_SPI(spi, csag, csm)
# Main loop will read the acceleration, magnetometer, gyroscope, Temperature
# values every second and print them out.
while True:
# Read acceleration, magnetometer, gyroscope, temperature.
accel_x, accel_y, accel_z = sensor.acceleration
mag_x, mag_y, mag_z = sensor.magnetic
gyro_x, gyro_y, gyro_z = sensor.gyro
temp = sensor.temperature
# 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("Temperature: {0:0.3f}C".format(temp))
# Delay for a second.
time.sleep(1.0 / Fs)
def __init__(self):
i2c = busio.I2C(board.SCL, board.SDA)
self.sensor = gyro.LSM9DS1_I2C(i2c)
print("Sensor: Created")
def setup():
i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
centerX, centerY, centerZ = 0, 0, 0
def __init__(self):
self.imu = False
self.baro = False
self.currentSense = False
self.gps = False
# I2C connection:
self.i2c = busio.I2C(board.SCL, board.SDA)
try:
self.imu = adafruit_lsm9ds1.LSM9DS1_I2C(self.i2c)
except:
print("could not connect to LSM9DS1")
try:
self.currentSense = adafruit_ina219.INA219(self.i2c, addr=0x45)
except:
print("could not connect to INA219")
try:
self.baro = adafruit_mpl3115a2.MPL3115A2(self.i2c)
# Calibrate altimeter
self.baro.sealevel_pressure = 102250
except:
print("could not connect to MPL3115A2")
#self.RX = board.D15 #RX
#self.TX = board.D14 #TX
# Create a serial connection for the GPS connection using default speed and
# a slightly higher timeout (GPS modules typically update once a second).
try:
#self.uart = busio.UART(self.TX, self.RX, baudrate=9600, timeout=3000)
self.uart = serial.Serial("/dev/ttyS0",
baudrate=9600,
timeout=3000)
self.gps = adafruit_gps.GPS(self.uart, debug=False)
# Initialize the GPS module by changing what data it sends and at what rate.
# These are NMEA extensions for PMTK_314_SET_NMEA_OUTPUT and
# PMTK_220_SET_NMEA_UPDATERATE but you can send anything from here to adjust
# the GPS module behavior:
# https://cdn-shop.adafruit.com/datasheets/PMTK_A11.pdf
# Turn on the basic GGA and RMC info (what you typically want)
self.gps.send_command(
b'PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0')
# Turn on just minimum info (RMC only, location):
#gps.send_command(b'PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0')
# Turn off everything:
#gps.send_command(b'PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0')
# Tuen on everything (not all of it is parsed!)
#gps.send_command(b'PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0')
# Set update rate to once a second (1hz) which is what you typically want.
self.gps.send_command(b'PMTK220,900')
# Or decrease to once every two seconds by doubling the millisecond value.
# Be sure to also increase your UART timeout above!
#gps.send_command(b'PMTK220,2000')
# You can also speed up the rate, but don't go too fast or else you can lose
# data during parsing. This would be twice a second (2hz, 500ms delay):
# gps.send_command(b'PMTK220,500')
print(self.gps)
except:
print("could not connect to GPS")
self.data = {
'accel': [0, 0, 0],
'gyro': [0, 0, 0],
'mag': [0, 0, 0],
'imu_temp': 0,
'temp': 0,
'pressure': 0,
'alt': 0,
'gps': 'no_fix',
'bus_voltage': 0,
'shunt_voltage': 0,
'current': 0,
'dt': 0
}
self.accel_offset = [0, 0, 0]
self.gyro_offset = [0, 0, 0]
super(Sensors, self).__init__()
# server.py
import socket
import time
import board
import busio
import adafruit_lsm9ds1
#Initializes the I2C bus
i2c = busio.I2C(board.SCL, board.SDA)
#Defines the acceleration on the I2C bus
accelerationSensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
#Initializes a counter variable
counter = 1
accelerationArray = []
# Method to initialize acceleration sensor using the global variable
def initializeSensor():
#Initialize the Acceleration Sensor (lsm9ds1)
global accelerationSensor
try:
accelerationSensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
#Preventing errors when the sensor is unplugged
except(OSError, ValueError):
print("Acceleration sensor not detected")
initializeSensor()
import time
import math
import adafruit_lsm9ds1
import board
import busio
import neopixel
PURPLE = (180, 0, 255)
i2c = busio.I2C(board.SCL, board.SDA)
compass = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
pixels = neopixel.NeoPixel(board.A1, 16, brightness=0.01, auto_write=False)
# (x, y, z) tuples:
MAG_MIN = (-0.25046, -0.23506, -0.322)
MAG_MAX = (0.68278, 0.70882, 0.59654)
def bearing_to_pixel(bearing, count=16):
pixel = count - int(round((bearing / 360) * count))
if (pixel == 16):
pixel = 0
return pixel
def map_range(x, in_min, in_max, out_min, out_max):
"""
Maps a number from one range to another.
:return: Returns value mapped to new range
:rtype: float
"""
# read the sensor and dump the contents into a CSV file for analysis
import csv
import time
import RPi.GPIO as GPIO
import board
import busio
import adafruit_lsm9ds1
import sys
import warnings
directory_path = str(sys.argv[1])
# I2C connection:
i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
global collecting
collecting = False
# set up hardware button to quit
GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(27, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# call back for exit
def leave(pin):
# we clean up gpio twice, one from each thread. The secont one to run
# throughs a warning, since it's not a big deal we suppress the warning
# instead of fixing the threading problem
with warnings.catch_warnings():
warnings.simplefilter("ignore")
from _thread import *
import threading
import struct
print_lock = threading.Lock()
ADDR = "/tmp/openuvr_hmd_input_socket"
if os.path.exists(ADDR):
os.remove(ADDR)
server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
server.bind(ADDR)
server.listen(5)
i2c = busio.I2C(board.SCL, board.SDA) # Connect sensors via I2C
sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c) # Identify sensor as Adafruit LSM9DS1
def euler_to_quaternion(yaw, pitch, roll):
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
q = {'w': 0.0, 'x': 0.0, 'y': 0.0, 'z': 0.0}
q['w'] = cy * cp * cr + sy * sp * sr
q['x'] = cy * cp * sr - sy * sp * cr
q['y'] = sy * cp * sr + cy * sp * cr
q['z'] = sy * cp * cr - cy * sp * sr
def setup_i2c():
# I2C connection:
i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
pass
self.i2cbus.readfrom_into(self.ctrl_address, addr,self.buffer, start=1)
self.i2cbus.unlock()
return self.buffer[1]
lps25h_addr = 0x5c
lsm9ds1_mag_addr = 0x1c
hts221_addr = 0x5f # Matches Adafruit breakout and library - here for reference
led2472g_addr = 0x46
lsm9ds1_xg_addr = 0x6a
i2c=busio.I2C(board.GP21,board.GP20)
lps25h=adafruit_lps2x.LPS25(i2c,lps25h_addr)
hts221=adafruit_hts221.HTS221(i2c)
lsm9ds1=adafruit_lsm9ds1.LSM9DS1_I2C(i2c,lsm9ds1_mag_addr,lsm9ds1_xg_addr)
dpad=DPad(i2c)
leds=LedMatrix(i2c)
leds.clear()
i=0
while True:
print("LPS25H")
print("Pressure: %.2f hPa" % lps25h.pressure)
print("Temperature: %.2f C" % lps25h.temperature)
print()
print("HTS221")
print("Relative Humidity: %.2f %% rH" % hts221.relative_humidity)
self.lastGyro = (self.lastGyro + omega * delta_t) % 360
def get_angle(self):
rad = math.atan2(self.lastSin, self.lastCos)
return math.degrees(rad) % 360
pub = Publisher(8220)
offset = 10 #random starting value mostly correct aroudn stanford
offset_sub = Subscriber(8210, timeout=5)
# I2C connection to IMU
i2c = busio.I2C(board.SCL, board.SDA)
imu = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
# I2C connection to IMU
compass = HMC6343()
# initialize filter
# TODO find good value
filt = ComplementaryFilter(0.99)
lastGyro = 0
lastMag = 0
lastPub = 0
while True:
if time() - lastMag > 0.10:
heading = compass.readHeading()
def __init__(self):
i2c = busio.I2C(board.SCL, board.SDA)
self.sensor = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
