From f968713be863a7a2f16528c33646149f5389060f Mon Sep 17 00:00:00 2001
From: David Kiliani <mail@davidkiliani.de>
Date: Tue, 1 Jun 2021 11:46:54 +0200
Subject: [PATCH] Add optional lambda to BLESensor for raw data parsing (#1851)

Co-authored-by: Jesse Hills <3060199+jesserockz@users.noreply.github.com>
---
 esphome/components/ble_client/sensor/__init__.py    | 11 +++++++++++
 esphome/components/ble_client/sensor/ble_sensor.cpp | 13 +++++++++++--
 esphome/components/ble_client/sensor/ble_sensor.h   |  5 +++++
 tests/test1.yaml                                    |  3 +++
 4 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/esphome/components/ble_client/sensor/__init__.py b/esphome/components/ble_client/sensor/__init__.py
index a7ab0acd73..a0bc996d87 100644
--- a/esphome/components/ble_client/sensor/__init__.py
+++ b/esphome/components/ble_client/sensor/__init__.py
@@ -4,6 +4,7 @@ from esphome.components import sensor, ble_client, esp32_ble_tracker
 from esphome.const import (
     DEVICE_CLASS_EMPTY,
     CONF_ID,
+    CONF_LAMBDA,
     UNIT_EMPTY,
     ICON_EMPTY,
     CONF_TRIGGER_ID,
@@ -20,6 +21,9 @@ CONF_DESCRIPTOR_UUID = "descriptor_uuid"
 CONF_NOTIFY = "notify"
 CONF_ON_NOTIFY = "on_notify"
 
+adv_data_t = cg.std_vector.template(cg.uint8)
+adv_data_t_const_ref = adv_data_t.operator("ref").operator("const")
+
 BLESensor = ble_client_ns.class_(
     "BLESensor", sensor.Sensor, cg.PollingComponent, ble_client.BLEClientNode
 )
@@ -35,6 +39,7 @@ CONFIG_SCHEMA = cv.All(
             cv.Required(CONF_SERVICE_UUID): esp32_ble_tracker.bt_uuid,
             cv.Required(CONF_CHARACTERISTIC_UUID): esp32_ble_tracker.bt_uuid,
             cv.Optional(CONF_DESCRIPTOR_UUID): esp32_ble_tracker.bt_uuid,
+            cv.Optional(CONF_LAMBDA): cv.returning_lambda,
             cv.Optional(CONF_NOTIFY, default=False): cv.boolean,
             cv.Optional(CONF_ON_NOTIFY): automation.validate_automation(
                 {
@@ -105,6 +110,12 @@ async def to_code(config):
             uuid128 = esp32_ble_tracker.as_hex_array(config[CONF_DESCRIPTOR_UUID])
             cg.add(var.set_descr_uuid128(uuid128))
 
+    if CONF_LAMBDA in config:
+        lambda_ = await cg.process_lambda(
+            config[CONF_LAMBDA], [(adv_data_t_const_ref, "x")], return_type=cg.float_
+        )
+        cg.add(var.set_data_to_value(lambda_))
+
     await cg.register_component(var, config)
     await ble_client.register_ble_node(var, config)
     cg.add(var.set_enable_notify(config[CONF_NOTIFY]))
diff --git a/esphome/components/ble_client/sensor/ble_sensor.cpp b/esphome/components/ble_client/sensor/ble_sensor.cpp
index ef1d6c120f..69a4b23313 100644
--- a/esphome/components/ble_client/sensor/ble_sensor.cpp
+++ b/esphome/components/ble_client/sensor/ble_sensor.cpp
@@ -84,7 +84,7 @@ void BLESensor::gattc_event_handler(esp_gattc_cb_event_t event, esp_gatt_if_t ga
       }
       if (param->read.handle == this->handle) {
         this->status_clear_warning();
-        this->publish_state((float) param->read.value[0]);
+        this->publish_state(this->parse_data(param->read.value, param->read.value_len));
       }
       break;
     }
@@ -93,7 +93,7 @@ void BLESensor::gattc_event_handler(esp_gattc_cb_event_t event, esp_gatt_if_t ga
         break;
       ESP_LOGV(TAG, "[%s] ESP_GATTC_NOTIFY_EVT: handle=0x%x, value=0x%x", this->get_name().c_str(),
                param->notify.handle, param->notify.value[0]);
-      this->publish_state((float) param->notify.value[0]);
+      this->publish_state(this->parse_data(param->notify.value, param->notify.value_len));
       break;
     }
     case ESP_GATTC_REG_FOR_NOTIFY_EVT: {
@@ -105,6 +105,15 @@ void BLESensor::gattc_event_handler(esp_gattc_cb_event_t event, esp_gatt_if_t ga
   }
 }
 
+float BLESensor::parse_data(uint8_t *value, uint16_t value_len) {
+  if (this->data_to_value_func_.has_value()) {
+    std::vector<uint8_t> data(value, value + value_len);
+    return (*this->data_to_value_func_)(data);
+  } else {
+    return value[0];
+  }
+}
+
 void BLESensor::update() {
   if (this->node_state != espbt::ClientState::Established) {
     ESP_LOGW(TAG, "[%s] Cannot poll, not connected", this->get_name().c_str());
diff --git a/esphome/components/ble_client/sensor/ble_sensor.h b/esphome/components/ble_client/sensor/ble_sensor.h
index e3a8a8ea25..25e996b6ee 100644
--- a/esphome/components/ble_client/sensor/ble_sensor.h
+++ b/esphome/components/ble_client/sensor/ble_sensor.h
@@ -13,6 +13,8 @@ namespace ble_client {
 
 namespace espbt = esphome::esp32_ble_tracker;
 
+using data_to_value_t = std::function<float(std::vector<uint8_t>)>;
+
 class BLESensor : public sensor::Sensor, public PollingComponent, public BLEClientNode {
  public:
   void loop() override;
@@ -30,11 +32,14 @@ class BLESensor : public sensor::Sensor, public PollingComponent, public BLEClie
   void set_descr_uuid16(uint16_t uuid) { this->descr_uuid_ = espbt::ESPBTUUID::from_uint16(uuid); }
   void set_descr_uuid32(uint32_t uuid) { this->descr_uuid_ = espbt::ESPBTUUID::from_uint32(uuid); }
   void set_descr_uuid128(uint8_t *uuid) { this->descr_uuid_ = espbt::ESPBTUUID::from_raw(uuid); }
+  void set_data_to_value(data_to_value_t &&lambda_) { this->data_to_value_func_ = lambda_; }
   void set_enable_notify(bool notify) { this->notify_ = notify; }
   uint16_t handle;
 
  protected:
   uint32_t hash_base() override;
+  float parse_data(uint8_t *value, uint16_t value_len);
+  optional<data_to_value_t> data_to_value_func_{};
   bool notify_;
   espbt::ESPBTUUID service_uuid_;
   espbt::ESPBTUUID char_uuid_;
diff --git a/tests/test1.yaml b/tests/test1.yaml
index 98e7292091..e9e89defd3 100644
--- a/tests/test1.yaml
+++ b/tests/test1.yaml
@@ -271,6 +271,9 @@ sensor:
     descriptor_uuid: 'ffe2'
     notify: true
     update_interval: never
+    lambda: |-
+      ESP_LOGD("main", "Length of data is %i", x.size());
+      return x[0];
     on_notify:
       then:
         - lambda: |-