implement accelerometer

app/backend.cpp

@@ -148,7 +148,7 @@ void Backend::updateTouch(int x, int y)
     }
 }
 
-void Backend::setButton(int button, int16_t value)
+void Backend::setButton(int button, int32_t value)
 {
     if (m_pipe) {
         m_pipeMutex.lock();

app/backend.h

@@ -53,7 +53,7 @@ public slots:
     void sync(const QString &wirelessInterface, uint16_t code);
     void connectToConsole(const QString &wirelessInterface);
     void updateTouch(int x, int y);
-    void setButton(int button, int16_t value);
+    void setButton(int button, int32_t value);
 
 private:
     BackendPipe *m_pipe;

app/gamepadhandler.cpp

@@ -52,6 +52,20 @@ void GamepadHandler::setController(int index)
     m_mutex.unlock();
 }
 
+void EnableSensorIfAvailable(SDL_GameController *controller, SDL_SensorType sensor)
+{
+    if (SDL_GameControllerHasSensor(controller, sensor)) {
+        SDL_GameControllerSetSensorEnabled(controller, sensor, SDL_TRUE);
+    }
+}
+
+int32_t packFloat(float f)
+{
+    int32_t x;
+    memcpy(&x, &f, sizeof(int32_t));
+    return x;
+}
+
 void GamepadHandler::run()
 {
     m_mutex.lock();
@@ -63,6 +77,8 @@ void GamepadHandler::run()
             }
             if (m_nextGamepad != -1) {
                 m_controller = SDL_GameControllerOpen(m_nextGamepad);
+                EnableSensorIfAvailable(m_controller, SDL_SENSOR_ACCEL);
+                EnableSensorIfAvailable(m_controller, SDL_SENSOR_GYRO);
             } else {
                 m_controller = nullptr;
             }
@@ -110,6 +126,17 @@ void GamepadHandler::run()
                     }
                 }
                 break;
+            case SDL_CONTROLLERSENSORUPDATE:
+                if (event.csensor.sensor == SDL_SENSOR_ACCEL) {
+                    emit buttonStateChanged(VANILLA_SENSOR_ACCEL_X, packFloat(event.csensor.data[0]));
+                    emit buttonStateChanged(VANILLA_SENSOR_ACCEL_Y, packFloat(event.csensor.data[1]));
+                    emit buttonStateChanged(VANILLA_SENSOR_ACCEL_Z, packFloat(event.csensor.data[2]));
+                } else if (event.csensor.sensor == SDL_SENSOR_GYRO) {
+                    emit buttonStateChanged(VANILLA_SENSOR_GYRO_PITCH, packFloat(event.csensor.data[0]));
+                    emit buttonStateChanged(VANILLA_SENSOR_GYRO_YAW, packFloat(event.csensor.data[1]));
+                    emit buttonStateChanged(VANILLA_SENSOR_GYRO_ROLL, packFloat(event.csensor.data[2]));
+                }
+                break;
             }
         }
 

app/gamepadhandler.h

@@ -18,7 +18,7 @@ public:
 signals:
     void gamepadsChanged();
 
-    void buttonStateChanged(int button, int16_t value);
+    void buttonStateChanged(int button, int32_t value);
 
 public slots:
     void run();

lib/gamepad/input.c

@@ -1,6 +1,7 @@
 #include "input.h"
 
 #include <arpa/inet.h>
+#include <math.h>
 #include <pthread.h>
 #include <stdint.h>
 #include <stdlib.h>
@@ -11,29 +12,25 @@
 #include "vanilla.h"
 #include "util.h"
 
-#define MIN(a,b) (((a)<(b))?(a):(b))
-#define MAX(a,b) (((a)>(b))?(a):(b))
-#define CLAMP(x, min, max) (MIN(MAX(x, min), max))
-
 #pragma pack(push, 1)
 
 typedef struct {
     // Little endian
+    int16_t z;
     int16_t x;
     int16_t y;
-    int16_t z;
-} InputPacketAccelerometerWiiU;
+} InputPacketAccelerometer;
 
 typedef struct {
     // Little endian
-    signed roll : 24;
     signed yaw : 24;
     signed pitch : 24;
-} InputPacketGyroscopeWiiU;
+    signed roll : 24;
+} InputPacketGyroscope;
 
 typedef struct {
     signed char unknown[6];
-} InputPacketMagnetWiiU;
+} InputPacketMagnet;
 
 typedef struct {
     unsigned pad : 1;
@@ -52,7 +49,7 @@ typedef struct {
 } TouchScreenState;
 
 pthread_mutex_t button_mtx;
-int16_t current_buttons[VANILLA_BTN_COUNT] = {0};
+int32_t current_buttons[VANILLA_BTN_COUNT] = {0};
 int current_touch_x = -1;
 int current_touch_y = -1;
 
@@ -67,9 +64,9 @@ typedef struct {
     uint16_t stick_right_x;
     uint16_t stick_right_y;
     uint8_t audio_volume;
-    InputPacketAccelerometerWiiU accelerometer;
-    InputPacketGyroscopeWiiU gyroscope;
-    InputPacketMagnetWiiU magnet;
+    InputPacketAccelerometer accelerometer;
+    InputPacketGyroscope gyroscope;
+    InputPacketMagnet magnet;
     TouchScreenState touchscreen; // byte 36 - 76
     unsigned char unknown_0[4];
     uint8_t extra_buttons;
@@ -79,7 +76,7 @@ typedef struct {
 
 #pragma pack(pop)
 
-void set_button_state(int button, int16_t value)
+void set_button_state(int button, int32_t value)
 {
     pthread_mutex_lock(&button_mtx);
     current_buttons[button] = value;
@@ -114,10 +111,8 @@ uint16_t resolve_axis_value(float axis, float neg, float pos, int flip)
     return ((int) (val * 1024)) + 2048;
 }
 
-int64_t scale_x_touch_value(uint16_t val)
+int64_t scale_x_touch_value(int64_t v)
 {
-    int64_t v = val;
-
     // Scales 0-854 to 0-4096 with a 2.5% margin on each side
     const int scale_percent = 95;
 
@@ -130,10 +125,8 @@ int64_t scale_x_touch_value(uint16_t val)
     return v;
 }
 
-int64_t scale_y_touch_value(uint16_t val)
+int64_t scale_y_touch_value(int64_t v)
 {
-    int64_t v = val;
-
     // Scales 0-854 to 0-4096 with a 5% margin on the bottom and 3% margin on the top (I don't know why, but these values worked best)
     const int scale_percent = 92;
 
@@ -147,6 +140,23 @@ int64_t scale_y_touch_value(uint16_t val)
     return v;
 }
 
+float unpack_float(int32_t x)
+{
+    float f;
+    memcpy(&f, &x, sizeof(int32_t));
+    return f;
+}
+
+enum BatteryStatus {
+    BATTERY_STATUS_CHARGING,
+    BATTERY_STATUS_UNKNOWN,
+    BATTERY_STATUS_VERY_LOW,
+    BATTERY_STATUS_LOW,
+    BATTERY_STATUS_MEDIUM,
+    BATTERY_STATUS_HIGH,
+    BATTERY_STATUS_FULL,
+};
+
 void send_input(int socket_hid)
 {
     InputPacket ip;
@@ -156,6 +166,8 @@ void send_input(int socket_hid)
 
     pthread_mutex_lock(&button_mtx);
 
+    ip.touchscreen.points[9].x.extra = reverse_bits(BATTERY_STATUS_VERY_LOW, 3);
+
     if (current_touch_x >= 0 && current_touch_y >= 0) {
         for (int i = 0; i < 10; i++) {
             ip.touchscreen.points[i].x.pad = 1;
@@ -163,16 +175,18 @@ void send_input(int socket_hid)
             ip.touchscreen.points[i].x.value = reverse_bits(scale_x_touch_value(current_touch_x), 12);
             ip.touchscreen.points[i].y.value = reverse_bits(scale_y_touch_value(current_touch_y), 12);
         }
-        ip.touchscreen.points[0].x.extra = 0;
+
         ip.touchscreen.points[0].y.extra = reverse_bits(2, 3);
         ip.touchscreen.points[1].x.extra = reverse_bits(7, 3);
         ip.touchscreen.points[1].y.extra = reverse_bits(3, 3);
-        for (int byte = 0; byte < sizeof(ip.touchscreen); byte += 2) {
-            unsigned char *touchscreen_bytes = (unsigned char *) (&ip.touchscreen);
-            unsigned char first = (unsigned char) reverse_bits(touchscreen_bytes[byte], 8);
-            touchscreen_bytes[byte] = (unsigned char) reverse_bits(touchscreen_bytes[byte + 1], 8);
-            touchscreen_bytes[byte + 1] = first;
-        }
+    }
+
+    for (int byte = 0; byte < sizeof(ip.touchscreen); byte += 2)
+    {
+        unsigned char *touchscreen_bytes = (unsigned char *)(&ip.touchscreen);
+        unsigned char first = (unsigned char)reverse_bits(touchscreen_bytes[byte], 8);
+        touchscreen_bytes[byte] = (unsigned char)reverse_bits(touchscreen_bytes[byte + 1], 8);
+        touchscreen_bytes[byte + 1] = first;
     }
 
     uint16_t button_mask = 0;
@@ -207,6 +221,14 @@ void send_input(int socket_hid)
     ip.stick_right_x = resolve_axis_value(current_buttons[VANILLA_AXIS_R_X], current_buttons[VANILLA_AXIS_R_LEFT], current_buttons[VANILLA_AXIS_R_RIGHT], 0);
     ip.stick_right_y = resolve_axis_value(current_buttons[VANILLA_AXIS_R_Y], current_buttons[VANILLA_AXIS_R_UP], current_buttons[VANILLA_AXIS_R_DOWN], 1);
 
+    ip.accelerometer.x = unpack_float(current_buttons[VANILLA_SENSOR_ACCEL_X]) * -800;
+    ip.accelerometer.y = unpack_float(current_buttons[VANILLA_SENSOR_ACCEL_Y]) * -800;
+    ip.accelerometer.z = unpack_float(current_buttons[VANILLA_SENSOR_ACCEL_Z]) * 800;
+
+    // ip.gyroscope.yaw = (unpack_float(current_buttons[VANILLA_SENSOR_GYRO_YAW]) * (180.0f/M_PI)) / ((200.0f * 6.0f) / 154000.0f);
+    // ip.gyroscope.pitch = (unpack_float(current_buttons[VANILLA_SENSOR_GYRO_PITCH]) * (180.0f/M_PI)) / ((200.0f * 6.0f) / 154000.0f);
+    // ip.gyroscope.roll = (unpack_float(current_buttons[VANILLA_SENSOR_GYRO_ROLL]) * (180.0f/M_PI)) / ((200.0f * 6.0f) / 154000.0f);
+
     pthread_mutex_unlock(&button_mtx);
 
     ip.seq_id = htons(seq_id);
@@ -225,7 +247,7 @@ void *listen_input(void *x)
 
     do {
         send_input(info->socket_hid);
-        usleep(10 * 1000); // Produces 100Hz input, probably no need to go higher for the Wii U, but potentially could
+        usleep(5 * 1000); // Produces 200Hz input, probably no need to go higher for the Wii U, but potentially could
     } while (!is_interrupted());
 
     pthread_mutex_destroy(&button_mtx);

lib/gamepad/input.h

@@ -4,7 +4,7 @@
 #include <stdint.h>
 
 void *listen_input(void *x);
-void set_button_state(int button, int16_t value);
+void set_button_state(int button, int32_t value);
 void set_touch_state(int x, int y);
 
 #endif // GAMEPAD_INPUT_H

lib/util.h

@@ -5,6 +5,10 @@
 #include <stdio.h>
 #include <sys/types.h>
 
+#define MIN(a,b) (((a)<(b))?(a):(b))
+#define MAX(a,b) (((a)>(b))?(a):(b))
+#define CLAMP(x, min, max) (MIN(MAX(x, min), max))
+
 size_t read_line_from_fd(int fd, char *output, size_t max_output_size);
 size_t read_line_from_file(FILE *file, char *output, size_t max_output_size);
 size_t get_home_directory(char *buf, size_t buf_size);

lib/vanilla.c

@@ -75,7 +75,7 @@ void vanilla_stop()
     force_interrupt();
 }
 
-void vanilla_set_button(int button, int16_t value)
+void vanilla_set_button(int button, int32_t value)
 {
     set_button_state(button, value);
 }

lib/vanilla.h

@@ -16,7 +16,8 @@ extern "C" {
 #define VANILLA_UNKNOWN_COMMAND     -4
 #define VANILLA_INVALID_ARGUMENT    -5
 
-enum VanillaGamepadButtons {
+enum VanillaGamepadButtons
+{
     VANILLA_BTN_A,
     VANILLA_BTN_B,
     VANILLA_BTN_X,
@@ -46,6 +47,12 @@ enum VanillaGamepadButtons {
     VANILLA_AXIS_R_UP,
     VANILLA_AXIS_R_RIGHT,
     VANILLA_AXIS_R_DOWN,
+    VANILLA_SENSOR_ACCEL_X,
+    VANILLA_SENSOR_ACCEL_Y,
+    VANILLA_SENSOR_ACCEL_Z,
+    VANILLA_SENSOR_GYRO_PITCH,
+    VANILLA_SENSOR_GYRO_YAW,
+    VANILLA_SENSOR_GYRO_ROLL,
     VANILLA_BTN_COUNT
 };
 
@@ -63,14 +70,14 @@ typedef void (*vanilla_event_handler_t)(void *context, int event_type, const cha
 
 /**
  * Attempt to sync with the console
- * 
+ *
  * This will block until the task is over or vanilla_stop() is called from another thread.
  */
-int vanilla_sync_with_console(const char *wireless_interface, uint16_t code);  
+int vanilla_sync_with_console(const char *wireless_interface, uint16_t code);
 
 /**
  * Attempt gameplay connection with console
- * 
+ *
  * This will block until the task is over or vanilla_stop() is called from another thread.
  */
 int vanilla_connect_to_console(const char *wireless_interface, vanilla_event_handler_t event_handler, void *context);
@@ -82,26 +89,28 @@ int vanilla_has_config();
 
 /**
  * Attempt to stop the current action
- * 
+ *
  * This can be called from another thread to safely exit a blocking call to vanilla_sync_with_console() or vanilla_connect_to_console().
  */
 void vanilla_stop();
 
 /**
  * Set button/axis state
- * 
+ *
  * This can be called from another thread to change the button state while vanilla_connect_to_console() is running.
- * 
+ *
  * For buttons, anything non-zero will be considered a press.
- * For axes, the range is -32,768 - 32,767.
+ * For axes, the range is signed 16-bit (-32,768 to 32,767).
+ * For accelerometers, cast a float value in m/s^2.
+ * For gyroscopes, cast a float value in radians per second.
  */
-void vanilla_set_button(int button, int16_t value);
+void vanilla_set_button(int button, int32_t value);
 
 /**
  * Set touch screen coordinates to `x` and `y`
- * 
+ *
  * This can be called from another thread to change the button state while vanilla_connect_to_console() is running.
- * 
+ *
  * `x` and `y` are expected to be in gamepad screen coordinates (0x0 to 853x479).
  * If either `x` or `y` are -1, this point will be disabled.
  */

pipe/main.c

@@ -216,7 +216,7 @@ int main()
         case VANILLA_PIPE_IN_BUTTON:
         {
             int32_t button_id;
-            int16_t button_value;
+            int32_t button_value;
             read(fd_in, &button_id, sizeof(button_id));
             read(fd_in, &button_value, sizeof(button_value));
             vanilla_set_button(button_id, button_value);