HI, im creating a DIY setup for ESP32_S3 and i need the VEMSv3 specification XML

Hi, I have been looking at real dash for a little while now, and I have decided to connect my ESP32 to realdash.

The s3 lacks can and uses BLE, so I want to configure BLE Serial using the aim 5 byte stream config,

The Yamaha YDS 2003-2015 natively uses this 5 byte stream also, however the bytes are naturally unique from the aim 5 byte.

1st Byte is Request 0x01 (stops polling after 8000 m/s) ** Then the ECU goes into stream**

2nd byte is RPM (value * 50)
3rd Byte is Velocity (8 bytes summed = KM/h) ** We need to wait until 8 bytes are received**
4th Byte is Error code
5th Byte is Coolant Temp
6th Byte is Checksum

If you can assist with a config or the VEMSv3 specification XML, I will be more than happy to create the code for the ESP to send bytes in a format that real dash expects, with all bytes adjusted in their values to the correct scaling.

Real dash ideally needs to be configured to accept the incoming serial.read stream of bytes,
Keeping it simple

Thanks

@realdashdev any chance you have the VEMSv3 specification XML please?

It was listed on your readme, but the drop box points to GitHub and i cannot find it.

Cheers

@realdashdev any chance you know what UUID are supported in real dash?

Still stuck, this is my code, i cannot get realdash, to connect at all, BLE scanner connects fine,

Also bluetooth serial connects fine, stumped on this one!

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>

// UUIDs for the BLE service and characteristics of your ELM327 device
#define SERVICE_UUID “0000fff0-0000-1000-8000-00805f9b34fb”
#define CHARACTERISTIC_UUID_RX “0000fff1-0000-1000-8000-00805f9b34fb”
#define CHARACTERISTIC_UUID_TX “0000fff2-0000-1000-8000-00805f9b34fb”

BLEServer *pServer = NULL;
BLECharacteristic *pTxCharacteristic;
bool deviceConnected = false;
bool oldDeviceConnected = false;

class ServerCallbacks : public BLEServerCallbacks {
void onConnect(BLEServer *pServer) {
deviceConnected = true;
Serial.println(“Device connected”);
};

void onDisconnect(BLEServer *pServer) {
deviceConnected = false;
Serial.println(“Device disconnected”);
}
};

class CharacteristicCallbacks : public BLECharacteristicCallbacks {
void onWrite(BLECharacteristic *pCharacteristic) {
std::string rxValue = pCharacteristic->getValue();

if (rxValue.length() > 0) {
  Serial.println("Received Value: ");
  for (int i = 0; i < rxValue.length(); i++) {
    Serial.print(rxValue[i]);
  }
  Serial.println();
}

}
};

void setup() {
Serial.begin(115200);

BLEDevice::init(“VEEPEAK”);
pServer = BLEDevice::createServer();
pServer->setCallbacks(new ServerCallbacks());
BLEService *pService = pServer->createService(SERVICE_UUID);

BLECharacteristic *pRxCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID_RX,
BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY
);
pRxCharacteristic->setCallbacks(new CharacteristicCallbacks());

pTxCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID_TX,
BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE
);
pTxCharacteristic->addDescriptor(new BLE2902());

pService->start();
pServer->getAdvertising()->start();
Serial.println(“Waiting for a client connection to notify…”);
}

void loop() {
// Handle connection status changes
if (deviceConnected != oldDeviceConnected) {
if (!deviceConnected) {
pServer->startAdvertising();
}
oldDeviceConnected = deviceConnected;
}

// Read data from Serial and send over BLE
if (Serial.available()) {
String data = Serial.readStringUntil(‘\n’); // Read the incoming data until newline
if (deviceConnected) {
pTxCharacteristic->setValue(data.c_str()); // Set the value to the Tx Characteristic
pTxCharacteristic->notify(); // Notify the connected device
Serial.print("Transmitted Value: ");
Serial.println(data); // Print the transmitted data
}
}
}

What are the service and TX and RX characteristic UUIDs of your BLE device?

@realdashdev

CHARACTERISTIC_UUID_RX “0000fff1-0000-1000-8000-00805f9b34fb”
service with uuid FFF0 has characteristic with uuid FFF1

CHARACTERISTIC_UUID_TX “0000fff2-0000-1000-8000-00805f9b34fb”
service with uuid FFF0 has characteristic with uuid FFF2

Veekpeak and others like carista should be using the same UUIDS

I have Carista adapter here, and it seems to work fine with Android RealDash.

thanks for the swift reply, are the UUID’s the same?

Does not appear to be so.

Send me a debug log:

How to send a debug log - General / Frequently Asked Questions - RealDash Forum

Sure stand by, i will

Standby I forgot to add in both RX and TX

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>

// UUIDs for the BLE service and characteristic
#define SERVICE_UUID “6E400001-B5A3-F393-E0A9-E50E24DCCA9E”
#define CHARACTERISTIC_UUID_RX “6E400002-B5A3-F393-E0A9-E50E24DCCA9E”
#define CHARACTERISTIC_UUID_TX “6E400003-B5A3-F393-E0A9-E50E24DCCA9E”

BLEServer *pServer = NULL;
BLECharacteristic *pTxCharacteristic;
bool deviceConnected = false;
bool oldDeviceConnected = false;

class ServerCallbacks : public BLEServerCallbacks {
void onConnect(BLEServer *pServer) {
deviceConnected = true;
Serial.println(“Device connected”);
};

void onDisconnect(BLEServer *pServer) {
deviceConnected = false;
Serial.println(“Device disconnected”);
}
};

class CharacteristicCallbacks : public BLECharacteristicCallbacks {
void onWrite(BLECharacteristic *pCharacteristic) {
std::string rxValue = pCharacteristic->getValue();

if (rxValue.length() > 0) {
  Serial.println("Received Value: ");
  for (int i = 0; i < rxValue.length(); i++) {
    Serial.print(rxValue[i]);
  }
  Serial.println();
}

}
};

void setup() {
Serial.begin(115200);

BLEDevice::init(“UARTService”);
pServer = BLEDevice::createServer();
pServer->setCallbacks(new ServerCallbacks());
BLEService *pService = pServer->createService(SERVICE_UUID);

BLECharacteristic *pRxCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID_RX,
BLECharacteristic::PROPERTY_WRITE);
pRxCharacteristic->setCallbacks(new CharacteristicCallbacks());

pTxCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID_TX,
BLECharacteristic::PROPERTY_NOTIFY);
pTxCharacteristic->addDescriptor(new BLE2902());

pService->start();
pServer->getAdvertising()->start();
Serial.println(“Waiting for a client connection to notify…”);
}

void loop() {
// Handle connection status changes
if (deviceConnected != oldDeviceConnected) {
if (!deviceConnected) {
pServer->startAdvertising();
}
oldDeviceConnected = deviceConnected;
}

// Read data from Serial and send over BLE
if (Serial.available()) {
String data = Serial.readStringUntil(‘\n’); // Read the incoming data until newline
if (deviceConnected) {
pTxCharacteristic->setValue(data.c_str()); // Set the value to the Tx Characteristic
pTxCharacteristic->notify(); // Notify the connected device
Serial.print("Transmitted Value: ");
Serial.println(data); // Print the transmitted data
}
}
}

I changed over to this code, as BLE serial works fine,

I have sent over another debug log ( i am using the ODB2 option with ble)
@realdashdev

Also thank you for your time into this!

@realdashdev I am using the UUID’S in my code so that real dash believes that it is talking to a BLE serial device, not trying to connect a ble device.

Just wanted to let you know, this in case of confusion.

My hardware is a Yamaha----L9637D—ESP32_S3/UART/BLE(serial)------Realdash

I will once again try the DIY racechrono UUID

Can confirm that the racechrono ble uuids work fine for racechrono.

Can you kindly let me know what RX/TX UUID’s are supported please

I will have another attempt

Thanks

@realdashdev, here is my updated code, I can confirm that I receive ELM327 commands with both racechrono and torque, as a carista elm327 ble odb,

but real dash doesn’t connect,

#include <BLE2902.h>
#include <BLECharacteristic.h>
#include <BLEDevice.h>

namespace elm327 {

class device final : public BLEServerCallbacks {
public:
~device() noexcept override = default;
static device& get() noexcept;
bool connected() const noexcept { return _server->getConnectedCount() > 0; }
bool start(BLECharacteristicCallbacks* callbacks) noexcept;
void send(uint8_t* data, size_t len) noexcept;
void onConnect(BLEServer*) override;
void onDisconnect(BLEServer*) override;
void stats() noexcept {}

private:
static constexpr uint16_t elm327_service_uuid = 0xfff0;
static constexpr uint16_t ELM327_RX_uuid = 0xfff1;
static constexpr uint16_t ELM327_TX_uuid = 0xfff2;

device() noexcept;

BLEServer* _server;
BLEService* _service;
BLECharacteristic* _ELM327_RX; 
BLECharacteristic* _ELM327_TX; 
BLE2902 _2902_desc;
bool _client_connected;
unsigned long _ble_count;

};

device& device::get() noexcept {
static device instance;
return instance;
}

device::device() noexcept : _server(nullptr), _service(nullptr), _ELM327_RX(nullptr), _ELM327_TX(nullptr), _2902_desc(), _client_connected(false), _ble_count(0UL) {
_2902_desc.setNotifications(true);
}

bool device::start(BLECharacteristicCallbacks* callbacks) noexcept {
BLEDevice::init(“Carista”);
BLEDevice::setPower(ESP_PWR_LVL_P9);

_server = BLEDevice::createServer();
_server->setCallbacks(this);

_service = _server->createService(elm327_service_uuid);

// ELM327_RX with Notify and Indicate properties
_ELM327_RX = _service->createCharacteristic(ELM327_RX_uuid, 
                        BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_INDICATE);
_ELM327_RX->addDescriptor(new BLE2902());

// ELM327_TX with Write and Write No Response properties
_ELM327_TX = _service->createCharacteristic(ELM327_TX_uuid, 
                        BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
_ELM327_TX->setCallbacks(callbacks);

_service->start();

BLEAdvertising* advertising = BLEDevice::getAdvertising();
advertising->addServiceUUID(_service->getUUID());
advertising->setScanResponse(false);
BLEDevice::startAdvertising();

return true;

}

void device::onConnect(BLEServer*) {
_client_connected = true;
Serial.println(“Device connected!”);

// Send "OK\n" upon connection
const char* welcomeMsg = "OK\n";
_ELM327_TX->setValue((uint8_t*)welcomeMsg, strlen(welcomeMsg));
_ELM327_TX->notify();

}

void device::onDisconnect(BLEServer*) {
_client_connected = false;
Serial.println(“Device disconnected!”);
BLEDevice::startAdvertising();
}

void device::send(uint8_t* data, size_t len) noexcept {
if (_client_connected) {
_ELM327_RX->setValue(data, len);
_ELM327_RX->notify();
++_ble_count;
Serial.print("Data sent: ");
for (size_t i = 0; i < len; ++i) {
Serial.print((char)data[i]);
}
Serial.println();
}
}

} // namespace elm327

class MyCallbacks : public BLECharacteristicCallbacks {
void onWrite(BLECharacteristic* characteristic) override {
std::string value = characteristic->getValue();
if (!value.empty()) {
Serial.print("Received ASCII Data: ");
Serial.println(value.c_str());
} else {
Serial.println(“No data received or data is empty”);
}
}
};

elm327::device& ELM327 = elm327::device::get();

void setup() {
Serial.begin(115200);
MyCallbacks* myCallbacks = new MyCallbacks();
if (ELM327.start(myCallbacks)) {
Serial.println(“BLE startup successful.”);
} else {
Serial.println(“ERROR: BLE startup failed!”);
esp_restart();
}
}

void loop() {
// Your main code here
}


can you kindly check my code aginst realdash to see why real dash is not working still ?

The photo is both racechrono and torque sending commands to the esp32 s3

But realdash doesn’t connect.

Im guessing its UUID related still.

Thanks

Here is my current code, I have been building for both Racechrono and Torque, Its a WIP,
@realdashdev I have just submitted a debug log file, if you could kindly have a look and let me know what’s wrong with real dash not connecting.

Then I can be sure that all apps are supported natively :smiley:

thanks

#include <BLE2902.h>
#include <BLECharacteristic.h>
#include <BLEDevice.h>

namespace elm327 {

class Device final : public BLEServerCallbacks {
public:
  ~Device() noexcept override = default;
  static Device& getInstance() noexcept;
  bool isConnected() const noexcept {
    return server->getConnectedCount() > 0;
  }
  bool start(BLECharacteristicCallbacks* callbacks) noexcept;
  void send(uint8_t* data, size_t len) noexcept;
  void onConnect(BLEServer*) override;
  void onDisconnect(BLEServer*) override;
  bool headersOn = false;  // State variable for header

private:
  static constexpr uint16_t SERVICE_UUID = 0xfff0;
  static constexpr uint16_t RX_UUID = 0xfff1;
  static constexpr uint16_t TX_UUID = 0xfff2;

  Device() noexcept;

  BLEServer* server;
  BLEService* service;
  BLECharacteristic* rxCharacteristic;
  BLECharacteristic* txCharacteristic;
  bool clientConnected;
  unsigned long bleCount;
};

Device& Device::getInstance() noexcept {
  static Device instance;
  return instance;
}

Device::Device() noexcept
  : server(nullptr), service(nullptr), rxCharacteristic(nullptr), txCharacteristic(nullptr), clientConnected(false), bleCount(0UL) {}

bool Device::start(BLECharacteristicCallbacks* callbacks) noexcept {
  BLEDevice::init("Carista");
  BLEDevice::setPower(ESP_PWR_LVL_P9);

  server = BLEDevice::createServer();
  server->setCallbacks(this);

  service = server->createService(BLEUUID((uint16_t)SERVICE_UUID));

  rxCharacteristic = service->createCharacteristic(BLEUUID((uint16_t)RX_UUID),
                                                   BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_INDICATE);
  rxCharacteristic->addDescriptor(new BLE2902());

  txCharacteristic = service->createCharacteristic(BLEUUID((uint16_t)TX_UUID),
                                                   BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
  txCharacteristic->setCallbacks(callbacks);

  service->start();

  BLEAdvertising* advertising = BLEDevice::getAdvertising();
  advertising->addServiceUUID(service->getUUID());
  advertising->setScanResponse(false);
  BLEDevice::startAdvertising();

  return true;
}

void Device::onConnect(BLEServer*) {
  clientConnected = true;
  Serial.println("Device connected");
}

void Device::onDisconnect(BLEServer*) {
  clientConnected = false;
  Serial.println("Device disconnected");
  BLEDevice::startAdvertising();
}

void Device::send(uint8_t* data, size_t len) noexcept {
  if (clientConnected) {
    // Send the response with the carriage return
    data[len] = '\r';  // Append carriage return
    rxCharacteristic->setValue(data, len + 1);
    rxCharacteristic->notify();
    bleCount++;

    Serial.print("Sent: ");
    Serial.write(data, len + 1);
    Serial.println();

    // Send the '>' character without carriage return
    uint8_t promptChar = '>';
    rxCharacteristic->setValue(&promptChar, 1);
    rxCharacteristic->notify();

    Serial.println("Sent: >");  // Debugging
  }
}

}  // namespace elm327

class MyCallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic* characteristic) override {
    uint8_t* value = characteristic->getData();
    size_t len = characteristic->getLength();

    Serial.print("Received: ");
    Serial.write(value, len);
    Serial.println();

    // Trim leading and trailing carriage returns or newline characters
    while (len > 0 && (value[0] == '\r' || value[0] == '\n')) {
      value++;
      len--;
    }
    while (len > 0 && (value[len - 1] == '\r' || value[len - 1] == '\n')) {
      len--;
    }

    // Convert value to a string for easier comparison
    std::string command((char*)value, len);

    // Handle commands and send responses
    if (strncmp((char*)value, "ATI", 3) == 0 && len == 3) {
      uint8_t response[] = "ELM327 v2.1";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: ELM327 v2.1");
    } else if (strncmp((char*)value, "ATZ", 3) == 0 && len == 3) {
      uint8_t response[] = "ELM327 v2.1";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: ATZ ELM327 v2.1");  //<----- Bug Here stops ESP booting
    } else if (strncmp((char*)value, "ATE0", 4) == 0 && len == 4) {
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
    } else if (strncmp((char*)value, "AT E0", 5) == 0 && len == 5) {
      uint8_t response[] = "AT E0 OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
    } else if (strncmp((char*)value, "ATPC", 4) == 0 && len == 4) {
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
    } else if (strncmp((char*)value, "ATM0", 4) == 0 && len == 4) {
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
    } else if (strncmp((char*)value, "ATL0", 4) == 0 && len == 4) {
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
    } else if (strncmp((char*)value, "ATST62", 6) == 0 && len == 6) {
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
    } else if (strncmp((char*)value, "ATS0", 4) == 0 && len == 4) {
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
    } else if (strncmp((char*)value, "AT@1", 4) == 0 && len == 4) {
      uint8_t response[] = "ELM327 v2.1";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: ELM327 v2.1");
    } else if (strncmp((char*)value, "ATAT1", 5) == 0 && len == 5) {
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
    } else if (strncmp((char*)value, "ATDPN", 5) == 0 && len == 5) {
      uint8_t response[] = "5";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: 5");
    } else if (strncmp((char*)value, "ATSPA5", 6) == 0 && len == 6) {
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
    } else if (strncmp((char*)value, "0100", 4) == 0 && len == 4) {
      uint8_t response[] = "41 00 0C 0D A4 05";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: 41 00 0C 0D A4 05");

      //ATH Switch Headers On/Off
    } else if (command == "ATH1") {
      elm327::Device::getInstance().headersOn = true;  // Turn headers on
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
    } else if (command == "ATH0") {
      elm327::Device::getInstance().headersOn = false;  // Turn headers off
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");

      //RaceChrono Related 
     } else if (strncmp((char*)value, "ATSP0", 5) == 0 && len == 5) {
      uint8_t response[] = "OK";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: OK");
      } else if (strncmp((char*)value, "0120", 4) == 0 && len == 4) {
      uint8_t response[] = "41 00";
      elm327::Device::getInstance().send(response, sizeof(response) - 1);
      Serial.println("Sent: 41 00");


      
    } else {
      Serial.println("Unknown command or length mismatch");
    }
  }
};

void setup() {
  Serial.begin(115200);
  MyCallbacks* myCallbacks = new MyCallbacks();
  if (elm327::Device::getInstance().start(myCallbacks)) {
    Serial.println("BLE startup successful.");
  } else {
    Serial.println("ERROR: BLE startup failed!");
    esp_restart();
  }
}

void loop() {
}

This is the error I see from the log:

Exception in BLE connect. One of RECEIVER_EXPORTED or RECEIVER_NOT_EXPORTED should be specified when a receiver isn’t being registered exclusively for system broadcasts

This is a first time I see such error. This may be related to some Android 13 restrictions. I have to do some research on that.

1 Like

Thank you for the reply, yes I am running android 14,

"As discussed at Google I/O 2023, registering receivers with intention using the RECEIVER_EXPORTED / RECEIVER_NOT_EXPORTED flag was introduced as part of Android 13 and is now a requirement for apps running on Android 14 or higher (U+).

If you do not implement this, the system will throw a security exception."

Try this

You can also add SDK Version Check

if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.TIRAMISU) {
     registerReceiver(broadcastReceiver, intentFilter, RECEIVER_EXPORTED)
}else {
     registerReceiver(broadcastReceiver, intentFilter)
}

Also, can you change the name of the thread title please to:
DIY ESP32_S3 Realdash for Yamaha Euro 3.

I will update photos with my PCB and code for everybody

1 Like

This is soo far what I have got up too, it’s not perfect, and I need to fix headers and removal of spaces with another switch, but torque works without any configuration, as the code is configured to support ODB2 INIT

MPH is working as it should!

I will update my code with the rest of the AT commands and switches,

the neat switch case makes it easy to add in other menu options,

Later on, I will add in functionality to feed bytes received from the L9637D into buffers, then apply the formulas to match the ODB2 Spec.

When the android 14 situations is resolved, I can test realdash.

#include <BLE2902.h>
#include <BLECharacteristic.h>
#include <BLEDevice.h>

namespace elm327 {

// Define UUIDs as constants
const uint16_t SERVICE_UUID = 0xFFF0;
const uint16_t RX_UUID = 0xFFF1;
const uint16_t TX_UUID = 0xFFF2;

class Device final : public BLEServerCallbacks {
public:
    ~Device() noexcept override = default;

    static Device& getInstance() noexcept {
        static Device instance;
        return instance;
    }

    bool isConnected() const noexcept {
        return server->getConnectedCount() > 0;
    }

    bool start(BLECharacteristicCallbacks* callbacks) noexcept {
        BLEDevice::init("Carista");
        BLEDevice::setPower(ESP_PWR_LVL_P9);

        server = BLEDevice::createServer();
        server->setCallbacks(this);

        service = server->createService(BLEUUID(SERVICE_UUID));

        // Create RX characteristic
        rxCharacteristic = createCharacteristic(RX_UUID,
            BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_INDICATE);

        // Create TX characteristic
        txCharacteristic = createCharacteristic(TX_UUID,
            BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
        txCharacteristic->setCallbacks(callbacks);

        service->start();

        // Set up advertising
        setupAdvertising();

        return true;
    }

    void send(const char* data) noexcept {
        if (clientConnected) {
            rxCharacteristic->setValue((uint8_t*)data, strlen(data));
            rxCharacteristic->notify();
            Serial.print("Sent: ");
            Serial.println(data);
        }
    }

    void onConnect(BLEServer*) override {
        clientConnected = true;
        Serial.println("Device connected");
    }

    void onDisconnect(BLEServer*) override {
        clientConnected = false;
        Serial.println("Device disconnected");
        BLEDevice::startAdvertising();
    }

    bool headersOn = false;  // State variable for header

private:
    Device() noexcept
        : server(nullptr), service(nullptr), rxCharacteristic(nullptr),
        txCharacteristic(nullptr), clientConnected(false) {}

    // Helper function to create characteristics
    BLECharacteristic* createCharacteristic(uint16_t uuid, uint32_t properties) {
        BLECharacteristic* characteristic = service->createCharacteristic(BLEUUID(uuid), properties);
        characteristic->addDescriptor(new BLE2902());
        return characteristic;
    }

    void setupAdvertising() {
        BLEAdvertising* advertising = BLEDevice::getAdvertising();
        advertising->addServiceUUID(service->getUUID());
        advertising->setScanResponse(false);
        BLEDevice::startAdvertising();
    }

    BLEServer* server;
    BLEService* service;
    BLECharacteristic* rxCharacteristic;
    BLECharacteristic* txCharacteristic;
    bool clientConnected;
};

class MyCallbacks : public BLECharacteristicCallbacks {
public:
    void onWrite(BLECharacteristic* characteristic) override {
        uint8_t* value = characteristic->getData();
        size_t len = characteristic->getLength();

        // Trim all trailing and leading whitespace characters
        size_t start = 0;
        while (start < len &&
            (value[start] == ' ' || value[start] == '\t' ||
                value[start] == '\n' || value[start] == '\r')) {
            start++;
        }

        while (len > start &&
            (value[len - 1] == ' ' || value[len - 1] == '\t' ||
                value[len - 1] == '\n' || value[len - 1] == '\r')) {
            len--;
        }

        Serial.print("Received: ");
        Serial.write(value + start, len - start);
        Serial.println();

        // Add debugging output to print the length of the received data
        Serial.print("Received data length: ");
        Serial.println(len - start);

        std::string command((char*)(value + start), len - start);

        // Handle commands here using a map or switch-case for better organization
        handleCommand(command);
    }

private:
void handleCommand(const std::string& command) {
    std::string response;

    if (command == "ATI") {
        response = "ELM327 v2.1";
        Device::getInstance().headersOn = false;
    } else if (command == "ATZ" || command == "AT@1") {
        response = "ELM327 v2.1";
    } else if (command == "ATE0" || command == "ATPC" || command == "ATM0" ||
               command == "ATL0" || command == "ATST62" || command == "ATS0" ||
               command == "ATSP0" || command == "ATAT1" || command == "ATAT2" ||
               command == "ATSPA5") {
        response = "OK";
    } else if (command == "ATH1" || command == "AT H1") {
        handleATHCommand(true);  // Turn headers off
        response = "OK";
    } else if (command == "ATH0" || command == "AT H0") {
        handleATHCommand(false);  // Turn headers on
        response = "OK";
    } else if (command == "ATDPN") {
        response = "0";
    } else if (command == "0100") {
        response = "41 00 05 0D A4";
    } else if (command == "0120") {
        response = "41 20 NO DATA";
    } else if (command == "010D") {
        response = "41 0D 64";
    } else if (command == "0111") {
        response = "41 00";
    } else {
        response = "Unknown command or length mismatch. Received: " + command;
    }

    // Send a carriage return to indicate readiness for the next command
    sendResponse(response + "\r"); 
      
    return;

}



    void sendResponse(const std::string& response) {
        // Create a new string by appending ">" to the response after '\r'
        std::string responseWithModifiedCR = response;
        size_t pos = responseWithModifiedCR.find_last_of('\r');
        if (pos != std::string::npos) {
            responseWithModifiedCR.insert(pos + 1, ">");
        }

        // Send the modified response
        elm327::Device::getInstance().send(responseWithModifiedCR.c_str());
    }

    void handleATHCommand(bool state) {
        elm327::Device::getInstance().headersOn = state;
        sendResponse("OK");
    }
};

}  // namespace elm327

void setup() {
    Serial.begin(115200);
    elm327::MyCallbacks* myCallbacks = new elm327::MyCallbacks();
    if (elm327::Device::getInstance().start(myCallbacks)) {
        Serial.println("BLE startup successful.");
    } else {
        Serial.println("ERROR: BLE startup failed!");
        esp_restart();
    }
}

void loop() {
    // Your main loop code here
}