Use compact binary payload for LoRa batches

src/main.cpp

@@ -2,6 +2,7 @@
 #include "config.h"
 #include "data_model.h"
 #include "json_codec.h"
+#include "payload_codec.h"
 #include "compressor.h"
 #include "lora_transport.h"
 #include "meter_driver.h"
@@ -12,8 +13,8 @@
 #include "web_server.h"
 #include "display_ui.h"
 #include "test_mode.h"
-#include <ArduinoJson.h>
 #include <stdarg.h>
+#include <math.h>
 #ifdef ARDUINO_ARCH_ESP32
 #include <esp_task_wdt.h>
 #include <esp_system.h>
@@ -51,7 +52,6 @@ static bool g_receiver_discovery_sent = false;
 static constexpr size_t BATCH_HEADER_SIZE = 6;
 static constexpr size_t BATCH_CHUNK_PAYLOAD = LORA_MAX_PAYLOAD - BATCH_HEADER_SIZE;
 static constexpr size_t BATCH_MAX_COMPRESSED = 4096;
-static constexpr size_t BATCH_MAX_DECOMPRESSED = 8192;
 static constexpr uint32_t BATCH_RX_MARGIN_MS = 800;
 
 struct BatchBuffer {
@@ -102,25 +102,6 @@ static void serial_debug_printf(const char *fmt, ...) {
   Serial.println(buf);
 }
 
-static void serial_debug_print_json(const String &json) {
-  if (!SERIAL_DEBUG_MODE || !SERIAL_DEBUG_DUMP_JSON) {
-    return;
-  }
-  const char *data = json.c_str();
-  size_t len = json.length();
-  const size_t chunk = 128;
-  for (size_t i = 0; i < len; i += chunk) {
-    size_t n = len - i;
-    if (n > chunk) {
-      n = chunk;
-    }
-    Serial.write(reinterpret_cast<const uint8_t *>(data + i), n);
-    watchdog_kick();
-    delay(0);
-  }
-  Serial.write('\n');
-}
-
 static uint16_t g_last_batch_id_rx[NUM_SENDERS] = {};
 
 struct BatchRxState {
@@ -283,6 +264,63 @@ static uint16_t read_u16_le(const uint8_t *src) {
   return static_cast<uint16_t>(src[0]) | (static_cast<uint16_t>(src[1]) << 8);
 }
 
+static uint16_t sender_id_from_short_id(uint16_t short_id) {
+  for (uint8_t i = 0; i < NUM_SENDERS; ++i) {
+    if (EXPECTED_SENDER_IDS[i] == short_id) {
+      return static_cast<uint16_t>(i + 1);
+    }
+  }
+  return 0;
+}
+
+static uint16_t short_id_from_sender_id(uint16_t sender_id) {
+  if (sender_id == 0 || sender_id > NUM_SENDERS) {
+    return 0;
+  }
+  return EXPECTED_SENDER_IDS[sender_id - 1];
+}
+
+static uint32_t kwh_to_wh_from_float(float value) {
+  if (isnan(value)) {
+    return 0;
+  }
+  double wh = static_cast<double>(value) * 1000.0;
+  if (wh < 0.0) {
+    wh = 0.0;
+  }
+  if (wh > static_cast<double>(UINT32_MAX)) {
+    wh = static_cast<double>(UINT32_MAX);
+  }
+  return static_cast<uint32_t>(llround(wh));
+}
+
+static bool float_to_i16_w(float value, int16_t &out) {
+  if (isnan(value)) {
+    out = 0;
+    return true;
+  }
+  long rounded = lroundf(value);
+  if (rounded < INT16_MIN || rounded > INT16_MAX) {
+    return false;
+  }
+  out = static_cast<int16_t>(rounded);
+  return true;
+}
+
+static uint16_t battery_mv_from_voltage(float value) {
+  if (isnan(value) || value <= 0.0f) {
+    return 0;
+  }
+  long mv = lroundf(value * 1000.0f);
+  if (mv < 0) {
+    mv = 0;
+  }
+  if (mv > UINT16_MAX) {
+    mv = UINT16_MAX;
+  }
+  return static_cast<uint16_t>(mv);
+}
+
 static uint32_t compute_batch_rx_timeout_ms(uint16_t total_len, uint8_t chunk_count) {
   if (total_len == 0 || chunk_count == 0) {
     return 10000;
@@ -306,22 +344,6 @@ static uint32_t compute_batch_ack_timeout_ms(size_t payload_len) {
   return timeout_ms < 10000 ? 10000 : timeout_ms;
 }
 
-static bool inject_batch_meta(String &json, int16_t rssi_dbm, float snr_db, uint32_t rx_ts_utc) {
-  DynamicJsonDocument doc(8192);
-  DeserializationError err = deserializeJson(doc, json);
-  if (err) {
-    return false;
-  }
-
-  JsonObject meta = doc.createNestedObject("meta");
-  meta["rssi"] = rssi_dbm;
-  meta["snr"] = snr_db;
-  meta["rx_ts"] = rx_ts_utc;
-
-  json = "";
-  return serializeJson(doc, json) > 0;
-}
-
 static bool send_batch_payload(const uint8_t *data, size_t len, uint32_t ts_for_display, uint16_t batch_id) {
   if (!data || len == 0 || len > BATCH_MAX_COMPRESSED) {
     return false;
@@ -411,41 +433,48 @@ static bool send_inflight_batch(uint32_t ts_for_display) {
   if (!g_inflight_active || g_inflight_count == 0) {
     return false;
   }
-  uint32_t json_start = millis();
+  BatchInput input = {};
-  String json;
+  input.sender_id = sender_id_from_short_id(g_short_id);
-  if (!meterBatchToJson(g_inflight_samples, g_inflight_count, g_inflight_batch_id, json, &g_sender_faults, g_sender_last_error)) {
+  input.batch_id = g_inflight_batch_id;
+  input.t_last = g_inflight_samples[g_inflight_count - 1].ts_utc;
+  uint32_t dt_s = METER_SAMPLE_INTERVAL_MS / 1000;
+  input.dt_s = dt_s > 0 ? static_cast<uint8_t>(dt_s) : 1;
+  input.n = g_inflight_count;
+  input.battery_mV = battery_mv_from_voltage(g_inflight_samples[g_inflight_count - 1].battery_voltage_v);
+  for (uint8_t i = 0; i < g_inflight_count; ++i) {
+    input.energy_wh[i] = kwh_to_wh_from_float(g_inflight_samples[i].energy_total_kwh);
+    if (!float_to_i16_w(g_inflight_samples[i].phase_power_w[0], input.p1_w[i]) ||
+        !float_to_i16_w(g_inflight_samples[i].phase_power_w[1], input.p2_w[i]) ||
+        !float_to_i16_w(g_inflight_samples[i].phase_power_w[2], input.p3_w[i])) {
       return false;
     }
-  uint32_t json_ms = millis() - json_start;
-  if (SERIAL_DEBUG_MODE) {
-    serial_debug_printf("tx: batch_id=%u count=%u json_len=%u", g_inflight_batch_id, g_inflight_count, static_cast<unsigned>(json.length()));
-    if (json_ms > 200) {
-      serial_debug_printf("tx: json encode took %lums", static_cast<unsigned long>(json_ms));
-    }
-    serial_debug_print_json(json);
   }
 
-  static uint8_t compressed[BATCH_MAX_COMPRESSED];
+  static uint8_t encoded[BATCH_MAX_COMPRESSED];
-  size_t compressed_len = 0;
+  size_t encoded_len = 0;
-  uint32_t compress_start = millis();
+  uint32_t encode_start = millis();
-  if (!compressBuffer(reinterpret_cast<const uint8_t *>(json.c_str()), json.length(), compressed, sizeof(compressed), compressed_len)) {
+  if (!encode_batch(input, encoded, sizeof(encoded), &encoded_len)) {
     return false;
   }
-  uint32_t compress_ms = millis() - compress_start;
+  uint32_t encode_ms = millis() - encode_start;
-  if (SERIAL_DEBUG_MODE && compress_ms > 200) {
+  if (SERIAL_DEBUG_MODE) {
-    serial_debug_printf("tx: compress took %lums", static_cast<unsigned long>(compress_ms));
+    serial_debug_printf("tx: batch_id=%u count=%u bin_len=%u", g_inflight_batch_id, g_inflight_count,
+                        static_cast<unsigned>(encoded_len));
+    if (encode_ms > 200) {
+      serial_debug_printf("tx: encode took %lums", static_cast<unsigned long>(encode_ms));
     }
-  g_batch_ack_timeout_ms = compute_batch_ack_timeout_ms(compressed_len);
+  }
+  g_batch_ack_timeout_ms = compute_batch_ack_timeout_ms(encoded_len);
 
   uint32_t send_start = millis();
-  bool ok = send_batch_payload(compressed, compressed_len, ts_for_display, g_inflight_batch_id);
+  bool ok = send_batch_payload(encoded, encoded_len, ts_for_display, g_inflight_batch_id);
   uint32_t send_ms = millis() - send_start;
   if (SERIAL_DEBUG_MODE && send_ms > 1000) {
     serial_debug_printf("tx: send batch took %lums", static_cast<unsigned long>(send_ms));
   }
   if (ok) {
     g_last_batch_send_ms = millis();
-    serial_debug_printf("tx: sent batch_id=%u len=%u", g_inflight_batch_id, static_cast<unsigned>(compressed_len));
+    serial_debug_printf("tx: sent batch_id=%u len=%u", g_inflight_batch_id, static_cast<unsigned>(encoded_len));
   } else {
     serial_debug_printf("tx: send failed batch_id=%u", g_inflight_batch_id);
   }
@@ -498,7 +527,7 @@ static void reset_batch_rx() {
   g_batch_rx.timeout_ms = 0;
 }
 
-static bool process_batch_packet(const LoraPacket &pkt, String &out_json, bool &decode_error, uint16_t &out_batch_id) {
+static bool process_batch_packet(const LoraPacket &pkt, BatchInput &out_batch, bool &decode_error, uint16_t &out_batch_id) {
   decode_error = false;
   if (pkt.payload_len < BATCH_HEADER_SIZE) {
     return false;
@@ -545,20 +574,11 @@ static bool process_batch_packet(const LoraPacket &pkt, String &out_json, bool &
   g_batch_rx.last_rx_ms = now_ms;
 
   if (g_batch_rx.next_index == g_batch_rx.expected_chunks && g_batch_rx.received_len == g_batch_rx.total_len) {
-    static uint8_t decompressed[BATCH_MAX_DECOMPRESSED];
+    if (!decode_batch(g_batch_rx.buffer, g_batch_rx.received_len, &out_batch)) {
-    size_t decompressed_len = 0;
-    if (!decompressBuffer(g_batch_rx.buffer, g_batch_rx.received_len, decompressed, sizeof(decompressed) - 1, decompressed_len)) {
       decode_error = true;
       reset_batch_rx();
       return false;
     }
-    if (decompressed_len >= sizeof(decompressed)) {
-      decode_error = true;
-      reset_batch_rx();
-      return false;
-    }
-    decompressed[decompressed_len] = '\0';
-    out_json = String(reinterpret_cast<const char *>(decompressed));
     out_batch_id = batch_id;
     reset_batch_rx();
     return true;
@@ -570,6 +590,9 @@ static bool process_batch_packet(const LoraPacket &pkt, String &out_json, bool &
 void setup() {
   Serial.begin(115200);
   delay(200);
+#ifdef PAYLOAD_CODEC_TEST
+  payload_codec_self_test();
+#endif
 
   watchdog_init();
   g_boot_ms = millis();
@@ -806,15 +829,10 @@ static void receiver_loop() {
         }
       }
     } else if (pkt.payload_type == PayloadType::MeterBatch) {
-      String json;
+      BatchInput batch = {};
       bool decode_error = false;
       uint16_t batch_id = 0;
-      if (process_batch_packet(pkt, json, decode_error, batch_id)) {
+      if (process_batch_packet(pkt, batch, decode_error, batch_id)) {
-        uint32_t rx_ts_utc = time_get_utc();
-        if (rx_ts_utc == 0) {
-          rx_ts_utc = millis() / 1000;
-        }
-        inject_batch_meta(json, pkt.rssi_dbm, pkt.snr_db, rx_ts_utc);
         MeterData samples[METER_BATCH_MAX_SAMPLES];
         size_t count = 0;
         int8_t sender_idx = -1;
@@ -827,13 +845,59 @@ static void receiver_loop() {
         bool duplicate = sender_idx >= 0 && g_last_batch_id_rx[sender_idx] == batch_id;
         if (duplicate) {
           send_batch_ack(batch_id, pkt.device_id_short);
-        } else if (jsonToMeterBatch(json, samples, METER_BATCH_MAX_SAMPLES, count)) {
+        } else {
+          count = batch.n;
+          if (count == 0 || count > METER_BATCH_MAX_SAMPLES) {
+            note_fault(g_receiver_faults, g_receiver_last_error, g_receiver_last_error_utc, g_receiver_last_error_ms, FaultType::Decode);
+            display_set_last_error(g_receiver_last_error, g_receiver_last_error_utc, g_receiver_last_error_ms);
+            return;
+          }
+          uint16_t short_id = pkt.device_id_short;
+          if (short_id == 0) {
+            short_id = short_id_from_sender_id(batch.sender_id);
+          }
+          uint64_t span = static_cast<uint64_t>(batch.dt_s) * static_cast<uint64_t>(count - 1);
+          if (batch.t_last < span) {
+            note_fault(g_receiver_faults, g_receiver_last_error, g_receiver_last_error_utc, g_receiver_last_error_ms, FaultType::Decode);
+            display_set_last_error(g_receiver_last_error, g_receiver_last_error_utc, g_receiver_last_error_ms);
+            return;
+          }
+          uint32_t t_first = batch.t_last - static_cast<uint32_t>(span);
+          float bat_v = batch.battery_mV > 0 ? static_cast<float>(batch.battery_mV) / 1000.0f : NAN;
+          for (size_t s = 0; s < count; ++s) {
+            MeterData &data = samples[s];
+            data = {};
+            data.short_id = short_id;
+            if (short_id != 0) {
+              snprintf(data.device_id, sizeof(data.device_id), "dd3-%04X", short_id);
+            } else {
+              snprintf(data.device_id, sizeof(data.device_id), "dd3-0000");
+            }
+            data.ts_utc = t_first + static_cast<uint32_t>(s) * batch.dt_s;
+            data.energy_total_kwh = static_cast<float>(batch.energy_wh[s]) / 1000.0f;
+            data.phase_power_w[0] = static_cast<float>(batch.p1_w[s]);
+            data.phase_power_w[1] = static_cast<float>(batch.p2_w[s]);
+            data.phase_power_w[2] = static_cast<float>(batch.p3_w[s]);
+            data.total_power_w = data.phase_power_w[0] + data.phase_power_w[1] + data.phase_power_w[2];
+            data.battery_voltage_v = bat_v;
+            if (!isnan(bat_v)) {
+              data.battery_percent = battery_percent_from_voltage(bat_v);
+            } else {
+              data.battery_percent = 0;
+            }
+            data.valid = true;
+            data.link_valid = true;
+            data.link_rssi_dbm = pkt.rssi_dbm;
+            data.link_snr_db = pkt.snr_db;
+            data.err_meter_read = 0;
+            data.err_decode = 0;
+            data.err_lora_tx = 0;
+            data.last_error = FaultType::None;
+          }
+
           if (sender_idx >= 0) {
             web_server_set_last_batch(static_cast<uint8_t>(sender_idx), samples, count);
             for (size_t s = 0; s < count; ++s) {
-              samples[s].link_valid = true;
-              samples[s].link_rssi_dbm = pkt.rssi_dbm;
-              samples[s].link_snr_db = pkt.snr_db;
               samples[s].short_id = pkt.device_id_short;
               mqtt_publish_state(samples[s]);
             }
@@ -856,9 +920,6 @@ static void receiver_loop() {
             g_last_batch_id_rx[sender_idx] = batch_id;
             send_batch_ack(batch_id, pkt.device_id_short);
           }
-        } else {
-          note_fault(g_receiver_faults, g_receiver_last_error, g_receiver_last_error_utc, g_receiver_last_error_ms, FaultType::Decode);
-          display_set_last_error(g_receiver_last_error, g_receiver_last_error_utc, g_receiver_last_error_ms);
         }
       } else if (decode_error) {
         note_fault(g_receiver_faults, g_receiver_last_error, g_receiver_last_error_utc, g_receiver_last_error_ms, FaultType::Decode);

src/payload_codec.cpp

@@ -0,0 +1,325 @@
+#include "payload_codec.h"
+#include <limits.h>
+
+static constexpr uint16_t kMagic = 0xDDB3;
+static constexpr uint8_t kSchema = 1;
+static constexpr uint8_t kFlags = 0x01;
+static constexpr size_t kMaxSamples = 30;
+
+static void write_u16_le(uint8_t *dst, uint16_t value) {
+  dst[0] = static_cast<uint8_t>(value & 0xFF);
+  dst[1] = static_cast<uint8_t>((value >> 8) & 0xFF);
+}
+
+static void write_u32_le(uint8_t *dst, uint32_t value) {
+  dst[0] = static_cast<uint8_t>(value & 0xFF);
+  dst[1] = static_cast<uint8_t>((value >> 8) & 0xFF);
+  dst[2] = static_cast<uint8_t>((value >> 16) & 0xFF);
+  dst[3] = static_cast<uint8_t>((value >> 24) & 0xFF);
+}
+
+static uint16_t read_u16_le(const uint8_t *src) {
+  return static_cast<uint16_t>(src[0]) | (static_cast<uint16_t>(src[1]) << 8);
+}
+
+static uint32_t read_u32_le(const uint8_t *src) {
+  return static_cast<uint32_t>(src[0]) |
+         (static_cast<uint32_t>(src[1]) << 8) |
+         (static_cast<uint32_t>(src[2]) << 16) |
+         (static_cast<uint32_t>(src[3]) << 24);
+}
+
+size_t uleb128_encode(uint32_t v, uint8_t *out, size_t cap) {
+  size_t i = 0;
+  do {
+    if (i >= cap) {
+      return 0;
+    }
+    uint8_t byte = static_cast<uint8_t>(v & 0x7F);
+    v >>= 7;
+    if (v != 0) {
+      byte |= 0x80;
+    }
+    out[i++] = byte;
+  } while (v != 0);
+  return i;
+}
+
+bool uleb128_decode(const uint8_t *in, size_t len, size_t *pos, uint32_t *v) {
+  if (!in || !pos || !v) {
+    return false;
+  }
+  uint32_t result = 0;
+  uint8_t shift = 0;
+  size_t p = *pos;
+  for (uint8_t i = 0; i < 5; ++i) {
+    if (p >= len) {
+      return false;
+    }
+    uint8_t byte = in[p++];
+    if (i == 4 && (byte & 0xF0) != 0) {
+      return false;
+    }
+    result |= static_cast<uint32_t>(byte & 0x7F) << shift;
+    if ((byte & 0x80) == 0) {
+      *pos = p;
+      *v = result;
+      return true;
+    }
+    shift = static_cast<uint8_t>(shift + 7);
+  }
+  return false;
+}
+
+uint32_t zigzag32(int32_t x) {
+  return (static_cast<uint32_t>(x) << 1) ^ static_cast<uint32_t>(x >> 31);
+}
+
+int32_t unzigzag32(uint32_t u) {
+  return static_cast<int32_t>((u >> 1) ^ (static_cast<uint32_t>(-static_cast<int32_t>(u & 1))));
+}
+
+size_t svarint_encode(int32_t x, uint8_t *out, size_t cap) {
+  uint32_t zz = zigzag32(x);
+  return uleb128_encode(zz, out, cap);
+}
+
+bool svarint_decode(const uint8_t *in, size_t len, size_t *pos, int32_t *x) {
+  uint32_t u = 0;
+  if (!uleb128_decode(in, len, pos, &u)) {
+    return false;
+  }
+  *x = unzigzag32(u);
+  return true;
+}
+
+static bool ensure_capacity(size_t needed, size_t cap, size_t pos) {
+  return pos + needed <= cap;
+}
+
+bool encode_batch(const BatchInput &in, uint8_t *out, size_t out_cap, size_t *out_len) {
+  if (!out || !out_len) {
+    return false;
+  }
+  if (in.n == 0 || in.n > kMaxSamples) {
+    return false;
+  }
+  if (in.dt_s == 0) {
+    return false;
+  }
+  size_t pos = 0;
+  if (!ensure_capacity(16, out_cap, pos)) {
+    return false;
+  }
+  write_u16_le(&out[pos], kMagic);
+  pos += 2;
+  out[pos++] = kSchema;
+  out[pos++] = kFlags;
+  write_u16_le(&out[pos], in.sender_id);
+  pos += 2;
+  write_u16_le(&out[pos], in.batch_id);
+  pos += 2;
+  write_u32_le(&out[pos], in.t_last);
+  pos += 4;
+  out[pos++] = in.dt_s;
+  out[pos++] = in.n;
+  write_u16_le(&out[pos], in.battery_mV);
+  pos += 2;
+
+  if (!ensure_capacity(4, out_cap, pos)) {
+    return false;
+  }
+  write_u32_le(&out[pos], in.energy_wh[0]);
+  pos += 4;
+  for (uint8_t i = 1; i < in.n; ++i) {
+    if (in.energy_wh[i] < in.energy_wh[i - 1]) {
+      return false;
+    }
+    uint32_t delta = in.energy_wh[i] - in.energy_wh[i - 1];
+    size_t wrote = uleb128_encode(delta, &out[pos], out_cap - pos);
+    if (wrote == 0) {
+      return false;
+    }
+    pos += wrote;
+  }
+
+  auto encode_phase = [&](const int16_t *phase) -> bool {
+    if (!ensure_capacity(2, out_cap, pos)) {
+      return false;
+    }
+    write_u16_le(&out[pos], static_cast<uint16_t>(phase[0]));
+    pos += 2;
+    for (uint8_t i = 1; i < in.n; ++i) {
+      int32_t delta = static_cast<int32_t>(phase[i]) - static_cast<int32_t>(phase[i - 1]);
+      size_t wrote = svarint_encode(delta, &out[pos], out_cap - pos);
+      if (wrote == 0) {
+        return false;
+      }
+      pos += wrote;
+    }
+    return true;
+  };
+
+  if (!encode_phase(in.p1_w)) {
+    return false;
+  }
+  if (!encode_phase(in.p2_w)) {
+    return false;
+  }
+  if (!encode_phase(in.p3_w)) {
+    return false;
+  }
+
+  *out_len = pos;
+  return true;
+}
+
+bool decode_batch(const uint8_t *buf, size_t len, BatchInput *out) {
+  if (!buf || !out) {
+    return false;
+  }
+  size_t pos = 0;
+  if (len < 16) {
+    return false;
+  }
+  uint16_t magic = read_u16_le(&buf[pos]);
+  pos += 2;
+  uint8_t schema = buf[pos++];
+  uint8_t flags = buf[pos++];
+  if (magic != kMagic || schema != kSchema || (flags & 0x01) == 0) {
+    return false;
+  }
+  out->sender_id = read_u16_le(&buf[pos]);
+  pos += 2;
+  out->batch_id = read_u16_le(&buf[pos]);
+  pos += 2;
+  out->t_last = read_u32_le(&buf[pos]);
+  pos += 4;
+  out->dt_s = buf[pos++];
+  out->n = buf[pos++];
+  out->battery_mV = read_u16_le(&buf[pos]);
+  pos += 2;
+
+  if (out->n == 0 || out->n > kMaxSamples || out->dt_s == 0) {
+    return false;
+  }
+  if (pos + 4 > len) {
+    return false;
+  }
+  out->energy_wh[0] = read_u32_le(&buf[pos]);
+  pos += 4;
+  for (uint8_t i = 1; i < out->n; ++i) {
+    uint32_t delta = 0;
+    if (!uleb128_decode(buf, len, &pos, &delta)) {
+      return false;
+    }
+    uint64_t sum = static_cast<uint64_t>(out->energy_wh[i - 1]) + static_cast<uint64_t>(delta);
+    if (sum > UINT32_MAX) {
+      return false;
+    }
+    out->energy_wh[i] = static_cast<uint32_t>(sum);
+  }
+
+  auto decode_phase = [&](int16_t *phase) -> bool {
+    if (pos + 2 > len) {
+      return false;
+    }
+    phase[0] = static_cast<int16_t>(read_u16_le(&buf[pos]));
+    pos += 2;
+    int32_t prev = static_cast<int32_t>(phase[0]);
+    for (uint8_t i = 1; i < out->n; ++i) {
+      int32_t delta = 0;
+      if (!svarint_decode(buf, len, &pos, &delta)) {
+        return false;
+      }
+      int32_t value = prev + delta;
+      if (value < INT16_MIN || value > INT16_MAX) {
+        return false;
+      }
+      phase[i] = static_cast<int16_t>(value);
+      prev = value;
+    }
+    return true;
+  };
+
+  if (!decode_phase(out->p1_w)) {
+    return false;
+  }
+  if (!decode_phase(out->p2_w)) {
+    return false;
+  }
+  if (!decode_phase(out->p3_w)) {
+    return false;
+  }
+
+  for (uint8_t i = out->n; i < kMaxSamples; ++i) {
+    out->energy_wh[i] = 0;
+    out->p1_w[i] = 0;
+    out->p2_w[i] = 0;
+    out->p3_w[i] = 0;
+  }
+
+  return pos == len;
+}
+
+#ifdef PAYLOAD_CODEC_TEST
+bool payload_codec_self_test() {
+  BatchInput in = {};
+  in.sender_id = 1;
+  in.batch_id = 42;
+  in.t_last = 1700000000;
+  in.dt_s = 1;
+  in.n = 5;
+  in.battery_mV = 3750;
+  in.energy_wh[0] = 100000;
+  in.energy_wh[1] = 100001;
+  in.energy_wh[2] = 100050;
+  in.energy_wh[3] = 100050;
+  in.energy_wh[4] = 100200;
+  in.p1_w[0] = -120;
+  in.p1_w[1] = -90;
+  in.p1_w[2] = 1910;
+  in.p1_w[3] = -90;
+  in.p1_w[4] = 500;
+  in.p2_w[0] = 50;
+  in.p2_w[1] = -1950;
+  in.p2_w[2] = 60;
+  in.p2_w[3] = 2060;
+  in.p2_w[4] = -10;
+  in.p3_w[0] = 0;
+  in.p3_w[1] = 10;
+  in.p3_w[2] = -1990;
+  in.p3_w[3] = 10;
+  in.p3_w[4] = 20;
+
+  uint8_t buf[256];
+  size_t len = 0;
+  if (!encode_batch(in, buf, sizeof(buf), &len)) {
+    Serial.println("payload_codec_self_test: encode failed");
+    return false;
+  }
+
+  BatchInput out = {};
+  if (!decode_batch(buf, len, &out)) {
+    Serial.println("payload_codec_self_test: decode failed");
+    return false;
+  }
+
+  if (out.sender_id != in.sender_id || out.batch_id != in.batch_id || out.t_last != in.t_last ||
+      out.dt_s != in.dt_s || out.n != in.n || out.battery_mV != in.battery_mV) {
+    Serial.println("payload_codec_self_test: header mismatch");
+    return false;
+  }
+
+  for (uint8_t i = 0; i < in.n; ++i) {
+    if (out.energy_wh[i] != in.energy_wh[i] || out.p1_w[i] != in.p1_w[i] || out.p2_w[i] != in.p2_w[i] ||
+        out.p3_w[i] != in.p3_w[i]) {
+      Serial.println("payload_codec_self_test: sample mismatch");
+      return false;
+    }
+  }
+
+  Serial.printf("payload_codec_self_test: ok len=%u\n", static_cast<unsigned>(len));
+  return true;
+}
+#endif

src/payload_codec.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <Arduino.h>
+
+struct BatchInput {
+  uint16_t sender_id;
+  uint16_t batch_id;
+  uint32_t t_last;
+  uint8_t dt_s;
+  uint8_t n;
+  uint16_t battery_mV;
+  uint32_t energy_wh[30];
+  int16_t p1_w[30];
+  int16_t p2_w[30];
+  int16_t p3_w[30];
+};
+
+bool encode_batch(const BatchInput &in, uint8_t *out, size_t out_cap, size_t *out_len);
+bool decode_batch(const uint8_t *buf, size_t len, BatchInput *out);
+
+size_t uleb128_encode(uint32_t v, uint8_t *out, size_t cap);
+bool uleb128_decode(const uint8_t *in, size_t len, size_t *pos, uint32_t *v);
+
+uint32_t zigzag32(int32_t x);
+int32_t unzigzag32(uint32_t u);
+
+size_t svarint_encode(int32_t x, uint8_t *out, size_t cap);
+bool svarint_decode(const uint8_t *in, size_t len, size_t *pos, int32_t *x);
+
+#ifdef PAYLOAD_CODEC_TEST
+bool payload_codec_self_test();
+#endif