diff --git a/avaspark-rgb.csv b/avaspark-rgb.csv new file mode 100644 index 0000000..d42b809 --- /dev/null +++ b/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, \ No newline at end of file diff --git a/lib/bms/battery_fuel_gauge.cc b/lib/bms/battery_fuel_gauge.cc new file mode 100644 index 0000000..21ef363 --- /dev/null +++ b/lib/bms/battery_fuel_gauge.cc @@ -0,0 +1,69 @@ +#include "battery_fuel_gauge.h" + +#include + +#include "defer.h" + +#include + +namespace { + +template +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(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()); +} \ No newline at end of file diff --git a/lib/bms/battery_fuel_gauge.h b/lib/bms/battery_fuel_gauge.h new file mode 100644 index 0000000..42236ba --- /dev/null +++ b/lib/bms/battery_fuel_gauge.h @@ -0,0 +1,44 @@ +#ifndef BATTERY_FUEL_GAUGE_H +#define BATTERY_FUEL_GAUGE_H + +#include + +#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 diff --git a/lib/bms/bms_relay.cpp b/lib/bms/bms_relay.cpp new file mode 100644 index 0000000..0cff1b6 --- /dev/null +++ b/lib/bms/bms_relay.cpp @@ -0,0 +1,127 @@ +#include "bms_relay.h" + +#include +#include + +#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::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 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]); + } + } +} \ No newline at end of file diff --git a/lib/bms/bms_relay.h b/lib/bms/bms_relay.h new file mode 100644 index 0000000..1e7dc0e --- /dev/null +++ b/lib/bms/bms_relay.h @@ -0,0 +1,166 @@ +#ifndef BMS_RELAY_H +#define BMS_RELAY_H + +#include +#include +#include +#include + +#include "battery_fuel_gauge.h" +#include "packet_tracker.h" + +class Packet; + +class BmsRelay { + public: + /** + * @brief Function polled for new byte from BMS. Expected to return negative + * value when there's no data available on the wire. + */ + typedef std::function Source; + /** + * @brief Function called to send data to the MB. + */ + typedef std::function Sink; + + /** + * @brief Packet callback. + */ + typedef std::function PacketCallback; + + typedef std::function MillisProvider; + + BmsRelay(const Source& source, const Sink& sink, + const MillisProvider& millisProvider); + + /** + * @brief Call from arduino loop. + */ + void loop(); + + void addReceivedPacketCallback(const PacketCallback& callback) { + receivedPacketCallbacks_.push_back(callback); + } + + void addForwardedPacketCallback(const PacketCallback& callback) { + forwardedPacketCallbacks_.push_back(callback); + } + + void setPowerOffCallback(const std::function& c) { + powerOffCallback_ = c; + } + + void setSocRewriterCallback(const std::function& c) { + socRewriterCallback_ = c; + } + + void setUnknownDataCallback(const Sink& c) { unknownDataCallback_ = c; } + + /** + * @brief If set to non-zero value, spoofs captured BMS serial + * with the number provided here. The serial number can be found + * on the sticker on the bottom side of BMS. The lower number on + * the sticker without the 4 leading (most significant) digits + * goes here. + */ + void setBMSSerialOverride(uint32_t serial) { serial_override_ = serial; } + + /** + * @brief Get the captured BMS serial number. + * + * @return non-zero serial, if it's already been captured. + */ + uint32_t getCapturedBMSSerial() { return captured_serial_; } + + /** + * @brief Battery percentage as reported by the BMS. + */ + int8_t getBmsReportedSOC() { return bms_soc_percent_; } + + /** + * @brief Spoofed battery percentage sent to the controller. + */ + int8_t getOverriddenSOC() { return overridden_soc_percent_; } + /** + * @brief Cell voltages in millivolts. + * @return pointer to a 15 element array. + */ + uint16_t* const getCellMillivolts() { return cell_millivolts_; } + + uint16_t getTotalVoltageMillivolts() { return total_voltage_millivolts_; } + + /** + * @brief Cell voltages in millivolts. + * @return pointer to a 5 element array. + */ + int8_t* const getTemperaturesCelsius() { return temperatures_celsius_; } + + uint8_t getAverageTemperatureCelsius() { return avg_temperature_celsius_; } + + /** + * @brief Current In Amps. + */ + int32_t getCurrentMilliamps() { return current_milliamps_; } + + int32_t getUsedChargeMah() { + return battery_fuel_gauge_.getMilliampSecondsDischarged() / 3600; + } + int32_t getRegeneratedChargeMah() { + return battery_fuel_gauge_.getMilliampSecondsRecharged() / 3600; + } + + bool isCharging() { return last_status_byte_ & 0x20; } + bool isBatteryEmpty() { return last_status_byte_ & 0x4; } + bool isBatteryTempOutOfRange() { + // Both these bits throw the same error, one is prob hot and the other is + // cold. + return last_status_byte_ & 3; + } + bool isBatteryOvercharged() { return last_status_byte_ & 8; } + + const PacketTracker& getPacketTracker() const { return packet_tracker_; } + + private: + // BMS current units to milliamps. + static constexpr int CURRENT_SCALER = 55; + void processNextByte(); + void purgeUnknownData(); + void maybeReplayPackets(); + void ingestPacket(Packet& p); + + std::vector receivedPacketCallbacks_; + std::vector forwardedPacketCallbacks_; + Sink unknownDataCallback_; + std::function socRewriterCallback_; + std::function powerOffCallback_; + + std::vector sourceBuffer_; + uint32_t serial_override_ = 0; + uint32_t captured_serial_ = 0; + int16_t current_milliamps_ = 0; + + int8_t bms_soc_percent_ = -1; + int8_t overridden_soc_percent_ = -1; + uint16_t cell_millivolts_[15] = {0}; + uint16_t total_voltage_millivolts_ = 0; + + int8_t temperatures_celsius_[5] = {0}; + int8_t avg_temperature_celsius_ = 0; + uint8_t last_status_byte_ = 0; + const Source source_; + const Sink sink_; + const MillisProvider millis_provider_; + int32_t now_millis_; + PacketTracker packet_tracker_; + BatteryFuelGauge battery_fuel_gauge_; + + void bmsStatusParser(Packet& p); + void bmsSerialParser(Packet& p); + void currentParser(Packet& p); + void batteryPercentageParser(Packet& p); + void cellVoltageParser(Packet& p); + void temperatureParser(Packet& p); + void powerOffParser(Packet& p); +}; + +#endif // BMS_RELAY_H \ No newline at end of file diff --git a/lib/bms/defer.h b/lib/bms/defer.h new file mode 100644 index 0000000..eab2cba --- /dev/null +++ b/lib/bms/defer.h @@ -0,0 +1,18 @@ +// Taken from +// https://stackoverflow.com/questions/32432450/what-is-standard-defer-finalizer-implementation-in-c +#ifndef DEFER_H +struct defer_dummy {}; + +template +struct deferrer { + F f; + ~deferrer() { f(); } +}; +template +deferrer operator*(defer_dummy, F f) { + return {f}; +} +#define DEFER_(LINE) zz_defer##LINE +#define DEFER(LINE) DEFER_(LINE) +#define defer auto DEFER(__LINE__) = defer_dummy{} *[&]() +#endif // DEFER_H diff --git a/lib/bms/filter.h b/lib/bms/filter.h new file mode 100644 index 0000000..6f6358a --- /dev/null +++ b/lib/bms/filter.h @@ -0,0 +1,41 @@ +#ifndef FILTER_H +#define FILTER_H + +// Based on +// http://www.schwietering.com/jayduino/filtuino/index.php?characteristic=bu&passmode=lp&order=2&usesr=usesr&sr=1&frequencyLow=0.0025¬eLow=¬eHigh=&pw=pw&calctype=float&run=Send +// With alpha=0.0025 and observed sample rate of the voltage messages of 1Hz, +// we get low pass period of 400 seconds. +// +// Low pass butterworth filter order=2 alpha1=0.0025 +class LowPassFilter { + public: + LowPassFilter() = default; + + private: + bool initialized = false; + + float v[3] = {0}; + + void setTo(float value) { + const float val = value / 4; + v[0] = val; + v[1] = val; + v[2] = val; + } + + public: + float get() { return (v[0] + v[2]) + 2 * v[1]; } + void step(float x) { + if (!initialized) { + initialized = true; + setTo(x); + return; + } + v[0] = v[1]; + v[1] = v[2]; + v[2] = (6.100617875806624291e-5 * x) + (-0.97803050849179629100 * v[0]) + + (1.97778648377676402603 * v[1]); + } +}; + +#endif \ No newline at end of file diff --git a/lib/bms/packet.cpp b/lib/bms/packet.cpp new file mode 100644 index 0000000..f7771a9 --- /dev/null +++ b/lib/bms/packet.cpp @@ -0,0 +1,27 @@ +#include "packet.h" + +void Packet::validate() { + valid_ = false; + if (len_ < 6) { + return; + } + uint16_t crc = ((uint16_t)(start_[len_ - 2])) << 8 | start_[len_ - 1]; + for (int i = 0; i < len_ - 2; i++) { + crc -= start_[i]; + } + if (crc == 0) { + valid_ = true; + } +} + +void Packet::recalculateCrcIfValid() { + if (!valid_) { + return; + } + uint16_t crc = 0; + for (int i = 0; i < len_ - 2; i++) { + crc += start_[i]; + } + start_[len_ - 2] = (crc >> 8); + start_[len_ - 1] = (crc & 0xFF); +} \ No newline at end of file diff --git a/lib/bms/packet.h b/lib/bms/packet.h new file mode 100644 index 0000000..7d035e6 --- /dev/null +++ b/lib/bms/packet.h @@ -0,0 +1,58 @@ +#ifndef PACKET_H +#define PACKET_H + +#include + +// A bunch of places use sizeof(), don't change type. +static const int8_t PACKET_LENGTHS_BY_TYPE[] = { + 7, -1, 38, 7, 11, 8, 10, 13, 7, 7, -1, 8, 8, 9, -1, 11, 16, 10}; + +class Packet { + public: + Packet(uint8_t* start, uint8_t len) : start_(start), len_(len) { validate(); } + + int getType() const { + if (!valid_) { + return -1; + } + return start_[3]; + }; + + bool isValid() const { return valid_; } + + uint8_t* data() const { + if (!valid_) { + return nullptr; + } + return start_ + 4; + } + + int dataLength() const { + if (!valid_) { + return -1; + } + // 3 byte preamble + 1 byte type + 2 byte CRC + return len_ - 6; + } + + void setShouldForward(bool shouldForward) { + this->shouldForward_ = shouldForward; + } + + bool shouldForward() { return this->shouldForward_; } + + void recalculateCrcIfValid(); + + uint8_t* const start() const { return start_; } + + uint8_t len() const { return len_; } + + private: + void validate(); + uint8_t* start_; + uint8_t len_; + bool valid_ = false; + bool shouldForward_ = true; +}; + +#endif // PACKET_H \ No newline at end of file diff --git a/lib/bms/packet_parsers.cpp b/lib/bms/packet_parsers.cpp new file mode 100644 index 0000000..7ec61e5 --- /dev/null +++ b/lib/bms/packet_parsers.cpp @@ -0,0 +1,122 @@ +#include +#include +#include + +#include "bms_relay.h" +#include "packet.h" + +namespace { + +inline int16_t int16FromNetworkOrder(const void* const p) { + uint8_t* const charPointer = (uint8_t* const)p; + return ((uint16_t)(*charPointer)) << 8 | *(charPointer + 1); +} + +} // namespace + +void BmsRelay::batteryPercentageParser(Packet& p) { + if (p.getType() != 3) { + return; + } + // 0x3 message is just one byte containing battery percentage. + bms_soc_percent_ = *(int8_t*)p.data(); + overridden_soc_percent_ = battery_fuel_gauge_.getBatteryPercentage(); + if (overridden_soc_percent_ < 0) { + p.setShouldForward(false); + return; + } + p.data()[0] = overridden_soc_percent_; +} + +void BmsRelay::currentParser(Packet& p) { + if (p.getType() != 5) { + return; + } + p.setShouldForward(isCharging()); + + // 0x5 message encodes current as signed int16. + // The scaling factor (tested on a Pint) seems to be 0.055 + // i.e. 1 in the data message below corresponds to 55 milliamps. + current_milliamps_ = int16FromNetworkOrder(p.data()) * CURRENT_SCALER; + battery_fuel_gauge_.updateCurrent(current_milliamps_, now_millis_); +} + +void BmsRelay::bmsSerialParser(Packet& p) { + if (p.getType() != 6) { + return; + } + // 0x6 message has the BMS serial number encoded inside of it. + // It is the last seven digits from the sticker on the back of the BMS. + if (captured_serial_ == 0) { + for (int i = 0; i < 4; i++) { + captured_serial_ |= p.data()[i] << (8 * (3 - i)); + } + } + if (serial_override_ == 0) { + return; + } + uint32_t serial_override_copy = serial_override_; + for (int i = 3; i >= 0; i--) { + p.data()[i] = serial_override_copy & 0xFF; + serial_override_copy >>= 8; + } +} + +void BmsRelay::cellVoltageParser(Packet& p) { + if (p.getType() != 2) { + return; + } + // The data in this packet is 16 int16-s. First 15 of them is + // individual cell voltages in millivolts. The last value is mysterious. + const uint8_t* const data = p.data(); + uint16_t total_voltage = 0; + uint16_t min_voltage = 0xFFFF; + for (int i = 0; i < 15; i++) { + uint16_t cellVoltage = int16FromNetworkOrder(data + (i << 1)); + total_voltage += cellVoltage; + if (cellVoltage < min_voltage) { + min_voltage = cellVoltage; + } + cell_millivolts_[i] = cellVoltage; + } + total_voltage_millivolts_ = total_voltage; + battery_fuel_gauge_.updateVoltage(min_voltage, now_millis_); +} + +void BmsRelay::temperatureParser(Packet& p) { + if (p.getType() != 4) { + return; + } + int8_t* const temperatures = (int8_t*)p.data(); + int16_t temperature_sum = 0; + for (int i = 0; i < 5; i++) { + int8_t temp = temperatures[i]; + temperature_sum += temp; + temperatures_celsius_[i] = temp; + } + avg_temperature_celsius_ = temperature_sum / 5; +} + +void BmsRelay::powerOffParser(Packet& p) { + if (p.getType() != 11) { + return; + } + // We're about to shut down. + if (powerOffCallback_) { + powerOffCallback_(); + } +} + +void BmsRelay::bmsStatusParser(Packet& p) { + if (p.getType() != 0) { + return; + } + last_status_byte_ = p.data()[0]; + + // Forwarding the status packet during normal operation seems to drive + // an event loop in the controller that tallies used Ah and eventually + // makes the board throw Error 23. Swallowing the packet does the trick. + p.setShouldForward(isCharging() || isBatteryEmpty() || + isBatteryTempOutOfRange() || isBatteryOvercharged()); + battery_fuel_gauge_.updateChargingStatus(isCharging()); +} diff --git a/lib/bms/packet_tracker.cpp b/lib/bms/packet_tracker.cpp new file mode 100644 index 0000000..e40c7fb --- /dev/null +++ b/lib/bms/packet_tracker.cpp @@ -0,0 +1,46 @@ +#include "packet_tracker.h" + +#include + +#include "defer.h" + +PacketTracker::PacketTracker() + : individual_packet_stats_(sizeof(PACKET_LENGTHS_BY_TYPE)) {} + +void PacketTracker::processPacket(const Packet& packet, + const unsigned long now_millis) { + if (!packet.isValid()) { + global_stats_.total_packet_checksum_mismatches++; + } + // Though this should never happen, let's sanity check the packet + // type so we don't accidentally OOM. + int type = packet.getType(); + if (type < 0 || type > 255) { + return; + } + if (type >= (int)individual_packet_stats_.size()) { + individual_packet_stats_.resize(type + 1); + } + global_stats_.total_known_bytes_received += packet.len(); + global_stats_.total_known_packets_received++; + + IndividualPacketStat* stat = &individual_packet_stats_[type]; + defer { stat->last_packet_millis = now_millis; }; + + stat->last_seen_valid_packet.resize(packet.len()); + memcpy(&stat->last_seen_valid_packet[0], packet.start(), packet.len()); + + if (stat->total_num++ == 0) { + stat->id = type; + return; + } + // Shouldn't happen but let's guard against. + if (now_millis < stat->last_packet_millis) { + return; + } + stat->mean_and_dev_.add_value(float(now_millis - stat->last_packet_millis)); +} + +void PacketTracker::unknownBytes(int num) { + global_stats_.total_unknown_bytes_received += num; +} \ No newline at end of file diff --git a/lib/bms/packet_tracker.h b/lib/bms/packet_tracker.h new file mode 100644 index 0000000..80a4047 --- /dev/null +++ b/lib/bms/packet_tracker.h @@ -0,0 +1,55 @@ +#ifndef PACKET_TRACKER_H +#define PACKET_TRACKER_H + +#include +#include +#include + +#include "packet.h" +#include "welford.h" + +class IndividualPacketStat { + public: + IndividualPacketStat() : id(-1), total_num(0), last_packet_millis(0) {} + // Packet message id, -1 if not initialized + int id; + int32_t total_num; + std::vector last_seen_valid_packet; + unsigned long last_packet_millis; + int32_t mean_period_millis() const { return (int32_t)mean_and_dev_.mean(); } + int32_t deviation_millis() const { return (int32_t)mean_and_dev_.sd(); }; + + private: + Welford mean_and_dev_; + friend class PacketTracker; +}; + +struct GlobalStats { + GlobalStats() + : total_known_packets_received(0), + total_known_bytes_received(0), + total_packet_checksum_mismatches(0), + total_unknown_bytes_received(0) {} + int32_t total_known_packets_received; + int32_t total_known_bytes_received; + int32_t total_packet_checksum_mismatches; + int32_t total_unknown_bytes_received; +}; + +class PacketTracker { + public: + PacketTracker(); + + void processPacket(const Packet& packet, const unsigned long millis); + void unknownBytes(int num); + const GlobalStats& getGlobalStats() const { return global_stats_; } + const std::vector& getIndividualPacketStats() const { + return individual_packet_stats_; + } + + private: + GlobalStats global_stats_; + std::vector individual_packet_stats_; +}; + +#endif \ No newline at end of file diff --git a/lib/bms/task_queue_type.cpp b/lib/bms/task_queue_type.cpp new file mode 100644 index 0000000..807d62c --- /dev/null +++ b/lib/bms/task_queue_type.cpp @@ -0,0 +1,40 @@ +#include "task_queue_type.h" + +#include +#include + +namespace { +struct { + bool operator()(const std::pair& a, + std::pair& b) const { + return a.first > b.first; + } +} comparator; +} // namespace + +void TaskQueueType::postOneShotTask(const TaskQueueType::Task& task, + unsigned long delayMs) { + unsigned long currentMillis = millis_(); + timedTasks_.push_back(std::make_pair(currentMillis + delayMs, task)); + std::push_heap(timedTasks_.begin(), timedTasks_.end(), comparator); +} + +void TaskQueueType::postRecurringTask(const Task& task) { + recurringTasks_.push_back(task); +} + +void TaskQueueType::process() { + auto time = millis_(); + std::vector toExecute; + while (!timedTasks_.empty() && time > timedTasks_.front().first) { + toExecute.push_back(timedTasks_.front().second); + std::pop_heap(timedTasks_.begin(), timedTasks_.end(), comparator); + timedTasks_.pop_back(); + } + for (const auto& c : toExecute) { + c(); + } + for (const auto& c : recurringTasks_) { + c(); + } +} diff --git a/lib/bms/task_queue_type.h b/lib/bms/task_queue_type.h new file mode 100644 index 0000000..37cd98f --- /dev/null +++ b/lib/bms/task_queue_type.h @@ -0,0 +1,24 @@ +#ifndef TASK_QUEUE_TYPE_H +#define TASK_QUEUE_TYPE_H + +#include +#include +#include + +class TaskQueueType { + public: + typedef std::function Task; + + TaskQueueType(const std::function& millis) + : millis_(millis){}; + void postOneShotTask(const Task& task, unsigned long delayMs); + void postRecurringTask(const Task& task); + void process(); + + private: + std::function millis_; + std::vector recurringTasks_; + std::vector> timedTasks_; +}; + +#endif // TASK_QUEUE_TYPE_H \ No newline at end of file diff --git a/lib/bms/welford.h b/lib/bms/welford.h new file mode 100644 index 0000000..f2d1ef5 --- /dev/null +++ b/lib/bms/welford.h @@ -0,0 +1,40 @@ +// From https://github.com/patrickmineault/welford/blob/master/src/Welford.h +// A basic implementation of Welford's algorithm for numerically stable +// incremental mean and variance computation. +// Usage example: +// Welford accum = Welford{}; +// for(int i = 0; i < 100; i++) { +// accum.add_value(float(i)); +// } +// accum.mean() // = 50 +// accum.var() // = 10000 / 12 +#include + +template +class Welford { + public: + void add_value(T x) { + n++; + T delta = x - m; + m += delta / T(n); + T delta2 = x - m; + ss += delta * delta2; + } + + float mean() const { return m; } + + float var() const { + // Unbiased estimate. + return ss / T(n - 1); + } + + float sd() const { return sqrt(ss / T(n - 1)); } + + private: + // Mean. + T m = 0; + // Sum-of-squares. + T ss = 0; + // N. + int n = 0; +}; \ No newline at end of file diff --git a/lib/vesc-protocol/src/buffer.cpp b/lib/vesc-protocol/src/buffer.cpp new file mode 100644 index 0000000..e3cef89 --- /dev/null +++ b/lib/vesc-protocol/src/buffer.cpp @@ -0,0 +1,173 @@ +/* + Copyright 2016 Benjamin Vedder benjamin@vedder.se + + This file is part of the VESC firmware. + + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "buffer.h" +#include +#include + +void buffer_append_int16(uint8_t* buffer, int16_t number, int32_t *index) { + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_uint16(uint8_t* buffer, uint16_t number, int32_t *index) { + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index) { + buffer[(*index)++] = number >> 24; + buffer[(*index)++] = number >> 16; + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_uint32(uint8_t* buffer, uint32_t number, int32_t *index) { + buffer[(*index)++] = number >> 24; + buffer[(*index)++] = number >> 16; + buffer[(*index)++] = number >> 8; + buffer[(*index)++] = number; +} + +void buffer_append_float16(uint8_t* buffer, float number, float scale, int32_t *index) { + buffer_append_int16(buffer, (int16_t)(number * scale), index); +} + +void buffer_append_float32(uint8_t* buffer, float number, float scale, int32_t *index) { + buffer_append_int32(buffer, (int32_t)(number * scale), index); +} + +/* + * See my question: + * http://stackoverflow.com/questions/40416682/portable-way-to-serialize-float-as-32-bit-integer + * + * Regarding the float32_auto functions: + * + * Noticed that frexp and ldexp fit the format of the IEEE float representation, so + * they should be quite fast. They are (more or less) equivalent with the following: + * + * float frexp_slow(float f, int *e) { + * if (f == 0.0) { + * *e = 0; + * return 0.0; + * } + * + * *e = ceilf(log2f(fabsf(f))); + * float res = f / powf(2.0, (float)*e); + * + * if (res >= 1.0) { + * res -= 0.5; + * *e += 1; + * } + * + * if (res <= -1.0) { + * res += 0.5; + * *e += 1; + * } + * + * return res; + * } + * + * float ldexp_slow(float f, int e) { + * return f * powf(2.0, (float)e); + * } + * + * 8388608.0 is 2^23, which scales the result to fit within 23 bits if sig_abs < 1.0. + * + * This should be a relatively fast and efficient way to serialize + * floating point numbers in a fully defined manner. + */ +void buffer_append_float32_auto(uint8_t* buffer, float number, int32_t *index) { + int e = 0; + float sig = frexpf(number, &e); + float sig_abs = fabsf(sig); + uint32_t sig_i = 0; + + if (sig_abs >= 0.5) { + sig_i = (uint32_t)((sig_abs - 0.5f) * 2.0f * 8388608.0f); + e += 126; + } + + uint32_t res = ((e & 0xFF) << 23) | (sig_i & 0x7FFFFF); + if (sig < 0) { + res |= 1U << 31; + } + + buffer_append_uint32(buffer, res, index); +} + +int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index) { + int16_t res = ((uint16_t) buffer[*index]) << 8 | + ((uint16_t) buffer[*index + 1]); + *index += 2; + return res; +} + +uint16_t buffer_get_uint16(const uint8_t *buffer, int32_t *index) { + uint16_t res = ((uint16_t) buffer[*index]) << 8 | + ((uint16_t) buffer[*index + 1]); + *index += 2; + return res; +} + +int32_t buffer_get_int32(const uint8_t *buffer, int32_t *index) { + int32_t res = ((uint32_t) buffer[*index]) << 24 | + ((uint32_t) buffer[*index + 1]) << 16 | + ((uint32_t) buffer[*index + 2]) << 8 | + ((uint32_t) buffer[*index + 3]); + *index += 4; + return res; +} + +uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index) { + uint32_t res = ((uint32_t) buffer[*index]) << 24 | + ((uint32_t) buffer[*index + 1]) << 16 | + ((uint32_t) buffer[*index + 2]) << 8 | + ((uint32_t) buffer[*index + 3]); + *index += 4; + return res; +} + +float buffer_get_float16(const uint8_t *buffer, float scale, int32_t *index) { + return (float)buffer_get_int16(buffer, index) / scale; +} + +float buffer_get_float32(const uint8_t *buffer, float scale, int32_t *index) { + return (float)buffer_get_int32(buffer, index) / scale; +} + +float buffer_get_float32_auto(const uint8_t *buffer, int32_t *index) { + uint32_t res = buffer_get_uint32(buffer, index); + + int e = (res >> 23) & 0xFF; + uint32_t sig_i = res & 0x7FFFFF; + bool neg = res & (1U << 31); + + float sig = 0.0; + if (e != 0 || sig_i != 0) { + sig = (float)sig_i / (8388608.0 * 2.0) + 0.5; + e -= 126; + } + + if (neg) { + sig = -sig; + } + + return ldexpf(sig, e); +} diff --git a/platformio.ini b/platformio.ini index 367d1f1..8d42885 100644 --- a/platformio.ini +++ b/platformio.ini @@ -65,14 +65,16 @@ build_flags= ;-D PIN_FORWARD=18 ; digital signal from Cheap Focer 2 if direction is forward ;-D PIN_BACKWARD=19 ; digital signal from Cheap Focer 2 if direction is backward ;-D PIN_BRAKE=21 ; digital signal from Cheap Focer 2 if electric motor brakes is on - -D BUZPIN=19 ; PIN for Piezo buzzer for acoustic signals (e.g. battery warning) + -D BUZPIN=4 ; PIN for Piezo buzzer for acoustic signals (e.g. battery warning) -D BATTERY_PIN=34 ; analog input for battery monitor, connected to voltage divider - -D LIGHT_BAR_PIN=4 ; DIN of WS28xx for battery indicator - -D VESC_RX_PIN=27 ; UART RX to Cheap Focer 2, connent to TX on CF2 - -D VESC_TX_PIN=25 ; UART TX to Cheap Focer 2, connent to RX on CF2 + -D LIGHT_BAR_PIN=19 ; DIN of WS28xx for battery indicator + ;-D VESC_RX_PIN=32 ; UART RX to Cheap Focer 2, connent to TX on CF2 + ;-D VESC_TX_PIN=33 ; UART TX to Cheap Focer 2, connent to RX on CF2 -D CAN_TX_PIN=_33 -D CAN_RX_PIN=_32 -D PIN_BOARD_LED=16 ; Onboard LED for avaspark-rgb + -D BMS_TX_PIN=23 ; Map UART RX to BMS TX Pin + -D BMS_ON_PIN=25 ; Map GPIO to trigger IRF540N 100v N-channel mosfet which emulates momentary button for OWIE BMS on (battery voltage on blue wire) to ground. Connect the Source pin of the MOSFET to ground. Connect the Drain pin to the BMS's wake-up wire. Connect the Gate pin to the microcontroller's TX pin. [env:native] platform = native diff --git a/src/AppConfiguration.cpp b/src/AppConfiguration.cpp index c80def8..3e2d107 100644 --- a/src/AppConfiguration.cpp +++ b/src/AppConfiguration.cpp @@ -58,9 +58,14 @@ boolean AppConfiguration::readPreferences() { config.lightbarMaxBrightness = doc["lightbarMaxBrightness"] | 100; config.lightbarTurnOffErpm = doc["lightbarTurnOffErpm"] | 1000; config.ledType = doc["ledType"] | "RGB"; + config.lightBarLedType = doc["lightBarLedType"] | "GRB"; config.ledFrequency = doc["ledFrequency"] | "KHZ800"; + config.lightBarLedFrequency = doc["lightBarLedFrequency"] | "KHZ800"; + config.isLightBarReversed = doc["isLightBarReversed"] | false; + config.isLightBarLedTypeDifferent = doc["isLightBarLedTypeDifferent"] | false; config.idleLightTimeout = doc["idleLightTimeout"] | 60000; - config.logLevel = doc["logLevel"] | Logger::WARNING; + config.mallGrab = doc["mallGrab"] | false; + config.logLevel = doc["logLevel"] | Logger::SILENT; config.mtuSize = doc["mtuSize"] | 512; config.oddevenActive = doc["oddevenActive"] | true; config.lightsSwitch = true; @@ -98,8 +103,13 @@ boolean AppConfiguration::savePreferences() { doc["numberPixelLight"] = config.numberPixelLight; doc["numberPixelBatMon"] = config.numberPixelBatMon; doc["ledType"] = config.ledType; + doc["lightBarLedType"] = config.lightBarLedType; doc["ledFrequency"] = config.ledFrequency; + doc["lightBarLedFrequency"] = config.lightBarLedFrequency; + doc["isLightBarReversed"] = config.isLightBarReversed; + doc["isLightBarLedTypeDifferent"] = config.isLightBarLedTypeDifferent; doc["idleLightTimeout"] = config.idleLightTimeout; + doc["mallGrab"] = config.mallGrab; doc["logLevel"] = config.logLevel; doc["mtuSize"] = config.mtuSize; String json = ""; diff --git a/src/AppConfiguration.h b/src/AppConfiguration.h index 49395bb..fb0ae6e 100644 --- a/src/AppConfiguration.h +++ b/src/AppConfiguration.h @@ -49,8 +49,13 @@ struct Config { VISITABLE(boolean, sendConfig); VISITABLE(boolean, saveConfig); VISITABLE(String , ledType); + VISITABLE(String , lightBarLedType); VISITABLE(String , ledFrequency); + VISITABLE(String , lightBarLedFrequency); + VISITABLE(boolean , isLightBarReversed); + VISITABLE(boolean , isLightBarLedTypeDifferent); VISITABLE(int , idleLightTimeout); + VISITABLE(boolean , mallGrab); VISITABLE(int , mtuSize); VISITABLE(boolean , oddevenActive); VISITABLE(boolean, lightsSwitch); diff --git a/src/BMSController.cpp b/src/BMSController.cpp new file mode 100644 index 0000000..9e8c8d4 --- /dev/null +++ b/src/BMSController.cpp @@ -0,0 +1,291 @@ +#include "BMSController.h" + +#ifndef BMS_TX_PIN +#define BMS_TX_PIN 3 +#endif + +#ifndef BMS_ON_PIN +#define BMS_ON_PIN 16 +#endif + +BMSController::BMSController(VescData *vescData) +{ + this->vescData = vescData; +} + +void BMSController::init(CanBus* canbus) { + canbus_=canbus; + relay = new BmsRelay([this]() { return bms.read(); }, + [](uint8_t b) { + }, + millis); +bms.begin(115200, SERIAL_8N1, BMS_TX_PIN, -1); + + pinMode(BMS_ON_PIN, OUTPUT); + + relay->addReceivedPacketCallback([this](BmsRelay *, Packet *packet) { + //do something with data. this is where we can construct a can packet for VESC BMS + unsigned long now = millis(); + if(canbus_->isInitialized() && (lastBMSData <= now && now - lastBMSData > canbus_->getInterval()) && !canbus_->proxy->processing) + { + broadcastVESCBMS(); + lastBMSData = (millis() + 500); + } + }); + relay->setUnknownDataCallback([this](uint8_t b) { + static std::vector unknownData = {0}; + if (unknownData.size() > 128) { + return; + } + //unknownData.push_back(b); + //do something with unknown data + //streamBMSPacket(&unknownData[0], unknownData.size()); + }); + //relay->setBMSSerialOverride(0xFFABCDEF); +} + +void BMSController::loop() +{ + //keep track of time and if no packet has come through in awhile try toggling bmsON(); + unsigned long now = millis(); + if(lastBMSData <= now && now - lastBMSData > interval) + { + bmsON(); + lastBMSData = now; + } + relay->loop(); +} + +void BMSController::broadcastVESCBMS() +{ + SOC = (useOverriddenSOC) ? relay->getOverriddenSOC() : relay->getBmsReportedSOC(); + + //String json=generateOwieStatusJson(); //one step at a time but we can start by grabbing json blob of OWIE status. Can print it out. + //if(json &&json.length()!=0){ + // DynamicJsonDocument doc(1024); + // deserializeJson(doc, json); + // if (doc["TOTAL_VOLTAGE"]&&strcmp(doc["TOTAL_VOLTAGE"], "0.00v") != 0) { + // printf("%s\n",json.c_str()); + // } + //} + canbus_->bmsAHWH(ampHoursActual,ampHoursActual*avgVoltage); + canbus_->bmsVTOT(relay->getTotalVoltageMillivolts() / 1000.0f,relay->isCharging() ? maxVoltage : 0.0f); + + canbus_->bmsI(relay->getCurrentMilliamps() / 1000.0f > 0.0f ? relay->getCurrentMilliamps() / 1000.0f : 0.0f); + + boolean isBalancing; + boolean isCharging=relay->isCharging(); + if (isCharging && relay->getBmsReportedSOC()>=99) + { + isBalancing=true; + } + else + { + isBalancing=false; + } + + boolean isChargeAllowed; + if(!relay->isBatteryTempOutOfRange() && !relay->isBatteryOvercharged()) + { + isChargeAllowed=true; + } + else + { + isChargeAllowed=false; + } + + boolean isChargeOk; + if(isCharging && !relay->isBatteryTempOutOfRange() && !relay->isBatteryOvercharged()) + { + isChargeOk=true; + } + { + isChargeOk=false; + } + + canbus_->bmsSOCSOHTempStat(vCellMin,vCellMax,SOC,SOH,cellTempMax,isCharging,isBalancing,isChargeAllowed,isChargeOk); + + canbus_->bmsAHWHDischargeTotal(relay->getUsedChargeMah() / 1000.0,relay->getUsedChargeMah() / 1000.0*avgVoltage); + + canbus_->bmsAHWHChargeTotal(relay->getRegeneratedChargeMah() / 1000.0,relay->getRegeneratedChargeMah() / 1000.0*avgVoltage); + + const uint16_t *cellMillivolts = relay->getCellMillivolts(); + canbus_->bmsVCell(cellMillivolts,cellSeries); + + const int8_t *thermTemps = relay->getTemperaturesCelsius(); + canbus_->bmsTemps(thermTemps,cellThermistors); + + //maybe make this update only when there's an actual change + canbus_->bmsBal(isBalancing); + + bms_op_state op_state=BMS_OP_STATE_INIT; + bms_fault_state fault_state=BMS_FAULT_CODE_NONE; + if(isCharging) op_state=BMS_OP_STATE_CHARGING; + if(isBalancing) op_state=BMS_OP_STATE_BALANCING; + //if(relay->isBatteryEmpty()) op_state=BMS_OP_STATE_BATTERY_DEAD; //not sure if we want this if it's using the default SoC. + if(relay->isBatteryOvercharged()) fault_state=BMS_FAULT_CODE_PACK_OVER_VOLTAGE; + if(relay->isBatteryTempOutOfRange()) fault_state = (isCharging) ? BMS_FAULT_CODE_CHARGE_OVER_TEMP_CELLS : BMS_FAULT_CODE_DISCHARGE_OVER_TEMP_CELLS; + + if(isBatteryCellOvercharged(cellMillivolts,cellSeries)) fault_state=BMS_FAULT_CODE_CELL_SOFT_OVER_VOLTAGE; + if(isBatteryCellUndercharged(cellMillivolts,cellSeries)) fault_state=BMS_FAULT_CODE_CELL_SOFT_UNDER_VOLTAGE; + //if(batteryCellVariance(cellMillivolts,cellSeries)>=cellMaxVarianceSoft) fault_state=BMS_FAULT_CODE_CELL_SOFT_IMBALANCE; + //if(batteryCellVariance(cellMillivolts,cellSeries)>=cellMaxVarianceHard) fault_state=BMS_FAULT_CODE_CELL_HARD_IMBALANCE; + if(isBatteryCellTempMax(thermTemps,cellThermistors)) fault_state = (isCharging) ? BMS_FAULT_CODE_CHARGE_OVER_TEMP_CELLS : BMS_FAULT_CODE_DISCHARGE_OVER_TEMP_CELLS; + if(isBatteryCellTempMin(thermTemps,cellThermistors)) fault_state = (isCharging) ? BMS_FAULT_CODE_CHARGE_UNDER_TEMP_CELLS : BMS_FAULT_CODE_DISCHARGE_UNDER_TEMP_CELLS; + const int8_t bmsTemp=thermTemps[4]; + if(isBMSTempMax(bmsTemp)) fault_state=BMS_FAULT_CODE_OVER_TEMP_BMS; + + canbus_->bmsState(op_state, fault_state); +} + +boolean BMSController::isBatteryCellOvercharged(const uint16_t* cellMillivolts, int cell_max) +{ + int cell_now = 0; + while (cell_now < cell_max) + { + if (cellMillivolts[cell_now++]/1000.0f>=vCellMax) return true; + } + return false; +} + +boolean BMSController::isBatteryCellUndercharged(const uint16_t* cellMillivolts, int cell_max) +{ + int cell_now = 0; + while (cell_now < cell_max) + { + if (cellMillivolts[cell_now++]/1000.0f<=vCellMin) return true; + } + return false; +} + +float BMSController::batteryCellVariance(const uint16_t* cellMillivolts, int numCells) +{ + // Calculate the mean + float sum = 0.0; + for (size_t i = 0; i < numCells; ++i) { + sum += cellMillivolts[i]/1000.0f; + } + float mean = sum / numCells; + + // Calculate the variance + float variance = 0.0; + for (size_t i = 0; i < numCells; ++i) { + variance += (cellMillivolts[i]/1000.0f - mean) * (cellMillivolts[i]/1000.0f - mean); + } + variance /= numCells; + + return variance; +} + +boolean BMSController::isBatteryCellTempMax(const int8_t* thermTemps, int temp_max) +{ + int temp_now = 0; + while (temp_now < temp_max) + { + if (thermTemps[temp_now++]>=cellTempMax) return true; + } + return false; +} + +boolean BMSController::isBatteryCellTempMin(const int8_t* thermTemps, int temp_max) +{ + int temp_now = 0; + while (temp_now < temp_max) + { + if (thermTemps[temp_now++]<=cellTempMin) return true; + } + return false; +} + + +boolean BMSController::isBMSTempMax(const int8_t bmsTemp) +{ + + if (bmsTemp>=bmsTempMax) return true; + return false; +} + +boolean BMSController::isBMSTempMin(const int8_t bmsTemp) +{ + + if (bmsTemp<=bmsTempMin) return true; + return false; +} + +String BMSController::uptimeString() { + const unsigned long nowSecs = millis() / 1000; + const int hrs = nowSecs / 3600; + const int mins = (nowSecs % 3600) / 60; + const int secs = nowSecs % 60; + String ret; + if (hrs) { + ret.concat(hrs); + ret.concat('h'); + } + ret.concat(mins); + ret.concat('m'); + ret.concat(secs); + ret.concat('s'); + return ret; +} + +String BMSController::getTempString() { + const int8_t *thermTemps = relay->getTemperaturesCelsius(); + String temps; + temps.reserve(256); + temps.concat(""); + for (int i = 0; i < 5; i++) { + temps.concat(""); + temps.concat(thermTemps[i]); + temps.concat(""); + } + temps.concat(""); + return temps; +} + +String BMSController::getVCellString() +{ + const uint16_t *cellMillivolts = relay->getCellMillivolts(); + String vcells; + vcells.reserve(256); + for (int i = 0; i < 3; i++) { + vcells.concat(""); + for (int j = 0; j < 5; j++) { + vcells.concat(""); + vcells.concat(cellMillivolts[i * 5 + j] / 1000.0); + vcells.concat(""); + } + vcells.concat(""); + } + return vcells; +} + + +String BMSController::generateOwieStatusJson() { + DynamicJsonDocument status(1024); + String jsonOutput; + + status["TOTAL_VOLTAGE"] = + String(relay->getTotalVoltageMillivolts() / 1000.0, 2) + "v"; + status["CURRENT_AMPS"] = + String(relay->getCurrentMilliamps() / 1000.0, 1) + " Amps"; + status["BMS_SOC"] = String(relay->getBmsReportedSOC()) + "%"; + status["OVERRIDDEN_SOC"] = String(relay->getOverriddenSOC()) + "%"; + status["USED_CHARGE_MAH"] = String(relay->getUsedChargeMah()) + " mAh"; + status["REGENERATED_CHARGE_MAH"] = + String(relay->getRegeneratedChargeMah()) + " mAh"; + status["UPTIME"] = uptimeString(); + status["CELL_VOLTAGE_TABLE"] = getVCellString(); + status["TEMPERATURE_TABLE"] = getTempString(); + + serializeJson(status, jsonOutput); + return jsonOutput; +} + +//Map GPIO TX to trigger IRF540N 100v N-channel mosfet which emulates momentary button for OWIE BMS on (battery voltage on blue wire) to ground. Connect the Source pin of the MOSFET to ground. Connect the Drain pin to the BMS's wake-up wire. Connect the Gate pin to the microcontroller's TX pin. +void BMSController::bmsON() { + //turn on BMS + digitalWrite(BMS_ON_PIN, HIGH); // Turn MOSFET ON + delay(10); + digitalWrite(BMS_ON_PIN, LOW); // Turn MOSFET OFF +} \ No newline at end of file diff --git a/src/BMSController.h b/src/BMSController.h new file mode 100644 index 0000000..968e259 --- /dev/null +++ b/src/BMSController.h @@ -0,0 +1,66 @@ +#include +#include + +#include "bms_relay.h" +#include "packet.h" +#include "settings.h" +#include "ArduinoJson.h" +#include "CanBus.h" +#include "VescData.h" +#include "VescCanConstants.h" + +//https://github.com/lolwheel/Owie/commit/8f10e0897eda7f6e3ca40afeaff909a97b9f7751 + +// UART RX is connected to the *BMS* White line for OneWheel Pint. +class BMSController { +public: + BMSController(VescData *vescData); + VescData *vescData; + void init(CanBus*); + void loop(); + BmsRelay *relay; + HardwareSerial bms = HardwareSerial(1); // Using UART1 + String uptimeString(); + String getTempString(); + String generateOwieStatusJson(); + String getVCellString(); + void broadcastVESCBMS(); +private: + CanBus* canbus_; + //Map GPIO to trigger IRF540N 100v N-channel mosfet which emulates momentary button for OWIE BMS on (battery voltage on blue wire) to ground. Connect the Source pin of the MOSFET to ground. Connect the Drain pin to the BMS's wake-up wire. Connect the Gate pin to the microcontroller's TX pin. + void bmsON(); + + boolean isBatteryCellOvercharged(const uint16_t*, int); + boolean isBatteryCellUndercharged(const uint16_t*, int); + float batteryCellVariance(const uint16_t*, int); + boolean isBatteryCellTempMax(const int8_t*, int); + boolean isBatteryCellTempMin(const int8_t*, int); + boolean isBMSTempMax(const int8_t); + boolean isBMSTempMin(const int8_t); + + unsigned long lastBMSData = 0; + int interval = 5000; + + //quart battery + float ampHoursSpec=4.25f; + float ampHoursActual=4.25f; + float SOC; + boolean useOverriddenSOC=true; + float SOH=ampHoursActual/ampHoursSpec*100.0f; + int cellSeries=15; + int cellThermistors=4; + float avgVoltage=cellSeries*3.6; + float maxVoltage=cellSeries*4.2; + float wattHours=ampHoursActual*avgVoltage; + //as far as I can tell these are specs for FM BMS. + float vCellMin=2.7; + float vCellMax=4.3; + float cellTempMax=45; + //https://www.reddit.com/r/onewheel/comments/knvrh2/battery_temperature_out_of_safety_range/ + float cellTempMin=10; + float bmsTempMax=60; + float bmsTempMin=10; + float cellMaxVarianceSoft=0.00001f; + float cellMaxVarianceHard=0.00005f; + boolean chargeOnly=true; +}; \ No newline at end of file diff --git a/src/BleCanProxy.cpp b/src/BleCanProxy.cpp index 575c247..a3a77a5 100644 --- a/src/BleCanProxy.cpp +++ b/src/BleCanProxy.cpp @@ -56,7 +56,7 @@ void BleCanProxy::proxyIn(std::string in) { } //Serial.printf("Buffer full now processing, needed %d, is %d\n", length + 5, longPackBuffer.size()); - int pos = longPacket ? 3 : 2; + int pos = longPacket && length>>8 != 0 ? 3 : 2; longPackBuffer = longPackBuffer.substr(pos, longPackBuffer.length()-pos); unsigned int end_a = 0; diff --git a/src/CanBus.cpp b/src/CanBus.cpp index 46ba86e..178a2aa 100644 --- a/src/CanBus.cpp +++ b/src/CanBus.cpp @@ -48,7 +48,7 @@ void CanBus::loop() { } if (lastBalanceData <= now && now - lastBalanceData > interval && !proxy->processing) { - if(!requestBalanceData()) { + if(!requestFloatPackageData()) { lastBalanceData = (millis() + 500); this->vescData->connected = false; } @@ -94,6 +94,246 @@ void CanBus::loop() { } } +boolean CanBus::isInitialized() +{ + return initialized; +} + +int CanBus::getInterval() +{ + return interval; +} + +boolean CanBus::bmsVTOT(float v_tot, float v_charge) { + twai_message_t tx_frame = {}; + int32_t send_index = 0; + uint8_t buffer[8]; // 8 bytes for two 32-bit floats + + // Prepare the payload + buffer_append_float32_auto(buffer, v_tot, &send_index); //v_tot + buffer_append_float32_auto(buffer, v_charge, &send_index); //v_charge + + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_V_TOT) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + + memcpy(&tx_frame.data[0], buffer, 8); + + // Send CAN frame + return candevice->sendCanFrame(&tx_frame); +} + + /* + * CAN_PACKET_BMS_SOC_SOH_TEMP_STAT + * + * b[0] - b[1]: V_CELL_MIN (mV) + * b[2] - b[3]: V_CELL_MAX (mV) + * b[4]: SoC (0 - 255) + * b[5]: SoH (0 - 255) + * b[6]: T_CELL_MAX (-128 to +127 degC) + * b[7]: State bitfield: + * [B7 B6 B5 B4 B3 B2 B1 B0 ] + * [RSV RSV RSV RSV RSV CHG_OK IS_BAL IS_CHG ] + */ +boolean CanBus::bmsSOCSOHTempStat(float vCellMin, float vCellMax, float SOC, float SOH, float cellMaxTemp, boolean isCharging, boolean isBalancing, boolean isChargeAllowed, boolean isChargeOk) { + twai_message_t tx_frame = {}; + int32_t send_index = 0; + uint8_t buffer[8]; // 8 bytes for two 32-bit floats + + // Prepare the payload + buffer_append_float16(buffer, vCellMin, 1e3, &send_index); + buffer_append_float16(buffer, vCellMax, 1e3, &send_index); + buffer[send_index++] = (uint8_t)(SOC / 100 * 255); + buffer[send_index++] = (uint8_t)(SOH / 100 * 255); + buffer[send_index++] = (int8_t)cellMaxTemp; + buffer[send_index++] = + ((isCharging ? 1 : 0) << 0) | + ((isBalancing ? 1 : 0) << 1) | + ((isChargeAllowed ? 1 : 0) << 2) | + ((isChargeOk ? 1 : 0) << 3); + + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_SOC_SOH_TEMP_STAT) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + + memcpy(&tx_frame.data[0], buffer, 8); + + // Send CAN frame + return candevice->sendCanFrame(&tx_frame); +} + +boolean CanBus::bmsAHWHDischargeTotal(float ampHours, float wattHours) { + twai_message_t tx_frame = {}; + int32_t send_index = 0; + uint8_t buffer[8]; // 8 bytes for two 32-bit floats + buffer_append_float32_auto(buffer, ampHours, &send_index); + buffer_append_float32_auto(buffer, wattHours, &send_index); + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_AH_WH_DIS_TOTAL) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + + memcpy(&tx_frame.data[0], buffer, 8); + // Send CAN frame + return candevice->sendCanFrame(&tx_frame); +} + +boolean CanBus::bmsAHWHChargeTotal(float ampHours, float wattHours) { + twai_message_t tx_frame = {}; + int32_t send_index = 0; + uint8_t buffer[8]; // 8 bytes for two 32-bit floats + buffer_append_float32_auto(buffer, ampHours, &send_index); + buffer_append_float32_auto(buffer, wattHours, &send_index); + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_AH_WH_CHG_TOTAL) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + + memcpy(&tx_frame.data[0], buffer, 8); + // Send CAN frame + return candevice->sendCanFrame(&tx_frame); +} + +boolean CanBus::bmsAHWH(float ampHours, float wattHours) { + twai_message_t tx_frame = {}; + int32_t send_index = 0; + uint8_t buffer[8]; // 8 bytes for two 32-bit floats + buffer_append_float32_auto(buffer, ampHours, &send_index); + buffer_append_float32_auto(buffer, wattHours, &send_index); + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_AH_WH) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + + memcpy(&tx_frame.data[0], buffer, 8); + // Send CAN frame + return candevice->sendCanFrame(&tx_frame); +} + +boolean CanBus::bmsI(float currentAmps) { + twai_message_t tx_frame = {}; + int32_t send_index = 0; + uint8_t buffer[8]; // 8 bytes for two 32-bit floats + buffer_append_float32_auto(buffer, currentAmps, &send_index); + buffer_append_float32_auto(buffer, currentAmps, &send_index); + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_I) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + + memcpy(&tx_frame.data[0], buffer, 8); + // Send CAN frame + return candevice->sendCanFrame(&tx_frame); +} + +boolean CanBus::bmsBal(boolean isBalancing) { + twai_message_t tx_frame = {}; + int32_t send_index = 0; + uint8_t buffer[8]; // 8 bytes for two 32-bit floats + buffer[send_index++] = 15; + + uint64_t bal_state = 0x0; + if (isBalancing) bal_state=0xFFFFFFFFFFFFFFFF; + buffer[send_index++] = (bal_state >> 48) & 0xFF; + buffer[send_index++] = (bal_state >> 40) & 0xFF; + buffer[send_index++] = (bal_state >> 32) & 0xFF; + buffer[send_index++] = (bal_state >> 24) & 0xFF; + buffer[send_index++] = (bal_state >> 16) & 0xFF; + buffer[send_index++] = (bal_state >> 8) & 0xFF; + buffer[send_index++] = (bal_state >> 0) & 0xFF; + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_BAL) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + + memcpy(&tx_frame.data[0], buffer, 8); + // Send CAN frame + return candevice->sendCanFrame(&tx_frame); +} + +boolean CanBus::bmsVCell(const uint16_t* cellMillivolts, int cell_max) { + int cell_now = 0; + while (cell_now < cell_max) { + twai_message_t tx_frame = {}; + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_V_CELL) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + uint8_t buffer[8]; + int32_t send_index = 0; + buffer[send_index++] = cell_now; + buffer[send_index++] = cell_max; + if (cell_now < cell_max) { + buffer_append_float16(buffer, cellMillivolts[cell_now++]/1000.0f, 1e3, &send_index); + } + if (cell_now < cell_max) { + buffer_append_float16(buffer, cellMillivolts[cell_now++]/1000.0f, 1e3, &send_index); + } + if (cell_now < cell_max) { + buffer_append_float16(buffer, cellMillivolts[cell_now++]/1000.0f, 1e3, &send_index); + } + memcpy(&tx_frame.data[0], buffer, 8); + // Send CAN frame + candevice->sendCanFrame(&tx_frame); + } + return true; +} + +boolean CanBus::bmsTemps(const int8_t* thermTemps, int temp_max) { + int temp_now = 0; + while (temp_now < temp_max) { + twai_message_t tx_frame = {}; + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_TEMPS) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + uint8_t buffer[8]; + int32_t send_index = 0; + buffer[send_index++] = temp_now; + buffer[send_index++] = temp_max; + if (temp_now < temp_max) { + buffer_append_float16(buffer, thermTemps[temp_now++], 1e2, &send_index); + } + if (temp_now < temp_max) { + buffer_append_float16(buffer, thermTemps[temp_now++], 1e2, &send_index); + } + if (temp_now < temp_max) { + buffer_append_float16(buffer, thermTemps[temp_now++], 1e2, &send_index); + } + memcpy(&tx_frame.data[0], buffer, 8); + // Send CAN frame + candevice->sendCanFrame(&tx_frame); + } + twai_message_t tx_frame = {}; + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_HUM) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + uint8_t buffer[8]; + int32_t send_index = 0; + buffer_append_float16(buffer, thermTemps[4], 1e2, &send_index); + buffer_append_float16(buffer, 0.0f, 1e2, &send_index); + buffer_append_float16(buffer, thermTemps[4], 1e2, &send_index); // Put IC temp here instead of making mew msg + memcpy(&tx_frame.data[0], buffer, 8); + candevice->sendCanFrame(&tx_frame); + return true; +} + +boolean CanBus::bmsState(bms_op_state op_state, bms_fault_state fault_state) { + twai_message_t tx_frame = {}; + // Configure CAN frame + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_BMS_STATE) << 8) + vesc_id; + tx_frame.data_length_code = 0x8; //sending 8 bytes + uint8_t buffer[8]; + int32_t send_index = 0; + buffer[send_index++] = op_state; + buffer[send_index++] = fault_state; + return candevice->sendCanFrame(&tx_frame); +} + boolean CanBus::requestFirmwareVersion() { Logger::notice(LOG_TAG_CANBUS, "requestFirmwareVersion"); twai_message_t tx_frame = {}; @@ -138,6 +378,21 @@ boolean CanBus::requestBalanceData() { return candevice->sendCanFrame(&tx_frame); } +boolean CanBus::requestFloatPackageData() { + Logger::notice(LOG_TAG_CANBUS, "requestFloatPackageData"); + twai_message_t tx_frame = {}; + + tx_frame.extd = 1; + tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_SHORT_BUFFER) << 8) + vesc_id; + tx_frame.data_length_code = 0x05; + tx_frame.data[0] = esp_can_id; + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x24; // COMM_CUSTOM_APP_DATA (0x24) + tx_frame.data[3] = 0x65; // FLOAT PACKAGE (0x65) + tx_frame.data[4] = 0x1; // FLOAT_COMMAND_GET_RTDATA (0x1) + return candevice->sendCanFrame(&tx_frame); +} + void CanBus::ping() { twai_message_t tx_frame = {}; tx_frame.extd = 1; @@ -184,31 +439,32 @@ void CanBus::processFrame(twai_message_t rx_frame, int frameCount) { vescData->current = readInt16Value(rx_frame, 4) / 10.0; vescData->dutyCycle = readInt16Value(rx_frame, 6); } - if (RECV_STATUS_2 == ID) { + if (RECV_STATUS_2 == ID) { frametype = "status2"; vescData->ampHours = readInt32Value(rx_frame, 0) / 10000.0; vescData->ampHoursCharged = readInt32Value(rx_frame, 4) / 10000.0; } - if (RECV_STATUS_3 == ID) { + if (RECV_STATUS_3 == ID) { frametype = "status3"; vescData->wattHours = readInt32Value(rx_frame, 0) / 10000.0; vescData->wattHoursCharged = readInt32Value(rx_frame, 4) / 10000.0; } - if (RECV_STATUS_4 == ID) { + if (RECV_STATUS_4 == ID) { frametype = "status4"; vescData->mosfetTemp = readInt16Value(rx_frame, 0) / 10.0; vescData->motorTemp = readInt16Value(rx_frame, 2) / 10.0; vescData->totalCurrentIn = readInt16Value(rx_frame, 4) / 10.0; vescData->pidPosition = readInt16Value(rx_frame, 6) / 50.0; + vescData->motorPosition = readInt16Value(rx_frame, 6) / 50.0; } - if (RECV_STATUS_5 == ID) { + if (RECV_STATUS_5 == ID) { frametype = "status5"; vescData->tachometer = readInt32Value(rx_frame, 0); vescData->inputVoltage = readInt16Value(rx_frame, 4) / 10.0; vescData->inputVoltage += AppConfiguration::getInstance()->config.batteryDrift; } - if (RECV_PROCESS_SHORT_BUFFER_PROXY == ID) { + if (RECV_PROCESS_SHORT_BUFFER_PROXY == ID) { frametype = "process short buffer for <>"; for (int i = 1; i < rx_frame.data_length_code; i++) { proxybuffer.push_back(rx_frame.data[i]); @@ -217,14 +473,14 @@ void CanBus::processFrame(twai_message_t rx_frame, int frameCount) { proxybuffer.clear(); } - if (RECV_FILL_RX_BUFFER == ID) { + if (RECV_FILL_RX_BUFFER == ID) { frametype = "fill rx buffer"; for (int i = 1; i < rx_frame.data_length_code; i++) { buffer.push_back(rx_frame.data[i]); } } - if (RECV_FILL_RX_BUFFER_PROXY == ID || RECV_FILL_RX_BUFFER_LONG_PROXY == ID) { + if (RECV_FILL_RX_BUFFER_PROXY == ID || RECV_FILL_RX_BUFFER_LONG_PROXY == ID) { boolean longBuffer = RECV_FILL_RX_BUFFER_LONG_PROXY == ID; frametype = longBuffer ? "fill rx long buffer" : "fill rx buffer"; for (int i = (longBuffer ? 2 : 1); i < rx_frame.data_length_code; i++) { @@ -266,6 +522,47 @@ void CanBus::processFrame(twai_message_t rx_frame, int frameCount) { vescData->adc1 = readInt32ValueFromBuffer(28 + offset, isProxyRequest) / 1000000.0; vescData->adc2 = readInt32ValueFromBuffer(32 + offset, isProxyRequest) / 1000000.0; lastBalanceData = millis(); + } else if (command == 0x24) { //0x24 = 36 DEC + frametype += "COMM_CUSTOM_APP_DATA"; + if(readInt8ValueFromBuffer(1,isProxyRequest) == 101) //magic number + { + if(readInt8ValueFromBuffer(2,isProxyRequest) == 1 ) //FLOAT_COMMAND_GET_RTDATA (0x1) + { + int offset = 3; + //FLOAT PACKAGE (0x65) + //FLOAT_COMMAND_GET_RTDATA (0x1) + //printFrame(rx_frame,frameCount); + //dumpVescValues(); + // Reading floats + vescData->pidOutput = readFloatValueFromBuffer(0 + offset, isProxyRequest); + vescData->pitch = readFloatValueFromBuffer(4 + offset, isProxyRequest); + vescData->roll = readFloatValueFromBuffer(8 + offset, isProxyRequest); + //vescData->loopTime = readInt32ValueFromBuffer(12 + offset, isProxyRequest); No functional equivilent + //vescData->motorCurrent = readInt32ValueFromBuffer(16 + offset, isProxyRequest) / 1000000.0; Done in COMM_GET_VALUES 0x4 + //vescData->motorPosition = readInt32ValueFromBuffer(20 + offset, isProxyRequest) / 1000000.0; Done in COMM_GET_VALUES 0x4 + // Reading state (1 byte) + vescData->balanceState = readInt8ValueFromBuffer(12 + offset, isProxyRequest); + // Reading switch_state (1 byte) + uint16_t switchState = readInt8ValueFromBuffer(13 + offset, isProxyRequest); + // Reading adc1 and adc2 (floats) + vescData->adc1 = readFloatValueFromBuffer(14 + offset, isProxyRequest); + vescData->adc2 = readFloatValueFromBuffer(18 + offset, isProxyRequest); + + switch(switchState) + { + case 0: + vescData->switchState=0; + break; + case 1: + vescData->switchState = (vescData->adc1 > vescData->adc2) ? 1 : 2; + break; + case 2: + vescData->switchState=3; + break; + } + lastBalanceData = millis(); + } + } } else if (command == 0x32) { //0x32 = 50 DEC frametype += "COMM_GET_VALUES_SELECTIVE"; int offset = 1; @@ -389,7 +686,7 @@ void CanBus::processFrame(twai_message_t rx_frame, int frameCount) { } if (isProxyRequest) { proxy->proxyOut(proxybuffer.data(), proxybuffer.size(), rx_frame.data[4], rx_frame.data[5]); - proxybuffer.clear(); + proxybuffer.clear(); } else { buffer.clear(); } @@ -479,6 +776,13 @@ void CanBus::dumpVescValues() { lastDump = millis(); } +float CanBus::readFloatValueFromBuffer(int startbyte, boolean isProxyRequest) { + int32_t index = startbyte; + const uint8_t *currBuffer = isProxyRequest ? proxybuffer.data() : buffer.data(); + + return buffer_get_float32_auto(currBuffer, &index); +} + int32_t CanBus::readInt32Value(twai_message_t rx_frame, int startbyte) { int32_t intVal = ( ((int32_t) rx_frame.data[startbyte] << 24) + diff --git a/src/CanBus.h b/src/CanBus.h index ec6f3a9..ccf8483 100644 --- a/src/CanBus.h +++ b/src/CanBus.h @@ -12,7 +12,7 @@ #include "CanDevice.h" #include "VescData.h" #include - +#include "buffer.h" #define B10000001 129 #define B11000011 195 @@ -27,6 +27,18 @@ class CanBus { void init(); void loop(); void dumpVescValues(); + boolean isInitialized(); + int getInterval(); + boolean bmsVTOT(float,float); + boolean bmsVCell(const uint16_t*,int); + boolean bmsTemps(const int8_t*,int); + boolean bmsBal(boolean); + boolean bmsI(float); + boolean bmsAHWH(float,float); + boolean bmsAHWHDischargeTotal(float,float); + boolean bmsAHWHChargeTotal(float,float); + boolean bmsSOCSOHTempStat(float,float,float,float,float,boolean,boolean,boolean,boolean); + boolean bmsState(bms_op_state op_state, bms_fault_state fault_state); private: const static int bufSize = 128; char buf[bufSize]; @@ -34,10 +46,12 @@ class CanBus { boolean requestFirmwareVersion(); boolean requestRealtimeData(); boolean requestBalanceData(); + boolean requestFloatPackageData(); void ping(); static void clearFrame(twai_message_t rx_frame); static void printFrame(twai_message_t rx_frame, int frameCount); void processFrame(twai_message_t rx_frame, int frameCount); + float readFloatValueFromBuffer(int startbyte, boolean isProxyRequest); static int32_t readInt32Value(twai_message_t rx_frame, int startbyte); static int16_t readInt16Value(twai_message_t rx_frame, int startbyte); int32_t readInt32ValueFromBuffer(int startbyte, boolean isProxyRequest); diff --git a/src/ILedController.cpp b/src/ILedController.cpp index 6e6c204..06abdd5 100644 --- a/src/ILedController.cpp +++ b/src/ILedController.cpp @@ -4,7 +4,7 @@ unsigned long idleTimer = 0; -void ILedController::loop(const int *new_forward, const int *new_backward, const int *new_idle, const int *new_brake) { +void ILedController::loop(const int *new_forward, const int *new_backward, const int *new_idle, const int *new_brake, const int *new_mall_grab) { if(!AppConfiguration::getInstance()->config.lightsSwitch) { this->changePattern(Pattern::NONE, *(new_forward) == HIGH, false); @@ -15,8 +15,6 @@ void ILedController::loop(const int *new_forward, const int *new_backward, const this->changePattern(Pattern::RESCUE_FLASH_LIGHT, *(new_forward) == HIGH, false); } - this->update(); - // is there a change detected if (old_forward != *(new_forward) || old_backward != *(new_backward)) { if (Logger::getLogLevel() == Logger::VERBOSE) { @@ -32,14 +30,22 @@ void ILedController::loop(const int *new_forward, const int *new_backward, const } //idle state??? - if (old_idle != *(new_idle)) { - if (*(new_idle) == HIGH) { + if (old_idle != *(new_idle) || old_mall_grab != *(new_mall_grab)) { + if (*(new_idle) == HIGH || old_mall_grab != *(new_mall_grab)) { if (idleTimer == 0) { idleTimer = millis(); } - this->idleSequence(); + if (AppConfiguration::getInstance()->config.mallGrab && *(new_mall_grab) == HIGH) + { + this->changePattern(Pattern::BATTERY_INDICATOR, true, false); + } + else + { + this->idleSequence(); + } } old_idle = *(new_idle); + old_mall_grab = *(new_mall_grab); } if (idleTimer != 0 && AppConfiguration::getInstance()->config.idleLightTimeout > 0 && @@ -48,6 +54,7 @@ void ILedController::loop(const int *new_forward, const int *new_backward, const this->changePattern(NONE, true, false); idleTimer = 0; } + this->update(); } diff --git a/src/ILedController.h b/src/ILedController.h index aa22d77..c8e1eea 100644 --- a/src/ILedController.h +++ b/src/ILedController.h @@ -8,7 +8,7 @@ #define LOG_TAG_LED "ILedController" // Pattern types supported: -enum Pattern { NONE, RAINBOW_CYCLE, THEATER_CHASE, COLOR_WIPE, CYLON, FADE, RESCUE_FLASH_LIGHT, PULSE, SLIDE, BATTERY_INDICATOR}; +enum Pattern { NONE, RAINBOW_CYCLE, THEATER_CHASE, COLOR_WIPE, CYLON, FADE, RESCUE_FLASH_LIGHT, PULSE, SLIDE, BATTERY_INDICATOR, TRANS_PRIDE}; // Patern directions supported: enum Direction { FORWARD, REVERSE }; @@ -21,7 +21,7 @@ class ILedController { virtual void startSequence() = 0; virtual void changePattern(Pattern pattern, boolean isForward, boolean repeatPattern ) = 0; virtual void update() = 0; - void loop(const int* new_forward, const int* new_backward, const int* idle, const int* new_brake); + void loop(const int* new_forward, const int* new_backward, const int* idle, const int* new_brake, const int* mall_grab); private: const static int bufSize = 128; @@ -29,12 +29,14 @@ class ILedController { int old_forward = LOW; int old_backward = LOW; int old_idle = LOW; + int old_mall_grab= LOW; }; class LedControllerFactory { public: static LedControllerFactory* getInstance(); static uint8_t determineLedType(); + static uint8_t determineLedType(bool lightBar); static ILedController* createLedController(VescData *vescData); private: diff --git a/src/LedControllerFactory.cpp b/src/LedControllerFactory.cpp index b73ff03..e620ece 100644 --- a/src/LedControllerFactory.cpp +++ b/src/LedControllerFactory.cpp @@ -30,8 +30,18 @@ ILedController *LedControllerFactory::createLedController(VescData *vescData) { } uint8_t LedControllerFactory::determineLedType() { + return determineLedType(false); +} + +uint8_t LedControllerFactory::determineLedType(bool lightBar) { uint8_t ledType = 0; - std::string ledTypeStr = std::string(AppConfiguration::getInstance()->config.ledType.c_str()); + std::string ledTypeStr=""; + std::string ledFreqStr=""; + if (lightBar) { + ledTypeStr = std::string(AppConfiguration::getInstance()->config.lightBarLedType.c_str()); + } else { + ledTypeStr = std::string(AppConfiguration::getInstance()->config.ledType.c_str()); + } if (ledTypeStr == "RGB") { ledType = NEO_RGB; } else if (ledTypeStr == "RBG") { @@ -49,8 +59,11 @@ uint8_t LedControllerFactory::determineLedType() { } else { ledType = NEO_RGB; } - - std::string ledFreqStr = std::string(AppConfiguration::getInstance()->config.ledType.c_str()); + if (lightBar) { + ledFreqStr = std::string(AppConfiguration::getInstance()->config.lightBarLedFrequency.c_str()); + } else { + ledFreqStr = std::string(AppConfiguration::getInstance()->config.ledFrequency.c_str()); + } if (ledFreqStr == "KHZ800") { ledType = ledType + 0x0000; } else { diff --git a/src/LightBarController.cpp b/src/LightBarController.cpp index 8939bd0..9cda2df 100644 --- a/src/LightBarController.cpp +++ b/src/LightBarController.cpp @@ -17,13 +17,26 @@ LightBarController::LightBarController() { min_voltage = (int) AppConfiguration::getInstance()->config.minBatteryVoltage * 100; max_voltage = (int) AppConfiguration::getInstance()->config.maxBatteryVoltage * 100; pixelCountOdd = pixel_count % 2 == 1; - uint8_t ledType = LedControllerFactory::determineLedType(); + uint8_t ledType; + if(AppConfiguration::getInstance()->config.isLightBarLedTypeDifferent) + { + ledType = LedControllerFactory::getInstance()->determineLedType(true); + } + else + { + ledType = LedControllerFactory::getInstance()->determineLedType(); + } voltage_range = max_voltage - min_voltage; lightPixels.updateLength(pixel_count); lightPixels.updateType(ledType); lightPixels.begin(); // This initializes the NeoPixel library. for (int j = 0; j < pixel_count; j++) { - lightPixels.setPixelColor(j, 51, 153, 255); + int actualIndex = j; + if(AppConfiguration::getInstance()->config.isLightBarReversed) + { + actualIndex = pixel_count - 1 - j; + } + lightPixels.setPixelColor(actualIndex, 51, 153, 255); } lightPixels.show(); } @@ -56,10 +69,14 @@ void LightBarController::updateLightBar(double voltage, uint16_t switchstate, do if (adcState != lastAdcState) { for (int i = 0; i < pixel_count; i++) { - lightPixels.setPixelColor(i, 0, 0, 0); + int actualIndex = i; + #ifdef REVERSE_LED_STRIP + actualIndex = pixel_count - 1 - i; + #endif + lightPixels.setPixelColor(actualIndex, 0, 0, 0); switch (adcState) { case ADC_NONE: - lightPixels.setPixelColor(i, 153, 0, 153); // full purple + lightPixels.setPixelColor(actualIndex, 153, 0, 153); // full purple break; case ADC_HALF_ADC1: if ((pixelCountOdd && i > (pixel_count / 2)) || (!pixelCountOdd && i >= (pixel_count / 2))) { @@ -68,11 +85,11 @@ void LightBarController::updateLightBar(double voltage, uint16_t switchstate, do break; case ADC_HALF_ADC2: if (i < (pixel_count / 2)) { - lightPixels.setPixelColor(i, 153, 0, 153); // half purple + lightPixels.setPixelColor(actualIndex, 153, 0, 153); // half purple } break; case ADC_FULL: - lightPixels.setPixelColor(i, 0, 0, 153); // full blue + lightPixels.setPixelColor(actualIndex, 0, 0, 153); // full blue break; } } @@ -80,6 +97,10 @@ void LightBarController::updateLightBar(double voltage, uint16_t switchstate, do } else if (adcState == lastAdcState && millis() - lastAdcStateChange >= 2000) { // update every pixel individually for (int i = 0; i < pixel_count; i++) { + int actualIndex = i; + #ifdef REVERSE_LED_STRIP + actualIndex = pixel_count - 1 - i; + #endif if (i == whole) { // the last pixel, the battery voltage somewhere in the range of this pixel // the lower the remaining value the more the pixel goes from green to red @@ -89,15 +110,15 @@ void LightBarController::updateLightBar(double voltage, uint16_t switchstate, do } if (i > whole) { // these pixels must be turned off, we already reached a lower battery voltage - lightPixels.setPixelColor(i, 0, 0, 0); + lightPixels.setPixelColor(actualIndex, 0, 0, 0); } if (i < whole) { // turn on this pixel completely green, the battery voltage is still above this value - lightPixels.setPixelColor(i, 0, AppConfiguration::getInstance()->config.lightbarMaxBrightness, 0); + lightPixels.setPixelColor(actualIndex, 0, AppConfiguration::getInstance()->config.lightbarMaxBrightness, 0); } if (value < 0) { // ohhh, we already hit the absolute minimum, set all pixel to full red. - lightPixels.setPixelColor(i, AppConfiguration::getInstance()->config.lightbarMaxBrightness, 0, 0); + lightPixels.setPixelColor(actualIndex, AppConfiguration::getInstance()->config.lightbarMaxBrightness, 0, 0); } } } diff --git a/src/VescCanConstants.h b/src/VescCanConstants.h index 201e70b..fbbbf03 100644 --- a/src/VescCanConstants.h +++ b/src/VescCanConstants.h @@ -3,53 +3,143 @@ #define BUFFER_SIZE 65535 +// CAN commands typedef enum { - CAN_PACKET_SET_DUTY = 0, - CAN_PACKET_SET_CURRENT, - CAN_PACKET_SET_CURRENT_BRAKE, - CAN_PACKET_SET_RPM, - CAN_PACKET_SET_POS, - CAN_PACKET_FILL_RX_BUFFER, - CAN_PACKET_FILL_RX_BUFFER_LONG, - CAN_PACKET_PROCESS_RX_BUFFER, - CAN_PACKET_PROCESS_SHORT_BUFFER, - CAN_PACKET_STATUS, - CAN_PACKET_SET_CURRENT_REL, - CAN_PACKET_SET_CURRENT_BRAKE_REL, - CAN_PACKET_SET_CURRENT_HANDBRAKE, - CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, - CAN_PACKET_STATUS_2, - CAN_PACKET_STATUS_3, - CAN_PACKET_STATUS_4, - CAN_PACKET_PING, - CAN_PACKET_PONG, - CAN_PACKET_DETECT_APPLY_ALL_FOC, - CAN_PACKET_DETECT_APPLY_ALL_FOC_RES, - CAN_PACKET_CONF_CURRENT_LIMITS, - CAN_PACKET_CONF_STORE_CURRENT_LIMITS, - CAN_PACKET_CONF_CURRENT_LIMITS_IN, - CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, - CAN_PACKET_CONF_FOC_ERPMS, - CAN_PACKET_CONF_STORE_FOC_ERPMS, - CAN_PACKET_STATUS_5, - CAN_PACKET_POLL_TS5700N8501_STATUS, - CAN_PACKET_CONF_BATTERY_CUT, - CAN_PACKET_CONF_STORE_BATTERY_CUT, - CAN_PACKET_SHUTDOWN, - CAN_PACKET_IO_BOARD_ADC_1_TO_4, - CAN_PACKET_IO_BOARD_ADC_5_TO_8, - CAN_PACKET_IO_BOARD_ADC_9_TO_12, - CAN_PACKET_IO_BOARD_DIGITAL_IN, - CAN_PACKET_IO_BOARD_SET_OUTPUT_DIGITAL, - CAN_PACKET_IO_BOARD_SET_OUTPUT_PWM, - CAN_PACKET_BMS_V_TOT, - CAN_PACKET_BMS_I, - CAN_PACKET_BMS_AH_WH, - CAN_PACKET_BMS_V_CELL, - CAN_PACKET_BMS_BAL, - CAN_PACKET_BMS_TEMPS, - CAN_PACKET_BMS_HUM, - CAN_PACKET_BMS_SOC_SOH_TEMP_STAT + CAN_PACKET_SET_DUTY = 0, + CAN_PACKET_SET_CURRENT, + CAN_PACKET_SET_CURRENT_BRAKE, + CAN_PACKET_SET_RPM, + CAN_PACKET_SET_POS, + CAN_PACKET_FILL_RX_BUFFER, + CAN_PACKET_FILL_RX_BUFFER_LONG, + CAN_PACKET_PROCESS_RX_BUFFER, + CAN_PACKET_PROCESS_SHORT_BUFFER, + CAN_PACKET_STATUS, + CAN_PACKET_SET_CURRENT_REL, + CAN_PACKET_SET_CURRENT_BRAKE_REL, + CAN_PACKET_SET_CURRENT_HANDBRAKE, + CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, + CAN_PACKET_STATUS_2, + CAN_PACKET_STATUS_3, + CAN_PACKET_STATUS_4, + CAN_PACKET_PING, + CAN_PACKET_PONG, + CAN_PACKET_DETECT_APPLY_ALL_FOC, + CAN_PACKET_DETECT_APPLY_ALL_FOC_RES, + CAN_PACKET_CONF_CURRENT_LIMITS, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS, + CAN_PACKET_CONF_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_FOC_ERPMS, + CAN_PACKET_CONF_STORE_FOC_ERPMS, + CAN_PACKET_STATUS_5, + CAN_PACKET_POLL_TS5700N8501_STATUS, + CAN_PACKET_CONF_BATTERY_CUT, + CAN_PACKET_CONF_STORE_BATTERY_CUT, + CAN_PACKET_SHUTDOWN, + CAN_PACKET_IO_BOARD_ADC_1_TO_4, + CAN_PACKET_IO_BOARD_ADC_5_TO_8, + CAN_PACKET_IO_BOARD_ADC_9_TO_12, + CAN_PACKET_IO_BOARD_DIGITAL_IN, + CAN_PACKET_IO_BOARD_SET_OUTPUT_DIGITAL, + CAN_PACKET_IO_BOARD_SET_OUTPUT_PWM, + CAN_PACKET_BMS_V_TOT, + CAN_PACKET_BMS_I, + CAN_PACKET_BMS_AH_WH, + CAN_PACKET_BMS_V_CELL, + CAN_PACKET_BMS_BAL, + CAN_PACKET_BMS_TEMPS, + CAN_PACKET_BMS_HUM, + CAN_PACKET_BMS_SOC_SOH_TEMP_STAT, + CAN_PACKET_PSW_STAT, + CAN_PACKET_PSW_SWITCH, + CAN_PACKET_BMS_HW_DATA_1, + CAN_PACKET_BMS_HW_DATA_2, + CAN_PACKET_BMS_HW_DATA_3, + CAN_PACKET_BMS_HW_DATA_4, + CAN_PACKET_BMS_HW_DATA_5, + CAN_PACKET_BMS_AH_WH_CHG_TOTAL, + CAN_PACKET_BMS_AH_WH_DIS_TOTAL, + CAN_PACKET_UPDATE_PID_POS_OFFSET, + CAN_PACKET_POLL_ROTOR_POS, + CAN_PACKET_NOTIFY_BOOT, + CAN_PACKET_STATUS_6, + CAN_PACKET_GNSS_TIME, + CAN_PACKET_GNSS_LAT, + CAN_PACKET_GNSS_LON, + CAN_PACKET_GNSS_ALT_SPEED_HDOP, + CAN_PACKET_BMS_STATE, + CAN_PACKET_MAKE_ENUM_32_BITS = 0xFFFFFFFF, } CAN_PACKET_ID; +typedef enum { + BMS_FAULT_CODE_NONE = 0, + BMS_FAULT_CODE_PACK_OVER_VOLTAGE, + BMS_FAULT_CODE_PACK_UNDER_VOLTAGE, + BMS_FAULT_CODE_LOAD_OVER_VOLTAGE, + BMS_FAULT_CODE_LOAD_UNDER_VOLTAGE, + BMS_FAULT_CODE_CHARGER_OVER_VOLTAGE, + BMS_FAULT_CODE_CHARGER_UNDER_VOLTAGE, + BMS_FAULT_CODE_CELL_HARD_OVER_VOLTAGE, + BMS_FAULT_CODE_CELL_HARD_UNDER_VOLTAGE, + BMS_FAULT_CODE_CELL_SOFT_OVER_VOLTAGE, + BMS_FAULT_CODE_CELL_SOFT_UNDER_VOLTAGE, + BMS_FAULT_CODE_MAX_UVP_OVP_ERRORS, + BMS_FAULT_CODE_MAX_UVT_OVT_ERRORS, + BMS_FAULT_CODE_OVER_CURRENT, + BMS_FAULT_CODE_OVER_TEMP_BMS, + BMS_FAULT_CODE_UNDER_TEMP_BMS, + BMS_FAULT_CODE_DISCHARGE_OVER_TEMP_CELLS, + BMS_FAULT_CODE_DISCHARGE_UNDER_TEMP_CELLS, + BMS_FAULT_CODE_CHARGE_OVER_TEMP_CELLS, + BMS_FAULT_CODE_CHARGE_UNDER_TEMP_CELLS, + BMS_FAULT_CODE_PRECHARGE_TIMEOUT, + BMS_FAULT_CODE_DISCHARGE_RETRY, + BMS_FAULT_CODE_CHARGE_RETRY, + BMS_FAULT_CODE_CAN_DELAYED_POWER_DOWN, + BMS_FAULT_CODE_NOT_USED_TIMEOUT, + BMS_FAULT_CODE_CHARGER_DISCONNECT, + BMS_FAULT_CODE_CHARGER_CURRENT_THRESHOLD_TIMEOUT +} bms_fault_state; + +typedef enum { + BMS_OP_STATE_INIT = 0, // 0 + BMS_OP_STATE_CHARGING, // 1 + BMS_OP_STATE_PRE_CHARGE, // 2 + BMS_OP_STATE_LOAD_ENABLED, // 3 + BMS_OP_STATE_BATTERY_DEAD, // 4 + BMS_OP_STATE_POWER_DOWN, // 5 + BMS_OP_STATE_EXTERNAL, // 6 + BMS_OP_STATE_ERROR, // 7 + BMS_OP_STATE_ERROR_PRECHARGE, // 8 + BMS_OP_STATE_BALANCING, // 9 + BMS_OP_STATE_CHARGED, // 10 + BMS_OP_STATE_FORCEON, // 11 + BMS_OP_STATE_UNKNOWN = 255 +} bms_op_state; + +typedef struct { + float v_tot; + float v_charge; + float i_in; + float i_in_ic; + float ah_cnt; + float wh_cnt; + int cell_num; + float v_cell[32]; + bool bal_state[32]; + int temp_adc_num; + float temps_adc[10]; + float temp_ic; + float temp_hum; + float hum; + float temp_max_cell; + float soc; + float soh; + int can_id; + uint32_t update_time; + bms_op_state op_state; + bms_fault_state fault_state; +} bms_values; + #endif //RESCUE_VESCCANCONSTANTS_H diff --git a/src/Ws28xxController.cpp b/src/Ws28xxController.cpp index ec0b985..bb71518 100644 --- a/src/Ws28xxController.cpp +++ b/src/Ws28xxController.cpp @@ -50,6 +50,9 @@ void Ws28xxController::update() { case RAINBOW_CYCLE: rainbowCycleUpdate(); break; + case TRANS_PRIDE: + transPrideUpdate(); + break; case THEATER_CHASE: theaterChaseUpdate(); break; @@ -160,6 +163,9 @@ void Ws28xxController::changePattern(Pattern pattern, boolean isForward, boolean case RAINBOW_CYCLE: rainbowCycle(10, isForward ? Direction::FORWARD : Direction::REVERSE); break; + case TRANS_PRIDE: + transPride(120, isForward ? Direction::FORWARD : Direction::REVERSE); + break; case THEATER_CHASE: theaterChase( Color((config.lightColorPrimaryRed * maxBrightness) >> 8, @@ -226,6 +232,33 @@ void Ws28xxController::rainbowCycleUpdate() { } } +void Ws28xxController::transPride(uint8_t timeinterval, Direction dir) { + activePattern = Pattern::TRANS_PRIDE; + interval = timeinterval; + totalSteps = numPixels(); + index = 0; + direction = dir; +} + +void Ws28xxController::transPrideUpdate() { + int pixelsPerStripe = numPixels() / 5; + for (int i = 0; i < numPixels(); i++) { + int shiftedIndex = (i + index) % (pixelsPerStripe * 5); // Use modulo here for wrap-around + int stripe = shiftedIndex / pixelsPerStripe; // Integer division to find the stripe color + uint32_t color; + switch(stripe % 5) { + case 0: color = 0x0000FF; break; // Blue + case 1: color = 0xFF69B4; break; // Pink + case 2: color = 0xFFFFFF; break; // White + case 3: color = 0xFF69B4; break; // Pink + case 4: color = 0x0000FF; break; // Blue + } + setPixelColor(i, color); + } + index = (index + 1) % (pixelsPerStripe * 5); // Increment and wrap around +} + + void Ws28xxController::flashLight(uint8_t timeinterval, Direction dir) { activePattern = Pattern::RESCUE_FLASH_LIGHT; interval = timeinterval; @@ -513,11 +546,14 @@ void Ws28xxController::idleSequence() { pattern = Pattern::RAINBOW_CYCLE; break; case 4: - pattern = Pattern::PULSE; - break; + pattern = Pattern::PULSE; + break; case 5: - pattern = Pattern::BATTERY_INDICATOR; - break; + pattern = Pattern::BATTERY_INDICATOR; + break; + case 6: + pattern = Pattern::TRANS_PRIDE; + break; default: pattern = Pattern::PULSE; } diff --git a/src/Ws28xxController.h b/src/Ws28xxController.h index d36fb3a..ff4e4c0 100644 --- a/src/Ws28xxController.h +++ b/src/Ws28xxController.h @@ -43,6 +43,8 @@ class Ws28xxController : public ILedController, Adafruit_NeoPixel { void reverse(); void rainbowCycle(uint8_t interval, Direction dir = FORWARD); void rainbowCycleUpdate(); + void transPride(uint8_t interval, Direction dir = FORWARD); + void transPrideUpdate(); void flashLight(uint8_t interval = 80, Direction dir = FORWARD); void flashLightUpdate(); void fadeLight(uint8_t interval = 80, Direction dir = FORWARD); diff --git a/src/main.cpp b/src/main.cpp index fcfa317..af33238 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -15,6 +15,10 @@ const int mainBufSize = 128; char mainBuf[mainBufSize]; +#if defined(CANBUS_ENABLED) && defined(BMS_TX_PIN) && defined(BMS_ON_PIN) + #include "BMSController.h" +#endif + unsigned long mainLoop = 0; unsigned long loopTime = 0; unsigned long maxLoopTime = 0; @@ -22,6 +26,7 @@ int new_forward = LOW; int new_backward = LOW; int new_brake = LOW; int idle = LOW; +int mall_grab = LOW; double idle_erpm = 10.0; boolean updateInProgress = false; @@ -45,6 +50,10 @@ LightBarController *lightbar = new LightBarController(); // Declare the local logger function before it is called. void localLogger(Logger::Level level, const char *module, const char *message); +#if defined(CANBUS_ENABLED) && defined(BMS_TX_PIN) && defined(BMS_ON_PIN) + BMSController *bmsController = new BMSController(&vescData); +#endif + void setup() { //Debug LED on board @@ -90,6 +99,12 @@ Serial.println("after createLED"); Serial.println("after canbis init"); +#if defined(CANBUS_ENABLED) && defined(BMS_TX_PIN) && defined(BMS_ON_PIN) + bmsController->init(canbus); +#endif + +Serial.println("after BMS init\n"); + // initializes the battery monitor batMonitor->init(); // initialize the UART bridge from VESC to BLE and the BLE support for Blynk (https://blynk.io) @@ -145,19 +160,25 @@ void loop() { new_backward = vescData.erpm < -idle_erpm ? HIGH : LOW; idle = (abs(vescData.erpm) < idle_erpm && vescData.switchState == 0) ? HIGH : LOW; new_brake = (abs(vescData.erpm) > idle_erpm && vescData.current < -4.0) ? HIGH : LOW; + mall_grab = (vescData.pitch > 70.0) ? HIGH : LOW; #else new_forward = digitalRead(PIN_FORWARD); new_backward = digitalRead(PIN_BACKWARD); new_brake = digitalRead(PIN_BRAKE); idle = new_forward == LOW && new_backward == LOW; + mall_grab = LOW; #endif #ifdef CANBUS_ENABLED canbus->loop(); #endif +#if defined(CANBUS_ENABLED) && defined(BMS_TX_PIN) && defined(BMS_ON_PIN) + bmsController->loop(); +#endif + // call the led controller loop - ledController->loop(&new_forward, &new_backward, &idle, &new_brake); + ledController->loop(&new_forward, &new_backward, &idle, &new_brake, &mall_grab); // measure and check voltage batMonitor->checkValues();