diff --git a/.gitmodules b/.gitmodules
index 1b7381f..ea43512 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -13,3 +13,6 @@
 [submodule "WebApp"]
 	path = WebApp
 	url = https://github.com/wiredopposite/OGX-Mini-WebApp.git
+[submodule "Firmware/external/libfixmath"]
+	path = Firmware/external/libfixmath
+	url = https://github.com/PetteriAimonen/libfixmath.git
diff --git a/Firmware/ESP32/CMakeLists.txt b/Firmware/ESP32/CMakeLists.txt
index f4aa783..e62b842 100644
--- a/Firmware/ESP32/CMakeLists.txt
+++ b/Firmware/ESP32/CMakeLists.txt
@@ -13,7 +13,10 @@ include(${EXTERNAL_CMAKE_DIR}/patch_libs.cmake)
 include(${EXTERNAL_CMAKE_DIR}/generate_gatt_header.cmake)
 include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/integrate_btstack.cmake)
 
-init_git_submodules(${EXTERNAL_DIR})
+init_git_submodules(${EXTERNAL_DIR}
+    ${EXTERNAL_DIR}/bluepad32
+    ${EXTERNAL_DIR}/libfixmath
+)
 apply_lib_patches(${EXTERNAL_DIR})
 integrate_btstack(${EXTERNAL_DIR})
 generate_gatt_header(
diff --git a/Firmware/ESP32/components/libfixmath/CMakeLists.txt b/Firmware/ESP32/components/libfixmath/CMakeLists.txt
new file mode 100644
index 0000000..dd31ca8
--- /dev/null
+++ b/Firmware/ESP32/components/libfixmath/CMakeLists.txt
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.5)
+
+set(LIBFIXMATH_ROOT ${CMAKE_CURRENT_LIST_DIR}/../../../external/libfixmath) 
+
+if (NOT EXISTS ${LIBFIXMATH_ROOT})
+    message(FATAL_ERROR "External directory not found: ${LIBFIXMATH_ROOT}")
+else()
+    message(STATUS "Found libfixmath at ${LIBFIXMATH_ROOT}")
+endif()
+
+file(GLOB SRCS ${LIBFIXMATH_ROOT}/libfixmath/*.c)
+
+idf_component_register(
+    SRCS
+        ${SRCS}
+    INCLUDE_DIRS
+        ${LIBFIXMATH_ROOT}
+        ${LIBFIXMATH_ROOT}/libfixmath
+)
+
+target_compile_definitions(${COMPONENT_LIB} PRIVATE
+    FIXMATH_FAST_SIN
+    FIXMATH_NO_64BIT
+    FIXMATH_NO_CACHE
+    FIXMATH_NO_HARD_DIVISION
+    FIXMATH_NO_OVERFLOW
+)
\ No newline at end of file
diff --git a/Firmware/ESP32/main/BLEServer/BLEServer.cpp b/Firmware/ESP32/main/BLEServer/BLEServer.cpp
index 4d8b64d..8ba99db 100644
--- a/Firmware/ESP32/main/BLEServer/BLEServer.cpp
+++ b/Firmware/ESP32/main/BLEServer/BLEServer.cpp
@@ -6,7 +6,8 @@
 #include "att_server.h"
 #include "btstack.h"
 
-#include "Gamepad.h"
+#include "BTManager/BTManager.h" 
+#include "Gamepad/Gamepad.h"
 #include "BLEServer/BLEServer.h"
 #include "BLEServer/att_delayed_response.h"
 #include "UserSettings/UserProfile.h"
@@ -14,33 +15,21 @@
 
 namespace BLEServer {
 
-static constexpr uint16_t PACKET_LEN_MAX = 18;
-
-#pragma pack(push, 1)
-struct SetupPacket
-{
-    uint8_t max_gamepads{1};
-    uint8_t index{0};
-    uint8_t device_type{0};
-    uint8_t profile_id{1};
-};
-static_assert(sizeof(SetupPacket) == 4, "BLEServer::SetupPacket struct size mismatch");
-#pragma pack(pop)
-
-SetupPacket setup_packet_;
+constexpr uint16_t PACKET_LEN_MAX = 20;
+constexpr size_t GAMEPAD_LEN = 23;
 
 namespace Handle
 {
-    static constexpr uint16_t FW_VERSION    = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789020_01_VALUE_HANDLE;
-    static constexpr uint16_t FW_NAME       = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789021_01_VALUE_HANDLE;
+    constexpr uint16_t FW_VERSION    = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789020_01_VALUE_HANDLE;
+    constexpr uint16_t FW_NAME       = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789021_01_VALUE_HANDLE;
 
-    static constexpr uint16_t START_UPDATE  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789030_01_VALUE_HANDLE;
-    static constexpr uint16_t COMMIT_UPDATE = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789031_01_VALUE_HANDLE;
+    constexpr uint16_t SETUP_READ  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789030_01_VALUE_HANDLE;
+    constexpr uint16_t SETUP_WRITE = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789031_01_VALUE_HANDLE;
+    constexpr uint16_t GET_SETUP   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789032_01_VALUE_HANDLE;
 
-    static constexpr uint16_t SETUP_PACKET  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789040_01_VALUE_HANDLE;
-    static constexpr uint16_t PROFILE_PT1   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789041_01_VALUE_HANDLE;
-    static constexpr uint16_t PROFILE_PT2   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789042_01_VALUE_HANDLE;
-    static constexpr uint16_t PROFILE_PT3   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789043_01_VALUE_HANDLE;
+    constexpr uint16_t PROFILE  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789040_01_VALUE_HANDLE;
+
+    constexpr uint16_t GAMEPAD  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789050_01_VALUE_HANDLE;
 }
 
 namespace ADV
@@ -62,23 +51,163 @@ namespace ADV
             std::memcpy(flags, FLAGS, sizeof(flags));
             name_len = sizeof(FIRMWARE_NAME);
             name_type = NAME_TYPE;
-            std::memcpy(name, FIRMWARE_NAME, sizeof(name));
+            std::string fw_name = FIRMWARE_NAME;
+            std::memcpy(name, fw_name.c_str(), std::min(sizeof(name), fw_name.size()));
         }
     };
     static_assert(sizeof(Data) == 5 + sizeof(FIRMWARE_NAME) - 1, "BLEServer::ADV::Data struct size mismatch");
     #pragma pack(pop)
 }
 
-static int verify_write(const uint16_t buffer_size, const uint16_t expected_size, bool pending_write = false, bool expected_pending_write = false)
+#pragma pack(push, 1)
+struct SetupPacket
+{
+    DeviceDriverType device_type{DeviceDriverType::NONE};
+    uint8_t max_gamepads{MAX_GAMEPADS};
+    uint8_t player_idx{0};
+    uint8_t profile_id{0};
+};
+static_assert(sizeof(SetupPacket) == 4, "BLEServer::SetupPacket struct size mismatch");
+#pragma pack(pop)
+
+class ProfileReader
+{
+public:
+    ProfileReader() = default;
+    ~ProfileReader() = default;
+
+    void set_setup_packet(const SetupPacket& setup_packet)
+    {
+        setup_packet_ = setup_packet;
+        current_offset_ = 0;
+    }
+    const SetupPacket& get_setup_packet() const
+    {
+        return setup_packet_;
+    }
+    uint16_t get_xfer_len()
+    {
+        return static_cast<uint16_t>(std::min(static_cast<size_t>(PACKET_LEN_MAX), sizeof(UserProfile) - current_offset_));
+    }
+    uint16_t get_profile_data(uint8_t* buffer, uint16_t buffer_len)
+    {
+        size_t copy_len = get_xfer_len();
+        if (!buffer || buffer_len < copy_len)
+        {
+            return 0;
+        }
+
+        if (current_offset_ == 0 && !set_profile())
+        {
+            return 0;
+        }
+
+        std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile_) + current_offset_, copy_len);
+
+        current_offset_ += copy_len;
+        if (current_offset_ >= sizeof(UserProfile))
+        {
+            current_offset_ = 0;
+            OGXM_LOG("ProfileReader: Read complete for profile ID: %i\n", profile_.id);
+        }
+        return copy_len;
+    }
+
+private:
+    SetupPacket setup_packet_;
+    UserProfile profile_;
+    size_t current_offset_ = 0;
+
+    bool set_profile()
+    {
+        if (setup_packet_.profile_id == 0xFF)
+        {
+            if (setup_packet_.player_idx >= UserSettings::MAX_PROFILES)
+            {
+                return false;
+            }
+            profile_ = UserSettings::get_instance().get_profile_by_index(setup_packet_.player_idx);
+            OGXM_LOG("ProfileReader: Reading profile for player %d\n", setup_packet_.player_idx);
+        }
+        else
+        {
+            if (setup_packet_.profile_id > UserSettings::MAX_PROFILES)
+            {
+                return false;
+            }
+            profile_ = UserSettings::get_instance().get_profile_by_id(setup_packet_.profile_id);
+            OGXM_LOG("ProfileReader: Reading profile with ID %d\n", setup_packet_.profile_id);
+        }
+        return true;
+    }
+};
+
+class ProfileWriter
+{
+public:
+    ProfileWriter() = default;
+    ~ProfileWriter() = default;
+
+    void set_setup_packet(const SetupPacket& setup_packet)
+    {
+        setup_packet_ = setup_packet;
+        current_offset_ = 0;
+    }
+    const SetupPacket& get_setup_packet() const
+    {
+        return setup_packet_;
+    }
+    uint16_t get_xfer_len()
+    {
+        return static_cast<uint16_t>(std::min(static_cast<size_t>(PACKET_LEN_MAX), sizeof(UserProfile) - current_offset_));
+    }
+    size_t set_profile_data(const uint8_t* buffer, uint16_t buffer_len)
+    {
+        size_t copy_len = get_xfer_len();
+        if (!buffer || buffer_len < copy_len)
+        {
+            return 0;
+        }
+
+        std::memcpy(reinterpret_cast<uint8_t*>(&profile_) + current_offset_, buffer, copy_len);
+
+        current_offset_ += copy_len;
+        size_t ret = current_offset_;
+
+        if (current_offset_ >= sizeof(UserProfile))
+        {
+            current_offset_ = 0;
+        }
+        return ret;
+    }
+    bool commit_profile()
+    {
+        if (setup_packet_.device_type != DeviceDriverType::NONE)
+        {
+            UserSettings::get_instance().store_profile_and_driver_type(setup_packet_.device_type, setup_packet_.player_idx, profile_);
+        }
+        else
+        {
+            UserSettings::get_instance().store_profile(setup_packet_.player_idx, profile_);
+        }
+        return true;
+    }
+
+private:
+    SetupPacket setup_packet_;
+    UserProfile profile_;
+    size_t current_offset_ = 0;
+};
+
+ProfileReader profile_reader_;
+ProfileWriter profile_writer_;
+
+static int verify_write(const uint16_t buffer_size, const uint16_t expected_size)
 {
     if (buffer_size != expected_size)
     {
         return ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH;
     }
-    if (pending_write != expected_pending_write)
-    {
-        return ATT_ERROR_WRITE_NOT_PERMITTED;
-    }
     return 0;
 }
 
@@ -88,8 +217,6 @@ static uint16_t att_read_callback(  hci_con_handle_t connection_handle,
                                     uint8_t *buffer,
                                     uint16_t buffer_size)
 {
-    static UserProfile profile;
-    SetupPacket setup_packet_resp;
     std::string fw_version;
     std::string fw_name;
 
@@ -107,45 +234,34 @@ static uint16_t att_read_callback(  hci_con_handle_t connection_handle,
             fw_name = FIRMWARE_NAME;
             if (buffer)
             {
-                std::memcpy(buffer, reinterpret_cast<const uint8_t*>(fw_name.c_str()), fw_name.size());
+                std::memcpy(buffer, reinterpret_cast<const uint8_t*>(fw_name.c_str()), fw_name.size());;
             }
             return static_cast<uint16_t>(fw_name.size());
 
-        case Handle::SETUP_PACKET:
+        case Handle::GET_SETUP:
             if (buffer)
             {
-                //App has already written a setup packet with the index
-                setup_packet_resp.max_gamepads = static_cast<uint8_t>(MAX_GAMEPADS);
-                setup_packet_resp.index = setup_packet_.index;
-                setup_packet_resp.device_type = static_cast<uint8_t>(UserSettings::get_instance().get_current_driver());
-                setup_packet_resp.profile_id = UserSettings::get_instance().get_active_profile_id(setup_packet_.index);
-
-                std::memcpy(buffer, &setup_packet_resp, sizeof(setup_packet_resp));
+                buffer[0] = static_cast<uint8_t>(UserSettings::get_instance().get_current_driver());
+                buffer[1] = MAX_GAMEPADS;
+                buffer[2] = 0;
+                buffer[3] = UserSettings::get_instance().get_active_profile_id(0);
             }
-            return sizeof(setup_packet_);
+            return static_cast<uint16_t>(sizeof(SetupPacket));
 
-        case Handle::PROFILE_PT1:
+        case Handle::PROFILE:
             if (buffer)
             {
-                //App has already written the profile id it wants to the setup packet
-                profile = UserSettings::get_instance().get_profile_by_id(setup_packet_.profile_id);
-                std::memcpy(buffer, &profile, PACKET_LEN_MAX);
+                return profile_reader_.get_profile_data(buffer, buffer_size);
             }
-            return PACKET_LEN_MAX;
+            return profile_reader_.get_xfer_len();
 
-        case Handle::PROFILE_PT2:
+        case Handle::GAMEPAD:
             if (buffer)
             {
-                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX, PACKET_LEN_MAX);
+                I2CDriver::PacketIn packet_in = BTManager::get_instance().get_packet_in(0);
+                std::memcpy(buffer, &packet_in.dpad, 13);
             }
-            return PACKET_LEN_MAX;
-
-        case Handle::PROFILE_PT3:
-            if (buffer)
-            {
-                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX * 2, sizeof(UserProfile) - PACKET_LEN_MAX * 2);
-            }
-            return sizeof(UserProfile) - PACKET_LEN_MAX * 2;
+            return static_cast<uint16_t>(13);
 
         default:
             break;
@@ -153,84 +269,42 @@ static uint16_t att_read_callback(  hci_con_handle_t connection_handle,
     return 0;
 }
 
-static int att_write_callback(hci_con_handle_t connection_handle,
-                              uint16_t att_handle,
-                              uint16_t transaction_mode,
-                              uint16_t offset,
-                              uint8_t *buffer,
-                              uint16_t buffer_size)
+static int att_write_callback(  hci_con_handle_t connection_handle,
+                                uint16_t att_handle,
+                                uint16_t transaction_mode,
+                                uint16_t offset,
+                                uint8_t *buffer,
+                                uint16_t buffer_size)
 {
-    static UserProfile temp_profile;
-    static bool pending_write = false;
-
     int ret = 0;
 
     switch (att_handle)
     {
-        case Handle::START_UPDATE:
-            pending_write = true;
-            break;
-
-        case Handle::SETUP_PACKET:
+        case Handle::SETUP_READ:
             if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0)
             {
                 break;
             }
-
-            std::memcpy(&setup_packet_, buffer, buffer_size);
-            if (setup_packet_.index >= MAX_GAMEPADS)
-            {
-                setup_packet_.index = 0;
-                ret = ATT_ERROR_OUT_OF_RANGE;
-            }
-            if (setup_packet_.profile_id > UserSettings::MAX_PROFILES)
-            {
-                setup_packet_.profile_id = 1;
-                ret = ATT_ERROR_OUT_OF_RANGE;
-            }
-            if (ret)
-            {
-                break;
-            }
-
-            if (pending_write)
-            {
-                //App wants to store a new device driver type
-                UserSettings::get_instance().store_driver_type(static_cast<DeviceDriverType>(setup_packet_.device_type));
-            }
+            profile_reader_.set_setup_packet(*reinterpret_cast<SetupPacket*>(buffer));
             break;
 
-        case Handle::PROFILE_PT1:
-            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+        case Handle::SETUP_WRITE:
+            if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0)
             {
                 break;
             }
-            std::memcpy(&temp_profile, buffer, buffer_size);
+            profile_writer_.set_setup_packet(*reinterpret_cast<SetupPacket*>(buffer));
             break;
 
-        case Handle::PROFILE_PT2:
-            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+        case Handle::PROFILE:
+            if ((ret = verify_write(buffer_size, profile_writer_.get_xfer_len())) != 0)
             {
                 break;
             }
-            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX, buffer, buffer_size);
-            break;
-
-        case Handle::PROFILE_PT3:
-            if ((ret = verify_write(buffer_size, sizeof(UserProfile) - PACKET_LEN_MAX * 2, pending_write, true)) != 0)
+            if (profile_writer_.set_profile_data(buffer, buffer_size) == sizeof(UserProfile))
             {
-                break;
+                profile_writer_.commit_profile();
             }
-            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX * 2, buffer, buffer_size);
-            break;
-
-        case Handle::COMMIT_UPDATE:
-            if ((ret = verify_write(0, 0, pending_write, true)) != 0)
-            {
-                break;
-            }
-            UserSettings::get_instance().store_profile(setup_packet_.index, temp_profile);
-            pending_write = false;
             break;
 
         default:
@@ -241,12 +315,8 @@ static int att_write_callback(hci_con_handle_t connection_handle,
 
 void init_server()
 {
-    UserSettings::get_instance().initialize_flash();
-
-    // setup ATT server
     att_server_init(profile_data, att_read_callback, att_write_callback);
 
-    // setup advertisements
     uint16_t adv_int_min = 0x0030;
     uint16_t adv_int_max = 0x0030;
     uint8_t adv_type = 0;
diff --git a/Firmware/ESP32/main/BLEServer/BLEServer.h b/Firmware/ESP32/main/BLEServer/BLEServer.h
index d72962e..6159971 100644
--- a/Firmware/ESP32/main/BLEServer/BLEServer.h
+++ b/Firmware/ESP32/main/BLEServer/BLEServer.h
@@ -3,6 +3,8 @@
 
 #include <cstdint>
 
+#include "Gamepad/Gamepad.h"
+
 namespace BLEServer 
 {
     void init_server();
diff --git a/Firmware/ESP32/main/BTManager/BTManager.cpp b/Firmware/ESP32/main/BTManager/BTManager.cpp
index ad0b366..c17301d 100644
--- a/Firmware/ESP32/main/BTManager/BTManager.cpp
+++ b/Firmware/ESP32/main/BTManager/BTManager.cpp
@@ -7,6 +7,7 @@
 #include "Board/ogxm_log.h"
 #include "Board/board_api.h"
 #include "BTManager/BTManager.h"
+#include "BLEServer/BLEServer.h"
 
 void BTManager::run_task()
 {
@@ -23,7 +24,8 @@ void BTManager::run_task()
         static_cast<i2c_port_t>(CONFIG_I2C_PORT), 
         static_cast<gpio_num_t>(CONFIG_I2C_SDA_PIN), 
         static_cast<gpio_num_t>(CONFIG_I2C_SCL_PIN),  
-        CONFIG_I2C_BAUDRATE);
+        CONFIG_I2C_BAUDRATE
+    );
 
     xTaskCreatePinnedToCore(
         [](void* parameter)
@@ -35,7 +37,8 @@ void BTManager::run_task()
         nullptr,
         configMAX_PRIORITIES-8,
         nullptr,
-        1 );
+        1 
+    );
 
     btstack_init();
 
@@ -54,6 +57,8 @@ void BTManager::run_task()
     btstack_run_loop_set_timer(&driver_update_timer, UserSettings::GP_CHECK_DELAY_MS);
     btstack_run_loop_add_timer(&driver_update_timer);
 
+    BLEServer::init_server();
+
     //Doesn't return
     btstack_run_loop_execute();
 }
@@ -102,8 +107,6 @@ void BTManager::check_led_cb(btstack_timer_source *ts)
 
 void BTManager::send_driver_type(DeviceDriverType driver_type)
 {
-    OGXM_LOG("BP32: Sending driver type: %s\n", DRIVER_NAME(driver_type).c_str());
-
     if constexpr (I2CDriver::MULTI_SLAVE)
     {
         for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
@@ -242,4 +245,13 @@ void BTManager::manage_connection(uint8_t index, bool connected)
         packet_in.index = index;
         i2c_driver_.write_packet(I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, packet_in);
     }
+}
+
+I2CDriver::PacketIn BTManager::get_packet_in(uint8_t index)
+{
+    if (index >= devices_.size())
+    {
+        return I2CDriver::PacketIn();
+    }
+    return devices_[index].packet_in;
 }
\ No newline at end of file
diff --git a/Firmware/ESP32/main/BTManager/BTManager.h b/Firmware/ESP32/main/BTManager/BTManager.h
index c5f9ce4..5d7d08b 100644
--- a/Firmware/ESP32/main/BTManager/BTManager.h
+++ b/Firmware/ESP32/main/BTManager/BTManager.h
@@ -9,7 +9,7 @@
 #include <freertos/task.h>
 
 #include "I2CDriver/I2CDriver.h"
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 
 class BTManager
 {
@@ -23,6 +23,7 @@ public:
     void run_task();
     bool any_connected();
     bool is_connected(uint8_t index);
+    I2CDriver::PacketIn get_packet_in(uint8_t index);
 
 private:
     BTManager() = default;
diff --git a/Firmware/ESP32/main/BTManager/BTManager_BP32.cpp b/Firmware/ESP32/main/BTManager/BTManager_BP32.cpp
index 016e9aa..c1cc1f7 100644
--- a/Firmware/ESP32/main/BTManager/BTManager_BP32.cpp
+++ b/Firmware/ESP32/main/BTManager/BTManager_BP32.cpp
@@ -5,6 +5,7 @@
 
 #include "Board/ogxm_log.h"
 #include "BTManager/BTManager.h"
+#include "BLEServer/BLEServer.h"
 
 void BTManager::init(int argc, const char** arg_V) 
 {
@@ -123,10 +124,16 @@ void BTManager::controller_data_cb(uni_hid_device_t* bp_device, uni_controller_t
     packet_in.trigger_l = mapper.scale_trigger_l<10>(static_cast<uint16_t>(uni_gp->brake));
     packet_in.trigger_r = mapper.scale_trigger_r<10>(static_cast<uint16_t>(uni_gp->throttle));
 
-    packet_in.joystick_lx = mapper.scale_joystick_lx<10>(uni_gp->axis_x);
-    packet_in.joystick_ly = mapper.scale_joystick_ly<10>(uni_gp->axis_y);
-    packet_in.joystick_rx = mapper.scale_joystick_rx<10>(uni_gp->axis_rx);
-    packet_in.joystick_ry = mapper.scale_joystick_ry<10>(uni_gp->axis_ry);
+    // auto joy_l = mapper.scale_joystick_l<10>(uni_gp->axis_x, uni_gp->axis_y);
+    // auto joy_r = mapper.scale_joystick_r<10>(uni_gp->axis_rx, uni_gp->axis_ry);
+
+    // packet_in.joystick_lx = joy_l.first;
+    // packet_in.joystick_ly = joy_l.second;
+    // packet_in.joystick_rx = joy_r.first;
+    // packet_in.joystick_ry = joy_r.second;
+
+    std::tie(packet_in.joystick_lx, packet_in.joystick_ly) = mapper.scale_joystick_l<10>(uni_gp->axis_x, uni_gp->axis_y);
+    std::tie(packet_in.joystick_rx, packet_in.joystick_ry) = mapper.scale_joystick_r<10>(uni_gp->axis_rx, uni_gp->axis_ry);
 
     i2c_driver_.write_packet(I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, packet_in);
 
diff --git a/Firmware/ESP32/main/Board/ogxm_log.h b/Firmware/ESP32/main/Board/ogxm_log.h
index d67985d..1913adf 100644
--- a/Firmware/ESP32/main/Board/ogxm_log.h
+++ b/Firmware/ESP32/main/Board/ogxm_log.h
@@ -33,9 +33,20 @@ namespace OGXM
 
         std::ostringstream hex_stream;
         hex_stream << std::hex << std::setfill('0');
+        int char_num = 0;
         for (uint16_t i = 0; i < len; ++i)
         {
             hex_stream << std::setw(2) << static_cast<int>(data[i]) << " ";
+            char_num++;
+            if (char_num == 16)
+            {
+                hex_stream << "\n";
+                char_num = 0;
+            }
+        }
+        if (char_num != 0)
+        {
+            hex_stream << "\n";
         }
 
         log(hex_stream.str());
diff --git a/Firmware/ESP32/main/CMakeLists.txt b/Firmware/ESP32/main/CMakeLists.txt
index bee4fda..20603a4 100644
--- a/Firmware/ESP32/main/CMakeLists.txt
+++ b/Firmware/ESP32/main/CMakeLists.txt
@@ -11,13 +11,16 @@ idf_component_register(
         "I2CDriver/I2CDriver.cpp"
         "UserSettings/UserSettings.cpp"
         "UserSettings/UserProfile.cpp"
+        "UserSettings/TriggerSettings.cpp"
+        "UserSettings/JoystickSettings.cpp"
     INCLUDE_DIRS 
         "."
     REQUIRES 
         bluepad32 
         btstack 
         driver 
-        nvs_flash
+        nvs_flash 
+        libfixmath 
 )
 
 target_compile_definitions(${COMPONENT_LIB} PRIVATE
diff --git a/Firmware/ESP32/main/Gamepad.h b/Firmware/ESP32/main/Gamepad.h
deleted file mode 100644
index c66e2ff..0000000
--- a/Firmware/ESP32/main/Gamepad.h
+++ /dev/null
@@ -1,364 +0,0 @@
-#ifndef GAMEPAD_H
-#define GAMEPAD_H
-
-#include <cstdint>
-
-#include "sdkconfig.h"
-#include "Range.h"
-#include "UserSettings/UserProfile.h"
-#include "UserSettings/UserSettings.h"
-#include "Board/ogxm_log.h"
-
-#define MAX_GAMEPADS CONFIG_BLUEPAD32_MAX_DEVICES
-static_assert(  MAX_GAMEPADS > 0 && 
-                MAX_GAMEPADS <= 4, 
-                "MAX_GAMEPADS must be between 1 and 4");
-
-namespace Gamepad
-{
-    static constexpr uint8_t DPAD_UP         = 0x01;
-    static constexpr uint8_t DPAD_DOWN       = 0x02;
-    static constexpr uint8_t DPAD_LEFT       = 0x04;
-    static constexpr uint8_t DPAD_RIGHT      = 0x08;
-    static constexpr uint8_t DPAD_UP_LEFT    = DPAD_UP | DPAD_LEFT;
-    static constexpr uint8_t DPAD_UP_RIGHT   = DPAD_UP | DPAD_RIGHT;
-    static constexpr uint8_t DPAD_DOWN_LEFT  = DPAD_DOWN | DPAD_LEFT;
-    static constexpr uint8_t DPAD_DOWN_RIGHT = DPAD_DOWN | DPAD_RIGHT;
-    static constexpr uint8_t DPAD_NONE       = 0x00;
-
-    static constexpr uint16_t BUTTON_A     = 0x0001;
-    static constexpr uint16_t BUTTON_B     = 0x0002;
-    static constexpr uint16_t BUTTON_X     = 0x0004;
-    static constexpr uint16_t BUTTON_Y     = 0x0008;
-    static constexpr uint16_t BUTTON_L3    = 0x0010;
-    static constexpr uint16_t BUTTON_R3    = 0x0020;
-    static constexpr uint16_t BUTTON_BACK  = 0x0040;
-    static constexpr uint16_t BUTTON_START = 0x0080;
-    static constexpr uint16_t BUTTON_LB    = 0x0100;
-    static constexpr uint16_t BUTTON_RB    = 0x0200;
-    static constexpr uint16_t BUTTON_SYS   = 0x0400;
-    static constexpr uint16_t BUTTON_MISC  = 0x0800;
-}
-
-class GamepadMapper
-{
-public:
-    uint8_t DPAD_UP         = Gamepad::DPAD_UP        ;
-    uint8_t DPAD_DOWN       = Gamepad::DPAD_DOWN      ;
-    uint8_t DPAD_LEFT       = Gamepad::DPAD_LEFT      ;
-    uint8_t DPAD_RIGHT      = Gamepad::DPAD_RIGHT     ;
-    uint8_t DPAD_UP_LEFT    = Gamepad::DPAD_UP_LEFT   ;
-    uint8_t DPAD_UP_RIGHT   = Gamepad::DPAD_UP_RIGHT  ;
-    uint8_t DPAD_DOWN_LEFT  = Gamepad::DPAD_DOWN_LEFT ;
-    uint8_t DPAD_DOWN_RIGHT = Gamepad::DPAD_DOWN_RIGHT;
-    uint8_t DPAD_NONE       = Gamepad::DPAD_NONE      ;
-
-    uint16_t BUTTON_A     = Gamepad::BUTTON_A    ;
-    uint16_t BUTTON_B     = Gamepad::BUTTON_B    ;
-    uint16_t BUTTON_X     = Gamepad::BUTTON_X    ;
-    uint16_t BUTTON_Y     = Gamepad::BUTTON_Y    ;
-    uint16_t BUTTON_L3    = Gamepad::BUTTON_L3   ;
-    uint16_t BUTTON_R3    = Gamepad::BUTTON_R3   ;
-    uint16_t BUTTON_BACK  = Gamepad::BUTTON_BACK ;
-    uint16_t BUTTON_START = Gamepad::BUTTON_START;
-    uint16_t BUTTON_LB    = Gamepad::BUTTON_LB   ;
-    uint16_t BUTTON_RB    = Gamepad::BUTTON_RB   ;
-    uint16_t BUTTON_SYS   = Gamepad::BUTTON_SYS  ;
-    uint16_t BUTTON_MISC  = Gamepad::BUTTON_MISC ;
-
-    GamepadMapper() = default;
-    ~GamepadMapper() = default;
-
-    void set_profile(const UserProfile& profile)
-    {
-        set_profile_options(profile);
-        set_profile_mappings(profile);
-        set_profile_deadzones(profile);
-    }
-
-    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
-        <bits> param is optional, used for scaling specific bit values as opposed 
-        to full range values. */
-    template <uint8_t bits = 0, typename T>
-    inline int16_t scale_joystick_rx(T value) const
-    {
-        int16_t joy_value = 0;
-        if constexpr (bits > 0)
-        {
-            joy_value = Range::scale_from_bits<int16_t, bits>(value);
-        }
-        else if constexpr (!std::is_same_v<T, int16_t>)
-        {
-            joy_value = Range::scale<int16_t>(value);
-        }
-        else
-        {
-            joy_value = value;
-        }
-        if (joy_value > dz_.joystick_r_pos)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            dz_.joystick_r_pos, 
-                            Range::MAX<int16_t>, 
-                            Range::MID<int16_t>, 
-                            Range::MAX<int16_t>);
-        }
-        else if (joy_value < dz_.joystick_r_neg)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            Range::MIN<int16_t>, 
-                            dz_.joystick_r_neg, 
-                            Range::MIN<int16_t>, 
-                            Range::MID<int16_t>);
-        }
-        else
-        {
-            joy_value = 0;
-        }
-        return joy_value;
-    }
-
-    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
-        <bits> param is optional, used for scaling specific bit values as opposed 
-        to full range values. */
-    template <uint8_t bits = 0, typename T>
-    inline int16_t scale_joystick_ry(T value, bool invert = false) const
-    {
-        int16_t joy_value = 0;
-        if constexpr (bits > 0)
-        {
-            joy_value = Range::scale_from_bits<int16_t, bits>(value);
-        }
-        else if constexpr (!std::is_same_v<T, int16_t>)
-        {
-            joy_value = Range::scale<int16_t>(value);
-        }
-        else
-        {
-            joy_value = value;
-        }
-        if (joy_value > dz_.joystick_r_pos)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            dz_.joystick_r_pos, 
-                            Range::MAX<int16_t>, 
-                            Range::MID<int16_t>, 
-                            Range::MAX<int16_t>);
-        }
-        else if (joy_value < dz_.joystick_r_neg)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            Range::MIN<int16_t>, 
-                            dz_.joystick_r_neg, 
-                            Range::MIN<int16_t>, 
-                            Range::MID<int16_t>);
-        }
-        else
-        {
-            joy_value = 0;
-        }
-        return profile_invert_ry_ ? (invert ? joy_value : Range::invert(joy_value)) : (invert ? Range::invert(joy_value) : joy_value);
-    }
-
-    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
-        <bits> param is optional, used for scaling specific bit values as opposed 
-        to full range values. */
-    template <uint8_t bits = 0, typename T>
-    inline int16_t scale_joystick_lx(T value) const
-    {
-        int16_t joy_value = 0;
-        if constexpr (bits > 0)
-        {
-            joy_value = Range::scale_from_bits<int16_t, bits>(value);
-        }
-        else if constexpr (!std::is_same_v<T, int16_t>)
-        {
-            joy_value = Range::scale<int16_t>(value);
-        }
-        else
-        {
-            joy_value = value;
-        }
-        if (joy_value > dz_.joystick_l_pos)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            dz_.joystick_l_pos, 
-                            Range::MAX<int16_t>, 
-                            Range::MID<int16_t>, 
-                            Range::MAX<int16_t>);
-        }
-        else if (joy_value < dz_.joystick_l_neg)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            Range::MIN<int16_t>, 
-                            dz_.joystick_l_neg, 
-                            Range::MIN<int16_t>, 
-                            Range::MID<int16_t>);
-        }
-        else
-        {
-            joy_value = 0;
-        }
-        return joy_value;
-    }
-
-    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
-        <bits> param is optional, used for scaling specific bit values as opposed 
-        to full range values. */
-    template <uint8_t bits = 0, typename T>
-    inline int16_t scale_joystick_ly(T value, bool invert = false) const
-    {
-        int16_t joy_value = 0;
-        if constexpr (bits > 0)
-        {
-            joy_value = Range::scale_from_bits<int16_t, bits>(value);
-        }
-        else if constexpr (!std::is_same_v<T, int16_t>)
-        {
-            joy_value = Range::scale<int16_t>(value);
-        }
-        else
-        {
-            joy_value = value;
-        }
-        if (joy_value > dz_.joystick_l_pos)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            dz_.joystick_l_pos, 
-                            Range::MAX<int16_t>, 
-                            Range::MID<int16_t>, 
-                            Range::MAX<int16_t>);
-        }
-        else if (joy_value < dz_.joystick_l_neg)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            Range::MIN<int16_t>, 
-                            dz_.joystick_l_neg, 
-                            Range::MIN<int16_t>, 
-                            Range::MID<int16_t>);
-        }
-        else
-        {
-            joy_value = 0;
-        }
-        return profile_invert_ly_ ? (invert ? joy_value : Range::invert(joy_value)) : (invert ? Range::invert(joy_value) : joy_value);
-    }
-
-    /*  Get trigger value adjusted for deadzones, scaling, and inversion. 
-        <bits> param is optional, used for scaling speicifc bit values 
-        as opposed to full range values */
-    template <uint8_t bits = 0, typename T>
-    inline uint8_t scale_trigger_l(T value) const
-    {
-        uint8_t trigger_value = 0;
-        if constexpr (bits > 0)
-        {
-            trigger_value = Range::scale_from_bits<uint8_t, bits>(value);
-        }
-        else if constexpr (!std::is_same_v<T, uint8_t>)
-        {
-            trigger_value = Range::scale<uint8_t>(value);
-        }
-        else
-        {
-            trigger_value = value;
-        }
-        return trigger_value > dz_.trigger_l ? Range::scale(trigger_value, dz_.trigger_l, Range::MAX<uint8_t>) : 0;
-    }
-
-    /*  Get trigger value adjusted for deadzones, scaling, and inversion. 
-        <bits> param is optional, used for scaling speicifc bit values 
-        as opposed to full range values */
-    template <uint8_t bits = 0, typename T>
-    inline uint8_t scale_trigger_r(T value) const
-    {
-        uint8_t trigger_value = 0;
-        if constexpr (bits > 0)
-        {
-            trigger_value = Range::scale_from_bits<uint8_t, bits>(value);
-        }
-        else if constexpr (!std::is_same_v<T, uint8_t>)
-        {
-            trigger_value = Range::scale<uint8_t>(value);
-        }
-        else
-        {
-            trigger_value = value;
-        }
-        return trigger_value > dz_.trigger_l ? Range::scale(trigger_value, dz_.trigger_l, Range::MAX<uint8_t>) : 0;
-    }
-
-private:
-    bool profile_invert_ly_{false};
-    bool profile_invert_ry_{false};
-
-    struct Deadzones
-    {
-        uint8_t trigger_l{0};
-        uint8_t trigger_r{0};
-        int16_t joystick_l_neg{0};
-        int16_t joystick_l_pos{0};
-        int16_t joystick_r_neg{0};
-        int16_t joystick_r_pos{0};
-    } dz_;
-
-    void set_profile_options(const UserProfile& profile)
-    {
-        profile_invert_ly_ = profile.invert_ly ? true : false;
-        profile_invert_ry_ = profile.invert_ry ? true : false;
-    }
-
-    void set_profile_mappings(const UserProfile& profile)
-    {
-        DPAD_UP         = profile.dpad_up;
-        DPAD_DOWN       = profile.dpad_down;
-        DPAD_LEFT       = profile.dpad_left;
-        DPAD_RIGHT      = profile.dpad_right;
-        DPAD_UP_LEFT    = profile.dpad_up | profile.dpad_left;
-        DPAD_UP_RIGHT   = profile.dpad_up | profile.dpad_right;
-        DPAD_DOWN_LEFT  = profile.dpad_down | profile.dpad_left;
-        DPAD_DOWN_RIGHT = profile.dpad_down | profile.dpad_right;
-        DPAD_NONE       = 0;
-
-        BUTTON_A     = profile.button_a;
-        BUTTON_B     = profile.button_b;
-        BUTTON_X     = profile.button_x;
-        BUTTON_Y     = profile.button_y;
-        BUTTON_L3    = profile.button_l3;
-        BUTTON_R3    = profile.button_r3;
-        BUTTON_BACK  = profile.button_back;
-        BUTTON_START = profile.button_start;
-        BUTTON_LB    = profile.button_lb;
-        BUTTON_RB    = profile.button_rb;
-        BUTTON_SYS   = profile.button_sys;
-        BUTTON_MISC  = profile.button_misc;
-
-        OGXM_LOG("Mappings: A: %i B: %i X: %i Y: %i L3: %i R3: %i BACK: %i START: %i LB: %i RB: %i SYS: %i MISC: %i\n", 
-            BUTTON_A, BUTTON_B, BUTTON_X, BUTTON_Y, BUTTON_L3, BUTTON_R3, BUTTON_BACK, BUTTON_START, BUTTON_LB, BUTTON_RB, BUTTON_SYS, BUTTON_MISC);
-    }
-
-    void set_profile_deadzones(const UserProfile& profile) //Deadzones in the profile are 0-255 (0-100%)
-    {
-        dz_.trigger_l = profile.dz_trigger_l;
-        dz_.trigger_r = profile.dz_trigger_r;
-
-        dz_.joystick_l_pos = profile.dz_joystick_l ? Range::scale<int16_t>(static_cast<int8_t>(profile.dz_joystick_l / 2)) : 0;
-        dz_.joystick_l_neg = Range::invert(dz_.joystick_l_pos);
-        dz_.joystick_r_pos = profile.dz_joystick_r ? Range::scale<int16_t>(static_cast<int8_t>(profile.dz_joystick_r / 2)) : 0;
-        dz_.joystick_r_neg = Range::invert(dz_.joystick_r_pos);
-
-        OGXM_LOG("Deadzones: TL: %i TR: %i JL: %i JR: %i\n", 
-            dz_.trigger_l, dz_.trigger_r, dz_.joystick_l_pos, dz_.joystick_r_pos);
-    }
-
-}; // class GamepadMapper
-
-#endif // GAMEPAD_H
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Gamepad/Gamepad.h b/Firmware/ESP32/main/Gamepad/Gamepad.h
new file mode 100644
index 0000000..7fc2660
--- /dev/null
+++ b/Firmware/ESP32/main/Gamepad/Gamepad.h
@@ -0,0 +1,450 @@
+#ifndef GAMEPAD_H
+#define GAMEPAD_H
+
+#include <cstdint>
+
+#include "sdkconfig.h"
+#include "Gamepad/Range.h"
+#include "Gamepad/fix16ext.h"
+#include "UserSettings/UserProfile.h"
+#include "UserSettings/UserSettings.h"
+#include "Board/ogxm_log.h"
+
+#define MAX_GAMEPADS CONFIG_BLUEPAD32_MAX_DEVICES
+static_assert(  MAX_GAMEPADS > 0 && 
+                MAX_GAMEPADS <= 4, 
+                "MAX_GAMEPADS must be between 1 and 4");
+
+namespace Gamepad
+{
+    static constexpr uint8_t DPAD_UP         = 0x01;
+    static constexpr uint8_t DPAD_DOWN       = 0x02;
+    static constexpr uint8_t DPAD_LEFT       = 0x04;
+    static constexpr uint8_t DPAD_RIGHT      = 0x08;
+    static constexpr uint8_t DPAD_UP_LEFT    = DPAD_UP | DPAD_LEFT;
+    static constexpr uint8_t DPAD_UP_RIGHT   = DPAD_UP | DPAD_RIGHT;
+    static constexpr uint8_t DPAD_DOWN_LEFT  = DPAD_DOWN | DPAD_LEFT;
+    static constexpr uint8_t DPAD_DOWN_RIGHT = DPAD_DOWN | DPAD_RIGHT;
+    static constexpr uint8_t DPAD_NONE       = 0x00;
+
+    static constexpr uint16_t BUTTON_A     = 0x0001;
+    static constexpr uint16_t BUTTON_B     = 0x0002;
+    static constexpr uint16_t BUTTON_X     = 0x0004;
+    static constexpr uint16_t BUTTON_Y     = 0x0008;
+    static constexpr uint16_t BUTTON_L3    = 0x0010;
+    static constexpr uint16_t BUTTON_R3    = 0x0020;
+    static constexpr uint16_t BUTTON_BACK  = 0x0040;
+    static constexpr uint16_t BUTTON_START = 0x0080;
+    static constexpr uint16_t BUTTON_LB    = 0x0100;
+    static constexpr uint16_t BUTTON_RB    = 0x0200;
+    static constexpr uint16_t BUTTON_SYS   = 0x0400;
+    static constexpr uint16_t BUTTON_MISC  = 0x0800;
+
+    static constexpr uint8_t ANALOG_OFF_UP    = 0;
+    static constexpr uint8_t ANALOG_OFF_DOWN  = 1;
+    static constexpr uint8_t ANALOG_OFF_LEFT  = 2;
+    static constexpr uint8_t ANALOG_OFF_RIGHT = 3;
+    static constexpr uint8_t ANALOG_OFF_A     = 4;
+    static constexpr uint8_t ANALOG_OFF_B     = 5;
+    static constexpr uint8_t ANALOG_OFF_X     = 6;
+    static constexpr uint8_t ANALOG_OFF_Y     = 7;
+    static constexpr uint8_t ANALOG_OFF_LB    = 8;
+    static constexpr uint8_t ANALOG_OFF_RB    = 9;
+}
+
+class GamepadMapper
+{
+public:
+    uint8_t DPAD_UP         = Gamepad::DPAD_UP        ;
+    uint8_t DPAD_DOWN       = Gamepad::DPAD_DOWN      ;
+    uint8_t DPAD_LEFT       = Gamepad::DPAD_LEFT      ;
+    uint8_t DPAD_RIGHT      = Gamepad::DPAD_RIGHT     ;
+    uint8_t DPAD_UP_LEFT    = Gamepad::DPAD_UP_LEFT   ;
+    uint8_t DPAD_UP_RIGHT   = Gamepad::DPAD_UP_RIGHT  ;
+    uint8_t DPAD_DOWN_LEFT  = Gamepad::DPAD_DOWN_LEFT ;
+    uint8_t DPAD_DOWN_RIGHT = Gamepad::DPAD_DOWN_RIGHT;
+    uint8_t DPAD_NONE       = Gamepad::DPAD_NONE      ;
+
+    uint16_t BUTTON_A     = Gamepad::BUTTON_A    ;
+    uint16_t BUTTON_B     = Gamepad::BUTTON_B    ;
+    uint16_t BUTTON_X     = Gamepad::BUTTON_X    ;
+    uint16_t BUTTON_Y     = Gamepad::BUTTON_Y    ;
+    uint16_t BUTTON_L3    = Gamepad::BUTTON_L3   ;
+    uint16_t BUTTON_R3    = Gamepad::BUTTON_R3   ;
+    uint16_t BUTTON_BACK  = Gamepad::BUTTON_BACK ;
+    uint16_t BUTTON_START = Gamepad::BUTTON_START;
+    uint16_t BUTTON_LB    = Gamepad::BUTTON_LB   ;
+    uint16_t BUTTON_RB    = Gamepad::BUTTON_RB   ;
+    uint16_t BUTTON_SYS   = Gamepad::BUTTON_SYS  ;
+    uint16_t BUTTON_MISC  = Gamepad::BUTTON_MISC ;
+
+    GamepadMapper() = default;
+    ~GamepadMapper() = default;
+
+    void set_profile(const UserProfile& profile)
+    {
+        set_profile_settings(profile);
+        set_profile_mappings(profile);
+    }
+
+    template <uint8_t bits = 0, typename T>
+    inline std::pair<int16_t, int16_t> scale_joystick_r(T x, T y, bool invert_y = false) const
+    {
+        int16_t joy_x = 0;
+        int16_t joy_y = 0;
+        if constexpr (bits > 0)
+        {
+            joy_x = Range::scale_from_bits<int16_t, bits>(x);
+            joy_y = Range::scale_from_bits<int16_t, bits>(y);
+        }
+        else if constexpr (!std::is_same_v<T, int16_t>)
+        {
+            joy_x = Range::scale<int16_t>(x);
+            joy_y = Range::scale<int16_t>(y);
+        }
+        else
+        {
+            joy_x = x;
+            joy_y = y;
+        }
+
+        return  joy_settings_r_en_ 
+                    ? apply_joystick_settings(joy_x, joy_y, joy_settings_r_, invert_y) 
+                    : std::make_pair(joy_x, (invert_y ? Range::invert(joy_y) : joy_y));
+    }
+
+    template <uint8_t bits = 0, typename T>
+    inline std::pair<int16_t, int16_t> scale_joystick_l(T x, T y, bool invert_y = false) const
+    {
+        int16_t joy_x = 0;
+        int16_t joy_y = 0;
+        if constexpr (bits > 0)
+        {
+            joy_x = Range::scale_from_bits<int16_t, bits>(x);
+            joy_y = Range::scale_from_bits<int16_t, bits>(y);
+        }
+        else if constexpr (!std::is_same_v<T, int16_t>)
+        {
+            joy_x = Range::scale<int16_t>(x);
+            joy_y = Range::scale<int16_t>(y);
+        }
+        else
+        {
+            joy_x = x;
+            joy_y = y;
+        }
+
+        return  joy_settings_l_en_ 
+                    ? apply_joystick_settings(joy_x, joy_y, joy_settings_l_, invert_y) 
+                    : std::make_pair(joy_x, (invert_y ? Range::invert(joy_y) : joy_y));
+    }
+
+    template <uint8_t bits = 0, typename T>
+    inline uint8_t scale_trigger_l(T value) const
+    {
+        uint8_t trigger_value = 0;
+        if constexpr (bits > 0)
+        {
+            trigger_value = Range::scale_from_bits<uint8_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, uint8_t>)
+        {
+            trigger_value = Range::scale<uint8_t>(value);
+        }
+        else
+        {
+            trigger_value = value;
+        }
+        return  trig_settings_l_en_ 
+                    ? apply_trigger_settings(trigger_value, trig_settings_l_) 
+                    : trigger_value;
+    }
+
+    template <uint8_t bits = 0, typename T>
+    inline uint8_t scale_trigger_r(T value) const
+    {
+        uint8_t trigger_value = 0;
+        if constexpr (bits > 0)
+        {
+            trigger_value = Range::scale_from_bits<uint8_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, uint8_t>)
+        {
+            trigger_value = Range::scale<uint8_t>(value);
+        }
+        else
+        {
+            trigger_value = value;
+        }
+        return  trig_settings_r_en_ 
+                    ? apply_trigger_settings(trigger_value, trig_settings_r_) 
+                    : trigger_value;
+    }
+
+private:
+    JoystickSettings joy_settings_l_;
+    JoystickSettings joy_settings_r_;
+    TriggerSettings trig_settings_l_;
+    TriggerSettings trig_settings_r_;
+
+    bool joy_settings_l_en_{false};
+    bool joy_settings_r_en_{false};
+    bool trig_settings_l_en_{false};
+    bool trig_settings_r_en_{false};
+
+    void set_profile_settings(const UserProfile& profile)
+    {
+        if ((joy_settings_l_en_ = !joy_settings_l_.is_same(profile.joystick_settings_l)))
+        {
+            joy_settings_l_.set_from_raw(profile.joystick_settings_l);
+            //This needs to be addressed in the webapp, just multiply here for now
+            joy_settings_l_.axis_restrict *= static_cast<int16_t>(100);
+            joy_settings_l_.angle_restrict *= static_cast<int16_t>(100);
+            joy_settings_l_.anti_dz_angular *= static_cast<int16_t>(100);
+        }
+        if ((joy_settings_r_en_ = !joy_settings_r_.is_same(profile.joystick_settings_r)))
+        {
+            joy_settings_r_.set_from_raw(profile.joystick_settings_r);
+            //This needs to be addressed in the webapp, just multiply here for now
+            joy_settings_r_.axis_restrict *= static_cast<int16_t>(100);
+            joy_settings_r_.angle_restrict *= static_cast<int16_t>(100);
+            joy_settings_r_.anti_dz_angular *= static_cast<int16_t>(100);
+        }
+        if ((trig_settings_l_en_ = !trig_settings_l_.is_same(profile.trigger_settings_l)))
+        {
+            trig_settings_l_.set_from_raw(profile.trigger_settings_l);
+        }
+        if ((trig_settings_r_en_ = !trig_settings_r_.is_same(profile.trigger_settings_r)))
+        {
+            trig_settings_r_.set_from_raw(profile.trigger_settings_r);
+        }
+
+        OGXM_LOG("GamepadMapper: JoyL: %s, JoyR: %s, TrigL: %s, TrigR: %s\n",
+            joy_settings_l_en_ ? "Enabled" : "Disabled",
+            joy_settings_r_en_ ? "Enabled" : "Disabled",
+            trig_settings_l_en_ ? "Enabled" : "Disabled",
+            trig_settings_r_en_ ? "Enabled" : "Disabled");
+    }
+
+    void set_profile_mappings(const UserProfile& profile)
+    {
+        DPAD_UP         = profile.dpad_up;
+        DPAD_DOWN       = profile.dpad_down;
+        DPAD_LEFT       = profile.dpad_left;
+        DPAD_RIGHT      = profile.dpad_right;
+        DPAD_UP_LEFT    = profile.dpad_up | profile.dpad_left;
+        DPAD_UP_RIGHT   = profile.dpad_up | profile.dpad_right;
+        DPAD_DOWN_LEFT  = profile.dpad_down | profile.dpad_left;
+        DPAD_DOWN_RIGHT = profile.dpad_down | profile.dpad_right;
+        DPAD_NONE       = 0;
+
+        BUTTON_A     = profile.button_a;
+        BUTTON_B     = profile.button_b;
+        BUTTON_X     = profile.button_x;
+        BUTTON_Y     = profile.button_y;
+        BUTTON_L3    = profile.button_l3;
+        BUTTON_R3    = profile.button_r3;
+        BUTTON_BACK  = profile.button_back;
+        BUTTON_START = profile.button_start;
+        BUTTON_LB    = profile.button_lb;
+        BUTTON_RB    = profile.button_rb;
+        BUTTON_SYS   = profile.button_sys;
+        BUTTON_MISC  = profile.button_misc;
+    }
+
+    static inline std::pair<int16_t, int16_t> apply_joystick_settings(
+        int16_t gp_joy_x, 
+        int16_t gp_joy_y, 
+        const JoystickSettings& set,
+        bool invert_y)
+    {
+        static const Fix16 
+            FIX_0(0.0f),
+            FIX_1(1.0f),
+            FIX_2(2.0f),
+            FIX_45(45.0f),
+            FIX_90(90.0f),
+            FIX_100(100.0f),
+            FIX_180(180.0f),
+            FIX_EPSILON(0.0001f),
+            FIX_EPSILON2(0.001f),
+            FIX_ELLIPSE_DEF(1.570796f),
+            FIX_DIAG_DIVISOR(0.29289f);
+
+        Fix16 x = (set.invert_x ? Fix16(Range::invert(gp_joy_x)) : Fix16(gp_joy_x)) / Range::MAX<int16_t>;
+        Fix16 y = ((set.invert_y ^ invert_y) ? Fix16(Range::invert(gp_joy_y)) : Fix16(gp_joy_y)) / Range::MAX<int16_t>;
+
+        const Fix16 abs_x = fix16::abs(x);
+        const Fix16 abs_y = fix16::abs(y);
+        const Fix16 inv_axis_restrict = FIX_1 / (FIX_1 - set.axis_restrict);
+
+        Fix16 rAngle = (abs_x < FIX_EPSILON) 
+            ? FIX_90 
+            : fix16::rad2deg(fix16::abs(fix16::atan(y / x)));
+
+        Fix16 axial_x = (abs_x <= set.axis_restrict && rAngle > FIX_45) 
+            ? FIX_0 
+            : ((abs_x - set.axis_restrict) * inv_axis_restrict);
+                
+        Fix16 axial_y = (abs_y <= set.axis_restrict && rAngle <= FIX_45) 
+            ? FIX_0 
+            : ((abs_y - set.axis_restrict) * inv_axis_restrict);
+
+        Fix16 in_magnitude = fix16::sqrt(fix16::sq(axial_x) + fix16::sq(axial_y));
+
+        if (in_magnitude < set.dz_inner)
+        {
+            return { 0, 0 };
+        }
+
+        Fix16 angle = 
+            fix16::abs(axial_x) < FIX_EPSILON 
+                ? FIX_90 
+                : fix16::rad2deg(fix16::abs(fix16::atan(axial_y / axial_x)));
+
+        Fix16 anti_r_scale = (set.anti_dz_square_y_scale == FIX_0) ? set.anti_dz_square : set.anti_dz_square_y_scale;
+        Fix16 anti_dz_c = set.anti_dz_circle;
+
+        if (anti_r_scale > FIX_0 && anti_dz_c > FIX_0)
+        {
+            Fix16 anti_ellip_scale = anti_ellip_scale / anti_dz_c;
+            Fix16 ellipse_angle = fix16::atan((FIX_1 / anti_ellip_scale) * fix16::tan(fix16::rad2deg(rAngle)));
+            ellipse_angle = (ellipse_angle < FIX_0) ? FIX_ELLIPSE_DEF : ellipse_angle;
+
+            Fix16 ellipse_x = fix16::cos(ellipse_angle);
+            Fix16 ellipse_y = fix16::sqrt(fix16::sq(anti_ellip_scale) * (FIX_1 - fix16::sq(ellipse_x)));
+            anti_dz_c *= fix16::sqrt(fix16::sq(ellipse_x) + fix16::sq(ellipse_y));
+        }
+
+        if (anti_dz_c > FIX_0)
+        {
+            anti_dz_c = anti_dz_c / ((anti_dz_c * (FIX_1 - set.anti_dz_circle / set.dz_outer)) / (anti_dz_c * (FIX_1 - set.anti_dz_square)));
+        }
+
+        if (abs_x > set.axis_restrict && abs_y > set.axis_restrict)
+        {
+            const Fix16 FIX_ANGLE_MAX = set.angle_restrict / 2.0f;
+
+            if (angle > FIX_0 && angle < FIX_ANGLE_MAX)
+            {
+                angle = FIX_0;
+            }
+            if (angle > (FIX_90 - FIX_ANGLE_MAX))
+            {
+                angle = FIX_90;
+            }
+            if (angle > FIX_ANGLE_MAX && angle < (FIX_90 - FIX_ANGLE_MAX))
+            {
+                angle = ((angle - FIX_ANGLE_MAX) * FIX_90) / ((FIX_90 - FIX_ANGLE_MAX) - FIX_ANGLE_MAX);
+            }
+        }
+
+        Fix16 ref_angle = (angle < FIX_EPSILON2) ? FIX_0 : angle;
+        Fix16 diagonal = (angle > FIX_45) ? (((angle - FIX_45) * (-FIX_45)) / FIX_45) + FIX_45 : angle;
+
+        const Fix16 angle_comp = set.angle_restrict / FIX_2;
+
+        if (angle < FIX_90 && angle > FIX_0)
+        {
+            angle = ((angle * ((FIX_90 - angle_comp) - angle_comp)) / FIX_90) + angle_comp;
+        }
+
+        if (axial_x < FIX_0 && axial_y > FIX_0)
+        {
+            angle = -angle;
+        }
+        if (axial_x > FIX_0 && axial_y < FIX_0)
+        {
+            angle = angle - FIX_180;
+        }
+        if (axial_x < FIX_0 && axial_y < FIX_0)
+        {
+            angle = angle + FIX_180;
+        }
+
+        //Deadzone Warp
+        Fix16 out_magnitude = (in_magnitude - set.dz_inner) / (set.anti_dz_outer - set.dz_inner);
+        out_magnitude = fix16::pow(out_magnitude, (FIX_1 / set.curve)) * (set.dz_outer - anti_dz_c) + anti_dz_c;
+        out_magnitude = (out_magnitude > set.dz_outer && !set.uncap_radius) ? set.dz_outer : out_magnitude;
+
+		Fix16 d_scale = (((out_magnitude - anti_dz_c) * (set.diag_scale_max - set.diag_scale_min)) / (set.dz_outer - anti_dz_c)) + set.diag_scale_min;		
+		Fix16 c_scale = (diagonal * (FIX_1 / fix16::sqrt(FIX_2))) / FIX_45; //Both these lines scale the intensity of the warping
+		c_scale       = FIX_1 - fix16::sqrt(FIX_1 - c_scale * c_scale);     //based on a circular curve to the perfect diagonal
+		d_scale       = (c_scale * (d_scale - FIX_1)) / FIX_DIAG_DIVISOR + FIX_1;
+
+		out_magnitude = out_magnitude * d_scale;
+
+		//Scaling values for square antideadzone
+		Fix16 new_x = fix16::cos(fix16::deg2rad(angle)) * out_magnitude;
+		Fix16 new_y = fix16::sin(fix16::deg2rad(angle)) * out_magnitude;
+		
+		//Magic angle wobble fix by user ME.
+		// if (angle > 45.0 && angle < 225.0) {
+		// 	newX = inv(Math.sin(deg2rad(angle - 90.0)))*outputMagnitude;
+		// 	newY = inv(Math.cos(deg2rad(angle - 270.0)))*outputMagnitude;
+		// }
+		
+		//Square antideadzone scaling
+		Fix16 output_x = fix16::abs(new_x) * (FIX_1 - set.anti_dz_square / set.dz_outer) + set.anti_dz_square;
+		if (x < FIX_0)
+        {
+            output_x = -output_x;
+        }
+		if (ref_angle == FIX_90)
+        {
+            output_x = FIX_0;
+        }
+		
+		Fix16 output_y = fix16::abs(new_y) * (FIX_1 - anti_r_scale / set.dz_outer) + anti_r_scale;
+		if (y < FIX_0)
+        {
+            output_y = -output_y;
+        }
+		if (ref_angle == FIX_0)
+        {
+            output_y = FIX_0;
+        }
+
+        output_x = fix16::clamp(output_x, -FIX_1, FIX_1) * Range::MAX<int16_t>;
+        output_y = fix16::clamp(output_y, -FIX_1, FIX_1) * Range::MAX<int16_t>;
+
+        return { static_cast<int16_t>(fix16_to_int(output_x)), static_cast<int16_t>(fix16_to_int(output_y)) };
+    }
+
+    static inline uint8_t apply_trigger_settings(uint8_t value, const TriggerSettings& set)
+    {
+        Fix16 abs_value = fix16::abs(Fix16(static_cast<int16_t>(value)) / static_cast<int16_t>(Range::MAX<uint8_t>));
+
+        if (abs_value < set.dz_inner)
+        {
+            return 0;
+        }
+
+        static const Fix16 
+            FIX_0(0.0f),
+            FIX_1(1.0f),
+            FIX_2(2.0f);
+
+        Fix16 value_out = (abs_value - set.dz_inner) / (set.anti_dz_outer - set.dz_inner);
+        value_out = fix16::clamp(value_out, FIX_0, FIX_1);
+
+        if (set.anti_dz_inner > FIX_0)
+        {
+            value_out = set.anti_dz_inner + (FIX_1 - set.anti_dz_inner) * value_out;
+        }
+        if (set.curve != FIX_1)
+        {
+            value_out = fix16::pow(value_out, FIX_1 / set.curve);
+        }
+        if (set.anti_dz_outer < FIX_1)
+        {
+            value_out = fix16::clamp(value_out * (FIX_1 / (FIX_1 - set.anti_dz_outer)), FIX_0, FIX_1);
+        }
+
+        value_out *= set.dz_outer;
+        return static_cast<uint8_t>(fix16_to_int(value_out * static_cast<int16_t>(Range::MAX<uint8_t>)));
+    }
+
+}; // class GamepadMapper
+
+#endif // GAMEPAD_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Range.h b/Firmware/ESP32/main/Gamepad/Range.h
similarity index 65%
rename from Firmware/RP2040/src/Range.h
rename to Firmware/ESP32/main/Gamepad/Range.h
index 115db96..667bc61 100644
--- a/Firmware/RP2040/src/Range.h
+++ b/Firmware/ESP32/main/Gamepad/Range.h
@@ -79,77 +79,31 @@ namespace Range {
     requires std::is_integral_v<To> && std::is_integral_v<From>
     static inline To clamp(From value) 
     {
-        if constexpr (std::is_signed_v<From> != std::is_signed_v<To>)
-        {
-            using CommonType = std::common_type_t<To, From>;
-            return static_cast<To>((static_cast<CommonType>(value) < static_cast<CommonType>(Range::MIN<To>)) 
-                                    ? Range::MIN<To> 
-                                    : (static_cast<CommonType>(value) > static_cast<CommonType>(Range::MAX<To>)) 
-                                        ? Range::MAX<To> 
-                                        : static_cast<CommonType>(value));
-        }
-        else
-        {
-            return static_cast<To>((value < Range::MIN<To>) 
-                                    ? Range::MIN<To> 
-                                    : (value > Range::MAX<To>) 
-                                        ? Range::MAX<To> 
-                                        : value);
-        }
+        return static_cast<To>((value < Range::MIN<To>) 
+                                ? Range::MIN<To> 
+                                : (value > Range::MAX<To>) 
+                                    ? Range::MAX<To> 
+                                    : value);
     }
 
     template <typename T>
-    requires std::is_integral_v<T>
     static inline T clamp(T value, T min, T max) 
     {
         return (value < min) ? min : (value > max) ? max : value;
     }
 
+    template <typename To, typename From> 
+    static inline To clamp(From value, To min_to, To max_to) 
+    {
+        return (value < min_to) ? min_to : (value > max_to) ? max_to : static_cast<To>(value);
+    }
+
     template <typename To, typename From>
     requires std::is_integral_v<To> && std::is_integral_v<From>
-    static inline To scale(From value, From min_from, From max_from, To min_to, To max_to) 
+    static constexpr To scale(From value, From min_from, From max_from, To min_to, To max_to) 
     {
-        if constexpr (std::is_unsigned_v<From> && std::is_unsigned_v<To>) 
-        {
-            // Both unsigned
-            uint64_t scaled = static_cast<uint64_t>(value - min_from) * 
-                            (max_to - min_to) / 
-                            (max_from - min_from) + min_to;
-            return static_cast<To>(scaled);
-        } 
-        else if constexpr (std::is_signed_v<From> && std::is_unsigned_v<To>) 
-        {
-            // From signed, To unsigned
-            uint64_t shift_from = static_cast<uint64_t>(-min_from);
-            uint64_t u_value = static_cast<uint64_t>(value) + shift_from;
-            uint64_t u_min_from = static_cast<uint64_t>(min_from) + shift_from;
-            uint64_t u_max_from = static_cast<uint64_t>(max_from) + shift_from;
-
-            uint64_t scaled = (u_value - u_min_from) * 
-                            (max_to - min_to) / 
-                            (u_max_from - u_min_from) + min_to;
-            return static_cast<To>(scaled);
-        } 
-        else if constexpr (std::is_unsigned_v<From> && std::is_signed_v<To>) 
-        {
-            // From unsigned, To signed
-            uint64_t shift_to = static_cast<uint64_t>(-min_to);
-            uint64_t scaled = static_cast<uint64_t>(value - min_from) * 
-                            (static_cast<uint64_t>(max_to) + shift_to - static_cast<uint64_t>(min_to) - shift_to) / 
-                            (max_from - min_from) + static_cast<uint64_t>(min_to) + shift_to;
-            return static_cast<To>(scaled - shift_to);
-        } 
-        else 
-        {
-            // Both signed
-            int64_t shift_from = -min_from;
-            int64_t shift_to = -min_to;
-
-            int64_t scaled = (static_cast<int64_t>(value) + shift_from - (min_from + shift_from)) * 
-                            (max_to + shift_to - (min_to + shift_to)) / 
-                            (max_from - min_from) + (min_to + shift_to);
-            return static_cast<To>(scaled - shift_to);
-        }
+        return static_cast<To>(
+            (static_cast<int64_t>(value - min_from) * (max_to - min_to) / (max_from - min_from)) + min_to);
     }
 
     template <typename To, typename From>
diff --git a/Firmware/ESP32/main/Gamepad/fix16ext.h b/Firmware/ESP32/main/Gamepad/fix16ext.h
new file mode 100644
index 0000000..b59b027
--- /dev/null
+++ b/Firmware/ESP32/main/Gamepad/fix16ext.h
@@ -0,0 +1,106 @@
+#ifndef FIX16_EXT_H
+#define FIX16_EXT_H
+
+#include <cstdint>
+
+#include "libfixmath/fix16.hpp"
+
+namespace fix16 {
+
+inline Fix16 abs(Fix16 x)
+{
+    return Fix16(fix16_abs(x.value));
+}
+
+inline Fix16 rad2deg(Fix16 rad)
+{
+    return Fix16(fix16_rad_to_deg(rad.value));
+}
+
+inline Fix16 deg2rad(Fix16 deg)
+{
+    return Fix16(fix16_deg_to_rad(deg.value));
+}
+
+inline Fix16 atan(Fix16 x)
+{
+    return Fix16(fix16_atan(x.value));
+}
+
+inline Fix16 atan2(Fix16 y, Fix16 x)
+{
+    return Fix16(fix16_atan2(y.value, x.value));
+}
+
+inline Fix16 tan(Fix16 x)
+{
+    return Fix16(fix16_tan(x.value));
+}
+
+inline Fix16 cos(Fix16 x)
+{
+    return Fix16(fix16_cos(x.value));
+}
+
+inline Fix16 sin(Fix16 x)
+{
+    return Fix16(fix16_sin(x.value));
+}
+
+inline Fix16 sqrt(Fix16 x)
+{
+    return Fix16(fix16_sqrt(x.value));
+}
+
+inline Fix16 sq(Fix16 x)
+{
+    return Fix16(fix16_sq(x.value));
+}
+
+inline Fix16 clamp(Fix16 x, Fix16 min, Fix16 max)
+{
+    return Fix16(fix16_clamp(x.value, min.value, max.value));
+}
+
+inline Fix16 pow(Fix16 x, Fix16 y)
+{
+    fix16_t& base = x.value;
+    fix16_t& exponent = y.value;
+
+    if (exponent == F16(0.0)) 
+        return Fix16(fix16_from_int(1));
+    if (base == F16(0.0)) 
+        return Fix16(fix16_from_int(0));
+
+    int32_t int_exp = fix16_to_int(exponent);
+
+    if (fix16_from_int(int_exp) == exponent) 
+    {
+        fix16_t result = F16(1.0);
+        fix16_t current_base = base;
+
+        if (int_exp < 0) 
+        {
+            current_base = fix16_div(F16(1.0), base);
+            int_exp = -int_exp;
+        }
+
+        while (int_exp) 
+        {
+            if (int_exp & 1) 
+            {
+                result = fix16_mul(result, current_base);
+            }
+            current_base = fix16_mul(current_base, current_base);
+            int_exp >>= 1;
+        }
+
+        return Fix16(result);
+    }
+
+    return Fix16(fix16_exp(fix16_mul(exponent, fix16_log(base))));
+}
+
+} // namespace fix16
+
+#endif // FIX16_EXT_H
\ No newline at end of file
diff --git a/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp b/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
index 493e29a..453443a 100644
--- a/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
+++ b/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
@@ -15,7 +15,7 @@ void I2CDriver::initialize_i2c(i2c_port_t i2c_port, gpio_num_t sda, gpio_num_t s
 {
     if (initialized_)
     {
-        i2c_driver_delete(i2c_port_);
+        return;
     }
 
     i2c_port_ = i2c_port;
diff --git a/Firmware/ESP32/main/UserSettings/JoystickSettings.cpp b/Firmware/ESP32/main/UserSettings/JoystickSettings.cpp
new file mode 100644
index 0000000..9e5bc99
--- /dev/null
+++ b/Firmware/ESP32/main/UserSettings/JoystickSettings.cpp
@@ -0,0 +1,63 @@
+#include "Board/ogxm_log.h"
+#include "UserSettings/JoystickSettings.h"
+
+bool JoystickSettings::is_same(const JoystickSettingsRaw& raw) const
+{
+    return  dz_inner == Fix16(raw.dz_inner) &&
+            dz_outer == Fix16(raw.dz_outer) &&
+            anti_dz_circle == Fix16(raw.anti_dz_circle) &&
+            anti_dz_circle_y_scale == Fix16(raw.anti_dz_circle_y_scale) &&
+            anti_dz_square == Fix16(raw.anti_dz_square) &&
+            anti_dz_square_y_scale == Fix16(raw.anti_dz_square_y_scale) &&
+            anti_dz_angular == Fix16(raw.anti_dz_angular) &&
+            anti_dz_outer == Fix16(raw.anti_dz_outer) &&
+            axis_restrict == Fix16(raw.axis_restrict) &&
+            angle_restrict == Fix16(raw.angle_restrict) &&
+            diag_scale_min == Fix16(raw.diag_scale_min) &&
+            diag_scale_max == Fix16(raw.diag_scale_max) &&
+            curve == Fix16(raw.curve) &&
+            uncap_radius == raw.uncap_radius &&
+            invert_y == raw.invert_y &&
+            invert_x == raw.invert_x;
+}
+
+void JoystickSettings::set_from_raw(const JoystickSettingsRaw& raw)
+{
+    dz_inner = Fix16(raw.dz_inner);
+    dz_outer = Fix16(raw.dz_outer);
+    anti_dz_circle = Fix16(raw.anti_dz_circle);
+    anti_dz_circle_y_scale = Fix16(raw.anti_dz_circle_y_scale);
+    anti_dz_square = Fix16(raw.anti_dz_square);
+    anti_dz_square_y_scale = Fix16(raw.anti_dz_square_y_scale);
+    anti_dz_angular = Fix16(raw.anti_dz_angular);
+    anti_dz_outer = Fix16(raw.anti_dz_outer);
+    axis_restrict = Fix16(raw.axis_restrict);
+    angle_restrict = Fix16(raw.angle_restrict);
+    diag_scale_min = Fix16(raw.diag_scale_min);
+    diag_scale_max = Fix16(raw.diag_scale_max);
+    curve = Fix16(raw.curve);
+    uncap_radius = raw.uncap_radius;
+    invert_y = raw.invert_y;
+    invert_x = raw.invert_x;
+}
+
+void JoystickSettingsRaw::log_values()
+{
+    OGXM_LOG("dz_inner: %f\n", fix16_to_float(dz_inner));
+    OGXM_LOG_HEX("dz_inner: ", reinterpret_cast<uint8_t*>(&dz_inner), sizeof(dz_inner));
+    OGXM_LOG("dz_outer: %f\n", fix16_to_float(dz_outer));
+    OGXM_LOG("anti_dz_circle: %f\n", fix16_to_float(anti_dz_circle));
+    OGXM_LOG("anti_dz_circle_y_scale: %f\n", fix16_to_float(anti_dz_circle_y_scale));
+    OGXM_LOG("anti_dz_square: %f\n", fix16_to_float(anti_dz_square));
+    OGXM_LOG("anti_dz_square_y_scale: %f\n", fix16_to_float(anti_dz_square_y_scale));
+    OGXM_LOG("anti_dz_angular: %f\n", fix16_to_float(anti_dz_angular));
+    OGXM_LOG("anti_dz_outer: %f\n", fix16_to_float(anti_dz_outer));
+    OGXM_LOG("axis_restrict: %f\n", fix16_to_float(axis_restrict));
+    OGXM_LOG("angle_restrict: %f\n", fix16_to_float(angle_restrict));
+    OGXM_LOG("diag_scale_min: %f\n", fix16_to_float(diag_scale_min));
+    OGXM_LOG("diag_scale_max: %f\n", fix16_to_float(diag_scale_max));
+    OGXM_LOG("curve: %f\n", fix16_to_float(curve));
+    OGXM_LOG("uncap_radius: %d\n", uncap_radius);
+    OGXM_LOG("invert_y: %d\n", invert_y);
+    OGXM_LOG("invert_x: %d\n", invert_x);
+}
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/JoystickSettings.h b/Firmware/ESP32/main/UserSettings/JoystickSettings.h
new file mode 100644
index 0000000..88b6109
--- /dev/null
+++ b/Firmware/ESP32/main/UserSettings/JoystickSettings.h
@@ -0,0 +1,68 @@
+#ifndef _JOYSTICK_SETTINGS_H_
+#define _JOYSTICK_SETTINGS_H_
+
+#include <cstdint>
+
+#include "libfixmath/fix16.hpp"
+
+struct JoystickSettingsRaw;
+
+struct JoystickSettings
+{
+    Fix16 dz_inner{Fix16(0.0f)};
+    Fix16 dz_outer{Fix16(1.0f)};
+
+    Fix16 anti_dz_circle{Fix16(0.0f)};
+    Fix16 anti_dz_circle_y_scale{Fix16(0.0f)};
+    Fix16 anti_dz_square{Fix16(0.0f)};
+    Fix16 anti_dz_square_y_scale{Fix16(0.0f)};
+    Fix16 anti_dz_angular{Fix16(0.0f)};
+    Fix16 anti_dz_outer{Fix16(1.0f)};
+    
+    Fix16 axis_restrict{Fix16(0.0f)};
+    Fix16 angle_restrict{Fix16(0.0f)};
+    
+    Fix16 diag_scale_min{Fix16(1.0f)};
+    Fix16 diag_scale_max{Fix16(1.0f)};
+
+    Fix16 curve{Fix16(1.0f)};
+
+    bool uncap_radius{true};
+    bool invert_y{false};
+    bool invert_x{false};
+
+    bool is_same(const JoystickSettingsRaw& raw) const;
+    void set_from_raw(const JoystickSettingsRaw& raw);
+};
+
+#pragma pack(push, 1)
+struct JoystickSettingsRaw
+{
+    fix16_t dz_inner{fix16_from_int(0)};
+    fix16_t dz_outer{fix16_from_int(1)};
+
+    fix16_t anti_dz_circle{fix16_from_int(0)};
+    fix16_t anti_dz_circle_y_scale{fix16_from_int(0)};
+    fix16_t anti_dz_square{fix16_from_int(0)};
+    fix16_t anti_dz_square_y_scale{fix16_from_int(0)};
+    fix16_t anti_dz_angular{fix16_from_int(0)};
+    fix16_t anti_dz_outer{fix16_from_int(1)};
+    
+    fix16_t axis_restrict{fix16_from_int(0)};
+    fix16_t angle_restrict{fix16_from_int(0)};
+    
+    fix16_t diag_scale_min{fix16_from_int(1)};
+    fix16_t diag_scale_max{fix16_from_int(1)};
+
+    fix16_t curve{fix16_from_int(1)};
+
+    uint8_t uncap_radius{true};
+    uint8_t invert_y{false};
+    uint8_t invert_x{false};
+
+    void log_values();
+};
+static_assert(sizeof(JoystickSettingsRaw) == 55, "JoystickSettingsRaw is an unexpected size");
+#pragma pack(pop)
+
+#endif // _JOYSTICK_SETTINGS_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/NVSHelper.h b/Firmware/ESP32/main/UserSettings/NVSHelper.h
index c28ec49..e4390b7 100644
--- a/Firmware/ESP32/main/UserSettings/NVSHelper.h
+++ b/Firmware/ESP32/main/UserSettings/NVSHelper.h
@@ -106,7 +106,7 @@ private:
 
     SemaphoreHandle_t nvs_mutex_;
 
-    static constexpr const char NVS_NAMESPACE[] = "user_data";
+    static constexpr char NVS_NAMESPACE[] = "user_data";
     
 }; // class NVSHelper
 
diff --git a/Firmware/ESP32/main/UserSettings/TriggerSettings.cpp b/Firmware/ESP32/main/UserSettings/TriggerSettings.cpp
new file mode 100644
index 0000000..de2806b
--- /dev/null
+++ b/Firmware/ESP32/main/UserSettings/TriggerSettings.cpp
@@ -0,0 +1,29 @@
+#include "Board/ogxm_log.h"
+#include "UserSettings/TriggerSettings.h"
+
+bool TriggerSettings::is_same(const TriggerSettingsRaw& raw) const
+{
+    return  dz_inner == Fix16(raw.dz_inner) &&
+            dz_outer == Fix16(raw.dz_outer) &&
+            anti_dz_inner == Fix16(raw.anti_dz_inner) &&
+            anti_dz_outer == Fix16(raw.anti_dz_outer) &&
+            curve == Fix16(raw.curve);
+}
+
+void TriggerSettings::set_from_raw(const TriggerSettingsRaw& raw)
+{
+    dz_inner = Fix16(raw.dz_inner);
+    dz_outer = Fix16(raw.dz_outer);
+    anti_dz_inner = Fix16(raw.anti_dz_inner);
+    anti_dz_outer = Fix16(raw.anti_dz_outer);
+    curve = Fix16(raw.curve);
+}
+
+void TriggerSettingsRaw::log_values()
+{
+    OGXM_LOG("dz_inner: %f\n", fix16_to_float(dz_inner));
+    OGXM_LOG("dz_outer: %f\n", fix16_to_float(dz_outer));
+    OGXM_LOG("anti_dz_inner: %f\n", fix16_to_float(anti_dz_inner));
+    OGXM_LOG("anti_dz_outer: %f\n", fix16_to_float(anti_dz_outer));
+    OGXM_LOG("curve: %f\n", fix16_to_float(curve));
+}
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/TriggerSettings.h b/Firmware/ESP32/main/UserSettings/TriggerSettings.h
new file mode 100644
index 0000000..5a8f807
--- /dev/null
+++ b/Firmware/ESP32/main/UserSettings/TriggerSettings.h
@@ -0,0 +1,40 @@
+#ifndef TRIGGER_SETTINGS_H
+#define TRIGGER_SETTINGS_H
+
+#include <cstdint>
+
+#include "libfixmath/fix16.hpp"
+
+struct TriggerSettingsRaw;
+
+struct TriggerSettings
+{
+    Fix16 dz_inner{Fix16(0.0f)};
+    Fix16 dz_outer{Fix16(1.0f)};
+
+    Fix16 anti_dz_inner{Fix16(0.0f)};
+    Fix16 anti_dz_outer{Fix16(1.0f)};
+
+    Fix16 curve{Fix16(1.0f)};
+
+    bool is_same(const TriggerSettingsRaw& raw) const;
+    void set_from_raw(const TriggerSettingsRaw& raw);
+};
+
+#pragma pack(push, 1)
+struct TriggerSettingsRaw
+{
+    fix16_t dz_inner{fix16_from_int(0)};
+    fix16_t dz_outer{fix16_from_int(1)};
+
+    fix16_t anti_dz_inner{fix16_from_int(0)};
+    fix16_t anti_dz_outer{fix16_from_int(1)};
+    
+    fix16_t curve{fix16_from_int(1)};
+
+    void log_values();
+};
+static_assert(sizeof(TriggerSettingsRaw) == 20, "TriggerSettingsRaw is an unexpected size");
+#pragma pack(pop)
+
+#endif // TRIGGER_SETTINGS_H
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/UserProfile.cpp b/Firmware/ESP32/main/UserSettings/UserProfile.cpp
index 1f77b59..4e50d97 100644
--- a/Firmware/ESP32/main/UserSettings/UserProfile.cpp
+++ b/Firmware/ESP32/main/UserSettings/UserProfile.cpp
@@ -1,20 +1,12 @@
 #include <cstring>
 
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 #include "UserSettings/UserProfile.h"
 
 UserProfile::UserProfile()
 {
     id = 1;
 
-    dz_trigger_l = 0;
-    dz_trigger_r = 0;
-    dz_joystick_l = 0;
-    dz_joystick_r = 0;
-
-    invert_ly = 0;
-    invert_ry = 0;
-
     dpad_up = Gamepad::DPAD_UP;
     dpad_down = Gamepad::DPAD_DOWN;
     dpad_left = Gamepad::DPAD_LEFT;
@@ -33,16 +25,16 @@ UserProfile::UserProfile()
     button_sys = Gamepad::BUTTON_SYS;
     button_misc = Gamepad::BUTTON_MISC;
 
-    analog_enabled = 1;
+    analog_enabled = 0;
 
-    analog_off_up = 0;
-    analog_off_down = 1;
-    analog_off_left = 2;
-    analog_off_right = 3;
-    analog_off_a = 4;
-    analog_off_b = 5;
-    analog_off_x = 6;
-    analog_off_y = 7;
-    analog_off_lb = 8;
-    analog_off_rb = 9;
+    analog_off_up = Gamepad::ANALOG_OFF_UP;
+    analog_off_down = Gamepad::ANALOG_OFF_DOWN;
+    analog_off_left = Gamepad::ANALOG_OFF_LEFT;
+    analog_off_right = Gamepad::ANALOG_OFF_RIGHT;
+    analog_off_a = Gamepad::ANALOG_OFF_A;
+    analog_off_b = Gamepad::ANALOG_OFF_B;
+    analog_off_x = Gamepad::ANALOG_OFF_X;
+    analog_off_y = Gamepad::ANALOG_OFF_Y;
+    analog_off_lb = Gamepad::ANALOG_OFF_LB;
+    analog_off_rb = Gamepad::ANALOG_OFF_RB;
 }
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/UserProfile.h b/Firmware/ESP32/main/UserSettings/UserProfile.h
index 2efa406..6bd28f2 100644
--- a/Firmware/ESP32/main/UserSettings/UserProfile.h
+++ b/Firmware/ESP32/main/UserSettings/UserProfile.h
@@ -3,19 +3,18 @@
 
 #include <cstdint>
 
+#include "UserSettings/JoystickSettings.h"
+#include "UserSettings/TriggerSettings.h"   
+
 #pragma pack(push, 1)
 struct UserProfile
 {
     uint8_t id;
 
-    uint8_t dz_trigger_l;
-    uint8_t dz_trigger_r;
-
-    uint8_t dz_joystick_l;
-    uint8_t dz_joystick_r;
-
-    uint8_t invert_ly;
-    uint8_t invert_ry;
+    JoystickSettingsRaw joystick_settings_l;
+    JoystickSettingsRaw joystick_settings_r;
+    TriggerSettingsRaw trigger_settings_l;
+    TriggerSettingsRaw trigger_settings_r;
 
     uint8_t dpad_up;
     uint8_t dpad_down;
@@ -50,7 +49,7 @@ struct UserProfile
 
     UserProfile();
 };
-static_assert(sizeof(UserProfile) == 46, "UserProfile struct size mismatch");
+static_assert(sizeof(UserProfile) == 190, "UserProfile struct size mismatch");
 #pragma pack(pop)
 
 #endif // _USER_PROFILE_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/UserSettings.cpp b/Firmware/ESP32/main/UserSettings/UserSettings.cpp
index 00e9852..28d2ffe 100644
--- a/Firmware/ESP32/main/UserSettings/UserSettings.cpp
+++ b/Firmware/ESP32/main/UserSettings/UserSettings.cpp
@@ -6,7 +6,7 @@
 #include <esp_err.h>
 #include <esp_log.h>
 
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 #include "UserSettings/NVSHelper.h"
 
 static constexpr uint32_t BUTTON_COMBO(const uint16_t& buttons, const uint8_t& dpad = 0)
@@ -85,7 +85,6 @@ const std::string UserSettings::FIRMWARE_VER_KEY()
     return std::string("firmware_ver");
 }
 
-//Checks for first boot and initializes user profiles, call before tusb is inited.
 void UserSettings::initialize_flash()
 {
     ESP_LOGD("UserSettings", "Checking for UserSettings init flag");
@@ -100,6 +99,9 @@ void UserSettings::initialize_flash()
         return;
     }
 
+    ESP_ERROR_CHECK(nvs_helper_.erase_all());
+    OGXM_LOG("Initializing UserSettings\n");
+
     current_driver_ = DEFAULT_DRIVER();
     uint8_t driver_type = static_cast<uint8_t>(current_driver_);
     ESP_ERROR_CHECK(nvs_helper_.write(DRIVER_TYPE_KEY(), &driver_type, sizeof(driver_type)));
@@ -198,20 +200,24 @@ void UserSettings::store_driver_type(DeviceDriverType new_driver_type)
     nvs_helper_.write(DRIVER_TYPE_KEY(), &new_driver, sizeof(new_driver));
 }
 
-void UserSettings::store_profile(const uint8_t index, const UserProfile& profile)
+void UserSettings::store_profile(const uint8_t index, UserProfile& profile)
 {
     if (index > MAX_GAMEPADS || profile.id < 1 || profile.id > MAX_PROFILES)
     {
         return;
     }
 
+    OGXM_LOG("Storing profile %d for gamepad %d\n", profile.id, index);
+
     if (nvs_helper_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile)) == ESP_OK)
     {
+        OGXM_LOG("Profile %d stored successfully\n", profile.id);
+
         nvs_helper_.write(ACTIVE_PROFILE_KEY(index), &profile.id, sizeof(profile.id));
     }
 }
 
-void UserSettings::store_profile_and_driver_type(DeviceDriverType new_driver_type, const uint8_t index, const UserProfile& profile)
+void UserSettings::store_profile_and_driver_type(DeviceDriverType new_driver_type, const uint8_t index, UserProfile& profile)
 {
     store_driver_type(new_driver_type);
     store_profile(index, profile);
@@ -243,10 +249,10 @@ UserProfile UserSettings::get_profile_by_id(const uint8_t profile_id)
         profile_id > MAX_PROFILES ||
         nvs_helper_.read(PROFILE_KEY(profile_id), &profile, sizeof(UserProfile)) != ESP_OK)
     {
-        return profile;
+        return UserProfile();
     }
 
-    return UserProfile();
+    return profile;
 }
 
 DeviceDriverType UserSettings::DEFAULT_DRIVER()
diff --git a/Firmware/ESP32/main/UserSettings/UserSettings.h b/Firmware/ESP32/main/UserSettings/UserSettings.h
index 16f016a..8868a34 100644
--- a/Firmware/ESP32/main/UserSettings/UserSettings.h
+++ b/Firmware/ESP32/main/UserSettings/UserSettings.h
@@ -32,8 +32,8 @@ public:
     uint8_t get_active_profile_id(const uint8_t index);
 
     void store_driver_type(DeviceDriverType new_driver_type);
-    void store_profile(uint8_t index, const UserProfile& profile);
-    void store_profile_and_driver_type(DeviceDriverType new_driver_type, uint8_t index, const UserProfile& profile);
+    void store_profile(uint8_t index, UserProfile& profile);
+    void store_profile_and_driver_type(DeviceDriverType new_driver_type, uint8_t index, UserProfile& profile);
 
 private:
     UserSettings() = default;
@@ -42,7 +42,7 @@ private:
     UserSettings& operator=(const UserSettings&) = delete;
 
     static constexpr uint8_t GP_CHECK_COUNT = 3000 / GP_CHECK_DELAY_MS;
-    static constexpr uint8_t INIT_FLAG = 0x82;
+    static constexpr uint8_t INIT_FLAG = 0x12;
 
     NVSHelper& nvs_helper_{NVSHelper::get_instance()};
     DeviceDriverType current_driver_{DeviceDriverType::NONE};
diff --git a/Firmware/ESP32/main/1btstack_config.h b/Firmware/ESP32/main/btstack_config.h
similarity index 98%
rename from Firmware/ESP32/main/1btstack_config.h
rename to Firmware/ESP32/main/btstack_config.h
index be4f0c4..7c3c33c 100644
--- a/Firmware/ESP32/main/1btstack_config.h
+++ b/Firmware/ESP32/main/btstack_config.h
@@ -55,8 +55,8 @@
 // #define NVM_NUM_LINK_KEYS 16
 
 // We don't give btstack a malloc, so use a fixed-size ATT DB.
-// #define MAX_ATT_DB_SIZE 512
-#define HAVE_MALLOC
+#define MAX_ATT_DB_SIZE 512
+// #define HAVE_MALLOC
 
 // BTstack HAL configuration
 // #define HAVE_EMBEDDED_TIME_MS
diff --git a/Firmware/ESP32/sdkconfig b/Firmware/ESP32/sdkconfig
index 014fb2f..ed156d5 100644
--- a/Firmware/ESP32/sdkconfig
+++ b/Firmware/ESP32/sdkconfig
@@ -1381,17 +1381,20 @@ CONFIG_HEAP_TRACING_OFF=y
 #
 # Log output
 #
-# CONFIG_LOG_DEFAULT_LEVEL_NONE is not set
+CONFIG_LOG_DEFAULT_LEVEL_NONE=y
 # CONFIG_LOG_DEFAULT_LEVEL_ERROR is not set
 # CONFIG_LOG_DEFAULT_LEVEL_WARN is not set
-CONFIG_LOG_DEFAULT_LEVEL_INFO=y
+# CONFIG_LOG_DEFAULT_LEVEL_INFO is not set
 # CONFIG_LOG_DEFAULT_LEVEL_DEBUG is not set
 # CONFIG_LOG_DEFAULT_LEVEL_VERBOSE is not set
-CONFIG_LOG_DEFAULT_LEVEL=3
+CONFIG_LOG_DEFAULT_LEVEL=0
 CONFIG_LOG_MAXIMUM_EQUALS_DEFAULT=y
+# CONFIG_LOG_MAXIMUM_LEVEL_ERROR is not set
+# CONFIG_LOG_MAXIMUM_LEVEL_WARN is not set
+# CONFIG_LOG_MAXIMUM_LEVEL_INFO is not set
 # CONFIG_LOG_MAXIMUM_LEVEL_DEBUG is not set
 # CONFIG_LOG_MAXIMUM_LEVEL_VERBOSE is not set
-CONFIG_LOG_MAXIMUM_LEVEL=3
+CONFIG_LOG_MAXIMUM_LEVEL=0
 CONFIG_LOG_COLORS=y
 CONFIG_LOG_TIMESTAMP_SOURCE_RTOS=y
 # CONFIG_LOG_TIMESTAMP_SOURCE_SYSTEM is not set
@@ -1980,11 +1983,11 @@ CONFIG_BLUEPAD32_PLATFORM_CUSTOM=y
 # CONFIG_BLUEPAD32_PLATFORM_MAKEFILE is not set
 CONFIG_BLUEPAD32_MAX_DEVICES=1
 CONFIG_BLUEPAD32_GAP_SECURITY=y
-# CONFIG_BLUEPAD32_LOG_LEVEL_NONE is not set
+CONFIG_BLUEPAD32_LOG_LEVEL_NONE=y
 # CONFIG_BLUEPAD32_LOG_LEVEL_ERROR is not set
-CONFIG_BLUEPAD32_LOG_LEVEL_INFO=y
+# CONFIG_BLUEPAD32_LOG_LEVEL_INFO is not set
 # CONFIG_BLUEPAD32_LOG_LEVEL_DEBUG is not set
-CONFIG_BLUEPAD32_LOG_LEVEL=2
+CONFIG_BLUEPAD32_LOG_LEVEL=0
 # CONFIG_BLUEPAD32_USB_CONSOLE_ENABLE is not set
 CONFIG_BLUEPAD32_ENABLE_BLE_BY_DEFAULT=y
 CONFIG_BLUEPAD32_MAX_ALLOWLIST=4
diff --git a/Firmware/RP2040/.vscode/settings.json b/Firmware/RP2040/.vscode/settings.json
index 26c31ef..a3decc0 100644
--- a/Firmware/RP2040/.vscode/settings.json
+++ b/Firmware/RP2040/.vscode/settings.json
@@ -2,7 +2,7 @@
     "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools",
 
     "cmake.configureArgs": [
-        "-DOGXM_BOARD=PI_PICOW",
+        "-DOGXM_BOARD=ADA_FEATHER",
         "-DMAX_GAMEPADS=1"
     ],
     
diff --git a/Firmware/RP2040/CMakeLists.txt b/Firmware/RP2040/CMakeLists.txt
index 43f46d9..0b156ad 100644
--- a/Firmware/RP2040/CMakeLists.txt
+++ b/Firmware/RP2040/CMakeLists.txt
@@ -18,15 +18,19 @@ include(${CMAKE_CURRENT_LIST_DIR}/../cmake/patch_libs.cmake)
 include(${CMAKE_CURRENT_LIST_DIR}/../cmake/generate_gatt_header.cmake)
 include(${CMAKE_CURRENT_LIST_DIR}/cmake/get_pico_sdk.cmake)
 
-get_pico_sdk(${EXTERNAL_DIR} ${PICOSDK_VERSION_TAG})
-init_git_submodules(${EXTERNAL_DIR})
-apply_lib_patches(${EXTERNAL_DIR})
-
 set(PICO_PIO_USB_PATH ${EXTERNAL_DIR}/Pico-PIO-USB)
 set(PICO_TINYUSB_PATH ${EXTERNAL_DIR}/tinyusb)
 set(BLUEPAD32_ROOT ${EXTERNAL_DIR}/bluepad32)
 set(BTSTACK_ROOT ${BLUEPAD32_ROOT}/external/btstack)
 set(PICO_BTSTACK_PATH ${BTSTACK_ROOT})
+set(LIBFIXMATH_PATH ${EXTERNAL_DIR}/libfixmath)
+
+get_pico_sdk(${EXTERNAL_DIR} ${PICOSDK_VERSION_TAG})
+init_git_submodules(${EXTERNAL_DIR}
+    ${BLUEPAD32_ROOT}
+    ${PICO_TINYUSB_PATH}
+)
+apply_lib_patches(${EXTERNAL_DIR})
 
 set(SOURCES_BOARD 
     ${SRC}/main.cpp
@@ -48,6 +52,8 @@ set(SOURCES_BOARD
     
     ${SRC}/UserSettings/UserSettings.cpp
     ${SRC}/UserSettings/UserProfile.cpp
+    ${SRC}/UserSettings/JoystickSettings.cpp
+    ${SRC}/UserSettings/TriggerSettings.cpp
 
     ${SRC}/USBDevice/tud_callbacks.cpp
     ${SRC}/USBDevice/DeviceManager.cpp
@@ -77,22 +83,21 @@ set(LIBS_BOARD
     # UART
     hardware_uart
     hardware_irq
+    #fix16
+    libfixmath
 )
 set(INC_DIRS_BOARD
 )
 
 # Config options
 
-# Max gamepads
 set(MAX_GAMEPADS 1 CACHE STRING "Set number of gamepads, 1 to 4")
 if (MAX_GAMEPADS GREATER 4 OR MAX_GAMEPADS LESS 1)
     message(FATAL_ERROR "MAX_GAMEPADS must be between 1 and 4")
 endif()
 add_definitions(-DMAX_GAMEPADS=${MAX_GAMEPADS})
 
-# Board type
 set(OGXM_BOARD "PI_PICO" CACHE STRING "Set board type, options can be found in src/board_config.h")
-
 set(FLASH_SIZE_MB 2)
 set(PICO_BOARD none)
 
@@ -136,6 +141,11 @@ elseif(OGXM_BOARD STREQUAL "PICO_ESP32")
     set(EN_ESP32 TRUE)
     set(EN_UART_BRIDGE TRUE)
 
+    if(OGXM_RETAIL STREQUAL "TRUE")
+        message(STATUS "Retail mode enabled.")
+        add_definitions(-DOGXM_ESP32_RETAIL)
+    endif()
+
 else()
     message(FATAL_ERROR "Invalid OGXM_BOARD value. See options in src/board_config.h")
 
@@ -246,6 +256,8 @@ if(EN_UART_BRIDGE)
     )
 endif()
 
+string(TIMESTAMP CURRENT_DATETIME "%Y-%m-%d %H:%M:%S")
+add_compile_definitions(BUILD_DATETIME="${CURRENT_DATETIME}")
 add_compile_definitions(FIRMWARE_NAME="${FW_NAME}")
 add_compile_definitions(FIRMWARE_VERSION="${FW_VERSION}")
 add_compile_definitions(PICO_FLASH_SIZE_BYTES=${FLASH_SIZE_MB}*1024*1024)
@@ -288,8 +300,8 @@ if(CMAKE_BUILD_TYPE STREQUAL "Debug")
     message(STATUS "Debug build enabled.")
     set(BUILD_STR "-Debug")
 
-    set(TX_PIN 0)
-    set(RX_PIN 1)
+    set(TX_PIN 4)
+    set(RX_PIN 5)
     set(UART_PORT)
 
     include(${CMAKE_CURRENT_LIST_DIR}/cmake/get_pico_uart_port.cmake)
@@ -304,7 +316,7 @@ if(CMAKE_BUILD_TYPE STREQUAL "Debug")
         PICO_DEFAULT_UART_RX_PIN=${RX_PIN}
     )
  
-    add_compile_definitions(CFG_TUSB_DEBUG=2)
+    add_compile_definitions(CFG_TUSB_DEBUG=1)
     add_compile_definitions(OGXM_DEBUG=1)
 
     target_compile_options(${FW_NAME} PRIVATE
@@ -338,7 +350,9 @@ endif()
 pico_set_program_name(${FW_NAME} ${FW_NAME})
 pico_set_program_version(${FW_NAME} ${FW_VERSION})
 
-target_include_directories(${FW_NAME} PRIVATE ${SRC})
+target_include_directories(${FW_NAME} PRIVATE 
+    ${SRC}
+)
 
 if(EN_RGB)
     pico_generate_pio_header(${FW_NAME} ${SRC}/Board/Pico_WS2812/WS2812.pio)
@@ -354,9 +368,26 @@ if(EN_BLUETOOTH)
     add_subdirectory(${BLUEPAD32_ROOT}/src/components/bluepad32 libbluepad32)
 endif()
 
+add_subdirectory(${LIBFIXMATH_PATH} libfixmath)
+
 target_link_libraries(${FW_NAME} PRIVATE ${LIBS_BOARD})
 
-set(EXE_FILENAME "${FW_NAME}-${FW_VERSION}-${OGXM_BOARD}${BUILD_STR}")
+target_compile_definitions(libfixmath PRIVATE
+    FIXMATH_FAST_SIN
+    FIXMATH_NO_64BIT
+    FIXMATH_NO_CACHE
+    FIXMATH_NO_HARD_DIVISION
+    FIXMATH_NO_OVERFLOW
+    # FIXMATH_NO_ROUNDING
+    # FIXMATH_OPTIMIZE_8BIT
+)
+
+if(OGXM_RETAIL STREQUAL "TRUE")
+    set(EXE_FILENAME "${FW_NAME}-${FW_VERSION}-${OGXM_BOARD}${BUILD_STR}-Retail")
+else()
+    set(EXE_FILENAME "${FW_NAME}-${FW_VERSION}-${OGXM_BOARD}${BUILD_STR}")
+endif()
+
 set_target_properties(${FW_NAME} PROPERTIES OUTPUT_NAME ${EXE_FILENAME})
 
 pico_add_extra_outputs(${FW_NAME})
\ No newline at end of file
diff --git a/Firmware/RP2040/src/BLEServer/BLEServer.cpp b/Firmware/RP2040/src/BLEServer/BLEServer.cpp
index ab85c65..881858b 100644
--- a/Firmware/RP2040/src/BLEServer/BLEServer.cpp
+++ b/Firmware/RP2040/src/BLEServer/BLEServer.cpp
@@ -12,20 +12,20 @@
 
 namespace BLEServer {
 
-static constexpr uint16_t PACKET_LEN_MAX = 18;
+static constexpr uint16_t PACKET_LEN_MAX = 20;
 
 namespace Handle
 {
     static constexpr uint16_t FW_VERSION    = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789020_01_VALUE_HANDLE;
     static constexpr uint16_t FW_NAME       = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789021_01_VALUE_HANDLE;
 
-    static constexpr uint16_t START_UPDATE  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789030_01_VALUE_HANDLE;
-    static constexpr uint16_t COMMIT_UPDATE = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789031_01_VALUE_HANDLE;
+    static constexpr uint16_t SETUP_READ  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789030_01_VALUE_HANDLE;
+    static constexpr uint16_t SETUP_WRITE = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789031_01_VALUE_HANDLE;
+    static constexpr uint16_t GET_SETUP   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789032_01_VALUE_HANDLE;
 
-    static constexpr uint16_t SETUP_PACKET  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789040_01_VALUE_HANDLE;
-    static constexpr uint16_t PROFILE_PT1   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789041_01_VALUE_HANDLE;
-    static constexpr uint16_t PROFILE_PT2   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789042_01_VALUE_HANDLE;
-    static constexpr uint16_t PROFILE_PT3   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789043_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789040_01_VALUE_HANDLE;
+
+    static constexpr uint16_t GAMEPAD  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789050_01_VALUE_HANDLE;
 }
 
 namespace ADV
@@ -58,26 +58,159 @@ namespace ADV
 #pragma pack(push, 1)
 struct SetupPacket
 {
-    uint8_t max_gamepads{1};
-    uint8_t index{0};
-    uint8_t device_type{0};
-    uint8_t profile_id{1};
+    DeviceDriverType device_type{DeviceDriverType::NONE};
+    uint8_t max_gamepads{MAX_GAMEPADS};
+    uint8_t player_idx{0};
+    uint8_t profile_id{0};
 };
 static_assert(sizeof(SetupPacket) == 4, "BLEServer::SetupPacket struct size mismatch");
 #pragma pack(pop)
 
-SetupPacket setup_packet_;
+class ProfileReader
+{
+public:
+    ProfileReader() = default;
+    ~ProfileReader() = default;
 
-static int verify_write(const uint16_t buffer_size, const uint16_t expected_size, bool pending_write = false, bool expected_pending_write = false)
+    void set_setup_packet(const SetupPacket& setup_packet)
+    {
+        setup_packet_ = setup_packet;
+        current_offset_ = 0;
+    }
+    const SetupPacket& get_setup_packet() const
+    {
+        return setup_packet_;
+    }
+    uint16_t get_xfer_len()
+    {
+        return static_cast<uint16_t>(std::min(static_cast<size_t>(PACKET_LEN_MAX), sizeof(UserProfile) - current_offset_));
+    }
+    uint16_t get_profile_data(uint8_t* buffer, uint16_t buffer_len)
+    {
+        size_t copy_len = get_xfer_len();
+        if (!buffer || buffer_len < copy_len)
+        {
+            return 0;
+        }
+
+        if (current_offset_ == 0 && !set_profile())
+        {
+            return 0;
+        }
+
+        std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile_) + current_offset_, copy_len);
+
+        current_offset_ += copy_len;
+        if (current_offset_ >= sizeof(UserProfile))
+        {
+            current_offset_ = 0;
+        }
+        return copy_len;
+    }
+
+private:
+    SetupPacket setup_packet_;
+    UserProfile profile_;
+    size_t current_offset_ = 0;
+
+    bool set_profile()
+    {
+        if (setup_packet_.profile_id == 0xFF)
+        {
+            if (setup_packet_.player_idx >= UserSettings::MAX_PROFILES)
+            {
+                return false;
+            }
+            profile_ = UserSettings::get_instance().get_profile_by_index(setup_packet_.player_idx);
+        }
+        else
+        {
+            if (setup_packet_.profile_id > UserSettings::MAX_PROFILES)
+            {
+                return false;
+            }
+            profile_ = UserSettings::get_instance().get_profile_by_id(setup_packet_.profile_id);
+        }
+        return true;
+    }
+};
+
+class ProfileWriter
+{
+public:
+    ProfileWriter() = default;
+    ~ProfileWriter() = default;
+
+    void set_setup_packet(const SetupPacket& setup_packet)
+    {
+        setup_packet_ = setup_packet;
+        current_offset_ = 0;
+    }
+    const SetupPacket& get_setup_packet() const
+    {
+        return setup_packet_;
+    }
+    uint16_t get_xfer_len()
+    {
+        return static_cast<uint16_t>(std::min(static_cast<size_t>(PACKET_LEN_MAX), sizeof(UserProfile) - current_offset_));
+    }
+    size_t set_profile_data(const uint8_t* buffer, uint16_t buffer_len)
+    {
+        size_t copy_len = get_xfer_len();
+        if (!buffer || buffer_len < copy_len)
+        {
+            return 0;
+        }
+
+        std::memcpy(reinterpret_cast<uint8_t*>(&profile_) + current_offset_, buffer, copy_len);
+
+        current_offset_ += copy_len;
+        size_t ret = current_offset_;
+
+        if (current_offset_ >= sizeof(UserProfile))
+        {
+            current_offset_ = 0;
+        }
+        return ret;
+    }
+    bool commit_profile()
+    {
+        bool success = false;
+        if (setup_packet_.device_type != DeviceDriverType::NONE)
+        {
+            success = TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false,
+                [driver_type = setup_packet_.device_type, profile = profile_, index = setup_packet_.player_idx]
+                {
+                    UserSettings::get_instance().store_profile_and_driver_type(driver_type, index, profile);
+                });
+        }
+        else
+        {
+            success = TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false,
+                [index = setup_packet_.player_idx, profile = profile_]
+                {
+                    UserSettings::get_instance().store_profile(index, profile);
+                });
+        }
+        return success;
+    }
+
+private:
+    SetupPacket setup_packet_;
+    UserProfile profile_;
+    size_t current_offset_ = 0;
+};
+
+std::array<Gamepad*, MAX_GAMEPADS> gamepads_;
+ProfileReader profile_reader_;
+ProfileWriter profile_writer_;
+
+static int verify_write(const uint16_t buffer_size, const uint16_t expected_size)
 {
     if (buffer_size != expected_size)
     {
         return ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH;
     }
-    if (pending_write != expected_pending_write)
-    {
-        return ATT_ERROR_WRITE_NOT_PERMITTED;
-    }
     return 0;
 }
 
@@ -107,10 +240,9 @@ static uint16_t att_read_callback(  hci_con_handle_t connection_handle,
                                     uint8_t *buffer,
                                     uint16_t buffer_size)
 {
-    static UserProfile profile;
-    SetupPacket setup_packet_resp;
     std::string fw_version;
     std::string fw_name;
+    Gamepad::PadIn pad_in;
 
     switch (att_handle)
     {
@@ -130,41 +262,30 @@ static uint16_t att_read_callback(  hci_con_handle_t connection_handle,
             }
             return static_cast<uint16_t>(fw_name.size());
 
-        case Handle::SETUP_PACKET:
+        case Handle::GET_SETUP:
             if (buffer)
             {
-                setup_packet_resp.max_gamepads = static_cast<uint8_t>(MAX_GAMEPADS);
-                setup_packet_resp.index = setup_packet_.index;
-                setup_packet_resp.device_type = static_cast<uint8_t>(UserSettings::get_instance().get_current_driver());
-                //App has already written a setup packet with the player index
-                setup_packet_resp.profile_id = UserSettings::get_instance().get_active_profile_id(setup_packet_.index);
-
-                std::memcpy(buffer, &setup_packet_resp, sizeof(setup_packet_resp));
+                buffer[0] = static_cast<uint8_t>(UserSettings::get_instance().get_current_driver());
+                buffer[1] = MAX_GAMEPADS;
+                buffer[2] = 0;
+                buffer[3] = UserSettings::get_instance().get_active_profile_id(0);
             }
-            return sizeof(setup_packet_);
+            return static_cast<uint16_t>(sizeof(SetupPacket));
 
-        case Handle::PROFILE_PT1:
+        case Handle::PROFILE:
             if (buffer)
             {
-                //App has already written the profile id it wants to the setup packet
-                profile = UserSettings::get_instance().get_profile_by_id(setup_packet_.profile_id);
-                std::memcpy(buffer, &profile, PACKET_LEN_MAX);
+                return profile_reader_.get_profile_data(buffer, buffer_size);
             }
-            return PACKET_LEN_MAX;
+            return profile_reader_.get_xfer_len();
 
-        case Handle::PROFILE_PT2:
+        case Handle::GAMEPAD:
             if (buffer)
             {
-                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX, PACKET_LEN_MAX);
+                pad_in = gamepads_.front()->get_pad_in();
+                std::memcpy(buffer, &pad_in, sizeof(Gamepad::PadIn));
             }
-            return PACKET_LEN_MAX;
-
-        case Handle::PROFILE_PT3:
-            if (buffer)
-            {
-                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX * 2, sizeof(UserProfile) - PACKET_LEN_MAX * 2);
-            }
-            return sizeof(UserProfile) - PACKET_LEN_MAX * 2;
+            return static_cast<uint16_t>(sizeof(Gamepad::PadIn));
 
         default:
             break;
@@ -179,98 +300,36 @@ static int att_write_callback(  hci_con_handle_t connection_handle,
                                 uint8_t *buffer,
                                 uint16_t buffer_size)
 {
-    static UserProfile temp_profile;
-    static DeviceDriverType temp_driver_type = DeviceDriverType::NONE;
-    static bool pending_write = false;
-
     int ret = 0;
 
     switch (att_handle)
     {
-        case Handle::START_UPDATE:
-            pending_write = true;
-            break;
-
-        case Handle::SETUP_PACKET:
+        case Handle::SETUP_READ:
             if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0)
             {
                 break;
             }
-
-            std::memcpy(&setup_packet_, buffer, buffer_size);
-            if (setup_packet_.index >= MAX_GAMEPADS)
-            {
-                setup_packet_.index = 0;
-                ret = ATT_ERROR_OUT_OF_RANGE;
-            }
-            if (setup_packet_.profile_id > UserSettings::MAX_PROFILES)
-            {
-                setup_packet_.profile_id = 1;
-                ret = ATT_ERROR_OUT_OF_RANGE;
-            }
-            if (ret)
-            {
-                break;
-            }
-
-            if (pending_write)
-            {
-                //App wants to store a new device driver type
-                temp_driver_type = static_cast<DeviceDriverType>(setup_packet_.device_type);
-            }
+            profile_reader_.set_setup_packet(*reinterpret_cast<SetupPacket*>(buffer));
             break;
 
-        case Handle::PROFILE_PT1:
-            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+        case Handle::SETUP_WRITE:
+            if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0)
             {
                 break;
             }
-            std::memcpy(&temp_profile, buffer, buffer_size);
+            profile_writer_.set_setup_packet(*reinterpret_cast<SetupPacket*>(buffer));
             break;
 
-        case Handle::PROFILE_PT2:
-            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+        case Handle::PROFILE:
+            if ((ret = verify_write(buffer_size, profile_writer_.get_xfer_len())) != 0)
             {
                 break;
             }
-            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX, buffer, buffer_size);
-            break;
-
-        case Handle::PROFILE_PT3:
-            if ((ret = verify_write(buffer_size, sizeof(UserProfile) - PACKET_LEN_MAX * 2, pending_write, true)) != 0)
+            if (profile_writer_.set_profile_data(buffer, buffer_size) == sizeof(UserProfile))
             {
-                break;
+                queue_disconnect(connection_handle, 500);
+                profile_writer_.commit_profile();
             }
-            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX * 2, buffer, buffer_size);
-            break;
-
-        case Handle::COMMIT_UPDATE:
-            if ((ret = verify_write(0, 0, pending_write, true)) != 0)
-            {
-                break;
-            }
-
-            //Delay until after we've returned
-            queue_disconnect(connection_handle, 500);
-
-            //We don't want to write to the flash from here, it'll reset core1 before writing, queue the task on core0
-            if (temp_driver_type != DeviceDriverType::NONE && temp_driver_type != UserSettings::get_instance().get_current_driver())
-            {
-                TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false,
-                []{
-                    UserSettings::get_instance().store_profile_and_driver_type(temp_driver_type, setup_packet_.index, temp_profile);
-                });
-            }
-            else
-            {
-                TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false,
-                []{
-                    UserSettings::get_instance().store_profile(setup_packet_.index, temp_profile);
-                });
-            }
-            
-            temp_driver_type = DeviceDriverType::NONE;
-            pending_write = false;
             break;
 
         default:
@@ -279,8 +338,13 @@ static int att_write_callback(  hci_con_handle_t connection_handle,
     return ret;
 }
 
-void init_server()
+void init_server(Gamepad(&gamepads)[MAX_GAMEPADS])
 {
+    for (uint8_t i = 0; i < MAX_GAMEPADS; i++)
+    {
+        gamepads_[i] = &gamepads[i];
+    }
+
     UserSettings::get_instance().initialize_flash();
 
     // setup ATT server
diff --git a/Firmware/RP2040/src/BLEServer/BLEServer.h b/Firmware/RP2040/src/BLEServer/BLEServer.h
index d72962e..567b6f9 100644
--- a/Firmware/RP2040/src/BLEServer/BLEServer.h
+++ b/Firmware/RP2040/src/BLEServer/BLEServer.h
@@ -3,9 +3,11 @@
 
 #include <cstdint>
 
+#include "Gamepad/Gamepad.h"
+
 namespace BLEServer 
 {
-    void init_server();
+    void init_server(Gamepad(&gamepads)[MAX_GAMEPADS]);
 }
 
 #endif // BLE_SERVER_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/BLEServer/att_delayed_response.gatt b/Firmware/RP2040/src/BLEServer/att_delayed_response.gatt
index 4f059d3..035138b 100644
--- a/Firmware/RP2040/src/BLEServer/att_delayed_response.gatt
+++ b/Firmware/RP2040/src/BLEServer/att_delayed_response.gatt
@@ -13,20 +13,17 @@ CHARACTERISTIC,  12345678-1234-1234-1234-123456789020, READ | DYNAMIC,
 // Handle::FW_NAME
 CHARACTERISTIC,  12345678-1234-1234-1234-123456789021, READ | DYNAMIC,
 
-// Handle::UPDATE_START
+// Handle::SETUP_READ
 CHARACTERISTIC,  12345678-1234-1234-1234-123456789030, WRITE | DYNAMIC,
 
-// Handle::UPDATE_COMMIT
+// Handle::SETUP_WRITE
 CHARACTERISTIC,  12345678-1234-1234-1234-123456789031, WRITE | DYNAMIC,
 
-// Handle::SETUP_PACKET
+// Handle::GET_SETUP
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789032, READ | DYNAMIC,
+
+// Handle::PROFILE
 CHARACTERISTIC,  12345678-1234-1234-1234-123456789040, READ | WRITE | DYNAMIC,
 
-// Handle::PROFILE_PT1
-CHARACTERISTIC,  12345678-1234-1234-1234-123456789041, READ | WRITE | DYNAMIC,
-
-// Handle::PROFILE_PT2
-CHARACTERISTIC,  12345678-1234-1234-1234-123456789042, READ | WRITE | DYNAMIC,
-
-// Handle::PROFILE_PT3
-CHARACTERISTIC,  12345678-1234-1234-1234-123456789043, READ | WRITE | DYNAMIC,
\ No newline at end of file
+// Handle::GAMEPAD
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789050, READ | WRITE | DYNAMIC,
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp b/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
index d0df285..4c9b0a5 100644
--- a/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
+++ b/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
@@ -9,7 +9,6 @@
 
 #include "sdkconfig.h"
 #include "Bluepad32/Bluepad32.h"
-#include "BLEServer/BLEServer.h"
 #include "Board/board_api.h"
 #include "Board/ogxm_log.h"
 
@@ -269,11 +268,9 @@ static void controller_data_cb(uni_hid_device_t* device, uni_controller_t* contr
 
     gp_in.trigger_l = gamepad->scale_trigger_l<10>(static_cast<uint16_t>(uni_gp->brake));
     gp_in.trigger_r = gamepad->scale_trigger_r<10>(static_cast<uint16_t>(uni_gp->throttle));
-
-    gp_in.joystick_lx = gamepad->scale_joystick_lx<10>(uni_gp->axis_x);
-    gp_in.joystick_ly = gamepad->scale_joystick_ly<10>(uni_gp->axis_y);
-    gp_in.joystick_rx = gamepad->scale_joystick_rx<10>(uni_gp->axis_rx);
-    gp_in.joystick_ry = gamepad->scale_joystick_ry<10>(uni_gp->axis_ry);
+    
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad->scale_joystick_l<10>(uni_gp->axis_x, uni_gp->axis_y);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad->scale_joystick_r<10>(uni_gp->axis_rx, uni_gp->axis_ry);
 
     gamepad->set_pad_in(gp_in);
 }
@@ -303,15 +300,13 @@ uni_platform* get_driver()
 
 //Public API
 
-void run_task(Gamepad (&gamepads)[MAX_GAMEPADS])
+void run_task(Gamepad(&gamepads)[MAX_GAMEPADS])
 {
     for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
     {
         bt_devices_[i].gamepad = &gamepads[i];
     }
 
-    BLEServer::init_server();
-
     uni_platform_set_custom(get_driver());
     uni_init(0, nullptr);
 
diff --git a/Firmware/RP2040/src/Bluepad32/Bluepad32.h b/Firmware/RP2040/src/Bluepad32/Bluepad32.h
index 747f190..dfcec61 100644
--- a/Firmware/RP2040/src/Bluepad32/Bluepad32.h
+++ b/Firmware/RP2040/src/Bluepad32/Bluepad32.h
@@ -4,7 +4,7 @@
 #include <cstdint>
 #include <array>
 
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 #include "board_config.h"
 
 /*  NOTE: Everything bluepad32/uni needs to be wrapped
@@ -12,7 +12,7 @@
 
 namespace bluepad32 
 {
-    void run_task(Gamepad (&gamepads)[MAX_GAMEPADS]);
+    void run_task(Gamepad(&gamepads)[MAX_GAMEPADS]);
 } 
 
 #endif // _BLUEPAD_32_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Board/board_api.cpp b/Firmware/RP2040/src/Board/board_api.cpp
index 04f90bf..b711126 100644
--- a/Firmware/RP2040/src/Board/board_api.cpp
+++ b/Firmware/RP2040/src/Board/board_api.cpp
@@ -59,10 +59,11 @@ bool usb::host_connected()
 void usb::disconnect_all()
 {
     OGXM_LOG("Disconnecting USB and resetting Core1\n");
-    tud_disconnect();
-    sleep_ms(300);
+
     multicore_reset_core1();
     sleep_ms(300);
+    tud_disconnect();
+    sleep_ms(300);
 }
 
 // If using PicoW, only use this method from the core running btstack and after you've called init_bluetooth
diff --git a/Firmware/RP2040/src/Gamepad.h b/Firmware/RP2040/src/Gamepad/Gamepad.h
similarity index 50%
rename from Firmware/RP2040/src/Gamepad.h
rename to Firmware/RP2040/src/Gamepad/Gamepad.h
index 4ea0e66..6b6465f 100644
--- a/Firmware/RP2040/src/Gamepad.h
+++ b/Firmware/RP2040/src/Gamepad/Gamepad.h
@@ -6,10 +6,16 @@
 #include <limits>
 #include <cstring>
 #include <array>
+#include <cmath>
 #include <pico/mutex.h>
 
-#include "Range.h"
+#include "libfixmath/fix16.hpp"
+
+#include "Gamepad/Range.h"
+#include "Gamepad/fix16ext.h"
 #include "UserSettings/UserProfile.h"
+#include "UserSettings/JoystickSettings.h"
+#include "UserSettings/TriggerSettings.h"
 #include "Board/ogxm_log.h"
 
 class Gamepad 
@@ -192,9 +198,8 @@ public:
 
     void set_profile(const UserProfile& user_profile) 
     { 
-        set_profile_options(user_profile);
         set_profile_mappings(user_profile);
-        set_profile_deadzones(user_profile);
+        set_profile_settings(user_profile);
     }
 
     inline void set_pad_in(PadIn pad_in)
@@ -243,185 +248,58 @@ public:
         mutex_exit(&chatpad_in_mutex_);
     }
 
-    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
-        <bits> param is optional, used for scaling specific bit values as opposed 
-        to full range values. */
     template <uint8_t bits = 0, typename T>
-    inline int16_t scale_joystick_rx(T value) const
+    inline std::pair<int16_t, int16_t> scale_joystick_r(T x, T y, bool invert_y = false) const
     {
-        int16_t joy_value = 0;
+        int16_t joy_x = 0;
+        int16_t joy_y = 0;
         if constexpr (bits > 0)
         {
-            joy_value = Range::scale_from_bits<int16_t, bits>(value);
+            joy_x = Range::scale_from_bits<int16_t, bits>(x);
+            joy_y = Range::scale_from_bits<int16_t, bits>(y);
         }
         else if constexpr (!std::is_same_v<T, int16_t>)
         {
-            joy_value = Range::scale<int16_t>(value);
+            joy_x = Range::scale<int16_t>(x);
+            joy_y = Range::scale<int16_t>(y);
         }
         else
         {
-            joy_value = value;
+            joy_x = x;
+            joy_y = y;
         }
-        if (joy_value > dz_.joystick_r_pos)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            dz_.joystick_r_pos, 
-                            Range::MAX<int16_t>, 
-                            Range::MID<int16_t>, 
-                            Range::MAX<int16_t>);
-        }
-        else if (joy_value < dz_.joystick_r_neg)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            Range::MIN<int16_t>, 
-                            dz_.joystick_r_neg, 
-                            Range::MIN<int16_t>, 
-                            Range::MID<int16_t>);
-        }
-        else
-        {
-            joy_value = 0;
-        }
-        return joy_value;
+
+        return  joy_settings_r_en_ 
+                    ? apply_joystick_settings(joy_x, joy_y, joy_settings_r_, invert_y) 
+                    : std::make_pair(joy_x, invert_y ? Range::invert(joy_y) : joy_y);
     }
 
-    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
-        <bits> param is optional, used for scaling specific bit values as opposed 
-        to full range values. */
     template <uint8_t bits = 0, typename T>
-    inline int16_t scale_joystick_ry(T value, bool invert = false) const
+    inline std::pair<int16_t, int16_t> scale_joystick_l(T x, T y, bool invert_y = false) const
     {
-        int16_t joy_value = 0;
+        int16_t joy_x = 0;
+        int16_t joy_y = 0;
         if constexpr (bits > 0)
         {
-            joy_value = Range::scale_from_bits<int16_t, bits>(value);
+            joy_x = Range::scale_from_bits<int16_t, bits>(x);
+            joy_y = Range::scale_from_bits<int16_t, bits>(y);
         }
         else if constexpr (!std::is_same_v<T, int16_t>)
         {
-            joy_value = Range::scale<int16_t>(value);
+            joy_x = Range::scale<int16_t>(x);
+            joy_y = Range::scale<int16_t>(y);
         }
         else
         {
-            joy_value = value;
+            joy_x = x;
+            joy_y = y;
         }
-        if (joy_value > dz_.joystick_r_pos)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            dz_.joystick_r_pos, 
-                            Range::MAX<int16_t>, 
-                            Range::MID<int16_t>, 
-                            Range::MAX<int16_t>);
-        }
-        else if (joy_value < dz_.joystick_r_neg)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            Range::MIN<int16_t>, 
-                            dz_.joystick_r_neg, 
-                            Range::MIN<int16_t>, 
-                            Range::MID<int16_t>);
-        }
-        else
-        {
-            joy_value = 0;
-        }
-        return profile_invert_ry_ ? (invert ? joy_value : Range::invert(joy_value)) : (invert ? Range::invert(joy_value) : joy_value);
+
+        return  joy_settings_l_en_ 
+                    ? apply_joystick_settings(joy_x, joy_y, joy_settings_l_, invert_y) 
+                    : std::make_pair(joy_x, invert_y ? Range::invert(joy_y) : joy_y);
     }
 
-    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
-        <bits> param is optional, used for scaling specific bit values as opposed 
-        to full range values. */
-    template <uint8_t bits = 0, typename T>
-    inline int16_t scale_joystick_lx(T value) const
-    {
-        int16_t joy_value = 0;
-        if constexpr (bits > 0)
-        {
-            joy_value = Range::scale_from_bits<int16_t, bits>(value);
-        }
-        else if constexpr (!std::is_same_v<T, int16_t>)
-        {
-            joy_value = Range::scale<int16_t>(value);
-        }
-        else
-        {
-            joy_value = value;
-        }
-        if (joy_value > dz_.joystick_l_pos)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            dz_.joystick_l_pos, 
-                            Range::MAX<int16_t>, 
-                            Range::MID<int16_t>, 
-                            Range::MAX<int16_t>);
-        }
-        else if (joy_value < dz_.joystick_l_neg)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            Range::MIN<int16_t>, 
-                            dz_.joystick_l_neg, 
-                            Range::MIN<int16_t>, 
-                            Range::MID<int16_t>);
-        }
-        else
-        {
-            joy_value = 0;
-        }
-        return joy_value;
-    }
-
-    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
-        <bits> param is optional, used for scaling specific bit values as opposed 
-        to full range values. */
-    template <uint8_t bits = 0, typename T>
-    inline int16_t scale_joystick_ly(T value, bool invert = false) const
-    {
-        int16_t joy_value = 0;
-        if constexpr (bits > 0)
-        {
-            joy_value = Range::scale_from_bits<int16_t, bits>(value);
-        }
-        else if constexpr (!std::is_same_v<T, int16_t>)
-        {
-            joy_value = Range::scale<int16_t>(value);
-        }
-        else
-        {
-            joy_value = value;
-        }
-        if (joy_value > dz_.joystick_l_pos)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            dz_.joystick_l_pos, 
-                            Range::MAX<int16_t>, 
-                            Range::MID<int16_t>, 
-                            Range::MAX<int16_t>);
-        }
-        else if (joy_value < dz_.joystick_l_neg)
-        {
-            joy_value = Range::scale(
-                            joy_value, 
-                            Range::MIN<int16_t>, 
-                            dz_.joystick_l_neg, 
-                            Range::MIN<int16_t>, 
-                            Range::MID<int16_t>);
-        }
-        else
-        {
-            joy_value = 0;
-        }
-        return profile_invert_ly_ ? (invert ? joy_value : Range::invert(joy_value)) : (invert ? Range::invert(joy_value) : joy_value);
-    }
-
-    /*  Get trigger value adjusted for deadzones, scaling, and inversion. 
-        <bits> param is optional, used for scaling speicifc bit values 
-        as opposed to full range values */
     template <uint8_t bits = 0, typename T>
     inline uint8_t scale_trigger_l(T value) const
     {
@@ -438,12 +316,11 @@ public:
         {
             trigger_value = value;
         }
-        return trigger_value > dz_.trigger_l ? Range::scale(trigger_value, dz_.trigger_l, Range::MAX<uint8_t>) : 0;
+        return  trig_settings_l_en_ 
+                    ? apply_trigger_settings(trigger_value, trig_settings_l_) 
+                    : trigger_value;
     }
 
-    /*  Get trigger value adjusted for deadzones, scaling, and inversion. 
-        <bits> param is optional, used for scaling speicifc bit values 
-        as opposed to full range values */
     template <uint8_t bits = 0, typename T>
     inline uint8_t scale_trigger_r(T value) const
     {
@@ -460,17 +337,19 @@ public:
         {
             trigger_value = value;
         }
-        return trigger_value > dz_.trigger_l ? Range::scale(trigger_value, dz_.trigger_l, Range::MAX<uint8_t>) : 0;
+        return  trig_settings_r_en_ 
+                    ? apply_trigger_settings(trigger_value, trig_settings_r_) 
+                    : trigger_value;
     }
 
-private:
+private:    
     mutex_t pad_in_mutex_;
     mutex_t pad_out_mutex_;
     mutex_t chatpad_in_mutex_;
 
     PadOut pad_out_;
     PadIn pad_in_;
-    ChatpadIn chatpad_in_;
+    ChatpadIn chatpad_in_{0};
 
     std::atomic<bool> new_pad_in_{false};
     std::atomic<bool> new_pad_out_{false};
@@ -479,25 +358,53 @@ private:
     std::atomic<bool> analog_host_{false};
     std::atomic<bool> analog_device_{false};
 
-    bool profile_invert_ly_{false};
-    bool profile_invert_ry_{false};
     bool profile_analog_enabled_{false};
 
-    struct Deadzones
-    {
-        uint8_t trigger_l{0};
-        uint8_t trigger_r{0};
-        int16_t joystick_l_neg{0};
-        int16_t joystick_l_pos{0};
-        int16_t joystick_r_neg{0};
-        int16_t joystick_r_pos{0};
-    } dz_;
+    JoystickSettings joy_settings_l_;
+    JoystickSettings joy_settings_r_;
+    TriggerSettings trig_settings_l_;
+    TriggerSettings trig_settings_r_;
 
-    void set_profile_options(const UserProfile& profile)
+    bool joy_settings_l_en_{false};
+    bool joy_settings_r_en_{false};
+    bool trig_settings_l_en_{false};
+    bool trig_settings_r_en_{false};
+
+    void set_profile_settings(const UserProfile& profile)
     {
-        profile_invert_ly_ = profile.invert_ly ? true : false;
-        profile_invert_ry_ = profile.invert_ry ? true : false;
         profile_analog_enabled_ = profile.analog_enabled ? true : false;
+        OGXM_LOG("profile_analog_enabled_: %d\n", profile_analog_enabled_);
+
+        if ((joy_settings_l_en_ = !joy_settings_l_.is_same(profile.joystick_settings_l)))
+        {
+            joy_settings_l_.set_from_raw(profile.joystick_settings_l);
+            //This needs to be addressed in the webapp, just multiply here for now
+            joy_settings_l_.axis_restrict *= static_cast<int16_t>(100);
+            joy_settings_l_.angle_restrict *= static_cast<int16_t>(100);
+            joy_settings_l_.anti_dz_angular *= static_cast<int16_t>(100);
+        }
+        if ((joy_settings_r_en_ = !joy_settings_r_.is_same(profile.joystick_settings_r)))
+        {
+            joy_settings_r_.set_from_raw(profile.joystick_settings_r);
+            //This needs to be addressed in the webapp, just multiply here for now
+            joy_settings_r_.axis_restrict *= static_cast<int16_t>(100);
+            joy_settings_r_.angle_restrict *= static_cast<int16_t>(100);
+            joy_settings_r_.anti_dz_angular *= static_cast<int16_t>(100);
+        }
+        if ((trig_settings_l_en_ = !trig_settings_l_.is_same(profile.trigger_settings_l)))
+        {
+            trig_settings_l_.set_from_raw(profile.trigger_settings_l);
+        }
+        if ((trig_settings_r_en_ = !trig_settings_r_.is_same(profile.trigger_settings_r)))
+        {
+            trig_settings_r_.set_from_raw(profile.trigger_settings_r);
+        }
+
+        OGXM_LOG("GamepadMapper: JoyL: %s, JoyR: %s, TrigL: %s, TrigR: %s\n",
+            joy_settings_l_en_ ? "Enabled" : "Disabled",
+            joy_settings_r_en_ ? "Enabled" : "Disabled",
+            trig_settings_l_en_ ? "Enabled" : "Disabled",
+            trig_settings_r_en_ ? "Enabled" : "Disabled");
     }
 
     void set_profile_mappings(const UserProfile& profile)
@@ -537,27 +444,197 @@ private:
         MAP_ANALOG_OFF_RB    = profile.analog_off_rb;
     }
 
-    void set_profile_deadzones(const UserProfile& profile) //Deadzones in the profile are 0-255 (0-100%)
+    static inline std::pair<int16_t, int16_t> apply_joystick_settings(
+        int16_t gp_joy_x, 
+        int16_t gp_joy_y, 
+        const JoystickSettings& set,
+        bool invert_y)
     {
-        dz_.trigger_l = profile.dz_trigger_l;
-        dz_.trigger_r = profile.dz_trigger_r;
+        static const Fix16 
+            FIX_0(0.0f),
+            FIX_1(1.0f),
+            FIX_2(2.0f),
+            FIX_45(45.0f),
+            FIX_90(90.0f),
+            FIX_100(100.0f),
+            FIX_180(180.0f),
+            FIX_EPSILON(0.0001f),
+            FIX_EPSILON2(0.001f),
+            FIX_ELLIPSE_DEF(1.570796f),
+            FIX_DIAG_DIVISOR(0.29289f);
 
-        OGXM_LOG("dz_.trigger_l: %d\n", dz_.trigger_l);
-        OGXM_LOG("dz_.trigger_r: %d\n", dz_.trigger_r);
-        OGXM_LOG("profile.dz_joystick_l: %d\n", profile.dz_joystick_l);
-        OGXM_LOG("profile.dz_joystick_r: %d\n", profile.dz_joystick_r);
+        Fix16 x = (set.invert_x ? Fix16(Range::invert(gp_joy_x)) : Fix16(gp_joy_x)) / Range::MAX<int16_t>;
+        Fix16 y = ((set.invert_y ^ invert_y) ? Fix16(Range::invert(gp_joy_y)) : Fix16(gp_joy_y)) / Range::MAX<int16_t>;
 
-        dz_.joystick_l_pos = profile.dz_joystick_l ? Range::scale<int16_t>(static_cast<int8_t>(profile.dz_joystick_l / 2)) : 0;
-        dz_.joystick_l_neg = Range::invert(dz_.joystick_l_pos);
-        dz_.joystick_r_pos = profile.dz_joystick_r ? Range::scale<int16_t>(static_cast<int8_t>(profile.dz_joystick_r / 2)) : 0;
-        dz_.joystick_r_neg = Range::invert(dz_.joystick_r_pos);
+        const Fix16 abs_x = fix16::abs(x);
+        const Fix16 abs_y = fix16::abs(y);
+        const Fix16 inv_axis_restrict = FIX_1 / (FIX_1 - set.axis_restrict);
 
-        OGXM_LOG("dz_.trigger_l: %d\n", dz_.trigger_l);
-        OGXM_LOG("dz_.trigger_r: %d\n", dz_.trigger_r);
-        OGXM_LOG("dz_.joystick_l_pos: %d\n", dz_.joystick_l_pos);
-        OGXM_LOG("dz_.joystick_l_neg: %d\n", dz_.joystick_l_neg);
-        OGXM_LOG("dz_.joystick_r_pos: %d\n", dz_.joystick_r_pos);
-        OGXM_LOG("dz_.joystick_r_neg: %d\n", dz_.joystick_r_neg);
+        Fix16 rAngle = (abs_x < FIX_EPSILON) 
+            ? FIX_90 
+            : fix16::rad2deg(fix16::abs(fix16::atan(y / x)));
+
+        Fix16 axial_x = (abs_x <= set.axis_restrict && rAngle > FIX_45) 
+            ? FIX_0 
+            : ((abs_x - set.axis_restrict) * inv_axis_restrict);
+                
+        Fix16 axial_y = (abs_y <= set.axis_restrict && rAngle <= FIX_45) 
+            ? FIX_0 
+            : ((abs_y - set.axis_restrict) * inv_axis_restrict);
+
+        Fix16 in_magnitude = fix16::sqrt(fix16::sq(axial_x) + fix16::sq(axial_y));
+
+        if (in_magnitude < set.dz_inner)
+        {
+            return { 0, 0 };
+        }
+
+        Fix16 angle = 
+            fix16::abs(axial_x) < FIX_EPSILON 
+                ? FIX_90 
+                : fix16::rad2deg(fix16::abs(fix16::atan(axial_y / axial_x)));
+
+        Fix16 anti_r_scale = (set.anti_dz_square_y_scale == FIX_0) ? set.anti_dz_square : set.anti_dz_square_y_scale;
+        Fix16 anti_dz_c = set.anti_dz_circle;
+
+        if (anti_r_scale > FIX_0 && anti_dz_c > FIX_0)
+        {
+            Fix16 anti_ellip_scale = anti_ellip_scale / anti_dz_c;
+            Fix16 ellipse_angle = fix16::atan((FIX_1 / anti_ellip_scale) * fix16::tan(fix16::rad2deg(rAngle)));
+            ellipse_angle = (ellipse_angle < FIX_0) ? FIX_ELLIPSE_DEF : ellipse_angle;
+
+            Fix16 ellipse_x = fix16::cos(ellipse_angle);
+            Fix16 ellipse_y = fix16::sqrt(fix16::sq(anti_ellip_scale) * (FIX_1 - fix16::sq(ellipse_x)));
+            anti_dz_c *= fix16::sqrt(fix16::sq(ellipse_x) + fix16::sq(ellipse_y));
+        }
+
+        if (anti_dz_c > FIX_0)
+        {
+            anti_dz_c = anti_dz_c / ((anti_dz_c * (FIX_1 - set.anti_dz_circle / set.dz_outer)) / (anti_dz_c * (FIX_1 - set.anti_dz_square)));
+        }
+
+        if (abs_x > set.axis_restrict && abs_y > set.axis_restrict)
+        {
+            const Fix16 FIX_ANGLE_MAX = set.angle_restrict / 2.0f;
+
+            if (angle > FIX_0 && angle < FIX_ANGLE_MAX)
+            {
+                angle = FIX_0;
+            }
+            if (angle > (FIX_90 - FIX_ANGLE_MAX))
+            {
+                angle = FIX_90;
+            }
+            if (angle > FIX_ANGLE_MAX && angle < (FIX_90 - FIX_ANGLE_MAX))
+            {
+                angle = ((angle - FIX_ANGLE_MAX) * FIX_90) / ((FIX_90 - FIX_ANGLE_MAX) - FIX_ANGLE_MAX);
+            }
+        }
+
+        Fix16 ref_angle = (angle < FIX_EPSILON2) ? FIX_0 : angle;
+        Fix16 diagonal = (angle > FIX_45) ? (((angle - FIX_45) * (-FIX_45)) / FIX_45) + FIX_45 : angle;
+
+        const Fix16 angle_comp = set.angle_restrict / FIX_2;
+
+        if (angle < FIX_90 && angle > FIX_0)
+        {
+            angle = ((angle * ((FIX_90 - angle_comp) - angle_comp)) / FIX_90) + angle_comp;
+        }
+
+        if (axial_x < FIX_0 && axial_y > FIX_0)
+        {
+            angle = -angle;
+        }
+        if (axial_x > FIX_0 && axial_y < FIX_0)
+        {
+            angle = angle - FIX_180;
+        }
+        if (axial_x < FIX_0 && axial_y < FIX_0)
+        {
+            angle = angle + FIX_180;
+        }
+
+        //Deadzone Warp
+        Fix16 out_magnitude = (in_magnitude - set.dz_inner) / (set.anti_dz_outer - set.dz_inner);
+        out_magnitude = fix16::pow(out_magnitude, (FIX_1 / set.curve)) * (set.dz_outer - anti_dz_c) + anti_dz_c;
+        out_magnitude = (out_magnitude > set.dz_outer && !set.uncap_radius) ? set.dz_outer : out_magnitude;
+
+		Fix16 d_scale = (((out_magnitude - anti_dz_c) * (set.diag_scale_max - set.diag_scale_min)) / (set.dz_outer - anti_dz_c)) + set.diag_scale_min;		
+		Fix16 c_scale = (diagonal * (FIX_1 / fix16::sqrt(FIX_2))) / FIX_45; //Both these lines scale the intensity of the warping
+		c_scale       = FIX_1 - fix16::sqrt(FIX_1 - c_scale * c_scale);     //based on a circular curve to the perfect diagonal
+		d_scale       = (c_scale * (d_scale - FIX_1)) / FIX_DIAG_DIVISOR + FIX_1;
+
+		out_magnitude = out_magnitude * d_scale;
+
+		//Scaling values for square antideadzone
+		Fix16 new_x = fix16::cos(fix16::deg2rad(angle)) * out_magnitude;
+		Fix16 new_y = fix16::sin(fix16::deg2rad(angle)) * out_magnitude;
+		
+		//Magic angle wobble fix by user ME.
+		// if (angle > 45.0 && angle < 225.0) {
+		// 	newX = inv(Math.sin(deg2rad(angle - 90.0)))*outputMagnitude;
+		// 	newY = inv(Math.cos(deg2rad(angle - 270.0)))*outputMagnitude;
+		// }
+		
+		//Square antideadzone scaling
+		Fix16 output_x = fix16::abs(new_x) * (FIX_1 - set.anti_dz_square / set.dz_outer) + set.anti_dz_square;
+		if (x < FIX_0)
+        {
+            output_x = -output_x;
+        }
+		if (ref_angle == FIX_90)
+        {
+            output_x = FIX_0;
+        }
+		
+		Fix16 output_y = fix16::abs(new_y) * (FIX_1 - anti_r_scale / set.dz_outer) + anti_r_scale;
+		if (y < FIX_0)
+        {
+            output_y = -output_y;
+        }
+		if (ref_angle == FIX_0)
+        {
+            output_y = FIX_0;
+        }
+
+        output_x = fix16::clamp(output_x, -FIX_1, FIX_1) * Range::MAX<int16_t>;
+        output_y = fix16::clamp(output_y, -FIX_1, FIX_1) * Range::MAX<int16_t>;
+
+        return { static_cast<int16_t>(fix16_to_int(output_x)), static_cast<int16_t>(fix16_to_int(output_y)) };
+    }
+
+    uint8_t apply_trigger_settings(uint8_t value, const TriggerSettings& set) const
+    {
+        Fix16 abs_value = fix16::abs(Fix16(static_cast<int16_t>(value)) / static_cast<int16_t>(Range::MAX<uint8_t>));
+
+        if (abs_value < set.dz_inner)
+        {
+            return 0;
+        }
+
+        static const Fix16 
+            FIX_0(0.0f),
+            FIX_1(1.0f),
+            FIX_2(2.0f);
+
+        Fix16 value_out = (abs_value - set.dz_inner) / (set.anti_dz_outer - set.dz_inner);
+        value_out = fix16::clamp(value_out, FIX_0, FIX_1);
+
+        if (set.anti_dz_inner > FIX_0)
+        {
+            value_out = set.anti_dz_inner + (FIX_1 - set.anti_dz_inner) * value_out;
+        }
+        if (set.curve != FIX_1)
+        {
+            value_out = fix16::pow(value_out, FIX_1 / set.curve);
+        }
+        if (set.anti_dz_outer < FIX_1)
+        {
+            value_out = fix16::clamp(value_out * (FIX_1 / (FIX_1 - set.anti_dz_outer)), FIX_0, FIX_1);
+        }
+
+        value_out *= set.dz_outer;
+        return static_cast<uint8_t>(fix16_to_int(value_out * static_cast<int16_t>(Range::MAX<uint8_t>)));
     }
 };
 
diff --git a/Firmware/ESP32/main/Range.h b/Firmware/RP2040/src/Gamepad/Range.h
similarity index 57%
rename from Firmware/ESP32/main/Range.h
rename to Firmware/RP2040/src/Gamepad/Range.h
index ae08dbb..667bc61 100644
--- a/Firmware/ESP32/main/Range.h
+++ b/Firmware/RP2040/src/Gamepad/Range.h
@@ -4,6 +4,7 @@
 #include <cstdint>
 #include <limits>
 #include <type_traits>
+#include "Board/ogxm_log.h"
 
 namespace Range {
 
@@ -78,77 +79,31 @@ namespace Range {
     requires std::is_integral_v<To> && std::is_integral_v<From>
     static inline To clamp(From value) 
     {
-        if constexpr (std::is_signed_v<From> != std::is_signed_v<To>)
-        {
-            using CommonType = std::common_type_t<To, From>;
-            return static_cast<To>((static_cast<CommonType>(value) < static_cast<CommonType>(Range::MIN<To>)) 
-                                    ? Range::MIN<To> 
-                                    : (static_cast<CommonType>(value) > static_cast<CommonType>(Range::MAX<To>)) 
-                                        ? Range::MAX<To> 
-                                        : static_cast<CommonType>(value));
-        }
-        else
-        {
-            return static_cast<To>((value < Range::MIN<To>) 
-                                    ? Range::MIN<To> 
-                                    : (value > Range::MAX<To>) 
-                                        ? Range::MAX<To> 
-                                        : value);
-        }
+        return static_cast<To>((value < Range::MIN<To>) 
+                                ? Range::MIN<To> 
+                                : (value > Range::MAX<To>) 
+                                    ? Range::MAX<To> 
+                                    : value);
     }
 
     template <typename T>
-    requires std::is_integral_v<T>
     static inline T clamp(T value, T min, T max) 
     {
         return (value < min) ? min : (value > max) ? max : value;
     }
 
+    template <typename To, typename From> 
+    static inline To clamp(From value, To min_to, To max_to) 
+    {
+        return (value < min_to) ? min_to : (value > max_to) ? max_to : static_cast<To>(value);
+    }
+
     template <typename To, typename From>
     requires std::is_integral_v<To> && std::is_integral_v<From>
-    static inline To scale(From value, From min_from, From max_from, To min_to, To max_to) 
+    static constexpr To scale(From value, From min_from, From max_from, To min_to, To max_to) 
     {
-        if constexpr (std::is_unsigned_v<From> && std::is_unsigned_v<To>) 
-        {
-            // Both unsigned
-            uint64_t scaled = static_cast<uint64_t>(value - min_from) * 
-                            (max_to - min_to) / 
-                            (max_from - min_from) + min_to;
-            return static_cast<To>(scaled);
-        } 
-        else if constexpr (std::is_signed_v<From> && std::is_unsigned_v<To>) 
-        {
-            // From signed, To unsigned
-            uint64_t shift_from = static_cast<uint64_t>(-min_from);
-            uint64_t u_value = static_cast<uint64_t>(value) + shift_from;
-            uint64_t u_min_from = static_cast<uint64_t>(min_from) + shift_from;
-            uint64_t u_max_from = static_cast<uint64_t>(max_from) + shift_from;
-
-            uint64_t scaled = (u_value - u_min_from) * 
-                            (max_to - min_to) / 
-                            (u_max_from - u_min_from) + min_to;
-            return static_cast<To>(scaled);
-        } 
-        else if constexpr (std::is_unsigned_v<From> && std::is_signed_v<To>) 
-        {
-            // From unsigned, To signed
-            uint64_t shift_to = static_cast<uint64_t>(-min_to);
-            uint64_t scaled = static_cast<uint64_t>(value - min_from) * 
-                            (static_cast<uint64_t>(max_to) + shift_to - static_cast<uint64_t>(min_to) - shift_to) / 
-                            (max_from - min_from) + static_cast<uint64_t>(min_to) + shift_to;
-            return static_cast<To>(scaled - shift_to);
-        } 
-        else 
-        {
-            // Both signed
-            int64_t shift_from = -min_from;
-            int64_t shift_to = -min_to;
-
-            int64_t scaled = (static_cast<int64_t>(value) + shift_from - (min_from + shift_from)) * 
-                            (max_to + shift_to - (min_to + shift_to)) / 
-                            (max_from - min_from) + (min_to + shift_to);
-            return static_cast<To>(scaled - shift_to);
-        }
+        return static_cast<To>(
+            (static_cast<int64_t>(value - min_from) * (max_to - min_to) / (max_from - min_from)) + min_to);
     }
 
     template <typename To, typename From>
@@ -189,4 +144,61 @@ namespace Range {
 
 } // namespace Range
 
+namespace Scale //Scale and invert values
+{
+    static inline uint8_t int16_to_uint8(int16_t value)
+    {
+        uint16_t shifted_value = static_cast<uint16_t>(value + Range::MID<uint16_t>);
+        return static_cast<uint8_t>(shifted_value >> 8);
+    }
+    static inline uint16_t int16_to_uint16(int16_t value)
+    {
+        return static_cast<uint16_t>(value + Range::MID<uint16_t>);
+    }
+    static inline int8_t int16_to_int8(int16_t value)
+    {
+        return static_cast<int8_t>((value + Range::MID<uint16_t>) >> 8);
+    }
+
+    static inline uint8_t uint16_to_uint8(uint16_t value)
+    {
+        return static_cast<uint8_t>(value >> 8);
+    }
+    static inline int16_t uint16_to_int16(uint16_t value)
+    {
+        return static_cast<int16_t>(value - Range::MID<uint16_t>);
+    }
+    static inline int8_t uint16_to_int8(uint16_t value)
+    {
+        return static_cast<int8_t>((value >> 8) - Range::MID<uint8_t>);
+    }
+
+    static inline int16_t uint8_to_int16(uint8_t value)
+    {
+        return static_cast<int16_t>((static_cast<int32_t>(value) << 8) - Range::MID<uint16_t>);
+    }
+    static inline uint16_t uint8_to_uint16(uint8_t value)
+    {
+        return static_cast<uint16_t>(value) << 8;
+    }
+    static inline int8_t uint8_to_int8(uint8_t value)
+    {
+        return static_cast<int8_t>(value - Range::MID<uint8_t>);
+    }
+
+    static inline int16_t int8_to_int16(int8_t value)
+    {
+        return static_cast<int16_t>(value) << 8;
+    }
+    static inline uint16_t int8_to_uint16(int8_t value)
+    {
+        return static_cast<uint16_t>((value + Range::MID<uint8_t>) << 8);
+    }
+    static inline uint8_t int8_to_uint8(int8_t value)
+    {
+        return static_cast<uint8_t>(value + Range::MID<uint8_t>);
+    }
+
+} // namespace Scale
+
 #endif // _RANGE_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Gamepad/fix16ext.h b/Firmware/RP2040/src/Gamepad/fix16ext.h
new file mode 100644
index 0000000..b59b027
--- /dev/null
+++ b/Firmware/RP2040/src/Gamepad/fix16ext.h
@@ -0,0 +1,106 @@
+#ifndef FIX16_EXT_H
+#define FIX16_EXT_H
+
+#include <cstdint>
+
+#include "libfixmath/fix16.hpp"
+
+namespace fix16 {
+
+inline Fix16 abs(Fix16 x)
+{
+    return Fix16(fix16_abs(x.value));
+}
+
+inline Fix16 rad2deg(Fix16 rad)
+{
+    return Fix16(fix16_rad_to_deg(rad.value));
+}
+
+inline Fix16 deg2rad(Fix16 deg)
+{
+    return Fix16(fix16_deg_to_rad(deg.value));
+}
+
+inline Fix16 atan(Fix16 x)
+{
+    return Fix16(fix16_atan(x.value));
+}
+
+inline Fix16 atan2(Fix16 y, Fix16 x)
+{
+    return Fix16(fix16_atan2(y.value, x.value));
+}
+
+inline Fix16 tan(Fix16 x)
+{
+    return Fix16(fix16_tan(x.value));
+}
+
+inline Fix16 cos(Fix16 x)
+{
+    return Fix16(fix16_cos(x.value));
+}
+
+inline Fix16 sin(Fix16 x)
+{
+    return Fix16(fix16_sin(x.value));
+}
+
+inline Fix16 sqrt(Fix16 x)
+{
+    return Fix16(fix16_sqrt(x.value));
+}
+
+inline Fix16 sq(Fix16 x)
+{
+    return Fix16(fix16_sq(x.value));
+}
+
+inline Fix16 clamp(Fix16 x, Fix16 min, Fix16 max)
+{
+    return Fix16(fix16_clamp(x.value, min.value, max.value));
+}
+
+inline Fix16 pow(Fix16 x, Fix16 y)
+{
+    fix16_t& base = x.value;
+    fix16_t& exponent = y.value;
+
+    if (exponent == F16(0.0)) 
+        return Fix16(fix16_from_int(1));
+    if (base == F16(0.0)) 
+        return Fix16(fix16_from_int(0));
+
+    int32_t int_exp = fix16_to_int(exponent);
+
+    if (fix16_from_int(int_exp) == exponent) 
+    {
+        fix16_t result = F16(1.0);
+        fix16_t current_base = base;
+
+        if (int_exp < 0) 
+        {
+            current_base = fix16_div(F16(1.0), base);
+            int_exp = -int_exp;
+        }
+
+        while (int_exp) 
+        {
+            if (int_exp & 1) 
+            {
+                result = fix16_mul(result, current_base);
+            }
+            current_base = fix16_mul(current_base, current_base);
+            int_exp >>= 1;
+        }
+
+        return Fix16(result);
+    }
+
+    return Fix16(fix16_exp(fix16_mul(exponent, fix16_log(base))));
+}
+
+} // namespace fix16
+
+#endif // FIX16_EXT_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h
index e31780a..a06b1a3 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h
@@ -7,7 +7,7 @@
 #include <hardware/gpio.h>
 
 #include "board_config.h"
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 #include "I2CDriver/4Channel/I2CMaster.h"
 #include "I2CDriver/4Channel/I2CSlave.h"
 #include "I2CDriver/4Channel/I2CDriver.h"
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h
index 922fa7a..2825c8d 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h
@@ -7,7 +7,7 @@
 #include <hardware/i2c.h>
 
 #include "board_config.h"
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 #include "I2CDriver/4Channel/I2CDriver.h"
 
 class I2CMaster : public I2CDriver
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h
index 396ba46..19ad2e0 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h
@@ -8,7 +8,7 @@
 #include <pico/i2c_slave.h>
 
 #include "board_config.h"
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 #include "I2CDriver/4Channel/I2CDriver.h"
 
 class I2CSlave : public I2CDriver
diff --git a/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp b/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp
index a4d75c8..edc16db 100644
--- a/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp
+++ b/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp
@@ -4,7 +4,6 @@
 #include <hardware/gpio.h>
 #include <hardware/i2c.h>
 
-#include "Gamepad.h"
 #include "board_config.h"
 #include "Board/board_api.h"
 #include "I2CDriver/ESP32/I2CDriver.h"
@@ -76,8 +75,6 @@ static inline void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
                     break;
 
                 case PacketID::SET_DRIVER:
-                    OGXM_LOG("I2C: Received SET_DRIVER packet: " + OGXM_TO_STRING(packet_in.device_type) + "\n");
-
                     if (packet_in.device_type != DeviceDriverType::NONE &&
                         packet_in.device_type != current_device_type)
                     {
diff --git a/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.h b/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.h
index 61191fa..81a3b2b 100644
--- a/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.h
+++ b/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.h
@@ -3,7 +3,7 @@
 
 #include <cstdint>
 
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 
 namespace I2CDriver
 {
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp
index c1671c8..3510b39 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp
@@ -10,7 +10,7 @@
 #include "Board/board_api.h"
 #include "OGXMini/OGXMini.h"
 #include "I2CDriver/ESP32/I2CDriver.h"
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 #include "TaskQueue/TaskQueue.h"
 
 namespace OGXMini {
@@ -30,14 +30,30 @@ void core1_task()
     }
 }
 
-void run_uart_bridge()
+void run_uart_bridge(UserSettings& user_settings)
 {
     DeviceManager& device_manager = DeviceManager::get_instance();
     device_manager.initialize_driver(DeviceDriverType::UART_BRIDGE, gamepads_);
 
     board_api::esp32::enter_programming_mode();
+
+    OGXM_LOG("Entering UART Bridge mode\n");
     
-    device_manager.get_driver()->process(0, gamepads_[0]); //Runs UART Bridge task, doesn't return
+    device_manager.get_driver()->process(0, gamepads_[0]); //Runs UART Bridge task, doesn't return unless programming is complete
+
+    OGXM_LOG("Exiting UART Bridge mode\n");
+
+    board_api::usb::disconnect_all(); 
+    user_settings.write_datetime();
+    board_api::reboot();
+}
+
+bool update_needed(UserSettings& user_settings)
+{
+#if defined(OGXM_ESP32_RETAIL)
+    return !user_settings.verify_datetime();
+#endif
+    return false;
 }
 
 void run_program()
@@ -48,9 +64,9 @@ void run_program()
     user_settings.initialize_flash();
 
     //MODE_SEL_PIN is used to determine if UART bridge should be run
-    if (board_api::esp32::uart_bridge_mode())
+    if (board_api::esp32::uart_bridge_mode() || update_needed(user_settings))
     {
-        run_uart_bridge();
+        run_uart_bridge(user_settings);
         return;
     }
 
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp
index dd87118..72f13ea 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp
@@ -11,7 +11,8 @@
 #include "Board/board_api.h"
 #include "Bluepad32/Bluepad32.h"
 #include "OGXMini/OGXMini.h"
-#include "Gamepad.h"
+#include "BLEServer/BLEServer.h"
+#include "Gamepad/Gamepad.h"
 #include "TaskQueue/TaskQueue.h"
 
 namespace OGXMini {
@@ -25,6 +26,7 @@ void core1_task()
 {
     board_api::init_bluetooth();
     board_api::set_led(true);
+    BLEServer::init_server(gamepads_);
     bluepad32::run_task(gamepads_);
 }
 
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp
index b1db7f0..7162282 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp
@@ -11,7 +11,7 @@
 #include "USBDevice/DeviceManager.h"
 #include "OGXMini/OGXMini.h"
 #include "TaskQueue/TaskQueue.h"
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 #include "Board/board_api.h"
 #include "Board/ogxm_log.h"
 
@@ -78,7 +78,6 @@ void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
 {
     TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
     {
-        OGXM_LOG("Checking for driver change.\n");
         //Check gamepad inputs for button combo to change usb device driver
         if (user_settings.check_for_driver_change(gamepads_[0]))
         {
@@ -101,17 +100,25 @@ void run_program()
         gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
     }
 
+    DeviceDriverType current_driver = user_settings.get_current_driver();
     DeviceManager& device_manager = DeviceManager::get_instance();
-    device_manager.initialize_driver(user_settings.get_current_driver(), gamepads_);
+    device_manager.initialize_driver(current_driver, gamepads_);
 
     multicore_reset_core1();
     multicore_launch_core1(core1_task);
 
-    // Wait for something to call host_mounted()
-    while (!tud_inited())
+    if (current_driver != DeviceDriverType::WEBAPP)
     {
-        TaskQueue::Core0::process_tasks();
-        sleep_ms(100);
+        // Wait for something to call host_mounted()
+        while (!tud_inited())
+        {
+            TaskQueue::Core0::process_tasks();
+            sleep_ms(100);
+        }
+    }
+    else //Connect immediately in WebApp mode
+    {
+        host_mounted(true);
     }
 
     uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h
index ed6a749..fed7709 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h
@@ -7,7 +7,7 @@
 #include "class/hid/hid.h"
 #include "device/usbd_pvt.h"
 
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 
 #if CFG_TUSB_DEBUG >= CFG_TUD_LOG_LEVEL
     #define TUD_DRV_NAME(name) name
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.h
index bef81da..8f760ac 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.h
@@ -5,7 +5,6 @@
 
 #include "tusb.h"
 
-#include "Gamepad.h"
 #include "Descriptors/PSClassic.h"
 #include "USBDevice/DeviceDriver/DeviceDriver.h"
 
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.c b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.c
index 004d534..7d11093 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.c
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.c
@@ -31,6 +31,9 @@
 #define DEF_PARITY 0
 #define DEF_DATA_BITS 8
 
+const char COMPLETE_FLAG[] = "PROGRAMMING_COMPLETE";
+const size_t COMPLETE_FLAG_READ_LEN = 22;
+
 typedef struct {
 	uart_inst_t *const inst;
 	uint irq;
@@ -74,6 +77,7 @@ const uart_id_t UART_ID[CFG_TUD_CDC] = {
 };
 
 uart_data_t UART_DATA[CFG_TUD_CDC];
+bool programming_complete = false;
 
 static inline uint databits_usb2uart(uint8_t data_bits)
 {
@@ -143,13 +147,28 @@ void usb_read_bytes(uint8_t itf)
 	uart_data_t *ud = &UART_DATA[itf];
 	uint32_t len = tud_cdc_n_available(itf);
 
-	if (len &&
-	    mutex_try_enter(&ud->usb_mtx, NULL)) {
+	if (len && mutex_try_enter(&ud->usb_mtx, NULL)) 
+	{
 		len = MIN(len, BUFFER_SIZE - ud->usb_pos);
-		if (len) {
+		if (len) 
+		{
 			uint32_t count;
-
 			count = tud_cdc_n_read(itf, &ud->usb_buffer[ud->usb_pos], len);
+			
+			if (count >= COMPLETE_FLAG_READ_LEN) 
+			{
+				for (uint32_t i = 0; i < count; i++)
+				{
+					uint32_t remaining = BUFFER_SIZE - ud->usb_pos - i;
+					if (remaining >= sizeof(COMPLETE_FLAG) - 1 &&
+						memcmp(&ud->usb_buffer[ud->usb_pos + i], COMPLETE_FLAG, sizeof(COMPLETE_FLAG) - 1) == 0)
+					{
+						programming_complete = true;
+						break;
+					}
+				}
+			}
+
 			ud->usb_pos += count;
 		}
 
@@ -288,14 +307,14 @@ void init_uart_data(uint8_t itf)
 
 void core1_entry(void)
 {
-	for (int itf = 0; itf < CFG_TUD_CDC; itf++)
+	for (uint8_t itf = 0; itf < CFG_TUD_CDC; itf++)
 	{
 		init_uart_data(0);
 	}
 
 	while (1) 
 	{
-		for (int itf = 0; itf < CFG_TUD_CDC; itf++) 
+		for (uint8_t itf = 0; itf < CFG_TUD_CDC; itf++) 
 		{
 			update_uart_cfg(itf);
 			uart_write_bytes(itf);
@@ -316,13 +335,19 @@ int uart_bridge_run(void)
 	{
 		tud_task();
 
-		for (int itf = 0; itf < CFG_TUD_CDC; itf++) 
+		for (uint8_t itf = 0; itf < CFG_TUD_CDC; itf++) 
 		{
 			if (tud_cdc_n_connected(itf)) 
 			{
 				usb_cdc_process(itf);
 			}
 		}
+
+		if (programming_complete)
+		{
+			multicore_reset_core1();
+			break;
+		}
 	}
 
 	return 0;
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.h
index 6a31d49..f4edd82 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.h
@@ -6,9 +6,6 @@ extern "C" {
 #endif
 
 int uart_bridge_run(void);
-// const uint8_t *uart_bridge_descriptor_device_cb(void);
-// const uint8_t *uart_bridge_descriptor_configuration_cb(uint8_t index);
-// const uint16_t *uart_bridge_descriptor_string_cb(uint8_t index, uint16_t langid);
 
 #ifdef __cplusplus
 }
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp
index 9346cc6..4c25701 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp
@@ -1,6 +1,7 @@
 #include "class/cdc/cdc_device.h"
 #include "bsp/board_api.h"
 
+#include "Board/ogxm_log.h"
 #include "Descriptors/CDCDev.h"
 #include "USBDevice/DeviceDriver/WebApp/WebApp.h"
 
@@ -19,62 +20,245 @@ void WebAppDevice::initialize()
     };
 }
 
+bool WebAppDevice::read_serial(void* buffer, size_t len, bool block)
+{
+    if (!block && !tud_cdc_available()) 
+    {
+        return false;
+    }
+
+    uint8_t* buf_ptr = reinterpret_cast<uint8_t*>(buffer);
+    size_t total_read = 0;
+
+    while (total_read < len)
+    {
+        if (!tud_cdc_connected())
+        {
+            return false;
+        }
+        tud_task();
+
+        if (tud_cdc_available())
+        {
+            size_t read = tud_cdc_read(buf_ptr + total_read, len - total_read);
+            total_read += read;
+        }
+    }
+    tud_cdc_read_flush();
+    return total_read == len;
+}
+
+bool WebAppDevice::write_serial(const void* buffer, size_t len)
+{
+    const uint8_t* buf_ptr = reinterpret_cast<const uint8_t*>(buffer);
+    size_t total_written = 0;
+
+    while (total_written < len)
+    {
+        if (!tud_cdc_connected())
+        {
+            return false;
+        }
+        tud_task();
+
+        if (tud_cdc_write_available())
+        {
+            size_t written = tud_cdc_write(buf_ptr + total_written, len - total_written);
+            total_written += written;
+            tud_cdc_write_flush();
+        }
+    }
+    return total_written == len;
+}
+
+bool WebAppDevice::write_packet(const Packet& packet)
+{
+    if (!write_serial(&packet, sizeof(Packet)))
+    {
+        return false;
+    }
+    return true;
+}
+
+bool WebAppDevice::read_packet(Packet& packet, bool block)
+{
+    if (!read_serial(&packet, sizeof(Packet), block))
+    {
+        return false;
+    }
+    return true;
+}
+
+//Blocking read
+bool WebAppDevice::read_profile(UserProfile& profile)
+{
+    uint8_t* profile_data = reinterpret_cast<uint8_t*>(&profile);
+    uint8_t current_chunk = 0;
+    uint8_t expected_chunks = 0;
+
+    while (current_chunk < expected_chunks || expected_chunks == 0)
+    {
+        if (!read_packet(packet_out_, true))
+        {
+            return false;
+        }
+        if (packet_out_.header.packet_id != PacketID::SET_PROFILE)
+        {
+            return false;
+        }
+        if (expected_chunks == 0 && packet_out_.header.chunks_total > 0)
+        {
+            expected_chunks = packet_out_.header.chunks_total;
+        }
+        else if (expected_chunks == 0)
+        {
+            return false;
+        }
+
+        size_t offset = packet_out_.header.chunk_idx * packet_out_.header.chunk_len;
+        size_t bytes_to_copy = std::min(static_cast<size_t>(packet_out_.header.chunk_len), sizeof(UserProfile) - offset);
+
+        std::memcpy(profile_data + offset, packet_out_.data.data(), bytes_to_copy);
+        current_chunk++;
+    }
+    return true;
+}
+
+bool WebAppDevice::write_profile(uint8_t index, const UserProfile& profile)
+{
+    const uint8_t* profile_data = reinterpret_cast<const uint8_t*>(&profile);
+    uint8_t total_chunks = static_cast<uint8_t>((sizeof(UserProfile) + packet_in_.data.size() - 1) / packet_in_.data.size());
+    uint8_t current_chunk = 0;
+
+    while (current_chunk < total_chunks)
+    {
+        size_t offset = current_chunk * packet_in_.data.size();
+        size_t remaining_bytes = sizeof(UserProfile) - offset;
+        uint8_t current_chunk_len = static_cast<uint8_t>(std::min(packet_in_.data.size(), remaining_bytes));
+
+        packet_in_.header.packet_id = PacketID::GET_PROFILE_RESP_OK;
+        packet_in_.header.max_gamepads = MAX_GAMEPADS;
+        packet_in_.header.player_idx = index;
+        packet_in_.header.profile_id = profile.id;
+        packet_in_.header.chunks_total = total_chunks;
+        packet_in_.header.chunk_idx = current_chunk;
+        packet_in_.header.chunk_len = current_chunk_len;
+
+        std::memcpy(packet_in_.data.data(), profile_data + offset, packet_in_.header.chunk_len);
+
+        if (!write_packet(packet_in_))
+        {
+            return false;
+        }
+        current_chunk++;
+    }
+    return true;
+}
+
+bool WebAppDevice::write_gamepad(uint8_t index, const Gamepad::PadIn& pad_in)
+{
+    const uint8_t* pad_in_data = reinterpret_cast<const uint8_t*>(&pad_in);
+    const uint8_t total_chunks = static_cast<uint8_t>((sizeof(Gamepad::PadIn) + packet_in_.data.size() - 1) / packet_in_.data.size());
+    uint8_t current_chunk = 0;
+
+    while (current_chunk < total_chunks)
+    {
+        size_t offset = current_chunk * packet_in_.data.size();
+        size_t remaining_bytes = sizeof(Gamepad::PadIn) - offset;
+        uint8_t current_chunk_len = static_cast<uint8_t>(std::min(packet_in_.data.size(), remaining_bytes));
+
+        packet_in_.header.packet_id = PacketID::SET_GP_IN;
+        packet_in_.header.max_gamepads = MAX_GAMEPADS;
+        packet_in_.header.player_idx = index;
+        packet_in_.header.chunks_total = total_chunks;
+        packet_in_.header.chunk_idx = current_chunk;
+        packet_in_.header.chunk_len = current_chunk_len;
+
+        std::memcpy(packet_in_.data.data(), pad_in_data + offset, packet_in_.header.chunk_len);
+
+        if (!write_packet(packet_in_))
+        {
+            return false;
+        }
+        current_chunk++;
+    }
+    return true;
+}
+
+void WebAppDevice::write_error()
+{
+    packet_in_.header.packet_id = PacketID::RESP_ERROR;
+    write_packet(packet_in_);
+}
+
 void WebAppDevice::process(const uint8_t idx, Gamepad& gamepad) 
 {
-    if (!tud_cdc_available() || !tud_cdc_connected())
+    if (!tud_cdc_connected())
     {
         return;
     }
 
     tud_cdc_write_flush();
-    tud_cdc_read(reinterpret_cast<void*>(&in_report_), sizeof(Report));
-    tud_cdc_read_flush();
+    bool success = false;   
 
-    bool success = false;
+    OGXM_LOG("Processing WebApp device\n");
 
-    switch (in_report_.report_id)
+    if (read_packet(packet_out_, false))
     {
-        case ReportID::INIT_READ:
-            in_report_.input_mode = static_cast<uint8_t>(user_settings_.get_current_driver());
-            in_report_.player_idx = 0;
-            in_report_.report_id = ReportID::RESP_OK;
-            in_report_.max_gamepads = MAX_GAMEPADS;
+        OGXM_LOG("Received packet with ID: %d\n", packet_out_.header.packet_id);
+        
+        switch (packet_out_.header.packet_id)
+        {
+            case PacketID::GET_PROFILE_BY_ID:
+                profile_ = user_settings_.get_profile_by_id(packet_out_.header.profile_id);
+                if (!write_profile(packet_out_.header.profile_id, profile_))
+                {
+                    write_error();
+                    return;
+                }
+                break;
 
-            in_report_.profile.id = user_settings_.get_active_profile_id(in_report_.player_idx);
-            in_report_.profile = user_settings_.get_profile_by_id(in_report_.profile.id);
+            case PacketID::GET_PROFILE_BY_IDX:
+                profile_ = user_settings_.get_profile_by_index(packet_out_.header.player_idx);
+                if (!write_profile(packet_out_.header.player_idx, profile_))
+                {
+                    write_error();
+                    return;
+                }
+                break;
 
-            tud_cdc_write(reinterpret_cast<const void*>(&in_report_), sizeof(Report));
-            tud_cdc_write_flush();
-            break;
+            case PacketID::SET_PROFILE_START:
+                if (!read_profile(profile_))
+                {
+                    write_error();
+                    return;
+                }
+                if (packet_out_.header.device_driver != DeviceDriverType::WEBAPP &&
+                    user_settings_.is_valid_driver(packet_out_.header.device_driver))
+                {
+                    success = user_settings_.store_profile_and_driver_type(packet_out_.header.device_driver, packet_out_.header.player_idx, profile_);
+                }
+                else
+                {
+                    success = user_settings_.store_profile(packet_out_.header.player_idx, profile_);
+                }
+                if (!success)
+                {
+                    write_error();
+                    return;
+                }
+                break;
 
-        case ReportID::READ_PROFILE:
-            in_report_.input_mode = static_cast<uint8_t>(user_settings_.get_current_driver());
-            in_report_.profile = user_settings_.get_profile_by_id(in_report_.profile.id);
-            in_report_.report_id = ReportID::RESP_OK;
-
-            tud_cdc_write(reinterpret_cast<const void*>(&in_report_), sizeof(Report));
-            tud_cdc_write_flush();
-            break;
-
-        case ReportID::WRITE_PROFILE:
-            if (user_settings_.is_valid_driver(static_cast<DeviceDriverType>(in_report_.input_mode)))
-            {
-                success = user_settings_.store_profile_and_driver_type(static_cast<DeviceDriverType>(in_report_.input_mode), in_report_.player_idx, in_report_.profile);
-            }
-            else
-            {
-                success = user_settings_.store_profile(in_report_.player_idx, in_report_.profile);
-            }
-            if (!success)
-            {
-                in_report_.report_id = ReportID::RESP_ERROR;
-                tud_cdc_write(reinterpret_cast<const void*>(&in_report_), sizeof(Report));
-                tud_cdc_write_flush();
-            }
-            break;
-            
-        default:
-            return;
+            default:
+                write_error();
+                return;
+        }
+    } 
+    else if (gamepad.new_pad_in())
+    {
+        OGXM_LOG("Writing gamepad input\n");
+        Gamepad::PadIn gp_in = gamepad.get_pad_in();
+        write_gamepad(idx, gp_in);
     }
 }
 
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.h
index 9477a21..d9d1be2 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.h
@@ -1,6 +1,8 @@
 #ifndef _WEBAAPP_DEVICE_H_
 #define _WEBAAPP_DEVICE_H_
 
+#include <array>
+
 #include "USBDevice/DeviceDriver/DeviceDriver.h"
 #include "UserSettings/UserSettings.h"
 #include "UserSettings/UserProfile.h"
@@ -20,30 +22,56 @@ public:
     const uint8_t* get_descriptor_device_qualifier_cb() override;
 
 private:
-    struct ReportID
+    enum class PacketID : uint8_t
     {
-        static constexpr uint8_t INIT_READ = 0x88;
-        static constexpr uint8_t READ_PROFILE = 0x01;
-        static constexpr uint8_t WRITE_PROFILE = 0x02;
-        static constexpr uint8_t RESP_OK = 0x10;
-        static constexpr uint8_t RESP_ERROR = 0x11;
+        NONE = 0,
+        GET_PROFILE_BY_ID = 0x50,
+        GET_PROFILE_BY_IDX = 0x51,
+        GET_PROFILE_RESP_OK = 0x52,
+        SET_PROFILE_START = 0x60,
+        SET_PROFILE = 0x61,
+        SET_PROFILE_RESP_OK = 0x62,
+        SET_GP_IN = 0x80,
+        SET_GP_OUT = 0x81,
+        RESP_ERROR = 0xFF
     };
-
+    
     #pragma pack(push, 1)
-    struct Report
+    struct PacketHeader
     {
-        uint8_t report_id{0};
-        uint8_t input_mode{0};
+        uint8_t packet_len{64};
+        PacketID packet_id{PacketID::NONE};
+        DeviceDriverType device_driver{DeviceDriverType::WEBAPP};
         uint8_t max_gamepads{MAX_GAMEPADS};
         uint8_t player_idx{0};
-        UserProfile profile{UserProfile()};
+        uint8_t profile_id{0};
+        uint8_t chunks_total{0};
+        uint8_t chunk_idx{0};
+        uint8_t chunk_len{0};
+    };
+    static_assert(sizeof(PacketHeader) == 9, "WebApp report size mismatch");
+
+    struct Packet
+    {
+        PacketHeader header;
+        std::array<uint8_t, 64 - sizeof(PacketHeader)> data{0};
     };
-    static_assert(sizeof(Report) == 50, "WebApp report size mismatch");
     #pragma pack(pop)
 
+    Packet packet_in_;
+    Packet packet_out_;
+
     UserSettings& user_settings_{UserSettings::get_instance()};
-    Report in_report_{Report()};
-    DeviceDriverType driver_type_{DeviceDriverType::WEBAPP};
+    UserProfile profile_;
+
+    bool read_profile(UserProfile& profile);
+    bool read_serial(void* buffer, size_t len, bool block);
+    bool read_packet(Packet& packet, bool block);
+    bool write_serial(const void* buffer, size_t len);
+    bool write_packet(const Packet& packet);
+    bool write_profile(uint8_t index, const UserProfile& profile);
+    bool write_gamepad(uint8_t index, const Gamepad::PadIn& pad_in);
+    void write_error();
 };
 
 #endif // _WEBAAPP_DEVICE_H_
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.h
index 02f99b1..ea98c99 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.h
@@ -4,7 +4,6 @@
 #include <cstdint>
 #include <numeric>
 
-#include "Gamepad.h"
 #include "USBDevice/DeviceDriver/DeviceDriver.h"
 #include "Descriptors/XboxOG.h"
 
diff --git a/Firmware/RP2040/src/USBDevice/DeviceManager.h b/Firmware/RP2040/src/USBDevice/DeviceManager.h
index 515f688..6a76853 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceManager.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceManager.h
@@ -4,7 +4,6 @@
 #include <cstdint>
 #include <memory>
 
-#include "Gamepad.h"
 #include "USBDevice/DeviceDriver/DeviceDriverTypes.h"
 #include "USBDevice/DeviceDriver/DeviceDriver.h"
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp b/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp
index 267ebba..e1e97eb 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp
@@ -96,10 +96,8 @@ void DInputHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t insta
         gp_in.trigger_r = (in_report->buttons[0] & DInput::Buttons0::R2) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
     }
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(in_report->joystick_lx, in_report->joystick_ly);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(in_report->joystick_rx, in_report->joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp b/Firmware/RP2040/src/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp
index 7fa1b16..0065bf1 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp
@@ -68,10 +68,8 @@ void HIDHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
             break;
     }
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(hid_joystick_data_.X);
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(hid_joystick_data_.Y);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(hid_joystick_data_.Z);
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(hid_joystick_data_.Rz);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(hid_joystick_data_.X, hid_joystick_data_.Y);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(hid_joystick_data_.Z, hid_joystick_data_.Rz);
 
     if (hid_joystick_data_.buttons[1])  gp_in.buttons |= gamepad.MAP_BUTTON_X;
     if (hid_joystick_data_.buttons[2])  gp_in.buttons |= gamepad.MAP_BUTTON_A;
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h b/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h
index d213e20..0af51b0 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h
+++ b/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h
@@ -5,7 +5,7 @@
 
 #include "UserSettings/UserProfile.h"
 #include "UserSettings/UserSettings.h"
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 #include "USBHost/HostDriver/HostDriverTypes.h"
 
 //Use HostManager, don't use this directly
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/N64/N64.cpp b/Firmware/RP2040/src/USBHost/HostDriver/N64/N64.cpp
index ffef075..a5939aa 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/N64/N64.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/N64/N64.cpp
@@ -94,14 +94,11 @@ void N64Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
             break;
     }
 
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_y);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_x);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(in_report->joystick_x, in_report->joystick_y);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(joy_rx, joy_ry);
 
     gp_in.trigger_l = (in_report->buttons & N64::Buttons::L) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
 
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(joy_ry);
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(joy_rx);
-
     gamepad.set_pad_in(gp_in);
 
     tuh_hid_receive_report(address, instance);
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp b/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp
index cd34fbb..075acfb 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp
@@ -140,10 +140,8 @@ void PS3Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     gp_in.trigger_l = gamepad.scale_trigger_l(in_report->l2_axis);
     gp_in.trigger_r = gamepad.scale_trigger_r(in_report->r2_axis);
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
-    gp_in.joystick_ry = gamepad.scale_joystick_ly(in_report->joystick_ry);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(in_report->joystick_lx, in_report->joystick_ly);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(in_report->joystick_rx, in_report->joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/PS4/PS4.cpp b/Firmware/RP2040/src/USBHost/HostDriver/PS4/PS4.cpp
index 3ac3ebf..8b8b39d 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/PS4/PS4.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/PS4/PS4.cpp
@@ -72,10 +72,8 @@ void PS4Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     gp_in.trigger_l = gamepad.scale_trigger_l(in_report_.trigger_l);
     gp_in.trigger_r = gamepad.scale_trigger_r(in_report_.trigger_r);
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report_.joystick_lx);
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report_.joystick_ly);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report_.joystick_rx);
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report_.joystick_ry);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(in_report_.joystick_lx, in_report_.joystick_ly);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(in_report_.joystick_rx, in_report_.joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp b/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp
index c710182..0389b78 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp
@@ -89,10 +89,8 @@ void PS5Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     gp_in.trigger_l = gamepad.scale_trigger_l(in_report->trigger_l);
     gp_in.trigger_r = gamepad.scale_trigger_r(in_report->trigger_r);
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(in_report->joystick_lx, in_report->joystick_ly);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(in_report->joystick_rx, in_report->joystick_ry);
     
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.cpp b/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.cpp
index e5d7caf..105f308 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.cpp
@@ -168,10 +168,11 @@ void SwitchProHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t in
     uint16_t joy_rx = in_report->joysticks[3] | ((in_report->joysticks[4] & 0xF) << 8);
     uint16_t joy_ry = (in_report->joysticks[4] >> 4) | (in_report->joysticks[5] << 4);
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(normalize_axis(joy_lx));
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(normalize_axis(joy_ly), true);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(normalize_axis(joy_rx));
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(normalize_axis(joy_ry), true);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = 
+        gamepad.scale_joystick_l(normalize_axis(joy_lx), normalize_axis(joy_ly), true);
+        
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = 
+        gamepad.scale_joystick_r(normalize_axis(joy_rx), normalize_axis(joy_ry), true);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/SwitchWired/SwitchWired.cpp b/Firmware/RP2040/src/USBHost/HostDriver/SwitchWired/SwitchWired.cpp
index 8c8c7c7..b5c6210 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/SwitchWired/SwitchWired.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/SwitchWired/SwitchWired.cpp
@@ -67,10 +67,8 @@ void SwitchWiredHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t
     gp_in.trigger_l = (in_report->buttons & SwitchWired::Buttons::ZL) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
     gp_in.trigger_r = (in_report->buttons & SwitchWired::Buttons::ZR) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(in_report->joystick_lx, in_report->joystick_ly);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(in_report->joystick_rx, in_report->joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360.cpp
index ae3776e..58e7505 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360.cpp
@@ -42,10 +42,8 @@ void Xbox360Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t inst
     gp_in.trigger_l = gamepad.scale_trigger_l(in_report_->trigger_l);
     gp_in.trigger_r = gamepad.scale_trigger_r(in_report_->trigger_r);
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report_->joystick_lx);
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report_->joystick_ly, true);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report_->joystick_rx);
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report_->joystick_ry, true);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(in_report_->joystick_lx, in_report_->joystick_ly, true);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(in_report_->joystick_rx, in_report_->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp
index 1e1dbc3..39a4803 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp
@@ -74,10 +74,8 @@ void Xbox360WHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t ins
     gp_in.trigger_l = gamepad.scale_trigger_l(in_report->trigger_l);
     gp_in.trigger_r = gamepad.scale_trigger_r(in_report->trigger_r);
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly, true);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry, true);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(in_report->joystick_lx, in_report->joystick_ly, true);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(in_report->joystick_rx, in_report->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp
index 858b073..78122c3 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp
@@ -57,10 +57,8 @@ void XboxOGHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t insta
     gp_in.trigger_l = gamepad.scale_trigger_l(in_report->trigger_l);
     gp_in.trigger_r = gamepad.scale_trigger_r(in_report->trigger_r);
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly, true);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry, true);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(in_report->joystick_lx, in_report->joystick_ly, true);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(in_report->joystick_rx, in_report->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOne.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOne.cpp
index bf3667c..6dab811 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOne.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOne.cpp
@@ -42,10 +42,8 @@ void XboxOneHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t inst
     gp_in.trigger_l = gamepad.scale_trigger_l(static_cast<uint8_t>(in_report->trigger_l >> 2));
     gp_in.trigger_r = gamepad.scale_trigger_r(static_cast<uint8_t>(in_report->trigger_r >> 2));
 
-    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
-    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly, true);
-    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
-    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry, true);
+    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad.scale_joystick_l(in_report->joystick_lx, in_report->joystick_ly, true);
+    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad.scale_joystick_r(in_report->joystick_rx, in_report->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostManager.h b/Firmware/RP2040/src/USBHost/HostManager.h
index 0826204..d15ab57 100644
--- a/Firmware/RP2040/src/USBHost/HostManager.h
+++ b/Firmware/RP2040/src/USBHost/HostManager.h
@@ -9,8 +9,6 @@
 #include <hardware/resets.h>
 
 #include "board_config.h"
-#include "Gamepad.h"
-#include "OGXMini/OGXMini.h"
 #include "USBHost/HardwareIDs.h"
 #include "USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h"
 #include "USBHost/HostDriver/HostDriver.h"
diff --git a/Firmware/RP2040/src/UserSettings/JoystickSettings.cpp b/Firmware/RP2040/src/UserSettings/JoystickSettings.cpp
new file mode 100644
index 0000000..e50321d
--- /dev/null
+++ b/Firmware/RP2040/src/UserSettings/JoystickSettings.cpp
@@ -0,0 +1,41 @@
+#include "UserSettings/JoystickSettings.h"
+
+bool JoystickSettings::is_same(const JoystickSettingsRaw& raw) const
+{
+    return  dz_inner == Fix16(raw.dz_inner) &&
+            dz_outer == Fix16(raw.dz_outer) &&
+            anti_dz_circle == Fix16(raw.anti_dz_circle) &&
+            anti_dz_circle_y_scale == Fix16(raw.anti_dz_circle_y_scale) &&
+            anti_dz_square == Fix16(raw.anti_dz_square) &&
+            anti_dz_square_y_scale == Fix16(raw.anti_dz_square_y_scale) &&
+            anti_dz_angular == Fix16(raw.anti_dz_angular) &&
+            anti_dz_outer == Fix16(raw.anti_dz_outer) &&
+            axis_restrict == Fix16(raw.axis_restrict) &&
+            angle_restrict == Fix16(raw.angle_restrict) &&
+            diag_scale_min == Fix16(raw.diag_scale_min) &&
+            diag_scale_max == Fix16(raw.diag_scale_max) &&
+            curve == Fix16(raw.curve) &&
+            uncap_radius == raw.uncap_radius &&
+            invert_y == raw.invert_y &&
+            invert_x == raw.invert_x;
+}
+
+void JoystickSettings::set_from_raw(const JoystickSettingsRaw& raw)
+{
+    dz_inner = Fix16(raw.dz_inner);
+    dz_outer = Fix16(raw.dz_outer);
+    anti_dz_circle = Fix16(raw.anti_dz_circle);
+    anti_dz_circle_y_scale = Fix16(raw.anti_dz_circle_y_scale);
+    anti_dz_square = Fix16(raw.anti_dz_square);
+    anti_dz_square_y_scale = Fix16(raw.anti_dz_square_y_scale);
+    anti_dz_angular = Fix16(raw.anti_dz_angular);
+    anti_dz_outer = Fix16(raw.anti_dz_outer);
+    axis_restrict = Fix16(raw.axis_restrict);
+    angle_restrict = Fix16(raw.angle_restrict);
+    diag_scale_min = Fix16(raw.diag_scale_min);
+    diag_scale_max = Fix16(raw.diag_scale_max);
+    curve = Fix16(raw.curve);
+    uncap_radius = raw.uncap_radius;
+    invert_y = raw.invert_y;
+    invert_x = raw.invert_x;
+}
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/JoystickSettings.h b/Firmware/RP2040/src/UserSettings/JoystickSettings.h
new file mode 100644
index 0000000..2e7e06c
--- /dev/null
+++ b/Firmware/RP2040/src/UserSettings/JoystickSettings.h
@@ -0,0 +1,66 @@
+#ifndef _JOYSTICK_SETTINGS_H_
+#define _JOYSTICK_SETTINGS_H_
+
+#include <cstdint>
+
+#include "libfixmath/fix16.hpp"
+
+struct JoystickSettingsRaw;
+
+struct JoystickSettings
+{
+    Fix16 dz_inner{Fix16(0.0f)};
+    Fix16 dz_outer{Fix16(1.0f)};
+
+    Fix16 anti_dz_circle{Fix16(0.0f)};
+    Fix16 anti_dz_circle_y_scale{Fix16(0.0f)};
+    Fix16 anti_dz_square{Fix16(0.0f)};
+    Fix16 anti_dz_square_y_scale{Fix16(0.0f)};
+    Fix16 anti_dz_angular{Fix16(0.0f)};
+    Fix16 anti_dz_outer{Fix16(1.0f)};
+    
+    Fix16 axis_restrict{Fix16(0.0f)};
+    Fix16 angle_restrict{Fix16(0.0f)};
+    
+    Fix16 diag_scale_min{Fix16(1.0f)};
+    Fix16 diag_scale_max{Fix16(1.0f)};
+
+    Fix16 curve{Fix16(1.0f)};
+
+    bool uncap_radius{true};
+    bool invert_y{false};
+    bool invert_x{false};
+
+    bool is_same(const JoystickSettingsRaw& raw) const;
+    void set_from_raw(const JoystickSettingsRaw& raw);
+};
+
+#pragma pack(push, 1)
+struct JoystickSettingsRaw
+{
+    fix16_t dz_inner{fix16_from_int(0)};
+    fix16_t dz_outer{fix16_from_int(1)};
+
+    fix16_t anti_dz_circle{fix16_from_int(0)};
+    fix16_t anti_dz_circle_y_scale{fix16_from_int(0)};
+    fix16_t anti_dz_square{fix16_from_int(0)};
+    fix16_t anti_dz_square_y_scale{fix16_from_int(0)};
+    fix16_t anti_dz_angular{fix16_from_int(0)};
+    fix16_t anti_dz_outer{fix16_from_int(1)};
+    
+    fix16_t axis_restrict{fix16_from_int(0)};
+    fix16_t angle_restrict{fix16_from_int(0)};
+    
+    fix16_t diag_scale_min{fix16_from_int(1)};
+    fix16_t diag_scale_max{fix16_from_int(1)};
+
+    fix16_t curve{fix16_from_int(1)};
+
+    uint8_t uncap_radius{true};
+    uint8_t invert_y{false};
+    uint8_t invert_x{false};
+};
+static_assert(sizeof(JoystickSettingsRaw) == 55, "JoystickSettingsRaw is an unexpected size");
+#pragma pack(pop)
+
+#endif // _JOYSTICK_SETTINGS_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/NVSTool.h b/Firmware/RP2040/src/UserSettings/NVSTool.h
index 740b8ce..1cce5a2 100644
--- a/Firmware/RP2040/src/UserSettings/NVSTool.h
+++ b/Firmware/RP2040/src/UserSettings/NVSTool.h
@@ -80,6 +80,27 @@ public:
         return false;
     }
 
+    void erase_all()
+    {
+        mutex_enter_blocking(&nvs_mutex_);
+
+        for (uint32_t i = 0; i < NVS_SECTORS; ++i) 
+        {
+            flash_range_erase(NVS_START_OFFSET + i * FLASH_SECTOR_SIZE, FLASH_SECTOR_SIZE);
+        }
+
+        Entry entry;
+
+        for (uint32_t i = 0; i < MAX_ENTRIES + 1; ++i) 
+        {
+            flash_range_program(NVS_START_OFFSET + i * sizeof(Entry), 
+                                reinterpret_cast<const uint8_t*>(&entry), 
+                                sizeof(Entry));
+        }
+
+        mutex_exit(&nvs_mutex_);
+    }
+
 private:
     NVSTool()
     {
@@ -89,23 +110,7 @@ private:
 
         if (std::strcmp(initial_entry->key, INVALID_KEY) != 0) 
         {
-            mutex_enter_blocking(&nvs_mutex_);
-
-            for (uint32_t i = 0; i < NVS_SECTORS; ++i) 
-            {
-                flash_range_erase(NVS_START_OFFSET + i * FLASH_SECTOR_SIZE, FLASH_SECTOR_SIZE);
-            }
-
-            Entry entry;
-
-            for (uint32_t i = 0; i < MAX_ENTRIES; ++i) 
-            {
-                flash_range_program(NVS_START_OFFSET + i * sizeof(Entry), 
-                                    reinterpret_cast<const uint8_t*>(&entry), 
-                                    sizeof(Entry));
-            }
-
-            mutex_exit(&nvs_mutex_);
+            erase_all();
         }
     }
 
diff --git a/Firmware/RP2040/src/UserSettings/TriggerSettings.cpp b/Firmware/RP2040/src/UserSettings/TriggerSettings.cpp
new file mode 100644
index 0000000..6508f07
--- /dev/null
+++ b/Firmware/RP2040/src/UserSettings/TriggerSettings.cpp
@@ -0,0 +1,19 @@
+#include "UserSettings/TriggerSettings.h"
+
+bool TriggerSettings::is_same(const TriggerSettingsRaw& raw) const
+{
+    return  dz_inner == Fix16(raw.dz_inner) &&
+            dz_outer == Fix16(raw.dz_outer) &&
+            anti_dz_inner == Fix16(raw.anti_dz_inner) &&
+            anti_dz_outer == Fix16(raw.anti_dz_outer) &&
+            curve == Fix16(raw.curve);
+}
+
+void TriggerSettings::set_from_raw(const TriggerSettingsRaw& raw)
+{
+    dz_inner = Fix16(raw.dz_inner);
+    dz_outer = Fix16(raw.dz_outer);
+    anti_dz_inner = Fix16(raw.anti_dz_inner);
+    anti_dz_outer = Fix16(raw.anti_dz_outer);
+    curve = Fix16(raw.curve);
+}
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/TriggerSettings.h b/Firmware/RP2040/src/UserSettings/TriggerSettings.h
new file mode 100644
index 0000000..ad29487
--- /dev/null
+++ b/Firmware/RP2040/src/UserSettings/TriggerSettings.h
@@ -0,0 +1,38 @@
+#ifndef TRIGGER_SETTINGS_H
+#define TRIGGER_SETTINGS_H
+
+#include <cstdint>
+
+#include "libfixmath/fix16.hpp"
+
+struct TriggerSettingsRaw;
+
+struct TriggerSettings
+{
+    Fix16 dz_inner{Fix16(0.0f)};
+    Fix16 dz_outer{Fix16(1.0f)};
+
+    Fix16 anti_dz_inner{Fix16(0.0f)};
+    Fix16 anti_dz_outer{Fix16(1.0f)};
+
+    Fix16 curve{Fix16(1.0f)};
+
+    bool is_same(const TriggerSettingsRaw& raw) const;
+    void set_from_raw(const TriggerSettingsRaw& raw);
+};
+
+#pragma pack(push, 1)
+struct TriggerSettingsRaw
+{
+    fix16_t dz_inner{fix16_from_int(0)};
+    fix16_t dz_outer{fix16_from_int(1)};
+
+    fix16_t anti_dz_inner{fix16_from_int(0)};
+    fix16_t anti_dz_outer{fix16_from_int(1)};
+    
+    fix16_t curve{fix16_from_int(1)};
+};
+static_assert(sizeof(TriggerSettingsRaw) == 20, "TriggerSettingsRaw is an unexpected size");
+#pragma pack(pop)
+
+#endif // TRIGGER_SETTINGS_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/UserProfile.cpp b/Firmware/RP2040/src/UserSettings/UserProfile.cpp
index 8375735..332993a 100644
--- a/Firmware/RP2040/src/UserSettings/UserProfile.cpp
+++ b/Firmware/RP2040/src/UserSettings/UserProfile.cpp
@@ -1,20 +1,12 @@
 #include <cstring>
 
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 #include "UserSettings/UserProfile.h"
 
 UserProfile::UserProfile()
 {
     id = 1;
 
-    dz_trigger_l = 0;
-    dz_trigger_r = 0;
-    dz_joystick_l = 0;
-    dz_joystick_r = 0;
-
-    invert_ly = 0;
-    invert_ry = 0;
-
     dpad_up = Gamepad::DPAD_UP;
     dpad_down = Gamepad::DPAD_DOWN;
     dpad_left = Gamepad::DPAD_LEFT;
diff --git a/Firmware/RP2040/src/UserSettings/UserProfile.h b/Firmware/RP2040/src/UserSettings/UserProfile.h
index 2efa406..6bd28f2 100644
--- a/Firmware/RP2040/src/UserSettings/UserProfile.h
+++ b/Firmware/RP2040/src/UserSettings/UserProfile.h
@@ -3,19 +3,18 @@
 
 #include <cstdint>
 
+#include "UserSettings/JoystickSettings.h"
+#include "UserSettings/TriggerSettings.h"   
+
 #pragma pack(push, 1)
 struct UserProfile
 {
     uint8_t id;
 
-    uint8_t dz_trigger_l;
-    uint8_t dz_trigger_r;
-
-    uint8_t dz_joystick_l;
-    uint8_t dz_joystick_r;
-
-    uint8_t invert_ly;
-    uint8_t invert_ry;
+    JoystickSettingsRaw joystick_settings_l;
+    JoystickSettingsRaw joystick_settings_r;
+    TriggerSettingsRaw trigger_settings_l;
+    TriggerSettingsRaw trigger_settings_r;
 
     uint8_t dpad_up;
     uint8_t dpad_down;
@@ -50,7 +49,7 @@ struct UserProfile
 
     UserProfile();
 };
-static_assert(sizeof(UserProfile) == 46, "UserProfile struct size mismatch");
+static_assert(sizeof(UserProfile) == 190, "UserProfile struct size mismatch");
 #pragma pack(pop)
 
 #endif // _USER_PROFILE_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/UserSettings.cpp b/Firmware/RP2040/src/UserSettings/UserSettings.cpp
index 3820675..9b4a10e 100644
--- a/Firmware/RP2040/src/UserSettings/UserSettings.cpp
+++ b/Firmware/RP2040/src/UserSettings/UserSettings.cpp
@@ -1,5 +1,6 @@
 #include <cstring>
 #include <array>
+#include <memory>
 #include <pico/multicore.h>
 
 #include "tusb.h"
@@ -94,9 +95,9 @@ const std::string UserSettings::DRIVER_TYPE_KEY()
     return std::string("driver_type");
 }
 
-const std::string UserSettings::FIRMWARE_VER_KEY()
+const std::string UserSettings::DATETIME_KEY()
 {
-    return std::string("firmware_ver");
+    return std::string("datetime");
 }
 
 DeviceDriverType UserSettings::DEFAULT_DRIVER()
@@ -201,10 +202,10 @@ bool UserSettings::store_profile_and_driver_type(DeviceDriverType new_driver_typ
 
     board_api::usb::disconnect_all();
 
+    nvs_tool_.write(DRIVER_TYPE_KEY(), reinterpret_cast<const uint8_t*>(&new_driver_type), sizeof(new_driver_type));
     nvs_tool_.write(ACTIVE_PROFILE_KEY(index), &profile.id, sizeof(uint8_t));
     nvs_tool_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile));
-    nvs_tool_.write(DRIVER_TYPE_KEY(), &new_driver_type, sizeof(uint8_t));
-
+    
     board_api::reboot();
     
     return true;
@@ -298,22 +299,21 @@ DeviceDriverType UserSettings::get_current_driver()
     return current_driver_;
 }
 
-bool UserSettings::verify_firmware_version()
+void UserSettings::write_datetime()
 {
-    std::string fw_version = FIRMWARE_VERSION;
-    char read_fw_version[fw_version.size()];
-    std::fill(read_fw_version, read_fw_version + fw_version.size(), '\0');
-
-    nvs_tool_.read(FIRMWARE_VER_KEY(), read_fw_version, fw_version.size());
-
-    return (std::memcmp(read_fw_version, fw_version.c_str(), fw_version.size()) == 0);
+    nvs_tool_.write(DATETIME_KEY(), DATETIME_TAG.c_str(), DATETIME_TAG.size() + 1);
 }
 
-bool UserSettings::write_firmware_version()
+bool UserSettings::verify_datetime()
 {
-    std::string fw_version = FIRMWARE_VERSION;
-    nvs_tool_.write(FIRMWARE_VER_KEY(), fw_version.c_str(), fw_version.size());
-    return verify_firmware_version();
+    char read_dt_tag[DATETIME_TAG.size() + 1] = {0};
+
+    if (!nvs_tool_.read(DATETIME_KEY(), read_dt_tag, sizeof(read_dt_tag)) ||
+        (std::strcmp(read_dt_tag, DATETIME_TAG.c_str()) != 0))
+    {
+        return false;
+    }
+    return true;
 }
 
 //Checks for first boot and initializes user profiles, call before tusb is inited.
@@ -326,16 +326,13 @@ void UserSettings::initialize_flash()
 
     if (read_init_flag == FLASH_INIT_FLAG)
     {
-        OGXM_LOG("Flash already initialized\n");
-
-        if (!verify_firmware_version())
-        {
-            OGXM_LOG("Firmware version mismatch, writing new version\n");
-            write_firmware_version();
-        }
+        OGXM_LOG("Flash already initialized: %i\n", read_init_flag);
         return;
     }
 
+    OGXM_LOG("Flash not initialized, erasing\n");
+    nvs_tool_.erase_all();
+
     OGXM_LOG("Writing default driver\n");
 
     uint8_t device_mode_buffer = static_cast<uint8_t>(DEFAULT_DRIVER());
@@ -352,11 +349,14 @@ void UserSettings::initialize_flash()
     OGXM_LOG("Writing default profiles\n");
 
     {
-        UserProfile profile = UserProfile();
+        UserProfile profile;
+        OGXM_LOG("Profile size: %i\n", sizeof(UserProfile));
+
         for (uint8_t i = 0; i < MAX_PROFILES; i++)
         {
             profile.id = i + 1;
             nvs_tool_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile));
+            OGXM_LOG("Profile " + std::to_string(profile.id) + " written\n");
         }
     }
 
@@ -365,7 +365,5 @@ void UserSettings::initialize_flash()
     uint8_t init_flag_buffer = FLASH_INIT_FLAG;
     nvs_tool_.write(INIT_FLAG_KEY(), &init_flag_buffer, sizeof(uint8_t));
 
-    write_firmware_version();
-
     OGXM_LOG("Flash initialized\n");
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/UserSettings.h b/Firmware/RP2040/src/UserSettings/UserSettings.h
index 8d6e83a..f65c4f3 100644
--- a/Firmware/RP2040/src/UserSettings/UserSettings.h
+++ b/Firmware/RP2040/src/UserSettings/UserSettings.h
@@ -8,7 +8,7 @@
 #include "USBDevice/DeviceDriver/DeviceDriverTypes.h"
 #include "UserSettings/UserProfile.h"
 #include "UserSettings/NVSTool.h"
-#include "Gamepad.h"
+#include "Gamepad/Gamepad.h"
 
 /* Only write/store flash from Core0 */
 class UserSettings
@@ -25,10 +25,9 @@ public:
 
     void initialize_flash();
 
-    bool verify_firmware_version();
-    bool write_firmware_version();
-
     bool is_valid_driver(DeviceDriverType driver);
+    bool verify_datetime();
+    void write_datetime();
 
     DeviceDriverType get_current_driver();
     bool check_for_driver_change(Gamepad& gamepad);
@@ -48,7 +47,8 @@ private:
     UserSettings& operator=(const UserSettings&) = delete;
 
     static constexpr uint8_t GP_CHECK_COUNT = 3000 / GP_CHECK_DELAY_MS;
-    static constexpr uint8_t FLASH_INIT_FLAG = 0x21;
+    static constexpr uint8_t FLASH_INIT_FLAG = 0xF8;
+    const std::string DATETIME_TAG = BUILD_DATETIME; 
     
     NVSTool& nvs_tool_{NVSTool::get_instance()};
     DeviceDriverType current_driver_{DeviceDriverType::NONE};
@@ -58,7 +58,7 @@ private:
     const std::string PROFILE_KEY(const uint8_t profile_id);
     const std::string ACTIVE_PROFILE_KEY(const uint8_t index);
     const std::string DRIVER_TYPE_KEY();
-    const std::string FIRMWARE_VER_KEY();
+    const std::string DATETIME_KEY();
 };
 
 #endif // _USER_SETTINGS_H_
\ No newline at end of file
diff --git a/Firmware/cmake/init_submodules.cmake b/Firmware/cmake/init_submodules.cmake
index 468514d..d7ff985 100644
--- a/Firmware/cmake/init_submodules.cmake
+++ b/Firmware/cmake/init_submodules.cmake
@@ -1,18 +1,20 @@
-function(init_git_submodules EXTERNAL_DIR)
+function(init_git_submodules EXTERNAL_DIR SUBMODULE_PATHS)
     set(REPO_DIR "${EXTERNAL_DIR}/../../")
     message(STATUS "Initializing submodules in ${REPO_DIR}")
 
-    execute_process(
-        COMMAND git submodule update --init --recursive
-        WORKING_DIRECTORY ${REPO_DIR}
-        RESULT_VARIABLE INIT_SUBMODULES_RESULT
-        OUTPUT_VARIABLE INIT_SUBMODULES_OUTPUT
-        ERROR_VARIABLE INIT_SUBMODULES_ERROR
-    )
+    foreach(SUBMODULE_PATH ${SUBMODULE_PATHS})
+        execute_process(
+            COMMAND git submodule update --init --recursive ${SUBMODULE_PATH}
+            WORKING_DIRECTORY ${REPO_DIR}
+            RESULT_VARIABLE INIT_SUBMODULE_RESULT
+            OUTPUT_VARIABLE INIT_SUBMODULE_OUTPUT
+            ERROR_VARIABLE INIT_SUBMODULE_ERROR
+        )
 
-    if(INIT_SUBMODULES_RESULT EQUAL 0)
-        message(STATUS "Subnmodules initialized successfully.")
-    else()
-        message(FATAL_ERROR "Failed to initialize submodules: ${INIT_SUBMODULES_ERROR}")
-    endif()
+        if(INIT_SUBMODULE_RESULT EQUAL 0)
+            message(STATUS "Submodules initialized successfully.")
+        else()
+            message(FATAL_ERROR "Failed to initialize submodules: ${INIT_SUBMODULES_ERROR}")
+        endif()
+    endforeach()
 endfunction()
\ No newline at end of file
diff --git a/Firmware/external/libfixmath b/Firmware/external/libfixmath
new file mode 160000
index 0000000..d308e46
--- /dev/null
+++ b/Firmware/external/libfixmath
@@ -0,0 +1 @@
+Subproject commit d308e466e1a09118d03f677c52e5fbf402f6fdd0
diff --git a/README.md b/README.md
index a6bd64f..79aaa49 100644
--- a/README.md
+++ b/README.md
@@ -3,7 +3,7 @@
 
 Firmware for the RP2040, capable of emulating gamepads for several game consoles. The firmware comes in many flavors, supported on the [Adafruit Feather USB Host board](https://www.adafruit.com/product/5723), Pi Pico, Pi Pico 2, Pi Pico W, Pi Pico 2 W, Waveshare RP2040-Zero, Pico/ESP32 hybrid, and a 4-Channel RP2040-Zero setup.
 
-[**Visit the web app here**](https://wiredopposite.github.io/OGX-Mini-WebApp/) to change your mappings and deadzone settings. To pair the OGX-Mini with the web app, plug your controller in, then connect it to your PC, hold **Start + Left Bumper + Right Bumper** to enter web app mode. Click "Connect" in the web app and select the OGX-Mini.
+[**Visit the web app here**](https://wiredopposite.github.io/OGX-Mini-WebApp/) to change your mappings and deadzone settings. To pair the OGX-Mini with the web app via USB, plug your controller in, then connect it to your PC, hold **Start + Left Bumper + Right Bumper** to enter web app mode. Click "Connect via USB" in the web app and select the OGX-Mini. You can also pair via Bluetooth, no extra steps are needed in that case. 
 
 ## Supported platforms
 - Original Xbox
@@ -72,21 +72,20 @@ Note: There are some third party controllers that can change their VID/PID, thes
 Please visit [**this page**](https://bluepad32.readthedocs.io/en/latest/supported_gamepads/) for a more comprehensive list of supported controllers and Bluetooth pairing instructions.
 
 ## Features new to v1.0.0
-- Bluetooth functionality for the Pico W and Pico+ESP32.
+- Bluetooth functionality for the Pico W, Pico 2 W, and Pico+ESP32.
 - Web application (connectable via USB or Bluetooth) for configuring deadzones and buttons mappings, supports up to 8 saved profiles.
 - Pi Pico 2 and Pico 2 W (RP2350) support.
-- Reduced latency by about 3-4 ms, graphs showing comparisons are coming
+- Reduced latency by about 3-4 ms, graphs showing comparisons are coming.
 - 4 channel functionality, connect 4 Picos and use one Xbox 360 wireless adapter to control all 4.
 - Delayed USB mount until a controller is plugged in, useful for internal installation (non-Bluetooth boards only). 
 - Generic HID controller support.
 - Dualshock 3 emulation (minus gyros), rumble now works.
 - Steel Battalion controller emulation with a wireless Xbox 360 chatpad.
-- Xbox DVD dongle emulation. You must provide or dump your own firmware, see the Tools directory.
+- Xbox DVD dongle emulation. You must provide or dump your own dongle firmware, see the Tools directory.
 - Analog button support on OG Xbox and PS3.
 - RGB LED support for RP2040-Zero and Adafruit Feather boards.
 
 ## Planned additions
-- Anti-deadzone settings
 - More accurate report parser for unknown HID controllers
 - Hardware design for internal OG Xbox install
 - Hardware design for 4 channel RP2040-Zero adapter
@@ -95,6 +94,7 @@ Please visit [**this page**](https://bluepad32.readthedocs.io/en/latest/supporte
 - Switch (as input) rumble support
 - OG Xbox communicator support (in some form)
 - Generic bluetooth dongle support
+- Button macros
 
 ## Hardware
 For Pi Pico, RP2040-Zero, 4 channel, and ESP32 configurations, please see the hardware folder for diagrams.
diff --git a/WebApp b/WebApp
index 0e572ef..79b0077 160000
--- a/WebApp
+++ b/WebApp
@@ -1 +1 @@
-Subproject commit 0e572eff08b25950c06f77fb9c8eac71d9f79a48
+Subproject commit 79b00778da0e7be925d055884ea89c3168d587f0
diff --git a/images/WebAppPreview.png b/images/WebAppPreview.png
new file mode 100644
index 0000000..996d10c
Binary files /dev/null and b/images/WebAppPreview.png differ