def __init__(self, init_state=Quaternion(1, 0, 0, 0), target_freq=30.0):
"""Initialize IMU bus connection, filter and threading.
Initialize a bus connection to the ICM20948 IMU, a MadgwickAHRS
filter for filtering noisy sensor data, and a deamon thread
for background updating.
Args:
init_state (madgwick_py.quaternion, optional): Initial state of the filter. Defaults to Quaternion(1, 0, 0, 0).
target_freq (float, optional): Target update frequency. Defaults to 30.0.
"""
self.lock = threading.Lock()
self.target_freq = target_freq
self.imu = ICM20948(i2c_bus=SMBus(0))
self.filter = MadgwickAHRS(
sampleperiod=1.0 / target_freq,
quaternion=init_state,
beta=0.05,
)
thread = threading.Thread(target=self.__update, args=())
thread.daemon = True
thread.start()
def test_accel_gyro(smbus):
from icm20948 import ICM20948
icm20948 = ICM20948()
ax, ay, az, gx, gy, gz = icm20948.read_accelerometer_gyro_data()
assert (round(ax, 2), round(ay, 2), round(az, 2), int(gx), int(gy),
int(gz)) == (0.05, 0.11, 0.16, 3, 4, 5)
del icm20948
def __init__(self, config, level):
super().__init__()
self._log = Logger('imu', level)
if config is None:
raise ValueError('no configuration provided.')
_config = config['ros'].get('imu')
self._icm20948 = ICM20948(i2c_addr=0x69)
self._log.info('ready.')
def test_setup_present():
smbus = mock.Mock()
smbus.SMBus = MockSMBus
sys.modules['smbus'] = smbus
from icm20948 import ICM20948
icm20948 = ICM20948()
del icm20948
def test_setup():
sys.modules['smbus'] = mock.Mock()
from icm20948 import ICM20948
with pytest.raises(RuntimeError):
icm20948 = ICM20948()
del icm20948
def get_icm20948_object():
global __icm20948
from icm20948 import ICM20948
from bus import get_i2c_object
if __icm20948 is None:
__icm20948 = ICM20948(get_i2c_object())
return __icm20948
def test_accel_gyro():
smbus = mock.Mock()
smbus.SMBus = MockSMBus
sys.modules['smbus'] = smbus
from icm20948 import ICM20948
icm20948 = ICM20948()
ax, ay, az, gx, gy, gz = icm20948.read_accelerometer_gyro_data()
assert (round(ax, 2), round(ay, 2), round(az, 2), int(gx), int(gy), int(gz)) == (0.05, 0.11, 0.16, 3, 4, 5)
del icm20948
def __init__(self, config, level):
super().__init__()
self._log = Logger('imu', level)
if config is None:
raise ValueError('no configuration provided.')
_config = config['ros'].get('imu')
self._icm20948 = ICM20948(i2c_addr=0x69)
self._amin = list(self._icm20948.read_magnetometer_data())
self._amax = list(self._icm20948.read_magnetometer_data())
self._log.info('amin: {}; amax: {}'.format(type(self._amin), type(self._amax)))
self._log.info('ready.')
def sensor_data():
from icm20948 import ICM20948
imu = ICM20948()
x, y, z = imu.read_magnetometer_data()
ax, ay, az, gx, gy, gz = imu.read_accelerometer_gyro_data()
heading_azimuth = 90 - np.arctan2(x, y) * 180 / np.pi
data = {'heading_azimuth': heading_azimuth}
response = app.response_class(response=json.dumps(data),
status=200,
mimetype='application/json')
return response
def __init__(self, config, level):
self._log = Logger("imu", level)
if config is None:
raise ValueError("no configuration provided.")
# ICM20948 configuration .....................................
_icm20948_config = config['ros'].get('icm20948')
self._icm20948_heading_trim = _icm20948_config.get('heading_trim')
self._log.info('trim: heading: {:<5.2f}° (ICM20948)'.format(
self._icm20948_heading_trim))
self._icm20948 = ICM20948(i2c_addr=0x69)
self._amin = list(self._icm20948.read_magnetometer_data())
self._amax = list(self._icm20948.read_magnetometer_data())
# BNO055 configuration .......................................
self._bno055 = BNO055(config, Level.INFO)
self._log.info('ready.')
def reset():
global imu
logger.warning("Resetting motion sensor ICM20948")
logger.info("Waiting 5 seconds..")
time.sleep(5)
logger.info("Resetting ICM20948 sensor")
try:
imu = ICM20948()
logger.info("Reset complete")
except Exception as exception:
logger.error('Unable to restart ICM20948 due to {}'.format(exception),
exc_info=True)
raise Exception(exception)
test_mx, test_my, test_mz = imu.read_magnetometer_data()
test_ax, test_ay, test_az, test_gx, test_gy, test_gz = imu.read_accelerometer_gyro_data(
)
logger.info(
f'initial data from ICM20948 ax:${test_ax}, ay:${test_ay}, az:${test_az}, gx:${test_gx}, sgy:${test_gy}, sgz:${test_gz}, mx:${test_mx}, my:${test_my}, mz:${test_mz})'
)
def test_setup(smbus_fail):
from icm20948 import ICM20948
with pytest.raises(RuntimeError):
icm20948 = ICM20948()
del icm20948
def test_setup_present(smbus):
from icm20948 import ICM20948
icm20948 = ICM20948()
del icm20948
def __init__(self, gyro_offsets=None, mag_offsets=None):
self._imu = ICM20948()
self.gyro_offsets = gyro_offsets or vector(0, 0, 0)
self.mag_offsets = mag_offsets or vector(0, 0, 0)
import smbus
import time
from icm20948 import ICM20948
from dryer import Dryer
if __name__ == '__main__':
icm20948 = ICM20948()
dryer = Dryer()
while True:
ax, ay, az, gx, gy, gz = icm20948.read_accelerometer_gyro_data()
mx, my, mz = icm20948.read_magnetometer_data()
t = int(time.time())
print(f'{t} {ax} {ay} {az} {gx} {gy} {gz} {mx} {my} {mz}')
dryer.add_reading(ax, ay, az, gx, gy, gz, mx, my, mz)
time.sleep(1)
def __init__(self):
self._imu = ICM20948()
def test_magnetometer(smbus):
from icm20948 import ICM20948
icm20948 = ICM20948()
x, y, z = icm20948.read_magnetometer_data()
assert (round(x, 2), round(y, 2), round(z, 2)) == (16.65, 33.3, 49.95)
del icm20948
def test_temperature(smbus):
from icm20948 import ICM20948
icm20948 = ICM20948()
temp_degrees = icm20948.read_temperature()
assert (round(temp_degrees, 2) == 24)
import smbus from icm20948 import ICM20948 import numpy as np from servoctl import PCA9685 from baro import * import serial SERIAL_PORT = "/dev/ttyACM0" running = True CONTROLLER_IP = "10.102.162.242" # CONTROLLER_IP = "10.253.0.3" CONTROLLER_TELE_PORT = 8888 CONTROLLER_VIDEO_PORT = 8890 imu = ICM20948() THROTTLE_SERVO = 0 ELEVATOR_SERVO = 1 YAW_SERVO = 2 AILERON_SERVO_1 = 3 AILERON_SERVO_2 = 4 AIL_1 = 2150 AIL_2 = 2150 # DATA INIT start_time = time.time() # start time ds = 0 # data counter ds_mean = [0, 0, 0, 0, 0] ds_iter = 0
def log():
# declaring a bunch of global variables that will be used accross threads
global p
global t
global t_0
global t_run
t_run = 0
global index
# initialise variables
index = 0
# initialise IMU object
imu = ICM20948()
# audio recording parameters
INDEX = 2
CHUNK = 1024
FORMAT = pyaudio.paInt32
CHANNELS = 1
RATE = 44100
RECORD_SECONDS = 40
WAVE_OUTPUT_FILENAME = "/home/pi/Desktop/Crimson/output.wav"
# Create the I2C interface for the screen
i2c = busio.I2C(SCL, SDA)
# Create the SSD1306 OLED class.
disp = adafruit_ssd1306.SSD1306_I2C(128, 32, i2c)
# Clear display.
disp.fill(0)
disp.show()
# Create blank image for drawing.
# Make sure to create image with mode '1' for 1-bit color.
width = disp.width
height = disp.height
image = Image.new("1", (width, height))
# Get drawing object to draw on image.
draw = ImageDraw.Draw(image)
# Draw a black filled box to clear the image.
draw.rectangle((0, 0, width, height), outline=0, fill=0)
# Draw some shapes.
# First define some constants to allow easy resizing of shapes.
padding = -2
top = padding
bottom = height - padding
# Move left to right keeping track of the current x position for drawing shapes.
x = 0
# Load nice silkscreen font
font = ImageFont.truetype("/home/pi/Desktop/Crimson/slkscr.ttf", 18)
# Draw a black filled box to clear the image.
draw.rectangle((0, 0, width, height), outline=0, fill=0)
# Display RECORDING label on screen
draw.text((x, top + 0), "RECORDING... ", font=font, fill=255)
disp.image(image)
disp.show()
# class to handle logging accelerometer data
class accel_recording:
# When you initialise an instance of class, the ._running flag is set to True
def __init__(self):
self._running = True
# when you terminate the instance of the class, the _running flag is set to False
def terminate(self):
self._running = False
print("logging session finished")
# What happens after the class is initialised
def run(self):
# first create a csv file top to save all the logged data
with open("/home/pi/Desktop/Crimson/sensor_log.csv",
"a",
newline="") as log:
# create an instance of csv writer
csv_write = csv.writer(log)
# write header file
csv_write.writerow(
["Index", "Timestamp", "Ax", "Ay", "Az", "Gx", "Gy", "Gz"])
# start time
global t_0
t_0 = time.time()
while self._running:
global index
global t
global t_run
# read IMU data
ax, ay, az, gx, gy, gz = imu.read_accelerometer_gyro_data()
# increment index counter
index = index + 1
# save IMU data in a row
# right now we don't use gyroscope data. however it can be used later to supplement raising/lowering
# of the arm detection with an exoskeleton mounted sensor
log_row = [
index,
datetime.now().strftime("%Y-%m-%d %H:%M:%S"),
ax,
ay,
az,
gx,
gy,
gz,
]
# write row to csv file
csv_write.writerow(log_row)
t = time.time()
# the sampling timing isn't precise at all but that doens't matter for our application
# half a second precision is enough to get the results we want. therefore a rough estimate
# of the loop time is used here to find time elapsed.
t_run = t - t_0
time.sleep(0.01)
class mic_recording:
def __init__(self):
self._running = True
def terminate(self):
self._running = False
def run(self):
p = pyaudio.PyAudio()
stream = p.open(
format=FORMAT,
channels=CHANNELS,
rate=RATE,
input=True,
input_device_index=INDEX,
frames_per_buffer=CHUNK,
)
print("* recording")
frames = []
for i in range(0, int(RATE / CHUNK * RECORD_SECONDS)):
data = stream.read(CHUNK, exception_on_overflow=False)
frames.append(data)
print("* done recording")
stream.stop_stream()
stream.close()
p.terminate()
wf = wave.open(WAVE_OUTPUT_FILENAME, "wb")
wf.setnchannels(CHANNELS)
wf.setsampwidth(p.get_sample_size(FORMAT))
wf.setframerate(RATE)
wf.writeframes(b"".join(frames))
wf.close()
# create Class
mic_1 = mic_recording()
# Create_Thread
mic_Thread = Thread(target=mic_1.run)
# start Thread
mic_Thread.start()
# the recording and accelerometer logging don't start at the same time because of differences in initialisation processes
# this is not a clean way of doing things but gets the job done realigning them
time.sleep(0.3)
# Create Class
imu_1 = accel_recording()
# Create Thread
imu_Thread = Thread(target=imu_1.run)
# Start Thread
imu_Thread.start()
Exit = False # Exit flag
while t_run <= 40.0:
if t_run > 45.0:
Exit = True # Exit Program
imu_1.terminate()
mic_1.terminate()
# here you can split the files in ax, ay, az
# import csv file
df = pd.read_csv("/home/pi/Desktop/Crimson/sensor_log.csv", header=0)
df_ax = df[["Ax"]].copy()
# print(df_ax.head())
df_ax.Ax = pd.to_numeric(df_ax.Ax, errors="coerce")
df_ax.to_csv(r"/home/pi/Desktop/Crimson/ax.csv", index=False)
# ay dataset
df_ay = df[["Ay"]].copy()
# print(df_ay.head())
df_ay.Ay = pd.to_numeric(df_ay.Ay, errors="coerce")
df_ay.to_csv(r"/home/pi/Desktop/Crimson/ay.csv", index=False)
df_az = df[["Az"]].copy()
# print(df_az.head())
df_az.Az = pd.to_numeric(df_az.Az, errors="coerce")
df_az.to_csv(r"/home/pi/Desktop/Crimson/az.csv", index=False)
# Draw a black filled box to clear the image.
draw.rectangle((0, 0, width, height), outline=0, fill=0)
# Display RECORDING label on screen
draw.text((x, top + 0), "Done", font=font, fill=255)
draw.text((x, top + 16), "Recording", font=font, fill=255)
disp.image(image)
disp.show()
time.sleep(2)
if __name__ == "__main__":
log()