diff --git a/joycontrol/report.py b/joycontrol/report.py index c6db581..0801592 100644 --- a/joycontrol/report.py +++ b/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