def close(self):
libmetawear.mbl_mw_sensor_fusion_stop(self.board)
libmetawear.mbl_mw_sensor_fusion_clear_enabled_mask(self.board)
libmetawear.mbl_mw_datasignal_unsubscribe(self.euler_signal)
self.device.disconnect()
sleep(1)
print("DISCONNETED")
def run(self):
loop_rate = rospy.Rate(100)
while not rospy.is_shutdown():
loop_rate.sleep()
print("Disconnect")
libmetawear.mbl_mw_sensor_fusion_stop(self.device.board)
#libmetawear.mbl_mw_datasignal_unsubscribe(self.gyro_signal)
#libmetawear.mbl_mw_datasignal_unsubscribe(self.acc_signal)
libmetawear.mbl_mw_datasignal_unsubscribe(self.rot_signal)
#libmetawear.mbl_mw_datasignal_unsubscribe(self.acc_lin_signal)
#libmetawear.mbl_mw_datasignal_unsubscribe(self.g_signal)
libmetawear.mbl_mw_debug_disconnect(self.device.board)
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 stop(self):
libmetawear.mbl_mw_sensor_fusion_stop(self.device.board)
AccSignal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(
self.device.board, SensorFusionData.LINEAR_ACC)
libmetawear.mbl_mw_datasignal_unsubscribe(AccSignal)
gpioSignal = libmetawear.mbl_mw_gpio_get_pin_monitor_data_signal(
self.device.board, 0)
libmetawear.mbl_mw_datasignal_unsubscribe(gpioSignal)
libmetawear.mbl_mw_gpio_stop_pin_monitoring(self.device.board, 0)
resetSignal = libmetawear.mbl_mw_gpio_get_pin_monitor_data_signal(
self.device.board, 1)
libmetawear.mbl_mw_datasignal_unsubscribe(resetSignal)
libmetawear.mbl_mw_gpio_stop_pin_monitoring(self.device.board, 1)
libmetawear.mbl_mw_debug_disconnect(self.device.board)
sleep(5.0)
self.device.disconnect()
sleep(1.0)
print("DISCONNECTED!")
# 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)
acc = libmetawear.mbl_mw_sensor_fusion_get_data_signal(
s.device.board, SensorFusionData.LINEAR_ACC)
print("ths")
ths = libmetawear.mbl_mw_dataprocessor_threshold_create(
acc, ThresholdMode.BINARY, 0.2, 0.0, None, s.callbackax)
libmetawear.mbl_mw_datasignal_unsubscribe(ths)
print("ths ended")
def DevClose(self):
libmetawear.mbl_mw_sensor_fusion_stop(self.board)
libmetawear.mbl_mw_sensor_fusion_clear_enabled_mask(self.board)
libmetawear.mbl_mw_datasignal_unsubscribe(self.euler_signal)
self.device.disconnect()
time.sleep(1)
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()
def progress_update_handler(context, entries_left, total_entries):
if (entries_left == 0):
e.set()
fn_wrapper = FnVoid_VoidP_UInt_UInt(progress_update_handler)
