rumble test

joycontrol/report.py

@@ -1,3 +1,4 @@
+import math
 from enum import Enum
 
 from joycontrol.controller import Controller
@@ -250,6 +251,56 @@ class OutputReport:
     def get_rumble_data(self):
         return self.data[3:11]
 
+    @staticmethod
+    def _encode_rumble_data(freq, amp):
+        if not 0 <= freq <= 1252:
+            raise ValueError('Frequency must be in [0, 1252].')
+
+        # Float frequency to hex conversion
+        encoded_hex_freq = int(round(math.log2(freq / 10) * 32))
+
+        # Convert to Joy-Con HF range. Range in big-endian: 0x0004-0x01FC with +0x0004 steps.
+        hf = (encoded_hex_freq - 0x60) * 4
+        # Convert to Joy-Con LF range. Range: 0x01-0x7F.
+        lf = encoded_hex_freq - 0x40
+
+        # Float amplitude to hex conversion
+        encoded_hex_amp = 0
+        if amp > 0.23:
+            encoded_hex_amp = int(round(math.log2(amp * 8.7) * 32))
+        elif amp > 0.12:
+            encoded_hex_amp = int(round(math.log2(amp * 17) * 16))
+        else:
+            # TBD
+            pass
+
+        hf_amp = encoded_hex_amp << 1
+        lf_amp = (encoded_hex_amp >> 1) + 0x40
+
+        return hf, hf_amp, lf, lf_amp
+
+    def set_left_rumble_data(self, freq, amp):
+        hf, hf_amp, lf, lf_amp = OutputReport._encode_rumble_data(freq, amp)
+
+        # Byte swapping
+        self.data[3] = hf & 0xFF
+        self.data[4] = hf_amp + ((hf >> 8) & 0xFF)  # Add amp + 1st byte of frequency to amplitude byte
+
+        # Byte swapping
+        self.data[5] = lf + ((lf_amp >> 8) & 0xFF)  # Add freq + 1st byte of LF amplitude to the frequency byte
+        self.data[6] = lf_amp & 0xFF
+
+    def set_right_rumble_data(self, freq, amp):
+        hf, hf_amp, lf, lf_amp = OutputReport._encode_rumble_data(freq, amp)
+
+        # Byte swapping
+        self.data[7] = hf & 0xFF
+        self.data[8] = hf_amp + ((hf >> 8) & 0xFF)  # Add amp + 1st byte of frequency to amplitude byte
+
+        # Byte swapping
+        self.data[9] = lf + ((lf_amp >> 8) & 0xFF)  # Add freq + 1st byte of LF amplitude to the frequency byte
+        self.data[10] = lf_amp & 0xFF
+
     def get_sub_command(self):
         if len(self.data) < 12:
             return None