def start(self):
libmetawear.mbl_mw_sensor_fusion_enable_data(
self.device.board, SensorFusionData.LINEAR_ACC)
libmetawear.mbl_mw_sensor_fusion_start(self.device.board)
libmetawear.mbl_mw_gpio_start_pin_monitoring(self.device.board, 0)
libmetawear.mbl_mw_gpio_start_pin_monitoring(self.device.board, 1)
print("Start Drawing")
def DevRun(self):
try:
## Get data and print to console ##
self.euler_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(
self.board, SensorFusionData.EULER_ANGLE)
self.euler_callback = FnVoid_VoidP_DataP(
lambda context, data: print("epoch: %s, euler %s\n" % (
data.contents.epoch, parse_value(data))))
## Required for data extraction ##
libmetawear.mbl_mw_datasignal_subscribe(self.euler_signal, None,
self.euler_callback)
libmetawear.mbl_mw_sensor_fusion_enable_data(
self.board, SensorFusionData.EULER_ANGLE)
libmetawear.mbl_mw_sensor_fusion_set_mode(self.board,
SensorFusionMode.NDOF)
libmetawear.mbl_mw_sensor_fusion_write_config(self.board)
libmetawear.mbl_mw_sensor_fusion_start(self.board)
#input('') # Don't seem to need this ? ?
except OSError as err:
print("OS ERROR {}".format(err))
self.DevClose()
print("Device closed properly...")
sys.exit()
except ValueError:
print("Error with variable...")
self.DevClose()
print("Device closed properly...")
sys.exit()
except:
print("Unexpected Error:", sys.exc_info()[0])
self.DevClose()
print("Device closed properly...")
sys.exit()
def run(self):
try:
self.euler_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(self.board, SensorFusionData.EULER_ANGLE)
libmetawear.mbl_mw_datasignal_subscribe(self.euler_signal, None, self.euler_callback)
libmetawear.mbl_mw_sensor_fusion_enable_data(self.board, SensorFusionData.EULER_ANGLE)
libmetawear.mbl_mw_sensor_fusion_set_mode(self.board, SensorFusionMode.NDOF)
libmetawear.mbl_mw_sensor_fusion_write_config(self.board)
libmetawear.mbl_mw_sensor_fusion_start(self.board)
self.e.wait()
input("")
except KeyboardInterrupt as e:
print (e)
self.close()
sys.exit()
def calibrate(self):
e = Event()
def calibration_data_handler(ctx, board, pointer):
print("calibration data: %s" % (pointer.contents))
libmetawear.mbl_mw_sensor_fusion_write_calibration_data(
board, pointer)
libmetawear.mbl_mw_memory_free(pointer)
e.set()
def calibration_handler(ctx, pointer):
value = parse_value(pointer)
print("state: %s" % (value))
if (value.accelrometer >=
Const.SENSOR_FUSION_CALIBRATION_ACCURACY_HIGH
and value.gyroscope >=
Const.SENSOR_FUSION_CALIBRATION_ACCURACY_HIGH
and value.magnetometer >=
Const.SENSOR_FUSION_CALIBRATION_ACCURACY_HIGH):
libmetawear.mbl_mw_sensor_fusion_read_calibration_data(
self.device.board, None, fn_wrapper_01)
else:
sleep(1.0)
libmetawear.mbl_mw_datasignal_read(signal)
fn_wrapper_01 = FnVoid_VoidP_VoidP_CalibrationDataP(
calibration_data_handler)
fn_wrapper_02 = FnVoid_VoidP_DataP(calibration_handler)
signal = libmetawear.mbl_mw_sensor_fusion_calibration_state_data_signal(
self.device.board)
libmetawear.mbl_mw_sensor_fusion_set_mode(self.device.board,
SensorFusionMode.NDOF)
libmetawear.mbl_mw_sensor_fusion_write_config(self.device.board)
libmetawear.mbl_mw_datasignal_subscribe(signal, None, fn_wrapper_02)
libmetawear.mbl_mw_sensor_fusion_start(self.device.board)
libmetawear.mbl_mw_datasignal_read(signal)
e.wait()
sleep(1.0)
libmetawear.mbl_mw_sensor_fusion_stop(self.device.board)
libmetawear.mbl_mw_datasignal_unsubscribe(signal)
e.clear()
def DevRun(self):
try:
## Retrieve Sensor Data? ##
self.euler_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(
self.board, SensorFusionData.EULER_ANGLE)
## View Data Directly - Removed ##
#self.euler_callback = FnVoid_VoidP_DataP(lambda context,data:print("epoch: %s, euler %s\n" % (data.contents.epoch, parse_value(data))))
## From initi function ##
self.euler_callback = FnVoid_VoidP_DataP(self.data_handler)
## All required for Data Extraction? ##
libmetawear.mbl_mw_datasignal_subscribe(self.euler_signal, None,
self.euler_callback)
libmetawear.mbl_mw_sensor_fusion_enable_data(
self.board, SensorFusionData.EULER_ANGLE)
libmetawear.mbl_mw_sensor_fusion_set_mode(self.board,
SensorFusionMode.NDOF)
libmetawear.mbl_mw_sensor_fusion_write_config(self.board)
libmetawear.mbl_mw_sensor_fusion_start(self.board)
#self.e.wait() # what is this for ? ? - removed
#input('') # what is this for ? ? - Does nothing
## Catch ANY errors ##
except OSError as err:
print("OS ERROR {}".format(err))
self.DevClose()
print("Device closed properly...")
sys.exit()
except ValueError:
print("Error with variable...")
self.DevClose()
print("Device closed properly...")
sys.exit()
except:
print("Unexpected Error:", sys.exc_info()[0])
self.DevClose()
print("Device closed properly...")
sys.exit()
def run(self):
try:
self.euler_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(
self.board, SensorFusionData.EULER_ANGLE)
self.euler_callback = FnVoid_VoidP_DataP(
lambda context, data: print("epoch: %s, euler %s\n" % (
data.contents.epoch, parse_value(data))))
libmetawear.mbl_mw_datasignal_subscribe(self.euler_signal, None,
self.euler_callback)
libmetawear.mbl_mw_sensor_fusion_enable_data(
self.board, SensorFusionData.EULER_ANGLE)
libmetawear.mbl_mw_sensor_fusion_set_mode(self.board,
SensorFusionMode.NDOF)
libmetawear.mbl_mw_sensor_fusion_write_config(self.board)
libmetawear.mbl_mw_sensor_fusion_start(self.board)
input('')
except Exception as e:
print("RUN ERROR - {}".format(e))
self.close()
sys.exit()
s.device.board, SensorFusionData.EULER_ANGLE)
libmetawear.mbl_mw_datasignal_subscribe(signal, None, s.callback)
# Get the linear accelerometer data signal
acc = libmetawear.mbl_mw_sensor_fusion_get_data_signal(
s.device.board, SensorFusionData.LINEAR_ACC)
# setup threshold detector - detect anything above 1
# print("ths")
# ths = libmetawear.mbl_mw_dataprocessor_threshold_create(acc, ThresholdMode.BINARY, 0.2 , 0.0, None, s.callbackax)
libmetawear.mbl_mw_datasignal_subscribe(acc, None, s.callbackax)
print("ths ended")
libmetawear.mbl_mw_sensor_fusion_enable_data(s.device.board,
SensorFusionData.EULER_ANGLE)
libmetawear.mbl_mw_sensor_fusion_enable_data(s.device.board,
SensorFusionData.LINEAR_ACC)
libmetawear.mbl_mw_sensor_fusion_start(s.device.board)
print("start streaming")
sleep(5.0)
for s in states:
print("stop streaming")
libmetawear.mbl_mw_sensor_fusion_stop(s.device.board)
libmetawear.mbl_mw_acc_stop(s.device.board)
libmetawear.mbl_mw_acc_disable_acceleration_sampling(s.device.board)
print("Unsubscribing.. ")
signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(
s.device.board, SensorFusionData.EULER_ANGLE)
libmetawear.mbl_mw_datasignal_unsubscribe(signal)
def __init__(self, mac_adr):
# Create publisher
self.frame_id = rospy.get_name().split('/')[-1]
self.pub_rotation = rospy.Publisher(self.frame_id + '/rotation',
QuaternionStamped,
queue_size=10)
self.pub_accel = rospy.Publisher(self.frame_id + '/accel',
Vector3Stamped,
queue_size=10)
self.pub_gyro = rospy.Publisher(self.frame_id + '/gyro',
Vector3Stamped,
queue_size=10)
self.pub_g = rospy.Publisher(self.frame_id + '/gravity',
Vector3Stamped,
queue_size=10)
self.pub_acc_lin = rospy.Publisher(self.frame_id + '/lin_acc',
Vector3Stamped,
queue_size=10)
d = MetaWear(mac_adr)
d.connect()
print("Connected to " + d.address)
self.device = d
self.samples_acc = 0
self.samples_rot = 0
self.samples_gyro = 0
self.cb_gyro_cpp = FnVoid_VoidP_DataP(self.gyro_cb)
self.cb_acc_cpp = FnVoid_VoidP_DataP(self.acc_cb)
self.cb_rot_cpp = FnVoid_VoidP_DataP(self.rot_cb)
self.cb_g_cpp = FnVoid_VoidP_DataP(self.g_cb)
self.cb_acc_lin_cpp = FnVoid_VoidP_DataP(self.acc_lin_cb)
print("Configuring device")
libmetawear.mbl_mw_settings_set_connection_parameters(
self.device.board, 10, 20, 0, 6000)
print("Sensor Fusion")
libmetawear.mbl_mw_sensor_fusion_set_mode(self.device.board,
SensorFusionMode.IMU_PLUS)
libmetawear.mbl_mw_sensor_fusion_set_acc_range(
self.device.board, SensorFusionAccRange._8G)
libmetawear.mbl_mw_sensor_fusion_write_config(self.device.board)
rospy.sleep(1)
#self.gyro_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(self.device.board, SensorFusionData.CORRECTED_GYRO)
#self.acc_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(self.device.board, SensorFusionData.CORRECTED_ACC)
self.rot_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(
self.device.board, SensorFusionData.QUATERNION)
#self.g_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(self.device.board, SensorFusionData.GRAVITY_VECTOR)
#self.acc_lin_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(self.device.board, SensorFusionData.LINEAR_ACC)
rospy.sleep(1)
#libmetawear.mbl_mw_datasignal_subscribe(self.gyro_signal, None, self.cb_gyro_cpp)
#libmetawear.mbl_mw_datasignal_subscribe(self.acc_signal, None, self.cb_acc_cpp)
libmetawear.mbl_mw_datasignal_subscribe(self.rot_signal, None,
self.cb_rot_cpp)
#libmetawear.mbl_mw_datasignal_subscribe(self.g_signal, None, self.cb_g_cpp)
#libmetawear.mbl_mw_datasignal_subscribe(self.acc_lin_signal, None, self.cb_acc_lin_cpp)
rospy.sleep(1)
#libmetawear.mbl_mw_sensor_fusion_enable_data(self.device.board, SensorFusionData.CORRECTED_GYRO)
#libmetawear.mbl_mw_sensor_fusion_enable_data(self.device.board, SensorFusionData.CORRECTED_ACC)
libmetawear.mbl_mw_sensor_fusion_enable_data(
self.device.board, SensorFusionData.QUATERNION)
#libmetawear.mbl_mw_sensor_fusion_enable_data(self.device.board, SensorFusionData.GRAVITY_VECTOR)
#libmetawear.mbl_mw_sensor_fusion_enable_data(self.device.board, SensorFusionData.LINEAR_ACC)
rospy.sleep(1)
libmetawear.mbl_mw_sensor_fusion_start(self.device.board)
try:
# sensorfusion.set_mode() function
libmetawear.mbl_mw_sensor_fusion_set_mode(d.board, SensorFusionMode.NDOF)
libmetawear.mbl_mw_sensor_fusion_write_config(d.board)
signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(
d.board, SensorFusionData.EULER_ANGLE)
logger = create_voidp(
lambda fn: libmetawear.mbl_mw_datasignal_log(signal, None, fn),
resource="Euler_logger")
libmetawear.mbl_mw_logging_start(d.board, 0)
libmetawear.mbl_mw_sensor_fusion_enable_data(d.board,
SensorFusionData.EULER_ANGLE)
libmetawear.mbl_mw_sensor_fusion_start(d.board)
print("Logging data for 5s")
sleep(5.0)
libmetawear.mbl_mw_sensor_fusion_stop(d.board)
libmetawear.mbl_mw_sensor_fusion_clear_enabled_mask(d.board)
libmetawear.mbl_mw_logging_stop(d.board)
print("Downloading data")
libmetawear.mbl_mw_settings_set_connection_parameters(
d.board, 7.5, 7.5, 0, 6000)
sleep(1.0)
e = Event()
