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)
states = []
for i in range(len(sys.argv) - 1):
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.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)
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)
