Switch to extended IR mode to match 0x33 report, add basic accel parserHEADmaster

1 files changed, 53 insertions, 4 deletions

diff --git a/starbit.c b/starbit.c
index a099af7..68f39e6 100644
--- a/starbit.c
+++ b/starbit.c
@@ -19,13 +19,52 @@ struct point {
     int is_ir;
 };
 
+struct accel {
+    int x;
+    int y;
+    int z;
+};
+
 struct wiimouse_dev {
     struct hid_device* hdev;
     struct input_dev* in_dev;
     struct point points[4];
+    struct accel accel;
 };
 
-static void parse_basic_mode_ir(struct wiimouse_dev* wdev, __u8 data[10]) {
+
+static void parse_accel(struct wiimouse_dev* wdev, __u8 data[5]) {
+    // Byte 0 of button data contains 2 lsbs of X - 10 bit precision
+    // Byte 1 contains lsbs of Y and Z - 9 bit precision
+    wdev->accel.x = (data[2] << 2) | (data[0] & 0b01100000) >> 5;
+    wdev->accel.y = data[3];
+    // Get bit 0
+    int y_bit0 = data[3] & 0x01;
+    // Get bit 1
+    int y_bit1 = data[1] & 0b00100000 >> 5;
+    // Clear last bit
+    wdev->accel.y &= 0b11111110;
+    // Apply bit 1 (pre-shift)
+    wdev->accel.y |= y_bit1;
+    // Shift to left by 1
+    wdev->accel.y <<= 1;
+    wdev->accel.y |= y_bit0;
+
+    wdev->accel.z = data[4];
+    // Get bit 0
+    int z_bit0 = data[4] & 0x01;
+    // Get bit 1
+    int z_bit1 = data[1] & 0b01000000 >> 6;
+    // Clear last bit
+    wdev->accel.z &= 0b11111110;
+    // Apply bit 1 (pre-shift)
+    wdev->accel.z |= z_bit1;
+    // Shift to left by 1
+    wdev->accel.z <<= 1;
+    wdev->accel.z |=  z_bit0;
+}
+
+static void parse_extended_mode_ir(struct wiimouse_dev* wdev, __u8 data[10]) {
     wdev->points[0].x = data[0] << 2 | ((data[2] & 0x30) >> 4);
     wdev->points[0].y = data[1] << 2 | ((data[2] & 0xc0) >> 6);
     wdev->points[1].x = data[3] << 2 | (data[2] & 0x03);
@@ -75,10 +114,15 @@ static int wiimote_ir_init(struct hid_device* hdev) {
     }
     msleep(IR_SLEEP);
 
-    __u8 enable[22] = { 0x16, 0x04, 0xb0, 0x00, 0x30, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, };
+    // Can be 0x08 or 0x01?? Weird behavior with wiimote... Wii writes 0x01 while libgc does former...
+    // Using 0x08 here (wii logic) which seems to work
+    __u8 enable[22] = { 0x16, 0x04, 0xb0, 0x00, 0x30, 0x01, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, };
+    // Max sensitivity
     __u8 sensitive_1[22] = { 0x16, 0x04, 0xb0, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, };
     __u8 sensitive_2[22] = { 0x16, 0x04, 0xb0, 0x00, 0x1a, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, };
-    __u8 mode[22] = { 0x16, 0x04, 0xb0, 0x00, 0x33, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, };
+    // Mode is 0x03 - extended mode
+    __u8 mode[22] = { 0x16, 0x04, 0xb0, 0x00, 0x33, 0x01, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, };
+    // Final rewrite to complete init
     __u8 final[22] = { 0x16, 0x04, 0xb0, 0x00, 0x30, 0x01, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, };
     if ((ret = hid_hw_output_report(hdev, (__u8*)enable, 22)) < 0) {
           printk(KERN_DEBUG "eeprom 1");
@@ -125,6 +169,9 @@ static int wiimote_init(struct hid_device* hdev) {
     if ((ret = hid_hw_output_report(hdev, (__u8*)rumble, 2)) < 0) return ret;
     __u8 led_p1[2] = { HID_REPORT_ID_WIIMOTE_LED_ENABLE, HID_REPORT_DATA_WIIMOTE_LED_ENABLE_P1 };
     if ((ret = hid_hw_output_report(hdev, (__u8*)led_p1, 2)) < 0) return ret;
+    // 0x33 - Core Buttons and Accelerometer with 12 IR bytes
+    // Format: (a1) 33 BB BB AA AA AA II II II II II II II II II II II II 
+    // 0x04 means continous reporting
     __u8 report_mode[3] = { 0x12, 0x04, 0x33 };
     if ((ret = hid_hw_output_report(hdev, (__u8*)report_mode, 3)) < 0) return ret;
 
@@ -135,12 +182,14 @@ static int wiimote_init(struct hid_device* hdev) {
 static int wiimote_event(struct hid_device* hdev, struct hid_report* report, u8 *data, int size) {
     printk(KERN_DEBUG "Report ID: %02x, Size: %d", report->id, size);
     printk(KERN_DEBUG "Button Data: %02x, %02x", data[1], data[2]);
+    printk(KERN_DEBUG "Accel Data: %02x,%02x,%02x", data[3], data[4], data[5]);
     printk(KERN_DEBUG "IR Data: %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x", data[6], data[7], data[8], data[9], data[10], data[11], data[12], data[13], data[14], data[15]);
 
     struct wiimouse_dev* wdev = hid_get_drvdata(hdev);
 
     //Parse and save IR data
-    parse_basic_mode_ir(wdev, data + 6);
+    parse_extended_mode_ir(wdev, data + 6);
+    parse_accel(wdev, data);
     if (data[2] & 0x08) {
         printk(KERN_DEBUG "A button was pressed\n");
         struct input_dev* in_dev = wdev->in_dev;