Merge pull request #69 from Relys/bms_feature

VESC BLE Proxy Fix, Advanced Light bar support, OneWheel Stock BMS support.

avaspark-rgb.csv

@@ -0,0 +1,6 @@
+# Name,   Type, SubType, Offset,  Size, Flags
+nvs,      data, nvs,     0x9000,  0x5000,
+otadata,  data, ota,     0xe000,  0x2000,
+app0,     app,  ota_0,   0x10000, 0x1C0000,
+app1,     app,  ota_1,   0x1D0000,0x1C0000,
+spiffs,   data, spiffs,  0x390000,0x470000,

lib/bms/battery_fuel_gauge.cc

@@ -0,0 +1,69 @@
+#include "battery_fuel_gauge.h"
+
+#include <cmath>
+
+#include "defer.h"
+
+#include <algorithm>
+
+namespace {
+
+template <class T>
+inline T clamp(T x, T lower, T upper) {
+  return std::min(upper, std::max(x, lower));
+}
+
+int openCircuitSocFromCellVoltage(int cellVoltageMillivolts) {
+  static constexpr int LOOKUP_TABLE_RANGE_MIN_MV = 2700;
+  static constexpr int LOOKUP_TABLE_RANGE_MAX_MV = 4200;
+  static uint8_t LOOKUP_TABLE[31] = {0,  0,  0,  0,  1,  2,  3,  4,  5,  7,  8,
+                                     11, 14, 16, 18, 19, 25, 30, 33, 37, 43, 48,
+                                     53, 60, 67, 71, 76, 82, 92, 97, 100};
+  static constexpr int LOOKUP_TABLE_SIZE =
+      (sizeof(LOOKUP_TABLE) / sizeof(*LOOKUP_TABLE));
+  static constexpr int RANGE =
+      LOOKUP_TABLE_RANGE_MAX_MV - LOOKUP_TABLE_RANGE_MIN_MV;
+  // (RANGE - 1) upper limit effectively clamps the leftIndex below to
+  // (LOOKUP_TABLE_SIZE - 2)
+  cellVoltageMillivolts =
+      clamp(cellVoltageMillivolts - LOOKUP_TABLE_RANGE_MIN_MV, 0, RANGE - 1);
+  float floatIndex =
+      float(cellVoltageMillivolts) * (LOOKUP_TABLE_SIZE - 1) / RANGE;
+  const int leftIndex = int(floatIndex);
+  const float fractional = floatIndex - leftIndex;
+  const int rightIndex = leftIndex + 1;
+  const int leftValue = LOOKUP_TABLE[leftIndex];
+  const int rightValue = LOOKUP_TABLE[rightIndex];
+  return clamp<int>(leftValue + round((rightValue - leftValue) * fractional), 0,
+                    100);
+}
+
+}  // namespace
+
+void BatteryFuelGauge::updateVoltage(int32_t voltageMillivolts,
+                                     int32_t nowMillis) {
+  cell_voltage_filter_.step(voltageMillivolts);
+}
+
+void BatteryFuelGauge::updateCurrent(int32_t currentMilliamps,
+                                     int32_t nowMillis) {
+  defer { last_current_update_time_millis_ = nowMillis; };
+  if (last_current_update_time_millis_ < 0) {
+    return;
+  }
+  const int32_t millisSinceLastUpdate =
+      nowMillis - last_current_update_time_millis_;
+  const int32_t milliampSecondsDelta =
+      millisSinceLastUpdate * currentMilliamps / 1000;
+  if (milliampSecondsDelta > 0) {
+    milliamp_seconds_discharged_ += milliampSecondsDelta;
+  } else {
+    milliamp_seconds_recharged_ -= milliampSecondsDelta;
+  }
+}
+
+void BatteryFuelGauge::restoreState() {}
+void BatteryFuelGauge::saveState() {}
+int32_t BatteryFuelGauge::getBatteryPercentage() {
+  return openCircuitSocFromCellVoltage((int)cell_voltage_filter_.get());
+}

lib/bms/battery_fuel_gauge.h

@@ -0,0 +1,44 @@
+#ifndef BATTERY_FUEL_GAUGE_H
+#define BATTERY_FUEL_GAUGE_H
+
+#include <stdint.h>
+
+#include "filter.h"
+
+/**
+ * Assumes that the class gets to see all of the energy going into and out of
+ * the battery.
+ */
+class BatteryFuelGauge {
+ public:
+  // restoreState must be called exatly once and before any other calls on this
+  // object.
+  void restoreState();
+  void saveState();
+
+  // Takes a single cell voltage in millivolts.
+  void updateVoltage(int32_t voltageMillivolts, int32_t nowMillis);
+  void updateCurrent(int32_t currentMilliamps, int32_t nowMillis);
+  // Only transition from true to false is expected.
+  void updateChargingStatus(bool charging) { charging_ = charging; }
+  int32_t getBatteryPercentage();
+
+  int32_t getMilliampSecondsDischarged() {
+    return milliamp_seconds_discharged_;
+  }
+  int32_t getMilliampSecondsRecharged() { return milliamp_seconds_recharged_; }
+
+ private:
+  LowPassFilter cell_voltage_filter_;
+  int32_t known_range_top_voltage_millivolts_ = -1;
+  int32_t known_range_bottom_voltage_millivolts_ = -1;
+  int32_t battery_capacity_hint_milliamp_seconds_ = -1;
+  int32_t spent_milliamps_second_ = -1;
+  int32_t regenerated_milliamps_second_ = -1;
+  int32_t last_current_update_time_millis_ = -1;
+  int32_t milliamp_seconds_discharged_ = 0;
+  int32_t milliamp_seconds_recharged_ = 0;
+  bool charging_ = false;
+};
+
+#endif  // BATTERY_FUEL_GAUGE_H

lib/bms/bms_relay.cpp

@@ -0,0 +1,127 @@
+#include "bms_relay.h"
+
+#include <cstring>
+#include <limits>
+
+#include "packet.h"
+
+namespace {
+const uint8_t PREAMBLE[] = {0xFF, 0x55, 0xAA};
+
+unsigned long packetTypeRebroadcastTimeout(int type) {
+  if (type == 0 || type == 5) {
+    return 500;
+  }
+  // Never rebroadcast the shutdown packet.
+  if (type == 11) {
+    return std::numeric_limits<unsigned long>::max();
+  }
+  return 3000;
+}
+
+}  // namespace
+
+BmsRelay::BmsRelay(const Source& source, const Sink& sink,
+                   const MillisProvider& millis)
+    : source_(source), sink_(sink), millis_provider_(millis) {
+  sourceBuffer_.reserve(64);
+}
+
+void BmsRelay::loop() {
+  while (true) {
+    int byte = source_();
+    now_millis_ = millis_provider_();
+    if (byte < 0) {
+      maybeReplayPackets();
+      return;
+    }
+    sourceBuffer_.push_back(byte);
+    processNextByte();
+  }
+}
+
+void BmsRelay::maybeReplayPackets() {
+  for (const IndividualPacketStat& stat :
+       packet_tracker_.getIndividualPacketStats()) {
+    if (stat.total_num < 1) {
+      continue;
+    }
+    if ((now_millis_ - stat.last_packet_millis) <
+        packetTypeRebroadcastTimeout(stat.id)) {
+      continue;
+    }
+    std::vector<uint8_t> data_copy(stat.last_seen_valid_packet);
+    Packet p(data_copy.data(), data_copy.size());
+    ingestPacket(p);
+  }
+}
+
+void BmsRelay::purgeUnknownData() {
+  for (uint8_t b : sourceBuffer_) {
+    sink_(b);
+  }
+  if (unknownDataCallback_) {
+    for (uint8_t b : sourceBuffer_) {
+      unknownDataCallback_(b);
+    }
+  }
+  packet_tracker_.unknownBytes(sourceBuffer_.size());
+  sourceBuffer_.clear();
+}
+
+// Called with every new byte.
+void BmsRelay::processNextByte() {
+  // If up to first three bytes of the sourceBuffer don't match expected
+  // preamble, flush the data unchanged.
+  for (unsigned int i = 0; i < std::min(sizeof(PREAMBLE), sourceBuffer_.size());
+       i++) {
+    if (sourceBuffer_[i] != PREAMBLE[i]) {
+      purgeUnknownData();
+      return;
+    }
+  }
+  // Check if we have the message type.
+  if (sourceBuffer_.size() < 4) {
+    return;
+  }
+  uint8_t type = sourceBuffer_[3];
+  if (type >= sizeof(PACKET_LENGTHS_BY_TYPE) ||
+      PACKET_LENGTHS_BY_TYPE[type] < 0) {
+    purgeUnknownData();
+    return;
+  }
+  uint8_t len = PACKET_LENGTHS_BY_TYPE[type];
+  if (sourceBuffer_.size() < len) {
+    return;
+  }
+  Packet p(sourceBuffer_.data(), len);
+  ingestPacket(p);
+  sourceBuffer_.clear();
+}
+
+void BmsRelay::ingestPacket(Packet& p) {
+  packet_tracker_.processPacket(p, now_millis_);
+  for (auto& callback : receivedPacketCallbacks_) {
+    callback(this, &p);
+  };
+  bmsStatusParser(p);
+  bmsSerialParser(p);
+  currentParser(p);
+  batteryPercentageParser(p);
+  cellVoltageParser(p);
+  temperatureParser(p);
+  powerOffParser(p);
+  //  Recalculate CRC so that logging callbacks see the correct CRCs
+  p.recalculateCrcIfValid();
+  if (p.shouldForward()) {
+    for (auto& callback : forwardedPacketCallbacks_) {
+      callback(this, &p);
+    }
+  }
+  p.recalculateCrcIfValid();
+  if (p.shouldForward()) {
+    for (int i = 0; i < p.len(); i++) {
+      sink_(p.start()[i]);
+    }
+  }
+}