Added direction sensor data parser

datacompboy · Feb 22, 2024 · ef2f73e · ef2f73e
1 parent 58bf6a3
commit ef2f73e
Show file tree

Hide file tree

Showing 8 changed files with 208 additions and 172 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -1,41 +1,46 @@
 #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;
 } katConnectionInfo;
 
 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__
diff --git a/src/kat_bt_pack.c 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 <zephyr/bluetooth/buf.h>
+
+#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;
+    }
+}