From ef2f73e65541e53cbb4aacd5111ede3240a77023 Mon Sep 17 00:00:00 2001 From: Anton Fedorov Date: Thu, 22 Feb 2024 17:24:50 +0100 Subject: [PATCH] Added direction sensor data parser --- CMakeLists.txt | 2 +- Kconfig | 8 +++ README.md | 2 +- prj.conf | 4 +- src/kat.h | 29 +++++---- src/kat_bt_pack.c | 162 ++++++++++++++++++++++++++++++++++++++++++++++ src/kat_feet.c | 132 ------------------------------------- src/main.c | 41 +++++------- 8 files changed, 208 insertions(+), 172 deletions(-) create mode 100644 src/kat_bt_pack.c delete mode 100644 src/kat_feet.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 5102c72..405dc2c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,4 +4,4 @@ find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) project(nrf-katvr-receiver) target_sources(app PRIVATE src/main.c) -target_sources(app PRIVATE src/kat_feet.c) +target_sources(app PRIVATE src/kat_bt_pack.c) diff --git a/Kconfig b/Kconfig index bd1903f..f596d9d 100644 --- a/Kconfig +++ b/Kconfig @@ -5,3 +5,11 @@ source "Kconfig.zephyr" config APP_WAIT_FOR_OBSERVER bool "If yes, application will wait for terminal connection to console before start" default y + +config APP_PACKET_LOG + bool "Should we spam all packets captured to the log or not" + default n + +config APP_ACCEPT_UNPAIRED_SENSORS + bool "Should we keep reading sensors that wasn't 'paired' by gateway or not" + default n diff --git a/README.md b/README.md index 3a56b5f..eae0b50 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,7 @@ This app aims to implement bluetooth receiver compatible with KatVR's C2/C2+ tre - Connect to multiple devices (static list: Direction, Left Feet, Right Feet) - Capture notifications sent by devices and know who sent them. - Parse BT notification from feet sensors + - Parse BT notification from direction sensor ## Tested on @@ -58,7 +59,6 @@ This app aims to implement bluetooth receiver compatible with KatVR's C2/C2+ tre ## TODO - KatVR Sensors BT protocol - - Parse BT notification from direction sensor - Connect & receive events from seat - KatVR Gateway USB protocol - Devices diff --git a/prj.conf b/prj.conf index a1cb8a1..0510c83 100644 --- a/prj.conf +++ b/prj.conf @@ -34,8 +34,10 @@ CONFIG_CONSOLE=y CONFIG_UART_CONSOLE=y CONFIG_UART_LINE_CTRL=y # +# Application settings CONFIG_APP_WAIT_FOR_OBSERVER=y - +CONFIG_APP_PACKET_LOG=n +CONFIG_APP_ACCEPT_UNPAIRED_SENSORS=y # Debugging options CONFIG_LOG=y diff --git a/src/kat.h b/src/kat.h index 5e1c7ca..1df8c78 100644 --- a/src/kat.h +++ b/src/kat.h @@ -1,7 +1,7 @@ #ifndef __KAT_H__ #define __KAT_H__ -typedef enum { KAT_NONE=0, KAT_DIR, KAT_LEFT, KAT_RIGHT, _KAT_MAX_DEVICE } tKatDevice; +typedef enum __packed { KAT_NONE=0, KAT_DIR, KAT_LEFT, KAT_RIGHT, _KAT_MAX_DEVICE } tKatDevice; typedef struct { tKatDevice deviceType; @@ -9,33 +9,38 @@ typedef struct { typedef struct { // Status - tKatDevice deviceType; char firmwareVersion; char chargeStatus; short chargeLevel; bool freshStatus; + // TODO: add timestamp + bool freshData; +} tKatDeviceStatus; + +typedef struct { // Latest data char status1; char status2; short speed_x; short speed_y; char color; - // TODO: add timestamp - bool fresh; } tKatFootDevice; typedef struct { + // Latest data + short axis[7]; + char button; +} tKatDirDevice; + +typedef struct { + tKatDevice deviceType; + tKatDeviceStatus deviceStatus; union { - tKatDevice deviceType; - tKatFootDevice foot; - //tKatDirDevice direction; + tKatFootDevice footData; + tKatDirDevice dirData; }; } tKatDeviceInfo; -BUILD_ASSERT(offsetof(tKatDeviceInfo, foot.deviceType) == offsetof(tKatDeviceInfo, deviceType), "All tKatDeviceInfo memvers should have the same deviceType"); - - -void parseFeet(struct net_buf *req_buf, tKatFootDevice* footDevice); - +void parseKatBtPacket(struct net_buf *req_buf, tKatDeviceInfo* katDevice); #endif // __KAT_H__ \ No newline at end of file diff --git a/src/kat_bt_pack.c b/src/kat_bt_pack.c new file mode 100644 index 0000000..974011e --- /dev/null +++ b/src/kat_bt_pack.c @@ -0,0 +1,162 @@ +/* + * KAT sensors BT packet handler. + * + * Sensors send updates as GATT NOTIFICATION (0x1B) for handle 0x002E of 20 bytes length. + * + * Sensors sends following packets: + * Bytes 0-1: 0000 => status (sent every 500 packets) + * Status data (6 bytes of data, rest is garbage): + * Byte 2 => sensor type (aka 3==right leg, 2==left leg, 1==direction, 0==unpaired) + * Byte 3 => charge status, two bits (bit 0 == charging, bit 1 == fully charged) + * Byte 4+5 => Short, charge level (in 1/256th, so 25600 == 100%) + * Byte 6 => (Current foot sensor firmware version, 5 for feet, 4 for direction) + * non-zero => data (499 other packets). + * Foot sensor data (byte 0 equals 1, byte 1 equals 0): + * Byte 2 & 3: two statuses (height from the ground & something else) + * Byte (1-5) may be (1 0 1 0 1) [when something happens] + * Byte 14-15: Speed X + * Byte 16-17: Speed Y + * Byte 18: always 0 + * Byte 19: brightness under the sensor (?) + * Direction sensor data: + * Bytes 0..13: 7 shorts, accelerometer data + * First 4: direction, last 3 -- rotational angles, currently always zero + * Byte 18: 0x80 if button press, 0 otherwise + * + * Example packets, status: + * KAT_LEFT [23] 1b 2e 00 00 00 00 02 00 5f 05 00 29 00 00 00 10 00 00 00 00 00 00 1c -- unpaired + * KAT_LEFT [23] 1b 2e 00 00 00 02 01 00 64 05 00 29 00 00 00 b0 3d 00 00 00 00 00 77 -- paired + * KAT_RIGHT [23] 1b 2e 00 00 00 03 02 00 64 05 00 29 00 00 00 10 00 00 00 00 00 00 82 -- paired + * KAT_DIR [23] 1b 2e 00 00 00 01 00 56 5e 04 3e 00 00 00 00 00 00 00 00 00 00 00 00 + * ^^^^^^^^ GATT prefix, notification for 0x002E + * ^^ Type = status + * ^^ always zero + * ^^ Sensor type (2 left, 3 right, 1 dir, 0 unpaired) + * ^^ charge status bits + * ^^^^^ charge level + * ^^ firmware version + * + * KAT_LEFT [23] 1b 2e 00 01 00 00 8d 18 00 00 00 29 00 00 00 b0 3d 00 00 00 00 00 78 -- ground + * KAT_LEFT [23] 1b 2e 00 01 08 4e 20 38 00 00 00 00 00 00 00 d0 00 00 00 00 00 00 1a -- air + * KAT_RIGHT [23] 1b 2e 00 01 00 00 92 18 00 00 00 29 00 00 00 10 00 00 00 00 00 00 83 -- ground + * ^^^^^^^^ GATT prefix, notification for 0x002E + * ^^ Packet type (1 == data update) + * ^^^^^ some status bytes + * KAT_LEFT [23] 1b 2e 00 01 00 00 73 9b 4f 05 00 84 01 00 00 00 00 51 fe 1f ff 00 7b + * ^^^^^ speed x + * ^^^^^ speed y + * ^^ color under + * + * KAT_DIR [23] 1b 2e 00 7d f3 9f 00 8f 05 84 3e 00 00 00 00 00 00 00 00 00 00 00 00 + * KAT_DIR [23] 1b 2e 00 1f e4 59 00 e9 0a 91 38 00 00 00 00 00 00 00 00 00 00 80 00 + * ^^^^^^^^ GATT prefix, notification for 0x002E ^^ is button pressed + * ^^^^^ Short 1 ^^^^^ short 4 ^^^^^ short 7 + * ^^^^^ Short 2 ^^^^^ short 5 + * ^^^^^ Short 3 ^^^^^ short 6 +*/ + +#include + +#include "kat.h" + +#define GATT_NOTIFICATION 0x1B +#define KAT_CHAR_HANDLE 0x2E +#define KAT_BT_STATUS 0 + +typedef __PACKED_STRUCT { + __PACKED_UNION { + short packetType; // 0 == status, non-zero == data + __PACKED_STRUCT { + short __zero; + tKatDevice deviceType; + char status; + short chargeLevel; // network order (high, low) + char firmwareVersion; + } status; + __PACKED_STRUCT { + short __undef0; + char status1; + char status2; + char __undef1[10]; + short speed_x; // network order (high, low) + short speed_y; // network order (high, low) + char __zero; + char color; + } footData; + __PACKED_STRUCT { + short axis[7]; + char __undef0[4]; + char button; + char __undef1; + } dirData; + }; +} tKatBtData; + +BUILD_ASSERT(sizeof(tKatBtData) == 20, "tKatBtData definition doesn't match expectation"); +BUILD_ASSERT(offsetof(tKatBtData, footData.color)==19, "footData structure doesn't match expectation"); +BUILD_ASSERT(offsetof(tKatBtData, dirData.button)==18, "dirData structure doesn't match expectation"); + +typedef __PACKED_STRUCT { + char gatt_comand; + char char_id; + char __zero; + tKatBtData data; +} tGattKatPacket; + +BUILD_ASSERT(sizeof(tGattKatPacket) == 23, "tGattKatPacket definition doesn't match expectation"); + + +void parseKatBtPacket(struct net_buf *req_buf, tKatDeviceInfo* katDevice) +{ + // Ignore not initialized sensors + if (katDevice->deviceType == KAT_NONE) + return; + // Process only valid packets + if (req_buf->len != sizeof(tGattKatPacket)) + return; + tGattKatPacket * pack = (tGattKatPacket *) req_buf->data; + if (pack->gatt_comand != GATT_NOTIFICATION) + return; + if (pack->char_id != KAT_CHAR_HANDLE || pack->__zero != 0) + return; + + const tKatBtData * data = &pack->data; + if (data->packetType == KAT_BT_STATUS) + { + if (!IS_ENABLED(CONFIG_APP_ACCEPT_UNPAIRED_SENSORS) && katDevice->deviceType != data->status.deviceType) { + /// Sensor not paired, mark it broken + printk("Sensor is not paired! Ignoring it"); + katDevice->deviceType = KAT_NONE; + return; + } + katDevice->deviceStatus.chargeLevel = data->status.chargeLevel; + katDevice->deviceStatus.chargeStatus = data->status.status; + katDevice->deviceStatus.firmwareVersion = data->status.firmwareVersion; + katDevice->deviceStatus.freshStatus = true; + } + else + { + switch(katDevice->deviceType) { + case KAT_LEFT: [[fallthrough]]; + case KAT_RIGHT: + /// refresh data + katDevice->footData.color = data->footData.color; + katDevice->footData.speed_x = data->footData.speed_x; + katDevice->footData.speed_y = data->footData.speed_y; + katDevice->footData.status1 = data->footData.status1; + katDevice->footData.status2 = data->footData.status2; + break; + case KAT_DIR: + BUILD_ASSERT(sizeof(katDevice->dirData.axis) == 14, "We copy whole sensor data batch (7 shorts)"); + BUILD_ASSERT(sizeof(katDevice->dirData.axis) == sizeof(data->dirData.axis), "We should copy matching arrays"); + memcpy(&katDevice->dirData.axis, &data->dirData.axis, sizeof(data->dirData.axis)); + katDevice->dirData.button = data->dirData.button; + break; + case _KAT_MAX_DEVICE: [[fallthrough]]; // Should not happen + case KAT_NONE: // Should not happen + return; + } + // TODO: add timestamp + katDevice->deviceStatus.freshData = true; + } +} diff --git a/src/kat_feet.c b/src/kat_feet.c deleted file mode 100644 index 06065c1..0000000 --- a/src/kat_feet.c +++ /dev/null @@ -1,132 +0,0 @@ -/* - * KAT Feet sensors handler. - * - * Sensor sends updates as GATT NOTIFICATION (0x1B) for handle 0x002E of 20 bytes length. - * - * Sensor sends following packets: - * Byte 0: 00 => status (sent every 500 packets) - * Status data (6 bytes of data, rest is garbage): - * Byte 1 => 00 always - * Byte 2 => sensor type (aka 2==left leg, 3==right leg, 1==direction, 0==unpaired) - * Byte 3 => some status, join of two flags: - * updateBuf[3] = (DAT_400e00f6 == 0) | (DAT_400e00fa == 0) << 1; - * Byte 4+5 => (Hi)(Lo), 2-byte integer DAT_20001fec or DAT_20001148 - * Charge level? scaled to 20..100 - * Byte 6 => 5 (Current foot sensor firmware version) - * 01 => data (499 other packets) - * Packet data: - * Byte 2 & 3: two statuses (height from the ground & something else) - * Byte (1-5) may be (1 0 1 0 1) [when something happens] - * Byte 14-15: Speed X - * Byte 16-17: Speed Y - * Byte 18: always 0 - * Byte 19: color under the sensor (?) - * - * Example packets, status: - * KAT_LEFT [23] 1b 2e 00 00 00 00 02 00 5f 05 00 29 00 00 00 10 00 00 00 00 00 00 1c -- unpaired - * KAT_LEFT [23] 1b 2e 00 00 00 02 01 00 64 05 00 29 00 00 00 b0 3d 00 00 00 00 00 77 -- paired - * KAT_RIGHT [23] 1b 2e 00 00 00 03 02 00 64 05 00 29 00 00 00 10 00 00 00 00 00 00 82 -- paired - * ^^^^^^^^ GATT prefix, notification for 0x002E - * ^^ Type = status - * ^^ always zero - * ^^ Sensor type (2 left, 3 right) - * ^^ two status bits - * ^^^^^ charge level - * ^^ firmware version - * - * KAT_LEFT [23] 1b 2e 00 01 00 00 8d 18 00 00 00 29 00 00 00 b0 3d 00 00 00 00 00 78 -- ground - * KAT_LEFT [23] 1b 2e 00 01 08 4e 20 38 00 00 00 00 00 00 00 d0 00 00 00 00 00 00 1a -- air - * KAT_RIGHT [23] 1b 2e 00 01 00 00 92 18 00 00 00 29 00 00 00 10 00 00 00 00 00 00 83 -- ground - * ^^^^^^^^ GATT prefix, notification for 0x002E - * ^^ Packet type (1 == data update) - * ^^^^^ some status bytes - * ^^^^^ speed x - * ^^^^^ speed y - * ^^ color under -*/ - -#include - -#include "kat.h" - -typedef enum { KAT_BT_STATUS=0, KAT_BT_DATA=1 } tKatFootStatus; -BUILD_ASSERT(sizeof(tKatFootStatus)==1, "tKatFootStatus should be 1 byte"); - -typedef __PACKED_STRUCT { - tKatFootStatus packetType; - __PACKED_UNION { - __PACKED_STRUCT { - char __zero; - tKatDevice deviceType; - char status; - short chargeLevel; // network order (high, low) - char firmwareVersion; - } status; - __PACKED_STRUCT { - char __unk; - char status1; - char status2; - char __unk2[10]; - short speed_x; // network order (high, low) - short speed_y; // network order (high, low) - char __zero; - char color; - } data; - }; -} tKatFootBtData; - -BUILD_ASSERT(sizeof(tKatFootBtData) == 20, "tKatFootBtData definition doesn't match expectation"); - -typedef __PACKED_STRUCT { - char gatt_comand; - char char_id; - char __zero; - tKatFootBtData foot; -} tGattKatPacket; - -BUILD_ASSERT(sizeof(tGattKatPacket) == 23, "tGattKatPacket definition doesn't match expectation"); - - -#define GATT_NOTIFICATION 0x1B -#define KAT_CHAR_HANDLE 0x2E -void parseFeet(struct net_buf *req_buf, tKatFootDevice* footDevice) -{ - // Ignore not initialized sensors - if (footDevice->deviceType == KAT_NONE) - return; - // Process only valid packets - if (req_buf->len != sizeof(tGattKatPacket)) - return; - tGattKatPacket * pack = (tGattKatPacket *) req_buf->data; - if (pack->gatt_comand != GATT_NOTIFICATION) - return; - if (pack->char_id != KAT_CHAR_HANDLE || pack->__zero != 0) - return; - const tKatFootBtData * foot = &pack->foot; - - if (foot->packetType == KAT_BT_STATUS) - { - /// copy - if (footDevice->deviceType != foot->status.deviceType) { - /// Sensor not paired, mark it broken - printk("Sensor is not paired! Ignoring it"); - footDevice->deviceType = KAT_NONE; - return; - } - footDevice->chargeLevel = foot->status.chargeLevel; - footDevice->chargeStatus = foot->status.status; - footDevice->firmwareVersion = foot->status.firmwareVersion; - footDevice->freshStatus = true; - } - else if (foot->packetType == KAT_BT_DATA) - { - /// refresh data - footDevice->color = foot->data.color; - footDevice->speed_x = foot->data.speed_x; - footDevice->speed_y = foot->data.speed_y; - footDevice->status1 = foot->data.status1; - footDevice->status2 = foot->data.status2; - // TODO: add timestamp - footDevice->fresh = true; - } -} diff --git a/src/main.c b/src/main.c index 182b038..535e0e3 100644 --- a/src/main.c +++ b/src/main.c @@ -17,14 +17,14 @@ const char * const stKatDevice[] = { "KAT_NONE", "KAT_DIR", "KAT_LEFT", "KAT_RIG BUILD_ASSERT(ARRAY_SIZE(stKatDevice)==_KAT_MAX_DEVICE+1, "Don't miss a thing!"); static katConnectionInfo connections[CONFIG_BT_MAX_CONN] = {KAT_NONE}; -static tKatDeviceInfo devices[_KAT_MAX_DEVICE] = { {{KAT_NONE}} }; +static tKatDeviceInfo devices[_KAT_MAX_DEVICE] = { {KAT_NONE} }; const bt_addr_le_t katDevices[] = { // KAT_DIR {.type=BT_ADDR_LE_PUBLIC, .a={.val={0x01, 0x74, 0xEB, 0x16, 0x4D, 0xAC}}}, // AC:4D:16:EB:74:01 - direction // KAT_LEFT - //{.type=BT_ADDR_LE_PUBLIC, .a={.val={0x54, 0x46, 0xF2, 0x66, 0x98, 0x60}}}, // 60:98:66:F2:46:54 - unpaired - {.type=BT_ADDR_LE_PUBLIC, .a={.val={0x0B, 0x9D, 0xF3, 0x66, 0x98, 0x60}}}, // 60:98:66:F3:9D:0B - paired left + {.type=BT_ADDR_LE_PUBLIC, .a={.val={0x54, 0x46, 0xF2, 0x66, 0x98, 0x60}}}, // 60:98:66:F2:46:54 - unpaired + // {.type=BT_ADDR_LE_PUBLIC, .a={.val={0x0B, 0x9D, 0xF3, 0x66, 0x98, 0x60}}}, // 60:98:66:F3:9D:0B - paired left // KAT_RIGHT {.type=BT_ADDR_LE_PUBLIC, .a={.val={0x6A, 0x1C, 0xF2, 0x66, 0x98, 0x60}}}, // 60:98:66:F2:1C:6A - paired right }; @@ -122,33 +122,24 @@ BT_CONN_CB_DEFINE(conn_callbacks) = { static int my_att_recv(struct bt_l2cap_chan *chan, struct net_buf *req_buf) { int id = bt_conn_index(chan->conn); - #if false - printk("Received packet for %p %s [%d]", chan->conn, stKatDevice[connections[id].deviceType], req_buf->len); + const tKatDevice dev = connections[id].deviceType; + + #if CONFIG_APP_PACKET_LOG + printk("Received packet for %p %8s [%d]", chan->conn, stKatDevice[dev], req_buf->len); for (int i = 0; ilen; ++i) { printk(" %02x", req_buf->data[i]); } printk("\n"); #endif - const tKatDevice dev = connections[id].deviceType; - switch(dev) { - case KAT_LEFT: [[fallthrough]]; - case KAT_RIGHT: - parseFeet(req_buf, &devices[dev].foot); - if (devices[dev].foot.freshStatus) { - devices[dev].foot.freshStatus = false; - printk("%s: charge_level=%d, charge_status=%d, firmware=%d\n", - stKatDevice[connections[id].deviceType], - devices[dev].foot.chargeLevel, - devices[dev].foot.chargeStatus, - devices[dev].foot.firmwareVersion); - } - break; - case KAT_DIR: //FIXME - // parseFeet(req_buf, &devices[dev].directionDevice); - break; - case _KAT_MAX_DEVICE: [[fallthrough]]; // should not happen, though - case KAT_NONE: - break; + + parseKatBtPacket(req_buf, &devices[dev]); + if (devices[dev].deviceStatus.freshStatus) { + devices[dev].deviceStatus.freshStatus = false; + printk("%s: charge_level=%d, charge_status=%d, firmware=%d\n", + stKatDevice[connections[id].deviceType], + devices[dev].deviceStatus.chargeLevel, + devices[dev].deviceStatus.chargeStatus, + devices[dev].deviceStatus.firmwareVersion); } return 0; }