Add joycon-python source code
This commit is contained in:
		
							parent
							
								
									749a2a92fe
								
							
						
					
					
						commit
						3d00015607
					
				
					 7 changed files with 995 additions and 0 deletions
				
			
		
							
								
								
									
										25
									
								
								pycon/__init__.py
									
										
									
									
									
										Normal file
									
								
							
							
						
						
									
										25
									
								
								pycon/__init__.py
									
										
									
									
									
										Normal file
									
								
							|  | @ -0,0 +1,25 @@ | |||
| from .joycon import JoyCon | ||||
| from .wrappers import PythonicJoyCon  # as JoyCon | ||||
| from .gyro import GyroTrackingJoyCon | ||||
| from .event import ButtonEventJoyCon | ||||
| from .device import get_device_ids, get_ids_of_type | ||||
| from .device import is_id_L | ||||
| from .device import get_R_ids, get_L_ids | ||||
| from .device import get_R_id, get_L_id | ||||
| 
 | ||||
| 
 | ||||
| __version__ = "0.2.4" | ||||
| 
 | ||||
| __all__ = [ | ||||
|     "ButtonEventJoyCon", | ||||
|     "GyroTrackingJoyCon", | ||||
|     "JoyCon", | ||||
|     "PythonicJoyCon", | ||||
|     "get_L_id", | ||||
|     "get_L_ids", | ||||
|     "get_R_id", | ||||
|     "get_R_ids", | ||||
|     "get_device_ids", | ||||
|     "get_ids_of_type", | ||||
|     "is_id_L", | ||||
| ] | ||||
							
								
								
									
										4
									
								
								pycon/constants.py
									
										
									
									
									
										Normal file
									
								
							
							
						
						
									
										4
									
								
								pycon/constants.py
									
										
									
									
									
										Normal file
									
								
							|  | @ -0,0 +1,4 @@ | |||
| JOYCON_VENDOR_ID    = 0x057E | ||||
| JOYCON_L_PRODUCT_ID = 0x2006 | ||||
| JOYCON_R_PRODUCT_ID = 0x2007 | ||||
| JOYCON_PRODUCT_IDS = (JOYCON_L_PRODUCT_ID, JOYCON_R_PRODUCT_ID) | ||||
							
								
								
									
										77
									
								
								pycon/device.py
									
										
									
									
									
										Normal file
									
								
							
							
						
						
									
										77
									
								
								pycon/device.py
									
										
									
									
									
										Normal file
									
								
							|  | @ -0,0 +1,77 @@ | |||
| import hid | ||||
| from .constants import JOYCON_VENDOR_ID, JOYCON_PRODUCT_IDS | ||||
| from .constants import JOYCON_L_PRODUCT_ID, JOYCON_R_PRODUCT_ID | ||||
| 
 | ||||
| 
 | ||||
| def get_device_ids(debug=False): | ||||
|     """ | ||||
|     returns a list of tuples like `(vendor_id, product_id, serial_number)` | ||||
|     """ | ||||
|     devices = hid.enumerate(0, 0) | ||||
| 
 | ||||
|     out = [] | ||||
|     for device in devices: | ||||
|         vendor_id      = device["vendor_id"] | ||||
|         product_id     = device["product_id"] | ||||
|         product_string = device["product_string"] | ||||
|         serial = device.get('serial') or device.get("serial_number") | ||||
| 
 | ||||
|         if vendor_id != JOYCON_VENDOR_ID: | ||||
|             continue | ||||
|         if product_id not in JOYCON_PRODUCT_IDS: | ||||
|             continue | ||||
|         if not product_string: | ||||
|             continue | ||||
| 
 | ||||
|         out.append((vendor_id, product_id, serial)) | ||||
| 
 | ||||
|         if debug: | ||||
|             print(product_string) | ||||
|             print(f"\tvendor_id  is {vendor_id!r}") | ||||
|             print(f"\tproduct_id is {product_id!r}") | ||||
|             print(f"\tserial     is {serial!r}") | ||||
| 
 | ||||
|     return out | ||||
| 
 | ||||
| 
 | ||||
| def is_id_L(id): | ||||
|     return id[1] == JOYCON_L_PRODUCT_ID | ||||
| 
 | ||||
| 
 | ||||
| def get_ids_of_type(lr, **kw): | ||||
|     """ | ||||
|     returns a list of tuples like `(vendor_id, product_id, serial_number)` | ||||
| 
 | ||||
|     arg: lr : str : put `R` or `L` | ||||
|     """ | ||||
|     if lr.lower() == "l": | ||||
|         product_id = JOYCON_L_PRODUCT_ID | ||||
|     else: | ||||
|         product_id = JOYCON_R_PRODUCT_ID | ||||
|     return [i for i in get_device_ids(**kw) if i[1] == product_id] | ||||
| 
 | ||||
| 
 | ||||
| def get_R_ids(**kw): | ||||
|     """returns a list of tuple like `(vendor_id, product_id, serial_number)`""" | ||||
|     return get_ids_of_type("R", **kw) | ||||
| 
 | ||||
| 
 | ||||
| def get_L_ids(**kw): | ||||
|     """returns a list of tuple like `(vendor_id, product_id, serial_number)`""" | ||||
|     return get_ids_of_type("L", **kw) | ||||
| 
 | ||||
| 
 | ||||
| def get_R_id(**kw): | ||||
|     """returns a tuple like `(vendor_id, product_id, serial_number)`""" | ||||
|     ids = get_R_ids(**kw) | ||||
|     if not ids: | ||||
|         return (None, None, None) | ||||
|     return ids[0] | ||||
| 
 | ||||
| 
 | ||||
| def get_L_id(**kw): | ||||
|     """returns a tuple like `(vendor_id, product_id, serial_number)`""" | ||||
|     ids = get_L_ids(**kw) | ||||
|     if not ids: | ||||
|         return (None, None, None) | ||||
|     return ids[0] | ||||
							
								
								
									
										133
									
								
								pycon/event.py
									
										
									
									
									
										Normal file
									
								
							
							
						
						
									
										133
									
								
								pycon/event.py
									
										
									
									
									
										Normal file
									
								
							|  | @ -0,0 +1,133 @@ | |||
| from .wrappers import PythonicJoyCon | ||||
| 
 | ||||
| 
 | ||||
| class ButtonEventJoyCon(PythonicJoyCon): | ||||
|     def __init__(self, *args, track_sticks=False, **kwargs): | ||||
|         super().__init__(*args, **kwargs) | ||||
| 
 | ||||
|         self._events_buffer = []  # TODO: perhaps use a deque instead? | ||||
| 
 | ||||
|         self._event_handlers = {} | ||||
|         self._event_track_sticks = track_sticks | ||||
| 
 | ||||
|         self._previous_stick_l_btn = 0 | ||||
|         self._previous_stick_r_btn = 0 | ||||
|         self._previous_stick_r  = self._previous_stick_l  = (0, 0) | ||||
|         self._previous_r        = self._previous_l        = 0 | ||||
|         self._previous_zr       = self._previous_zl       = 0 | ||||
|         self._previous_plus     = self._previous_minus    = 0 | ||||
|         self._previous_a        = self._previous_right    = 0 | ||||
|         self._previous_b        = self._previous_down     = 0 | ||||
|         self._previous_x        = self._previous_up       = 0 | ||||
|         self._previous_y        = self._previous_left     = 0 | ||||
|         self._previous_home     = self._previous_capture  = 0 | ||||
|         self._previous_right_sr = self._previous_left_sr  = 0 | ||||
|         self._previous_right_sl = self._previous_left_sl  = 0 | ||||
| 
 | ||||
|         if self.is_left(): | ||||
|             self.register_update_hook(self._event_tracking_update_hook_left) | ||||
|         else: | ||||
|             self.register_update_hook(self._event_tracking_update_hook_right) | ||||
| 
 | ||||
|     def joycon_button_event(self, button, state):  # overridable | ||||
|         self._events_buffer.append((button, state)) | ||||
| 
 | ||||
|     def events(self): | ||||
|         while self._events_buffer: | ||||
|             yield self._events_buffer.pop(0) | ||||
| 
 | ||||
|     @staticmethod | ||||
|     def _event_tracking_update_hook_right(self): | ||||
|         if self._event_track_sticks: | ||||
|             pressed = self.stick_r_btn | ||||
|             if self._previous_stick_r_btn != pressed: | ||||
|                 self._previous_stick_r_btn = pressed | ||||
|                 self.joycon_button_event("stick_r_btn", pressed) | ||||
|         pressed = self.r | ||||
|         if self._previous_r != pressed: | ||||
|             self._previous_r = pressed | ||||
|             self.joycon_button_event("r", pressed) | ||||
|         pressed = self.zr | ||||
|         if self._previous_zr != pressed: | ||||
|             self._previous_zr = pressed | ||||
|             self.joycon_button_event("zr", pressed) | ||||
|         pressed = self.plus | ||||
|         if self._previous_plus != pressed: | ||||
|             self._previous_plus = pressed | ||||
|             self.joycon_button_event("plus", pressed) | ||||
|         pressed = self.a | ||||
|         if self._previous_a != pressed: | ||||
|             self._previous_a = pressed | ||||
|             self.joycon_button_event("a", pressed) | ||||
|         pressed = self.b | ||||
|         if self._previous_b != pressed: | ||||
|             self._previous_b = pressed | ||||
|             self.joycon_button_event("b", pressed) | ||||
|         pressed = self.x | ||||
|         if self._previous_x != pressed: | ||||
|             self._previous_x = pressed | ||||
|             self.joycon_button_event("x", pressed) | ||||
|         pressed = self.y | ||||
|         if self._previous_y != pressed: | ||||
|             self._previous_y = pressed | ||||
|             self.joycon_button_event("y", pressed) | ||||
|         pressed = self.home | ||||
|         if self._previous_home != pressed: | ||||
|             self._previous_home = pressed | ||||
|             self.joycon_button_event("home", pressed) | ||||
|         pressed = self.right_sr | ||||
|         if self._previous_right_sr != pressed: | ||||
|             self._previous_right_sr = pressed | ||||
|             self.joycon_button_event("right_sr", pressed) | ||||
|         pressed = self.right_sl | ||||
|         if self._previous_right_sl != pressed: | ||||
|             self._previous_right_sl = pressed | ||||
|             self.joycon_button_event("right_sl", pressed) | ||||
| 
 | ||||
|     @staticmethod | ||||
|     def _event_tracking_update_hook_left(self): | ||||
|         if self._event_track_sticks: | ||||
|             pressed = self.stick_l_btn | ||||
|             if self._previous_stick_l_btn != pressed: | ||||
|                 self._previous_stick_l_btn = pressed | ||||
|                 self.joycon_button_event("stick_l_btn", pressed) | ||||
|         pressed = self.l | ||||
|         if self._previous_l != pressed: | ||||
|             self._previous_l = pressed | ||||
|             self.joycon_button_event("l", pressed) | ||||
|         pressed = self.zl | ||||
|         if self._previous_zl != pressed: | ||||
|             self._previous_zl = pressed | ||||
|             self.joycon_button_event("zl", pressed) | ||||
|         pressed = self.minus | ||||
|         if self._previous_minus != pressed: | ||||
|             self._previous_minus = pressed | ||||
|             self.joycon_button_event("minus", pressed) | ||||
|         pressed = self.up | ||||
|         if self._previous_up != pressed: | ||||
|             self._previous_up = pressed | ||||
|             self.joycon_button_event("up", pressed) | ||||
|         pressed = self.down | ||||
|         if self._previous_down != pressed: | ||||
|             self._previous_down = pressed | ||||
|             self.joycon_button_event("down", pressed) | ||||
|         pressed = self.left | ||||
|         if self._previous_left != pressed: | ||||
|             self._previous_left = pressed | ||||
|             self.joycon_button_event("left", pressed) | ||||
|         pressed = self.right | ||||
|         if self._previous_right != pressed: | ||||
|             self._previous_right = pressed | ||||
|             self.joycon_button_event("right", pressed) | ||||
|         pressed = self.capture | ||||
|         if self._previous_capture != pressed: | ||||
|             self._previous_capture = pressed | ||||
|             self.joycon_button_event("capture", pressed) | ||||
|         pressed = self.left_sr | ||||
|         if self._previous_left_sr != pressed: | ||||
|             self._previous_left_sr = pressed | ||||
|             self.joycon_button_event("left_sr", pressed) | ||||
|         pressed = self.left_sl | ||||
|         if self._previous_left_sl != pressed: | ||||
|             self._previous_left_sl = pressed | ||||
|             self.joycon_button_event("left_sl", pressed) | ||||
							
								
								
									
										84
									
								
								pycon/gyro.py
									
										
									
									
									
										Normal file
									
								
							
							
						
						
									
										84
									
								
								pycon/gyro.py
									
										
									
									
									
										Normal file
									
								
							|  | @ -0,0 +1,84 @@ | |||
| from .wrappers import PythonicJoyCon | ||||
| from glm import vec2, vec3, quat, angleAxis, eulerAngles | ||||
| from typing import Optional | ||||
| import time | ||||
| 
 | ||||
| 
 | ||||
| class GyroTrackingJoyCon(PythonicJoyCon): | ||||
|     """ | ||||
|     A specialized class based on PythonicJoyCon which tracks the gyroscope data | ||||
|     and deduces the current rotation of the JoyCon. Can be used to create a | ||||
|     pointer rotate an object or pointin a direction. Comes with the need to be | ||||
|     calibrated. | ||||
|     """ | ||||
|     def __init__(self, *args, **kwargs): | ||||
|         super().__init__(*args, simple_mode=False, **kwargs) | ||||
| 
 | ||||
|         # set internal state: | ||||
|         self.reset_orientation() | ||||
| 
 | ||||
|         # register the update callback | ||||
|         self.register_update_hook(self._gyro_update_hook) | ||||
| 
 | ||||
|     @property | ||||
|     def pointer(self) -> Optional[vec2]: | ||||
|         d = self.direction | ||||
|         if d.x <= 0: | ||||
|             return None | ||||
|         return vec2(d.y, -d.z) / d.x | ||||
| 
 | ||||
|     @property | ||||
|     def direction(self) -> vec3: | ||||
|         return self.direction_X | ||||
| 
 | ||||
|     @property | ||||
|     def rotation(self) -> vec3: | ||||
|         return -eulerAngles(self.direction_Q) | ||||
| 
 | ||||
|     is_calibrating = False | ||||
| 
 | ||||
|     def calibrate(self, seconds=2): | ||||
|         self.calibration_acumulator = vec3(0) | ||||
|         self.calibration_acumulations = 0 | ||||
|         self.is_calibrating = time.time() + seconds | ||||
| 
 | ||||
|     def _set_calibration(self, gyro_offset=None): | ||||
|         if not gyro_offset: | ||||
|             c = vec3(1, self._ime_yz_coeff, self._ime_yz_coeff) | ||||
|             gyro_offset = self.calibration_acumulator * c | ||||
|             gyro_offset /= self.calibration_acumulations | ||||
|             gyro_offset += vec3( | ||||
|                     self._GYRO_OFFSET_X, | ||||
|                     self._GYRO_OFFSET_Y, | ||||
|                     self._GYRO_OFFSET_Z, | ||||
|                 ) | ||||
|         self.is_calibrating = False | ||||
|         self.set_gyro_calibration(gyro_offset) | ||||
| 
 | ||||
|     def reset_orientation(self): | ||||
|         self.direction_X = vec3(1, 0, 0) | ||||
|         self.direction_Y = vec3(0, 1, 0) | ||||
|         self.direction_Z = vec3(0, 0, 1) | ||||
|         self.direction_Q = quat() | ||||
| 
 | ||||
|     @staticmethod | ||||
|     def _gyro_update_hook(self): | ||||
|         if self.is_calibrating: | ||||
|             if self.is_calibrating < time.time(): | ||||
|                 self._set_calibration() | ||||
|             else: | ||||
|                 for xyz in self.gyro: | ||||
|                     self.calibration_acumulator += xyz | ||||
|                 self.calibration_acumulations += 3 | ||||
| 
 | ||||
|         for gx, gy, gz in self.gyro_in_rad: | ||||
|             # TODO: find out why 1/86 works, and not 1/60 or 1/(60*30) | ||||
|             rotation \ | ||||
|                 = angleAxis(gx * (-1/86), self.direction_X) \ | ||||
|                 * angleAxis(gy * (-1/86), self.direction_Y) \ | ||||
|                 * angleAxis(gz * (-1/86), self.direction_Z) | ||||
| 
 | ||||
|             self.direction_X *= rotation | ||||
|             self.direction_Y *= rotation | ||||
|             self.direction_Z *= rotation | ||||
|             self.direction_Q *= rotation | ||||
							
								
								
									
										530
									
								
								pycon/joycon.py
									
										
									
									
									
										Normal file
									
								
							
							
						
						
									
										530
									
								
								pycon/joycon.py
									
										
									
									
									
										Normal file
									
								
							|  | @ -0,0 +1,530 @@ | |||
| from .constants import JOYCON_VENDOR_ID, JOYCON_PRODUCT_IDS | ||||
| from .constants import JOYCON_L_PRODUCT_ID, JOYCON_R_PRODUCT_ID | ||||
| import hid | ||||
| import time | ||||
| import threading | ||||
| from typing import Optional | ||||
| 
 | ||||
| # TODO: disconnect, power off sequence | ||||
| 
 | ||||
| 
 | ||||
| class JoyCon: | ||||
|     _INPUT_REPORT_SIZE = 49 | ||||
|     _INPUT_REPORT_PERIOD = 0.015 | ||||
|     _RUMBLE_DATA = b'\x00\x01\x40\x40\x00\x01\x40\x40' | ||||
| 
 | ||||
|     vendor_id  : int | ||||
|     product_id : int | ||||
|     serial     : Optional[str] | ||||
|     simple_mode: bool | ||||
|     color_body : (int, int, int) | ||||
|     color_btn  : (int, int, int) | ||||
|     stick_cal  : [int, int, int, int, int, int, int, int] | ||||
| 
 | ||||
|     def __init__(self, vendor_id: int, product_id: int, serial: str = None, simple_mode=False): | ||||
|         if vendor_id != JOYCON_VENDOR_ID: | ||||
|             raise ValueError(f'vendor_id is invalid: {vendor_id!r}') | ||||
| 
 | ||||
|         if product_id not in JOYCON_PRODUCT_IDS: | ||||
|             raise ValueError(f'product_id is invalid: {product_id!r}') | ||||
| 
 | ||||
|         self.vendor_id   = vendor_id | ||||
|         self.product_id  = product_id | ||||
|         self.serial      = serial | ||||
|         self.simple_mode = simple_mode  # TODO: It's for reporting mode 0x3f | ||||
| 
 | ||||
|         # setup internal state | ||||
|         self._input_hooks = [] | ||||
|         self._input_report = bytes(self._INPUT_REPORT_SIZE) | ||||
|         self._packet_number = 0 | ||||
|         self.set_accel_calibration((0, 0, 0), (1, 1, 1)) | ||||
|         self.set_gyro_calibration((0, 0, 0), (1, 1, 1)) | ||||
| 
 | ||||
|         # connect to joycon | ||||
|         self._joycon_device = self._open(vendor_id, product_id, serial=serial) | ||||
|         self._read_joycon_data() | ||||
|         self._setup_sensors() | ||||
| 
 | ||||
|         # start talking with the joycon in a daemon thread | ||||
|         self._update_input_report_thread \ | ||||
|             = threading.Thread(target=self._update_input_report) | ||||
|         self._update_input_report_thread.setDaemon(True) | ||||
|         self._update_input_report_thread.start() | ||||
| 
 | ||||
|     def _open(self, vendor_id, product_id, serial): | ||||
|         try: | ||||
|             if hasattr(hid, "device"):  # hidapi | ||||
|                 _joycon_device = hid.device() | ||||
|                 _joycon_device.open(vendor_id, product_id, serial) | ||||
|             elif hasattr(hid, "Device"):  # hid | ||||
|                 _joycon_device = hid.Device(vendor_id, product_id, serial) | ||||
|             else: | ||||
|                 raise Exception("Implementation of hid is not recognized!") | ||||
|         except IOError as e: | ||||
|             raise IOError('joycon connect failed') from e | ||||
|         return _joycon_device | ||||
| 
 | ||||
|     def _close(self): | ||||
|         if self._joycon_device: | ||||
|             self._joycon_device.close() | ||||
|             self._joycon_device = None | ||||
| 
 | ||||
|     def _read_input_report(self) -> bytes: | ||||
|         if self._joycon_device: | ||||
|             return bytes(self._joycon_device.read(self._INPUT_REPORT_SIZE)) | ||||
| 
 | ||||
|     def _write_output_report(self, command, subcommand, argument): | ||||
|         if not self._joycon_device: | ||||
|             return | ||||
| 
 | ||||
|         # TODO: add documentation | ||||
|         self._joycon_device.write(b''.join([ | ||||
|             command, | ||||
|             self._packet_number.to_bytes(1, byteorder='little'), | ||||
|             self._RUMBLE_DATA, | ||||
|             subcommand, | ||||
|             argument, | ||||
|         ])) | ||||
|         self._packet_number = (self._packet_number + 1) & 0xF | ||||
| 
 | ||||
|     def _send_subcmd_get_response(self, subcommand, argument) -> (bool, bytes): | ||||
|         # TODO: handle subcmd when daemon is running | ||||
|         self._write_output_report(b'\x01', subcommand, argument) | ||||
| 
 | ||||
|         report = self._read_input_report() | ||||
|         while report[0] != 0x21:  # TODO, avoid this, await daemon instead | ||||
|             report = self._read_input_report() | ||||
| 
 | ||||
|         # TODO, remove, see the todo above | ||||
|         assert report[1:2] != subcommand, "THREAD carefully" | ||||
| 
 | ||||
|         # TODO: determine if the cut bytes are worth anything | ||||
| 
 | ||||
|         return report[13] & 0x80, report[13:]  # (ack, data) | ||||
| 
 | ||||
|     def _spi_flash_read(self, address, size) -> bytes: | ||||
|         assert size <= 0x1d | ||||
|         argument = address.to_bytes(4, "little") + size.to_bytes(1, "little") | ||||
|         ack, report = self._send_subcmd_get_response(b'\x10', argument) | ||||
|         if not ack: | ||||
|             raise IOError("After SPI read @ {address:#06x}: got NACK") | ||||
| 
 | ||||
|         if report[:2] != b'\x90\x10': | ||||
|             raise IOError("Something else than the expected ACK was recieved!") | ||||
|         assert report[2:7] == argument, (report[2:5], argument) | ||||
| 
 | ||||
|         return report[7:size+7] | ||||
| 
 | ||||
|     def _update_input_report(self):  # daemon thread | ||||
|         try: | ||||
|             while self._joycon_device: | ||||
|                 report = self._read_input_report() | ||||
|                 # TODO, handle input reports of type 0x21 and 0x3f | ||||
|                 while report[0] != 0x30: | ||||
|                     report = self._read_input_report() | ||||
| 
 | ||||
|                 self._input_report = report | ||||
| 
 | ||||
|                 for callback in self._input_hooks: | ||||
|                     callback(self) | ||||
|         except OSError: | ||||
|             print('connection closed') | ||||
|             pass | ||||
| 
 | ||||
|     def _read_joycon_data(self): | ||||
|         color_data = self._spi_flash_read(0x6050, 6) | ||||
| 
 | ||||
|         self._read_stick_calibration_data() | ||||
| 
 | ||||
|         buf = self._spi_flash_read(0x6086 if self.is_left() else 0x6098, 16) | ||||
|         self.deadzone = (buf[4] << 8) & 0xF00 | buf[3] | ||||
| 
 | ||||
|         # user IME data | ||||
|         if self._spi_flash_read(0x8026, 2) == b"\xB2\xA1": | ||||
|             # print(f"Calibrate {self.serial} IME with user data") | ||||
|             imu_cal = self._spi_flash_read(0x8028, 24) | ||||
| 
 | ||||
|         # factory IME data | ||||
|         else: | ||||
|             # print(f"Calibrate {self.serial} IME with factory data") | ||||
|             imu_cal = self._spi_flash_read(0x6020, 24) | ||||
| 
 | ||||
|         self.color_body = tuple(color_data[:3]) | ||||
|         self.color_btn  = tuple(color_data[3:]) | ||||
| 
 | ||||
|         self.set_accel_calibration(( | ||||
|                 self._to_int16le_from_2bytes(imu_cal[ 0], imu_cal[ 1]), | ||||
|                 self._to_int16le_from_2bytes(imu_cal[ 2], imu_cal[ 3]), | ||||
|                 self._to_int16le_from_2bytes(imu_cal[ 4], imu_cal[ 5]), | ||||
|             ), ( | ||||
|                 self._to_int16le_from_2bytes(imu_cal[ 6], imu_cal[ 7]), | ||||
|                 self._to_int16le_from_2bytes(imu_cal[ 8], imu_cal[ 9]), | ||||
|                 self._to_int16le_from_2bytes(imu_cal[10], imu_cal[11]), | ||||
|             ) | ||||
|         ) | ||||
|         self.set_gyro_calibration(( | ||||
|                 self._to_int16le_from_2bytes(imu_cal[12], imu_cal[13]), | ||||
|                 self._to_int16le_from_2bytes(imu_cal[14], imu_cal[15]), | ||||
|                 self._to_int16le_from_2bytes(imu_cal[16], imu_cal[17]), | ||||
|             ), ( | ||||
|                 self._to_int16le_from_2bytes(imu_cal[18], imu_cal[19]), | ||||
|                 self._to_int16le_from_2bytes(imu_cal[20], imu_cal[21]), | ||||
|                 self._to_int16le_from_2bytes(imu_cal[22], imu_cal[23]), | ||||
|             ) | ||||
|         ) | ||||
| 
 | ||||
|     def _read_stick_calibration_data(self): | ||||
|         user_stick_cal_addr = 0x8012 if self.is_left() else 0x801D | ||||
|         buf = self._spi_flash_read(user_stick_cal_addr, 9) | ||||
|         use_user_data = False | ||||
| 
 | ||||
|         for b in buf: | ||||
|             if b != 0xFF: | ||||
|                 use_user_data = True | ||||
|                 break | ||||
| 
 | ||||
|         if not use_user_data: | ||||
|             factory_stick_cal_addr = 0x603D if self.is_left() else 0x6046 | ||||
|             buf = self._spi_flash_read(factory_stick_cal_addr, 9) | ||||
| 
 | ||||
|         self.stick_cal = [0] * 6 | ||||
| 
 | ||||
|         # X Axis Max above center | ||||
|         self.stick_cal[0 if self.is_left() else 2] = (buf[1] << 8) & 0xF00 | buf[0] | ||||
|         # Y Axis Max above center | ||||
|         self.stick_cal[1 if self.is_left() else 3] = (buf[2] << 4) | (buf[1] >> 4) | ||||
|         # X Axis Center | ||||
|         self.stick_cal[2 if self.is_left() else 4] = (buf[4] << 8) & 0xF00 | buf[3] | ||||
|         # Y Axis Center | ||||
|         self.stick_cal[3 if self.is_left() else 5] = (buf[5] << 4) | (buf[4] >> 4) | ||||
|         # X Axis Min below center | ||||
|         self.stick_cal[4 if self.is_left() else 0] = (buf[7] << 8) & 0xF00 | buf[6] | ||||
|         # Y Axis Min below center | ||||
|         self.stick_cal[5 if self.is_left() else 1] = (buf[8] << 4) | (buf[7] >> 4) | ||||
| 
 | ||||
|     def _setup_sensors(self): | ||||
|         # Enable 6 axis sensors | ||||
|         self._write_output_report(b'\x01', b'\x40', b'\x01') | ||||
|         # It needs delta time to update the setting | ||||
|         time.sleep(0.02) | ||||
|         # Change format of input report | ||||
|         self._write_output_report(b'\x01', b'\x03', b'\x30') | ||||
| 
 | ||||
|     @staticmethod | ||||
|     def _to_int16le_from_2bytes(hbytebe, lbytebe): | ||||
|         uint16le = (lbytebe << 8) | hbytebe | ||||
|         int16le = uint16le if uint16le < 32768 else uint16le - 65536 | ||||
|         return int16le | ||||
| 
 | ||||
|     def _get_nbit_from_input_report(self, offset_byte, offset_bit, nbit): | ||||
|         byte = self._input_report[offset_byte] | ||||
|         return (byte >> offset_bit) & ((1 << nbit) - 1) | ||||
| 
 | ||||
|     def __del__(self): | ||||
|         self._close() | ||||
| 
 | ||||
|     def set_gyro_calibration(self, offset_xyz=None, coeff_xyz=None): | ||||
|         if offset_xyz: | ||||
|             self._GYRO_OFFSET_X, \ | ||||
|             self._GYRO_OFFSET_Y, \ | ||||
|             self._GYRO_OFFSET_Z = offset_xyz | ||||
|         if coeff_xyz: | ||||
|             cx, cy, cz = coeff_xyz | ||||
|             self._GYRO_COEFF_X = 0x343b / cx if cx != 0x343b else 1 | ||||
|             self._GYRO_COEFF_Y = 0x343b / cy if cy != 0x343b else 1 | ||||
|             self._GYRO_COEFF_Z = 0x343b / cz if cz != 0x343b else 1 | ||||
| 
 | ||||
|     def set_accel_calibration(self, offset_xyz=None, coeff_xyz=None): | ||||
|         if offset_xyz and coeff_xyz: | ||||
|             self._ACCEL_OFFSET_X, \ | ||||
|             self._ACCEL_OFFSET_Y, \ | ||||
|             self._ACCEL_OFFSET_Z = offset_xyz | ||||
| 
 | ||||
|             cx, cy, cz = coeff_xyz | ||||
|             self._ACCEL_COEFF_X = (1.0 / (cx - self._ACCEL_OFFSET_X)) * 4.0 | ||||
|             self._ACCEL_COEFF_Y = (1.0 / (cy - self._ACCEL_OFFSET_Y)) * 4.0 | ||||
|             self._ACCEL_COEFF_Z = (1.0 / (cz - self._ACCEL_OFFSET_Z)) * 4.0 | ||||
| 
 | ||||
| 
 | ||||
|     def get_actual_stick_value(self, pre_cal, orientation):  # X/Horizontal = 0, Y/Vertical = 1 | ||||
|         diff = pre_cal - self.stick_cal[2 + orientation] | ||||
|         if (abs(diff) < self.deadzone): | ||||
|             return 0 | ||||
|         elif diff > 0:  # Axis is above center | ||||
|             return diff / self.stick_cal[orientation] | ||||
|         else: | ||||
|             return diff / self.stick_cal[4 + orientation] | ||||
| 
 | ||||
|     def register_update_hook(self, callback): | ||||
|         self._input_hooks.append(callback) | ||||
|         return callback  # this makes it so you could use it as a decorator | ||||
| 
 | ||||
|     def is_left(self): | ||||
|         return self.product_id == JOYCON_L_PRODUCT_ID | ||||
| 
 | ||||
|     def is_right(self): | ||||
|         return self.product_id == JOYCON_R_PRODUCT_ID | ||||
| 
 | ||||
|     def get_battery_charging(self): | ||||
|         return self._get_nbit_from_input_report(2, 4, 1) | ||||
| 
 | ||||
|     def get_battery_level(self): | ||||
|         return self._get_nbit_from_input_report(2, 5, 3) | ||||
| 
 | ||||
|     def get_button_y(self): | ||||
|         return self._get_nbit_from_input_report(3, 0, 1) | ||||
| 
 | ||||
|     def get_button_x(self): | ||||
|         return self._get_nbit_from_input_report(3, 1, 1) | ||||
| 
 | ||||
|     def get_button_b(self): | ||||
|         return self._get_nbit_from_input_report(3, 2, 1) | ||||
| 
 | ||||
|     def get_button_a(self): | ||||
|         return self._get_nbit_from_input_report(3, 3, 1) | ||||
| 
 | ||||
|     def get_button_right_sr(self): | ||||
|         return self._get_nbit_from_input_report(3, 4, 1) | ||||
| 
 | ||||
|     def get_button_right_sl(self): | ||||
|         return self._get_nbit_from_input_report(3, 5, 1) | ||||
| 
 | ||||
|     def get_button_r(self): | ||||
|         return self._get_nbit_from_input_report(3, 6, 1) | ||||
| 
 | ||||
|     def get_button_zr(self): | ||||
|         return self._get_nbit_from_input_report(3, 7, 1) | ||||
| 
 | ||||
|     def get_button_minus(self): | ||||
|         return self._get_nbit_from_input_report(4, 0, 1) | ||||
| 
 | ||||
|     def get_button_plus(self): | ||||
|         return self._get_nbit_from_input_report(4, 1, 1) | ||||
| 
 | ||||
|     def get_button_r_stick(self): | ||||
|         return self._get_nbit_from_input_report(4, 2, 1) | ||||
| 
 | ||||
|     def get_button_l_stick(self): | ||||
|         return self._get_nbit_from_input_report(4, 3, 1) | ||||
| 
 | ||||
|     def get_button_home(self): | ||||
|         return self._get_nbit_from_input_report(4, 4, 1) | ||||
| 
 | ||||
|     def get_button_capture(self): | ||||
|         return self._get_nbit_from_input_report(4, 5, 1) | ||||
| 
 | ||||
|     def get_button_charging_grip(self): | ||||
|         return self._get_nbit_from_input_report(4, 7, 1) | ||||
| 
 | ||||
|     def get_button_down(self): | ||||
|         return self._get_nbit_from_input_report(5, 0, 1) | ||||
| 
 | ||||
|     def get_button_up(self): | ||||
|         return self._get_nbit_from_input_report(5, 1, 1) | ||||
| 
 | ||||
|     def get_button_right(self): | ||||
|         return self._get_nbit_from_input_report(5, 2, 1) | ||||
| 
 | ||||
|     def get_button_left(self): | ||||
|         return self._get_nbit_from_input_report(5, 3, 1) | ||||
| 
 | ||||
|     def get_button_left_sr(self): | ||||
|         return self._get_nbit_from_input_report(5, 4, 1) | ||||
| 
 | ||||
|     def get_button_left_sl(self): | ||||
|         return self._get_nbit_from_input_report(5, 5, 1) | ||||
| 
 | ||||
|     def get_button_l(self): | ||||
|         return self._get_nbit_from_input_report(5, 6, 1) | ||||
| 
 | ||||
|     def get_button_zl(self): | ||||
|         return self._get_nbit_from_input_report(5, 7, 1) | ||||
| 
 | ||||
|     def get_stick_left_horizontal(self): | ||||
|         if not self.is_left(): | ||||
|             return 0 | ||||
| 
 | ||||
|         pre_cal = self._get_nbit_from_input_report(6, 0, 8) \ | ||||
|             | (self._get_nbit_from_input_report(7, 0, 4) << 8) | ||||
|         return self.get_actual_stick_value(pre_cal, 0) | ||||
| 
 | ||||
|     def get_stick_left_vertical(self): | ||||
|         if not self.is_left(): | ||||
|             return 0 | ||||
| 
 | ||||
|         pre_cal = self._get_nbit_from_input_report(7, 4, 4) \ | ||||
|             | (self._get_nbit_from_input_report(8, 0, 8) << 4) | ||||
|         return self.get_actual_stick_value(pre_cal, 1) | ||||
| 
 | ||||
|     def get_stick_right_horizontal(self): | ||||
|         if self.is_left(): | ||||
|             return 0 | ||||
| 
 | ||||
|         pre_cal = self._get_nbit_from_input_report(9, 0, 8) \ | ||||
|             | (self._get_nbit_from_input_report(10, 0, 4) << 8) | ||||
|         return self.get_actual_stick_value(pre_cal, 0) | ||||
| 
 | ||||
|     def get_stick_right_vertical(self): | ||||
|         if self.is_left(): | ||||
|             return 0 | ||||
| 
 | ||||
|         pre_cal = self._get_nbit_from_input_report(10, 4, 4) \ | ||||
|             | (self._get_nbit_from_input_report(11, 0, 8) << 4) | ||||
|         return self.get_actual_stick_value(pre_cal, 1) | ||||
| 
 | ||||
|     def get_accels(self): | ||||
|         input_report = bytes(self._input_report) | ||||
| 
 | ||||
|         x = self.get_accel_x(input_report) | ||||
|         y = self.get_accel_y(input_report) | ||||
|         z = self.get_accel_z(input_report) | ||||
| 
 | ||||
|         return (x, y, z) | ||||
| 
 | ||||
|     def get_accel_x(self, input_report=None, sample_idx=0): | ||||
|         if not input_report: | ||||
|             input_report = self._input_report | ||||
| 
 | ||||
|         if sample_idx not in (0, 1, 2): | ||||
|             raise IndexError('sample_idx should be between 0 and 2') | ||||
|         data = self._to_int16le_from_2bytes( | ||||
|             input_report[13 + sample_idx * 12], | ||||
|             input_report[14 + sample_idx * 12]) | ||||
|         return data * self._ACCEL_COEFF_X | ||||
| 
 | ||||
|     def get_accel_y(self, input_report=None, sample_idx=0): | ||||
|         if not input_report: | ||||
|             input_report = self._input_report | ||||
| 
 | ||||
|         if sample_idx not in (0, 1, 2): | ||||
|             raise IndexError('sample_idx should be between 0 and 2') | ||||
|         data = self._to_int16le_from_2bytes( | ||||
|             input_report[15 + sample_idx * 12], | ||||
|             input_report[16 + sample_idx * 12]) | ||||
|         return data * self._ACCEL_COEFF_Y * (1 if self.is_left() else -1) | ||||
| 
 | ||||
|     def get_accel_z(self, input_report=None, sample_idx=0): | ||||
|         if not input_report: | ||||
|             input_report = self._input_report | ||||
| 
 | ||||
|         if sample_idx not in (0, 1, 2): | ||||
|             raise IndexError('sample_idx should be between 0 and 2') | ||||
|         data = self._to_int16le_from_2bytes( | ||||
|             input_report[17 + sample_idx * 12], | ||||
|             input_report[18 + sample_idx * 12]) | ||||
|         return data * self._ACCEL_COEFF_Z * (1 if self.is_left() else -1) | ||||
| 
 | ||||
|     def get_gyro_x(self, sample_idx=0): | ||||
|         if sample_idx not in (0, 1, 2): | ||||
|             raise IndexError('sample_idx should be between 0 and 2') | ||||
|         data = self._to_int16le_from_2bytes( | ||||
|             self._input_report[19 + sample_idx * 12], | ||||
|             self._input_report[20 + sample_idx * 12]) | ||||
|         return (data - self._GYRO_OFFSET_X) * self._GYRO_COEFF_X | ||||
| 
 | ||||
|     def get_gyro_y(self, sample_idx=0): | ||||
|         if sample_idx not in (0, 1, 2): | ||||
|             raise IndexError('sample_idx should be between 0 and 2') | ||||
|         data = self._to_int16le_from_2bytes( | ||||
|             self._input_report[21 + sample_idx * 12], | ||||
|             self._input_report[22 + sample_idx * 12]) | ||||
|         return (data - self._GYRO_OFFSET_Y) * self._GYRO_COEFF_Y | ||||
| 
 | ||||
|     def get_gyro_z(self, sample_idx=0): | ||||
|         if sample_idx not in (0, 1, 2): | ||||
|             raise IndexError('sample_idx should be between 0 and 2') | ||||
|         data = self._to_int16le_from_2bytes( | ||||
|             self._input_report[23 + sample_idx * 12], | ||||
|             self._input_report[24 + sample_idx * 12]) | ||||
|         return (data - self._GYRO_OFFSET_Z) * self._GYRO_COEFF_Z | ||||
| 
 | ||||
|     def get_status(self) -> dict: | ||||
|         return { | ||||
|             "battery": { | ||||
|                 "charging": self.get_battery_charging(), | ||||
|                 "level": self.get_battery_level(), | ||||
|             }, | ||||
|             "buttons": { | ||||
|                 "right": { | ||||
|                     "y": self.get_button_y(), | ||||
|                     "x": self.get_button_x(), | ||||
|                     "b": self.get_button_b(), | ||||
|                     "a": self.get_button_a(), | ||||
|                     "sr": self.get_button_right_sr(), | ||||
|                     "sl": self.get_button_right_sl(), | ||||
|                     "r": self.get_button_r(), | ||||
|                     "zr": self.get_button_zr(), | ||||
|                 }, | ||||
|                 "shared": { | ||||
|                     "minus": self.get_button_minus(), | ||||
|                     "plus": self.get_button_plus(), | ||||
|                     "r-stick": self.get_button_r_stick(), | ||||
|                     "l-stick": self.get_button_l_stick(), | ||||
|                     "home": self.get_button_home(), | ||||
|                     "capture": self.get_button_capture(), | ||||
|                     "charging-grip": self.get_button_charging_grip(), | ||||
|                 }, | ||||
|                 "left": { | ||||
|                     "down": self.get_button_down(), | ||||
|                     "up": self.get_button_up(), | ||||
|                     "right": self.get_button_right(), | ||||
|                     "left": self.get_button_left(), | ||||
|                     "sr": self.get_button_left_sr(), | ||||
|                     "sl": self.get_button_left_sl(), | ||||
|                     "l": self.get_button_l(), | ||||
|                     "zl": self.get_button_zl(), | ||||
|                 } | ||||
|             }, | ||||
|             "analog-sticks": { | ||||
|                 "left": { | ||||
|                     "horizontal": self.get_stick_left_horizontal(), | ||||
|                     "vertical": self.get_stick_left_vertical(), | ||||
|                 }, | ||||
|                 "right": { | ||||
|                     "horizontal": self.get_stick_right_horizontal(), | ||||
|                     "vertical": self.get_stick_right_vertical(), | ||||
|                 }, | ||||
|             }, | ||||
|             "accel": { | ||||
|                 "x": self.get_accel_x(), | ||||
|                 "y": self.get_accel_y(), | ||||
|                 "z": self.get_accel_z(), | ||||
|             }, | ||||
|             "gyro": { | ||||
|                 "x": self.get_gyro_x(), | ||||
|                 "y": self.get_gyro_y(), | ||||
|                 "z": self.get_gyro_z(), | ||||
|             }, | ||||
|         } | ||||
| 
 | ||||
|     def set_player_lamp_on(self, on_pattern: int): | ||||
|         self._write_output_report( | ||||
|             b'\x01', b'\x30', | ||||
|             (on_pattern & 0xF).to_bytes(1, byteorder='little')) | ||||
| 
 | ||||
|     def set_player_lamp_flashing(self, flashing_pattern: int): | ||||
|         self._write_output_report( | ||||
|             b'\x01', b'\x30', | ||||
|             ((flashing_pattern & 0xF) << 4).to_bytes(1, byteorder='little')) | ||||
| 
 | ||||
|     def set_player_lamp(self, pattern: int): | ||||
|         self._write_output_report( | ||||
|             b'\x01', b'\x30', | ||||
|             pattern.to_bytes(1, byteorder='little')) | ||||
| 
 | ||||
|     def disconnect_device(self): | ||||
|         self._write_output_report(b'\x01', b'\x06', b'\x00') | ||||
| 
 | ||||
| 
 | ||||
| if __name__ == '__main__': | ||||
|     import pyjoycon.device as d | ||||
|     ids = d.get_L_id() if None not in d.get_L_id() else d.get_R_id() | ||||
| 
 | ||||
|     if None not in ids: | ||||
|         joycon = JoyCon(*ids) | ||||
|         lamp_pattern = 0 | ||||
|         while True: | ||||
|             print(joycon.get_status()) | ||||
|             joycon.set_player_lamp_on(lamp_pattern) | ||||
|             lamp_pattern = (lamp_pattern + 1) & 0xf | ||||
|             time.sleep(0.2) | ||||
							
								
								
									
										142
									
								
								pycon/wrappers.py
									
										
									
									
									
										Normal file
									
								
							
							
						
						
									
										142
									
								
								pycon/wrappers.py
									
										
									
									
									
										Normal file
									
								
							|  | @ -0,0 +1,142 @@ | |||
| from .joycon import JoyCon | ||||
| 
 | ||||
| 
 | ||||
| # Preferably, this class gets merged into the | ||||
| # parent class if approved by the original author | ||||
| class PythonicJoyCon(JoyCon): | ||||
|     """ | ||||
|     A wrapper class for the JoyCon parent class. | ||||
|     This creates a more pythonic interface by | ||||
|      *  using properties instead of requiring java-style getters and setters, | ||||
|      *  bundles related xy/xyz data in tuples | ||||
|      *  bundles the multiple measurements of the | ||||
|         gyroscope and accelerometer into a list | ||||
|      *  Adds the option to invert the y and z axis of the left joycon | ||||
|         to make it match the right joycon. This is enabled by default | ||||
|     """ | ||||
| 
 | ||||
|     def __init__(self, *a, invert_left_ime_yz=True, **kw): | ||||
|         super().__init__(*a, **kw) | ||||
|         self._ime_yz_coeff = -1 if invert_left_ime_yz and self.is_left() else 1 | ||||
| 
 | ||||
|     is_charging   = property(JoyCon.get_battery_charging) | ||||
|     battery_level = property(JoyCon.get_battery_level) | ||||
| 
 | ||||
|     r             = property(JoyCon.get_button_r) | ||||
|     zr            = property(JoyCon.get_button_zr) | ||||
|     plus          = property(JoyCon.get_button_plus) | ||||
|     a             = property(JoyCon.get_button_a) | ||||
|     b             = property(JoyCon.get_button_b) | ||||
|     x             = property(JoyCon.get_button_x) | ||||
|     y             = property(JoyCon.get_button_y) | ||||
|     stick_r_btn   = property(JoyCon.get_button_r_stick) | ||||
|     home          = property(JoyCon.get_button_home) | ||||
|     right_sr      = property(JoyCon.get_button_right_sr) | ||||
|     right_sl      = property(JoyCon.get_button_right_sl) | ||||
| 
 | ||||
|     l             = property(JoyCon.get_button_l)  # noqa: E741 | ||||
|     zl            = property(JoyCon.get_button_zl) | ||||
|     minus         = property(JoyCon.get_button_minus) | ||||
|     stick_l_btn   = property(JoyCon.get_button_l_stick) | ||||
|     up            = property(JoyCon.get_button_up) | ||||
|     down          = property(JoyCon.get_button_down) | ||||
|     left          = property(JoyCon.get_button_left) | ||||
|     right         = property(JoyCon.get_button_right) | ||||
|     capture       = property(JoyCon.get_button_capture) | ||||
|     left_sr       = property(JoyCon.get_button_left_sr) | ||||
|     left_sl       = property(JoyCon.get_button_left_sl) | ||||
| 
 | ||||
|     set_led_on       = JoyCon.set_player_lamp_on | ||||
|     set_led_flashing = JoyCon.set_player_lamp_flashing | ||||
|     set_led          = JoyCon.set_player_lamp | ||||
|     disconnect       = JoyCon.disconnect_device | ||||
| 
 | ||||
|     @property | ||||
|     def stick_l(self): | ||||
|         return ( | ||||
|             self.get_stick_left_horizontal(), | ||||
|             self.get_stick_left_vertical(), | ||||
|         ) | ||||
| 
 | ||||
|     @property | ||||
|     def stick_r(self): | ||||
|         return ( | ||||
|             self.get_stick_right_horizontal(), | ||||
|             self.get_stick_right_vertical(), | ||||
|         ) | ||||
| 
 | ||||
|     @property | ||||
|     def accel(self): | ||||
|         c = self._ime_yz_coeff | ||||
|         return [ | ||||
|             ( | ||||
|                 self.get_accel_x(i), | ||||
|                 self.get_accel_y(i) * c, | ||||
|                 self.get_accel_z(i) * c, | ||||
|             ) | ||||
|             for i in range(3) | ||||
|         ] | ||||
| 
 | ||||
|     @property | ||||
|     def accel_in_g(self): | ||||
|         c = 4.0 / 0x4000 | ||||
|         c2 = c * self._ime_yz_coeff | ||||
|         return [ | ||||
|             ( | ||||
|                 self.get_accel_x(i) * c, | ||||
|                 self.get_accel_y(i) * c2, | ||||
|                 self.get_accel_z(i) * c2, | ||||
|             ) | ||||
|             for i in range(3) | ||||
|         ] | ||||
| 
 | ||||
|     @property | ||||
|     def gyro(self): | ||||
|         c = self._ime_yz_coeff | ||||
|         return [ | ||||
|             ( | ||||
|                 self.get_gyro_x(i), | ||||
|                 self.get_gyro_y(i) * c, | ||||
|                 self.get_gyro_z(i) * c, | ||||
|             ) | ||||
|             for i in range(3) | ||||
|         ] | ||||
| 
 | ||||
|     @property | ||||
|     def gyro_in_deg(self): | ||||
|         c = 0.06103 | ||||
|         c2 = c * self._ime_yz_coeff | ||||
|         return [ | ||||
|             ( | ||||
|                 self.get_gyro_x(i) * c, | ||||
|                 self.get_gyro_y(i) * c2, | ||||
|                 self.get_gyro_z(i) * c2, | ||||
|             ) | ||||
|             for i in range(3) | ||||
|         ] | ||||
| 
 | ||||
|     @property | ||||
|     def gyro_in_rad(self): | ||||
|         c = 0.0001694 * 3.1415926536 | ||||
|         c2 = c * self._ime_yz_coeff | ||||
|         return [ | ||||
|             ( | ||||
|                 self.get_gyro_x(i) * c, | ||||
|                 self.get_gyro_y(i) * c2, | ||||
|                 self.get_gyro_z(i) * c2, | ||||
|             ) | ||||
|             for i in range(3) | ||||
|         ] | ||||
| 
 | ||||
|     @property | ||||
|     def gyro_in_rot(self): | ||||
|         c = 0.0001694 | ||||
|         c2 = c * self._ime_yz_coeff | ||||
|         return [ | ||||
|             ( | ||||
|                 self.get_gyro_x(i) * c, | ||||
|                 self.get_gyro_y(i) * c2, | ||||
|                 self.get_gyro_z(i) * c2, | ||||
|             ) | ||||
|             for i in range(3) | ||||
|         ] | ||||
		Loading…
	
	Add table
		Add a link
		
	
		Reference in a new issue