Ninebot_BMS_Emulator (STILL WIP)

Ninebot_BMS_Emulator.ino

#define BAUDRATE 115200
#define PRE_MSB 0x5A
#define PRE_LSB 0xA5
#define CMD_READ 0x01
#define CMD_WRITE 0x02
#define CMD_REPLY 0x04
#define CMD_WR_REPLY 0x05
#define c_MIN_BATT_V 3500
#define c_MAX_BATT_V 4200
#define c_BATT_CAP 15300
#define e_ESC 0x20
#define e_DASH 0x21
#define e_BMS 0x22
#define e_BMS2 0x23
#define e_BLE 0x3E
#define e_IOT 0x3D
#define i_SN 0x10
#define i_VERS 0x17
#define i_FCAP 0x18
#define i_ACAP 0x19
#define i_CHGCYCLES 0x1B
#define i_CHGCNT 0x1C
#define i_MFGDATE 0x20
#define i_STATUS 0x30
#define i_MAH 0x31
#define i_PERC 0x32
#define i_CURRENT 0x33
#define i_VOLT 0x34
#define i_HEALTH 0x3B
#define i_FIRSTCELL 0x40
#define i_LASTCELL 0x49
String BATTERY_SERIAL_NUMBER = "Ninebot-Zero00";
unsigned long previousMillis = 0;
const long interval = 999;
int BMS_MemoryMap[112];
void setup() {
  // populating memory map
  BMS_MemoryMap[i_VERS] = 0x550;
  BMS_MemoryMap[i_FCAP] = c_BATT_CAP;
  BMS_MemoryMap[i_ACAP] = c_BATT_CAP;
  BMS_MemoryMap[i_CHGCYCLES] = 69;
  BMS_MemoryMap[i_CHGCNT] = 42;
  BMS_MemoryMap[i_MFGDATE] = 4269;
  BMS_MemoryMap[i_STATUS] = 0;
  BMS_MemoryMap[i_MAH] = c_BATT_CAP;
  BMS_MemoryMap[i_PERC] = 100;
  BMS_MemoryMap[i_CURRENT] = 200;
  BMS_MemoryMap[i_VOLT] = c_MAX_BATT_V;
  BMS_MemoryMap[i_HEALTH] = 98;
  populateCells();
  Serial.begin(BAUDRATE);
  pinMode(13, OUTPUT);
  Serial.println("BOOTED!");
}

void loop() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis; // save the last time code was ran
    if ((previousMillis & 1) == 0) {
      byte getVolts[] = {e_BMS, e_ESC, CMD_READ, 0x47, 0x02};
      CreateMessage(getVolts, 1);
    }
    else
    {
      byte getMotorCurrent[] = {e_BMS, e_ESC, CMD_READ, 0x53, 0x02};
      CreateMessage(getMotorCurrent, 1);
    }
  }
  byte rxBuffer[4];
  byte payload[64];
  byte pLen = 0;
  bool gotMsg = false;
  if (Serial.available() > 1) {
    byte val_msb = Serial.read();
    byte val_lsb = Serial.read();
    if ((val_msb == PRE_MSB) && (val_lsb == PRE_LSB)) { // is 5AA5
      int retries = 0;
      while (!(Serial.available() > 0) && retries < 150) {
        ++retries;
      }
      byte val = Serial.read(); // getting pLen
      while (!(Serial.available() >= val + 6) && retries < 150) {
        ++retries;
      }
      Serial.readBytes(rxBuffer, 4); // reading message content
      Serial.readBytes(payload, val); // reading message payload
      pLen = val;
      gotMsg = true;
    }
  }
  if (gotMsg) {
    byte src = rxBuffer[0];
    byte dest = rxBuffer[1];
    byte cmd = rxBuffer[2];
    byte arg = rxBuffer[3];
    if (cmd == CMD_READ) {
      if (arg == i_SN) {
        byte message[4 + 0x0e] = {dest, src, CMD_REPLY, arg};
        byte numAray[0x0e];
        BATTERY_SERIAL_NUMBER.getBytes(numAray, 0x0e);
        int len = sizeof(numAray) / sizeof(numAray[0]);
        if (len > 32) {
          len = 0;
        }
        for (int i = 0; i <= len ; i++)
        {
          message[i + 4] = numAray[i];
        }
        CreateMessage(message, 0x0e);
      }
      else {
        byte message[6] = {dest, src, CMD_REPLY, arg};
        message[4] = highByte(BMS_MemoryMap[arg]);
        message[5] = lowByte(BMS_MemoryMap[arg]);
        CreateMessage(message, 2);

      }
    }
    if (cmd == CMD_REPLY) {
      if (src == e_ESC && dest == e_BMS && arg == 0x47) {
        BMS_MemoryMap[i_VOLT] = word(payload[0], payload[1]);
        BMS_MemoryMap[i_PERC] = ((BMS_MemoryMap[i_VOLT] - c_MIN_BATT_V) * 100) / (c_MAX_BATT_V - c_MIN_BATT_V);
        BMS_MemoryMap[i_MAH] = BMS_MemoryMap[i_ACAP] * (BMS_MemoryMap[i_PERC] / 100);
        populateCells();
      }
        if (src == e_ESC && dest == e_BMS && arg == 0x53) {
        int current = word(payload[0], payload[1]);
        BMS_MemoryMap[i_CURRENT] = sqrt(3) * 0.7 * current * BMS_MemoryMap[i_VOLT];
      }
    }
  }
}

void populateCells() {
  for (int i = i_FIRSTCELL; i <= i_LASTCELL; i++) {
    BMS_MemoryMap[i] = BMS_MemoryMap[i_VOLT];
  }
}

void CreateMessage (byte data[], int pLen)
{
  byte newData[pLen + 10] = {0x00, 0x00, pLen, data[0], data[1], data[2], data[3]};
  for (int i = 0; i <= pLen; ++i)
  {
    newData[i + 7] = data[i + 4];
  }

  uint16_t chksm = CheckSum1ByteIn2ByteOut(newData, 7 + pLen);
  newData[pLen + 7] = (byte)(chksm & 0xff);
  newData[pLen + 8] = (byte)((chksm >> 8) & 0xff);
  newData[0] = PRE_MSB;
  newData[1] = PRE_LSB;
  Serial.write(newData, sizeof(newData) / sizeof(newData[0]));
}
int Combine(unsigned int x_high, unsigned int x_low)
{
  int combined;
  combined = x_high;
  combined = combined * 256;
  combined |= x_low;
  return combined;
}
uint16_t CheckSum1ByteIn2ByteOut(uint8_t* data, int len) {
  uint16_t checksum = 0;
  for (int i = 0; i < len; i++) {
    checksum += data[i];
  }
  checksum = ~checksum;
  return checksum;
}