From 0d4fd1965c20d6b694935d183ada3cf86a7ddebb Mon Sep 17 00:00:00 2001
From: Chatblanc77 <107761260+Chatblanc77@users.noreply.github.com>
Date: Fri, 9 Sep 2022 13:26:39 +0700
Subject: [PATCH] Update central.c

---
 app/src/split/bluetooth/central.c | 58 +++++++++++++++++++++++++++++++
 1 file changed, 58 insertions(+)

diff --git a/app/src/split/bluetooth/central.c b/app/src/split/bluetooth/central.c
index e94a59ae..2ba07d3a 100644
--- a/app/src/split/bluetooth/central.c
+++ b/app/src/split/bluetooth/central.c
@@ -19,11 +19,13 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
 
 #include <zmk/stdlib.h>
 #include <zmk/ble.h>
+#include <zmk/sensors.h>
 #include <zmk/behavior.h>
 #include <zmk/split/bluetooth/uuid.h>
 #include <zmk/split/bluetooth/service.h>
 #include <zmk/event_manager.h>
 #include <zmk/events/position_state_changed.h>
+#include <zmk/events/sensor_event.h>
 #include <init.h>
 
 static int start_scan(void);
@@ -63,7 +65,49 @@ void peripheral_event_work_callback(struct k_work *work) {
 }
 
 K_WORK_DEFINE(peripheral_event_work, peripheral_event_work_callback);
+#if ZMK_KEYMAP_HAS_SENSORS
+K_MSGQ_DEFINE(peripheral_sensor_event_msgq, sizeof(struct zmk_sensor_event),
+              CONFIG_ZMK_SPLIT_BLE_CENTRAL_POSITION_QUEUE_SIZE, 4);
 
+void peripheral_sensor_event_work_callback(struct k_work *work) {
+    struct zmk_sensor_event ev;
+    while (k_msgq_get(&peripheral_sensor_event_msgq, &ev, K_NO_WAIT) == 0) {
+        LOG_DBG("Trigger sensor change for %d", ev.sensor_number);
+        ZMK_EVENT_RAISE(new_zmk_sensor_event(ev));
+    }
+}
+
+K_WORK_DEFINE(peripheral_sensor_event_work, peripheral_sensor_event_work_callback);
+
+struct sensor_event {
+    uint8_t sensor_number;
+    struct sensor_value value;
+};
+
+static uint8_t split_central_sensor_notify_func(struct bt_conn *conn,
+                                                struct bt_gatt_subscribe_params *params,
+                                                const void *data, uint16_t length) {
+
+    const struct sensor_event *sensor_event = data;
+
+    if (!data) {
+        LOG_DBG("[UNSUBSCRIBED]");
+        params->value_handle = 0U;
+        return BT_GATT_ITER_STOP;
+    }
+    LOG_DBG("[SENSOR NOTIFICATION] data %p length %u", data, length);
+
+    struct zmk_sensor_event ev = {
+        .sensor_number = sensor_event->sensor_number,
+        .value = {.val1 = (sensor_event->value).val1, .val2 = (sensor_event->value).val2},
+        .timestamp = k_uptime_get()};
+
+    k_msgq_put(&peripheral_sensor_event_msgq, &ev, K_NO_WAIT);
+    k_work_submit(&peripheral_sensor_event_work);
+
+    return BT_GATT_ITER_CONTINUE;
+}
+#endif /* ZMK_KEYMAP_HAS_SENSORS */
 int peripheral_slot_index_for_conn(struct bt_conn *conn) {
     for (int i = 0; i < ZMK_BLE_SPLIT_PERIPHERAL_COUNT; i++) {
         if (peripherals[i].conn == conn) {
@@ -226,6 +270,20 @@ static void split_central_subscribe(struct bt_conn *conn) {
         break;
     }
 }
+#if ZMK_KEYMAP_HAS_SENSORS
+        memcpy(&sensor_uuid, BT_UUID_DECLARE_128(ZMK_SPLIT_BT_CHAR_SENSOR_STATE_UUID),
+               sizeof(sensor_uuid));
+        sensor_discover_params.uuid = &sensor_uuid.uuid;
+        sensor_discover_params.start_handle = attr->handle;
+        sensor_discover_params.end_handle = 0xffff;
+        sensor_discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
+        sensor_discover_params.func = split_central_sensor_desc_discovery_func;
+
+        err = bt_gatt_discover(conn, &sensor_discover_params);
+        if (err) {
+            LOG_ERR("Discover failed (err %d)", err);
+        }
+#endif /* ZMK_KEYMAP_HAS_SENSORS */
 
 static uint8_t split_central_chrc_discovery_func(struct bt_conn *conn,
                                                  const struct bt_gatt_attr *attr,