diff --git a/rosserial_arduino/src/ros_lib/ArduinoBluetoothBLEHardware.h b/rosserial_arduino/src/ros_lib/ArduinoBluetoothBLEHardware.h new file mode 100644 index 000000000..db99c7d1c --- /dev/null +++ b/rosserial_arduino/src/ros_lib/ArduinoBluetoothBLEHardware.h @@ -0,0 +1,192 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Kei Okada + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of copyright holder nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +// Copied from esp32/hardware/esp32/2.0.9/libraries/BLE/examples/BLE_uart/BLE_uart.ino + +#ifndef ROS_ARDUINO_BLUETOOTH_BLE_HARDWARE_H_ +#define ROS_ARDUINO_BLUETOOTH_BLE_HARDWARE_H_ + +#if not defined(ESP32) + #error ARDUINO BLUETOOTH HARDWARE CURRENTLY SUPPORT ONLY ESP32 +#endif + +#include +#include +#include +#include + +// See the following for generating UUIDs: +// https://www.uuidgenerator.net/ + +#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID +#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" +#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" + +bool deviceConnected = false; + +class MyServerCallbacks: public BLEServerCallbacks { + void onConnect(BLEServer* pServer) { + deviceConnected = true; + }; + + void onDisconnect(BLEServer* pServer) { + deviceConnected = false; + } +}; + +#include // Need RingBuffer by Jean-Luc Locoduino + +RingBuf rx_buffer; +class MyCallbacks: public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *pCharacteristic) { + std::string rxValue = pCharacteristic->getValue(); + + // Serial.print("onWrite : "); + // Serial.println(rxValue.length()); + if (rxValue.length() > 0) { + for (int i = 0; i < rxValue.length(); i++) { + rx_buffer.push(rxValue[i]); + } + } + } +}; + +class ArduinoHardware { +public: + ArduinoHardware() + { + } + + void init() + { + init("rosserial BLE UART Service"); + } + + void init(char *name) + { + // Create the BLE Device + BLEDevice::init(name); + + // Create the BLE Server + pServer = BLEDevice::createServer(); + pServer->setCallbacks(new MyServerCallbacks()); + + // Create the BLE Service + BLEService *pService = pServer->createService(SERVICE_UUID); + + // Create a BLE Characteristic + pTxCharacteristic = pService->createCharacteristic( + CHARACTERISTIC_UUID_TX, + BLECharacteristic::PROPERTY_NOTIFY | + BLECharacteristic::PROPERTY_READ + ); + pTxCharacteristic->addDescriptor(new BLE2902()); + + BLECharacteristic * pRxCharacteristic = pService->createCharacteristic( + CHARACTERISTIC_UUID_RX, + BLECharacteristic::PROPERTY_WRITE + ); + + pRxCharacteristic->setCallbacks(new MyCallbacks()); + + // Start the service + pService->start(); + + // Start advertising + pServer->getAdvertising()->start(); + //Serial.println("Waiting a client connection to notify..."); + } + + int read(){ + int data; + // Serial.print("read : "); + // Serial.println(rx_buffer.size()); + if (rx_buffer.lockedPop(data)) { + return data; + } + return -1; + } + + void write(uint8_t* data, int length) + { + // Serial.print("write : "); + // Serial.println(length); + // Serial.println(ESP_GATT_MAX_ATTR_LEN); + size_t max_length = ESP_GATT_MAX_ATTR_LEN-100; + if (deviceConnected) { + for(size_t i = 0; i < length; i += max_length) { + // Serial.print("i = "); + // Serial.print(i); + // Serial.print(" / "); + // Serial.println(min(length-i, max_length)); + pTxCharacteristic->setValue(data+i, (size_t)(min(length-i, max_length))); + pTxCharacteristic->notify(); + delay(10); // bluetooth stack will go into congestion, if too many packets are sent + } + delay(20); + } + loop(); + } + + void loop() { + // disconnecting + if (!deviceConnected && oldDeviceConnected) { + delay(500); // give the bluetooth stack the chance to get things ready + pServer->startAdvertising(); // restart advertising + Serial.println("start advertising"); + oldDeviceConnected = deviceConnected; + } + // connecting + if (deviceConnected && !oldDeviceConnected) { + // do stuff here on connecting + oldDeviceConnected = deviceConnected; + } + } + + unsigned long time() + { + return millis(); + } + + +protected: + int rxvalue_length = 0; + + BLEServer *pServer = NULL; + BLECharacteristic * pTxCharacteristic; + bool oldDeviceConnected = false; + uint8_t txValue = 0; +}; + +#endif