Implement SixAxis sensor

app/src/main/cpp/emu_jni.cpp

@@ -217,6 +217,18 @@ extern "C" JNIEXPORT void JNICALL Java_emu_skyline_input_InputHandler_00024Compa
         device->SetAxisValue(static_cast<skyline::input::NpadAxisId>(axis), value);
 }
 
+extern "C" JNIEXPORT void JNICALL Java_emu_skyline_input_InputHandler_00024Companion_setMotionState(JNIEnv *env, jobject, jint index, jint motionId, jobject value) {
+    auto input{InputWeak.lock()};
+    if (!input)
+        return; // We don't mind if we miss motion updates while input hasn't been initialized
+
+    const auto motionValue = reinterpret_cast<skyline::input::MotionSensorState*>(env->GetDirectBufferAddress(value));
+
+    auto device{input->npad.controllers[static_cast<size_t>(index)].device};
+    if (device)
+        device->SetMotionValue(static_cast<skyline::input::MotionId>(motionId), motionValue);
+}
+
 extern "C" JNIEXPORT void JNICALL Java_emu_skyline_input_InputHandler_00024Companion_setTouchState(JNIEnv *env, jobject, jintArray pointsJni) {
     using Point = skyline::input::TouchScreenPoint;
 

app/src/main/cpp/skyline/input/npad_device.cpp

@@ -39,6 +39,8 @@ namespace skyline::input {
 
         ResetDeviceProperties();
         controllerInfo = nullptr;
+        sixAxisInfoLeft = nullptr;
+        sixAxisInfoRight = nullptr;
 
         connectionState.raw = 0;
         connectionState.connected = true;
@@ -155,6 +157,10 @@ namespace skyline::input {
         type = newType;
         controllerInfo = &GetControllerInfo();
 
+        sixAxisInfoLeft = &GetSixAxisInfo(MotionId::Left);
+        if (type == NpadControllerType::JoyconDual)
+            sixAxisInfoRight = &GetSixAxisInfo(MotionId::Right);
+
         UpdateSharedMemory();
         updateEvent->Signal();
     }
@@ -170,6 +176,8 @@ namespace skyline::input {
 
         type = NpadControllerType::None;
         controllerInfo = nullptr;
+        sixAxisInfoLeft = nullptr;
+        sixAxisInfoRight = nullptr;
 
         updateEvent->Signal();
         WriteEmptyEntries();
@@ -192,6 +200,25 @@ namespace skyline::input {
         }
     }
 
+    NpadSixAxisInfo &NpadDevice::GetSixAxisInfo(MotionId id) {
+        switch (type) {
+            case NpadControllerType::ProController:
+                return section.fullKeySixAxis;
+            case NpadControllerType::Handheld:
+                return section.handheldSixAxis;
+            case NpadControllerType::JoyconDual:
+                if (id == MotionId::Right)
+                    return section.dualRightSixAxis;
+                return section.dualLeftSixAxis;
+            case NpadControllerType::JoyconLeft:
+                return section.leftSixAxis;
+            case NpadControllerType::JoyconRight:
+                return section.rightSixAxis;
+            default:
+                throw exception("Cannot find corresponding section for ControllerType: {}", type);
+        }
+    }
+
     void NpadDevice::WriteNextEntry(NpadControllerInfo &info, NpadControllerState entry) {
         auto &lastEntry{info.state.at(info.header.currentEntry)};
 
@@ -212,6 +239,26 @@ namespace skyline::input {
         nextEntry.status.raw = connectionState.raw;
     }
 
+    void NpadDevice::WriteNextEntry(NpadSixAxisInfo &info, NpadSixAxisState entry) {
+        auto &lastEntry{info.state.at(info.header.currentEntry)};
+
+        info.header.timestamp = util::GetTimeTicks();
+        info.header.entryCount = std::min(static_cast<u8>(info.header.entryCount + 1), constant::HidEntryCount);
+        info.header.maxEntry = info.header.entryCount - 1;
+        info.header.currentEntry = (info.header.currentEntry < info.header.maxEntry) ? info.header.currentEntry + 1 : 0;
+
+        auto &nextEntry{info.state.at(info.header.currentEntry)};
+
+        nextEntry.globalTimestamp = globalTimestamp;
+        nextEntry.localTimestamp = lastEntry.localTimestamp + 1;
+        nextEntry.deltaTimestamp = entry.deltaTimestamp;
+        nextEntry.accelerometer = entry.accelerometer;
+        nextEntry.gyroscope = entry.gyroscope;
+        nextEntry.rotation = entry.rotation;
+        nextEntry.orientation = entry.orientation;
+        nextEntry.attribute = entry.attribute;
+    }
+
     void NpadDevice::WriteEmptyEntries() {
         NpadControllerState emptyEntry{};
 
@@ -250,6 +297,12 @@ namespace skyline::input {
             WriteNextEntry(*controllerInfo, controllerState);
         WriteNextEntry(section.defaultController, defaultState);
 
+        // TODO: SixAxis should be updated every 5 ms
+        if (sixAxisInfoLeft)
+            WriteNextEntry(*sixAxisInfoLeft, sixAxisStateLeft);
+        if (sixAxisInfoRight)
+            WriteNextEntry(*sixAxisInfoRight, sixAxisStateRight);
+
         globalTimestamp++;
     }
 
@@ -386,6 +439,39 @@ namespace skyline::input {
         }
     }
 
+    void NpadDevice::SetMotionValue(MotionId sensor, MotionSensorState *value) {
+        if (!connectionState.connected)
+            return;
+
+        NpadSixAxisState *sixAxisState{sensor == MotionId::Right? &sixAxisStateRight : &sixAxisStateLeft};
+
+        sixAxisState->accelerometer.x = value->accelerometer[0];
+        sixAxisState->accelerometer.y = value->accelerometer[1];
+        sixAxisState->accelerometer.z = value->accelerometer[2];
+
+        sixAxisState->gyroscope.x = value->gyroscope[0];
+        sixAxisState->gyroscope.y = value->gyroscope[1];
+        sixAxisState->gyroscope.z = value->gyroscope[2];
+
+        float deltaTime{static_cast<float>(value->deltaTimestamp) / 1000000000.0f};
+        sixAxisState->rotation.x += value->gyroscope[0] * deltaTime;
+        sixAxisState->rotation.y += value->gyroscope[1] * deltaTime;
+        sixAxisState->rotation.z += value->gyroscope[2] * deltaTime;
+
+        sixAxisState->orientation[0].x = value->orientationMatrix[0];
+        sixAxisState->orientation[0].y = value->orientationMatrix[1];
+        sixAxisState->orientation[0].z = value->orientationMatrix[2];
+        sixAxisState->orientation[1].x = value->orientationMatrix[3];
+        sixAxisState->orientation[1].y = value->orientationMatrix[4];
+        sixAxisState->orientation[1].z = value->orientationMatrix[5];
+        sixAxisState->orientation[2].x = value->orientationMatrix[6];
+        sixAxisState->orientation[2].y = value->orientationMatrix[7];
+        sixAxisState->orientation[2].z = value->orientationMatrix[8];
+
+        sixAxisState->deltaTimestamp = value->deltaTimestamp;
+        sixAxisState->attribute.isConnected = true;
+    }
+
     constexpr jlong MsInSecond{1000}; //!< The amount of milliseconds in a single second of time
     constexpr jint AmplitudeMax{std::numeric_limits<u8>::max()}; //!< The maximum amplitude for Android Vibration APIs
 

app/src/main/cpp/skyline/input/npad_device.h

@@ -7,6 +7,30 @@
 #include "shared_mem.h"
 
 namespace skyline::input {
+
+    /**
+     * @brief Motion sensor location
+     */
+    enum class MotionId {
+        Left,
+        Right,
+        Console,
+    };
+
+    /**
+     * @brief A description of a motion event
+     * @note This structure corresponds to MotionSensorInput, see that for details
+     */
+    struct MotionSensorState {
+        u64 timestamp;
+        u64 deltaTimestamp;
+        std::array<float,3> gyroscope;
+        std::array<float,3> accelerometer;
+        std::array<float,4> quaternion;
+        std::array<float,9> orientationMatrix;
+    };
+    static_assert(sizeof(MotionSensorState) == 0x60);
+
     /**
      * @brief How many joycons must be attached for handheld mode to be triggered
      */
@@ -146,8 +170,11 @@ namespace skyline::input {
         NpadManager &manager; //!< The manager responsible for managing this NpadDevice
         NpadSection &section; //!< The section in HID shared memory for this controller
         NpadControllerInfo *controllerInfo{}; //!< The NpadControllerInfo for this controller's type
+        NpadSixAxisInfo *sixAxisInfoLeft{}; //!< The NpadSixAxisInfo for the main or left side of this controller's type
+        NpadSixAxisInfo *sixAxisInfoRight{}; //!< The NpadSixAxisInfo for the right side of this controller's type
         u64 globalTimestamp{}; //!< An incrementing timestamp that's common across all sections
         NpadControllerState controllerState{}, defaultState{}; //!< The current state of the controller (normal and default)
+        NpadSixAxisState sixAxisStateLeft{}, sixAxisStateRight{}; //!< The current state of the sixaxis (left and right)
 
         /**
          * @brief Updates the headers and writes a new entry in HID Shared Memory
@@ -156,6 +183,13 @@ namespace skyline::input {
          */
         void WriteNextEntry(NpadControllerInfo &info, NpadControllerState entry);
 
+        /**
+         * @brief Updates the headers and writes a new entry in HID Shared Memory
+         * @param info The sixaxis controller info of the NPad that needs to be updated
+         * @param entry An entry with the state of the controller
+         */
+        void WriteNextEntry(NpadSixAxisInfo &info, NpadSixAxisState entry);
+
         /**
          * @brief Writes on all ring lifo buffers a new empty entry in HID Shared Memory
          */
@@ -171,6 +205,11 @@ namespace skyline::input {
          */
         NpadControllerInfo &GetControllerInfo();
 
+        /**
+         * @return The NpadSixAxisInfo for this controller based on its type
+         */
+        NpadSixAxisInfo &GetSixAxisInfo(MotionId id);
+
       public:
         NpadId id;
         static constexpr i8 NullIndex{-1}; //!< The placeholder index value when there is no device present
@@ -222,6 +261,13 @@ namespace skyline::input {
          */
         void SetAxisValue(NpadAxisId axis, i32 value);
 
+        /**
+         * @brief Sets the value of a motion sensor to the specified value
+         * @param motion The motion sensor to set the value of
+         * @param value The value to set
+         */
+        void SetMotionValue(MotionId sensor, MotionSensorState *value);
+
         /**
          * @brief Sets the vibration for both the Joy-Cons to the specified vibration values
          */

app/src/main/cpp/skyline/input/sections/Npad.h

@@ -137,12 +137,25 @@ namespace skyline::input {
     };
     static_assert(sizeof(SixAxisVector) == 0xC);
 
+    /**
+     * @brief Indicates if sixaxis sensor is connected or interpolated
+     * @url https://switchbrew.org/wiki/HID_services#SixAxisSensorAttribute
+     */
+    union SixAxisSensorAttribute {
+        u32 raw{};
+        struct {
+            bool isConnected : 1;
+            bool isInterpolated : 1;
+        };
+    };
+    static_assert(sizeof(SixAxisSensorAttribute) == 0x4);
+
     /**
      * @url https://switchbrew.org/wiki/HID_Shared_Memory#NpadSixAxisSensorHandheldState
      */
     struct NpadSixAxisState {
         u64 globalTimestamp; //!< The global timestamp in samples
-        u64 _unk0_;
+        u64 deltaTimestamp; //!< Time passed since last state
         u64 localTimestamp; //!< The local timestamp in samples
 
         SixAxisVector accelerometer;
@@ -150,7 +163,8 @@ namespace skyline::input {
         SixAxisVector rotation;
         std::array<SixAxisVector, 3> orientation; //!< The orientation basis data as a matrix
 
-        u64 _unk2_; //!< Always 1
+        SixAxisSensorAttribute attribute;
+        u32 _unk1_;
     };
     static_assert(sizeof(NpadSixAxisState) == 0x68);
 

app/src/main/java/emu/skyline/EmulationActivity.kt

@@ -129,6 +129,7 @@ class EmulationActivity : AppCompatActivity(), SurfaceHolder.Callback, View.OnTo
     @Suppress("unused")
     private fun initializeControllers() {
         inputHandler.initializeControllers()
+        inputHandler.initialiseMotionSensors(this)
     }
 
     /**

app/src/main/java/emu/skyline/input/InputHandler.kt

@@ -5,17 +5,27 @@
 
 package emu.skyline.input
 
+import android.content.Context
+import android.hardware.Sensor
+import android.hardware.SensorEvent
+import android.hardware.SensorEventListener
+import android.hardware.SensorManager
 import android.view.InputDevice
 import android.view.KeyEvent
 import android.view.MotionEvent
 import android.view.View
+import androidx.core.content.getSystemService
+import emu.skyline.utils.ByteBufferSerializable
 import emu.skyline.utils.PreferenceSettings
+import emu.skyline.utils.u64
+import java.nio.ByteBuffer
+import java.nio.ByteOrder
 import kotlin.math.abs
 
 /**
  * Handles input events during emulation
  */
-class InputHandler(private val inputManager : InputManager, private val preferenceSettings : PreferenceSettings) {
+class InputHandler(private val inputManager : InputManager, private val preferenceSettings : PreferenceSettings) : SensorEventListener {
     companion object {
         /**
          * This initializes a guest controller in libskyline
@@ -52,6 +62,15 @@ class InputHandler(private val inputManager : InputManager, private val preferen
          */
         external fun setAxisValue(index : Int, axis : Int, value : Int)
 
+        /**
+         * This sets the values of the points on the guest touch-screen
+         *
+         * @param index The index of the controller this is directed to
+         * @param motionId The ID of the motion sensor that is being modified
+         * @param value A byte buffer of skyline::input::MotionInput in C++
+         */
+        private external fun setMotionState(index : Int, motionId : Int, value : ByteBuffer)
+
         /**
          * This sets the values of the points on the guest touch-screen
          *
@@ -60,6 +79,20 @@ class InputHandler(private val inputManager : InputManager, private val preferen
         external fun setTouchState(points : IntArray)
     }
 
+    data class MotionSensorInput(
+        var timestamp : u64 = 0uL,
+        var deltaTimestamp : u64 = 0uL,
+        @param:ByteBufferSerializable.ByteBufferSerializableArray(3) var gyroscope : FloatArray = FloatArray(3),
+        @param:ByteBufferSerializable.ByteBufferSerializableArray(3) var accelerometer : FloatArray = FloatArray(3),
+        @param:ByteBufferSerializable.ByteBufferSerializableArray(4) var quaternion : FloatArray = FloatArray(4),
+        @param:ByteBufferSerializable.ByteBufferSerializableArray(9) var orientationMatrix : FloatArray = FloatArray(9),
+    ) : ByteBufferSerializable
+
+    /**
+     * The latest state of the motion sensor
+     */
+    private val motionSensor = MotionSensorInput()
+
     /**
      * Initializes all of the controllers from [InputManager] on the guest
      */
@@ -85,6 +118,28 @@ class InputHandler(private val inputManager : InputManager, private val preferen
         updateControllers()
     }
 
+    fun initialiseMotionSensors(context : Context) {
+        val sensorManager = context.getSystemService<SensorManager>() ?: return
+        val sensorList = sensorManager.getSensorList(Sensor.TYPE_ALL)
+        val hasRotationVector = sensorList.any { sensor -> sensor.type == Sensor.TYPE_ROTATION_VECTOR }
+
+        sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER)?.also { accelerometer ->
+            sensorManager.registerListener(this, accelerometer, SensorManager.SENSOR_DELAY_GAME)
+        }
+        sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE)?.also { gyroscope ->
+            sensorManager.registerListener(this, gyroscope, SensorManager.SENSOR_DELAY_GAME)
+        }
+        sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR)?.also { rotationVector ->
+            sensorManager.registerListener(this, rotationVector, SensorManager.SENSOR_DELAY_GAME)
+        }
+        // Avoid listening to two rotation vectors at once
+        if (!hasRotationVector) {
+            sensorManager.getDefaultSensor(Sensor.TYPE_GAME_ROTATION_VECTOR)?.also { rotationVector ->
+                sensorManager.registerListener(this, rotationVector, SensorManager.SENSOR_DELAY_GAME)
+            }
+        }
+    }
+
     /**
      * Handles translating any [KeyHostEvent]s to a [GuestEvent] that is passed into libskyline
      */
@@ -175,6 +230,56 @@ class InputHandler(private val inputManager : InputManager, private val preferen
         return false
     }
 
+    override fun onAccuracyChanged(sensor : Sensor?, accuracy : Int) {}
+
+    /**
+     * This handles translating any [SensorEvent]s to a [GuestEvent] that is passed into libskyline
+     */
+    override fun onSensorChanged(event : SensorEvent) {
+        when (event.sensor.type) {
+            Sensor.TYPE_ACCELEROMETER -> {
+                motionSensor.accelerometer[0] = event.values[0] / SensorManager.GRAVITY_EARTH
+                motionSensor.accelerometer[1] = event.values[1] / SensorManager.GRAVITY_EARTH
+                motionSensor.accelerometer[2] = event.values[2] / SensorManager.GRAVITY_EARTH
+            }
+
+            Sensor.TYPE_GYROSCOPE -> {
+                // Investigate why sensor value is off by 12x
+                motionSensor.gyroscope[0] = event.values[0] / 12.0f
+                motionSensor.gyroscope[1] = event.values[1] / 12.0f
+                motionSensor.gyroscope[2] = event.values[2] / 12.0f
+            }
+
+            Sensor.TYPE_ROTATION_VECTOR -> {
+                motionSensor.quaternion[0] = event.values[0]
+                motionSensor.quaternion[1] = event.values[1]
+                motionSensor.quaternion[2] = event.values[2]
+                motionSensor.quaternion[3] = event.values[3]
+                SensorManager.getRotationMatrixFromVector(motionSensor.orientationMatrix, event.values)
+            }
+
+            Sensor.TYPE_GAME_ROTATION_VECTOR -> {
+                motionSensor.quaternion[0] = event.values[0]
+                motionSensor.quaternion[1] = event.values[1]
+                motionSensor.quaternion[2] = event.values[2]
+                motionSensor.quaternion[3] = event.values[3]
+                SensorManager.getRotationMatrixFromVector(motionSensor.orientationMatrix, event.values)
+            }
+
+            else -> {}
+        }
+
+        // Only update state on accelerometer data
+        if (event.sensor.type != Sensor.TYPE_ACCELEROMETER)
+            return
+
+        motionSensor.deltaTimestamp = event.timestamp.toULong() - motionSensor.timestamp
+        motionSensor.timestamp = event.timestamp.toULong()
+        setMotionState(0, 0, motionSensor.writeToByteBuffer(ByteBuffer.allocateDirect(0x5C).order(ByteOrder.LITTLE_ENDIAN)))
+        setMotionState(0, 1, motionSensor.writeToByteBuffer(ByteBuffer.allocateDirect(0x5C).order(ByteOrder.LITTLE_ENDIAN)))
+        setMotionState(0, 2, motionSensor.writeToByteBuffer(ByteBuffer.allocateDirect(0x5C).order(ByteOrder.LITTLE_ENDIAN)))
+    }
+
     fun handleTouchEvent(view : View, event : MotionEvent) : Boolean {
         val count = event.pointerCount
         val points = IntArray(count * 7) // This is an array of skyline::input::TouchScreenPoint in C++ as that allows for efficient transfer of values to it