diff --git a/deoxys/deoxys/utils/io_devices/spacemouse.py b/deoxys/deoxys/utils/io_devices/spacemouse.py index bd88e24..5d8d8b7 100644 --- a/deoxys/deoxys/utils/io_devices/spacemouse.py +++ b/deoxys/deoxys/utils/io_devices/spacemouse.py @@ -116,6 +116,7 @@ def __init__( print("Opening SpaceMouse device") self.device = hid.device() self.device.open(vendor_id, product_id) # SpaceMouse + self.product_id = product_id self.pos_sensitivity = pos_sensitivity self.rot_sensitivity = rot_sensitivity @@ -219,25 +220,50 @@ def run(self): d = self.device.read(13) if d is not None and self._enabled: - if d[0] == 1: ## readings from 6-DoF sensor - self.y = convert(d[1], d[2]) - self.x = convert(d[3], d[4]) - self.z = convert(d[5], d[6]) * -1.0 - - self.roll = convert(d[7], d[8]) - self.pitch = convert(d[9], d[10]) - self.yaw = convert(d[11], d[12]) - - self._control = [ - self.x, - self.y, - self.z, - self.roll, - self.pitch, - self.yaw, - ] - - elif d[0] == 3: ## readings from the side buttons + if self.product_id == 50741: + ## logic for older spacemouse model + + if d[0] == 1: ## readings from 6-DoF sensor + self.y = convert(d[1], d[2]) + self.x = convert(d[3], d[4]) + self.z = convert(d[5], d[6]) * -1.0 + + elif d[0] == 2: + + self.roll = convert(d[1], d[2]) + self.pitch = convert(d[3], d[4]) + self.yaw = convert(d[5], d[6]) + + self._control = [ + self.x, + self.y, + self.z, + self.roll, + self.pitch, + self.yaw, + ] + else: + ## default logic for all other spacemouse models + + if d[0] == 1: ## readings from 6-DoF sensor + self.y = convert(d[1], d[2]) + self.x = convert(d[3], d[4]) + self.z = convert(d[5], d[6]) * -1.0 + + self.roll = convert(d[7], d[8]) + self.pitch = convert(d[9], d[10]) + self.yaw = convert(d[11], d[12]) + + self._control = [ + self.x, + self.y, + self.z, + self.roll, + self.pitch, + self.yaw, + ] + + if d[0] == 3: ## readings from the side buttons # press left button if d[1] == 1: