diff --git a/.github/workflows/build-nrf52.yml b/.github/workflows/build-nrf52.yml index bc0a0fe6..e598f35a 100644 --- a/.github/workflows/build-nrf52.yml +++ b/.github/workflows/build-nrf52.yml @@ -5,6 +5,7 @@ on: - 'firmware/**' - 'firmware-bluetooth/**' workflow_call: + workflow_dispatch: defaults: run: shell: bash --noprofile --norc -x -e -o pipefail {0} @@ -13,15 +14,27 @@ jobs: runs-on: ubuntu-22.04 strategy: matrix: - board: ["adafruit_feather_nrf52840", "seeed_xiao_nrf52840"] + include: + - board: "adafruit_feather_nrf52840" + name: "adafruit_feather_nrf52840" + extra_args: "" + - board: "seeed_xiao_nrf52840" + name: "seeed_xiao_nrf52840" + extra_args: "" + - board: "seeed_xiao_nrf52840" + name: "seeed_xiao_nrf52840_sense" + extra_args: "-- \"-DOVERLAY_CONFIG=boards/arm/seeed_xiao_nrf52840/seeed_xiao_nrf52840_sense.conf\" \"-DDTC_OVERLAY_FILE=boards/arm/seeed_xiao_nrf52840/seeed_xiao_nrf52840_sense.overlay\"" steps: - uses: actions/checkout@v6 - name: Build run: | docker run -v $PWD:/workdir/project -w /workdir/project/firmware-bluetooth nordicplayground/nrfconnect-sdk:v2.2-branch \ - west build -b ${{ matrix.board }} - cp firmware-bluetooth/build/zephyr/remapper.uf2 firmware-bluetooth/remapper_${{ matrix.board }}.uf2 + west build -b ${{ matrix.board }} ${{ matrix.extra_args }} + cp firmware-bluetooth/build/zephyr/remapper.uf2 firmware-bluetooth/remapper_${{ matrix.name }}.uf2 + cp firmware-bluetooth/build/zephyr/remapper.hex firmware-bluetooth/remapper_${{ matrix.name }}.hex - uses: actions/upload-artifact@v7 with: - name: artifact-${{ matrix.board }} - path: firmware-bluetooth/remapper_${{ matrix.board }}.uf2 + name: artifact-${{ matrix.name }} + path: | + firmware-bluetooth/remapper_${{ matrix.name }}.uf2 + firmware-bluetooth/remapper_${{ matrix.name }}.hex diff --git a/.gitignore b/.gitignore index db42cd16..69d5e8eb 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ config-tool/__pycache__ firmware-bluetooth/build firmware/build +/firmware-bluetooth/build-xiao +firmware/build-* diff --git a/BLUETOOTH.md b/BLUETOOTH.md index 5efa40f4..bf74d548 100644 --- a/BLUETOOTH.md +++ b/BLUETOOTH.md @@ -21,6 +21,36 @@ You can tell the remapper is in pairing mode if the blue LED is lit constantly. To make the remapper forget all currently paired devices, hold the "user switch" button for over 3 seconds, or click the "Forget all devices" button on the web configuration tool (or short pin 0 to GND for over 3 seconds on the Seeed Xiao board). +## BLE GATT peripheral input + +The Bluetooth firmware also advertises a Nordic UART Service compatible GATT +peripheral while continuing to scan for Bluetooth LE HID devices as a central. +Reports written to this service are treated as a separate virtual input device, +so they can be remapped together with connected BLE HID devices. + +* Service UUID: `6e400001-b5a3-f393-e0a9-e50e24dcca9e` +* Write characteristic: `6e400002-b5a3-f393-e0a9-e50e24dcca9e` +* Notify characteristic: `6e400003-b5a3-f393-e0a9-e50e24dcca9e` + +The write characteristic requires an encrypted BLE connection, so clients may +need to pair before reports can be written. + +Writes are SLIP-style framed packets with a little-endian CRC32 trailer: +start `0xc0`, escaped payload bytes, four CRC bytes, end `0xc0`. +The decoded payload is: + +| Byte | Meaning | +| ---: | --- | +| 0 | Protocol version, currently `1` | +| 1 | Advisory output descriptor number | +| 2 | HID report payload length | +| 3 | HID report ID | +| 4..N | HID report payload bytes, without the report ID | + +The descriptor number in the packet does not switch the USB descriptor at +runtime. Choose the USB descriptor through the normal HID Remapper configuration +so it is active when the board enumerates over USB. + ## Known issues * Quirks mechanism for fixing broken report descriptors doesn't work. diff --git a/config-tool-web/code.js b/config-tool-web/code.js index 01adc610..6c7f049c 100644 --- a/config-tool-web/code.js +++ b/config-tool-web/code.js @@ -7,8 +7,8 @@ const REPORT_ID_MONITOR = 101; const STICKY_FLAG = 1 << 0; const TAP_FLAG = 1 << 1; const HOLD_FLAG = 1 << 2; -const CONFIG_SIZE = 32; -const CONFIG_VERSION = 18; +const CONFIG_SIZE = 36; +const CONFIG_VERSION = 19; const VENDOR_ID = 0xCAFE; const PRODUCT_ID = 0xBAF2; const DEFAULT_PARTIAL_SCROLL_TIMEOUT = 1000000; @@ -16,6 +16,8 @@ const DEFAULT_TAP_HOLD_THRESHOLD = 200000; const DEFAULT_GPIO_DEBOUNCE_TIME = 5; const DEFAULT_SCALING = 1000; const DEFAULT_MACRO_ENTRY_DURATION = 1; +const DEFAULT_IMU_ANGLE_CLAMP_LIMIT = 45; +const DEFAULT_IMU_FILTER_BUFFER_SIZE = 10; const NLAYERS = 8; const NMACROS = 32; @@ -24,6 +26,7 @@ const MACRO_ITEMS_IN_PACKET = 6; const IGNORE_AUTH_DEV_INPUTS_FLAG = 1 << 4; const GPIO_OUTPUT_MODE_FLAG = 1 << 5; const NORMALIZE_GAMEPAD_INPUTS_FLAG = 1 << 6; +const IMU_ENABLE_FLAG = 1 << 7; const HUB_PORT_NONE = 255; const QUIRK_FLAG_RELATIVE_MASK = 0b10000000; @@ -148,6 +151,9 @@ let config = { 'gpio_output_mode': 0, 'input_labels': 0, 'normalize_gamepad_inputs': true, + 'imu_enabled': false, + 'imu_angle_clamp_limit': DEFAULT_IMU_ANGLE_CLAMP_LIMIT, + 'imu_filter_buffer_size': DEFAULT_IMU_FILTER_BUFFER_SIZE, mappings: [{ 'source_usage': '0x00000000', 'target_usage': '0x00000000', @@ -200,6 +206,8 @@ document.addEventListener("DOMContentLoaded", function () { document.getElementById("tap_hold_threshold_input").addEventListener("change", tap_hold_threshold_onchange); document.getElementById("gpio_debounce_time_input").addEventListener("change", gpio_debounce_time_onchange); document.getElementById("macro_entry_duration_input").addEventListener("change", macro_entry_duration_onchange); + document.getElementById("imu_angle_clamp_limit_input").addEventListener("change", imu_angle_clamp_limit_onchange); + document.getElementById("imu_filter_buffer_size_input").addEventListener("change", imu_filter_buffer_size_onchange); for (let i = 0; i < NLAYERS; i++) { document.getElementById("unmapped_passthrough_checkbox" + i).addEventListener("change", unmapped_passthrough_onchange); } @@ -210,6 +218,11 @@ document.addEventListener("DOMContentLoaded", function () { document.getElementById("input_labels_modal_dropdown").addEventListener("change", input_labels_onchange("input_labels_modal_dropdown")); document.getElementById("ignore_auth_dev_inputs_checkbox").addEventListener("change", ignore_auth_dev_inputs_onchange); document.getElementById("normalize_gamepad_inputs_checkbox").addEventListener("change", normalize_gamepad_inputs_onchange); + document.getElementById("imu_enabled_checkbox").addEventListener("change", imu_enabled_onchange); + document.getElementById("imu_angle_clamp_limit_input").addEventListener("change", imu_angle_clamp_limit_onchange); + document.getElementById("imu_filter_buffer_size_input").addEventListener("change", imu_filter_buffer_size_onchange); + document.getElementById("imu_roll_inverted_checkbox").addEventListener("change", imu_roll_inverted_onchange); + document.getElementById("imu_pitch_inverted_checkbox").addEventListener("change", imu_pitch_inverted_onchange); document.getElementById("nav-monitor-tab").addEventListener("shown.bs.tab", monitor_tab_shown); document.getElementById("nav-monitor-tab").addEventListener("hide.bs.tab", monitor_tab_hide); @@ -290,8 +303,8 @@ async function load_from_device() { try { await send_feature_command(GET_CONFIG); - const [config_version, flags, unmapped_passthrough_layer_mask, partial_scroll_timeout, mapping_count, our_usage_count, their_usage_count, interval_override, tap_hold_threshold, gpio_debounce_time_ms, our_descriptor_number, macro_entry_duration, quirk_count] = - await read_config_feature([UINT8, UINT8, UINT8, UINT32, UINT16, UINT32, UINT32, UINT8, UINT32, UINT8, UINT8, UINT8, UINT16]); + const [config_version, flags, unmapped_passthrough_layer_mask, partial_scroll_timeout, mapping_count, our_usage_count, their_usage_count, interval_override, tap_hold_threshold, gpio_debounce_time_ms, our_descriptor_number, macro_entry_duration, quirk_count, imu_angle_clamp_limit, imu_filter_buffer_size, imu_roll_inverted, imu_pitch_inverted] = + await read_config_feature([UINT8, UINT8, UINT8, UINT32, UINT16, UINT32, UINT32, UINT8, UINT32, UINT8, UINT8, UINT8, UINT16, UINT8, UINT8, UINT8, UINT8]); check_received_version(config_version); config['version'] = config_version; @@ -304,7 +317,12 @@ async function load_from_device() { config['ignore_auth_dev_inputs'] = !!(flags & IGNORE_AUTH_DEV_INPUTS_FLAG); config['gpio_output_mode'] = (flags & GPIO_OUTPUT_MODE_FLAG) ? 1 : 0; config['normalize_gamepad_inputs'] = !!(flags & NORMALIZE_GAMEPAD_INPUTS_FLAG); + config['imu_enabled'] = !!(flags & IMU_ENABLE_FLAG); config['macro_entry_duration'] = macro_entry_duration + 1; + config['imu_angle_clamp_limit'] = imu_angle_clamp_limit; + config['imu_filter_buffer_size'] = imu_filter_buffer_size; + config['imu_roll_inverted'] = !!imu_roll_inverted; + config['imu_pitch_inverted'] = !!imu_pitch_inverted; config['mappings'] = []; for (let i = 0; i < mapping_count; i++) { @@ -442,7 +460,8 @@ async function save_to_device() { await send_feature_command(SUSPEND); const flags = (config['ignore_auth_dev_inputs'] ? IGNORE_AUTH_DEV_INPUTS_FLAG : 0) | (config['gpio_output_mode'] ? GPIO_OUTPUT_MODE_FLAG : 0) | - (config['normalize_gamepad_inputs'] ? NORMALIZE_GAMEPAD_INPUTS_FLAG : 0); + (config['normalize_gamepad_inputs'] ? NORMALIZE_GAMEPAD_INPUTS_FLAG : 0) | + (config['imu_enabled'] ? IMU_ENABLE_FLAG : 0); await send_feature_command(SET_CONFIG, [ [UINT8, flags], [UINT8, layer_list_to_mask(config['unmapped_passthrough_layers'])], @@ -452,6 +471,10 @@ async function save_to_device() { [UINT8, config['gpio_debounce_time_ms']], [UINT8, config['our_descriptor_number']], [UINT8, config['macro_entry_duration'] - 1], + [UINT8, config['imu_angle_clamp_limit']], + [UINT8, config['imu_filter_buffer_size']], + [UINT8, config['imu_roll_inverted'] ? 1 : 0], + [UINT8, config['imu_pitch_inverted'] ? 1 : 0], ]); await send_feature_command(CLEAR_MAPPING); @@ -632,6 +655,11 @@ function set_config_ui_state() { document.getElementById('input_labels_dropdown').value = config['input_labels']; document.getElementById('input_labels_modal_dropdown').value = config['input_labels']; document.getElementById('normalize_gamepad_inputs_checkbox').checked = config['normalize_gamepad_inputs']; + document.getElementById('imu_enabled_checkbox').checked = config['imu_enabled']; + document.getElementById('imu_angle_clamp_limit_input').value = config['imu_angle_clamp_limit'] ?? DEFAULT_IMU_ANGLE_CLAMP_LIMIT; + document.getElementById('imu_filter_buffer_size_input').value = config['imu_filter_buffer_size'] ?? DEFAULT_IMU_FILTER_BUFFER_SIZE; + document.getElementById('imu_roll_inverted_checkbox').checked = config['imu_roll_inverted'] ?? false; + document.getElementById('imu_pitch_inverted_checkbox').checked = config['imu_pitch_inverted'] ?? false; } function set_mappings_ui_state() { @@ -764,6 +792,11 @@ function set_ui_state() { // set it to false to preserve previous behavior. config['normalize_gamepad_inputs'] = false; } + if (config['version'] < 19) { + // IMU settings were added in version 19 + config['imu_angle_clamp_limit'] = DEFAULT_IMU_ANGLE_CLAMP_LIMIT; + config['imu_filter_buffer_size'] = DEFAULT_IMU_FILTER_BUFFER_SIZE; + } if (config['version'] < CONFIG_VERSION) { config['version'] = CONFIG_VERSION; } @@ -1427,6 +1460,34 @@ function normalize_gamepad_inputs_onchange() { config['normalize_gamepad_inputs'] = document.getElementById("normalize_gamepad_inputs_checkbox").checked; } +function imu_enabled_onchange() { + config['imu_enabled'] = document.getElementById("imu_enabled_checkbox").checked; +} + +function imu_angle_clamp_limit_onchange() { + let value = parseInt(document.getElementById("imu_angle_clamp_limit_input").value, 10); + if (isNaN(value)) { + value = DEFAULT_IMU_ANGLE_CLAMP_LIMIT; + } + if (value > 90) { + value = 90; + document.getElementById("imu_angle_clamp_limit_input").value = 90; + } + config['imu_angle_clamp_limit'] = value; +} + +function imu_filter_buffer_size_onchange() { + config['imu_filter_buffer_size'] = parseInt(document.getElementById("imu_filter_buffer_size_input").value, 10); +} + +function imu_roll_inverted_onchange() { + config['imu_roll_inverted'] = document.getElementById("imu_roll_inverted_checkbox").checked; +} + +function imu_pitch_inverted_onchange() { + config['imu_pitch_inverted'] = document.getElementById("imu_pitch_inverted_checkbox").checked; +} + function macro_entry_duration_onchange() { let value = parseInt(document.getElementById("macro_entry_duration_input").value, 10); if (isNaN(value)) { diff --git a/config-tool-web/examples.js b/config-tool-web/examples.js index 1c0ee26f..cc5aa688 100644 --- a/config-tool-web/examples.js +++ b/config-tool-web/examples.js @@ -1,4 +1,215 @@ const examples = [ + { + 'description': 'IMU mouse control', + 'config': { + "version": 19, + "unmapped_passthrough_layers": [], + "partial_scroll_timeout": 1000000, + "interval_override": 0, + "tap_hold_threshold": 200000, + "mappings": [ + { + "target_usage": "0x00010030", + "source_usage": "0xfff30001", + "scaling": 1000, + "layers": [ + 0 + ], + "sticky": false, + "tap": false, + "hold": false, + "source_port": 0, + "target_port": 0 + }, + { + "target_usage": "0x00010031", + "source_usage": "0xfff30002", + "scaling": 1000, + "layers": [ + 0 + ], + "sticky": false, + "tap": false, + "hold": false, + "source_port": 0, + "target_port": 0 + }, + { + "target_usage": "0x00090001", + "source_usage": "0xfff30003", + "scaling": 1000, + "layers": [ + 0 + ], + "sticky": false, + "tap": true, + "hold": false, + "source_port": 0, + "target_port": 0 + } + ], + "macros": [ + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [] + ], + "expressions": [ + "0x0020008f input_state dup abs 5000000 gt mul 5000000 div", + "0x0020008e input_state dup abs 5000000 gt mul 5000000 div", + "0x00200073 input_state 100000 gt", + "", + "", + "", + "", + "" + ], + "gpio_debounce_time_ms": 5, + "our_descriptor_number": 0, + "ignore_auth_dev_inputs": false, + "macro_entry_duration": 1, + "gpio_output_mode": 0, + "quirks": [], + "input_labels": 0, + "normalize_gamepad_inputs": false, + "imu_enabled": true, + "imu_angle_clamp_limit": 30, + "imu_filter_buffer_size": 5 + } + }, + { + 'description': 'IMU Switch gamepad', + 'config': { + "version": 19, + "unmapped_passthrough_layers": [], + "partial_scroll_timeout": 1000000, + "interval_override": 0, + "tap_hold_threshold": 200000, + "mappings": [ + { + "target_usage": "0x00010030", + "source_usage": "0x0020008f", + "scaling": 1000, + "layers": [ + 0 + ], + "sticky": false, + "tap": false, + "hold": false, + "source_port": 0, + "target_port": 2 + }, + { + "target_usage": "0x00010031", + "source_usage": "0x0020008e", + "scaling": 1000, + "layers": [ + 0 + ], + "sticky": false, + "tap": false, + "hold": false, + "source_port": 0, + "target_port": 2 + }, + { + "target_usage": "0x00090003", + "source_usage": "0xfff30001", + "scaling": 1000, + "layers": [ + 0 + ], + "sticky": false, + "tap": true, + "hold": false, + "source_port": 0, + "target_port": 0 + } + ], + "macros": [ + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [], + [] + ], + "expressions": [ + "0x00200073 input_state 100000 gt", + "", + "", + "", + "", + "", + "" + ], + "gpio_debounce_time_ms": 5, + "our_descriptor_number": 2, + "ignore_auth_dev_inputs": false, + "macro_entry_duration": 1, + "gpio_output_mode": 0, + "quirks": [], + "input_labels": 0, + "normalize_gamepad_inputs": false, + "imu_enabled": true, + "imu_angle_clamp_limit": 30, + "imu_filter_buffer_size": 5 + } + }, { 'description': 'map caps lock to control', 'config': { @@ -10143,4 +10354,4 @@ const examples = [ } ]; -export default examples; +export default examples; \ No newline at end of file diff --git a/config-tool-web/index.html b/config-tool-web/index.html index 43d396bc..8249f6fb 100644 --- a/config-tool-web/index.html +++ b/config-tool-web/index.html @@ -227,9 +227,58 @@

HID Remapper Configuration

+
+
+ +
+
+ +
+
+
+
+ +
+
+
+ + ° +
+
1 to 90 degrees
+
+
+
+
+ +
+
+
+ + samples +
+
1(reactive) to 16(stable)
+
+
+
+
+ +
+
+ +
+
+
+
+ +
+
+ +
+

Changes to the emulated device type become active after disconnecting and reconnecting HID Remapper.

Changes to gamepad input normalization are applied after re-plugging the device or HID Remapper.

+

Changes to IMU enable setting are applied after re-plugging the device or HID Remapper.

Custom usages
diff --git a/config-tool-web/usages.js b/config-tool-web/usages.js index 3bce62d8..c396dfca 100644 --- a/config-tool-web/usages.js +++ b/config-tool-web/usages.js @@ -12,6 +12,11 @@ const usages = { "0x00010031": { 'name': 'Cursor Y', 'class': 'mouse' }, "0x00010038": { 'name': 'V scroll', 'class': 'mouse' }, "0x000c0238": { 'name': 'H scroll', 'class': 'mouse' }, + + "0x0020008d": { 'name': 'Yaw', 'class': 'mouse' }, + "0x0020008e": { 'name': 'Pitch', 'class': 'mouse' }, + "0x0020008f": { 'name': 'Roll', 'class': 'mouse' }, + "0x00200073": { 'name': 'Shake', 'class': 'mouse' }, }, 'source_1': { "0x00010030": { 'name': 'Left stick X', 'class': 'gamepad' }, @@ -199,6 +204,11 @@ const usages = { "0x000c00b6": { 'name': 'Previous track', 'class': 'media' }, "0x00000000": { 'name': 'Nothing', 'class': 'other' }, + + "0x0020008d": { 'name': 'Yaw', 'class': 'mouse' }, + "0x0020008e": { 'name': 'Pitch', 'class': 'mouse' }, + "0x0020008f": { 'name': 'Roll', 'class': 'mouse' }, + "0x00200073": { 'name': 'Shake', 'class': 'mouse' }, "0xfff30001": { 'name': 'Expression 1', 'class': 'other' }, "0xfff30002": { 'name': 'Expression 2', 'class': 'other' }, "0xfff30003": { 'name': 'Expression 3', 'class': 'other' }, @@ -425,6 +435,11 @@ const usages = { "0x000c00b7": { 'name': 'Stop', 'class': 'media' }, "0x000c00b5": { 'name': 'Next track', 'class': 'media' }, "0x000c00b6": { 'name': 'Previous track', 'class': 'media' }, + + "0x0020008d": { 'name': 'Yaw', 'class': 'mouse' }, + "0x0020008e": { 'name': 'Pitch', 'class': 'mouse' }, + "0x0020008f": { 'name': 'Roll', 'class': 'mouse' }, + "0x00200073": { 'name': 'Shake', 'class': 'mouse' }, }, 2: { "0x00090001": { 'name': 'Y', 'class': 'mouse' }, @@ -647,4 +662,4 @@ Object.assign(usages[4], common_target_usages); Object.assign(usages[5], common_target_usages); usages[1] = usages[0]; // absolute mouse & keyboard is the same as regular mouse & keyboard -export default usages; +export default usages; \ No newline at end of file diff --git a/config-tool/common.py b/config-tool/common.py index 1dcbcb91..62ebe554 100644 --- a/config-tool/common.py +++ b/config-tool/common.py @@ -11,8 +11,8 @@ CONFIG_USAGE_PAGE = 0xFF00 CONFIG_USAGE = 0x0020 -CONFIG_VERSION = 18 -CONFIG_SIZE = 32 +CONFIG_VERSION = 19 +CONFIG_SIZE = 36 REPORT_ID_CONFIG = 100 DEFAULT_PARTIAL_SCROLL_TIMEOUT = 1000000 @@ -61,6 +61,7 @@ IGNORE_AUTH_DEV_INPUTS_FLAG = 1 << 4 GPIO_OUTPUT_MODE_FLAG = 1 << 5 NORMALIZE_GAMEPAD_INPUTS_FLAG = 1 << 6 +IMU_ENABLE_FLAG = 1 << 7 NMACROS = 32 NEXPRESSIONS = 8 @@ -132,12 +133,12 @@ def check_crc(buf, crc_): - if binascii.crc32(buf[1:29]) != crc_: + if binascii.crc32(buf[1:CONFIG_SIZE-3]) != crc_: raise Exception("CRC mismatch") def add_crc(buf): - return buf + struct.pack("= 18 else False ) +imu_enabled = config.get("imu_enabled", False) +imu_angle_clamp_limit = config.get("imu_angle_clamp_limit", 90) +imu_filter_buffer_size = config.get("imu_filter_buffer_size", 10) +imu_roll_inverted = config.get("imu_roll_inverted", False) +imu_pitch_inverted = config.get("imu_pitch_inverted", False) flags = 0 flags |= IGNORE_AUTH_DEV_INPUTS_FLAG if ignore_auth_dev_inputs else 0 flags |= GPIO_OUTPUT_MODE_FLAG if gpio_output_mode == 1 else 0 flags |= NORMALIZE_GAMEPAD_INPUTS_FLAG if normalize_gamepad_inputs else 0 +flags |= IMU_ENABLE_FLAG if imu_enabled else 0 data = struct.pack( - " +#include + +/ { + aliases { + accel0 = &lsm6ds3tr_c; + }; + + lsm6ds3tr-c-en { + compatible = "regulator-fixed-sync", "regulator-fixed"; + enable-gpios = <&gpio1 8 (NRF_GPIO_DRIVE_S0H1 | GPIO_ACTIVE_HIGH)>; + regulator-name = "LSM6DS3TR_C_EN"; + regulator-boot-on; + regulator-always-on; + startup-delay-us = <10000>; + }; +}; + +&pinctrl { + i2c0_default: i2c0_default { + group1 { + psels = , + ; + }; + }; + + i2c0_sleep: i2c0_sleep { + group1 { + psels = , + ; + low-power-enable; + }; + }; +}; + +&i2c0 { + compatible = "nordic,nrf-twim"; + /* Cannot be used together with spi0. */ + status = "okay"; + pinctrl-0 = <&i2c0_default>; + pinctrl-1 = <&i2c0_sleep>; + pinctrl-names = "default", "sleep"; + clock-frequency = ; + + zephyr,concat-buf-size = <48>; + + lsm6ds3tr_c: lsm6ds3tr-c@6a { + compatible = "st,lsm6dsl"; + reg = <0x6a>; + irq-gpios = <&gpio0 11 GPIO_ACTIVE_HIGH>; + status = "okay"; + }; +}; \ No newline at end of file diff --git a/firmware-bluetooth/prj.conf b/firmware-bluetooth/prj.conf index 99e32201..90deb407 100644 --- a/firmware-bluetooth/prj.conf +++ b/firmware-bluetooth/prj.conf @@ -1,5 +1,5 @@ CONFIG_LOG=y -CONFIG_LOG_MAX_LEVEL=3 +CONFIG_LOG_MAX_LEVEL=2 CONFIG_LOG_FUNC_NAME_PREFIX_INF=y CONFIG_LOG_BUFFER_SIZE=4096 @@ -23,11 +23,23 @@ CONFIG_SETTINGS=y CONFIG_GPIO=y +CONFIG_SENSOR=y +CONFIG_I2C=y +CONFIG_LSM6DSL_TRIGGER_GLOBAL_THREAD=y +CONFIG_LSM6DSL=y +CONFIG_CBPRINTF_FP_SUPPORT=y + CONFIG_BT=y -CONFIG_BT_DEBUG_LOG=y CONFIG_BT_CENTRAL=y +CONFIG_BT_PERIPHERAL=y CONFIG_BT_SMP=y CONFIG_BT_L2CAP_TX_BUF_COUNT=5 +CONFIG_BT_L2CAP_TX_MTU=247 +CONFIG_BT_BUF_ACL_RX_SIZE=251 +CONFIG_BT_BUF_ACL_TX_SIZE=251 +CONFIG_BT_USER_DATA_LEN_UPDATE=y +CONFIG_BT_USER_PHY_UPDATE=y +CONFIG_BT_CTLR_PHY_2M=y CONFIG_BT_GATT_CLIENT=y CONFIG_BT_GATT_DM=y CONFIG_BT_HOGP=y diff --git a/firmware-bluetooth/src/imu.cc b/firmware-bluetooth/src/imu.cc new file mode 100644 index 00000000..f5c5f834 --- /dev/null +++ b/firmware-bluetooth/src/imu.cc @@ -0,0 +1,591 @@ +#include "imu.h" +#include "imu_descriptor.h" +#include "config.h" +#include "descriptor_parser.h" +#include "globals.h" +#include "platform.h" +#include "remapper.h" + +#include +#include +#include +#include +#include +#include + +#if DT_NODE_EXISTS(DT_NODELABEL(lsm6ds3tr_c)) + +#define IMU_VIRTUAL_INTERFACE 0x1000 +#define CALIBRATION_SAMPLES 200 + +#define IMU_SAMPLE_RATE_MS 15 +#define MAX_ERROR_COUNT_BEFORE_BACKOFF 5 +#define ERROR_BACKOFF_MULTIPLIER 4 +#define CALIBRATION_RETRY_DELAY_MS 500 + +#define PI 3.14159265359f +#define DEG_TO_RAD (PI / 180.0f) +#define RAD_TO_DEG (180.0f / PI) +#define GRAVITY 9.81f + +#define LED_ACTIVITY_DURATION_MS 50 + +#define MIN_DT_SECONDS 0.005f +#define MAX_DT_SECONDS 0.050f +#define EXPECTED_DT_SECONDS 0.015f +#define CALIBRATION_SAMPLE_DELAY_MS 5 + +#define IMU_ODR_FREQUENCY 52 +#define ACCEL_SCALE_RANGE 2 +#define GYRO_SCALE_RANGE 125 + +#define MAX_FILTER_BUFFER_SIZE 16 + +typedef struct { + float beta_base; + float beta_min; + float beta_max; + float stationary_threshold; + float accel_trust_threshold_high; + float accel_trust_threshold_low; + float bias_update_rate; + float gyro_deadzone; + float angle_clamp_limit; + float magnitude_filter_alpha; +} imu_config_t; + +static imu_config_t imu_config = { + .beta_base = 0.1f, + .beta_min = 0.01f, + .beta_max = 0.3f, + .stationary_threshold = 0.01f, + .accel_trust_threshold_high = 2.0f, + .accel_trust_threshold_low = 0.5f, + .bias_update_rate = 0.001f, + .gyro_deadzone = 0.001f, + .angle_clamp_limit = 45.0f, + .magnitude_filter_alpha = 0.9f +}; + +typedef struct { + float y, alpha; +} iir_t; + +typedef struct { + float buffer[MAX_FILTER_BUFFER_SIZE]; + int index; + int count; + int size; + bool initialized; +} moving_avg_filter_t; + +static volatile float madgwick_q0 = 1.0f; +static volatile float madgwick_q1 = 0.0f; +static volatile float madgwick_q2 = 0.0f; +static volatile float madgwick_q3 = 0.0f; + +static const struct device* imu_dev; +static void imu_work_fn(struct k_work* work); +static K_WORK_DELAYABLE_DEFINE(imu_work, imu_work_fn); + +static volatile float pitch_offset = 0.0f; +static volatile float roll_offset = 0.0f; +static int64_t last_timestamp = 0; + +static volatile float gyro_bias_x = 0.0f; +static volatile float gyro_bias_y = 0.0f; +static volatile float gyro_bias_z = 0.0f; +static float accel_bias_x = 0.0f; +static float accel_bias_y = 0.0f; +static float accel_bias_z = 0.0f; +static bool is_calibrated = false; + +static iir_t magnitude_filter = {.y = 9.81f, .alpha = 0.9f}; +static uint32_t error_count = 0; + +static uint8_t last_known_angle_clamp_limit = 90; + +static moving_avg_filter_t pitch_filter = { + .index = 0, + .count = 0, + .initialized = false +}; + +static moving_avg_filter_t roll_filter = { + .index = 0, + .count = 0, + .initialized = false +}; + +extern const struct gpio_dt_spec led0; +extern struct k_work_delayable activity_led_off_work; + +static float inv_sqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i >> 1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} + +static float compute_dynamic_beta(float hp_magnitude) { + if (hp_magnitude < imu_config.accel_trust_threshold_low) { + return imu_config.beta_max; + } else if (hp_magnitude > imu_config.accel_trust_threshold_high) { + return imu_config.beta_min; + } else { + float ratio = (hp_magnitude - imu_config.accel_trust_threshold_low) / + (imu_config.accel_trust_threshold_high - imu_config.accel_trust_threshold_low); + return imu_config.beta_max - ratio * (imu_config.beta_max - imu_config.beta_min); + } +} + +static void madgwick_update_imu(float gx, float gy, float gz, float ax, float ay, float az, float dt, float beta) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + qDot1 = 0.5f * (-madgwick_q1 * gx - madgwick_q2 * gy - madgwick_q3 * gz); + qDot2 = 0.5f * (madgwick_q0 * gx + madgwick_q2 * gz - madgwick_q3 * gy); + qDot3 = 0.5f * (madgwick_q0 * gy - madgwick_q1 * gz + madgwick_q3 * gx); + qDot4 = 0.5f * (madgwick_q0 * gz + madgwick_q1 * gy - madgwick_q2 * gx); + + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + recipNorm = inv_sqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + _2q0 = 2.0f * madgwick_q0; + _2q1 = 2.0f * madgwick_q1; + _2q2 = 2.0f * madgwick_q2; + _2q3 = 2.0f * madgwick_q3; + _4q0 = 4.0f * madgwick_q0; + _4q1 = 4.0f * madgwick_q1; + _4q2 = 4.0f * madgwick_q2; + _8q1 = 8.0f * madgwick_q1; + _8q2 = 8.0f * madgwick_q2; + q0q0 = madgwick_q0 * madgwick_q0; + q1q1 = madgwick_q1 * madgwick_q1; + q2q2 = madgwick_q2 * madgwick_q2; + q3q3 = madgwick_q3 * madgwick_q3; + + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * madgwick_q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * madgwick_q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * madgwick_q3 - _2q1 * ax + 4.0f * q2q2 * madgwick_q3 - _2q2 * ay; + recipNorm = inv_sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + madgwick_q0 += qDot1 * dt; + madgwick_q1 += qDot2 * dt; + madgwick_q2 += qDot3 * dt; + madgwick_q3 += qDot4 * dt; + + recipNorm = inv_sqrt(madgwick_q0 * madgwick_q0 + madgwick_q1 * madgwick_q1 + madgwick_q2 * madgwick_q2 + madgwick_q3 * madgwick_q3); + madgwick_q0 *= recipNorm; + madgwick_q1 *= recipNorm; + madgwick_q2 *= recipNorm; + madgwick_q3 *= recipNorm; +} + +static float madgwick_get_pitch(void) { + return asinf(-2.0f * (madgwick_q1 * madgwick_q3 - madgwick_q0 * madgwick_q2)) * RAD_TO_DEG; +} + +static float madgwick_get_roll(void) { + return atan2f(2.0f * (madgwick_q0 * madgwick_q1 + madgwick_q2 * madgwick_q3), + 1.0f - 2.0f * (madgwick_q1 * madgwick_q1 + madgwick_q2 * madgwick_q2)) * RAD_TO_DEG; +} + +static bool read_imu_raw(float *ax, float *ay, float *az, float *gx, float *gy, float *gz) { + struct sensor_value accel[3], gyro[3]; + + if (sensor_sample_fetch(imu_dev) < 0) return false; + + if (sensor_channel_get(imu_dev, SENSOR_CHAN_ACCEL_X, &accel[0]) < 0 || + sensor_channel_get(imu_dev, SENSOR_CHAN_ACCEL_Y, &accel[1]) < 0 || + sensor_channel_get(imu_dev, SENSOR_CHAN_ACCEL_Z, &accel[2]) < 0) { + return false; + } + + if (sensor_channel_get(imu_dev, SENSOR_CHAN_GYRO_X, &gyro[0]) < 0 || + sensor_channel_get(imu_dev, SENSOR_CHAN_GYRO_Y, &gyro[1]) < 0 || + sensor_channel_get(imu_dev, SENSOR_CHAN_GYRO_Z, &gyro[2]) < 0) { + return false; + } + + *ax = (float)sensor_value_to_double(&accel[0]); + *ay = (float)sensor_value_to_double(&accel[1]); + *az = (float)sensor_value_to_double(&accel[2]); + *gx = (float)sensor_value_to_double(&gyro[0]); + *gy = (float)sensor_value_to_double(&gyro[1]); + *gz = (float)sensor_value_to_double(&gyro[2]); + + return true; +} + +static int16_t scale_angle_to_int16(float angle, float min_angle, float max_angle) { + angle = fmaxf(min_angle, fminf(max_angle, angle)); + float normalized = (angle - min_angle) / (max_angle - min_angle); + + int scaled = (int)((normalized - 0.5f) * 65535.0f); + return (int16_t)fmaxf(-32768.0f, fminf(32767.0f, (float)scaled)); +} + +static uint16_t scale_magnitude_to_uint16(float magnitude, float max_magnitude) { + magnitude = fmaxf(0.0f, fminf(max_magnitude, magnitude)); + float normalized = magnitude / max_magnitude; + int scaled = (int)(normalized * 255.0f); + return (uint16_t)fmaxf(0.0f, fminf(255.0f, (float)scaled)); +} + +static void clamp_angle_to_limit(float* angle) { + float current_clamp_limit = (float)imu_angle_clamp_limit; + *angle = fmaxf(-current_clamp_limit, fminf(current_clamp_limit, *angle)); +} + +static float apply_deadzone(float value, float deadzone) { + if (fabsf(value) < deadzone) { + return 0.0f; + } + return value > 0 ? value - deadzone : value + deadzone; +} + +static void calibrate_orientation(float pitch, float roll) { + pitch_offset = pitch; + roll_offset = roll; +} + +static bool calibrate_sensors(void) { + float sum_accel_x = 0.0f, sum_accel_y = 0.0f, sum_accel_z = 0.0f; + float sum_gyro_x = 0.0f, sum_gyro_y = 0.0f, sum_gyro_z = 0.0f; + + for (int i = 0; i < CALIBRATION_SAMPLES; i++) { + float ax, ay, az, gx, gy, gz; + if (!read_imu_raw(&ax, &ay, &az, &gx, &gy, &gz)) { + return false; + } + + sum_accel_x += ax; + sum_accel_y += ay; + sum_accel_z += az; + sum_gyro_x += gx; + sum_gyro_y += gy; + sum_gyro_z += gz; + + k_msleep(CALIBRATION_SAMPLE_DELAY_MS); + } + + accel_bias_x = sum_accel_x / CALIBRATION_SAMPLES; + accel_bias_y = sum_accel_y / CALIBRATION_SAMPLES; + accel_bias_z = sum_accel_z / CALIBRATION_SAMPLES - GRAVITY; + + gyro_bias_x = sum_gyro_x / CALIBRATION_SAMPLES; + gyro_bias_y = sum_gyro_y / CALIBRATION_SAMPLES; + gyro_bias_z = sum_gyro_z / CALIBRATION_SAMPLES; + + return true; +} + +static float iir_update_magnitude(iir_t *filter, float input) { + filter->y = filter->alpha * filter->y + (1.0f - filter->alpha) * input; + return filter->y; +} + +static float moving_avg_filter_update(moving_avg_filter_t *filter, float input) { + int bufsize = filter->size; + if (bufsize < 1) bufsize = 1; + if (bufsize > MAX_FILTER_BUFFER_SIZE) bufsize = MAX_FILTER_BUFFER_SIZE; + filter->buffer[filter->index] = input; + filter->index = (filter->index + 1) % bufsize; + + if (!filter->initialized) { + filter->initialized = true; + filter->count = 1; + return input; + } + + if (filter->count < bufsize) { + filter->count++; + } + + float sum = 0.0f; + for (int i = 0; i < filter->count; i++) { + sum += filter->buffer[i]; + } + + return sum / filter->count; +} + +static void update_gyro_bias_if_stationary(float gx_raw, float gy_raw, float gz_raw, float hp_magnitude) { + if (hp_magnitude < imu_config.stationary_threshold) { + gyro_bias_x += imu_config.bias_update_rate * (gx_raw - gyro_bias_x); + gyro_bias_y += imu_config.bias_update_rate * (gy_raw - gyro_bias_y); + gyro_bias_z += imu_config.bias_update_rate * (gz_raw - gyro_bias_z); + } +} + +static void imu_work_fn(struct k_work* work) { + if (!imu_dev) { + error_count++; + if (error_count > MAX_ERROR_COUNT_BEFORE_BACKOFF) { + + k_work_reschedule(&imu_work, K_MSEC(IMU_SAMPLE_RATE_MS * ERROR_BACKOFF_MULTIPLIER)); + error_count = 0; + } else { + k_work_reschedule(&imu_work, K_MSEC(IMU_SAMPLE_RATE_MS)); + } + return; + } + + int64_t now = k_uptime_get(); + float dt = last_timestamp ? (now - last_timestamp) / 1000.0f : EXPECTED_DT_SECONDS; + last_timestamp = now; + + + if (dt < MIN_DT_SECONDS || dt > MAX_DT_SECONDS) { + dt = EXPECTED_DT_SECONDS; + } + + if (!is_calibrated) { + if (calibrate_sensors()) { + is_calibrated = true; + magnitude_filter.alpha = imu_config.magnitude_filter_alpha; + + float pitch = madgwick_get_pitch(); + float roll = madgwick_get_roll(); + calibrate_orientation(pitch, roll); + } else { + error_count++; + + k_work_reschedule(&imu_work, K_MSEC(CALIBRATION_RETRY_DELAY_MS)); + return; + } + } + + float ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw; + if (!read_imu_raw(&ax_raw, &ay_raw, &az_raw, &gx_raw, &gy_raw, &gz_raw)) { + error_count++; + gpio_pin_set_dt(&led0, false); + + + uint32_t delay_ms = (error_count > MAX_ERROR_COUNT_BEFORE_BACKOFF) ? + IMU_SAMPLE_RATE_MS * ERROR_BACKOFF_MULTIPLIER : + IMU_SAMPLE_RATE_MS; + k_work_reschedule(&imu_work, K_MSEC(delay_ms)); + return; + } + + error_count = 0; + + if (last_known_angle_clamp_limit != imu_angle_clamp_limit) { + last_known_angle_clamp_limit = imu_angle_clamp_limit; + imu_config.angle_clamp_limit = (float)imu_angle_clamp_limit; + + int adaptive_buffer_size = imu_filter_buffer_size; + + pitch_filter.size = adaptive_buffer_size; + roll_filter.size = adaptive_buffer_size; + pitch_filter.initialized = false; + roll_filter.initialized = false; + pitch_filter.count = 0; + roll_filter.count = 0; + pitch_filter.index = 0; + roll_filter.index = 0; + } + + float ax = ax_raw - accel_bias_x; + float ay = ay_raw - accel_bias_y; + float az = az_raw - accel_bias_z; + float gx = gx_raw - gyro_bias_x; + float gy = gy_raw - gyro_bias_y; + float gz = gz_raw - gyro_bias_z; + + gx = apply_deadzone(gx, imu_config.gyro_deadzone); + gy = apply_deadzone(gy, imu_config.gyro_deadzone); + gz = apply_deadzone(gz, imu_config.gyro_deadzone); + + float accel_mag = sqrtf(ax * ax + ay * ay + az * az); + float filtered_mag = iir_update_magnitude(&magnitude_filter, accel_mag); + float hp_magnitude = accel_mag - filtered_mag; + + update_gyro_bias_if_stationary(gx_raw, gy_raw, gz_raw, fabsf(hp_magnitude)); + + float beta = compute_dynamic_beta(fabsf(hp_magnitude)); + + madgwick_update_imu(gx, gy, gz, ax, ay, az, dt, beta); + + float pitch = madgwick_get_pitch(); + float roll = madgwick_get_roll(); + + float pitch_corrected = -(pitch - pitch_offset); + float roll_corrected = roll - roll_offset; + + if (imu_pitch_inverted) { + pitch_corrected = -pitch_corrected; + } + if (imu_roll_inverted) { + roll_corrected = -roll_corrected; + } + + pitch_corrected = moving_avg_filter_update(&pitch_filter, pitch_corrected); + roll_corrected = moving_avg_filter_update(&roll_filter, roll_corrected); + + clamp_angle_to_limit(&pitch_corrected); + clamp_angle_to_limit(&roll_corrected); + + float current_clamp_limit = (float)imu_angle_clamp_limit; + int16_t pitch_scaled = scale_angle_to_int16(pitch_corrected, -current_clamp_limit, current_clamp_limit); + int16_t roll_scaled = scale_angle_to_int16(roll_corrected, -current_clamp_limit, current_clamp_limit); + uint16_t magnitude_scaled = scale_magnitude_to_uint16(hp_magnitude, 25.0f); + + imu_report_t imu_report = { + .pitch = pitch_scaled, + .roll = roll_scaled, + .magnitude = magnitude_scaled + }; + + handle_received_report((uint8_t*)&imu_report, (int)sizeof(imu_report), IMU_VIRTUAL_INTERFACE); + + k_work_cancel_delayable(&activity_led_off_work); + gpio_pin_set_dt(&led0, true); + k_work_reschedule(&activity_led_off_work, K_MSEC(LED_ACTIVITY_DURATION_MS)); + + + k_work_reschedule(&imu_work, K_MSEC(IMU_SAMPLE_RATE_MS)); +} + +bool imu_init() { + imu_dev = DEVICE_DT_GET(DT_NODELABEL(lsm6ds3tr_c)); + + if (!device_is_ready(imu_dev)) { + return false; + } + + imu_config.angle_clamp_limit = (float)imu_angle_clamp_limit; + + int adaptive_buffer_size = imu_filter_buffer_size; + + pitch_filter.size = adaptive_buffer_size; + roll_filter.size = adaptive_buffer_size; + pitch_filter.initialized = false; + roll_filter.initialized = false; + pitch_filter.count = 0; + roll_filter.count = 0; + pitch_filter.index = 0; + roll_filter.index = 0; + + struct sensor_value odr_attr; + odr_attr.val1 = IMU_ODR_FREQUENCY; + odr_attr.val2 = 0; + + if (sensor_attr_set(imu_dev, SENSOR_CHAN_ACCEL_XYZ, + SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) { + return false; + } + + if (sensor_attr_set(imu_dev, SENSOR_CHAN_GYRO_XYZ, + SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) { + return false; + } + + struct sensor_value accel_scale_attr; + accel_scale_attr.val1 = ACCEL_SCALE_RANGE; + accel_scale_attr.val2 = 0; + + sensor_attr_set(imu_dev, SENSOR_CHAN_ACCEL_XYZ, + SENSOR_ATTR_FULL_SCALE, &accel_scale_attr); + + struct sensor_value angular_scale_attr; + angular_scale_attr.val1 = GYRO_SCALE_RANGE; + angular_scale_attr.val2 = 0; + + sensor_attr_set(imu_dev, SENSOR_CHAN_GYRO_XYZ, + SENSOR_ATTR_FULL_SCALE, &angular_scale_attr); + + + float ax, ay, az, gx, gy, gz; + if (!read_imu_raw(&ax, &ay, &az, &gx, &gy, &gz)) { + return false; + } + + parse_descriptor(0x0F0D, 0x00C1, imu_hid_report_desc, IMU_HID_REPORT_DESC_SIZE, IMU_VIRTUAL_INTERFACE, 0); + device_connected_callback(IMU_VIRTUAL_INTERFACE, 0x0F0D, 0x00C1, 0); + + their_descriptor_updated = true; + + + k_work_schedule(&imu_work, K_MSEC(IMU_SAMPLE_RATE_MS)); + + return true; +} + +void imu_recalibrate_orientation() { + if (is_calibrated) { + float pitch = madgwick_get_pitch(); + float roll = madgwick_get_roll(); + calibrate_orientation(pitch, roll); + + int adaptive_buffer_size = imu_filter_buffer_size; + + pitch_filter.size = adaptive_buffer_size; + roll_filter.size = adaptive_buffer_size; + pitch_filter.initialized = false; + roll_filter.initialized = false; + pitch_filter.count = 0; + roll_filter.count = 0; + pitch_filter.index = 0; + roll_filter.index = 0; + + + } +} + +void imu_recalibrate_sensors() { + if (is_calibrated) { + is_calibrated = false; + error_count = 0; + + madgwick_q0 = 1.0f; + madgwick_q1 = 0.0f; + madgwick_q2 = 0.0f; + madgwick_q3 = 0.0f; + + magnitude_filter = (iir_t){.y = 9.81f, .alpha = imu_config.magnitude_filter_alpha}; + + int adaptive_buffer_size = imu_filter_buffer_size; + + pitch_filter.size = adaptive_buffer_size; + roll_filter.size = adaptive_buffer_size; + pitch_filter.initialized = false; + roll_filter.initialized = false; + pitch_filter.count = 0; + roll_filter.count = 0; + pitch_filter.index = 0; + roll_filter.index = 0; + + + } +} + +#else + +bool imu_init() { + return true; +} + +#endif \ No newline at end of file diff --git a/firmware-bluetooth/src/imu.h b/firmware-bluetooth/src/imu.h new file mode 100644 index 00000000..90404d26 --- /dev/null +++ b/firmware-bluetooth/src/imu.h @@ -0,0 +1,13 @@ +#pragma once + +#include +#include +#include +#include "globals.h" + +extern const struct gpio_dt_spec led0; +extern struct k_work_delayable activity_led_off_work; + +bool imu_init(); +void imu_recalibrate_orientation(); +void imu_recalibrate_sensors(); \ No newline at end of file diff --git a/firmware-bluetooth/src/imu_descriptor.h b/firmware-bluetooth/src/imu_descriptor.h new file mode 100644 index 00000000..a1d7dd46 --- /dev/null +++ b/firmware-bluetooth/src/imu_descriptor.h @@ -0,0 +1,50 @@ +#ifndef _IMU_DESCRIPTOR_H_ +#define _IMU_DESCRIPTOR_H_ + +#include + +// Custom HID Report Descriptor for orientation data (pitch, roll) and acceleration magnitude +static const uint8_t imu_hid_report_desc[] = { + 0x05, 0x20, // Usage Page (Sensor) + 0x09, 0x8A, // Usage (Motion: Orientation) + 0xA1, 0x01, // Collection (Application) + + // Pitch (rotation around X-axis) + 0x05, 0x20, // Usage Page (Sensor) + 0x09, 0x8E, // Usage (Orientation: Pitch) + 0x16, 0x00, 0x80, // Logical Minimum (-32768) + 0x26, 0xFF, 0x7F, // Logical Maximum (+32767) + 0x75, 0x10, // Report Size (16 bits) + 0x95, 0x01, // Report Count (1) + 0x55, 0x00, // Unit Exponent (0) + 0x65, 0x14, // Unit (Degrees) + 0x81, 0x02, // Input (Data,Var,Abs) + + // Roll (rotation around Y-axis) + 0x09, 0x8F, // Usage (Orientation: Roll) + 0x81, 0x02, // Input (Data,Var,Abs) + + // Acceleration Magnitude + 0x05, 0x20, // Usage Page (Sensor) + 0x09, 0x73, // Usage (Motion: Acceleration) + 0x15, 0x00, // Logical Minimum (0) + 0x26, 0xFF, 0x00, // Logical Maximum (255) + 0x75, 0x10, // Report Size (16 bits) + 0x95, 0x01, // Report Count (1) + 0x55, 0x00, // Unit Exponent (0) + 0x66, 0x14, 0xF0, // Unit (m/s²) + 0x81, 0x02, // Input (Data,Var,Abs) + + 0xC0, // End Collection +}; + +#define IMU_HID_REPORT_DESC_SIZE sizeof(imu_hid_report_desc) + +// Orientation and magnitude report structure +typedef struct { + int16_t pitch; // Pitch angle (-32768 to +32767 representing -90 to +90 degrees) + int16_t roll; // Roll angle (-32768 to +32767 representing -90 to +90 degrees) + uint16_t magnitude; // High-pass filtered acceleration magnitude (0 to 255 representing dynamic motion intensity, always positive) +} __attribute__((packed)) imu_report_t; + +#endif // _IMU_DESCRIPTOR_H_ \ No newline at end of file diff --git a/firmware-bluetooth/src/main.cc b/firmware-bluetooth/src/main.cc index 913618aa..6e0e140c 100644 --- a/firmware-bluetooth/src/main.cc +++ b/firmware-bluetooth/src/main.cc @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -21,11 +22,13 @@ #include #include "config.h" +#include "imu.h" #include "descriptor_parser.h" #include "globals.h" #include "our_descriptor.h" #include "platform.h" #include "remapper.h" +#include "crc.h" LOG_MODULE_REGISTER(remapper, LOG_LEVEL_DBG); @@ -33,6 +36,49 @@ LOG_MODULE_REGISTER(remapper, LOG_LEVEL_DBG); static const int SCAN_DELAY_MS = 1000; static const int CLEAR_BONDS_BUTTON_PRESS_MS = 3000; +static const uint16_t NUS_VIRTUAL_INTERFACE = 0x7f00; +static const uint16_t NUS_VIRTUAL_VID = 0x0f0d; +static const uint16_t NUS_VIRTUAL_PID = 0x00c1; + +static struct bt_uuid_128 nus_service_uuid = BT_UUID_INIT_128( + 0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, + 0x93, 0xf3, 0xa3, 0xb5, 0x01, 0x00, 0x40, 0x6e); + +static struct bt_uuid_128 nus_rx_uuid = BT_UUID_INIT_128( + 0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, + 0x93, 0xf3, 0xa3, 0xb5, 0x02, 0x00, 0x40, 0x6e); + +static struct bt_uuid_128 nus_tx_uuid = BT_UUID_INIT_128( + 0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, + 0x93, 0xf3, 0xa3, 0xb5, 0x03, 0x00, 0x40, 0x6e); + +#define NUS_PROTOCOL_VERSION 1 +#define NUS_PACKET_BUFFER_SIZE 512 +#define END 0300 +#define ESC 0333 +#define ESC_END 0334 +#define ESC_ESC 0335 + +struct __attribute__((packed)) nus_packet_t { + uint8_t protocol_version; + uint8_t our_descriptor_number; + uint8_t len; + uint8_t report_id; + uint8_t data[0]; +}; + +static uint8_t nus_packet_buffer[NUS_PACKET_BUFFER_SIZE]; +static uint16_t nus_bytes_read = 0; +static bool nus_escaped = false; +static bool nus_overflowed = false; +static struct bt_conn* nus_conn; + +#define NUS_LATENCY_INSTRUMENTATION 1 +#if NUS_LATENCY_INSTRUMENTATION +static uint32_t nus_rx_cycles = 0; +static bool nus_latency_pending = false; +static uint32_t nus_latency_samples = 0; +#endif // these macros don't work in C++ when used directly ("taking address of temporary array") static auto const BT_UUID_HIDS_ = (struct bt_uuid_16) BT_UUID_INIT_16(BT_UUID_HIDS_VAL); @@ -53,8 +99,12 @@ static bool get_report_response_ready = false; static const struct device* hid_dev0; static const struct device* hid_dev1; // config interface +// Forward declarations +static bool do_send_report(uint8_t interface, const uint8_t* report_with_id, uint8_t len); + struct report_type { uint16_t interface; + uint8_t external_report_id; uint8_t len; uint8_t data[65]; }; @@ -87,6 +137,33 @@ K_MSGQ_DEFINE(disconnected_q, sizeof(struct disconnected_type), CONFIG_BT_MAX_CO K_MSGQ_DEFINE(set_report_q, sizeof(struct set_report_type), 8, 4); ATOMIC_DEFINE(tick_pending, 1); +static void nus_start_advertising(void); +static void nus_init_virtual_device(void); +static void nus_process_bytes(const uint8_t* data, uint16_t len); + +static ssize_t nus_rx_write_cb(struct bt_conn* conn, const struct bt_gatt_attr* attr, + const void* buf, uint16_t len, uint16_t offset, uint8_t flags) { + if (conn == nus_conn) { + nus_process_bytes((const uint8_t*) buf, len); + } + return len; +} + +static void nus_ccc_changed(const struct bt_gatt_attr* attr, uint16_t value) { + LOG_DBG("NUS CCC changed: %d", value); +} + +BT_GATT_SERVICE_DEFINE(nus_service, + BT_GATT_PRIMARY_SERVICE(&nus_service_uuid), + BT_GATT_CHARACTERISTIC(&nus_rx_uuid.uuid, + BT_GATT_CHRC_WRITE | BT_GATT_CHRC_WRITE_WITHOUT_RESP, + BT_GATT_PERM_WRITE_ENCRYPT, NULL, nus_rx_write_cb, NULL), + BT_GATT_CHARACTERISTIC(&nus_tx_uuid.uuid, + BT_GATT_CHRC_NOTIFY, + BT_GATT_PERM_NONE, NULL, NULL, NULL), + BT_GATT_CCC(nus_ccc_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE), +); + #define SW0_NODE DT_ALIAS(sw0) #if !DT_NODE_HAS_STATUS(SW0_NODE, okay) #error "Unsupported board: sw0 devicetree alias is not defined" @@ -106,7 +183,7 @@ static const struct gpio_dt_spec button = GPIO_DT_SPEC_GET(SW0_NODE, gpios); static struct gpio_callback button_cb_data; -static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(LED0_NODE, gpios); +const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(LED0_NODE, gpios); static const struct gpio_dt_spec led1 = GPIO_DT_SPEC_GET(LED1_NODE, gpios); static bool scanning = false; @@ -117,7 +194,7 @@ static struct bt_le_conn_param* conn_param = BT_LE_CONN_PARAM(6, 6, 44, 400); static void activity_led_off_work_fn(struct k_work* work) { gpio_pin_set_dt(&led0, false); } -static K_WORK_DELAYABLE_DEFINE(activity_led_off_work, activity_led_off_work_fn); +K_WORK_DELAYABLE_DEFINE(activity_led_off_work, activity_led_off_work_fn); enum class LedMode { OFF = 0, @@ -172,6 +249,200 @@ static void set_led_mode(LedMode led_mode_) { } } +static const uint8_t nus_virtual_gamepad_descriptor[] = { + 0x05, 0x01, // Usage Page (Generic Desktop Ctrls) + 0x09, 0x05, // Usage (Game Pad) + 0xA1, 0x01, // Collection (Application) + 0x85, 0x01, // Report ID (1) + 0x15, 0x00, // Logical Minimum (0) + 0x25, 0x01, // Logical Maximum (1) + 0x35, 0x00, // Physical Minimum (0) + 0x45, 0x01, // Physical Maximum (1) + 0x75, 0x01, // Report Size (1) + 0x95, 0x0E, // Report Count (14) + 0x05, 0x09, // Usage Page (Button) + 0x19, 0x01, // Usage Minimum (1) + 0x29, 0x0E, // Usage Maximum (14) + 0x81, 0x02, // Input (Data,Var,Abs) + 0x95, 0x02, // Report Count (2) + 0x81, 0x01, // Input (Const) + 0x05, 0x01, // Usage Page (Generic Desktop Ctrls) + 0x25, 0x0F, // Logical Maximum (15) + 0x46, 0x3B, 0x01, // Physical Maximum (315) + 0x75, 0x04, // Report Size (4) + 0x95, 0x01, // Report Count (1) + 0x65, 0x14, // Unit (English Rotation) + 0x09, 0x39, // Usage (Hat switch) + 0x81, 0x42, // Input (Data,Var,Abs,Null State) + 0x65, 0x00, // Unit (None) + 0x95, 0x01, // Report Count (1) + 0x81, 0x01, // Input (Const) + 0x26, 0xFF, 0x00, // Logical Maximum (255) + 0x46, 0xFF, 0x00, // Physical Maximum (255) + 0x09, 0x30, // Usage (X) + 0x09, 0x31, // Usage (Y) + 0x09, 0x32, // Usage (Z) + 0x09, 0x35, // Usage (Rz) + 0x75, 0x08, // Report Size (8) + 0x95, 0x04, // Report Count (4) + 0x81, 0x02, // Input (Data,Var,Abs) + 0x75, 0x08, // Report Size (8) + 0x95, 0x01, // Report Count (1) + 0x81, 0x01, // Input (Const) + 0xC0, // End Collection +}; + +static void nus_init_virtual_device(void) { + parse_descriptor(NUS_VIRTUAL_VID, NUS_VIRTUAL_PID, + nus_virtual_gamepad_descriptor, + sizeof(nus_virtual_gamepad_descriptor), + NUS_VIRTUAL_INTERFACE, 0); + device_connected_callback(NUS_VIRTUAL_INTERFACE, NUS_VIRTUAL_VID, NUS_VIRTUAL_PID, 0); + their_descriptor_updated = true; +} + +static void nus_handle_packet(const uint8_t* data, uint16_t len) { + static uint8_t last_descriptor_warning = 0xff; + + if (len < sizeof(nus_packet_t)) { + LOG_WRN("NUS packet too small: %d", len); + return; + } + + const nus_packet_t* msg = (const nus_packet_t*) data; + uint16_t payload_len = len - sizeof(nus_packet_t); + + if ((msg->protocol_version != NUS_PROTOCOL_VERSION) || + (msg->len != payload_len) || + (payload_len > 64) || + (msg->our_descriptor_number >= NOUR_DESCRIPTORS) || + ((msg->report_id == 0) && (payload_len >= 64))) { + LOG_WRN("Invalid NUS packet: proto=%d len=%d payload=%d desc=%d report_id=%d", + msg->protocol_version, msg->len, payload_len, + msg->our_descriptor_number, msg->report_id); + return; + } + + if ((msg->our_descriptor_number != our_descriptor_number) && + (msg->our_descriptor_number != last_descriptor_warning)) { + last_descriptor_warning = msg->our_descriptor_number; + LOG_WRN("NUS descriptor %d does not match active USB descriptor %d", + msg->our_descriptor_number, our_descriptor_number); + } + + struct report_type report = { + .interface = NUS_VIRTUAL_INTERFACE, + .external_report_id = msg->report_id, + .len = (uint8_t) payload_len, + }; + memcpy(report.data, msg->data, payload_len); + if (k_msgq_put(&report_q, &report, K_NO_WAIT)) { + LOG_WRN("Dropped NUS report: report queue full"); + } else { +#if NUS_LATENCY_INSTRUMENTATION + nus_rx_cycles = k_cycle_get_32(); + nus_latency_pending = true; +#endif + } +} + +static void nus_packet_append(uint8_t c) { + if (nus_bytes_read >= sizeof(nus_packet_buffer)) { + if (!nus_overflowed) { + LOG_WRN("NUS packet too large; dropping until frame end"); + } + nus_overflowed = true; + return; + } + nus_packet_buffer[nus_bytes_read++] = c; +} + +static void nus_process_byte(uint8_t c) { + if (nus_escaped) { + switch (c) { + case ESC_END: + nus_packet_append(END); + break; + case ESC_ESC: + nus_packet_append(ESC); + break; + default: + nus_packet_append(c); + break; + } + nus_escaped = false; + return; + } + + switch (c) { + case END: + if (!nus_overflowed && (nus_bytes_read > 4)) { + uint32_t crc = crc32(nus_packet_buffer, nus_bytes_read - 4); + uint32_t received_crc = + (nus_packet_buffer[nus_bytes_read - 4] << 0) | + (nus_packet_buffer[nus_bytes_read - 3] << 8) | + (nus_packet_buffer[nus_bytes_read - 2] << 16) | + (nus_packet_buffer[nus_bytes_read - 1] << 24); + if (crc == received_crc) { + nus_handle_packet(nus_packet_buffer, nus_bytes_read - 4); + } else { + LOG_WRN("NUS CRC error: expected 0x%08X, got 0x%08X", crc, received_crc); + } + } + nus_bytes_read = 0; + nus_escaped = false; + nus_overflowed = false; + break; + case ESC: + if (!nus_overflowed) { + nus_escaped = true; + } + break; + default: + nus_packet_append(c); + break; + } +} + +static void nus_process_bytes(const uint8_t* data, uint16_t len) { + gpio_pin_set_dt(&led0, true); + k_work_reschedule(&activity_led_off_work, K_MSEC(50)); + + for (uint16_t i = 0; i < len; i++) { + nus_process_byte(data[i]); + } +} + +static void nus_start_advertising(void) { + struct bt_le_adv_param adv_param = { + .id = BT_ID_DEFAULT, + .sid = 0, + .secondary_max_skip = 0, + .options = BT_LE_ADV_OPT_CONNECTABLE, + .interval_min = BT_GAP_ADV_FAST_INT_MIN_2, + .interval_max = BT_GAP_ADV_FAST_INT_MAX_2, + .peer = NULL, + }; + + static const struct bt_data ad[] = { + BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)), + BT_DATA_BYTES(BT_DATA_UUID128_ALL, + 0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, + 0x93, 0xf3, 0xa3, 0xb5, 0x01, 0x00, 0x40, 0x6e), + }; + + static const struct bt_data sd[] = { + BT_DATA(BT_DATA_NAME_COMPLETE, "HID Remapper", sizeof("HID Remapper") - 1), + }; + + int err = bt_le_adv_start(&adv_param, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd)); + if (err && err != -EALREADY) { + LOG_ERR("bt_le_adv_start returned %d", err); + return; + } + LOG_INF("NUS advertising started."); +} + static void scan_start() { if (CHK(bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE))) { LOG_DBG("Scanning started."); @@ -196,6 +467,12 @@ static void process_bond(const struct bt_bond_info* info, void* user_data) { } static void count_conn_cb(struct bt_conn* conn, void* data) { + struct bt_conn_info info; + + if (bt_conn_get_info(conn, &info) || (info.role != BT_CONN_ROLE_CENTRAL)) { + return; + } + (*((int*) data))++; } @@ -394,19 +671,35 @@ static void button_cb(const struct device* dev, struct gpio_callback* cb, uint32 static void connected(struct bt_conn* conn, uint8_t conn_err) { char addr[BT_ADDR_LE_STR_LEN]; - scanning = false; - count_connections(); - set_led_mode(LedMode::BLINK); - bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); if (conn_err) { LOG_ERR("Failed to connect to %s (conn_err=%u).", addr, conn_err); k_work_reschedule(&scan_start_work, K_MSEC(SCAN_DELAY_MS)); + return; + } + + struct bt_conn_info info; + if (!CHK(bt_conn_get_info(conn, &info))) { + return; + } + if (info.role == BT_CONN_ROLE_PERIPHERAL) { + if (!nus_conn) { + nus_conn = bt_conn_ref(conn); + LOG_INF("NUS client connected: %s", addr); + CHK(bt_conn_set_security(conn, BT_SECURITY_L2)); + } else { + LOG_WRN("Rejecting extra NUS client: %s", addr); + CHK(bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN)); + } return; } + scanning = false; + count_connections(); + set_led_mode(LedMode::BLINK); + LOG_INF("%s", addr); CHK(bt_conn_set_security(conn, BT_SECURITY_L2)); @@ -414,9 +707,28 @@ static void connected(struct bt_conn* conn, uint8_t conn_err) { static void disconnected(struct bt_conn* conn, uint8_t reason) { char addr[BT_ADDR_LE_STR_LEN]; + struct bt_conn_info info; bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + if (conn == nus_conn) { + LOG_INF("NUS client disconnected: %s (reason=%u)", addr, reason); + bt_conn_unref(nus_conn); + nus_conn = NULL; + nus_bytes_read = 0; + nus_escaped = false; + nus_overflowed = false; +#if NUS_LATENCY_INSTRUMENTATION + nus_latency_pending = false; +#endif + nus_start_advertising(); + return; + } + if (bt_conn_get_info(conn, &info) == 0 && info.role == BT_CONN_ROLE_PERIPHERAL) { + LOG_INF("Peripheral client disconnected: %s (reason=%u)", addr, reason); + return; + } + LOG_INF("%s (reason=%u)", addr, reason); uint8_t conn_idx = bt_conn_index(conn); @@ -433,8 +745,46 @@ static void disconnected(struct bt_conn* conn, uint8_t reason) { k_work_reschedule(&scan_start_work, K_MSEC(SCAN_DELAY_MS)); } +static void nus_optimize_connection(struct bt_conn* conn) { + struct bt_le_conn_param param = { + .interval_min = 6, + .interval_max = 9, + .latency = 0, + .timeout = 400, + }; + int err = bt_conn_le_param_update(conn, ¶m); + if (err) { + LOG_WRN("NUS conn param update failed: %d", err); + } else { + LOG_INF("NUS conn param update requested (7.5-11.25 ms)"); + } + +#if defined(CONFIG_BT_CTLR_PHY_2M) + struct bt_conn_le_phy_param phy = BT_CONN_LE_PHY_PARAM_INIT(BT_GAP_LE_PHY_2M, + BT_GAP_LE_PHY_2M); + err = bt_conn_le_phy_update(conn, &phy); + if (err) { + LOG_WRN("NUS 2M PHY update failed: %d", err); + } +#endif +} + static void security_changed(struct bt_conn* conn, bt_security_t level, enum bt_security_err err) { char addr[BT_ADDR_LE_STR_LEN]; + struct bt_conn_info info; + + if (conn == nus_conn) { + if (!err && level >= BT_SECURITY_L2) { + nus_optimize_connection(conn); + } else if (err) { + LOG_ERR("NUS security failed: level=%u, err=%d", level, err); + } + return; + } + + if (bt_conn_get_info(conn, &info) == 0 && info.role == BT_CONN_ROLE_PERIPHERAL) { + return; + } bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); @@ -504,10 +854,11 @@ static uint8_t hogp_notify_cb(struct bt_hogp* hogp, struct bt_hogp_rep_info* rep static struct report_type buf; buf.interface = hogp_index(hogp) << 8; + buf.external_report_id = 0; buf.len = bt_hogp_rep_size(rep) + 1; buf.data[0] = bt_hogp_rep_id(rep); - memcpy(buf.data + 1, data, buf.len); + memcpy(buf.data + 1, data, buf.len - 1); if (k_msgq_put(&report_q, &buf, K_NO_WAIT)) { // printk("error in k_msg_put(report_q\n"); } @@ -515,18 +866,37 @@ static uint8_t hogp_notify_cb(struct bt_hogp* hogp, struct bt_hogp_rep_info* rep return BT_GATT_ITER_CONTINUE; } -// XXX is this ready for simultaneous connection setup? is discovery ready? do we care? -static struct descriptor_type their_descriptor; +static struct descriptor_type their_descriptors[CONFIG_BT_MAX_CONN]; static void hogp_map_read_cb(struct bt_hogp* hogp, uint8_t err, const uint8_t* data, size_t size, size_t offset) { + int8_t conn_idx = hogp_index(hogp); + if (conn_idx < 0) { + return; + } + + if (err) { + LOG_ERR("HOGP descriptor read failed for conn_idx=%d err=%d", conn_idx, err); + return; + } + + struct descriptor_type* their_descriptor = &their_descriptors[conn_idx]; + if (data == NULL) { - their_descriptor.size = offset; - their_descriptor.conn_idx = hogp_index(hogp); - CHK(k_msgq_put(&descriptor_q, &their_descriptor, K_NO_WAIT)); + their_descriptor->size = offset; + their_descriptor->conn_idx = conn_idx; + CHK(k_msgq_put(&descriptor_q, their_descriptor, K_NO_WAIT)); return; } - memcpy(their_descriptor.data + offset, data, size); + if (offset >= sizeof(their_descriptor->data)) { + LOG_WRN("HOGP descriptor too large for conn_idx=%d; dropping remainder", conn_idx); + return; + } + if ((offset + size) > sizeof(their_descriptor->data)) { + LOG_WRN("HOGP descriptor too large for conn_idx=%d; truncating", conn_idx); + size = sizeof(their_descriptor->data) - offset; + } + memcpy(their_descriptor->data + offset, data, size); bt_hogp_map_read(hogp, hogp_map_read_cb, offset + size, K_NO_WAIT); } @@ -685,6 +1055,7 @@ static void int_out_ready_cb0(const struct device* dev) { uint32_t len; if (CHK(hid_int_ep_read(hid_dev0, buf.data, sizeof(buf.data), &len))) { buf.interface = OUR_OUT_INTERFACE; + buf.external_report_id = 0; buf.len = len; CHK(k_msgq_put(&report_q, &buf, K_NO_WAIT)); } @@ -712,12 +1083,22 @@ static bool do_send_report(uint8_t interface, const uint8_t* report_with_id, uin report_with_id++; len--; } + bool sent = false; if (interface == 0) { - return CHK(hid_int_ep_write(hid_dev0, report_with_id, len, NULL)); + sent = CHK(hid_int_ep_write(hid_dev0, report_with_id, len, NULL)); + } else if (interface == 1) { + sent = CHK(hid_int_ep_write(hid_dev1, report_with_id, len, NULL)); } - if (interface == 1) { - return CHK(hid_int_ep_write(hid_dev1, report_with_id, len, NULL)); +#if NUS_LATENCY_INSTRUMENTATION + if (sent && interface == 0 && nus_latency_pending) { + uint32_t delta = k_cycle_get_32() - nus_rx_cycles; + nus_latency_pending = false; + if ((nus_latency_samples++ & 0x3f) == 0) { + LOG_INF("NUS RX->USB latency: %u us", k_cyc_to_us_near32(delta)); + } } +#endif + return sent; } static void button_init() { @@ -905,6 +1286,7 @@ int main() { my_mutexes_init(); button_init(); leds_init(); + bt_init(); CHK(settings_subsys_init()); CHK(settings_register(&our_settings_handlers)); @@ -914,6 +1296,23 @@ int main() { scan_init(); parse_our_descriptor(); set_mapping_from_config(); + nus_init_virtual_device(); + update_their_descriptor_derivates(); + their_descriptor_updated = false; + nus_start_advertising(); + + // Initialize 6-axis IMU AFTER mapping system is ready +#if DT_NODE_EXISTS(DT_NODELABEL(lsm6ds3tr_c)) + if (imu_enabled) { + if (!imu_init()) { + LOG_ERR("Failed to initialize 6-axis IMU"); + } + } else { + LOG_INF("IMU disabled in configuration - skipping IMU initialization"); + } +#else + LOG_INF("IMU not available on this board - skipping IMU initialization"); +#endif k_work_reschedule(&scan_start_work, K_MSEC(SCAN_DELAY_MS)); @@ -927,7 +1326,9 @@ int main() { while (true) { if (!process_pending && !k_msgq_get(&report_q, &incoming_report, K_NO_WAIT)) { - handle_received_report(incoming_report.data, incoming_report.len, (uint16_t) incoming_report.interface); + handle_received_report(incoming_report.data, incoming_report.len, + (uint16_t) incoming_report.interface, + incoming_report.external_report_id); process_pending = true; } if (atomic_test_and_clear_bit(tick_pending, 0)) { diff --git a/firmware/src/config.cc b/firmware/src/config.cc index 18764e54..27a1c83c 100644 --- a/firmware/src/config.cc +++ b/firmware/src/config.cc @@ -10,7 +10,7 @@ #include "platform.h" #include "remapper.h" -const uint8_t CONFIG_VERSION = 18; +const uint8_t CONFIG_VERSION = 19; const uint8_t CONFIG_FLAG_UNMAPPED_PASSTHROUGH = 0x01; const uint8_t CONFIG_FLAG_UNMAPPED_PASSTHROUGH_MASK = 0b00001111; @@ -18,6 +18,7 @@ const uint8_t CONFIG_FLAG_UNMAPPED_PASSTHROUGH_BIT = 0; const uint8_t CONFIG_FLAG_IGNORE_AUTH_DEV_INPUTS_BIT = 4; const uint8_t CONFIG_FLAG_GPIO_OUTPUT_MODE_BIT = 5; const uint8_t CONFIG_FLAG_NORMALIZE_GAMEPAD_INPUTS_BIT = 6; +const uint8_t CONFIG_FLAG_IMU_ENABLE_BIT = 7; ConfigCommand last_config_command = ConfigCommand::NO_COMMAND; uint32_t requested_index = 0; @@ -549,6 +550,8 @@ void load_config_v13(const uint8_t* persisted_config) { my_mutex_exit(MutexId::QUIRKS); } + + void load_config(const uint8_t* persisted_config) { if (!checksum_ok(persisted_config, PERSISTED_CONFIG_SIZE) || !persisted_version_ok(persisted_config)) { return; @@ -562,6 +565,14 @@ void load_config(const uint8_t* persisted_config) { normalize_gamepad_inputs = false; } + if (version < 19) { + imu_angle_clamp_limit = 45; + imu_filter_buffer_size = 10; + imu_enabled = false; + imu_roll_inverted = false; + imu_pitch_inverted = false; + } + if ((version == 3) || (version == 4)) { load_config_v3_v4(persisted_config); return; @@ -616,11 +627,13 @@ void load_config(const uint8_t* persisted_config) { return; } - persist_config_v18_t* config = (persist_config_v18_t*) persisted_config; + + persist_config_v19_t* config = (persist_config_v19_t*) persisted_config; unmapped_passthrough_layer_mask = config->unmapped_passthrough_layer_mask; ignore_auth_dev_inputs = config->flags & (1 << CONFIG_FLAG_IGNORE_AUTH_DEV_INPUTS_BIT); gpio_output_mode = !!(config->flags & (1 << CONFIG_FLAG_GPIO_OUTPUT_MODE_BIT)); normalize_gamepad_inputs = !!(config->flags & (1 << CONFIG_FLAG_NORMALIZE_GAMEPAD_INPUTS_BIT)); + imu_enabled = !!(config->flags & (1 << CONFIG_FLAG_IMU_ENABLE_BIT)); partial_scroll_timeout = config->partial_scroll_timeout; tap_hold_threshold = config->tap_hold_threshold; gpio_debounce_time = config->gpio_debounce_time_ms * 1000; @@ -630,12 +643,27 @@ void load_config(const uint8_t* persisted_config) { our_descriptor_number = 0; } macro_entry_duration = config->macro_entry_duration; - mapping_config11_t* buffer_mappings = (mapping_config11_t*) (persisted_config + sizeof(persist_config_v18_t)); + if (version >= 19) { + imu_angle_clamp_limit = config->imu_angle_clamp_limit; + if (imu_angle_clamp_limit > 90) { + imu_angle_clamp_limit = 90; + } + imu_filter_buffer_size = config->imu_filter_buffer_size; + if (imu_filter_buffer_size < 1) { + imu_filter_buffer_size = 1; + } + if (imu_filter_buffer_size > 16) { + imu_filter_buffer_size = 16; + } + imu_roll_inverted = config->imu_roll_inverted; + imu_pitch_inverted = config->imu_pitch_inverted; + } + mapping_config11_t* buffer_mappings = (mapping_config11_t*) (persisted_config + sizeof(persist_config_v19_t)); for (uint32_t i = 0; i < config->mapping_count; i++) { config_mappings.push_back(buffer_mappings[i]); } - const uint8_t* macros_config_ptr = (persisted_config + sizeof(persist_config_v18_t) + config->mapping_count * sizeof(mapping_config11_t)); + const uint8_t* macros_config_ptr = (persisted_config + sizeof(persist_config_v19_t) + config->mapping_count * sizeof(mapping_config11_t)); my_mutex_enter(MutexId::MACROS); for (int i = 0; i < NMACROS; i++) { macros[i].clear(); @@ -690,6 +718,7 @@ void fill_get_config(get_config_t* config) { config->flags |= ignore_auth_dev_inputs << CONFIG_FLAG_IGNORE_AUTH_DEV_INPUTS_BIT; config->flags |= gpio_output_mode << CONFIG_FLAG_GPIO_OUTPUT_MODE_BIT; config->flags |= normalize_gamepad_inputs << CONFIG_FLAG_NORMALIZE_GAMEPAD_INPUTS_BIT; + config->flags |= imu_enabled << CONFIG_FLAG_IMU_ENABLE_BIT; config->unmapped_passthrough_layer_mask = unmapped_passthrough_layer_mask; config->partial_scroll_timeout = partial_scroll_timeout; config->tap_hold_threshold = tap_hold_threshold; @@ -700,6 +729,10 @@ void fill_get_config(get_config_t* config) { config->interval_override = interval_override; config->our_descriptor_number = our_descriptor_number; config->macro_entry_duration = macro_entry_duration; + config->imu_angle_clamp_limit = imu_angle_clamp_limit; + config->imu_filter_buffer_size = imu_filter_buffer_size; + config->imu_roll_inverted = imu_roll_inverted; + config->imu_pitch_inverted = imu_pitch_inverted; my_mutex_enter(MutexId::QUIRKS); config->quirk_count = quirks.size(); my_mutex_exit(MutexId::QUIRKS); @@ -711,6 +744,7 @@ void fill_persist_config(persist_config_t* config) { config->flags |= ignore_auth_dev_inputs << CONFIG_FLAG_IGNORE_AUTH_DEV_INPUTS_BIT; config->flags |= gpio_output_mode << CONFIG_FLAG_GPIO_OUTPUT_MODE_BIT; config->flags |= normalize_gamepad_inputs << CONFIG_FLAG_NORMALIZE_GAMEPAD_INPUTS_BIT; + config->flags |= imu_enabled << CONFIG_FLAG_IMU_ENABLE_BIT; config->unmapped_passthrough_layer_mask = unmapped_passthrough_layer_mask; config->partial_scroll_timeout = partial_scroll_timeout; config->tap_hold_threshold = tap_hold_threshold; @@ -719,6 +753,10 @@ void fill_persist_config(persist_config_t* config) { config->interval_override = interval_override; config->our_descriptor_number = our_descriptor_number; config->macro_entry_duration = macro_entry_duration; + config->imu_angle_clamp_limit = imu_angle_clamp_limit; + config->imu_filter_buffer_size = imu_filter_buffer_size; + config->imu_roll_inverted = imu_roll_inverted; + config->imu_pitch_inverted = imu_pitch_inverted; my_mutex_enter(MutexId::QUIRKS); config->quirk_count = quirks.size(); my_mutex_exit(MutexId::QUIRKS); @@ -803,7 +841,7 @@ PersistConfigReturnCode persist_config() { my_mutex_enter(MutexId::QUIRKS); quirk_t* quirk_config_ptr = (quirk_t*) expr_config_ptr; - for (uint16_t i = 0; i < quirks.size(); i++) { + for (size_t i = 0; i < quirks.size(); i++) { *quirk_config_ptr = quirks[i]; quirk_config_ptr++; } @@ -971,6 +1009,7 @@ void handle_set_report1(uint8_t report_id, uint8_t const* buffer, uint16_t bufsi ignore_auth_dev_inputs = config->flags & (1 << CONFIG_FLAG_IGNORE_AUTH_DEV_INPUTS_BIT); gpio_output_mode = !!(config->flags & (1 << CONFIG_FLAG_GPIO_OUTPUT_MODE_BIT)); normalize_gamepad_inputs = !!(config->flags & (1 << CONFIG_FLAG_NORMALIZE_GAMEPAD_INPUTS_BIT)); + imu_enabled = !!(config->flags & (1 << CONFIG_FLAG_IMU_ENABLE_BIT)); partial_scroll_timeout = config->partial_scroll_timeout; tap_hold_threshold = config->tap_hold_threshold; gpio_debounce_time = config->gpio_debounce_time_ms * 1000; @@ -984,6 +1023,19 @@ void handle_set_report1(uint8_t report_id, uint8_t const* buffer, uint16_t bufsi our_descriptor_number = 0; } macro_entry_duration = config->macro_entry_duration; + imu_angle_clamp_limit = config->imu_angle_clamp_limit; + if (imu_angle_clamp_limit > 90) { + imu_angle_clamp_limit = 90; + } + imu_filter_buffer_size = config->imu_filter_buffer_size; + if (imu_filter_buffer_size < 1) { + imu_filter_buffer_size = 1; + } + if (imu_filter_buffer_size > 16) { + imu_filter_buffer_size = 16; + } + imu_roll_inverted = config->imu_roll_inverted; + imu_pitch_inverted = config->imu_pitch_inverted; break; } case ConfigCommand::GET_CONFIG: diff --git a/firmware/src/globals.cc b/firmware/src/globals.cc index c0372823..00015c96 100644 --- a/firmware/src/globals.cc +++ b/firmware/src/globals.cc @@ -32,6 +32,11 @@ bool ignore_auth_dev_inputs = false; uint8_t macro_entry_duration = 0; // 0 means 1ms uint8_t gpio_output_mode = 0; bool normalize_gamepad_inputs = true; +bool imu_enabled = false; +uint8_t imu_angle_clamp_limit = 45; +uint8_t imu_filter_buffer_size = 10; +bool imu_roll_inverted = false; +bool imu_pitch_inverted = false; std::vector config_mappings; diff --git a/firmware/src/globals.h b/firmware/src/globals.h index 5e9e3ad2..9cdc3fcc 100644 --- a/firmware/src/globals.h +++ b/firmware/src/globals.h @@ -39,6 +39,11 @@ extern bool ignore_auth_dev_inputs; extern uint8_t macro_entry_duration; extern uint8_t gpio_output_mode; extern bool normalize_gamepad_inputs; +extern bool imu_enabled; +extern uint8_t imu_angle_clamp_limit; +extern uint8_t imu_filter_buffer_size; +extern bool imu_roll_inverted; +extern bool imu_pitch_inverted; extern std::vector config_mappings; diff --git a/firmware/src/our_descriptor.h b/firmware/src/our_descriptor.h index e09a03af..3f3fa01a 100644 --- a/firmware/src/our_descriptor.h +++ b/firmware/src/our_descriptor.h @@ -3,7 +3,7 @@ #include -#define CONFIG_SIZE 32 +#define CONFIG_SIZE 36 #define RESOLUTION_MULTIPLIER 120 #define REPORT_ID_LEDS 98 diff --git a/firmware/src/types.h b/firmware/src/types.h index 3bfa1be6..f912abfa 100644 --- a/firmware/src/types.h +++ b/firmware/src/types.h @@ -201,7 +201,7 @@ struct __attribute__((packed)) set_feature_t { }; struct __attribute__((packed)) get_feature_t { - uint8_t data[28]; + uint8_t data[32]; uint32_t crc32; }; @@ -310,7 +310,25 @@ struct __attribute__((packed)) persist_config_v12_t { typedef persist_config_v12_t persist_config_v13_t; -typedef persist_config_v13_t persist_config_v18_t; +struct __attribute__((packed)) persist_config_v19_t { + uint8_t version; + uint8_t flags; + uint8_t unmapped_passthrough_layer_mask; + uint32_t partial_scroll_timeout; + uint16_t mapping_count; + uint8_t interval_override; + uint32_t tap_hold_threshold; + uint8_t gpio_debounce_time_ms; + uint8_t our_descriptor_number; + uint8_t macro_entry_duration; + uint16_t quirk_count; + uint8_t imu_angle_clamp_limit; + uint8_t imu_filter_buffer_size; + uint8_t imu_roll_inverted; + uint8_t imu_pitch_inverted; +}; + +typedef persist_config_v19_t persist_config_v18_t; typedef persist_config_v18_t persist_config_t; @@ -328,6 +346,10 @@ struct __attribute__((packed)) get_config_t { uint8_t our_descriptor_number; uint8_t macro_entry_duration; uint16_t quirk_count; + uint8_t imu_angle_clamp_limit; + uint8_t imu_filter_buffer_size; + uint8_t imu_roll_inverted; + uint8_t imu_pitch_inverted; }; struct __attribute__((packed)) set_config_t { @@ -339,6 +361,10 @@ struct __attribute__((packed)) set_config_t { uint8_t gpio_debounce_time_ms; uint8_t our_descriptor_number; uint8_t macro_entry_duration; + uint8_t imu_angle_clamp_limit; + uint8_t imu_filter_buffer_size; + uint8_t imu_roll_inverted; + uint8_t imu_pitch_inverted; }; struct __attribute__((packed)) get_indexed_t { diff --git a/transmitter-ble-web/code.js b/transmitter-ble-web/code.js new file mode 100644 index 00000000..9c576fa5 --- /dev/null +++ b/transmitter-ble-web/code.js @@ -0,0 +1,238 @@ +import crc32 from "../config-tool-web/crc.js"; + +const NUS_SERVICE_UUID = "6e400001-b5a3-f393-e0a9-e50e24dcca9e"; +const NUS_RX_UUID = "6e400002-b5a3-f393-e0a9-e50e24dcca9e"; + +const SLIP_END = 0xc0; +const SLIP_ESC = 0xdb; +const SLIP_ESC_END = 0xdc; +const SLIP_ESC_ESC = 0xdd; + +const statusEl = document.querySelector("#status"); +const connectButton = document.querySelector("#connect"); +const connectFilteredButton = document.querySelector("#connect-filtered"); +const disconnectButton = document.querySelector("#disconnect"); +const pairButton = document.querySelector("#pair"); +const descriptorSelect = document.querySelector("#descriptor"); + +let device; +let server; +let rxCharacteristic; +let report = neutralReport(); +let encryptedLinkReady = false; +let reportDirty = false; +let writeInFlight = false; + +function setStatus(message) { + statusEl.textContent = message; +} + +function neutralReport() { + return new Uint8Array([0x00, 0x00, 0x0f, 0x80, 0x80, 0x80, 0x80, 0x00]); +} + +function setConnectedState(connected) { + connectButton.disabled = connected; + connectFilteredButton.disabled = connected; + disconnectButton.disabled = !connected; + pairButton.disabled = !connected; +} + +function formatWriteError(error) { + const message = error.message || String(error); + + if (error.name === "NetworkError" || /encrypt|security|auth|pair/i.test(message)) { + return `${message}. The firmware requires an encrypted BLE link; accept the browser or OS pairing prompt, then try again.`; + } + + if (error.name === "NotAllowedError") { + return `${message}. Bluetooth access or pairing was cancelled.`; + } + + return message; +} + +function framePacket(payload) { + const packet = new Uint8Array(payload.length + 4); + packet.set(payload); + + const view = new DataView(packet.buffer); + const crc = crc32(new DataView(payload.buffer, payload.byteOffset, payload.byteLength), payload.length); + view.setUint32(payload.length, crc, true); + + const framed = [SLIP_END]; + for (const byte of packet) { + if (byte === SLIP_END) { + framed.push(SLIP_ESC, SLIP_ESC_END); + } else if (byte === SLIP_ESC) { + framed.push(SLIP_ESC, SLIP_ESC_ESC); + } else { + framed.push(byte); + } + } + framed.push(SLIP_END); + return new Uint8Array(framed); +} + +async function sendCurrentReport(requireResponse = false) { + if (!rxCharacteristic) { + setStatus("Not connected"); + return; + } + + const payload = new Uint8Array(4 + report.length); + payload[0] = 1; + payload[1] = Number(descriptorSelect.value); + payload[2] = report.length; + payload[3] = 1; + payload.set(report, 4); + + const framed = framePacket(payload); + const useResponse = requireResponse || !encryptedLinkReady; + const t0 = performance.now(); + + if (useResponse && rxCharacteristic.writeValueWithResponse) { + await rxCharacteristic.writeValueWithResponse(framed); + encryptedLinkReady = true; + } else if (rxCharacteristic.writeValueWithoutResponse) { + await rxCharacteristic.writeValueWithoutResponse(framed); + } else if (rxCharacteristic.writeValue) { + await rxCharacteristic.writeValue(framed); + } else if (rxCharacteristic.writeValueWithResponse) { + await rxCharacteristic.writeValueWithResponse(framed); + encryptedLinkReady = true; + } + + const elapsedMs = performance.now() - t0; + const mode = useResponse ? "encrypted write" : "fast write"; + setStatus(`Sent ${report.length} byte report (${mode}, ${elapsedMs.toFixed(1)} ms) to ${device.name || "HID Remapper"}`); +} + +async function flushReportWrites(requireResponse = false) { + writeInFlight = true; + try { + while (reportDirty) { + reportDirty = false; + const useResponse = requireResponse || !encryptedLinkReady; + await sendCurrentReport(useResponse); + requireResponse = false; + } + } catch (error) { + if (requireResponse || !encryptedLinkReady) { + encryptedLinkReady = false; + } + setStatus(formatWriteError(error)); + } finally { + writeInFlight = false; + if (reportDirty) { + await flushReportWrites(false); + } + } +} + +function queueCurrentReport(requireResponse = false) { + reportDirty = true; + if (writeInFlight) { + return Promise.resolve(); + } + return flushReportWrites(requireResponse); +} + +function onDisconnected() { + rxCharacteristic = undefined; + server = undefined; + encryptedLinkReady = false; + reportDirty = false; + writeInFlight = false; + setConnectedState(false); + setStatus("Disconnected"); +} + +async function connectWithOptions(options) { + try { + if (!navigator.bluetooth) { + setStatus("Web Bluetooth is not available in this browser"); + return; + } + + setStatus("Opening Bluetooth picker..."); + device = await navigator.bluetooth.requestDevice(options); + setStatus(`Selected ${device.name || device.id}; connecting GATT...`); + device.addEventListener("gattserverdisconnected", onDisconnected); + + server = await device.gatt.connect(); + setStatus(`Connected to ${device.name || device.id}; opening NUS service...`); + const service = await server.getPrimaryService(NUS_SERVICE_UUID); + setStatus(`NUS service found on ${device.name || device.id}; opening write characteristic...`); + rxCharacteristic = await service.getCharacteristic(NUS_RX_UUID); + + encryptedLinkReady = false; + setConnectedState(true); + setStatus(`Connected to ${device.name || "HID Remapper"}. Use Pair / Send Neutral to trigger encryption, then reports use fast writes.`); + } catch (error) { + setStatus(error.message || String(error)); + } +} + +connectButton.addEventListener("click", async () => { + await connectWithOptions({ + acceptAllDevices: true, + optionalServices: [NUS_SERVICE_UUID], + }); +}); + +connectFilteredButton.addEventListener("click", async () => { + await connectWithOptions({ + filters: [{ services: [NUS_SERVICE_UUID] }], + optionalServices: [NUS_SERVICE_UUID], + }); +}); + +disconnectButton.addEventListener("click", () => { + if (device?.gatt?.connected) { + device.gatt.disconnect(); + } +}); + +pairButton.addEventListener("click", async () => { + report = neutralReport(); + encryptedLinkReady = false; + await queueCurrentReport(true); +}); + +document.querySelector("#neutral").addEventListener("click", async () => { + report = neutralReport(); + for (const slider of document.querySelectorAll("input[type='range']")) { + slider.value = 128; + } + await queueCurrentReport(); +}); + +for (const button of document.querySelectorAll("[data-button]")) { + const bit = Number(button.dataset.button); + button.addEventListener("click", async () => { + report[0] |= 1 << bit; + await queueCurrentReport(); + report[0] &= ~(1 << bit); + await queueCurrentReport(); + }); +} + +for (const button of document.querySelectorAll("[data-hat]")) { + button.addEventListener("click", async () => { + report[2] = Number(button.dataset.hat); + await queueCurrentReport(); + }); +} + +for (const [selector, index] of [ + ["#axis-x", 3], + ["#axis-y", 4], + ["#axis-z", 5], + ["#axis-rz", 6], +]) { + document.querySelector(selector).addEventListener("input", async (event) => { + report[index] = Number(event.target.value); + await queueCurrentReport(); + }); +} diff --git a/transmitter-ble-web/index.html b/transmitter-ble-web/index.html new file mode 100644 index 00000000..ca0db782 --- /dev/null +++ b/transmitter-ble-web/index.html @@ -0,0 +1,115 @@ + + + + + + HID Remapper BLE NUS Test + + + + +

HID Remapper BLE NUS Test

+ +
+
+ + + + + +
+
Disconnected. Use Connect first; the first write may ask to pair so the BLE link can be encrypted.
+
+ +
+

Buttons

+
+ + + + + + +
+
+ +
+
+ +
+

D-pad

+
+ + + + + +
+
+ +
+

Axes

+
+ + + + +
+
+ + diff --git a/transmitter-ble-web/serve.mjs b/transmitter-ble-web/serve.mjs new file mode 100644 index 00000000..6204b5f5 --- /dev/null +++ b/transmitter-ble-web/serve.mjs @@ -0,0 +1,32 @@ +import { createReadStream, existsSync, statSync } from "node:fs"; +import { createServer } from "node:http"; +import { extname, join, normalize } from "node:path"; + +const root = normalize(join(import.meta.dirname, "..")); +const port = Number(process.env.PORT || 8088); +const host = "127.0.0.1"; + +const contentTypes = new Map([ + [".html", "text/html; charset=utf-8"], + [".js", "text/javascript; charset=utf-8"], + [".css", "text/css; charset=utf-8"], + [".json", "application/json; charset=utf-8"], +]); + +createServer((request, response) => { + const url = new URL(request.url || "/", `http://${host}:${port}`); + const path = normalize(join(root, decodeURIComponent(url.pathname))); + + if (!path.startsWith(root) || !existsSync(path) || !statSync(path).isFile()) { + response.writeHead(404, { "Content-Type": "text/plain; charset=utf-8" }); + response.end("Not found\n"); + return; + } + + response.writeHead(200, { + "Content-Type": contentTypes.get(extname(path)) || "application/octet-stream", + }); + createReadStream(path).pipe(response); +}).listen(port, host, () => { + console.log(`Serving http://${host}:${port}/transmitter-ble-web/index.html`); +});