From f0746153cf134ca976e7df6069cd7378780b2ccc Mon Sep 17 00:00:00 2001 From: Dalton-V Date: Tue, 30 Jun 2026 14:06:00 -0500 Subject: [PATCH 1/5] Support new gyro protocol --- .../NintendoSwitch_OemController_State.cpp | 2 +- .../NintendoSwitch_OemController_State.h | 98 ++++++- .../PABotBase2_Controller_NS1_OemController.h | 10 + .../PABotBase2/PABotBase2_MessageProtocol.h | 2 +- .../ComputerPrograms_UnitTestRunner.cpp | 3 + .../Joycon/NintendoSwitch_Joycon.h | 36 +-- ...NintendoSwitch_ControllerWithScheduler.cpp | 36 ++- .../NintendoSwitch_ControllerWithScheduler.h | 97 ++----- .../Controllers/NintendoSwitch_Gyro.cpp | 140 ++++++++++ .../Controllers/NintendoSwitch_Gyro.h | 80 ++++++ .../NintendoSwitch_GyroQuaternion.cpp | 107 +++++++ .../NintendoSwitch_GyroQuaternion.h | 39 +++ .../Controllers/NintendoSwitch_GyroTests.cpp | 263 ++++++++++++++++++ .../Controllers/NintendoSwitch_GyroTests.h | 18 ++ .../NintendoSwitch_PABotBase2_Joycon.h | 99 ++----- ...intendoSwitch_PABotBase2_OemController.cpp | 151 +++++----- .../NintendoSwitch_PABotBase2_OemController.h | 18 +- .../NintendoSwitch_PABotBase2_ProController.h | 71 +---- ...intendoSwitch_PABotBase2_WiredController.h | 43 --- .../Procon/NintendoSwitch_ProController.h | 32 --- .../SysbotBase/SysbotBase3_ProController.h | 43 --- .../DevPrograms/JoyconGyroTester.cpp | 95 +++++++ .../DevPrograms/JoyconGyroTester.h | 37 +++ .../DevPrograms/TestProgramSwitch.cpp | 1 - .../NintendoSwitch/NintendoSwitch_Panels.cpp | 2 + SerialPrograms/cmake/SourceFiles.cmake | 20 +- 26 files changed, 1066 insertions(+), 477 deletions(-) create mode 100644 SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.cpp create mode 100644 SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.h create mode 100644 SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroQuaternion.cpp create mode 100644 SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroQuaternion.h create mode 100644 SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp create mode 100644 SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.h create mode 100644 SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp create mode 100644 SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h diff --git a/Common/ControllerStates/NintendoSwitch_OemController_State.cpp b/Common/ControllerStates/NintendoSwitch_OemController_State.cpp index a6b885ff9a..9cb3c9707b 100644 --- a/Common/ControllerStates/NintendoSwitch_OemController_State.cpp +++ b/Common/ControllerStates/NintendoSwitch_OemController_State.cpp @@ -27,7 +27,7 @@ extern const OemController_State0x3f OemController_State0x3f_NEUTRAL_STATE = { .right_joystick = {0x00, 0x80, 0x00, 0x80}, }; -const OemController_State0x30 OemController_State0x30_NEUTRAL_STATE = { +const OemController_State0x30 OemController_State0x30_NEUTRAL_STATE = { .buttons = { .button3 = 0, .button4 = 0, diff --git a/Common/ControllerStates/NintendoSwitch_OemController_State.h b/Common/ControllerStates/NintendoSwitch_OemController_State.h index 8939d735f0..703ca4e8db 100644 --- a/Common/ControllerStates/NintendoSwitch_OemController_State.h +++ b/Common/ControllerStates/NintendoSwitch_OemController_State.h @@ -9,6 +9,10 @@ #include #include +#include +#include "string.h" + +//#define PACKED __attribute__((packed)) namespace PokemonAutomation{ namespace NintendoSwitch{ @@ -56,6 +60,25 @@ struct OemController_State0x30_Buttons{ uint8_t vibrator; }; +static inline void write_bits(uint8_t* raw, int bit, int len, uint32_t value) { + uint64_t mask = ((1ULL << len) - 1); + value &= (uint32_t)mask; + int byte = bit / 8; + int shift = bit % 8; + // Touch only the bytes covered by this field, including at the packet end. + const int byte_count = (shift + len + 7) / 8; + uint64_t existing = 0; + for (int i = 0; i < byte_count; i++) + existing |= (uint64_t)raw[byte + i] << (i * 8); + existing &= ~(mask << shift); + existing |= ((uint64_t)value << shift); + for (int i = 0; i < byte_count; i++) + raw[byte + i] = (uint8_t)(existing >> (i * 8)); +} + + +#pragma pack(push, 1) + struct OemController_State0x30_Gyro{ int16_t accel_x; int16_t accel_y; @@ -65,17 +88,74 @@ struct OemController_State0x30_Gyro{ int16_t rotation_z; }; -struct OemController_State0x30_GyroX3{ - OemController_State0x30_Gyro time0; - OemController_State0x30_Gyro time1; - OemController_State0x30_Gyro time2; +struct OemController_State0x30_GyroQuaternion{ + uint8_t raw[36]; + + void set_quat_accel_x0(int16_t v) { memcpy(&raw[0], &v, 2); } + void set_quat_accel_y0(int16_t v) { memcpy(&raw[2], &v, 2); } + void set_quat_accel_z0(int16_t v) { memcpy(&raw[4], &v, 2); } + void set_quat_accel_x1(int16_t v) { memcpy(&raw[12], &v, 2); } + void set_quat_accel_y1(int16_t v) { memcpy(&raw[14], &v, 2); } + void set_quat_accel_z1(int16_t v) { memcpy(&raw[16], &v, 2); } + void set_quat_accel_x2(int16_t v) { memcpy(&raw[24], &v, 2); } + void set_quat_accel_y2(int16_t v) { memcpy(&raw[26], &v, 2); } + void set_quat_accel_z2(int16_t v) { memcpy(&raw[28], &v, 2); } + + void set_packing_mode(uint32_t v) { write_bits(raw, 48, 2, v); } + void set_max_index(uint32_t v) { write_bits(raw, 50, 2, v); } + void set_last_sample_0(uint32_t v) { write_bits(raw, 52, 21, v); } + void set_last_sample_1(uint32_t v) { + write_bits(raw, 73, 7, v & 0x7F); + write_bits(raw, 80, 14, v >> 7); + } + void set_last_sample_2(uint32_t v) { + write_bits(raw, 94, 2, v & 0x3); + write_bits(raw, 144, 19, v >> 2); + } + void set_delta_last_first_0(uint32_t v) { write_bits(raw, 163, 13, v); } + void set_delta_last_first_1(uint32_t v) { write_bits(raw, 176, 13, v); } + void set_delta_last_first_2(uint32_t v) { + write_bits(raw, 189, 3, v & 0x7); + write_bits(raw, 240, 10, v >> 3); + } + void set_delta_mid_avg_0(uint32_t v) { write_bits(raw, 250, 7, v); } + void set_delta_mid_avg_1(uint32_t v) { write_bits(raw, 257, 7, v); } + void set_delta_mid_avg_2(uint32_t v) { write_bits(raw, 264, 7, v); } + void set_timestamp_start(uint32_t v) { + write_bits(raw, 271, 1, v & 0x1); + write_bits(raw, 272, 10, v >> 1); + } + void set_timestamp_count(uint32_t v) { write_bits(raw, 282, 6, v); } + }; -struct OemController_State0x30{ - OemController_State0x30_Buttons buttons; - OemController_State0x30_GyroX3 gyro; +#pragma pack(pop) + +static_assert(sizeof(OemController_State0x30_Gyro) == 12, "Gyro struct must be 12 bytes"); +static_assert(sizeof(OemController_State0x30_GyroQuaternion) == 36, "Quaternion gyro struct must be 36 bytes"); + +struct OemController_State0x30_GyroX3{ + struct Samples{ + OemController_State0x30_Gyro time0; + OemController_State0x30_Gyro time1; + OemController_State0x30_Gyro time2; + }; + + union{ + Samples samples; + OemController_State0x30_GyroQuaternion quaternion; + }; }; +static_assert(sizeof(OemController_State0x30_GyroX3) == 36, "GyroX3 struct must be 36 bytes"); + +struct OemController_State0x30{ + OemController_State0x30_Buttons buttons; + OemController_State0x30_GyroX3 gyro; +}; + +static_assert(sizeof(OemController_State0x30) == 46, "Report 0x30 state must be 46 bytes"); + extern const OemController_State0x30 OemController_State0x30_NEUTRAL_STATE; bool OemController_State0x30_equals( @@ -97,6 +177,10 @@ struct OemController_Status{ uint8_t player_lights; uint8_t paired_mac_address[6]; + + // 0 = Standard + // 1 = Quaternion + //uint8_t gyro_protocol = 1; }; enum class DisconnectReason{ diff --git a/Common/PABotBase2/Controllers/PABotBase2_Controller_NS1_OemController.h b/Common/PABotBase2/Controllers/PABotBase2_Controller_NS1_OemController.h index 41c9312774..7f2c8ef711 100644 --- a/Common/PABotBase2/Controllers/PABotBase2_Controller_NS1_OemController.h +++ b/Common/PABotBase2/Controllers/PABotBase2_Controller_NS1_OemController.h @@ -56,6 +56,16 @@ struct Message_Command_NS1_OemController_FullState : MessageHeader{ }; +#define PABB2_MESSAGE_CMD_NS1_OEM_CONTROLLER_MOTION 0x99 +struct PABB_PACK Message_Command_NS1_OemController_Motion : MessageHeader{ + uint16_t milliseconds; + NintendoSwitch::OemController_State0x30_Buttons buttons; + int32_t acceleration_ug[3]; + int32_t angular_velocity_mdps[3]; +}; +static_assert(sizeof(Message_Command_NS1_OemController_Motion) == 40, "OEM motion message must be 40 bytes"); + + } } diff --git a/Common/PABotBase2/PABotBase2_MessageProtocol.h b/Common/PABotBase2/PABotBase2_MessageProtocol.h index e050a1c76a..8d83d6cea7 100644 --- a/Common/PABotBase2/PABotBase2_MessageProtocol.h +++ b/Common/PABotBase2/PABotBase2_MessageProtocol.h @@ -16,7 +16,7 @@ namespace PABotBase2{ -#define PABB2_MESSAGE_PROTOCOL_VERSION 2026061806 +#define PABB2_MESSAGE_PROTOCOL_VERSION 2026061807 struct MessageHeader{ diff --git a/SerialPrograms/Source/ComputerPrograms/ComputerPrograms_UnitTestRunner.cpp b/SerialPrograms/Source/ComputerPrograms/ComputerPrograms_UnitTestRunner.cpp index 9b48682bc1..1d74c40020 100644 --- a/SerialPrograms/Source/ComputerPrograms/ComputerPrograms_UnitTestRunner.cpp +++ b/SerialPrograms/Source/ComputerPrograms/ComputerPrograms_UnitTestRunner.cpp @@ -32,6 +32,8 @@ #include "NintendoSwitch/Controllers/SysbotBase/SysbotBase_Connection.h" #include "PokemonBDSP/Inference/PokemonBDSP_DialogDetector.h" #include "PokemonBDSP/Inference/PokemonBDSP_MenuDetector.h" +#include "NintendoSwitch/Controllers/NintendoSwitch_GyroTests.h" +#include "UnitTestRunner.h" #include "VideoGames/PokemonHome/PokemonHome_Tests.h" #include "VideoGames/PokemonSwSh/PokemonSwSh_Tests.h" @@ -76,6 +78,7 @@ UnitTestDatabase make_UNIT_TESTS_ALL(){ NintendoSwitch::Typewriter::add_tests_TypewriterTextMapping(ret); NintendoSwitch::PokemonBDSP::add_tests_DialogDetector(ret); NintendoSwitch::PokemonBDSP::add_tests_MenuDetector(ret); + NintendoSwitch::add_tests_Gyro(ret); NintendoSwitch::PokemonFRLG::add_tests(ret); NintendoSwitch::PokemonRSE::add_tests(ret); NintendoSwitch::PokemonHome::add_tests(ret); diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h b/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h index 64bcc48a85..1e79bacf69 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h @@ -12,6 +12,7 @@ #include "Controllers/ControllerTypes.h" #include "Controllers/Controller.h" #include "NintendoSwitch/Controllers/NintendoSwitch_ControllerButtons.h" +#include "NintendoSwitch/Controllers/NintendoSwitch_Gyro.h" namespace PokemonAutomation{ @@ -96,36 +97,13 @@ class JoyconController : public AbstractController{ const JoystickPosition& position ) = 0; - // Gyro: Accelerometer (experimental - API subject to change) - virtual void issue_gyro_accel_x( + // Issue a complete physical gyro trajectory. The callback is evaluated + // synchronously while this command is constructed and is never retained. + // A non-positive duration is a no-op. + virtual void issue_gyro_motion( Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - virtual void issue_gyro_accel_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - virtual void issue_gyro_accel_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - virtual void issue_gyro_rotate_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - virtual void issue_gyro_rotate_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - virtual void issue_gyro_rotate_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value + Milliseconds duration, + const GyroFunction& function ) = 0; // diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp index d1e3f316bb..a558a26edd 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp @@ -7,6 +7,7 @@ * */ +#include #include "Common/Cpp/PrettyPrint.h" #include "NintendoSwitch_ControllerWithScheduler.h" @@ -152,32 +153,39 @@ void ControllerWithScheduler::issue_right_joystick( -void ControllerWithScheduler::issue_gyro( +void ControllerWithScheduler::issue_gyro_motion( Cancellable* cancellable, - SwitchResource id, const char* name, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value + ControllerClass controller_class, + Milliseconds duration, + const GyroFunction& function ){ + const std::vector samples = Internal::sample_gyro_motion( + cancellable, controller_class, duration, function + ); + if (samples.empty()){ + return; + } + SuperscalarScheduler::Schedule schedule; - std::lock_guard lg0(m_issue_lock); + std::unique_lock issue_lock(m_issue_lock); { std::lock_guard lg1(m_state_lock); if (cancellable){ cancellable->throw_if_cancelled(); } - m_scheduler.issue_to_resource( - schedule, - std::make_unique(id, value), - delay, hold, cooldown - ); + for (const Internal::GyroMotionSample& sample : samples){ + m_scheduler.issue_to_resource( + schedule, + std::make_unique(sample.state), + WallDuration::zero(), sample.duration, WallDuration::zero() + ); + } } execute_schedule(cancellable, schedule); if (m_logging_throttler){ m_logger.log( - std::string(name) + "(): (" + std::to_string(value) + ")" + - ", delay = " + std::to_string(delay.count()) + "ms" + - ", hold = " + std::to_string(hold.count()) + "ms" + - ", cooldown = " + std::to_string(cooldown.count()) + "ms", + "issue_gyro_motion(): duration = " + std::to_string(duration.count()) + + "ms, samples = " + std::to_string(samples.size()), COLOR_DARKGREEN ); } diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.h b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.h index 0103b0f1b2..34224ee6c4 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.h @@ -10,10 +10,11 @@ #ifndef PokemonAutomation_NintendoSwitch_ControllerWithScheduler_H #define PokemonAutomation_NintendoSwitch_ControllerWithScheduler_H -#include "Common/Cpp/CancellableScope.h" -#include "Controllers/Joystick.h" -#include "Controllers/Schedulers/ControllerWithScheduler.h" -#include "NintendoSwitch_ControllerButtons.h" +#include "Common/Cpp/CancellableScope.h" +#include "Controllers/Joystick.h" +#include "Controllers/Schedulers/ControllerWithScheduler.h" +#include "NintendoSwitch_ControllerButtons.h" +#include "NintendoSwitch_Gyro.h" //#include //using std::cout; @@ -23,13 +24,13 @@ namespace PokemonAutomation{ namespace NintendoSwitch{ -struct SwitchControllerState{ +struct SwitchControllerState{ Button buttons = BUTTON_NONE; DpadPosition dpad = DpadPosition::DPAD_NONE; JoystickPosition left_joystick; JoystickPosition right_joystick; - uint16_t gyro[6] = {}; + GyroState gyro; }; @@ -65,12 +66,7 @@ enum class SwitchResource{ JOYSTICK_LEFT, JOYSTICK_RIGHT, - GYRO_ACCEL_X, - GYRO_ACCEL_Y, - GYRO_ACCEL_Z, - GYRO_ROTATE_X, - GYRO_ROTATE_Y, - GYRO_ROTATE_Z, + GYRO, }; class SwitchCommand : public SchedulerResource{ @@ -120,18 +116,17 @@ struct SwitchCommand_RightJoystick : public SwitchCommand{ state.right_joystick = position; } }; -struct SwitchCommand_Gyro : public SwitchCommand{ - int16_t value; - - SwitchCommand_Gyro(SwitchResource id, int16_t value) - : SwitchCommand((size_t)id) - , value(value) - {} - virtual void apply(SwitchControllerState& state) const override{ - size_t index = (ButtonFlagType)id - (ButtonFlagType)SwitchResource::GYRO_ACCEL_X; - state.gyro[index] = value; - } -}; +struct SwitchCommand_GyroState : public SwitchCommand{ + GyroState state; + + SwitchCommand_GyroState(const GyroState& state) + : SwitchCommand((size_t)SwitchResource::GYRO) + , state(state) + {} + virtual void apply(SwitchControllerState& controller_state) const override{ + controller_state.gyro = state; + } +}; @@ -198,54 +193,12 @@ class ControllerWithScheduler : public PokemonAutomation::ControllerWithSchedule const JoystickPosition& position ); - void issue_gyro( - Cancellable* cancellable, - SwitchResource id, const char* name, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ); - void issue_gyro_accel_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ){ - issue_gyro(cancellable, SwitchResource::GYRO_ACCEL_X, "issue_gyro_accel_x", delay, hold, cooldown, value); - } - void issue_gyro_accel_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ){ - issue_gyro(cancellable, SwitchResource::GYRO_ACCEL_Y, "issue_gyro_accel_y", delay, hold, cooldown, value); - } - void issue_gyro_accel_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ){ - issue_gyro(cancellable, SwitchResource::GYRO_ACCEL_Z, "issue_gyro_accel_z", delay, hold, cooldown, value); - } - void issue_gyro_rotate_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ){ - issue_gyro(cancellable, SwitchResource::GYRO_ROTATE_X, "issue_gyro_rotate_x", delay, hold, cooldown, value); - } - void issue_gyro_rotate_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ){ - issue_gyro(cancellable, SwitchResource::GYRO_ROTATE_Y, "issue_gyro_rotate_y", delay, hold, cooldown, value); - } - void issue_gyro_rotate_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ){ - issue_gyro(cancellable, SwitchResource::GYRO_ROTATE_Z, "issue_gyro_rotate_z", delay, hold, cooldown, value); - } + void issue_gyro_motion( + Cancellable* cancellable, + ControllerClass controller_class, + Milliseconds duration, + const GyroFunction& function + ); void issue_full_controller_state( Cancellable* cancellable, diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.cpp new file mode 100644 index 0000000000..fc0d9cc3b3 --- /dev/null +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.cpp @@ -0,0 +1,140 @@ +/* Nintendo Switch Gyro Motion + * + * From: https://github.com/PokemonAutomation/ + */ + +#include +#include +#include +#include +#include +#include "Common/Cpp/CancellableScope.h" +#include "NintendoSwitch_Gyro.h" + +namespace PokemonAutomation{ +namespace NintendoSwitch{ + +namespace{ + +bool is_finite(const GyroVector& vector){ + return std::isfinite(vector.x) && std::isfinite(vector.y) && std::isfinite(vector.z); +} + +GyroVector transform_vector(ControllerClass controller_class, const GyroVector& vector){ + switch (controller_class){ + case ControllerClass::NintendoSwitch_LeftJoycon: + case ControllerClass::NintendoSwitch_ProController: + return vector; + case ControllerClass::NintendoSwitch_RightJoycon: + // This is a 180-degree rotation about X, so the basis remains right-handed. + return {vector.x, -vector.y, -vector.z}; + default: + throw std::invalid_argument("Gyro motion is unsupported for this controller class."); + } +} + +} + + +bool is_finite(const GyroState& state){ + return is_finite(state.acceleration_g) && is_finite(state.angular_velocity_dps); +} +GyroState transform_gyro_state(ControllerClass controller_class, const GyroState& state){ + return { + transform_vector(controller_class, state.acceleration_g), + transform_vector(controller_class, state.angular_velocity_dps), + }; +} +GyroVector interpolate(const GyroVector& from, const GyroVector& to, double progress){ + return { + from.x + (to.x - from.x) * progress, + from.y + (to.y - from.y) * progress, + from.z + (to.z - from.z) * progress, + }; +} +GyroState interpolate(const GyroState& from, const GyroState& to, double progress){ + return { + interpolate(from.acceleration_g, to.acceleration_g, progress), + interpolate(from.angular_velocity_dps, to.angular_velocity_dps, progress), + }; +} + + +namespace Internal{ + +int32_t gyro_transport_value(double value, double scale, const char* label){ + if (!std::isfinite(value)){ + throw std::invalid_argument(std::string(label) + " must be finite."); + } + const double scaled = value * scale; + if (scaled < static_cast(std::numeric_limits::min()) + || scaled > static_cast(std::numeric_limits::max())){ + throw std::invalid_argument(std::string(label) + " is outside the firmware transport range."); + } + return static_cast(std::round(scaled)); +} + +namespace{ + +void validate_transport_state(const GyroState& state){ + gyro_transport_value(state.acceleration_g.x, GYRO_ACCEL_TRANSPORT_SCALE, "Gyro acceleration X"); + gyro_transport_value(state.acceleration_g.y, GYRO_ACCEL_TRANSPORT_SCALE, "Gyro acceleration Y"); + gyro_transport_value(state.acceleration_g.z, GYRO_ACCEL_TRANSPORT_SCALE, "Gyro acceleration Z"); + gyro_transport_value(state.angular_velocity_dps.x, GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE, "Gyro angular velocity X"); + gyro_transport_value(state.angular_velocity_dps.y, GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE, "Gyro angular velocity Y"); + gyro_transport_value(state.angular_velocity_dps.z, GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE, "Gyro angular velocity Z"); +} + +} + +std::vector sample_gyro_motion( + Cancellable* cancellable, + ControllerClass controller_class, + Milliseconds duration, + const GyroFunction& function +){ + if (duration <= Milliseconds::zero()){ + return {}; + } + if (!function){ + throw std::invalid_argument("Gyro motion function must not be empty."); + } + + // Validate the geometry even if the callback would otherwise return no + // interesting values. Unsupported classes must never silently use identity. + transform_gyro_state(controller_class, {}); + + std::vector samples; + const auto duration_count = duration.count(); + const auto interval_count = GYRO_REPORT_INTERVAL.count(); + const auto sample_count = duration_count / interval_count + (duration_count % interval_count != 0); + if (static_cast(sample_count) > samples.max_size()){ + throw std::length_error("Gyro motion duration requires too many samples."); + } + samples.reserve(static_cast(sample_count)); + Milliseconds elapsed = Milliseconds::zero(); + while (elapsed < duration){ + if (cancellable){ + cancellable->throw_if_cancelled(); + } + + const Milliseconds chunk = std::min(GYRO_REPORT_INTERVAL, duration - elapsed); + elapsed += chunk; + const GyroState canonical = function(elapsed); + if (!is_finite(canonical)){ + throw std::invalid_argument("Gyro motion values must be finite."); + } + GyroState transformed = transform_gyro_state(controller_class, canonical); + validate_transport_state(transformed); + samples.emplace_back(GyroMotionSample{ + chunk, + transformed, + }); + } + return samples; +} + + +} +} +} diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.h b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.h new file mode 100644 index 0000000000..34c921a6fd --- /dev/null +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.h @@ -0,0 +1,80 @@ +/* Nintendo Switch Gyro Motion + * + * From: https://github.com/PokemonAutomation/ + * + */ + +#ifndef PokemonAutomation_NintendoSwitch_Gyro_H +#define PokemonAutomation_NintendoSwitch_Gyro_H + +#include +#include +#include +#include "Common/Cpp/Time.h" +#include "Controllers/ControllerTypes.h" + +namespace PokemonAutomation{ + +class Cancellable; + +namespace NintendoSwitch{ + + +struct GyroVector{ + double x = 0; + double y = 0; + double z = 0; +}; + +// Canonical controller frame: +// +X points toward the trigger/shoulder-button edge. +// +Y points toward the controller's left when looking at its controls. +// +Z points out through the controls. +// Positive angular velocity follows the right-hand rule. Acceleration is +// sensor acceleration in g; gravity is not synthesized by this API. This is +// a fixed logical input convention, not a world-space reference frame. +struct GyroState{ + GyroVector acceleration_g; + GyroVector angular_velocity_dps; +}; + +using GyroFunction = std::function; + +constexpr Milliseconds GYRO_REPORT_INTERVAL{15}; + +bool is_finite(const GyroState& state); +GyroState transform_gyro_state(ControllerClass controller_class, const GyroState& state); +GyroVector interpolate(const GyroVector& from, const GyroVector& to, double progress); +GyroState interpolate(const GyroState& from, const GyroState& to, double progress); + + +namespace Internal{ + +constexpr double GYRO_ACCEL_TRANSPORT_SCALE = 1000000.0; +constexpr double GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE = 1000.0; + +// Converts a program-facing physical value to the signed transport unit used +// by the firmware protocol. Sampling calls this for every component so a +// complete trajectory is validated before any scheduler state is changed. +int32_t gyro_transport_value(double value, double scale, const char* label); + +// Internal representation used while a trajectory is being validated before +// it reaches the scheduler. It intentionally contains no raw sensor counts. +struct GyroMotionSample{ + Milliseconds duration; + GyroState state; +}; + +std::vector sample_gyro_motion( + Cancellable* cancellable, + ControllerClass controller_class, + Milliseconds duration, + const GyroFunction& function +); + +} + + +} +} +#endif diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroQuaternion.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroQuaternion.cpp new file mode 100644 index 0000000000..b7960bb8b0 --- /dev/null +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroQuaternion.cpp @@ -0,0 +1,107 @@ +/* Nintendo Switch Gyro Quaternion Helpers + * + * From: https://github.com/PokemonAutomation/ + */ + +#include +#include +#include +#include "NintendoSwitch_GyroQuaternion.h" + +namespace PokemonAutomation{ +namespace NintendoSwitch{ + +namespace{ + +bool is_finite(const GyroQuaternion& quaternion){ + return std::isfinite(quaternion.x) && std::isfinite(quaternion.y) + && std::isfinite(quaternion.z) && std::isfinite(quaternion.w); +} +GyroQuaternion multiply(const GyroQuaternion& left, const GyroQuaternion& right){ + return { + left.w * right.x + left.x * right.w + left.y * right.z - left.z * right.y, + left.w * right.y + left.y * right.w + left.z * right.x - left.x * right.z, + left.w * right.z + left.z * right.w + left.x * right.y - left.y * right.x, + left.w * right.w - left.x * right.x - left.y * right.y - left.z * right.z, + }; +} +GyroQuaternion normalize(const GyroQuaternion& quaternion){ + const double norm = std::sqrt( + quaternion.x * quaternion.x + quaternion.y * quaternion.y + + quaternion.z * quaternion.z + quaternion.w * quaternion.w + ); + if (!std::isfinite(norm) || norm == 0){ + throw std::invalid_argument("Gyro quaternion is not normalizable."); + } + GyroQuaternion normalized{ + quaternion.x / norm, quaternion.y / norm, + quaternion.z / norm, quaternion.w / norm, + }; + if (!is_finite(normalized)){ + throw std::invalid_argument("Gyro quaternion integration produced a non-finite value."); + } + return normalized; +} + +} + + +GyroQuaternion integrate_angular_velocity( + const GyroQuaternion& current, + const GyroVector& angular_velocity_dps, + Milliseconds duration +){ + if (!is_finite(current) || !std::isfinite(angular_velocity_dps.x) + || !std::isfinite(angular_velocity_dps.y) || !std::isfinite(angular_velocity_dps.z) + || duration < Milliseconds::zero()){ + throw std::invalid_argument("Gyro quaternion integration requires finite values and a non-negative duration."); + } + + const double seconds = static_cast(duration.count()) / 1000.0; + const double scale = std::numbers::pi / 180.0 * seconds; + const double x = angular_velocity_dps.x * scale; + const double y = angular_velocity_dps.y * scale; + const double z = angular_velocity_dps.z * scale; + const double angle = std::sqrt(x * x + y * y + z * z); + const double vector_scale = angle == 0 ? 0.5 : std::sin(angle / 2.0) / angle; + const GyroQuaternion delta{x * vector_scale, y * vector_scale, z * vector_scale, std::cos(angle / 2.0)}; + return normalize(multiply(current, delta)); +} + +void pack_quaternion( + OemController_State0x30_GyroQuaternion& output, + const GyroQuaternion& orientation, + uint64_t timestamp_ms +){ + if (!is_finite(orientation)){ + throw std::invalid_argument("Cannot pack a non-finite gyro quaternion."); + } + + // Packing assumes unit components. Normalize here so callers cannot reach + // the narrowing conversion below with a finite but out-of-range value. + const GyroQuaternion unit = normalize(orientation); + const double values[] = {unit.x, unit.y, unit.z, unit.w}; + int max_index = 0; + for (int i = 1; i < 4; ++i){ + if (std::fabs(values[i]) > std::fabs(values[max_index])){ + max_index = i; + } + } + + output.set_packing_mode(2); + output.set_max_index(max_index); + const double sign = values[max_index] < 0 ? -1.0 : 1.0; + int32_t components[3]; + for (int i = 0; i < 3; ++i){ + components[i] = static_cast(values[(max_index + i + 1) & 3] * 0x40000000 * sign); + } + output.set_last_sample_0(components[0] >> 10); + output.set_last_sample_1(components[1] >> 10); + output.set_last_sample_2(components[2] >> 10); + output.set_timestamp_start(timestamp_ms & 0x7ff); + output.set_timestamp_count(3); +} + + +} +} diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroQuaternion.h b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroQuaternion.h new file mode 100644 index 0000000000..9d20d1930f --- /dev/null +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroQuaternion.h @@ -0,0 +1,39 @@ +/* Nintendo Switch Gyro Quaternion Helpers + * + * From: https://github.com/PokemonAutomation/ + */ + +#ifndef PokemonAutomation_NintendoSwitch_GyroQuaternion_H +#define PokemonAutomation_NintendoSwitch_GyroQuaternion_H + +#include +#include "Common/ControllerStates/NintendoSwitch_OemController_State.h" +#include "NintendoSwitch_Gyro.h" + +namespace PokemonAutomation{ +namespace NintendoSwitch{ + + +struct GyroQuaternion{ + double x = 0; + double y = 0; + double z = 0; + double w = 1; +}; + +GyroQuaternion integrate_angular_velocity( + const GyroQuaternion& current, + const GyroVector& angular_velocity_dps, + Milliseconds duration +); + +void pack_quaternion( + OemController_State0x30_GyroQuaternion& output, + const GyroQuaternion& orientation, + uint64_t timestamp_ms +); + + +} +} +#endif diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp new file mode 100644 index 0000000000..bf6c7edec0 --- /dev/null +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp @@ -0,0 +1,263 @@ +/* Nintendo Switch Gyro Tests + * + * From: https://github.com/PokemonAutomation/ + */ + +#include +#include +#include +#include "NintendoSwitch_Gyro.h" +#include "NintendoSwitch_GyroQuaternion.h" +#include "NintendoSwitch_GyroTests.h" + +namespace PokemonAutomation{ +namespace NintendoSwitch{ + +namespace{ + +class Test_GyroSampling : public UnitTest{ +public: + Test_GyroSampling() + : UnitTest("NintendoSwitch::Gyro - sampling") + {} + + virtual UnitTestResult run(Logger&, CancellableScope&) const override{ + using namespace std::chrono_literals; + size_t calls = 0; + const GyroFunction function = [&calls](Milliseconds elapsed){ + ++calls; + return GyroState{{(double)elapsed.count(), 0, 0}, {0, 0, 0}}; + }; + if (!Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 0ms, function).empty()){ + return "0ms gyro motion generated a sample."; + } + if (!Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, -1ms, function).empty()){ + return "Negative gyro motion generated a sample."; + } + if (calls != 0){ + return "A non-positive gyro motion invoked its callback."; + } + + const auto one = Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 1ms, function); + if (one.size() != 1 || one[0].duration != 1ms || one[0].state.acceleration_g.x != 1){ + return "1ms gyro motion was not sampled at its right edge."; + } + const auto fifteen = Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 15ms, function); + if (fifteen.size() != 1 || fifteen[0].duration != 15ms || fifteen[0].state.acceleration_g.x != 15){ + return "15ms gyro motion was not sampled correctly."; + } + const auto sixteen = Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 16ms, function); + if (sixteen.size() != 2 || sixteen[0].duration != 15ms || sixteen[1].duration != 1ms + || sixteen[0].state.acceleration_g.x != 15 || sixteen[1].state.acceleration_g.x != 16){ + return "16ms gyro motion does not preserve the short final interval."; + } + const auto thirty_one = Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 31ms, function); + if (thirty_one.size() != 3 || thirty_one[0].duration != 15ms || thirty_one[1].duration != 15ms + || thirty_one[2].duration != 1ms || thirty_one[2].state.acceleration_g.x != 31){ + return "31ms gyro motion does not have right-edge samples at 15, 30, and 31ms."; + } + + bool threw = false; + try{ + Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 31ms, + [](Milliseconds elapsed){ + if (elapsed == 30ms){ + throw std::runtime_error("expected"); + } + return GyroState{}; + } + ); + }catch (const std::runtime_error&){ + threw = true; + } + if (!threw){ + return "A throwing gyro callback did not propagate its exception."; + } + + const double invalid[] = { + std::numeric_limits::quiet_NaN(), + std::numeric_limits::infinity(), + }; + for (double value : invalid){ + for (int field = 0; field < 6; ++field){ + bool invalid_threw = false; + try{ + Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 1ms, + [field, value](Milliseconds){ + GyroState state{}; + switch (field){ + case 0: state.acceleration_g.x = value; break; + case 1: state.acceleration_g.y = value; break; + case 2: state.acceleration_g.z = value; break; + case 3: state.angular_velocity_dps.x = value; break; + case 4: state.angular_velocity_dps.y = value; break; + case 5: state.angular_velocity_dps.z = value; break; + } + return state; + } + ); + }catch (const std::invalid_argument&){ + invalid_threw = true; + } + if (!invalid_threw){ + return "A non-finite gyro motion field was accepted."; + } + } + } + + size_t range_calls = 0; + bool range_threw = false; + try{ + Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 16ms, + [&range_calls](Milliseconds elapsed){ + ++range_calls; + GyroState state{}; + if (elapsed == 16ms){ + state.acceleration_g.z = + static_cast(std::numeric_limits::max()) + / Internal::GYRO_ACCEL_TRANSPORT_SCALE + 1.0; + } + return state; + } + ); + }catch (const std::invalid_argument&){ + range_threw = true; + } + if (!range_threw || range_calls != 2){ + return "A trajectory outside the firmware transport range was not rejected during sampling."; + } + + range_threw = false; + try{ + Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 1ms, + [](Milliseconds){ + GyroState state{}; + state.angular_velocity_dps.x = + static_cast(std::numeric_limits::min()) + / Internal::GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE - 1.0; + return state; + } + ); + }catch (const std::invalid_argument&){ + range_threw = true; + } + if (!range_threw){ + return "An angular velocity outside the firmware transport range was accepted."; + } + return true; + } +}; + +class Test_GyroTransform : public UnitTest{ +public: + Test_GyroTransform() + : UnitTest("NintendoSwitch::Gyro - canonical frame") + {} + + virtual UnitTestResult run(Logger&, CancellableScope&) const override{ + const GyroState input{{1, 2, 3}, {4, 5, 6}}; + for (ControllerClass controller_class : { + ControllerClass::NintendoSwitch_LeftJoycon, + ControllerClass::NintendoSwitch_ProController, + }){ + const GyroState output = transform_gyro_state(controller_class, input); + if (output.acceleration_g.x != 1 || output.acceleration_g.y != 2 || output.acceleration_g.z != 3 + || output.angular_velocity_dps.x != 4 || output.angular_velocity_dps.y != 5 || output.angular_velocity_dps.z != 6){ + return "An identity gyro frame transform changed an axis."; + } + } + const GyroState right = transform_gyro_state(ControllerClass::NintendoSwitch_RightJoycon, input); + if (right.acceleration_g.x != 1 || right.acceleration_g.y != -2 || right.acceleration_g.z != -3 + || right.angular_velocity_dps.x != 4 || right.angular_velocity_dps.y != -5 || right.angular_velocity_dps.z != -6){ + return "Right Joy-Con gyro frame transform is incorrect."; + } + const GyroState twice = transform_gyro_state(ControllerClass::NintendoSwitch_RightJoycon, right); + if (twice.acceleration_g.x != input.acceleration_g.x || twice.acceleration_g.y != input.acceleration_g.y + || twice.acceleration_g.z != input.acceleration_g.z || twice.angular_velocity_dps.x != input.angular_velocity_dps.x + || twice.angular_velocity_dps.y != input.angular_velocity_dps.y || twice.angular_velocity_dps.z != input.angular_velocity_dps.z){ + return "Right Joy-Con gyro transform is not its own inverse."; + } + try{ + transform_gyro_state(ControllerClass::None, input); + }catch (const std::invalid_argument&){ + return true; + } + return "Unsupported controller class did not reject gyro motion."; + } +}; + +class Test_GyroQuaternion : public UnitTest{ +public: + Test_GyroQuaternion() + : UnitTest("NintendoSwitch::Gyro - quaternion") + {} + + virtual UnitTestResult run(Logger&, CancellableScope&) const override{ + using namespace std::chrono_literals; + const GyroQuaternion identity{}; + const GyroQuaternion stationary = integrate_angular_velocity(identity, {}, 15ms); + if (stationary.x != 0 || stationary.y != 0 || stationary.z != 0 || stationary.w != 1){ + return "Stationary gyro motion did not preserve identity."; + } + const GyroQuaternion quarter_turn = integrate_angular_velocity(identity, {90, 0, 0}, 1000ms); + const double half_sqrt = std::sqrt(0.5); + if (std::fabs(quarter_turn.x - half_sqrt) > 1e-12 || std::fabs(quarter_turn.w - half_sqrt) > 1e-12 + || std::fabs(quarter_turn.y) > 1e-12 || std::fabs(quarter_turn.z) > 1e-12){ + return "90dps X-axis quaternion integration is incorrect."; + } + GyroQuaternion chunked{}; + for (int i = 0; i < 1000 / 15; ++i){ + chunked = integrate_angular_velocity(chunked, {90, 0, 0}, 15ms); + } + chunked = integrate_angular_velocity(chunked, {90, 0, 0}, 10ms); + if (std::fabs(chunked.x - quarter_turn.x) > 1e-12 || std::fabs(chunked.w - quarter_turn.w) > 1e-12){ + return "Chunked single-axis gyro integration drifted from one-step integration."; + } + const GyroQuaternion returned = integrate_angular_velocity(quarter_turn, {-90, 0, 0}, 1000ms); + if (std::fabs(returned.x) > 1e-12 || std::fabs(returned.y) > 1e-12 || std::fabs(returned.z) > 1e-12 + || std::fabs(returned.w - 1) > 1e-12){ + return "Inverse gyro angular velocity did not return to identity."; + } + + OemController_State0x30_GyroQuaternion packed{}; + pack_quaternion(packed, identity, 0); + const uint8_t identity_golden[] = { + 0, 0, 0, 0, 0, 0, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, + }; + for (size_t i = 0; i < sizeof(identity_golden); ++i){ + if (packed.raw[i] != identity_golden[i]){ + return "Identity gyro quaternion packing no longer matches its mode-2 bytes."; + } + } + OemController_State0x30_GyroQuaternion packed_half{}; + pack_quaternion(packed_half, {0.5, 0.5, 0.5, 0.5}, 0); + const uint8_t half_golden[] = { + 0, 0, 0, 0, 0, 0, 2, 0, 128, 0, 0, 16, 0, 0, 0, 0, 0, 0, + 0, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, + }; + for (size_t i = 0; i < sizeof(half_golden); ++i){ + if (packed_half.raw[i] != half_golden[i]){ + return "Nontrivial gyro quaternion packing no longer matches its mode-2 bytes."; + } + } + OemController_State0x30_GyroQuaternion packed_wrapped{}; + pack_quaternion(packed_wrapped, identity, 0xfff); + if (packed_wrapped.raw[33] != 128 || packed_wrapped.raw[34] != 255 || packed_wrapped.raw[35] != 15){ + return "Gyro quaternion timestamp does not wrap at 11 bits."; + } + return true; + } +}; + +} + +void add_tests_Gyro(UnitTestDatabase& database){ + database.add(); + database.add(); + database.add(); +} + + +} +} diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.h b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.h new file mode 100644 index 0000000000..9cedcdb398 --- /dev/null +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.h @@ -0,0 +1,18 @@ +/* Nintendo Switch Gyro Tests + * + * From: https://github.com/PokemonAutomation/ + */ + +#ifndef PokemonAutomation_NintendoSwitch_GyroTests_H +#define PokemonAutomation_NintendoSwitch_GyroTests_H + +#include "Common/Cpp/TestRunners/UnitTestDatabase.h" + +namespace PokemonAutomation{ +namespace NintendoSwitch{ + +void add_tests_Gyro(UnitTestDatabase& database); + +} +} +#endif diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h index f722b93909..75fd4ce375 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h @@ -4,10 +4,11 @@ * */ -#ifndef PokemonAutomation_NintendoSwitch_PABotBase2_Joycon_H -#define PokemonAutomation_NintendoSwitch_PABotBase2_Joycon_H - -#include "NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h" +#ifndef PokemonAutomation_NintendoSwitch_PABotBase2_Joycon_H +#define PokemonAutomation_NintendoSwitch_PABotBase2_Joycon_H + +#include +#include "NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h" #include "NintendoSwitch_PABotBase2_OemController.h" namespace PokemonAutomation{ @@ -124,49 +125,19 @@ class PABotBase2_JoyCon : } } - virtual void issue_gyro_accel_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_x(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_accel_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_y(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_accel_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_z(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_x(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_y(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_z(cancellable, delay, hold, cooldown, value); - } - + virtual void issue_gyro_motion( + Cancellable* cancellable, + Milliseconds duration, + const GyroFunction& function + ) override{ + if (duration > Milliseconds::zero() + && m_connection.device().device_protocol_version() < 2026061805){ + throw std::runtime_error("Gyro motion requires firmware protocol 2026061805 or newer."); + } + ControllerWithScheduler::issue_gyro_motion( + cancellable, this->controller_class(), duration, function + ); + } virtual void issue_full_controller_state( Cancellable* cancellable, bool enable_logging, @@ -223,9 +194,11 @@ class PABotBase2_JoyCon : Cancellable* cancellable, const SuperscalarScheduler::ScheduleEntry& entry ){ - SwitchControllerState controller_state; - for (auto& item : entry.state){ - static_cast(*item).apply(controller_state); + SwitchControllerState controller_state; + bool has_gyro_motion = false; + for (auto& item : entry.state){ + static_cast(*item).apply(controller_state); + has_gyro_motion |= item->id == (size_t)SwitchResource::GYRO; } OemController_State0x30_Buttons buttons{ @@ -253,22 +226,17 @@ class PABotBase2_JoyCon : controller_state.left_joystick ); - OemController_State0x30_Gyro gyro{}; - bool gyro_active = populate_report_gyro(gyro, controller_state); - - if (!gyro_active){ - issue_report(cancellable, entry.duration, buttons); - }else{ - issue_report(cancellable, entry.duration, buttons, gyro); - } + issue_motion_report(cancellable, entry.duration, buttons, controller_state, has_gyro_motion); } void execute_state_right_joycon( Cancellable* cancellable, const SuperscalarScheduler::ScheduleEntry& entry ){ - SwitchControllerState controller_state; - for (auto& item : entry.state){ - static_cast(*item).apply(controller_state); + SwitchControllerState controller_state; + bool has_gyro_motion = false; + for (auto& item : entry.state){ + static_cast(*item).apply(controller_state); + has_gyro_motion |= item->id == (size_t)SwitchResource::GYRO; } OemController_State0x30_Buttons buttons{ @@ -294,14 +262,7 @@ class PABotBase2_JoyCon : << std::chrono::duration_cast(entry.duration).count() << endl; #endif - OemController_State0x30_Gyro gyro{}; - bool gyro_active = populate_report_gyro(gyro, controller_state); - - if (!gyro_active){ - issue_report(cancellable, entry.duration, buttons); - }else{ - issue_report(cancellable, entry.duration, buttons, gyro); - } + issue_motion_report(cancellable, entry.duration, buttons, controller_state, has_gyro_motion); } virtual void execute_state( Cancellable* cancellable, diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.cpp index 4ba3cface9..a31af8bb30 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.cpp +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.cpp @@ -4,10 +4,11 @@ * */ -#include "Common/Cpp/ColoredText.h" +#include #include "Common/PABotBase2/Controllers/PABotBase2_Controller_NS1_OemController.h" #include "Controllers/SerialPort/SerialPABotBase.h" #include "NintendoSwitch/NintendoSwitch_Settings.h" +#include "NintendoSwitch/Controllers/NintendoSwitch_Gyro.h" #include "NintendoSwitch_PABotBase2_OemController.h" //#include @@ -19,7 +20,11 @@ namespace NintendoSwitch{ using namespace std::chrono_literals; +namespace { +constexpr uint32_t PHYSICAL_MOTION_PROTOCOL_VERSION = 2026061805; + +} void PABotBase2_OemController::add_message_loggers(PABotBase2::MessageLogger& message_logger){ using namespace PABotBase2; @@ -113,6 +118,21 @@ void PABotBase2_OemController::add_message_loggers(PABotBase2::MessageLogger& me return str; } ); + message_logger.add_message( + "PABB2_MESSAGE_CMD_NS1_OEM_CONTROLLER_MOTION", + PABB2_MESSAGE_CMD_NS1_OEM_CONTROLLER_MOTION, + false, + [](const Message_Command_NS1_OemController_Motion* message){ + return "id = " + std::to_string(message->id) + + ", ms = " + std::to_string(message->milliseconds) + + ", accel-ug = (" + std::to_string(message->acceleration_ug[0]) + + ", " + std::to_string(message->acceleration_ug[1]) + + ", " + std::to_string(message->acceleration_ug[2]) + ")" + + ", gyro-mdps = (" + std::to_string(message->angular_velocity_mdps[0]) + + ", " + std::to_string(message->angular_velocity_mdps[1]) + + ", " + std::to_string(message->angular_velocity_mdps[2]) + ")"; + } + ); } @@ -422,108 +442,77 @@ Button PABotBase2_OemController::populate_report_buttons( } return all_buttons; } -bool PABotBase2_OemController::populate_report_gyro( - OemController_State0x30_Gyro& gyro, - const SwitchControllerState& controller_state -){ - gyro.accel_x = controller_state.gyro[0]; - gyro.accel_y = controller_state.gyro[1]; - gyro.accel_z = controller_state.gyro[2]; - gyro.rotation_x = controller_state.gyro[3]; - gyro.rotation_y = controller_state.gyro[4]; - gyro.rotation_z = controller_state.gyro[5]; - - bool gyro_active = false; - gyro_active |= gyro.accel_x != 0; - gyro_active |= gyro.accel_y != 0; - gyro_active |= gyro.accel_z != 0; - gyro_active |= gyro.rotation_x != 0; - gyro_active |= gyro.rotation_y != 0; - gyro_active |= gyro.rotation_z != 0; -// cout << "gyro_active = " << gyro_active << endl; - return gyro_active; -} - -void PABotBase2_OemController::issue_report( +void PABotBase2_OemController::issue_motion_report( Cancellable* cancellable, WallDuration duration, - const OemController_State0x30_Buttons& buttons + const OemController_State0x30_Buttons& buttons, + const SwitchControllerState& controller_state, + bool has_gyro_motion ){ - // We will not do any throttling or timing adjustments here. We'll defer - // to the microcontroller to do that for us. + if (!has_gyro_motion){ + issue_report(cancellable, duration, buttons); + return; + } - // Divide the controller state into smaller chunks of 65535 milliseconds. - Milliseconds time_left = std::chrono::duration_cast(duration); + if (m_connection.device().device_protocol_version() < PHYSICAL_MOTION_PROTOCOL_VERSION){ + throw std::runtime_error("Gyro motion requires firmware protocol 2026061805 or newer."); + } - PABotBase2::Message_Command_NS1_OemController_Buttons request; + const GyroState& state = controller_state.gyro; + PABotBase2::Message_Command_NS1_OemController_Motion request{}; request.message_bytes = sizeof(request); - request.opcode = PABB2_MESSAGE_CMD_NS1_OEM_CONTROLLER_BUTTONS; + request.opcode = PABB2_MESSAGE_CMD_NS1_OEM_CONTROLLER_MOTION; request.buttons = buttons; + request.acceleration_ug[0] = Internal::gyro_transport_value( + state.acceleration_g.x, Internal::GYRO_ACCEL_TRANSPORT_SCALE, "Gyro acceleration X" + ); + request.acceleration_ug[1] = Internal::gyro_transport_value( + state.acceleration_g.y, Internal::GYRO_ACCEL_TRANSPORT_SCALE, "Gyro acceleration Y" + ); + request.acceleration_ug[2] = Internal::gyro_transport_value( + state.acceleration_g.z, Internal::GYRO_ACCEL_TRANSPORT_SCALE, "Gyro acceleration Z" + ); + request.angular_velocity_mdps[0] = Internal::gyro_transport_value( + state.angular_velocity_dps.x, + Internal::GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE, + "Gyro angular velocity X" + ); + request.angular_velocity_mdps[1] = Internal::gyro_transport_value( + state.angular_velocity_dps.y, + Internal::GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE, + "Gyro angular velocity Y" + ); + request.angular_velocity_mdps[2] = Internal::gyro_transport_value( + state.angular_velocity_dps.z, + Internal::GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE, + "Gyro angular velocity Z" + ); - while (time_left > Milliseconds::zero()){ - Milliseconds current = std::min(time_left, 65535ms); - request.milliseconds = (uint16_t)current.count(); + Milliseconds remaining = std::chrono::duration_cast(duration); + while (remaining > Milliseconds::zero()){ + const Milliseconds current = std::min(remaining, 65535ms); + request.milliseconds = static_cast(current.count()); m_connection.device().command_queue().send_command(cancellable, request); - time_left -= current; + remaining -= current; } } + void PABotBase2_OemController::issue_report( Cancellable* cancellable, WallDuration duration, - const OemController_State0x30_Buttons& buttons, - const OemController_State0x30_Gyro& gyro + const OemController_State0x30_Buttons& buttons ){ - // TODO: For now we duplicate the gyro data to all 3 5ms segments. - OemController_State0x30_GyroX3 gyro3{ - gyro, gyro, gyro - }; - -#if 0 - // Purturb results to show the Switch that they are not stuck. - if (gyro3.time1.accel_x > 0){ - gyro3.time0.accel_x--; - gyro3.time2.accel_x--; - }else if (gyro3.time1.accel_x < 0){ - gyro3.time0.accel_x++; - gyro3.time2.accel_x++; - }else{ - gyro3.time0.accel_x--; - gyro3.time2.accel_x++; - } - if (gyro3.time1.accel_y > 0){ - gyro3.time0.accel_y--; - gyro3.time2.accel_y--; - }else if (gyro3.time1.accel_y < 0){ - gyro3.time0.accel_y++; - gyro3.time2.accel_y++; - }else{ - gyro3.time0.accel_y--; - gyro3.time2.accel_y++; - } - if (gyro3.time1.accel_z > 0){ - gyro3.time0.accel_z--; - gyro3.time2.accel_z--; - }else if (gyro3.time1.accel_z < 0){ - gyro3.time0.accel_z++; - gyro3.time2.accel_z++; - }else{ - gyro3.time0.accel_z--; - gyro3.time2.accel_z++; - } -#endif - // We will not do any throttling or timing adjustments here. We'll defer // to the microcontroller to do that for us. // Divide the controller state into smaller chunks of 65535 milliseconds. Milliseconds time_left = std::chrono::duration_cast(duration); - PABotBase2::Message_Command_NS1_OemController_FullState request; + PABotBase2::Message_Command_NS1_OemController_Buttons request; request.message_bytes = sizeof(request); - request.opcode = PABB2_MESSAGE_CMD_NS1_OEM_CONTROLLER_FULL_STATE; - request.state.buttons = buttons; - request.state.gyro = gyro3; + request.opcode = PABB2_MESSAGE_CMD_NS1_OEM_CONTROLLER_BUTTONS; + request.buttons = buttons; while (time_left > Milliseconds::zero()){ Milliseconds current = std::min(time_left, 65535ms); @@ -532,8 +521,6 @@ void PABotBase2_OemController::issue_report( time_left -= current; } } - - void PABotBase2_OemController::update_status(Cancellable& cancellable){ using namespace PABotBase2; diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.h b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.h index f586f83cc4..b0466f84ab 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.h @@ -49,30 +49,23 @@ class PABotBase2_OemController : ControllerPlayerNumber get_player_number(Cancellable& cancellable); - protected: static Button populate_report_buttons( OemController_State0x30_Buttons& buttons, const SwitchControllerState& controller_state ); - static bool populate_report_gyro( - OemController_State0x30_Gyro& gyro, - const SwitchControllerState& controller_state - ); - - void issue_report( + void issue_motion_report( Cancellable* cancellable, WallDuration duration, - const OemController_State0x30_Buttons& buttons + const OemController_State0x30_Buttons& buttons, + const SwitchControllerState& controller_state, + bool has_gyro_motion ); void issue_report( Cancellable* cancellable, WallDuration duration, - const OemController_State0x30_Buttons& buttons, - const OemController_State0x30_Gyro& gyro + const OemController_State0x30_Buttons& buttons ); - - private: virtual void update_status(Cancellable& cancellable) override; virtual void stop_with_error(std::string message) override; @@ -172,6 +165,7 @@ class PABotBase2_OemController : std::string m_color_html; std::atomic m_player_number; + }; diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_ProController.h b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_ProController.h index 2ded16eaab..726f7334c7 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_ProController.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_ProController.h @@ -124,49 +124,6 @@ class PABotBase2_ProController final : ControllerWithScheduler::issue_right_joystick(cancellable, delay, hold, cooldown, position); } - virtual void issue_gyro_accel_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_x(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_accel_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_y(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_accel_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_z(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_x(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_y(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_z(cancellable, delay, hold, cooldown, value); - } - virtual void issue_full_controller_state( Cancellable* cancellable, bool enable_logging, @@ -226,9 +183,11 @@ class PABotBase2_ProController final : Cancellable* cancellable, const SuperscalarScheduler::ScheduleEntry& entry ) override{ - SwitchControllerState controller_state; - for (auto& item : entry.state){ - static_cast(*item).apply(controller_state); + SwitchControllerState controller_state; + bool has_gyro_motion = false; + for (auto& item : entry.state){ + static_cast(*item).apply(controller_state); + has_gyro_motion |= item->id == (size_t)SwitchResource::GYRO; } // https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/bluetooth_hid_notes.md @@ -264,25 +223,7 @@ class PABotBase2_ProController final : controller_state.right_joystick ); - OemController_State0x30_Gyro gyro{ - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - 0x0000, - }; - bool gyro_active = populate_report_gyro(gyro, controller_state); - - // gyro_active = true; - // gyro.rotation_y = 0x00ff; - // gyro.rotation_z = 0x000f; - - if (!gyro_active){ - issue_report(cancellable, entry.duration, buttons); - }else{ - issue_report(cancellable, entry.duration, buttons, gyro); - } + issue_motion_report(cancellable, entry.duration, buttons, controller_state, has_gyro_motion); #if 0 m_logger.log( diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_WiredController.h b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_WiredController.h index 2df4cb03cd..863ca81862 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_WiredController.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_WiredController.h @@ -118,49 +118,6 @@ class PABotBase2_WiredController final : ControllerWithScheduler::issue_right_joystick(cancellable, delay, hold, cooldown, position); } - virtual void issue_gyro_accel_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_x(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_accel_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_y(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_accel_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_z(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_x(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_y(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_z(cancellable, delay, hold, cooldown, value); - } - virtual void issue_full_controller_state( Cancellable* cancellable, bool enable_logging, diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/Procon/NintendoSwitch_ProController.h b/SerialPrograms/Source/NintendoSwitch/Controllers/Procon/NintendoSwitch_ProController.h index 5c1790c985..8b9047789f 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/Procon/NintendoSwitch_ProController.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/Procon/NintendoSwitch_ProController.h @@ -116,38 +116,6 @@ class ProController : public AbstractController{ const JoystickPosition& position ) = 0; - // Gyro: Accelerometer (experimental - API subject to change) - virtual void issue_gyro_accel_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - virtual void issue_gyro_accel_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - virtual void issue_gyro_accel_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - virtual void issue_gyro_rotate_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - virtual void issue_gyro_rotate_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - virtual void issue_gyro_rotate_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) = 0; - // // Press all the following buttons/joysticks simultaneously for the // specified duration. No wait is added at the end. Thus you can issue diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase3_ProController.h b/SerialPrograms/Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase3_ProController.h index 9561e8479f..534b4571dc 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase3_ProController.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase3_ProController.h @@ -112,49 +112,6 @@ class ProController_SysbotBase3 final : ControllerWithScheduler::issue_right_joystick(cancellable, delay, hold, cooldown, position); } - virtual void issue_gyro_accel_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_x(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_accel_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_y(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_accel_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_accel_z(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_x( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_x(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_y( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_y(cancellable, delay, hold, cooldown, value); - } - virtual void issue_gyro_rotate_z( - Cancellable* cancellable, - Milliseconds delay, Milliseconds hold, Milliseconds cooldown, - int16_t value - ) override{ - ControllerWithScheduler::issue_gyro_rotate_z(cancellable, delay, hold, cooldown, value); - } - virtual void issue_full_controller_state( Cancellable* cancellable, bool enable_logging, diff --git a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp new file mode 100644 index 0000000000..765c52c622 --- /dev/null +++ b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp @@ -0,0 +1,95 @@ +/* Joycon Gyro Tester + * + * From: https://github.com/PokemonAutomation/ + * + */ + +#include "NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h" +#include "JoyconGyroTester.h" + +namespace PokemonAutomation{ +namespace NintendoSwitch{ + + +JoyconGyroTester_Descriptor::JoyconGyroTester_Descriptor() + : SingleSwitchProgramDescriptor( + "NintendoSwitch:JoyconGyroTester", + "Nintendo Switch", "Joycon Gyro Tester", + "", + "Send test values to the Joycon accelerometer and gyroscope axes.", + ProgramControllerClass::SpecializedController, + FeedbackType::NONE, + AllowCommandsWhenRunning::DISABLE_COMMANDS + ) +{} + +JoyconGyroTester::JoyconGyroTester() +{ + +} + +JoyconGyroTester::~JoyconGyroTester() = default; + +void JoyconGyroTester::program(SingleSwitchProgramEnvironment& env, CancellableScope& scope){ + using namespace std::chrono_literals; + JoyconController& controller = env.console.controller(); + + controller.wait_for_all(&scope); + + // Enter the throw stance on every run so the game recenters the ball before + // it receives gyro motion. A short pause is enough for the stance animation. + controller.issue_buttons(&scope, 0ms, 64ms, 0ms, BUTTON_A); + controller.wait_for_all(&scope); + controller.issue_nop(&scope, 400ms); + + // These values are in the canonical, controller-independent frame. This + // tester uses a right Joy-Con, so the old raw Y/Z values are negated here; + // the motion API converts them back to that controller's report basis. + const GyroState neutral{}; + const GyroState wind_up{ + {-5.0, -0.3, 0.8}, + {-168.005384787974, -8.400269239399, -8.400269239399}, + }; + const GyroState release{ + {8.0, -1.5, -4.0}, + {630.020192954902, 35.001121830828, -42.001346196993}, + }; + const GyroState reinforced_release{ + {8.0, -1.0, -3.0}, + {700.022436616558, 21.000673098497, -21.000673098497}, + }; + + const auto forward_state = [&](Milliseconds elapsed){ + if (elapsed <= 150ms){ + return interpolate(neutral, wind_up, (double)elapsed.count() / 150.0); + } + if (elapsed <= 390ms){ + return interpolate(wind_up, release, (double)(elapsed - 150ms).count() / 240.0); + } + if (elapsed <= 540ms){ + return interpolate(release, reinforced_release, (double)(elapsed - 390ms).count() / 150.0); + } + return interpolate(reinforced_release, neutral, (double)(elapsed - 540ms).count() / 180.0); + }; + + controller.issue_gyro_motion(&scope, 1440ms, [=](Milliseconds elapsed){ + if (elapsed <= 720ms){ + return forward_state(elapsed); + } + + // Right-edge sampling means 735ms mirrors the 720ms forward sample, + // while 1440ms mirrors the 15ms sample. Acceleration stays neutral + // and inverse angular velocity returns the orientation to its start. + const GyroState forward = forward_state(1440ms - elapsed + 15ms); + return GyroState{{}, { + -forward.angular_velocity_dps.x, + -forward.angular_velocity_dps.y, + -forward.angular_velocity_dps.z, + }}; + }); + controller.wait_for_all(&scope); +} + + +} +} diff --git a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h new file mode 100644 index 0000000000..3dc054034d --- /dev/null +++ b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h @@ -0,0 +1,37 @@ +/* Joycon Gyro Tester + * + * From: https://github.com/PokemonAutomation/ + * + */ + +#ifndef PokemonAutomation_NintendoSwitch_JoyconGyroTester_H +#define PokemonAutomation_NintendoSwitch_JoyconGyroTester_H + +#include "Common/Cpp/Options/EnumDropdownOption.h" +#include "Common/Cpp/Options/SimpleIntegerOption.h" +#include "Common/Cpp/Options/TimeDurationOption.h" +#include "NintendoSwitch/NintendoSwitch_SingleSwitchProgram.h" + +namespace PokemonAutomation{ +namespace NintendoSwitch{ + + +class JoyconGyroTester_Descriptor : public SingleSwitchProgramDescriptor{ +public: + JoyconGyroTester_Descriptor(); +}; + + +class JoyconGyroTester : public SingleSwitchProgramInstance, private ConfigOption::Listener{ +public: + ~JoyconGyroTester(); + JoyconGyroTester(); + + virtual void program(SingleSwitchProgramEnvironment& env, CancellableScope& scope) override; + +}; + + +} +} +#endif diff --git a/SerialPrograms/Source/NintendoSwitch/DevPrograms/TestProgramSwitch.cpp b/SerialPrograms/Source/NintendoSwitch/DevPrograms/TestProgramSwitch.cpp index 11d280c2ac..9c9ba202dc 100644 --- a/SerialPrograms/Source/NintendoSwitch/DevPrograms/TestProgramSwitch.cpp +++ b/SerialPrograms/Source/NintendoSwitch/DevPrograms/TestProgramSwitch.cpp @@ -480,7 +480,6 @@ void TestProgram::program(MultiSwitchProgramEnvironment& env, CancellableScope& -// context->issue_gyro_accel_x(&scope, 1000ms, 1000ms, 0ms, 123); // OperationFailedExceptionWithScreenshot::fire(ErrorReportMode::SEND_ERROR_REPORT, "test", console); diff --git a/SerialPrograms/Source/NintendoSwitch/NintendoSwitch_Panels.cpp b/SerialPrograms/Source/NintendoSwitch/NintendoSwitch_Panels.cpp index 2979a83f4b..a461d55e62 100644 --- a/SerialPrograms/Source/NintendoSwitch/NintendoSwitch_Panels.cpp +++ b/SerialPrograms/Source/NintendoSwitch/NintendoSwitch_Panels.cpp @@ -27,6 +27,7 @@ #include "Programs/Typewriter/NintendoSwitch_Typewriter.h" #include "Programs/NintendoSwitch_MenuStabilityTester.h" +#include "DevPrograms/JoyconGyroTester.h" #include "DevPrograms/TestProgramComputer.h" #include "DevPrograms/TestProgramSwitch.h" #include "DevPrograms/JoyconProgram.h" @@ -84,6 +85,7 @@ std::vector PanelListFactory::make_panels() const{ ret.emplace_back(make_SingleSwitchProgram()); ret.emplace_back(make_ComputerProgram()); ret.emplace_back(make_MultiSwitchProgram()); + ret.emplace_back(make_single_switch_program()); ret.emplace_back(make_SingleSwitchProgram()); ret.emplace_back(GameConsole::make_ConsoleProgram()); ret.emplace_back(make_ComputerProgram()); diff --git a/SerialPrograms/cmake/SourceFiles.cmake b/SerialPrograms/cmake/SourceFiles.cmake index 99eb79af1f..59c806a5cd 100644 --- a/SerialPrograms/cmake/SourceFiles.cmake +++ b/SerialPrograms/cmake/SourceFiles.cmake @@ -320,12 +320,18 @@ file(GLOB CORE_LIBRARY_SOURCES Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon_from_Keyboard.h Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_JoyconState.cpp Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_JoyconState.h - Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerButtons.cpp - Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerButtons.h - Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerSettings.cpp - Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerSettings.h - Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp - Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.h + Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerButtons.cpp + Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerButtons.h + Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerSettings.cpp + Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerSettings.h + Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp + Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.h + Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.cpp + Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.h + Source/NintendoSwitch/Controllers/NintendoSwitch_GyroQuaternion.cpp + Source/NintendoSwitch/Controllers/NintendoSwitch_GyroQuaternion.h + Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp + Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.h Source/NintendoSwitch/Controllers/NintendoSwitch_KeyboardMapping.cpp Source/NintendoSwitch/Controllers/NintendoSwitch_KeyboardMapping.h Source/NintendoSwitch/Controllers/NintendoSwitch_VirtualControllerState.cpp @@ -1328,6 +1334,8 @@ file(GLOB LIBRARY_SOURCES Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase_Descriptor.h Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase_SelectorWidget.cpp Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase_SelectorWidget.h + Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp + Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h Source/NintendoSwitch/DevPrograms/JoyconProgram.cpp Source/NintendoSwitch/DevPrograms/JoyconProgram.h Source/NintendoSwitch/DevPrograms/TestDudunsparceFormDetector.cpp From 20801ae8a1e2b39067949fa6c3a4e1e2e616db7c Mon Sep 17 00:00:00 2001 From: Dalton-V Date: Tue, 10 Nov 2026 20:28:57 -0600 Subject: [PATCH 2/5] Updates to the gyro test program --- .../DevPrograms/JoyconGyroTester.cpp | 257 ++++++++++++++---- .../DevPrograms/JoyconGyroTester.h | 63 ++++- .../NintendoSwitch/NintendoSwitch_Panels.cpp | 2 +- 3 files changed, 263 insertions(+), 59 deletions(-) diff --git a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp index 765c52c622..461a34f1a3 100644 --- a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp +++ b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp @@ -4,19 +4,185 @@ * */ +#include +#include +#include "Common/Cpp/Exceptions.h" #include "NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h" #include "JoyconGyroTester.h" namespace PokemonAutomation{ namespace NintendoSwitch{ +using namespace std::chrono_literals; + +namespace{ + +constexpr double MAX_ACCEL = std::numeric_limits::max() / Internal::GYRO_ACCEL_TRANSPORT_SCALE; +constexpr double MAX_GYRO = std::numeric_limits::max() / Internal::GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE; + +GyroState inverse_rotation(const GyroState& state){ + return {{}, { + -state.angular_velocity_dps.x, + -state.angular_velocity_dps.y, + -state.angular_velocity_dps.z, + }}; +} + +} + +std::vector make_joycon_gyro_motion( + const std::vector& phases, + bool return_to_start +){ + if (phases.empty()){ + throw std::invalid_argument("Add at least one motion phase."); + } + Milliseconds total = 0ms; + for (const JoyconGyroPhase& phase : phases){ + if (phase.duration <= 0ms || phase.duration > Milliseconds::max() - total){ + throw std::invalid_argument("Motion phase durations must be positive and their total must fit in milliseconds."); + } + if (!is_finite(phase.target)){ + throw std::invalid_argument("Motion values must be finite."); + } + total += phase.duration; + } + + const GyroFunction forward = [phases](Milliseconds elapsed){ + GyroState previous{}; + for (const JoyconGyroPhase& phase : phases){ + if (elapsed <= phase.duration){ + return interpolate(previous, phase.target, (double)elapsed.count() / phase.duration.count()); + } + elapsed -= phase.duration; + previous = phase.target; + } + return previous; + }; + + std::vector motion{{total, forward}}; + if (!return_to_start){ + return motion; + } + + // Sampling uses 15ms intervals with a potentially shorter final interval. + // Reverse that short interval first, then the full intervals. Combining + // them into one callback would shift the sample boundaries and would no + // longer undo exactly the rotations that were sent in the forward pass. + const Milliseconds tail{total.count() % GYRO_REPORT_INTERVAL.count()}; + if (tail > 0ms){ + const GyroState inverse = inverse_rotation(forward(total)); + motion.push_back({tail, [inverse](Milliseconds){ return inverse; }}); + } + const Milliseconds full_intervals = total - tail; + if (full_intervals > 0ms){ + motion.push_back({full_intervals, [forward, full_intervals](Milliseconds elapsed){ + return inverse_rotation(forward(full_intervals - elapsed + GYRO_REPORT_INTERVAL)); + }}); + } + return motion; +} + + +JoyconGyroPhaseRow::JoyconGyroPhaseRow(EditableTableOption& parent_table) + : JoyconGyroPhaseRow(parent_table, "Phase", {150ms, {}}) +{} + +JoyconGyroPhaseRow::JoyconGyroPhaseRow( + EditableTableOption& parent_table, std::string phase_name, JoyconGyroPhase phase +) + : EditableTableRow(parent_table) + , name(false, LockMode::LOCK_WHILE_RUNNING, std::move(phase_name), "Phase name") + , duration(LockMode::LOCK_WHILE_RUNNING, 1ms, std::to_string(phase.duration.count())) + , accel_x(LockMode::LOCK_WHILE_RUNNING, phase.target.acceleration_g.x, -MAX_ACCEL, MAX_ACCEL) + , accel_y(LockMode::LOCK_WHILE_RUNNING, phase.target.acceleration_g.y, -MAX_ACCEL, MAX_ACCEL) + , accel_z(LockMode::LOCK_WHILE_RUNNING, phase.target.acceleration_g.z, -MAX_ACCEL, MAX_ACCEL) + , gyro_x(LockMode::LOCK_WHILE_RUNNING, phase.target.angular_velocity_dps.x, -MAX_GYRO, MAX_GYRO) + , gyro_y(LockMode::LOCK_WHILE_RUNNING, phase.target.angular_velocity_dps.y, -MAX_GYRO, MAX_GYRO) + , gyro_z(LockMode::LOCK_WHILE_RUNNING, phase.target.angular_velocity_dps.z, -MAX_GYRO, MAX_GYRO) +{ + PA_ADD_OPTION(name); + PA_ADD_OPTION(duration); + PA_ADD_OPTION(accel_x); + PA_ADD_OPTION(accel_y); + PA_ADD_OPTION(accel_z); + PA_ADD_OPTION(gyro_x); + PA_ADD_OPTION(gyro_y); + PA_ADD_OPTION(gyro_z); +} + +std::unique_ptr JoyconGyroPhaseRow::clone() const{ + auto copy = std::make_unique(parent(), (std::string)name, snapshot()); + copy->duration.set(duration.current_text()); + return copy; +} + +JoyconGyroPhase JoyconGyroPhaseRow::snapshot() const{ + return {duration.get(), {{accel_x, accel_y, accel_z}, {gyro_x, gyro_y, gyro_z}}}; +} + +JoyconGyroPhaseTable::JoyconGyroPhaseTable() + : EditableTableOption_t( + "Motion phases:
" + "Start at zero acceleration and angular velocity. Each row linearly ramps from the previous row's " + "target to this row's target over its duration. Repeat a target in the next row to hold it. " + "The API samples every 15ms, including the final endpoint; shorter phases may fall between samples.
" + "Canonical axes: +X toward the triggers, +Y toward the controller's left when looking at its controls, " + "+Z out through the controls. Positive rotation follows the right-hand rule. " + "Acceleration is in g; angular velocity is in degrees per second, not an angle. " + "Gravity is not added automatically. The API handles the selected Joy-Con's axis conversion.
" + "Defaults reproduce the right Joy-Con throw. Add, clone, or delete rows to build another motion.", + LockMode::LOCK_WHILE_RUNNING + ) +{ + set_default(make_defaults()); + restore_defaults(); +} + +std::vector JoyconGyroPhaseTable::make_header() const{ + return {"Phase", "Duration (ms)", "Accel X (g)", "Accel Y (g)", "Accel Z (g)", + "Gyro X (°/s)", "Gyro Y (°/s)", "Gyro Z (°/s)"}; +} + +std::string JoyconGyroPhaseTable::check_validity() const{ + std::string error = EditableTableOption::check_validity(); + if (!error.empty()){ + return error; + } + try{ + make_joycon_gyro_motion(snapshot(), false); + }catch (const std::invalid_argument& e){ + return e.what(); + } + return {}; +} + +std::vector> JoyconGyroPhaseTable::make_defaults(){ + // The original throw was tuned in mode-2 report coordinates. Firmware maps + // sensor (X, Y, Z) to mode-2 (Y, X, Z), and the right Joy-Con API maps + // canonical (X, Y, Z) to sensor (X, -Y, -Z). Thus each old report vector + // (x, y, z) is expressed here as canonical (y, -x, -z). + std::vector> rows; + rows.emplace_back(std::make_unique(*this, "Wind-up", JoyconGyroPhase{ + 150ms, {{0.3, 5.0, 0.8}, {8.400269239399, 168.005384787974, -8.400269239399}}, + })); + rows.emplace_back(std::make_unique(*this, "Release", JoyconGyroPhase{ + 240ms, {{1.5, -8.0, -4.0}, {-35.001121830828, -630.020192954902, -42.001346196993}}, + })); + rows.emplace_back(std::make_unique(*this, "Reinforce release", JoyconGyroPhase{ + 150ms, {{1.0, -8.0, -3.0}, {-21.000673098497, -700.022436616558, -21.000673098497}}, + })); + rows.emplace_back(std::make_unique(*this, "Settle", JoyconGyroPhase{180ms, {}})); + return rows; +} + JoyconGyroTester_Descriptor::JoyconGyroTester_Descriptor() : SingleSwitchProgramDescriptor( "NintendoSwitch:JoyconGyroTester", "Nintendo Switch", "Joycon Gyro Tester", "", - "Send test values to the Joycon accelerometer and gyroscope axes.", + "Build a motion trajectory using acceleration in g and angular velocity in degrees per second.", ProgramControllerClass::SpecializedController, FeedbackType::NONE, AllowCommandsWhenRunning::DISABLE_COMMANDS @@ -24,69 +190,50 @@ JoyconGyroTester_Descriptor::JoyconGyroTester_Descriptor() {} JoyconGyroTester::JoyconGyroTester() + : A_HOLD( + "Initial A press:
Hold A before the motion. Set to 0 to skip the press.", + LockMode::LOCK_WHILE_RUNNING, "64 ms" + ) + , START_DELAY( + "Delay before motion:
Wait after releasing A (or before motion when the press is skipped).", + LockMode::LOCK_WHILE_RUNNING, "400 ms" + ) + , RETURN_TO_START( + "Reverse rotation after motion:
Replay the sampled angular velocities in reverse order with opposite signs " + "and zero acceleration. This undoes the commanded rotation; it does not reset or calibrate the controller. " + "Turning this off leaves the final orientation in place. Zero angular velocity alone holds orientation.", + LockMode::LOCK_WHILE_RUNNING, true + ) { - + PA_ADD_OPTION(A_HOLD); + PA_ADD_OPTION(START_DELAY); + PA_ADD_OPTION(PHASES); + PA_ADD_OPTION(RETURN_TO_START); } JoyconGyroTester::~JoyconGyroTester() = default; void JoyconGyroTester::program(SingleSwitchProgramEnvironment& env, CancellableScope& scope){ - using namespace std::chrono_literals; JoyconController& controller = env.console.controller(); + const std::string error = PHASES.check_validity(); + if (!error.empty()){ + throw UserSetupError(env.logger(), error); + } + const auto motion = make_joycon_gyro_motion(PHASES.snapshot(), RETURN_TO_START); controller.wait_for_all(&scope); - - // Enter the throw stance on every run so the game recenters the ball before - // it receives gyro motion. A short pause is enough for the stance animation. - controller.issue_buttons(&scope, 0ms, 64ms, 0ms, BUTTON_A); - controller.wait_for_all(&scope); - controller.issue_nop(&scope, 400ms); - - // These values are in the canonical, controller-independent frame. This - // tester uses a right Joy-Con, so the old raw Y/Z values are negated here; - // the motion API converts them back to that controller's report basis. - const GyroState neutral{}; - const GyroState wind_up{ - {-5.0, -0.3, 0.8}, - {-168.005384787974, -8.400269239399, -8.400269239399}, - }; - const GyroState release{ - {8.0, -1.5, -4.0}, - {630.020192954902, 35.001121830828, -42.001346196993}, - }; - const GyroState reinforced_release{ - {8.0, -1.0, -3.0}, - {700.022436616558, 21.000673098497, -21.000673098497}, - }; - - const auto forward_state = [&](Milliseconds elapsed){ - if (elapsed <= 150ms){ - return interpolate(neutral, wind_up, (double)elapsed.count() / 150.0); - } - if (elapsed <= 390ms){ - return interpolate(wind_up, release, (double)(elapsed - 150ms).count() / 240.0); - } - if (elapsed <= 540ms){ - return interpolate(release, reinforced_release, (double)(elapsed - 390ms).count() / 150.0); - } - return interpolate(reinforced_release, neutral, (double)(elapsed - 540ms).count() / 180.0); - }; - - controller.issue_gyro_motion(&scope, 1440ms, [=](Milliseconds elapsed){ - if (elapsed <= 720ms){ - return forward_state(elapsed); - } - - // Right-edge sampling means 735ms mirrors the 720ms forward sample, - // while 1440ms mirrors the 15ms sample. Acceleration stays neutral - // and inverse angular velocity returns the orientation to its start. - const GyroState forward = forward_state(1440ms - elapsed + 15ms); - return GyroState{{}, { - -forward.angular_velocity_dps.x, - -forward.angular_velocity_dps.y, - -forward.angular_velocity_dps.z, - }}; - }); + const Milliseconds a_hold = A_HOLD; + if (a_hold > 0ms){ + controller.issue_buttons(&scope, 0ms, a_hold, 0ms, BUTTON_A); + controller.wait_for_all(&scope); + } + controller.issue_nop(&scope, START_DELAY); + + // Queue all segments without waiting between them so host transmission + // time does not stretch the motion. The API owns the 15ms sampling. + for (const JoyconGyroMotion& segment : motion){ + controller.issue_gyro_motion(&scope, segment.duration, segment.function); + } controller.wait_for_all(&scope); } diff --git a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h index 3dc054034d..eaccc97a00 100644 --- a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h +++ b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h @@ -7,28 +7,85 @@ #ifndef PokemonAutomation_NintendoSwitch_JoyconGyroTester_H #define PokemonAutomation_NintendoSwitch_JoyconGyroTester_H -#include "Common/Cpp/Options/EnumDropdownOption.h" -#include "Common/Cpp/Options/SimpleIntegerOption.h" +#include "Common/Cpp/Options/BooleanCheckBoxOption.h" +#include "Common/Cpp/Options/EditableTableOption.h" +#include "Common/Cpp/Options/FloatingPointOption.h" +#include "Common/Cpp/Options/StringOption.h" #include "Common/Cpp/Options/TimeDurationOption.h" +#include "NintendoSwitch/Controllers/NintendoSwitch_Gyro.h" #include "NintendoSwitch/NintendoSwitch_SingleSwitchProgram.h" namespace PokemonAutomation{ +class UnitTestDatabase; namespace NintendoSwitch{ +struct JoyconGyroPhase{ + Milliseconds duration; + GyroState target; +}; + +struct JoyconGyroMotion{ + Milliseconds duration; + GyroFunction function; +}; + +// Build the forward trajectory and, optionally, its sampled inverse rotation. +std::vector make_joycon_gyro_motion( + const std::vector& phases, + bool return_to_start +); + +void add_tests_JoyconGyroTester(UnitTestDatabase& database); + +class JoyconGyroPhaseRow : public EditableTableRow{ +public: + JoyconGyroPhaseRow(EditableTableOption& parent_table); + JoyconGyroPhaseRow(EditableTableOption& parent_table, std::string name, JoyconGyroPhase phase); + virtual std::unique_ptr clone() const override; + JoyconGyroPhase snapshot() const; + + StringCell name; + MillisecondsCell duration; + FloatingPointCell accel_x; + FloatingPointCell accel_y; + FloatingPointCell accel_z; + FloatingPointCell gyro_x; + FloatingPointCell gyro_y; + FloatingPointCell gyro_z; +}; + +class JoyconGyroPhaseTable : public EditableTableOption_t{ +public: + JoyconGyroPhaseTable(); + virtual std::vector make_header() const override; + virtual std::string check_validity() const override; + +private: + std::vector> make_defaults(); +}; + + class JoyconGyroTester_Descriptor : public SingleSwitchProgramDescriptor{ public: JoyconGyroTester_Descriptor(); }; -class JoyconGyroTester : public SingleSwitchProgramInstance, private ConfigOption::Listener{ +class JoyconGyroTester : public SingleSwitchProgramInstance{ public: + using Descriptor = JoyconGyroTester_Descriptor; + ~JoyconGyroTester(); JoyconGyroTester(); virtual void program(SingleSwitchProgramEnvironment& env, CancellableScope& scope) override; +private: + MillisecondsOption A_HOLD; + MillisecondsOption START_DELAY; + JoyconGyroPhaseTable PHASES; + BooleanCheckBoxOption RETURN_TO_START; }; diff --git a/SerialPrograms/Source/NintendoSwitch/NintendoSwitch_Panels.cpp b/SerialPrograms/Source/NintendoSwitch/NintendoSwitch_Panels.cpp index a461d55e62..7996505ae5 100644 --- a/SerialPrograms/Source/NintendoSwitch/NintendoSwitch_Panels.cpp +++ b/SerialPrograms/Source/NintendoSwitch/NintendoSwitch_Panels.cpp @@ -85,7 +85,7 @@ std::vector PanelListFactory::make_panels() const{ ret.emplace_back(make_SingleSwitchProgram()); ret.emplace_back(make_ComputerProgram()); ret.emplace_back(make_MultiSwitchProgram()); - ret.emplace_back(make_single_switch_program()); + ret.emplace_back(make_SingleSwitchProgram()); ret.emplace_back(make_SingleSwitchProgram()); ret.emplace_back(GameConsole::make_ConsoleProgram()); ret.emplace_back(make_ComputerProgram()); From 9b03f1242bfa7d3c526b339549b8bef4d069e885 Mon Sep 17 00:00:00 2001 From: Dalton-V Date: Wed, 11 Nov 2026 07:04:49 -0600 Subject: [PATCH 3/5] gyro helper methods --- .../Joycon/NintendoSwitch_Joycon.h | 7 + ...NintendoSwitch_ControllerWithScheduler.cpp | 21 +- .../NintendoSwitch_ControllerWithScheduler.h | 5 + .../Controllers/NintendoSwitch_Gyro.cpp | 116 +++++++--- .../Controllers/NintendoSwitch_Gyro.h | 55 +++-- .../Controllers/NintendoSwitch_GyroTests.cpp | 201 +++++++++++++++++- .../NintendoSwitch_PABotBase2_Joycon.h | 10 + .../DevPrograms/JoyconGyroTester.cpp | 59 ++--- .../DevPrograms/JoyconGyroTester.h | 15 +- 9 files changed, 378 insertions(+), 111 deletions(-) diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h b/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h index 1e79bacf69..994ed16a4d 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h @@ -106,6 +106,13 @@ class JoyconController : public AbstractController{ const GyroFunction& function ) = 0; + // Issue prepared canonical samples without resampling. The selected + // controller's frame and transport range are checked before queueing. + virtual void issue_gyro_motion( + Cancellable* cancellable, + const GyroMotion& motion + ) = 0; + // // Press all the following buttons/joysticks simultaneously for the // specified duration. No wait is added at the end. Thus you can issue diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp index a558a26edd..5059a7db53 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp @@ -159,8 +159,21 @@ void ControllerWithScheduler::issue_gyro_motion( Milliseconds duration, const GyroFunction& function ){ - const std::vector samples = Internal::sample_gyro_motion( - cancellable, controller_class, duration, function + if (duration <= Milliseconds::zero()){ + return; + } + // Reject an unsupported controller before invoking the program's callback. + transform_gyro_state(controller_class, {}); + issue_gyro_motion(cancellable, controller_class, prepare_gyro_motion(cancellable, duration, function)); +} + +void ControllerWithScheduler::issue_gyro_motion( + Cancellable* cancellable, + ControllerClass controller_class, + const GyroMotion& motion +){ + const std::vector samples = Internal::transform_gyro_motion( + cancellable, controller_class, motion ); if (samples.empty()){ return; @@ -173,7 +186,7 @@ void ControllerWithScheduler::issue_gyro_motion( if (cancellable){ cancellable->throw_if_cancelled(); } - for (const Internal::GyroMotionSample& sample : samples){ + for (const GyroMotionSample& sample : samples){ m_scheduler.issue_to_resource( schedule, std::make_unique(sample.state), @@ -184,7 +197,7 @@ void ControllerWithScheduler::issue_gyro_motion( execute_schedule(cancellable, schedule); if (m_logging_throttler){ m_logger.log( - "issue_gyro_motion(): duration = " + std::to_string(duration.count()) + + "issue_gyro_motion(): duration = " + std::to_string(motion.duration().count()) + "ms, samples = " + std::to_string(samples.size()), COLOR_DARKGREEN ); diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.h b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.h index 34224ee6c4..9ec9ff9399 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.h @@ -199,6 +199,11 @@ class ControllerWithScheduler : public PokemonAutomation::ControllerWithSchedule Milliseconds duration, const GyroFunction& function ); + void issue_gyro_motion( + Cancellable* cancellable, + ControllerClass controller_class, + const GyroMotion& motion + ); void issue_full_controller_state( Cancellable* cancellable, diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.cpp index fc0d9cc3b3..a4c3b4129f 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.cpp +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.cpp @@ -60,6 +60,83 @@ GyroState interpolate(const GyroState& from, const GyroState& to, double progres } +GyroMotion prepare_gyro_motion( + Cancellable* cancellable, + Milliseconds duration, + const GyroFunction& function +){ + if (duration <= Milliseconds::zero()){ + return {}; + } + if (!function){ + throw std::invalid_argument("Gyro motion function must not be empty."); + } + + GyroMotion motion; + std::vector& samples = motion.m_samples; + const auto duration_count = duration.count(); + const auto interval_count = GYRO_TRAJECTORY_SAMPLE_INTERVAL.count(); + const auto sample_count = duration_count / interval_count + (duration_count % interval_count != 0); + if (static_cast(sample_count) > samples.max_size()){ + throw std::length_error("Gyro motion duration requires too many samples."); + } + samples.reserve(static_cast(sample_count)); + Milliseconds elapsed = Milliseconds::zero(); + while (elapsed < duration){ + if (cancellable){ + cancellable->throw_if_cancelled(); + } + + const Milliseconds chunk = std::min(GYRO_TRAJECTORY_SAMPLE_INTERVAL, duration - elapsed); + elapsed += chunk; + const GyroState canonical = function(elapsed); + if (!is_finite(canonical)){ + throw std::invalid_argument("Gyro motion values must be finite."); + } + samples.emplace_back(GyroMotionSample{ + chunk, + canonical, + }); + } + return motion; +} + +Milliseconds GyroMotion::duration() const{ + Milliseconds total = Milliseconds::zero(); + for (const GyroMotionSample& sample : m_samples){ + total += sample.duration; + } + return total; +} + +void GyroMotion::append_inverse_rotation(Cancellable* cancellable){ + if (empty()){ + return; + } + if (duration() > Milliseconds::max() / 2 || m_samples.size() > m_samples.max_size() / 2){ + throw std::length_error("Gyro motion with inverse rotation is too long."); + } + + // Build separately so cancellation cannot leave a partially appended inverse. + std::vector combined; + combined.reserve(m_samples.size() * 2); + for (const GyroMotionSample& sample : m_samples){ + if (cancellable){ + cancellable->throw_if_cancelled(); + } + combined.push_back(sample); + } + for (auto iter = m_samples.rbegin(); iter != m_samples.rend(); ++iter){ + if (cancellable){ + cancellable->throw_if_cancelled(); + } + const GyroVector& omega = iter->state.angular_velocity_dps; + combined.push_back({iter->duration, {{}, {-omega.x, -omega.y, -omega.z}}}); + } + m_samples.swap(combined); +} + + namespace Internal{ int32_t gyro_transport_value(double value, double scale, const char* label){ @@ -87,49 +164,24 @@ void validate_transport_state(const GyroState& state){ } -std::vector sample_gyro_motion( +std::vector transform_gyro_motion( Cancellable* cancellable, ControllerClass controller_class, - Milliseconds duration, - const GyroFunction& function + const GyroMotion& motion ){ - if (duration <= Milliseconds::zero()){ + if (motion.empty()){ return {}; } - if (!function){ - throw std::invalid_argument("Gyro motion function must not be empty."); - } - - // Validate the geometry even if the callback would otherwise return no - // interesting values. Unsupported classes must never silently use identity. transform_gyro_state(controller_class, {}); - std::vector samples; - const auto duration_count = duration.count(); - const auto interval_count = GYRO_REPORT_INTERVAL.count(); - const auto sample_count = duration_count / interval_count + (duration_count % interval_count != 0); - if (static_cast(sample_count) > samples.max_size()){ - throw std::length_error("Gyro motion duration requires too many samples."); - } - samples.reserve(static_cast(sample_count)); - Milliseconds elapsed = Milliseconds::zero(); - while (elapsed < duration){ + samples.reserve(motion.samples().size()); + for (const GyroMotionSample& sample : motion.samples()){ if (cancellable){ cancellable->throw_if_cancelled(); } - - const Milliseconds chunk = std::min(GYRO_REPORT_INTERVAL, duration - elapsed); - elapsed += chunk; - const GyroState canonical = function(elapsed); - if (!is_finite(canonical)){ - throw std::invalid_argument("Gyro motion values must be finite."); - } - GyroState transformed = transform_gyro_state(controller_class, canonical); + const GyroState transformed = transform_gyro_state(controller_class, sample.state); validate_transport_state(transformed); - samples.emplace_back(GyroMotionSample{ - chunk, - transformed, - }); + samples.push_back({sample.duration, transformed}); } return samples; } diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.h b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.h index 34c921a6fd..1c0db31143 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_Gyro.h @@ -40,7 +40,8 @@ struct GyroState{ using GyroFunction = std::function; -constexpr Milliseconds GYRO_REPORT_INTERVAL{15}; +// Host trajectory resolution, independent of the firmware's IMU/report clocks. +constexpr Milliseconds GYRO_TRAJECTORY_SAMPLE_INTERVAL{15}; bool is_finite(const GyroState& state); GyroState transform_gyro_state(ControllerClass controller_class, const GyroState& state); @@ -48,28 +49,58 @@ GyroVector interpolate(const GyroVector& from, const GyroVector& to, double prog GyroState interpolate(const GyroState& from, const GyroState& to, double progress); +// A canonical physical state held for the given duration. These boundaries +// remain intact when a prepared motion is replayed or reversed. +struct GyroMotionSample{ + Milliseconds duration; + GyroState state; +}; + +class GyroMotion{ +public: + bool empty() const{ return m_samples.empty(); } + Milliseconds duration() const; + const std::vector& samples() const{ return m_samples; } + + // Append the inverse rotation of the entire current motion: reverse the + // stored samples, negate angular velocity, and use zero acceleration. + // Durations are preserved, including a short final sample. This undoes + // commanded rotation; it does not reset orientation or undo translation. + // Cancellation or failure leaves the original motion unchanged. + void append_inverse_rotation(Cancellable* cancellable = nullptr); + +private: + friend GyroMotion prepare_gyro_motion(Cancellable*, Milliseconds, const GyroFunction&); + std::vector m_samples; +}; + +// Evaluate the callback once at each interval's right edge, including the +// final endpoint, and retain only the resulting finite canonical states. +// Non-positive duration is a no-op. Controller frame conversion and transport +// range validation happen before scheduling, so this motion can be reused on +// different controllers without resampling or converting its axes twice. +GyroMotion prepare_gyro_motion( + Cancellable* cancellable, + Milliseconds duration, + const GyroFunction& function +); + + namespace Internal{ constexpr double GYRO_ACCEL_TRANSPORT_SCALE = 1000000.0; constexpr double GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE = 1000.0; // Converts a program-facing physical value to the signed transport unit used -// by the firmware protocol. Sampling calls this for every component so a +// by the firmware protocol. Frame conversion checks every component so a // complete trajectory is validated before any scheduler state is changed. int32_t gyro_transport_value(double value, double scale, const char* label); -// Internal representation used while a trajectory is being validated before -// it reaches the scheduler. It intentionally contains no raw sensor counts. -struct GyroMotionSample{ - Milliseconds duration; - GyroState state; -}; - -std::vector sample_gyro_motion( +// Convert and validate a copy; the prepared canonical motion is unchanged. +std::vector transform_gyro_motion( Cancellable* cancellable, ControllerClass controller_class, - Milliseconds duration, - const GyroFunction& function + const GyroMotion& motion ); } diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp index bf6c7edec0..82930c28e5 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp @@ -6,6 +6,7 @@ #include #include #include +#include "NintendoSwitch_ControllerWithScheduler.h" #include "NintendoSwitch_Gyro.h" #include "NintendoSwitch_GyroQuaternion.h" #include "NintendoSwitch_GyroTests.h" @@ -28,30 +29,30 @@ class Test_GyroSampling : public UnitTest{ ++calls; return GyroState{{(double)elapsed.count(), 0, 0}, {0, 0, 0}}; }; - if (!Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 0ms, function).empty()){ + if (!prepare_gyro_motion(nullptr, 0ms, function).empty()){ return "0ms gyro motion generated a sample."; } - if (!Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, -1ms, function).empty()){ + if (!prepare_gyro_motion(nullptr, -1ms, function).empty()){ return "Negative gyro motion generated a sample."; } if (calls != 0){ return "A non-positive gyro motion invoked its callback."; } - const auto one = Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 1ms, function); + const auto one = prepare_gyro_motion(nullptr, 1ms, function).samples(); if (one.size() != 1 || one[0].duration != 1ms || one[0].state.acceleration_g.x != 1){ return "1ms gyro motion was not sampled at its right edge."; } - const auto fifteen = Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 15ms, function); + const auto fifteen = prepare_gyro_motion(nullptr, 15ms, function).samples(); if (fifteen.size() != 1 || fifteen[0].duration != 15ms || fifteen[0].state.acceleration_g.x != 15){ return "15ms gyro motion was not sampled correctly."; } - const auto sixteen = Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 16ms, function); + const auto sixteen = prepare_gyro_motion(nullptr, 16ms, function).samples(); if (sixteen.size() != 2 || sixteen[0].duration != 15ms || sixteen[1].duration != 1ms || sixteen[0].state.acceleration_g.x != 15 || sixteen[1].state.acceleration_g.x != 16){ return "16ms gyro motion does not preserve the short final interval."; } - const auto thirty_one = Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 31ms, function); + const auto thirty_one = prepare_gyro_motion(nullptr, 31ms, function).samples(); if (thirty_one.size() != 3 || thirty_one[0].duration != 15ms || thirty_one[1].duration != 15ms || thirty_one[2].duration != 1ms || thirty_one[2].state.acceleration_g.x != 31){ return "31ms gyro motion does not have right-edge samples at 15, 30, and 31ms."; @@ -59,7 +60,7 @@ class Test_GyroSampling : public UnitTest{ bool threw = false; try{ - Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 31ms, + prepare_gyro_motion(nullptr, 31ms, [](Milliseconds elapsed){ if (elapsed == 30ms){ throw std::runtime_error("expected"); @@ -82,7 +83,7 @@ class Test_GyroSampling : public UnitTest{ for (int field = 0; field < 6; ++field){ bool invalid_threw = false; try{ - Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 1ms, + prepare_gyro_motion(nullptr, 1ms, [field, value](Milliseconds){ GyroState state{}; switch (field){ @@ -108,7 +109,7 @@ class Test_GyroSampling : public UnitTest{ size_t range_calls = 0; bool range_threw = false; try{ - Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 16ms, + const GyroMotion motion = prepare_gyro_motion(nullptr, 16ms, [&range_calls](Milliseconds elapsed){ ++range_calls; GyroState state{}; @@ -120,16 +121,17 @@ class Test_GyroSampling : public UnitTest{ return state; } ); + Internal::transform_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, motion); }catch (const std::invalid_argument&){ range_threw = true; } if (!range_threw || range_calls != 2){ - return "A trajectory outside the firmware transport range was not rejected during sampling."; + return "A trajectory outside the firmware transport range was not rejected before scheduling."; } range_threw = false; try{ - Internal::sample_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, 1ms, + const GyroMotion motion = prepare_gyro_motion(nullptr, 1ms, [](Milliseconds){ GyroState state{}; state.angular_velocity_dps.x = @@ -138,6 +140,7 @@ class Test_GyroSampling : public UnitTest{ return state; } ); + Internal::transform_gyro_motion(nullptr, ControllerClass::NintendoSwitch_LeftJoycon, motion); }catch (const std::invalid_argument&){ range_threw = true; } @@ -148,6 +151,180 @@ class Test_GyroSampling : public UnitTest{ } }; +class Test_GyroInverse : public UnitTest{ +public: + Test_GyroInverse() + : UnitTest("NintendoSwitch::Gyro - prepared inverse") + {} + + virtual UnitTestResult run(Logger&, CancellableScope&) const override{ + using namespace std::chrono_literals; + GyroMotion empty; + empty.append_inverse_rotation(); + if (!empty.empty() || empty.duration() != 0ms){ + return "An empty motion gained samples during inverse replay."; + } + for (Milliseconds duration : {1ms, 8ms, 15ms, 16ms, 31ms, 720ms}){ + size_t calls = 0; + GyroMotion motion = prepare_gyro_motion(nullptr, duration, [&calls](Milliseconds elapsed){ + // Deliberately stateful: reevaluating this callback cannot undo + // the original motion. Changing axes also exercises ordering. + const double n = (double)++calls; + return GyroState{{1, 2, 3}, {n * 10, (double)elapsed.count(), n * n}}; + }); + const auto forward = motion.samples(); + motion.append_inverse_rotation(); + if (calls != forward.size() || motion.samples().size() != 2 * calls + || motion.duration() != duration * 2){ + return "Inverse replay resampled the callback or changed the total duration."; + } + for (size_t i = 0; i < forward.size(); ++i){ + const GyroMotionSample& original = forward[i]; + const GyroMotionSample& inverse = motion.samples()[motion.samples().size() - 1 - i]; + if (inverse.duration != original.duration + || inverse.state.acceleration_g.x != 0 || inverse.state.acceleration_g.y != 0 + || inverse.state.acceleration_g.z != 0 + || inverse.state.angular_velocity_dps.x != -original.state.angular_velocity_dps.x + || inverse.state.angular_velocity_dps.y != -original.state.angular_velocity_dps.y + || inverse.state.angular_velocity_dps.z != -original.state.angular_velocity_dps.z){ + return "Inverse replay did not preserve the original sample boundaries and opposite rotation."; + } + } + for (ControllerClass controller_class : { + ControllerClass::NintendoSwitch_LeftJoycon, ControllerClass::NintendoSwitch_RightJoycon, + }){ + GyroQuaternion orientation{}; + for (const auto& sample : Internal::transform_gyro_motion(nullptr, controller_class, motion)){ + orientation = integrate_angular_velocity(orientation, sample.state.angular_velocity_dps, sample.duration); + } + if (std::fabs(orientation.x) > 1e-12 || std::fabs(orientation.y) > 1e-12 + || std::fabs(orientation.z) > 1e-12 || std::fabs(orientation.w - 1) > 1e-12){ + return "Prepared multi-axis motion and its inverse did not return to the starting orientation."; + } + } + } + + CancellableHolder cancelled; + cancelled.cancel(std::make_exception_ptr(std::runtime_error("expected cancellation"))); + GyroMotion motion = prepare_gyro_motion(nullptr, 31ms, [](Milliseconds){ return GyroState{}; }); + bool threw = false; + try{ + motion.append_inverse_rotation(&cancelled); + }catch (const std::runtime_error&){ + threw = true; + } + if (!threw || motion.duration() != 31ms || motion.samples().size() != 3){ + return "Cancelled inverse preparation modified the original motion."; + } + return true; + } +}; + +class GyroRecordingController : public ControllerWithScheduler{ +public: + using ControllerWithScheduler::ControllerWithScheduler; + struct Entry{ + WallDuration duration; + SwitchControllerState state; + }; + std::vector entries; + +private: + virtual void execute_state(Cancellable*, const SuperscalarScheduler::ScheduleEntry& entry) override{ + SwitchControllerState state{}; + bool has_gyro = false; + for (const auto& resource : entry.state){ + static_cast(*resource).apply(state); + has_gyro |= resource->id == (size_t)SwitchResource::GYRO; + } + if (has_gyro){ + entries.push_back({entry.duration, state}); + } + } +}; + +class Test_GyroScheduling : public UnitTest{ +public: + Test_GyroScheduling() + : UnitTest("NintendoSwitch::Gyro - prepared scheduling") + {} + + virtual UnitTestResult run(Logger& logger, CancellableScope&) const override{ + using namespace std::chrono_literals; + RecursiveThrottler throttler; + ThrottleScope suppress_logging(throttler); + const GyroFunction function = [](Milliseconds elapsed){ + return GyroState{{1, 2, 3}, {4, (double)elapsed.count(), 6}}; + }; + GyroMotion motion = prepare_gyro_motion(nullptr, 31ms, function); + motion.append_inverse_rotation(); + for (ControllerClass controller_class : { + ControllerClass::NintendoSwitch_LeftJoycon, ControllerClass::NintendoSwitch_RightJoycon, + }){ + GyroRecordingController controller(logger, throttler); + controller.issue_buttons(nullptr, 0ms, 62ms, 0ms, BUTTON_A); + controller.issue_gyro_motion(nullptr, controller_class, motion); + controller.issue_barrier(nullptr); + if (controller.entries.size() != motion.samples().size()){ + return "Scheduling changed the prepared sample boundaries."; + } + const double sign = controller_class == ControllerClass::NintendoSwitch_RightJoycon ? -1 : 1; + for (size_t i = 0; i < motion.samples().size(); ++i){ + const auto& expected = motion.samples()[i]; + const auto& actual = controller.entries[i]; + if (actual.duration != expected.duration || actual.state.buttons != BUTTON_A + || actual.state.gyro.acceleration_g.x != expected.state.acceleration_g.x + || actual.state.gyro.acceleration_g.y != sign * expected.state.acceleration_g.y + || actual.state.gyro.acceleration_g.z != sign * expected.state.acceleration_g.z + || actual.state.gyro.angular_velocity_dps.x != expected.state.angular_velocity_dps.x + || actual.state.gyro.angular_velocity_dps.y != sign * expected.state.angular_velocity_dps.y + || actual.state.gyro.angular_velocity_dps.z != sign * expected.state.angular_velocity_dps.z){ + return "Prepared motion lost button composition, timing, or canonical axis conversion."; + } + } + } + if (motion.samples()[0].state.acceleration_g.y != 2){ + return "Dispatch modified the reusable canonical motion."; + } + + GyroRecordingController legacy(logger, throttler); + GyroRecordingController prepared(logger, throttler); + legacy.issue_gyro_motion(nullptr, ControllerClass::NintendoSwitch_RightJoycon, 31ms, function); + prepared.issue_gyro_motion(nullptr, ControllerClass::NintendoSwitch_RightJoycon, + prepare_gyro_motion(nullptr, 31ms, function)); + legacy.issue_barrier(nullptr); + prepared.issue_barrier(nullptr); + if (legacy.entries.size() != 3 || prepared.entries.size() != 3){ + return "Callback and prepared APIs produced different sample counts."; + } + for (size_t i = 0; i < 3; ++i){ + if (legacy.entries[i].duration != prepared.entries[i].duration + || legacy.entries[i].state.gyro.angular_velocity_dps.y != prepared.entries[i].state.gyro.angular_velocity_dps.y){ + return "Callback and prepared APIs produced different schedules."; + } + } + + // Forward INT32_MIN is valid on X, but negating it is not encodable. + // Detect even an invalid inverse before queueing any forward samples. + GyroMotion invalid = prepare_gyro_motion(nullptr, 16ms, [](Milliseconds){ + return GyroState{{}, {std::numeric_limits::min() / Internal::GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE, 0, 0}}; + }); + invalid.append_inverse_rotation(); + GyroRecordingController rejected(logger, throttler); + bool threw = false; + try{ + rejected.issue_gyro_motion(nullptr, ControllerClass::NintendoSwitch_RightJoycon, invalid); + }catch (const std::invalid_argument&){ + threw = true; + } + rejected.issue_barrier(nullptr); + if (!threw || !rejected.entries.empty()){ + return "An invalid prepared motion changed scheduler state before validation finished."; + } + return true; + } +}; + class Test_GyroTransform : public UnitTest{ public: Test_GyroTransform() @@ -254,6 +431,8 @@ class Test_GyroQuaternion : public UnitTest{ void add_tests_Gyro(UnitTestDatabase& database){ database.add(); + database.add(); + database.add(); database.add(); database.add(); } diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h index 75fd4ce375..3b025f0605 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h @@ -138,6 +138,16 @@ class PABotBase2_JoyCon : cancellable, this->controller_class(), duration, function ); } + virtual void issue_gyro_motion( + Cancellable* cancellable, + const GyroMotion& motion + ) override{ + if (!motion.empty() + && m_connection.device().device_protocol_version() < 2026061805){ + throw std::runtime_error("Gyro motion requires firmware protocol 2026061805 or newer."); + } + ControllerWithScheduler::issue_gyro_motion(cancellable, this->controller_class(), motion); + } virtual void issue_full_controller_state( Cancellable* cancellable, bool enable_logging, diff --git a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp index 461a34f1a3..f0da9e1445 100644 --- a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp +++ b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp @@ -20,20 +20,7 @@ namespace{ constexpr double MAX_ACCEL = std::numeric_limits::max() / Internal::GYRO_ACCEL_TRANSPORT_SCALE; constexpr double MAX_GYRO = std::numeric_limits::max() / Internal::GYRO_ANGULAR_VELOCITY_TRANSPORT_SCALE; -GyroState inverse_rotation(const GyroState& state){ - return {{}, { - -state.angular_velocity_dps.x, - -state.angular_velocity_dps.y, - -state.angular_velocity_dps.z, - }}; -} - -} - -std::vector make_joycon_gyro_motion( - const std::vector& phases, - bool return_to_start -){ +Milliseconds validate_phases(const std::vector& phases){ if (phases.empty()){ throw std::invalid_argument("Add at least one motion phase."); } @@ -47,7 +34,17 @@ std::vector make_joycon_gyro_motion( } total += phase.duration; } + return total; +} +} + +GyroMotion make_joycon_gyro_motion( + const std::vector& phases, + bool return_to_start, + Cancellable* cancellable +){ + const Milliseconds total = validate_phases(phases); const GyroFunction forward = [phases](Milliseconds elapsed){ GyroState previous{}; for (const JoyconGyroPhase& phase : phases){ @@ -60,25 +57,9 @@ std::vector make_joycon_gyro_motion( return previous; }; - std::vector motion{{total, forward}}; - if (!return_to_start){ - return motion; - } - - // Sampling uses 15ms intervals with a potentially shorter final interval. - // Reverse that short interval first, then the full intervals. Combining - // them into one callback would shift the sample boundaries and would no - // longer undo exactly the rotations that were sent in the forward pass. - const Milliseconds tail{total.count() % GYRO_REPORT_INTERVAL.count()}; - if (tail > 0ms){ - const GyroState inverse = inverse_rotation(forward(total)); - motion.push_back({tail, [inverse](Milliseconds){ return inverse; }}); - } - const Milliseconds full_intervals = total - tail; - if (full_intervals > 0ms){ - motion.push_back({full_intervals, [forward, full_intervals](Milliseconds elapsed){ - return inverse_rotation(forward(full_intervals - elapsed + GYRO_REPORT_INTERVAL)); - }}); + GyroMotion motion = prepare_gyro_motion(cancellable, total, forward); + if (return_to_start){ + motion.append_inverse_rotation(cancellable); } return motion; } @@ -126,7 +107,7 @@ JoyconGyroPhaseTable::JoyconGyroPhaseTable() "Motion phases:
" "Start at zero acceleration and angular velocity. Each row linearly ramps from the previous row's " "target to this row's target over its duration. Repeat a target in the next row to hold it. " - "The API samples every 15ms, including the final endpoint; shorter phases may fall between samples.
" + "The host samples the trajectory every 15ms, including the final endpoint; shorter phases may fall between samples.
" "Canonical axes: +X toward the triggers, +Y toward the controller's left when looking at its controls, " "+Z out through the controls. Positive rotation follows the right-hand rule. " "Acceleration is in g; angular velocity is in degrees per second, not an angle. " @@ -150,7 +131,7 @@ std::string JoyconGyroPhaseTable::check_validity() const{ return error; } try{ - make_joycon_gyro_motion(snapshot(), false); + validate_phases(snapshot()); }catch (const std::invalid_argument& e){ return e.what(); } @@ -219,7 +200,7 @@ void JoyconGyroTester::program(SingleSwitchProgramEnvironment& env, CancellableS if (!error.empty()){ throw UserSetupError(env.logger(), error); } - const auto motion = make_joycon_gyro_motion(PHASES.snapshot(), RETURN_TO_START); + const GyroMotion motion = make_joycon_gyro_motion(PHASES.snapshot(), RETURN_TO_START, &scope); controller.wait_for_all(&scope); const Milliseconds a_hold = A_HOLD; @@ -229,11 +210,7 @@ void JoyconGyroTester::program(SingleSwitchProgramEnvironment& env, CancellableS } controller.issue_nop(&scope, START_DELAY); - // Queue all segments without waiting between them so host transmission - // time does not stretch the motion. The API owns the 15ms sampling. - for (const JoyconGyroMotion& segment : motion){ - controller.issue_gyro_motion(&scope, segment.duration, segment.function); - } + controller.issue_gyro_motion(&scope, motion); controller.wait_for_all(&scope); } diff --git a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h index eaccc97a00..547d63bb56 100644 --- a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h +++ b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.h @@ -16,7 +16,6 @@ #include "NintendoSwitch/NintendoSwitch_SingleSwitchProgram.h" namespace PokemonAutomation{ -class UnitTestDatabase; namespace NintendoSwitch{ @@ -25,19 +24,13 @@ struct JoyconGyroPhase{ GyroState target; }; -struct JoyconGyroMotion{ - Milliseconds duration; - GyroFunction function; -}; - -// Build the forward trajectory and, optionally, its sampled inverse rotation. -std::vector make_joycon_gyro_motion( +// Describe the table's ramps and prepare them through the shared motion API. +GyroMotion make_joycon_gyro_motion( const std::vector& phases, - bool return_to_start + bool return_to_start, + Cancellable* cancellable = nullptr ); -void add_tests_JoyconGyroTester(UnitTestDatabase& database); - class JoyconGyroPhaseRow : public EditableTableRow{ public: JoyconGyroPhaseRow(EditableTableOption& parent_table); From 9d705cf6b2f6ad8ae4ecd57490a2675bc74c00dc Mon Sep 17 00:00:00 2001 From: Dalton-V Date: Wed, 11 Nov 2026 07:44:50 -0600 Subject: [PATCH 4/5] missing duration --- ...NintendoSwitch_ControllerWithScheduler.cpp | 2 +- .../Controllers/NintendoSwitch_GyroTests.cpp | 28 +++++++++++++------ 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp index 5059a7db53..a455a32b11 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_ControllerWithScheduler.cpp @@ -190,7 +190,7 @@ void ControllerWithScheduler::issue_gyro_motion( m_scheduler.issue_to_resource( schedule, std::make_unique(sample.state), - WallDuration::zero(), sample.duration, WallDuration::zero() + sample.duration, sample.duration, WallDuration::zero() ); } } diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp index 82930c28e5..d868e194fb 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/NintendoSwitch_GyroTests.cpp @@ -226,6 +226,7 @@ class GyroRecordingController : public ControllerWithScheduler{ struct Entry{ WallDuration duration; SwitchControllerState state; + bool has_gyro; }; std::vector entries; @@ -237,9 +238,7 @@ class GyroRecordingController : public ControllerWithScheduler{ static_cast(*resource).apply(state); has_gyro |= resource->id == (size_t)SwitchResource::GYRO; } - if (has_gyro){ - entries.push_back({entry.duration, state}); - } + entries.push_back({entry.duration, state, has_gyro}); } }; @@ -264,15 +263,14 @@ class Test_GyroScheduling : public UnitTest{ GyroRecordingController controller(logger, throttler); controller.issue_buttons(nullptr, 0ms, 62ms, 0ms, BUTTON_A); controller.issue_gyro_motion(nullptr, controller_class, motion); - controller.issue_barrier(nullptr); if (controller.entries.size() != motion.samples().size()){ - return "Scheduling changed the prepared sample boundaries."; + return "Scheduling did not dispatch every prepared sample before returning."; } const double sign = controller_class == ControllerClass::NintendoSwitch_RightJoycon ? -1 : 1; for (size_t i = 0; i < motion.samples().size(); ++i){ const auto& expected = motion.samples()[i]; const auto& actual = controller.entries[i]; - if (actual.duration != expected.duration || actual.state.buttons != BUTTON_A + if (!actual.has_gyro || actual.duration != expected.duration || actual.state.buttons != BUTTON_A || actual.state.gyro.acceleration_g.x != expected.state.acceleration_g.x || actual.state.gyro.acceleration_g.y != sign * expected.state.acceleration_g.y || actual.state.gyro.acceleration_g.z != sign * expected.state.acceleration_g.z @@ -292,8 +290,6 @@ class Test_GyroScheduling : public UnitTest{ legacy.issue_gyro_motion(nullptr, ControllerClass::NintendoSwitch_RightJoycon, 31ms, function); prepared.issue_gyro_motion(nullptr, ControllerClass::NintendoSwitch_RightJoycon, prepare_gyro_motion(nullptr, 31ms, function)); - legacy.issue_barrier(nullptr); - prepared.issue_barrier(nullptr); if (legacy.entries.size() != 3 || prepared.entries.size() != 3){ return "Callback and prepared APIs produced different sample counts."; } @@ -304,6 +300,22 @@ class Test_GyroScheduling : public UnitTest{ } } + GyroRecordingController followed_by_wait(logger, throttler); + followed_by_wait.issue_gyro_motion(nullptr, ControllerClass::NintendoSwitch_RightJoycon, 15ms, function); + followed_by_wait.issue_nop(nullptr, 100ms); + if (followed_by_wait.entries.size() != 2 + || !followed_by_wait.entries[0].has_gyro || followed_by_wait.entries[0].duration != 15ms + || followed_by_wait.entries[1].has_gyro || followed_by_wait.entries[1].duration != 100ms){ + return "The wait after gyro motion did not get its full duration."; + } + + prepared.issue_buttons(nullptr, 20ms, 20ms, 0ms, BUTTON_A); + if (prepared.entries.size() != 4 || prepared.entries[2].state.buttons != BUTTON_NONE + || prepared.entries[3].has_gyro || prepared.entries[3].state.buttons != BUTTON_A + || prepared.entries[3].duration != 20ms){ + return "A button press after gyro motion did not start after the final sample."; + } + // Forward INT32_MIN is valid on X, but negating it is not encodable. // Detect even an invalid inverse before queueing any forward samples. GyroMotion invalid = prepare_gyro_motion(nullptr, 16ms, [](Milliseconds){ From bbcde3bcecf86db92dda1574076ad20a26d1ff8d Mon Sep 17 00:00:00 2001 From: Dalton-V Date: Wed, 11 Nov 2026 08:41:05 -0600 Subject: [PATCH 5/5] More rebase fixes --- .../NintendoSwitch_OemController_State.h | 14 ++-- .../PABotBase2_Controller_NS1_OemController.h | 3 +- .../ComputerPrograms_UnitTestRunner.cpp | 5 +- .../Joycon/NintendoSwitch_Joycon.h | 5 +- .../NintendoSwitch_PABotBase2_Joycon.h | 66 +++++++++---------- ...intendoSwitch_PABotBase2_OemController.cpp | 18 +++-- .../NintendoSwitch_PABotBase2_OemController.h | 1 + .../DevPrograms/JoyconGyroTester.cpp | 2 + 8 files changed, 65 insertions(+), 49 deletions(-) diff --git a/Common/ControllerStates/NintendoSwitch_OemController_State.h b/Common/ControllerStates/NintendoSwitch_OemController_State.h index 703ca4e8db..9852e0d77a 100644 --- a/Common/ControllerStates/NintendoSwitch_OemController_State.h +++ b/Common/ControllerStates/NintendoSwitch_OemController_State.h @@ -149,12 +149,12 @@ struct OemController_State0x30_GyroX3{ static_assert(sizeof(OemController_State0x30_GyroX3) == 36, "GyroX3 struct must be 36 bytes"); -struct OemController_State0x30{ - OemController_State0x30_Buttons buttons; - OemController_State0x30_GyroX3 gyro; -}; - -static_assert(sizeof(OemController_State0x30) == 46, "Report 0x30 state must be 46 bytes"); +struct OemController_State0x30{ + OemController_State0x30_Buttons buttons; + OemController_State0x30_GyroX3 gyro; +}; + +static_assert(sizeof(OemController_State0x30) == 46, "Report 0x30 state must be 46 bytes"); extern const OemController_State0x30 OemController_State0x30_NEUTRAL_STATE; @@ -177,7 +177,7 @@ struct OemController_Status{ uint8_t player_lights; uint8_t paired_mac_address[6]; - + // 0 = Standard // 1 = Quaternion //uint8_t gyro_protocol = 1; diff --git a/Common/PABotBase2/Controllers/PABotBase2_Controller_NS1_OemController.h b/Common/PABotBase2/Controllers/PABotBase2_Controller_NS1_OemController.h index 7f2c8ef711..103a30ec7b 100644 --- a/Common/PABotBase2/Controllers/PABotBase2_Controller_NS1_OemController.h +++ b/Common/PABotBase2/Controllers/PABotBase2_Controller_NS1_OemController.h @@ -57,13 +57,14 @@ struct Message_Command_NS1_OemController_FullState : MessageHeader{ #define PABB2_MESSAGE_CMD_NS1_OEM_CONTROLLER_MOTION 0x99 -struct PABB_PACK Message_Command_NS1_OemController_Motion : MessageHeader{ +struct Message_Command_NS1_OemController_Motion : MessageHeader{ uint16_t milliseconds; NintendoSwitch::OemController_State0x30_Buttons buttons; int32_t acceleration_ug[3]; int32_t angular_velocity_mdps[3]; }; static_assert(sizeof(Message_Command_NS1_OemController_Motion) == 40, "OEM motion message must be 40 bytes"); +static_assert(alignof(Message_Command_NS1_OemController_Motion) == 1, "OEM motion message must be packed"); diff --git a/SerialPrograms/Source/ComputerPrograms/ComputerPrograms_UnitTestRunner.cpp b/SerialPrograms/Source/ComputerPrograms/ComputerPrograms_UnitTestRunner.cpp index 1d74c40020..f28ab680c5 100644 --- a/SerialPrograms/Source/ComputerPrograms/ComputerPrograms_UnitTestRunner.cpp +++ b/SerialPrograms/Source/ComputerPrograms/ComputerPrograms_UnitTestRunner.cpp @@ -29,11 +29,10 @@ #include "NintendoSwitch/Inference/NintendoSwitch_FailedToConnectDetector.h" #include "NintendoSwitch/Inference/NintendoSwitch_UpdatePopupDetector.h" #include "NintendoSwitch/Programs/Typewriter/NintendoSwitch_Typewriter_Tests.h" +#include "NintendoSwitch/Controllers/NintendoSwitch_GyroTests.h" #include "NintendoSwitch/Controllers/SysbotBase/SysbotBase_Connection.h" #include "PokemonBDSP/Inference/PokemonBDSP_DialogDetector.h" #include "PokemonBDSP/Inference/PokemonBDSP_MenuDetector.h" -#include "NintendoSwitch/Controllers/NintendoSwitch_GyroTests.h" -#include "UnitTestRunner.h" #include "VideoGames/PokemonHome/PokemonHome_Tests.h" #include "VideoGames/PokemonSwSh/PokemonSwSh_Tests.h" @@ -78,7 +77,7 @@ UnitTestDatabase make_UNIT_TESTS_ALL(){ NintendoSwitch::Typewriter::add_tests_TypewriterTextMapping(ret); NintendoSwitch::PokemonBDSP::add_tests_DialogDetector(ret); NintendoSwitch::PokemonBDSP::add_tests_MenuDetector(ret); - NintendoSwitch::add_tests_Gyro(ret); + NintendoSwitch::add_tests_Gyro(ret); NintendoSwitch::PokemonFRLG::add_tests(ret); NintendoSwitch::PokemonRSE::add_tests(ret); NintendoSwitch::PokemonHome::add_tests(ret); diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h b/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h index 994ed16a4d..23ebd802bd 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h @@ -97,9 +97,12 @@ class JoyconController : public AbstractController{ const JoystickPosition& position ) = 0; + // Throws if the connected controller cannot execute physical gyro motion. + virtual void check_gyro_motion_supported() const = 0; + // Issue a complete physical gyro trajectory. The callback is evaluated // synchronously while this command is constructed and is never retained. - // A non-positive duration is a no-op. + // A non-positive duration is a no-op. virtual void issue_gyro_motion( Cancellable* cancellable, Milliseconds duration, diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h index 3b025f0605..965c4a460e 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_Joycon.h @@ -4,11 +4,10 @@ * */ -#ifndef PokemonAutomation_NintendoSwitch_PABotBase2_Joycon_H -#define PokemonAutomation_NintendoSwitch_PABotBase2_Joycon_H - -#include -#include "NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h" +#ifndef PokemonAutomation_NintendoSwitch_PABotBase2_Joycon_H +#define PokemonAutomation_NintendoSwitch_PABotBase2_Joycon_H + +#include "NintendoSwitch/Controllers/Joycon/NintendoSwitch_Joycon.h" #include "NintendoSwitch_PABotBase2_OemController.h" namespace PokemonAutomation{ @@ -125,26 +124,27 @@ class PABotBase2_JoyCon : } } - virtual void issue_gyro_motion( - Cancellable* cancellable, - Milliseconds duration, - const GyroFunction& function - ) override{ - if (duration > Milliseconds::zero() - && m_connection.device().device_protocol_version() < 2026061805){ - throw std::runtime_error("Gyro motion requires firmware protocol 2026061805 or newer."); - } - ControllerWithScheduler::issue_gyro_motion( - cancellable, this->controller_class(), duration, function - ); - } + virtual void check_gyro_motion_supported() const override{ + require_gyro_motion_protocol(); + } + virtual void issue_gyro_motion( + Cancellable* cancellable, + Milliseconds duration, + const GyroFunction& function + ) override{ + if (duration > Milliseconds::zero()){ + check_gyro_motion_supported(); + } + ControllerWithScheduler::issue_gyro_motion( + cancellable, this->controller_class(), duration, function + ); + } virtual void issue_gyro_motion( Cancellable* cancellable, const GyroMotion& motion ) override{ - if (!motion.empty() - && m_connection.device().device_protocol_version() < 2026061805){ - throw std::runtime_error("Gyro motion requires firmware protocol 2026061805 or newer."); + if (!motion.empty()){ + check_gyro_motion_supported(); } ControllerWithScheduler::issue_gyro_motion(cancellable, this->controller_class(), motion); } @@ -204,11 +204,11 @@ class PABotBase2_JoyCon : Cancellable* cancellable, const SuperscalarScheduler::ScheduleEntry& entry ){ - SwitchControllerState controller_state; - bool has_gyro_motion = false; - for (auto& item : entry.state){ - static_cast(*item).apply(controller_state); - has_gyro_motion |= item->id == (size_t)SwitchResource::GYRO; + SwitchControllerState controller_state; + bool has_gyro_motion = false; + for (auto& item : entry.state){ + static_cast(*item).apply(controller_state); + has_gyro_motion |= item->id == (size_t)SwitchResource::GYRO; } OemController_State0x30_Buttons buttons{ @@ -236,17 +236,17 @@ class PABotBase2_JoyCon : controller_state.left_joystick ); - issue_motion_report(cancellable, entry.duration, buttons, controller_state, has_gyro_motion); + issue_motion_report(cancellable, entry.duration, buttons, controller_state, has_gyro_motion); } void execute_state_right_joycon( Cancellable* cancellable, const SuperscalarScheduler::ScheduleEntry& entry ){ - SwitchControllerState controller_state; - bool has_gyro_motion = false; - for (auto& item : entry.state){ - static_cast(*item).apply(controller_state); - has_gyro_motion |= item->id == (size_t)SwitchResource::GYRO; + SwitchControllerState controller_state; + bool has_gyro_motion = false; + for (auto& item : entry.state){ + static_cast(*item).apply(controller_state); + has_gyro_motion |= item->id == (size_t)SwitchResource::GYRO; } OemController_State0x30_Buttons buttons{ @@ -272,7 +272,7 @@ class PABotBase2_JoyCon : << std::chrono::duration_cast(entry.duration).count() << endl; #endif - issue_motion_report(cancellable, entry.duration, buttons, controller_state, has_gyro_motion); + issue_motion_report(cancellable, entry.duration, buttons, controller_state, has_gyro_motion); } virtual void execute_state( Cancellable* cancellable, diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.cpp index a31af8bb30..d5671697bf 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.cpp +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.cpp @@ -5,6 +5,8 @@ */ #include +#include "Common/Cpp/ColoredText.h" +#include "Common/Cpp/Exceptions.h" #include "Common/PABotBase2/Controllers/PABotBase2_Controller_NS1_OemController.h" #include "Controllers/SerialPort/SerialPABotBase.h" #include "NintendoSwitch/NintendoSwitch_Settings.h" @@ -22,7 +24,8 @@ using namespace std::chrono_literals; namespace { -constexpr uint32_t PHYSICAL_MOTION_PROTOCOL_VERSION = 2026061805; +// Protocol 2026061806 from main does not support the physical motion opcode. +constexpr uint32_t PHYSICAL_MOTION_PROTOCOL_VERSION = 2026061807; } @@ -443,6 +446,15 @@ Button PABotBase2_OemController::populate_report_buttons( return all_buttons; } +void PABotBase2_OemController::require_gyro_motion_protocol() const{ + if (m_connection.device().device_protocol_version() < PHYSICAL_MOTION_PROTOCOL_VERSION){ + throw UserSetupError( + m_logger, "Gyro motion requires firmware protocol " + + std::to_string(PHYSICAL_MOTION_PROTOCOL_VERSION) + " or newer." + ); + } +} + void PABotBase2_OemController::issue_motion_report( Cancellable* cancellable, WallDuration duration, @@ -455,9 +467,7 @@ void PABotBase2_OemController::issue_motion_report( return; } - if (m_connection.device().device_protocol_version() < PHYSICAL_MOTION_PROTOCOL_VERSION){ - throw std::runtime_error("Gyro motion requires firmware protocol 2026061805 or newer."); - } + require_gyro_motion_protocol(); const GyroState& state = controller_state.gyro; PABotBase2::Message_Command_NS1_OemController_Motion request{}; diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.h b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.h index b0466f84ab..e5441f7c12 100644 --- a/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.h +++ b/SerialPrograms/Source/NintendoSwitch/Controllers/PABotBase2/NintendoSwitch_PABotBase2_OemController.h @@ -54,6 +54,7 @@ class PABotBase2_OemController : OemController_State0x30_Buttons& buttons, const SwitchControllerState& controller_state ); + void require_gyro_motion_protocol() const; void issue_motion_report( Cancellable* cancellable, WallDuration duration, diff --git a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp index f0da9e1445..366a979e7f 100644 --- a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp +++ b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconGyroTester.cpp @@ -164,6 +164,7 @@ JoyconGyroTester_Descriptor::JoyconGyroTester_Descriptor() "Nintendo Switch", "Joycon Gyro Tester", "", "Build a motion trajectory using acceleration in g and angular velocity in degrees per second.", + PanelQualityStanding::DEVELOPER, ProgramControllerClass::SpecializedController, FeedbackType::NONE, AllowCommandsWhenRunning::DISABLE_COMMANDS @@ -196,6 +197,7 @@ JoyconGyroTester::~JoyconGyroTester() = default; void JoyconGyroTester::program(SingleSwitchProgramEnvironment& env, CancellableScope& scope){ JoyconController& controller = env.console.controller(); + controller.check_gyro_motion_supported(); const std::string error = PHASES.check_validity(); if (!error.empty()){ throw UserSetupError(env.logger(), error);