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 setup(self): 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._4G) libmetawear.mbl_mw_sensor_fusion_set_gyro_range( self.device.board, SensorFusionGyroRange._1000DPS) libmetawear.mbl_mw_sensor_fusion_write_config(self.device.board) AccSignal = libmetawear.mbl_mw_sensor_fusion_get_data_signal( self.device.board, SensorFusionData.LINEAR_ACC) gpioSignal = libmetawear.mbl_mw_gpio_get_pin_monitor_data_signal( self.device.board, 0) resetSignal = libmetawear.mbl_mw_gpio_get_pin_monitor_data_signal( self.device.board, 1) libmetawear.mbl_mw_datasignal_subscribe(AccSignal, None, self.callback) libmetawear.mbl_mw_datasignal_subscribe(gpioSignal, None, self.gpioCallback) libmetawear.mbl_mw_datasignal_subscribe(resetSignal, None, self.resetCallback) libmetawear.mbl_mw_gpio_set_pull_mode(self.device.board, 0, GpioPullMode.UP) libmetawear.mbl_mw_gpio_set_pull_mode(self.device.board, 1, GpioPullMode.UP) libmetawear.mbl_mw_gpio_set_pin_change_type(self.device.board, 0, GpioPinChangeType.ANY) libmetawear.mbl_mw_gpio_set_pin_change_type(self.device.board, 1, GpioPinChangeType.FALLING)
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 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!")
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()
for s in states: print("Configuring device") libmetawear.mbl_mw_settings_set_connection_parameters( s.device.board, 7.5, 7.5, 0, 6000) sleep(1.5) libmetawear.mbl_mw_sensor_fusion_set_mode(s.device.board, SensorFusionMode.NDOF) libmetawear.mbl_mw_sensor_fusion_set_acc_range(s.device.board, SensorFusionAccRange._8G) libmetawear.mbl_mw_sensor_fusion_set_gyro_range( s.device.board, SensorFusionGyroRange._2000DPS) libmetawear.mbl_mw_sensor_fusion_write_config(s.device.board) signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal( 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)
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)
d = MetaWear(sys.argv[i + 1]) d.connect() print("Connected to " + d.address) states.append(State(d)) for s in states: print("Configuring device") libmetawear.mbl_mw_settings_set_connection_parameters(s.device.board, 7.5, 7.5, 0, 6000) sleep(1.5) libmetawear.mbl_mw_sensor_fusion_set_mode(s.device.board, SensorFusionMode.NDOF); libmetawear.mbl_mw_sensor_fusion_set_acc_range(s.device.board, SensorFusionAccRange._8G) libmetawear.mbl_mw_sensor_fusion_set_gyro_range(s.device.board, SensorFusionGyroRange._2000DPS) libmetawear.mbl_mw_sensor_fusion_write_config(s.device.board) signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(s.device.board, SensorFusionData.QUATERNION); libmetawear.mbl_mw_datasignal_subscribe(signal, None, s.callback) libmetawear.mbl_mw_sensor_fusion_enable_data(s.device.board, SensorFusionData.QUATERNION); libmetawear.mbl_mw_sensor_fusion_start(s.device.board); sleep(100.0) for s in states: libmetawear.mbl_mw_sensor_fusion_stop(s.device.board); signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(s.device.board, SensorFusionData.QUATERNION); libmetawear.mbl_mw_datasignal_unsubscribe(signal) libmetawear.mbl_mw_debug_disconnect(s.device.board) print("Total Samples Received")
import sys print("Searching for device...") d = MetaWear('C2:9B:59:07:56:C9') d.connect() print("Connected to " + d.address) print("Configuring device") 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)