Initial commit

src/meter_driver.cpp

@@ -0,0 +1,166 @@
+#include "meter_driver.h"
+#include "config.h"
+#include <math.h>
+#include <string.h>
+
+static constexpr uint32_t METER_READ_TIMEOUT_MS = 2000;
+static constexpr size_t SML_BUFFER_SIZE = 2048;
+
+static const uint8_t OBIS_ENERGY_TOTAL[6] = {0x01, 0x00, 0x01, 0x08, 0x00, 0xFF};
+static const uint8_t OBIS_TOTAL_POWER[6] = {0x01, 0x00, 0x10, 0x07, 0x00, 0xFF};
+static const uint8_t OBIS_P1[6] = {0x01, 0x00, 0x24, 0x07, 0x00, 0xFF};
+static const uint8_t OBIS_P2[6] = {0x01, 0x00, 0x38, 0x07, 0x00, 0xFF};
+static const uint8_t OBIS_P3[6] = {0x01, 0x00, 0x4C, 0x07, 0x00, 0xFF};
+static const uint8_t OBIS_V1[6] = {0x01, 0x00, 0x20, 0x07, 0x00, 0xFF};
+static const uint8_t OBIS_V2[6] = {0x01, 0x00, 0x34, 0x07, 0x00, 0xFF};
+static const uint8_t OBIS_V3[6] = {0x01, 0x00, 0x48, 0x07, 0x00, 0xFF};
+
+static bool find_obis_value(const uint8_t *buf, size_t len, const uint8_t *obis, float &out_value) {
+  for (size_t i = 0; i + 6 < len; ++i) {
+    if (memcmp(&buf[i], obis, 6) == 0) {
+      int8_t scaler = 0;
+      bool scaler_found = false;
+      bool value_found = false;
+      int64_t value = 0;
+      size_t cursor = i + 6;
+      size_t limit = (i + 6 + 120 < len) ? i + 6 + 120 : len;
+
+      while (cursor < limit) {
+        uint8_t tl = buf[cursor++];
+        if (tl == 0x00) {
+          continue;
+        }
+        uint8_t type = (tl >> 4) & 0x0F;
+        uint8_t tlen = tl & 0x0F;
+        if (tlen == 0 || cursor + tlen > len) {
+          continue;
+        }
+
+        if (type == 0x05 || type == 0x06) {
+          int64_t val = 0;
+          for (uint8_t b = 0; b < tlen; ++b) {
+            val = (val << 8) | buf[cursor + b];
+          }
+          if (type == 0x05) {
+            int64_t sign_bit = 1LL << ((tlen * 8) - 1);
+            if (val & sign_bit) {
+              int64_t mask = (1LL << (tlen * 8)) - 1;
+              val = -((~val + 1) & mask);
+            }
+          }
+
+          if (!scaler_found && tlen <= 2 && val >= -6 && val <= 6) {
+            scaler = static_cast<int8_t>(val);
+            scaler_found = true;
+          } else if (!value_found) {
+            value = val;
+            value_found = true;
+          }
+        }
+        cursor += tlen;
+        if (value_found && scaler_found) {
+          break;
+        }
+      }
+
+      if (value_found) {
+        out_value = static_cast<float>(value) * powf(10.0f, scaler);
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+void meter_init() {
+  Serial2.begin(9600, SERIAL_7E1, PIN_METER_RX, -1);
+}
+
+bool meter_read(MeterData &data) {
+  uint8_t buffer[SML_BUFFER_SIZE];
+  size_t len = 0;
+  bool started = false;
+  uint32_t start = millis();
+  const uint8_t start_seq[] = {0x1B, 0x1B, 0x1B, 0x1B, 0x01, 0x01, 0x01, 0x01};
+  const uint8_t end_seq[] = {0x1B, 0x1B, 0x1B, 0x1B, 0x1A};
+
+  while (millis() - start < METER_READ_TIMEOUT_MS) {
+    while (Serial2.available()) {
+      uint8_t b = Serial2.read();
+      if (!started) {
+        buffer[len++] = b;
+        if (len >= sizeof(start_seq)) {
+          if (memcmp(&buffer[len - sizeof(start_seq)], start_seq, sizeof(start_seq)) == 0) {
+            started = true;
+            len = 0;
+          }
+        }
+        if (len >= sizeof(buffer)) {
+          len = 0;
+        }
+      } else {
+        if (len < sizeof(buffer)) {
+          buffer[len++] = b;
+          if (len >= sizeof(end_seq)) {
+            if (memcmp(&buffer[len - sizeof(end_seq)], end_seq, sizeof(end_seq)) == 0) {
+              start = millis();
+              goto parse_frame;
+            }
+          }
+        }
+      }
+    }
+    delay(5);
+  }
+
+parse_frame:
+  if (!started || len == 0) {
+    return false;
+  }
+
+  data.energy_total_kwh = NAN;
+  data.total_power_w = NAN;
+  data.phase_power_w[0] = NAN;
+  data.phase_power_w[1] = NAN;
+  data.phase_power_w[2] = NAN;
+  data.phase_voltage_v[0] = NAN;
+  data.phase_voltage_v[1] = NAN;
+  data.phase_voltage_v[2] = NAN;
+
+  bool ok = true;
+  float value = 0.0f;
+
+  if (find_obis_value(buffer, len, OBIS_ENERGY_TOTAL, value)) {
+    data.energy_total_kwh = value;
+  } else {
+    ok = false;
+  }
+
+  if (find_obis_value(buffer, len, OBIS_TOTAL_POWER, value)) {
+    data.total_power_w = value;
+  } else {
+    ok = false;
+  }
+
+  if (find_obis_value(buffer, len, OBIS_P1, value)) {
+    data.phase_power_w[0] = value;
+  }
+  if (find_obis_value(buffer, len, OBIS_P2, value)) {
+    data.phase_power_w[1] = value;
+  }
+  if (find_obis_value(buffer, len, OBIS_P3, value)) {
+    data.phase_power_w[2] = value;
+  }
+  if (find_obis_value(buffer, len, OBIS_V1, value)) {
+    data.phase_voltage_v[0] = value;
+  }
+  if (find_obis_value(buffer, len, OBIS_V2, value)) {
+    data.phase_voltage_v[1] = value;
+  }
+  if (find_obis_value(buffer, len, OBIS_V3, value)) {
+    data.phase_voltage_v[2] = value;
+  }
+
+  data.valid = ok;
+  return ok;
+}