diff --git a/README.md b/README.md index a7bdae8..67c4907 100644 --- a/README.md +++ b/README.md @@ -660,7 +660,17 @@ and then add a sensor line like the following MMC5603_1234_0001,I2C,-,-,-,-,active,None,1,1,MMC5603IC2,MMC5603,1234,0001,-,OnBoard,NTP,magnetism,mmc magnetic sensor +#### 4.4.19 BNO08x I2C sensors +pMARTAS also supports the BNO08x 9-DoF sensor providing 3 axis magnetic vector, acceleration and gyro information. Installation is similar to the MMC5603 and requires the installation of adafruit python packages. + +Install the following package first: + + pip install adafruit-circuitpython-bno08x + +and then add a sensor line like the following + + BNO085_1234_0001,I2C,-,-,-,-,active,None,1,1,BNO085IC2,BNO085,1234,0001,-,OnBoard,NTP,magnetism,bno 9-dof sensor ### 4.5 Running the acquisition system diff --git a/martas/lib/bno085i2cprotocol.py b/martas/lib/bno085i2cprotocol.py new file mode 100644 index 0000000..790638c --- /dev/null +++ b/martas/lib/bno085i2cprotocol.py @@ -0,0 +1,265 @@ +# ################################################################### +# Import packages +# ################################################################### + +import socket # for hostname identification +import string # for ascii selection +import struct # for binary representation +import subprocess +import time +from datetime import datetime, timezone + +from twisted.python import log + +from martas.core import methods as mm + +try: + import adafruit_bn08x + import board + from adafruit_bno08x import ( + BNO_REPORT_ACCELEROMETER, + BNO_REPORT_GYROSCOPE, + BNO_REPORT_MAGNETOMETER, + ) + from adafruit_bno08x.i2c import BNO08X_I2C +except: + log.msg( + " bno085 i2c requires the following packages: board, and adafruit-circuitpython-bno08x" + ) + + ## I2C BNO08x protocol +## ----------------------- + + +class BNO085I2CProtocol(object): + """ + Protocol to read BNO08x data attached to I2C pins of i.e. an raspberry + This protocol makes use of the Adafruit library. + pip3 install adafruit-circuitpython-bno08x + + """ + + def __init__(self, client, sensordict, confdict): + self.client = client + self.sensordict = sensordict + self.confdict = confdict + self.count = 0 ## counter for sending header information + self.sensor = sensordict.get("sensorid") + self.hostname = socket.gethostname() + self.printable = set(string.printable) + self.datalst = [] + self.datacnt = 0 + self.metacnt = 10 + self.counter = {} + self.initi2c = True + + # extract data from configuration dictionary + debugtest = confdict.get("debug") + + # QOS + self.qos = int(confdict.get("mqttqos", 0)) + if not self.qos in [0, 1, 2]: + self.qos = 0 + log.msg(" -> setting QOS:", self.qos) + + # switch on debug mode + self.debug = False + if debugtest in ["true", "True", "TRUE"]: + log.msg( + "DEBUG - {}: Debug mode activated.".format( + self.sensordict.get("protocol") + ) + ) + self.debug = True # prints many test messages + else: + log.msg(" -> Debug mode = {}".format(debugtest)) + + # connect to i2c + self.conn = self.init_sensor() + + def datetime2array(self, t): + return [t.year, t.month, t.day, t.hour, t.minute, t.second, t.microsecond] + + def init_sensor(self): + # log.msg("connecting to I2C ...") + REPORT_INTERVAL = 100000 # 100 Hz + i2c = board.I2C() # uses board.SCL and board.SDA + sensor = BNO08X_I2C(i2c) + sensor.enable_feature(BNO_REPORT_ACCELEROMETER, REPORT_INTERVAL) + sensor.enable_feature(BNO_REPORT_GYROSCOPE, REPORT_INTERVAL) + sensor.enable_feature(BNO_REPORT_MAGNETOMETER, REPORT_INTERVAL) + sensor.soft_reset() + sensor.begin_calibration() + return sensor + + def processBNO085Data(self, sensorid, meta, data): + """""" + currenttime = datetime.now(timezone.utc).replace(tzinfo=None) + outdate = datetime.strftime(currenttime, "%Y-%m-%d") + filename = outdate + + datearray = self.datetime2array(currenttime) + + packcode = "6hL" + + values = [] + multiplier = [] + for dat in data: + try: + values.append(float(dat)) + datearray.append(int(float(dat) * 10000)) + packcode = packcode + "q" + multiplier.append(10000) + except: + log.msg( + "{} protocol: Error while appending data to file (non-float?): {}".format( + self.sensordict.get("protocol"), dat + ) + ) + + try: + data_bin = struct.pack("<" + packcode, *datearray) # little endian + except: + log.msg( + "{} protocol: Error while packing binary data".format( + self.sensordict.get("protocol") + ) + ) + pass + + key = "[" + str(meta.get("SensorKeys")).replace("'", "").strip() + "]" + ele = "[" + str(meta.get("SensorElements")).replace("'", "").strip() + "]" + unit = "[" + str(meta.get("SensorUnits")).replace("'", "").strip() + "]" + multplier = str(multiplier).replace(" ", "") + # Correct some common old problem + unit = unit.replace("deg C", "degC") + + header = "# MagPyBin %s %s %s %s %s %s %d" % ( + sensorid, + key, + ele, + unit, + multplier, + packcode, + struct.calcsize(packcode), + ) + + if not self.confdict.get("bufferdirectory", "") == "": + mm.data_to_file( + self.confdict.get("bufferdirectory"), + sensorid, + filename, + data_bin, + header, + ) + + return ",".join(list(map(str, datearray))), header + + def get_bno08x_data(self, sensor, debug=False): + mag_x, mag_y, mag_z = sensor.magnetic + accel_x, accel_y, accel_z = sensor.acceleration + gyro_x, gyro_y, gyro_z = sensor.gyro + if debug: + print(f"X:{mag_x:10.3f}, Y:{mag_y:10.3f}, Z:{mag_z:10.3f} uT") + print(f"X:{accel_x:10.3f}, Y:{accel_y:10.3f}, Z:{accel_z:10.3f} m/s^2") + print(f"X:{gyro_x:10.3f}, Y:{gyro_y:10.3f}, Z:{gyro_z:10.3f} rad/s") + data = [ + float(mag_x) * 1000.0, + float(mag_y) * 1000.0, + float(mag_z) * 1000.0, + float(accel_x), + float(accel_y), + float(accel_z), + float(gyro_x), + float(gyro_y), + float(gyro_z), + ] + return data + + def calibrate_bno08x_data(self, data, offsets=None, scales=None, debug=False): + if not offsets: + offsets = [0, 0, 0, 0, 0, 0, 0, 0, 0] + if not scales: + scales = [1, 1, 1, 1, 1, 1, 1, 1, 1] + for i, el in enumerate(data): + data[i] = el + offsets[i] + data[i] = data[i] * scales[i] + return data + + def define_bno08x_meta(self): + meta = {} + meta["SensorKeys"] = ( + "mag_x,mag_y,mag_z,accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z" + ) + meta["SensorElements"] = ( + "mag_X,mag_Y,mag_Z,accel_X,accel_Y,accel_Z,gyro_X,gyro_Y,gyro_Z" + ) + meta["SensorUnits"] = "nT,nT,nT,m/s^2,m/s^2,m/s^2,rad/s,rad/s,rad/s," + return meta + + def sendRequest(self): + + if not self.conn: + self.conn = self.init_sensor() + # or put everything in a try except and reconnect on except + # connect to i2c + data = self.get_bno08x_data(self.conn, debug=self.debug) + data = self.calibrate_bno08x_data(data, offsets=[0, 0, 0, 0, 0, 0, 0, 0, 0]) + meta = self.define_bno08x_meta() + self.publish_data(data, meta) + + def publish_data(self, data, meta): + + if len(data) > 0: + topic = self.confdict.get("station") + "/" + self.sensor + pdata, head = self.processBNO085Data(self.sensor, meta, data) + + senddata = False + try: + coll = int(self.sensordict.get("stack")) + except: + coll = 0 + + if coll > 1: + self.metacnt = 1 # send meta data with every block + if self.datacnt < coll: + self.datalst.append(pdata) + self.datacnt += 1 + else: + senddata = True + pdata = ";".join(self.datalst) + self.datalst = [] + self.datacnt = 0 + else: + senddata = True + + if senddata: + self.client.publish(topic + "/data", pdata, qos=self.qos) + # If multiple sensors are connected, self.count needs to be a dict (with topic) + # Initialize counter for each topic + if self.counter.get(self.sensor, "") == "": + self.counter[self.sensor] = 0 + cnt = self.counter.get(self.sensor) + + if cnt == 0: + ## 'Add' is a string containing dict info like: + ## SensorID:ENV05_2_0001,StationID:wic, PierID:xxx,SensorGroup:environment,... + add = "SensorID:{},StationID:{},DataPier:{},SensorModule:{},SensorGroup:{},SensorDescription:{},DataTimeProtocol:{}".format( + self.sensordict.get("sensorid", ""), + self.confdict.get("station", ""), + self.sensordict.get("pierid", ""), + self.sensordict.get("protocol", ""), + self.sensordict.get("sensorgroup", ""), + self.sensordict.get("sensordesc", ""), + self.sensordict.get("ptime", ""), + ) + self.client.publish(topic + "/dict", add, qos=self.qos) + self.client.publish(topic + "/meta", head, qos=self.qos) + + cnt += 1 + # self.count += 1 + if cnt >= self.metacnt: + cnt = 0 + + # update counter in dict + self.counter[self.sensor] = cnt