Add DS Motion Pak emulation

src/CMakeLists.txt

@@ -30,6 +30,7 @@ add_library(core STATIC
     FATStorage.cpp
     FIFO.h
     GBACart.cpp
+    GBACartMotionPak.cpp
     GPU.cpp
     GPU2D.cpp
     GPU2D_Soft.cpp

src/GBACart.cpp

@@ -843,6 +843,9 @@ std::unique_ptr<CartCommon> LoadAddon(int type, void* userdata)
     case GBAAddon_RumblePak:
         cart = std::make_unique<CartRumblePak>(userdata);
         break;
+    case GBAAddon_MotionPak:
+        Cart = std::make_unique<CartMotionPak>(userdata);
+        break;
 
     default:
         Log(LogLevel::Warn, "GBACart: !! invalid addon type %d\n", type);

src/GBACart.h

@@ -33,6 +33,7 @@ enum CartType
     GameSolarSensor = 0x102,
     RAMExpansion = 0x201,
     RumblePak = 0x202,
+    MotionPak = 0x203,
 };
 
 // CartCommon -- base code shared by all cart types
@@ -211,6 +212,25 @@ private:
     u16 RumbleState = 0;
 };
 
+// CartMotionPak -- DS Motion Pak (Kionix/homebrew)
+class CartMotionPak : public CartCommon
+{
+public:
+    CartMotionPak(void* userdata);
+    ~CartMotionPak() override;
+
+    void Reset() override;
+
+    void DoSavestate(Savestate* file) override;
+
+    u16 ROMRead(u32 addr) const override;
+    u8 SRAMRead(u32 addr) override;
+
+private:
+    void* UserData;
+    u16 ShiftVal = 0;
+};
+
 // possible inputs for GBA carts that might accept user input
 enum
 {

src/GBACartMotionPak.cpp

@@ -0,0 +1,134 @@
+/*
+    Copyright 2016-2024 melonDS team
+
+    This file is part of melonDS.
+
+    melonDS is free software: you can redistribute it and/or modify it under
+    the terms of the GNU General Public License as published by the Free
+    Software Foundation, either version 3 of the License, or (at your option)
+    any later version.
+
+    melonDS is distributed in the hope that it will be useful, but WITHOUT ANY
+    WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+    FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License along
+    with melonDS. If not, see http://www.gnu.org/licenses/.
+*/
+
+#include <assert.h>
+#include "NDS.h"
+#include "GBACart.h"
+#include "Platform.h"
+#include <algorithm>
+#include "math.h"
+
+namespace melonDS
+{
+using Platform::Log;
+using Platform::LogLevel;
+
+namespace GBACart
+{
+
+CartMotionPak::CartMotionPak(void* userdata) : 
+    CartCommon(MotionPak),
+    UserData(userdata)
+{
+}
+
+CartMotionPak::~CartMotionPak() = default;
+
+void CartMotionPak::Reset()
+{
+    ShiftVal = 0;
+}
+
+void CartMotionPak::DoSavestate(Savestate* file)
+{
+    CartCommon::DoSavestate(file);
+    file->Var16(&ShiftVal);
+}
+
+u16 CartMotionPak::ROMRead(u32 addr) const
+{
+    // CHECKME: Does this apply to the Kionix/homebrew cart as well?
+    return 0xFCFF;
+}
+
+static int AccelerationToMotionPak(float accel)
+{
+    const float GRAVITY_M_S2 = 9.80665f;
+    const int COUNTS_PER_G = 819;
+    const int CENTER = 2048;
+
+    return std::clamp(
+        (int) ((accel / GRAVITY_M_S2 * COUNTS_PER_G) + CENTER + 0.5),
+        0, 4095
+    );
+}
+
+static int RotationToMotionPak(float rot)
+{
+    const float DEGREES_PER_RAD = 180 / M_PI;
+    const float COUNTS_PER_DEG_PER_SEC = 0.825;
+    const int CENTER = 1680;
+
+    return std::clamp(
+        (int) ((rot * DEGREES_PER_RAD * COUNTS_PER_DEG_PER_SEC) + CENTER + 0.5),
+        0, 4095
+    );
+}
+
+u8 CartMotionPak::SRAMRead(u32 addr)
+{
+    // CHECKME: SRAM address mask
+    addr &= 0xFFFF;
+
+    switch (addr)
+    {
+    case 0:
+        // Read next byte
+        break;
+    case 2:
+        // Read X acceleration
+        ShiftVal = AccelerationToMotionPak(Platform::Addon_MotionQuery(Platform::MotionAccelerationX, UserData)) << 4;
+        // CHECKME: First byte returned when reading acceleration/rotation
+        return 0;
+    case 4:
+        // Read Y acceleration
+        ShiftVal = AccelerationToMotionPak(Platform::Addon_MotionQuery(Platform::MotionAccelerationY, UserData)) << 4;
+        return 0;
+    case 6:
+        // Read Z acceleration
+        ShiftVal = AccelerationToMotionPak(Platform::Addon_MotionQuery(Platform::MotionAccelerationZ, UserData)) << 4;
+        return 0;
+    case 8:
+        // Read Z rotation
+        ShiftVal = RotationToMotionPak(Platform::Addon_MotionQuery(Platform::MotionRotationZ, UserData)) << 4;
+        return 0;
+    case 10:
+        // Identify cart
+        ShiftVal = 0xF00F;
+        return 0;
+    case 12:
+    case 14:
+    case 16:
+    case 18:
+        // Read/enable analog inputs
+        //
+        // These are not connected by defualt and require do-it-yourself cart
+        // modification, so there is no reason to emulate them.
+        ShiftVal = 0;
+        break;
+    }
+
+    // Read high byte from the emulated shift register
+    u8 val = ShiftVal >> 8;
+    ShiftVal <<= 8;
+    return val;
+}
+
+}
+
+}

src/NDS.h

@@ -215,6 +215,7 @@ enum
 {
     GBAAddon_RAMExpansion = 1,
     GBAAddon_RumblePak = 2,
+    GBAAddon_MotionPak = 3,
 };
 
 class SPU;

src/Platform.h

@@ -331,6 +331,42 @@ void Addon_RumbleStart(u32 len, void* userdata);
 // rumble effects on the connected game controller, if available.
 void Addon_RumbleStop(void* userdata);
 
+enum MotionQueryType
+{
+    /**
+     * @brief X axis acceleration, measured in SI meters per second squared.
+     * On a DS, the X axis refers to the top screen X-axis (left ... right).
+     */
+    MotionAccelerationX,
+    /**
+     * @brief Y axis acceleration, measured in SI meters per second squared.
+     * On a DS, the Y axis refers to the top screen Y-axis (bottom ... top).
+     */
+    MotionAccelerationY,
+    /**
+     * @brief Z axis acceleration, measured in SI meters per second squared.
+     * On a DS, the Z axis refers to the axis perpendicular to the top screen (farther ... closer).
+     */
+    MotionAccelerationZ,
+    /**
+     * @brief X axis rotation, measured in radians per second.
+     */
+    MotionRotationX,
+    /**
+     * @brief Y axis rotation, measured in radians per second.
+     */
+    MotionRotationY,
+    /**
+     * @brief Z axis rotation, measured in radians per second.
+     */
+    MotionRotationZ,
+};
+
+// Called by the DS Motion Pak emulation to query the game controller's
+// aceelration and rotation, if available.
+// @param type The value being queried.
+float Addon_MotionQuery(MotionQueryType type, void* userdata);
+
 struct DynamicLibrary;
 
 /**

src/frontend/qt_sdl/EmuInstance.cpp

@@ -2142,6 +2142,8 @@ QString EmuInstance::gbaAddonName(int addon)
         return "Rumble Pak";
     case GBAAddon_RAMExpansion:
         return "Memory expansion";
+    case GBAAddon_MotionPak:
+        return "Motion Pak";
     }
 
     return "???";

src/frontend/qt_sdl/EmuInstance.h

@@ -21,6 +21,7 @@
 
 #include <SDL2/SDL.h>
 
+#include "Platform.h"
 #include "main.h"
 #include "NDS.h"
 #include "EmuThread.h"
@@ -142,6 +143,7 @@ public:
     void inputLoadConfig();
     void inputRumbleStart(melonDS::u32 len_ms);
     void inputRumbleStop();
+    float inputMotionQuery(melonDS::Platform::MotionQueryType type);
 
     void setJoystick(int id);
     int getJoystickID() { return joystickID; }
@@ -332,6 +334,8 @@ private:
     int joystickID;
     SDL_Joystick* joystick;
     SDL_GameController* controller;
+    bool hasAccelerometer = false;
+    bool hasGyroscope = false;
     bool hasRumble = false;
     bool isRumbling = false;
 

src/frontend/qt_sdl/EmuInstanceInput.cpp

@@ -19,6 +19,9 @@
 #include <QKeyEvent>
 #include <SDL2/SDL.h>
 
+#include "Platform.h"
+#include "SDL_gamecontroller.h"
+#include "SDL_sensor.h"
 #include "main.h"
 #include "Config.h"
 
@@ -81,6 +84,8 @@ void EmuInstance::inputInit()
     joystick = nullptr;
     controller = nullptr;
     hasRumble = false;
+    hasAccelerometer = false;
+    hasGyroscope = false;
     isRumbling = false;
     inputLoadConfig();
 }
@@ -128,6 +133,24 @@ void EmuInstance::inputRumbleStop()
     }
 }
 
+float EmuInstance::inputMotionQuery(melonDS::Platform::MotionQueryType type)
+{
+    float values[3];
+    if (type <= melonDS::Platform::MotionAccelerationZ)
+    {
+        if (controller && hasAccelerometer)
+            if (SDL_GameControllerGetSensorData(controller, SDL_SENSOR_ACCEL, values, 3) == 0)
+                return values[type % 3];
+    }
+    else if (type <= melonDS::Platform::MotionRotationZ)
+    {
+        if (controller && hasGyroscope)
+            if (SDL_GameControllerGetSensorData(controller, SDL_SENSOR_GYRO, values, 3) == 0)
+                return values[type % 3];
+    }
+    return 0.0f;
+}
+
 
 void EmuInstance::setJoystick(int id)
 {
@@ -147,6 +170,8 @@ void EmuInstance::openJoystick()
 	controller = nullptr;
         joystick = nullptr;
 	hasRumble = false;
+    hasAccelerometer = false;
+    hasGyroscope = false;
         return;
     }
 
@@ -163,9 +188,17 @@ void EmuInstance::openJoystick()
     if (controller)
     {
 	if (SDL_GameControllerHasRumble(controller))
-	{
+    {
 	    hasRumble = true;
-	}
+    }
+	if (SDL_GameControllerHasSensor(controller, SDL_SENSOR_ACCEL))
+    {
+	    hasAccelerometer = SDL_GameControllerSetSensorEnabled(controller, SDL_SENSOR_ACCEL, SDL_TRUE) == 0;
+    }
+	if (SDL_GameControllerHasSensor(controller, SDL_SENSOR_GYRO))
+    {
+	    hasGyroscope = SDL_GameControllerSetSensorEnabled(controller, SDL_SENSOR_GYRO, SDL_TRUE) == 0;
+    }
     }
 }
 
@@ -176,6 +209,8 @@ void EmuInstance::closeJoystick()
 	SDL_GameControllerClose(controller);
 	controller = nullptr;
 	hasRumble = false;
+    hasAccelerometer = false;
+    hasGyroscope = false;
     }
 
     if (joystick)

src/frontend/qt_sdl/Platform.cpp

@@ -35,6 +35,7 @@
 
 #include "Platform.h"
 #include "Config.h"
+#include "EmuInstance.h"
 #include "main.h"
 #include "CameraManager.h"
 #include "Net.h"
@@ -559,6 +560,11 @@ void Addon_RumbleStop(void* userdata)
     ((EmuInstance*)userdata)->inputRumbleStop();
 }
 
+float Addon_MotionQuery(MotionQueryType type, void* userdata)
+{
+    return ((EmuInstance*)userdata)->inputMotionQuery(type);
+}
+
 DynamicLibrary* DynamicLibrary_Load(const char* lib)
 {
     return (DynamicLibrary*) SDL_LoadObject(lib);

src/frontend/qt_sdl/Window.cpp

@@ -16,6 +16,7 @@
     with melonDS. If not, see http://www.gnu.org/licenses/.
 */
 
+#include "NDS.h"
 #include <stdlib.h>
 #include <time.h>
 #include <stdio.h>
@@ -320,7 +321,7 @@ MainWindow::MainWindow(int id, EmuInstance* inst, QWidget* parent) :
                 QMenu * submenu = menu->addMenu("Insert add-on cart");
                 QAction *act;
 
-                int addons[] = {GBAAddon_RAMExpansion, GBAAddon_RumblePak, -1};
+                int addons[] = {GBAAddon_RAMExpansion, GBAAddon_RumblePak, GBAAddon_MotionPak, -1};
                 for (int i = 0; addons[i] != -1; i++)
                 {
                     int addon = addons[i];

src/frontend/qt_sdl/main.cpp

@@ -307,6 +307,10 @@ int main(int argc, char** argv)
     {
         printf("SDL couldn't init joystick\n");
     }
+    if (SDL_Init(SDL_INIT_SENSOR) < 0)
+    {
+        printf("SDL couldn't init motion sensors\n");
+    }
     if (SDL_Init(SDL_INIT_AUDIO) < 0)
     {
         const char* err = SDL_GetError();