From 247db9f2ffe1fcaa4d3d297ca597e94f95042711 Mon Sep 17 00:00:00 2001 From: Jens Roth <98889125+jensroth-git@users.noreply.github.com> Date: Sun, 8 Mar 2026 21:54:11 +0100 Subject: [PATCH] Nav Controller Support Added Nav Controller support --- README.md | 8 +++- library.properties | 2 +- psNavResearch.md | 95 +++++++++++++++++++++++++++++++++++++++++++ src/Ps3Controller.cpp | 16 ++++++++ src/Ps3Controller.h | 4 ++ src/include/ps3.h | 12 ++++++ src/include/ps3_int.h | 6 ++- src/ps3.c | 78 ++++++++++++++++++++++++++++------- src/ps3_l2cap.c | 10 ++--- src/ps3_parser.c | 86 +++++++++++++++++++++++++++++++-------- 10 files changed, 278 insertions(+), 39 deletions(-) create mode 100644 psNavResearch.md diff --git a/README.md b/README.md index 917a866..0d291a6 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,8 @@ Description ============== Control your ESP32 projects with a PS3 controller! +Supports **DualShock 3 / SIXAXIS** and **PS Move Navigation Controller** (CECH-ZCS1). Both use the same pairing and connection flow; controller type is detected automatically from the input report. + This library is meant to be used with Espressif's [ESP-IDF](https://github.com/espressif/esp-idf) IoT Development Framework. However, Arduino is supported too! @@ -13,7 +15,7 @@ When a PS3 controller is 'paired' to a PS3 console, it just means that it has st Therefore, if you want to connect your PS3 controller to the ESP32, you either need to figure out what the Bluetooth MAC address of your PS3 console is and set the ESP32's address to it, or change the MAC address stored in the PS3 controller. -Whichever path you choose, you're going to need a tool to read and/or write the currently paired MAC address from the PS3 controller. I used [SixaxisPairTool](https://dancingpixelstudios.com/sixaxis-controller/sixaxispairtool/) for this, but you can try using [sixaxispairer](https://github.com/user-none/sixaxispairer) as well, if open source is important to you. +Whichever path you choose, you're going to need a tool to read and/or write the currently paired MAC address from the PS3 controller. I used [SixaxisPairTool](https://dancingpixelstudios.com/sixaxis-controller/sixaxispairtool/) for this, but you can try using [sixaxispairer](https://github.com/user-none/sixaxispairer) as well, if open source is important to you. The **PS Move Navigation Controller** uses the same pairing mechanism; tools like [8BitDo RR Tools](https://www.8bitdo.com/) or `sixpair` work for both. **Note for ESP-IDF:** If you opted to change the ESP32's MAC address, you'll need to include this snippet in your code **before** calling ```ps3Init()```, where the MAC address should match with the one stored on the PS3 controller: ```c @@ -90,6 +92,10 @@ In order to learn more about how to use this library, please refer to the exampl - Finally, `Ps3Accelerometer` allows you to draw live graphs of the accelerometer data inside the PS3 controller by using `Tools -> Serial Plotter`. +### Navigation Controller support ### + +The library auto-detects the **PS Move Navigation Controller**. Use `Ps3.isNavigationController()` or `Ps3.getControllerType()` to branch your logic. The Nav Controller has no right stick, no accelerometer/gyro, no rumble, and only one LED; the library handles these differences automatically. + Getting Started with ESP-IDF ============== diff --git a/library.properties b/library.properties index 29f43ec..0051ec1 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=PS3 Controller Host -version=1.1.0 +version=1.2.0 author=Jeffrey van Pernis maintainer=Jeffrey van Pernis sentence=Control your ESP32 projects with a PS3 controller! diff --git a/psNavResearch.md b/psNavResearch.md new file mode 100644 index 0000000..d8f4fe9 --- /dev/null +++ b/psNavResearch.md @@ -0,0 +1,95 @@ +# Adding PS Move Navigation Controller to your DualShock 3 driver + +The PS Move Navigation Controller (CECH-ZCS1) is, at the protocol level, **a stripped-down DualShock 3**. It uses the identical Bluetooth connection flow, identical L2CAP channel setup, identical pairing mechanism, identical activation command, and identical 49-byte input report format. If your driver already handles the DS3, the minimum change required is adding a single Product ID (`0x042F`) to your device recognition logic. Everything else—from the L2CAP PSM values to the Feature Report activation sequence—is byte-for-byte the same. The rest of this report details the nuances that matter for a robust implementation. + +## One PID stands between you and full support + +Both controllers share Sony's Vendor ID **`0x054C`** but use different Product IDs: + +| Controller | VID | PID | Bluetooth device name | +|---|---|---|---| +| DualShock 3 / SIXAXIS | `0x054C` | **`0x0268`** | "PLAYSTATION(R)3 Controller" | +| Navigation Controller | `0x054C` | **`0x042F`** | "Navigation Controller" | + +The Bluetooth Class of Device is identical: **`0x002504`** (Major: Peripheral, Minor: Joystick, Service Class: none). Both controllers advertise the same broken HID service via SDP, and both present 148-byte native HID report descriptors that are structurally identical and equally malformed (Collection types swapped between Physical and Application). The PS3 console never reads SDP or the HID descriptor from either controller—it uses hardcoded protocol knowledge—which is why Sony never bothered fixing the descriptors. + +**The single most common reason** a system supporting DS3 fails to recognize the Navigation Controller is a PID whitelist that only includes `0x0268`. Adding `0x042F` and routing it through the same code path is the critical first step. + +## Bluetooth connection and pairing are identical + +The Navigation Controller uses the exact same non-standard Bluetooth pairing and connection sequence as the DS3: + +**Pairing (USB-based, no Bluetooth PIN):** Connect the controller via USB, then write the host's Bluetooth MAC address to **Feature Report `0xF5`** (8 bytes: `[0xF5, 0x01, MAC[0..5]]`). Read the controller's own MAC via **Feature Report `0xF2`** (17 bytes; MAC at bytes 4–9, big-endian). No Bluetooth-level PIN, SSP, or link key exchange occurs. The `sixpair` tool and BlueZ `sixaxis` plugin work for both controllers without modification. + +**Bluetooth connection sequence (controller-initiated):** After pressing the PS button, the controller initiates an ACL connection to the stored host MAC, then opens two L2CAP channels: + +1. **HID Control** — PSM `0x0011` +2. **HID Interrupt** — PSM `0x0013` + +The host must accept incoming connections (page scan mode), perform a role switch to become piconet master, and then send the **activation command** on the HID Control channel: a SET_REPORT (Feature) with HIDP header `0x53`, Report ID `0xF4`, and payload **`0x42, 0x03, 0x00, 0x00`**. Without this activation, neither controller transmits input reports. This sequence is byte-identical for both devices. + +There is **no cryptographic authentication** or challenge-response for either controller. Security relies entirely on physical USB access for initial pairing. Both controllers connect without Bluetooth bonding, which means hosts enforcing `ClassicBondedOnly` (e.g., for CVE-2023-45866 mitigation) will reject both devices equally. + +## The 49-byte input report is the same structure, different contents + +Both controllers send **Report ID `0x01`**, **49 bytes**, at approximately **100 Hz** over the HID Interrupt channel (prefixed with `0xA1` over Bluetooth). The byte layout is identical; the Navigation Controller simply reports neutral/zero values for hardware it lacks: + +| Offset | Field | DS3 | Nav Controller | +|--------|-------|-----|----------------| +| 0 | Report ID | `0x01` | `0x01` | +| 1 | Reserved | `0x00`/`0x01` | Same | +| 2 | Buttons byte 1: Select(0), L3(1), R3(2), Start(3), D-Up(4), D-Right(5), D-Down(6), D-Left(7) | All active | L3, D-pad active; Select/R3/Start = 0 | +| 3 | Buttons byte 2: L2(0), R2(1), L1(2), R1(3), △(4), ○(5), ✕(6), □(7) | All active | L2, L1, ○, ✕ active; R2/R1/△/□ = 0 | +| 4 | PS button (bit 0) | Active | Active | +| 6–7 | Left stick X, Y | `0x00`–`0xFF` | `0x00`–`0xFF` (functional) | +| 8–9 | Right stick X, Y | `0x00`–`0xFF` | **Fixed ~`0x80`** (no hardware) | +| 14–17 | D-pad pressure (Up/Right/Down/Left) | `0x00`–`0xFF` | Functional | +| 18 | L2 analog pressure | `0x00`–`0xFF` | Functional | +| 19–21 | R2/L1/R1 pressure | Active | R2/R1 = `0x00`; L1 functional | +| 22–25 | △/○/✕/□ pressure | Active | ○/✕ functional; △/□ = `0x00` | +| 29 | Battery level | Available | Available | +| 41–42 | Accelerometer X (big-endian) | Live data | **`0x00`** (no hardware) | +| 43–44 | Accelerometer Y | Live data | `0x00` | +| 45–46 | Accelerometer Z | Live data | `0x00` (some revisions send non-zero) | +| 47–48 | Gyroscope Z (big-endian) | Live data | `0x00` (some revisions send non-zero) | + +The **physical buttons** on the Navigation Controller are: left analog stick (with L3 click), D-pad, L1, L2 (analog trigger), Cross (✕), Circle (○), and PS button. That's 9 inputs plus one analog stick. Every other field in the report exists but carries no meaningful data. + +One parsing subtlety: the Linux kernel byte-swaps the accelerometer/gyro values at offsets 41–48 from big-endian to little-endian for the DS3, but **skips this swap for the Navigation Controller** since those bytes are zero/undefined. If your driver applies the swap unconditionally, it's harmless on zeroes but technically incorrect—a clean implementation should gate the swap on controller type. + +**Host-specific packet format:** The HID transport layer may add a header before the 49-byte report. On ESP32/Bluedroid, L2CAP delivers a 50-byte payload: `[0x02][49-byte report]`. Other stacks (e.g. Linux HIDP) may use `[0xA1][49-byte report]`. The report structure (offsets 0–48) is identical; only the transport prefix differs. + +**Sensor bytes vary by revision:** Some Navigation Controller revisions send non-zero values in Accel Z and Gyroscope Z (report bytes 45–48). Accel X and Y (bytes 41–44) appear reliably zero across tested units. For packet-based controller detection (when PID is unavailable at connection time): check Accel X/Y for all-zero, then inspect Accel Z high byte—DS3 real sensor readings use high byte ≥ 0x30 (e.g. 0x33FC), while Nav sends 0 or garbage (e.g. 0x23FB). + +## Output reports, LEDs, and rumble: same format, no effect + +The output report is **Report ID `0x01`, 48 bytes**, sent via SET_REPORT on the HID Control channel (header `0x52`). The format is identical for both controllers: + +| Offset | Purpose | DS3 | Nav Controller | +|--------|---------|-----|----------------| +| 2–3 | Right (weak) motor: duration, on/off | Functional | **No motor** | +| 4–5 | Left (strong) motor: duration, force (`0x00`–`0xFF`) | Functional | **No motor** | +| 10 | LED bitmask (bit1=LED1 … bit4=LED4) | 4 LEDs | **1 LED** (bit 0 = `0x01` only) | +| 11–30 | LED blink parameters (5 bytes × 4 LEDs) | All functional | Only LED 1 | + +The Navigation Controller **accepts** output reports silently but has no rumble motors and only one status LED. Sending rumble commands is harmless. The Linux kernel registers only 1 LED for the Navigation Controller versus 4 for the DS3. + +## What to change in a working DS3 driver + +For a developer with existing DS3 Bluetooth support, here is a prioritized checklist of required and recommended changes: + +- **Required: Add PID `0x042F`** to your device recognition logic alongside `0x0268`. Route it through the same connection, pairing, and activation code paths—they are identical. On hosts where PID is not available at connection time (e.g. some embedded stacks), use packet-based detection: if Accel X/Y (report bytes 41–44) are all zero, treat as Navigation Controller. + +- **Required: Accept the Navigation Controller's SDP/HID descriptor** without errors. If your driver validates the HID descriptor, either skip validation (as the PS3 does) or replace the descriptor entirely. Both controllers ship broken descriptors that no compliant HID parser handles correctly. + +- **Recommended: Gate the accelerometer/gyro byte-swap** on controller type. For PID `0x042F`, skip the `swap(rd[41], rd[42])` through `swap(rd[47], rd[48])` operations since those bytes contain no sensor data. + +- **Recommended: Adjust input mapping** to expose only the Navigation Controller's physical inputs (left stick X/Y, D-pad, L1, L2, L3, Cross, Circle, PS button). Map absent buttons to nothing rather than exposing phantom zero-state inputs. + +- **Recommended: Reduce LED count** to 1 and disable rumble feedback paths for PID `0x042F` to avoid wasted output report traffic. + +- **Optional: Handle hardware revisions gracefully**. Sony shipped multiple Navigation Controller revisions with slightly different native report descriptors. Complete descriptor replacement (rather than patching) is the robust approach, as the Linux kernel learned. + +## Conclusion + +The Navigation Controller is architecturally a DualShock 3 with half the hardware removed. At the Bluetooth protocol level—connection initiation, L2CAP channels, pairing mechanism, activation sequence, report structure, and authentication—the two devices are **indistinguishable** except for the Product ID. The 49-byte input report uses the same byte layout; absent hardware simply reports neutral values. No changes to your Bluetooth stack, L2CAP handling, pairing flow, or HID transport are needed. The entire integration reduces to PID recognition, input mapping adjustments, and minor output report optimizations. The Linux kernel's `hid-sony.c` driver, which has maintained both controllers for over a decade, confirms this with shared code paths gated almost exclusively on quirk flags that differ only in which PID triggered them. \ No newline at end of file diff --git a/src/Ps3Controller.cpp b/src/Ps3Controller.cpp index d3fe457..737e43c 100644 --- a/src/Ps3Controller.cpp +++ b/src/Ps3Controller.cpp @@ -93,7 +93,18 @@ String Ps3Controller::getAddress() { bool Ps3Controller::isConnected() { return ps3IsConnected(); +} + + +ps3_controller_type_t Ps3Controller::getControllerType() +{ + return ps3GetControllerType(); +} + +bool Ps3Controller::isNavigationController() +{ + return ps3GetControllerType() == ps3_controller_nav; } @@ -149,7 +160,12 @@ void Ps3Controller::attachOnConnect(callback_t callback) void Ps3Controller::attachOnDisconnect(callback_t callback) { _callback_disconnect = callback; +} + +void Ps3Controller::setDebugCallback(void (*cb)(const char *)) +{ + ps3SetDebugPrintCallback(cb); } diff --git a/src/Ps3Controller.h b/src/Ps3Controller.h index d97dcfd..821c1dd 100644 --- a/src/Ps3Controller.h +++ b/src/Ps3Controller.h @@ -27,6 +27,8 @@ class Ps3Controller String getAddress(); bool isConnected(); + bool isNavigationController(); + ps3_controller_type_t getControllerType(); void setPlayer(int player); void setRumble(float intensity, int duration = -1); @@ -35,6 +37,8 @@ class Ps3Controller void attachOnConnect(callback_t callback); void attachOnDisconnect(callback_t callback); + void setDebugCallback(void (*cb)(const char *)); + private: static void _event_callback(void *object, ps3_t data, ps3_event_t event); static void _connection_callback(void *object, uint8_t is_connected); diff --git a/src/include/ps3.h b/src/include/ps3.h index f7626c3..99974b3 100644 --- a/src/include/ps3.h +++ b/src/include/ps3.h @@ -6,6 +6,15 @@ /* T Y P E S */ /********************************************************************************/ +/**********************************/ +/* C O N T R O L L E R T Y P E */ +/**********************************/ + +typedef enum { + ps3_controller_ds3 = 0, + ps3_controller_nav = 1 +} ps3_controller_type_t; + /********************/ /* A N A L O G */ /********************/ @@ -144,6 +153,7 @@ typedef struct { ps3_button_t button; ps3_status_t status; ps3_sensor_t sensor; + ps3_controller_type_t controller_type; } ps3_t; @@ -164,6 +174,7 @@ typedef void(*ps3_event_object_callback_t)( void *object, ps3_t ps3, ps3_event_t /********************************************************************************/ bool ps3IsConnected(); +ps3_controller_type_t ps3GetControllerType(); void ps3Init(); void ps3Deinit(); void ps3Enable(); @@ -175,6 +186,7 @@ void ps3SetEventObjectCallback( void *object, ps3_event_object_callback_t cb ); void ps3SetLed( uint8_t player ); void ps3SetLedCmd( ps3_cmd_t *cmd, uint8_t player ); void ps3SetBluetoothMacAddress( const uint8_t *mac ); +void ps3SetDebugPrintCallback( void (*cb)(const char *) ); #endif diff --git a/src/include/ps3_int.h b/src/include/ps3_int.h index 84279bd..ab2f5cc 100644 --- a/src/include/ps3_int.h +++ b/src/include/ps3_int.h @@ -130,7 +130,11 @@ void ps3_packet_event( ps3_t ps3, ps3_event_t event ); /* P A R S E R F U N C T I O N S */ /********************************************************************************/ -void ps3_parse_packet( uint8_t *packet ); +void ps3_parse_packet( uint8_t *packet, uint16_t len ); +ps3_controller_type_t ps3_parse_controller_type( uint8_t *packet, uint16_t len ); + +/* Optional debug output - set via ps3SetDebugPrintCallback() */ +extern void (*ps3_debug_print)(const char *); /********************************************************************************/ diff --git a/src/ps3.c b/src/ps3.c index 145976f..56bfda4 100644 --- a/src/ps3.c +++ b/src/ps3.c @@ -27,6 +27,8 @@ static ps3_event_object_callback_t ps3_event_object_cb = NULL; static void *ps3_event_object = NULL; static bool is_active = false; +static ps3_controller_type_t controller_type = ps3_controller_ds3; +void (*ps3_debug_print)(const char *) = NULL; /********************************************************************************/ @@ -85,6 +87,23 @@ bool ps3IsConnected() } +/******************************************************************************* +** +** Function ps3GetControllerType +** +** Description This returns the type of the connected controller, +** detected from the input report data. +** +** +** Returns ps3_controller_type_t +** +*******************************************************************************/ +ps3_controller_type_t ps3GetControllerType() +{ + return controller_type; +} + + /******************************************************************************* ** ** Function ps3Enable @@ -127,21 +146,31 @@ void ps3Cmd( ps3_cmd_t cmd ) hid_cmd.code = hid_cmd_code_set_report | hid_cmd_code_type_output; hid_cmd.identifier = hid_cmd_identifier_ps3_control; - hid_cmd.data[ps3_control_packet_index_rumble_right_duration] = cmd.rumble_right_duration; - hid_cmd.data[ps3_control_packet_index_rumble_right_intensity] = cmd.rumble_right_intensity; - hid_cmd.data[ps3_control_packet_index_rumble_left_duration] = cmd.rumble_left_duration; - hid_cmd.data[ps3_control_packet_index_rumble_left_intensity] = cmd.rumble_left_intensity; + if (controller_type == ps3_controller_ds3) { + hid_cmd.data[ps3_control_packet_index_rumble_right_duration] = cmd.rumble_right_duration; + hid_cmd.data[ps3_control_packet_index_rumble_right_intensity] = cmd.rumble_right_intensity; + hid_cmd.data[ps3_control_packet_index_rumble_left_duration] = cmd.rumble_left_duration; + hid_cmd.data[ps3_control_packet_index_rumble_left_intensity] = cmd.rumble_left_intensity; + } hid_cmd.data[ps3_control_packet_index_leds] = 0; - if (cmd.led1) hid_cmd.data[ps3_control_packet_index_leds] |= ps3_led_mask_led1; - if (cmd.led2) hid_cmd.data[ps3_control_packet_index_leds] |= ps3_led_mask_led2; - if (cmd.led3) hid_cmd.data[ps3_control_packet_index_leds] |= ps3_led_mask_led3; - if (cmd.led4) hid_cmd.data[ps3_control_packet_index_leds] |= ps3_led_mask_led4; - if (cmd.led1) memcpy( hid_cmd.data + ps3_control_packet_index_led1_arguments, hid_cmd_payload_led_arguments, sizeof(hid_cmd_payload_led_arguments)); - if (cmd.led2) memcpy( hid_cmd.data + ps3_control_packet_index_led2_arguments, hid_cmd_payload_led_arguments, sizeof(hid_cmd_payload_led_arguments)); - if (cmd.led3) memcpy( hid_cmd.data + ps3_control_packet_index_led3_arguments, hid_cmd_payload_led_arguments, sizeof(hid_cmd_payload_led_arguments)); - if (cmd.led4) memcpy( hid_cmd.data + ps3_control_packet_index_led4_arguments, hid_cmd_payload_led_arguments, sizeof(hid_cmd_payload_led_arguments)); + if (controller_type == ps3_controller_nav) { + if (cmd.led1 || cmd.led2 || cmd.led3 || cmd.led4) { + hid_cmd.data[ps3_control_packet_index_leds] |= ps3_led_mask_led1; + memcpy( hid_cmd.data + ps3_control_packet_index_led1_arguments, hid_cmd_payload_led_arguments, sizeof(hid_cmd_payload_led_arguments)); + } + } else { + if (cmd.led1) hid_cmd.data[ps3_control_packet_index_leds] |= ps3_led_mask_led1; + if (cmd.led2) hid_cmd.data[ps3_control_packet_index_leds] |= ps3_led_mask_led2; + if (cmd.led3) hid_cmd.data[ps3_control_packet_index_leds] |= ps3_led_mask_led3; + if (cmd.led4) hid_cmd.data[ps3_control_packet_index_leds] |= ps3_led_mask_led4; + + if (cmd.led1) memcpy( hid_cmd.data + ps3_control_packet_index_led1_arguments, hid_cmd_payload_led_arguments, sizeof(hid_cmd_payload_led_arguments)); + if (cmd.led2) memcpy( hid_cmd.data + ps3_control_packet_index_led2_arguments, hid_cmd_payload_led_arguments, sizeof(hid_cmd_payload_led_arguments)); + if (cmd.led3) memcpy( hid_cmd.data + ps3_control_packet_index_led3_arguments, hid_cmd_payload_led_arguments, sizeof(hid_cmd_payload_led_arguments)); + if (cmd.led4) memcpy( hid_cmd.data + ps3_control_packet_index_led4_arguments, hid_cmd_payload_led_arguments, sizeof(hid_cmd_payload_led_arguments)); + } ps3_l2cap_send_hid( &hid_cmd, len ); } @@ -267,11 +296,29 @@ void ps3SetEventObjectCallback( void *object, ps3_event_object_callback_t cb ) } +/******************************************************************************* +** +** Function ps3SetDebugPrintCallback +** +** Description Registers a callback for debug output (e.g. packet dump). +** Call with NULL to disable. Used for development/diagnostics. +** +** +** Returns void +** +*******************************************************************************/ +void ps3SetDebugPrintCallback( void (*cb)(const char *) ) +{ + ps3_debug_print = cb; +} + + /******************************************************************************* ** ** Function ps3SetBluetoothMacAddress ** -** Description Writes a Registers a callback for receiving PS3 controller events +** Description Sets the ESP32 base MAC so the controller pairs to this host. +** Call before ps3Init(). Base MAC byte 5 is adjusted by -2 for BT. ** ** ** Returns void @@ -298,14 +345,15 @@ void ps3_connect_event( uint8_t is_connected ) ps3Enable(); }else{ is_active = false; + controller_type = ps3_controller_ds3; } } void ps3_packet_event( ps3_t ps3, ps3_event_t event ) { - // Trigger packet event, but if this is the very first packet - // after connecting, trigger a connection event instead + controller_type = ps3.controller_type; + if(is_active){ if(ps3_event_cb != NULL) { diff --git a/src/ps3_l2cap.c b/src/ps3_l2cap.c index 24d7843..6fd3dc1 100644 --- a/src/ps3_l2cap.c +++ b/src/ps3_l2cap.c @@ -24,8 +24,8 @@ /* L O C A L F U N C T I O N P R O T O T Y P E S */ /********************************************************************************/ -static void ps3_l2cap_init_service( char *name, uint16_t psm, uint8_t security_id); -static void ps3_l2cap_deinit_service( char *name, uint16_t psm ); +static void ps3_l2cap_init_service( const char *name, uint16_t psm, uint8_t security_id); +static void ps3_l2cap_deinit_service( const char *name, uint16_t psm ); static void ps3_l2cap_connect_ind_cback (BD_ADDR bd_addr, uint16_t l2cap_cid, uint16_t psm, uint8_t l2cap_id); static void ps3_l2cap_connect_cfm_cback (uint16_t l2cap_cid, uint16_t result); static void ps3_l2cap_config_ind_cback (uint16_t l2cap_cid, tL2CAP_CFG_INFO *p_cfg); @@ -146,7 +146,7 @@ void ps3_l2cap_send_hid( hid_cmd_t *hid_cmd, uint8_t len ) ** Returns void ** *******************************************************************************/ -static void ps3_l2cap_init_service( char *name, uint16_t psm, uint8_t security_id) +static void ps3_l2cap_init_service( const char *name, uint16_t psm, uint8_t security_id) { /* Register the PSM for incoming connections */ if (!L2CA_Register(psm, (tL2CAP_APPL_INFO *) &dyn_info)) { @@ -172,7 +172,7 @@ static void ps3_l2cap_init_service( char *name, uint16_t psm, uint8_t security_i ** Returns void ** *******************************************************************************/ -static void ps3_l2cap_deinit_service( char *name, uint16_t psm ) +static void ps3_l2cap_deinit_service( const char *name, uint16_t psm ) { /* Deregister the PSM from incoming connections */ L2CA_Deregister(psm); @@ -310,7 +310,7 @@ static void ps3_l2cap_data_ind_cback(uint16_t l2cap_cid, BT_HDR *p_buf) { if ( p_buf->len > 2 ) { - ps3_parse_packet( p_buf->data ); + ps3_parse_packet( p_buf->data, p_buf->len ); } osi_free( p_buf ); diff --git a/src/ps3_parser.c b/src/ps3_parser.c index 62bef73..41c3f19 100644 --- a/src/ps3_parser.c +++ b/src/ps3_parser.c @@ -77,9 +77,10 @@ enum ps3_status_mask { /* L O C A L F U N C T I O N P R O T O T Y P E S */ /********************************************************************************/ -ps3_sensor_t ps3_parse_packet_sensor( uint8_t *packet ); +ps3_controller_type_t ps3_parse_controller_type( uint8_t *packet, uint16_t len ); +ps3_sensor_t ps3_parse_packet_sensor( uint8_t *packet, ps3_controller_type_t type ); ps3_status_t ps3_parse_packet_status( uint8_t *packet ); -ps3_analog_stick_t ps3_parse_packet_analog_stick( uint8_t *packet ); +ps3_analog_stick_t ps3_parse_packet_analog_stick( uint8_t *packet, ps3_controller_type_t type ); ps3_analog_button_t ps3_parse_packet_analog_button( uint8_t *packet ); ps3_button_t ps3_parse_packet_buttons( uint8_t *packet ); ps3_event_t ps3_parse_event( ps3_t prev, ps3_t cur ); @@ -101,21 +102,20 @@ void ps3_parser_set_event_cb( ps3_event_callback_t cb ) ps3_event_cb = cb; } -void ps3_parse_packet( uint8_t *packet ) +void ps3_parse_packet( uint8_t *packet, uint16_t len ) { ps3_t prev_ps3 = ps3; - ps3.button = ps3_parse_packet_buttons(packet); - ps3.analog.stick = ps3_parse_packet_analog_stick(packet); - ps3.analog.button = ps3_parse_packet_analog_button(packet); - ps3.sensor = ps3_parse_packet_sensor(packet); - ps3.status = ps3_parse_packet_status(packet); + ps3.controller_type = ps3_parse_controller_type(packet, len); + ps3.button = ps3_parse_packet_buttons(packet); + ps3.analog.stick = ps3_parse_packet_analog_stick(packet, ps3.controller_type); + ps3.analog.button = ps3_parse_packet_analog_button(packet); + ps3.sensor = ps3_parse_packet_sensor(packet, ps3.controller_type); + ps3.status = ps3_parse_packet_status(packet); ps3_event_t ps3_event = ps3_parse_event( prev_ps3, ps3 ); ps3_packet_event( ps3, ps3_event ); - - } @@ -200,10 +200,55 @@ ps3_event_t ps3_parse_event( ps3_t prev, ps3_t cur ) return ps3_event; } +/*****************************************/ +/* C O N T R O L L E R D E T E C T */ +/*****************************************/ +ps3_controller_type_t ps3_parse_controller_type( uint8_t *packet, uint16_t len ) +{ + /* Nav Controller has no accelerometer/gyro hardware. + Accel X/Y (4 bytes) are 0; some revisions send non-zero in Z/Gyro. + Support packet formats: + - [0x02][49-byte report] len=50 (ESP32 L2CAP): sensor bytes at 42-45 + - [0xA1][49-byte report]: sensor bytes at 42-49 + - [0x01] raw 49-byte report: sensor bytes at 41-48 + - [header][49-byte report]: sensor bytes at 51-58 */ + int sensor_offset; + if (packet[0] == 0x02 && len >= 50) { + sensor_offset = 42; + } else if (packet[0] == 0xA1 && len >= 50) { + sensor_offset = 42; + } else if (packet[0] == 0x01 && len >= 49) { + sensor_offset = 41; + } else if (len >= 59) { + sensor_offset = 51; + } else { + return ps3_controller_ds3; + } + + /* Nav: Accel X/Y (4 bytes) are 0. DS3 held flat can also have X/Y=0. Empirically: + DS3 Accel Z high byte >= 0x30 (e.g. 0x33FC), Nav sends 0 or garbage (0x23FB). */ + if (packet[0] == 0x02 && len >= 50) { + if (packet[42] == 0 && packet[43] == 0 && packet[44] == 0 && packet[45] == 0) { + if (packet[46] >= 0x30) { + return ps3_controller_ds3; /* Real sensor: high byte in DS3 range */ + } + return ps3_controller_nav; + } + return ps3_controller_ds3; + } + + for (int i = 0; i < 8; i++) { + if (packet[sensor_offset + i] != 0x00) { + return ps3_controller_ds3; + } + } + return ps3_controller_nav; +} + /********************/ /* A N A L O G */ /********************/ -ps3_analog_stick_t ps3_parse_packet_analog_stick( uint8_t *packet ) +ps3_analog_stick_t ps3_parse_packet_analog_stick( uint8_t *packet, ps3_controller_type_t type ) { ps3_analog_stick_t ps3_analog_stick; @@ -211,8 +256,14 @@ ps3_analog_stick_t ps3_parse_packet_analog_stick( uint8_t *packet ) ps3_analog_stick.lx = (int16_t)packet[ps3_packet_index_analog_stick_lx] - int_offset; ps3_analog_stick.ly = (int16_t)packet[ps3_packet_index_analog_stick_ly] - int_offset; - ps3_analog_stick.rx = (int16_t)packet[ps3_packet_index_analog_stick_rx] - int_offset; - ps3_analog_stick.ry = (int16_t)packet[ps3_packet_index_analog_stick_ry] - int_offset; + + if (type == ps3_controller_nav) { + ps3_analog_stick.rx = 0; + ps3_analog_stick.ry = 0; + } else { + ps3_analog_stick.rx = (int16_t)packet[ps3_packet_index_analog_stick_rx] - int_offset; + ps3_analog_stick.ry = (int16_t)packet[ps3_packet_index_analog_stick_ry] - int_offset; + } return ps3_analog_stick; } @@ -291,9 +342,13 @@ ps3_status_t ps3_parse_packet_status( uint8_t *packet ) /********************/ /* S E N S O R S */ /********************/ -ps3_sensor_t ps3_parse_packet_sensor( uint8_t *packet ) +ps3_sensor_t ps3_parse_packet_sensor( uint8_t *packet, ps3_controller_type_t type ) { - ps3_sensor_t ps3_sensor; + ps3_sensor_t ps3_sensor = {0}; + + if (type == ps3_controller_nav) { + return ps3_sensor; + } const uint16_t int_offset = 0x200; @@ -303,5 +358,4 @@ ps3_sensor_t ps3_parse_packet_sensor( uint8_t *packet ) ps3_sensor.gyroscope.z = (packet[ps3_packet_index_sensor_gyroscope_z] << 8) + packet[ps3_packet_index_sensor_gyroscope_z+1] - int_offset; return ps3_sensor; - }