From d720fa36ad4768ed1b948a92ba5287c30093fbec Mon Sep 17 00:00:00 2001 From: Trygve Laugstøl Date: Sun, 26 Jul 2015 18:33:15 +0200 Subject: o Overhaul of the bluetooth code. - Adding support for reading FLOAT (specified in IEEE 11073-20601) values from a bluetooth device. - More shared pointers to help keep track of the object's lifecycle. Makes sure that the connections are released back to Linux, Linux is way to sensitive with crashing applications. o Adding support for reading the temperature sensors from the SoilMoisture device. --- apps/SoilMoisture.cpp | 64 +++++++++++++++++++++---- apps/SoilMoisture.h | 15 ++++-- apps/ble-inspect-device.cpp | 10 ++-- apps/ble-scan.cpp | 15 +++--- apps/sm-get-value.cpp | 21 ++++++--- ble/Bluetooth.cpp | 19 +++++--- ble/BluetoothImpl.h | 18 ++++--- ble/ByteBuffer.cpp | 111 +++++++++++++++++++++++++++++++++++++++++-- ble/LinuxBluetooth.cpp | 112 +++++++++++++++++++++++--------------------- include/ble/Bluetooth.h | 18 +++---- include/ble/ByteBuffer.h | 34 ++++++++++++++ test/ByteBufferTest.cpp | 62 ++++++++++++++++++++++++ 12 files changed, 389 insertions(+), 110 deletions(-) diff --git a/apps/SoilMoisture.cpp b/apps/SoilMoisture.cpp index 1d6888c..88f881a 100644 --- a/apps/SoilMoisture.cpp +++ b/apps/SoilMoisture.cpp @@ -1,4 +1,5 @@ #include "SoilMoisture.h" +#include namespace trygvis { namespace sensor { @@ -9,22 +10,29 @@ namespace sensor { auto trygvis_io_base_uuid = boost::uuids::uuid{0x32, 0xd0, 0x00, 0x00, 0x03, 0x5d, 0x59, 0xc5, 0x70, 0xd3, 0xbc, 0x8e, 0x4a, 0x1f, 0xd8, 0x3f}; +auto bluetooth_base_uuid = + boost::uuids::uuid{0x00, 0x00, 0x18, 0x0f, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb}; + const boost::uuids::uuid soil_moisture_service = makeUuid(trygvis_io_base_uuid, 0x00, 0x10); const boost::uuids::uuid soil_moisture_characteristic = makeUuid(trygvis_io_base_uuid, 0x00, 0x11); +const boost::uuids::uuid temperature_characteristic = makeUuid(bluetooth_base_uuid, 0x2a, 0x1e); using namespace trygvis::bluetooth; +static ByteBuffer createGetSensorCount() { return ByteBuffer::alloc(100) // .write8(static_cast(sm_cmd_code::SM_CMD_GET_SENSOR_COUNT)); } +static ByteBuffer createGetValue(uint8_t sensor) { return ByteBuffer::alloc(100) // .write8(static_cast(sm_cmd_code::SM_CMD_GET_VALUE)) // .write16le(sensor); } +static ByteBuffer createSetWarningValue(uint8_t sensor, uint16_t warning_value) { return ByteBuffer::alloc(100) .write8(static_cast(sm_cmd_code::SM_CMD_SET_WARNING_VALUE)) @@ -32,6 +40,7 @@ ByteBuffer createSetWarningValue(uint8_t sensor, uint16_t warning_value) { .write16le(warning_value); } +static ByteBuffer createSetSensorName(uint8_t sensor, string name) { return ByteBuffer::alloc(100) .write8(static_cast(sm_cmd_code::SM_CMD_SET_SENSOR_NAME)) @@ -39,10 +48,12 @@ ByteBuffer createSetSensorName(uint8_t sensor, string name) { .write(reinterpret_cast(name.c_str()), name.length()); } +static ByteBuffer createGetSensorName(uint8_t sensor) { return ByteBuffer::alloc(100).write8(static_cast(sm_cmd_code::SM_CMD_GET_SENSOR_NAME)).write8(sensor); } +static ByteBuffer createSetUpdateInterval(uint8_t sensor, uint8_t interval_in_seconds) { return ByteBuffer::alloc(100) .write8(static_cast(sm_cmd_code::SM_CMD_SET_UPDATE_INTERVAL)) @@ -65,13 +76,18 @@ SoilMoisture SoilMoisture::create(shared_ptr gatt) { throw runtime_error("The device is missing the soil moisture characteristic"); } - return SoilMoisture(gatt, *service, *c); + auto temperature = service->findCharacteristic(temperature_characteristic); + + return SoilMoisture(gatt, *service, c.get(), temperature); } -SoilMoisture::SoilMoisture(shared_ptr gatt, BluetoothGattService &s, BluetoothGattCharacteristic &c) - : gatt(std::move(gatt)), s(s), c(c) {} +SoilMoisture::SoilMoisture(const shared_ptr &gatt, BluetoothGattService &s, + const BluetoothGattCharacteristic &soilMoistureCharacteristic, + const o temperatureCharacteristic) + : gatt(gatt), s(s), soilMoistureCharacteristic(soilMoistureCharacteristic), + temperatureCharacteristic(temperatureCharacteristic) {} -ByteBuffer SoilMoisture::writeAndRead(ByteBuffer &requestBytes) { +ByteBuffer SoilMoisture::writeAndRead(const BluetoothGattCharacteristic &c, ByteBuffer &requestBytes) { requestBytes.setCursor(0); uint8_t expectedCode = requestBytes.get8(0); @@ -81,7 +97,7 @@ ByteBuffer SoilMoisture::writeAndRead(ByteBuffer &requestBytes) { auto responseBytes = gatt->readValue(c); if (responseBytes.getSize() < 1) { - throw runtime_error("Unexpected number of bytes read: " + to_string(requestBytes.getSize())); + throw runtime_error("Unexpected number of bytes read: " + to_string(responseBytes.getSize())); } uint8_t actualCode = responseBytes.read8(); @@ -95,13 +111,13 @@ ByteBuffer SoilMoisture::writeAndRead(ByteBuffer &requestBytes) { uint8_t SoilMoisture::getSensorCount() { auto buffer = createGetSensorCount(); - return writeAndRead(buffer).read8(); + return writeAndRead(soilMoistureCharacteristic, buffer).read8(); } uint16_t SoilMoisture::getValue(uint8_t sensor) { auto req = createGetValue(sensor); - auto buffer = writeAndRead(req); + auto buffer = writeAndRead(soilMoistureCharacteristic, req); buffer.read8(); // sensor index return buffer.read16le(); } @@ -109,12 +125,44 @@ uint16_t SoilMoisture::getValue(uint8_t sensor) { string SoilMoisture::getName(uint8_t sensor) { auto req = createGetSensorName(sensor); - auto buffer = writeAndRead(req); + auto buffer = writeAndRead(soilMoistureCharacteristic, req); size_t bytesLeft = buffer.getBytesLeft(); uint8_t bytes[bytesLeft]; buffer.copy(bytes, bytesLeft); return string((char *) bytes, (unsigned long) bytesLeft); } + +bool SoilMoisture::hasTemperatureSensor() { + return temperatureCharacteristic.is_initialized(); +} + +/** + * https://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.intermediate_temperature.xml + */ +o SoilMoisture::readTemperature() { + if (!temperatureCharacteristic) { + return o(); + } + + auto &c = temperatureCharacteristic.get(); + + auto responseBytes = gatt->readValue(c); + + if (responseBytes.getSize() < 2) { + throw runtime_error("Unexpected number of bytes read: " + to_string(responseBytes.getSize())); + } + + bitset<8> b0 = responseBytes.read8(); + + bool is_fahrenheit = b0.test(0); + bool has_timestamp = b0.test(1); + bool has_type = b0.test(2); + + double temperature = responseBytes.readFLOAT(); + + return o(temperature); +} + } } diff --git a/apps/SoilMoisture.h b/apps/SoilMoisture.h index b5a8749..99b698d 100644 --- a/apps/SoilMoisture.h +++ b/apps/SoilMoisture.h @@ -7,6 +7,8 @@ namespace trygvis { namespace sensor { using namespace trygvis::bluetooth; +template +using o = boost::optional; enum class sm_cmd_code : uint8_t { SM_CMD_GET_SENSOR_COUNT = 1, @@ -32,14 +34,21 @@ public: string getName(uint8_t sensor); + bool hasTemperatureSensor(); + + o readTemperature(); + private: - SoilMoisture(shared_ptr gatt, BluetoothGattService &s, BluetoothGattCharacteristic &c); + SoilMoisture(const shared_ptr &gatt, BluetoothGattService &s, + const BluetoothGattCharacteristic &soilMoistureCharacteristic, + const o temperatureCharacteristic); - ByteBuffer writeAndRead(ByteBuffer &requestBytes); + ByteBuffer writeAndRead(const BluetoothGattCharacteristic &c, ByteBuffer &requestBytes); shared_ptr gatt; BluetoothGattService &s; - BluetoothGattCharacteristic &c; + const BluetoothGattCharacteristic &soilMoistureCharacteristic; + const o temperatureCharacteristic; }; } } diff --git a/apps/ble-inspect-device.cpp b/apps/ble-inspect-device.cpp index 6330512..2140e03 100644 --- a/apps/ble-inspect-device.cpp +++ b/apps/ble-inspect-device.cpp @@ -22,10 +22,10 @@ public: options("device", po::value()->required(), "The MAC of the device to inspect"); } - void scan_callback(BluetoothDevice &device) { - cout << "Inspecting device: " << device.getMac().str() << endl; + void scan_callback(const shared_ptr &device) { + cout << "Inspecting device: " << device->getMac().str() << endl; - auto gatt = device.connectGatt(); + auto gatt = device->connectGatt(); gatt->discoverServices(); @@ -53,9 +53,9 @@ public: try { Mac mac = Mac::parseMac(mac_str); - shared_ptr adapter = getAdapter(0); + auto adapter = bluetoothSystem.getAdapter("0"); - BluetoothDevice &device = adapter->getDevice(mac); + auto device = adapter->getDevice(mac); scan_callback(device); diff --git a/apps/ble-scan.cpp b/apps/ble-scan.cpp index 839a9fa..158af14 100644 --- a/apps/ble-scan.cpp +++ b/apps/ble-scan.cpp @@ -26,8 +26,11 @@ public: ~ble_scan() = default; + string adapter_name; + void add_options(po::options_description_easy_init &options) override { - options("adapter", po::value()->default_value(0), "Which adapter to use."); + auto adapter_value = po::value<>(&adapter_name)->default_value("0"); + options("adapter", adapter_value, "Which adapter to use."); } int main(app_execution &execution) override { @@ -45,16 +48,14 @@ public: sigaction(SIGINT, &sigIntHandler, NULL); try { - auto adapter_index = execution.vm["adapter"].as(); - - adapter = getAdapter(adapter_index); + adapter = bluetoothSystem.getAdapter(adapter_name); set seen_devices; - cout << "Scanning with adapter #" << adapter_index << ", mac=" << adapter->getMac().str() << endl; + cout << "Scanning with adapter " << adapter_name << ", mac=" << adapter->getMac().str() << endl; - adapter->runScan([&](BluetoothDevice &device) { - auto mac = device.getMac(); + adapter->runScan([&](auto device) { + auto mac = device->getMac(); cout << "Found: " << mac.str() << endl; diff --git a/apps/sm-get-value.cpp b/apps/sm-get-value.cpp index 92ef5f9..5d66320 100644 --- a/apps/sm-get-value.cpp +++ b/apps/sm-get-value.cpp @@ -50,7 +50,7 @@ public: } int main(app_execution &execution) override { - __attribute__((unused)) BluetoothSystem bluetoothSystem; + BluetoothSystem bluetoothSystem; auto desc = execution.desc; auto vm = execution.vm; @@ -66,18 +66,20 @@ public: Mac mac = Mac::parseMac(mac_string); - auto adapter = getAdapter(0); + auto adapter = bluetoothSystem.getAdapter("0"); - auto &device = adapter->getDevice(mac); + auto device = adapter->getDevice(mac); loop = sleepTime > 0; do { try { - LOG4CPLUS_INFO(execution.logger, "Connecting to device: " << device.getMac().str()); - auto gatt = device.connectGatt(); + LOG4CPLUS_INFO(execution.logger, "Connecting to device: " << device->getMac().str()); + auto gatt = device->connectGatt(); withConnection(format, gatt); + } catch (BluetoothException &e) { + LOG4CPLUS_ERROR(execution.logger, "Bluetooth error: " << e.what()); } catch (runtime_error &e) { LOG4CPLUS_ERROR(execution.logger, "Exception: " << e.what()); } @@ -112,6 +114,11 @@ public: .set(device_key, mac) .set(timestamp_key, std::to_string(timestamp)); + auto tempO = soilMoisture.readTemperature(); + if (tempO) { + sample.set(dict.indexOf("temperature"), std::to_string(tempO.get())); + } + int i = 0; for (auto s : sensors) { auto sensor = s.first; @@ -122,7 +129,9 @@ public: auto sensor_key = dict.indexOf("sensor" + std::to_string(i)); auto sensor_name_key = dict.indexOf("sensor_name" + std::to_string(i)); - sample.set(sensor_key, std::to_string(value)).set(sensor_name_key, name); + sample + .set(sensor_key, std::to_string(value)) + .set(sensor_name_key, name); i++; } diff --git a/ble/Bluetooth.cpp b/ble/Bluetooth.cpp index 3db0bfc..13c81b3 100644 --- a/ble/Bluetooth.cpp +++ b/ble/Bluetooth.cpp @@ -201,19 +201,26 @@ BluetoothAdapter::BluetoothAdapter() { BluetoothAdapter::~BluetoothAdapter() { } +// ----------------------------------------------------------------------- +// Bluetooth System. This is not sub-classed by implementations. +// ----------------------------------------------------------------------- + BluetoothSystem::BluetoothSystem() { } BluetoothSystem::~BluetoothSystem() { - shutdown(); + adapters.clear(); } -shared_ptr getAdapter(int hciDevice) { - return getAdapterImpl(hciDevice); -} +shared_ptr BluetoothSystem::getAdapter(string name) { + auto it = adapters.find(name); + + if (it == adapters.end()) { + auto adapter = adapters[name] = getAdapterImpl(name); + return adapter; + } -void shutdown() { - shutdownImpl(); + return it->second; } uuid_t makeUuid(const uuid_t base, uint8_t a, uint8_t b) { diff --git a/ble/BluetoothImpl.h b/ble/BluetoothImpl.h index 3f4615e..4627443 100644 --- a/ble/BluetoothImpl.h +++ b/ble/BluetoothImpl.h @@ -43,11 +43,11 @@ protected: // Shared classes -class DefaultBluetoothGattCharacteristic : LogSetup, public BluetoothGattCharacteristic { +class DefaultBluetoothGattCharacteristic : protected LogSetup, public BluetoothGattCharacteristic { public: DefaultBluetoothGattCharacteristic(BluetoothGattService &service, uint16_t handle, uuid_t uuid, uint8_t properties, uint16_t valueHandle) - : LogSetup("DefaultBluetoothGattCharacteristic"), service(service), handle(handle), uuid(uuid), + : LogSetup("BluetoothGattCharacteristic"), service(service), handle(handle), uuid(uuid), properties(properties), valueHandle(valueHandle) { } @@ -117,14 +117,14 @@ public: characteristics.push_back(characteristic); } - virtual const o findCharacteristic(uuid_t uuid) const { + virtual const o findCharacteristic(uuid_t uuid) const { for (auto c: characteristics) { if (memcmp(c->getUuid().data, uuid.data, 16) == 0) { - return o(*c); + return o(*c); } } - return o(); + return o(); } protected: @@ -187,7 +187,7 @@ protected: }; template -class DefaultBluetoothDevice : public BluetoothDevice { +class DefaultBluetoothDevice : protected LogSetup, public BluetoothDevice { public: virtual Mac const &getMac() override { @@ -199,7 +199,7 @@ public: } protected: - DefaultBluetoothDevice(A &adapter, Mac &mac) : + DefaultBluetoothDevice(A &adapter, Mac &mac) : LogSetup("BluetoothDevice"), adapter(adapter), mac(mac) { } @@ -233,9 +233,7 @@ protected: Mac &mac; }; -shared_ptr getAdapterImpl(int hciDevice); - -void shutdownImpl(); +shared_ptr getAdapterImpl(string name); } }; diff --git a/ble/ByteBuffer.cpp b/ble/ByteBuffer.cpp index f5e50d8..da61427 100644 --- a/ble/ByteBuffer.cpp +++ b/ble/ByteBuffer.cpp @@ -3,6 +3,8 @@ #include #include #include +#include +#include using namespace std; @@ -48,6 +50,15 @@ ByteBuffer &ByteBuffer::write16le(uint16_t value) { return *this; } +ByteBuffer &ByteBuffer::write32le(uint32_t value) { + checkAndUpdateEnd(4); + (*ptr++) = (uint8_t) (value & 0xff); + (*ptr++) = (uint8_t) ((value >> 8) & 0xff); + (*ptr++) = (uint8_t) ((value >> 16) & 0xff); + (*ptr++) = (uint8_t) ((value >> 24) & 0xff); + return *this; +} + ByteBuffer &ByteBuffer::write(const ByteBuffer &value) { return write(value.zero, value.getSize()); } @@ -62,6 +73,64 @@ ByteBuffer &ByteBuffer::write(const uint8_t *bytes, size_t len) { return *this; } +ByteBuffer &ByteBuffer::writeFLOAT(double d) { + uint32_t result; + + if (std::isnan(d)) { + result = static_cast(FLOAT::NaN); + } else if (d > FLOAT::max) { + result = static_cast(FLOAT::positive_infinity); + } else if (d < FLOAT::min) { + result = static_cast(FLOAT::negative_infinity); + } else if (d >= -FLOAT::epsilon && d <= FLOAT::epsilon) { + result = 0; + } else { + double sgn = d > 0 ? 1 : -1; + double mantissa = fabs(d); + int exponent = 0; + + if (mantissa > FLOAT::mantissa_max) { + while (mantissa > FLOAT::mantissa_max) { + mantissa /= 10.0; + ++exponent; + +// if (exponent > FLOAT::exponent_max) { +// result = sgn ? FLOAT::positive_infinity : FLOAT::negative_infinity; +// return write32le(result); +// } + } + } else if (mantissa < 1) { + while (mantissa < 1) { + mantissa *= 10; + --exponent; + +// if (exponent < FLOAT::exponent_min) { +// result = 0; +// return write32le(result); +// } + } + } + + // scale down if number needs more precision + double smantissa = round(mantissa * FLOAT::precision); + double rmantissa = round(mantissa) * FLOAT::precision; + double mdiff = abs(smantissa - rmantissa); + while (mdiff > 0.5 && exponent > FLOAT::exponent_min && + (mantissa * 10) <= FLOAT::mantissa_max) { + mantissa *= 10; + --exponent; + smantissa = round(mantissa * FLOAT::precision); + rmantissa = round(mantissa) * FLOAT::precision; + mdiff = abs(smantissa - rmantissa); + } + + uint32_t int_mantissa = (uint32_t) round(sgn * mantissa); + result = (exponent << 24) | (int_mantissa & 0xFFFFFF); + } + + return write32le(result); +} + uint8_t ByteBuffer::get8(size_t index) const { assertCanAccessRelative(index); return ptr[index]; @@ -73,13 +142,49 @@ uint8_t ByteBuffer::read8() { } uint16_t ByteBuffer::read16le() { - assertCanAccessRelative(0); + assertCanAccessRelative(1); uint16_t value; value = *ptr++; value |= ((uint16_t) *ptr++) << 8; return value; } +uint32_t ByteBuffer::read32le() { + assertCanAccessRelative(3); + uint32_t value; + value = *ptr++; + value |= ((uint32_t) *ptr++) << 8; + value |= ((uint32_t) *ptr++) << 16; + value |= ((uint32_t) *ptr++) << 24; + return value; +} + +double ByteBuffer::readFLOAT() { + uint32_t data = read32le(); + + int32_t mantissa = data & 0xFFFFFF; + int8_t exponent = (int8_t) (data >> 24); + double output = 0; + + if (mantissa >= FLOAT::positive_infinity && + mantissa <= FLOAT::negative_infinity) { + if (mantissa == FLOAT::positive_infinity) { + output = INFINITY; + } else if (mantissa == FLOAT::negative_infinity) { + output = -INFINITY; + } else { + output = NAN; + } + } else { + if (mantissa >= 0x800000) { + mantissa = -((0xFFFFFF + 1) - mantissa); + } + output = mantissa * pow(10.0f, exponent); + } + + return output; +} + void ByteBuffer::copy(uint8_t *bytes, size_t length) const { assertCanAccessRelative(length - 1); @@ -103,7 +208,7 @@ void ByteBuffer::checkAndUpdateEnd(size_t newBytes) { uint8_t *newEnd = ptr + newBytes; if (newEnd >= end) { if (newEnd >= &zero[capacity]) { - throw ByteBufferException(string("New size is too large! cursor=") + to_string(getCursor()) + ", size=" + to_string(getSize()) + ", capacity=" + to_string(capacity) + ", new bytes=" + to_string(newBytes)); + throw ByteBufferException("New size is too large! cursor=" + to_string(getCursor()) + ", size=" + to_string(getSize()) + ", capacity=" + to_string(capacity) + ", new bytes=" + to_string(newBytes)); } end = newEnd; } @@ -115,7 +220,7 @@ void ByteBuffer::assertCanAccessRelative(size_t diff) const { void ByteBuffer::assertCanAccessIndex(uint8_t *p) const { if (p >= end || p < zero) { - throw ByteBufferException(string("Out of bounds! size=") + to_string(getSize()) + ", index=" + to_string(p - zero)); + throw ByteBufferException("Out of bounds! size=" + to_string(getSize()) + ", index=" + to_string(p - zero)); } } diff --git a/ble/LinuxBluetooth.cpp b/ble/LinuxBluetooth.cpp index f662d45..b774644 100644 --- a/ble/LinuxBluetooth.cpp +++ b/ble/LinuxBluetooth.cpp @@ -28,15 +28,19 @@ class LinuxBluetoothAdapter; class LinuxBluetoothManager; -class LinuxBluetoothAdapter : public DefaultBluetoothAdapter { +class LinuxBluetoothAdapter final : public DefaultBluetoothAdapter { public: LinuxBluetoothAdapter(int hciDeviceId, Mac &mac); ~LinuxBluetoothAdapter(); - void runScan(std::function) override; + LinuxBluetoothAdapter(const LinuxBluetoothAdapter &) = delete; - BluetoothDevice &getDevice(Mac &mac) override; + LinuxBluetoothAdapter& operator=(const LinuxBluetoothAdapter&) = delete; + + void runScan(std::function &device)>) override; + + shared_ptr getDevice(Mac &mac) override; private: void startScan() override; @@ -48,25 +52,29 @@ private: struct hci_filter hciFilter; bool scanning; - map devices; + map> devices; }; -class LinuxBluetoothDevice : public DefaultBluetoothDevice { +class LinuxBluetoothDevice final : public DefaultBluetoothDevice, + public std::enable_shared_from_this { public: LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac &mac); - ~LinuxBluetoothDevice(); + virtual ~LinuxBluetoothDevice(); + + LinuxBluetoothDevice(const LinuxBluetoothDevice &) = delete; + + LinuxBluetoothDevice& operator=(const LinuxBluetoothDevice&) = delete; shared_ptr connectGatt() override; private: - weak_ptr gatt; - int l2cap; + LinuxBluetoothGatt *gatt; }; -class LinuxBluetoothGatt : public DefaultBluetoothGatt { +class LinuxBluetoothGatt final : public DefaultBluetoothGatt { public: - LinuxBluetoothGatt(LinuxBluetoothDevice &device, int l2cap); + LinuxBluetoothGatt(LinuxBluetoothDevice &device); ~LinuxBluetoothGatt(); @@ -94,8 +102,6 @@ private: ByteBuffer writeAndRead(ByteBuffer &out, shared_ptr buffer, size_t size); int l2cap; - - bool connected; }; // Utilities @@ -121,28 +127,29 @@ LinuxBluetoothDevice::LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac & } LinuxBluetoothDevice::~LinuxBluetoothDevice() { -// if (gatt) { -// delete gatt; -// } + LOG_DEBUG("Closing device " << mac.str()); + if (gatt) { + delete gatt; + } }; shared_ptr LinuxBluetoothDevice::connectGatt() { - if (auto p = gatt.lock()) { - return p; + // Make sure that we close the old connection and create a new one when the user asks for it. + if (gatt) { + delete gatt; } - auto ref = make_shared(*this, l2cap); - gatt = ref; + gatt = new LinuxBluetoothGatt(*this); - return ref; + return shared_ptr(shared_from_this(), gatt); } // ----------------------------------------------------------------------- // Gatt // ----------------------------------------------------------------------- -LinuxBluetoothGatt::LinuxBluetoothGatt(LinuxBluetoothDevice &device, int l2cap) : - DefaultBluetoothGatt(device), l2cap(l2cap) { +LinuxBluetoothGatt::LinuxBluetoothGatt(LinuxBluetoothDevice &device) : + DefaultBluetoothGatt(device) { connect(); } @@ -151,7 +158,7 @@ LinuxBluetoothGatt::~LinuxBluetoothGatt() { } bool LinuxBluetoothGatt::isConnected() const { - return connected; + return l2cap >= 0; } void LinuxBluetoothGatt::connect() { @@ -202,10 +209,10 @@ void LinuxBluetoothGatt::connect() { } void LinuxBluetoothGatt::disconnect() { - if (!connected) { + if (l2cap < 0) { return; } - LOG_DEBUG("mac = " << device.getMac().str()); + LOG_DEBUG("Disconnecting from mac = " << device.getMac().str()); close(l2cap); } @@ -439,15 +446,14 @@ LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId, Mac &mac) : Defaul } LinuxBluetoothAdapter::~LinuxBluetoothAdapter() { - LOG_DEBUG("Stopping scan on device #" << hciDeviceId); + LOG_DEBUG("Closing adapter #" << hciDeviceId); stopScan(); close(hciSocket); - for (auto &pair : devices) { - delete pair.second; - } + // Hopefully all devices are disposed at this point. + devices.clear(); } void LinuxBluetoothAdapter::startScan() { @@ -504,19 +510,19 @@ void LinuxBluetoothAdapter::stopScan() { } } -BluetoothDevice &LinuxBluetoothAdapter::getDevice(Mac &mac) { - map::iterator it = devices.find(mac); +shared_ptr LinuxBluetoothAdapter::getDevice(Mac &mac) { + auto it = devices.find(mac); if (it == devices.end()) { - LinuxBluetoothDevice *device = new LinuxBluetoothDevice(*this, mac); + auto device = make_shared(*this, mac); devices[mac] = device; - return *device; + return device; } - return *it->second; + return it->second; } -void LinuxBluetoothAdapter::runScan(std::function callback) { +void LinuxBluetoothAdapter::runScan(std::function &device)> callback) { fd_set rfds; FD_ZERO(&rfds); @@ -559,7 +565,7 @@ void LinuxBluetoothAdapter::runScan(std::function Mac mac = parseMac(advertisingInfo->bdaddr); - BluetoothDevice &device = getDevice(mac); + auto device = getDevice(mac); callback(device); } @@ -580,30 +586,28 @@ namespace trygvis { namespace bluetooth { using namespace trygvis::bluetooth::linux; -map> adapters; - -shared_ptr getAdapterImpl(int hciDevice) { - map>::iterator it = adapters.find(hciDevice); - - if (it == adapters.end()) { - struct hci_dev_info di; - if (hci_devinfo(hciDevice, &di) < 0) { - throw BluetoothException("Could not query device info: " + errnoAsString()); - } - auto mac = parseMac(di.bdaddr); +shared_ptr getAdapterImpl(string name) { + int hciDevice; + try { + hciDevice = std::stoi(name); + } catch (const std::invalid_argument &) { + throw BluetoothException("Bad device name: " + name); + } catch (const std::out_of_range &) { + throw BluetoothException("Bad device name: " + name); + } - auto adapter = adapters[hciDevice] = make_shared(hciDevice, mac); - return adapter; + struct hci_dev_info di; + if (hci_devinfo(hciDevice, &di) < 0) { + throw BluetoothException("Could not query device info: " + errnoAsString()); } + auto mac = parseMac(di.bdaddr); - return std::static_pointer_cast(it->second); + auto adapter = make_shared(hciDevice, mac); + + return std::static_pointer_cast(adapter); } void shutdownImpl() { - adapters.clear(); -// for (auto &pair: adapters) { -// delete pair.second; -// } } } diff --git a/include/ble/Bluetooth.h b/include/ble/Bluetooth.h index d238377..8da602f 100644 --- a/include/ble/Bluetooth.h +++ b/include/ble/Bluetooth.h @@ -7,6 +7,7 @@ #include #include #include +#include #include "ByteBuffer.h" @@ -115,7 +116,7 @@ public: virtual void addCharacteristic(BluetoothGattCharacteristic *characteristic) = 0; - virtual const boost::optional findCharacteristic(boost::uuids::uuid uuid) const = 0; + virtual const boost::optional findCharacteristic(boost::uuids::uuid uuid) const = 0; }; class BluetoothGatt { @@ -165,9 +166,9 @@ public: virtual void stopScan() = 0; - virtual void runScan(std::function) = 0; + virtual void runScan(std::function &device)>) = 0; - virtual BluetoothDevice &getDevice(Mac &mac) = 0; + virtual shared_ptr getDevice(Mac &mac) = 0; protected: BluetoothAdapter(); @@ -185,6 +186,11 @@ public: BluetoothSystem(); ~BluetoothSystem(); + + shared_ptr getAdapter(string name); + +private: + map> adapters; }; enum AttPduType { @@ -227,7 +233,7 @@ public: static void makeWrite(ByteBuffer &req, uint16_t handle, const ByteBuffer &bytes); private: - static void checkType(ByteBuffer &bytes, AttPduType type); +// static void checkType(ByteBuffer &bytes, AttPduType type); static vector parse(ByteBuffer &bytes, AttPduType type); @@ -247,10 +253,6 @@ private: AttributeData(uint16_t handle, ByteBuffer value); }; -shared_ptr getAdapter(int hciDevice); - -void shutdown(); - boost::uuids::uuid makeUuid(const boost::uuids::uuid base, uint8_t a, uint8_t b); } diff --git a/include/ble/ByteBuffer.h b/include/ble/ByteBuffer.h index d977c67..1032187 100644 --- a/include/ble/ByteBuffer.h +++ b/include/ble/ByteBuffer.h @@ -7,6 +7,29 @@ #include #include #include +#include + +namespace FLOAT { +static const uint32_t positive_infinity = 0x007FFFFE; +static const uint32_t negative_infinity = 0x00800002; +static const uint32_t NaN = 0x007FFFFF; +static const uint32_t NRes = 0x00800000; +static const uint32_t FLAOT_reserved_value = 0x00800001; + +// (2^23 - 3) * 10^127 +static const double max = 8.388604999999999e+133; +static const double min = -max; + +static const double mantissa_max = 0x007FFFFD; +static const double exponent_max = 127; +static const double exponent_min = -128; + +// 10^-128 +static const double epsilon = 1e-128; + +// 10^upper(23 * log(2) / log(10)) +static const double precision = 10000000; +} class ByteBufferException : public std::runtime_error { public: @@ -53,6 +76,8 @@ public: ByteBuffer &write16le(uint16_t value); + ByteBuffer &write32le(uint32_t value); + /** * Appends the entire buffer. Make a view if you want to write a part of it. */ @@ -60,12 +85,21 @@ public: ByteBuffer &write(const uint8_t *bytes, size_t len); + ByteBuffer &writeFLOAT(double d); + uint8_t get8(size_t index) const; uint8_t read8(); uint16_t read16le(); + uint32_t read32le(); + + /** + * IEEE-11073 32-bit FLOAT + */ + double readFLOAT(); + void copy(uint8_t *bytes, size_t length) const; /** diff --git a/test/ByteBufferTest.cpp b/test/ByteBufferTest.cpp index 5aad376..8659f64 100644 --- a/test/ByteBufferTest.cpp +++ b/test/ByteBufferTest.cpp @@ -1,4 +1,5 @@ #include "ble/ByteBuffer.h" +#include #define BOOST_TEST_MODULE "ByteBuffer" @@ -89,3 +90,64 @@ BOOST_AUTO_TEST_CASE(view) { BOOST_CHECK_EQUAL(view1.read8(), 3); } + +BOOST_AUTO_TEST_CASE(ieee_11073_20601_float_1_0d) { + Bytes b(1000); + ByteBuffer buffer(b.bytes, b.capacity); + + buffer.writeFLOAT(1.0); + buffer.setCursor(0); + BOOST_CHECK_EQUAL(buffer.readFLOAT(), 1.0); +} + +BOOST_AUTO_TEST_CASE(ieee_11073_20601_float_nan) { + Bytes b(1000); + ByteBuffer buffer(b.bytes, b.capacity); + + BOOST_CHECK_EQUAL(std::isnan(stof("NaN")), true); + buffer.writeFLOAT(stof("NaN")); + buffer.setCursor(0); + BOOST_CHECK_EQUAL(std::isnan(buffer.readFLOAT()), true); +} + +void c(double input, uint32_t hx, double sigma = 0.00001) { + Bytes b(1000); + ByteBuffer buffer(b.bytes, b.capacity); + buffer.writeFLOAT(input); + buffer.setCursor(0); + double output = buffer.readFLOAT(); + + stringstream str; + auto actual_hex = ((uint32_t *) b.bytes.get())[0]; + str << "input=" << setw(20) << setprecision(10) << input << + ", output=" << setw(20) << setprecision(10) << output << + ", diff=" << setw(20) << setprecision(10) << fabs(input - output) << + ", expected=" << setw(8) << setfill('0') << hex << hx << + ", actual=" << setw(8) << setfill('0') << hex << actual_hex; + + if (std::isnan(input) || std::isnan(output)) { + BOOST_CHECK_EQUAL(std::isnan(input), std::isnan(output)); + } else if (input >= FLOAT::max) { + BOOST_CHECK_EQUAL(INFINITY, output); + } else if (input <= FLOAT::min) { + BOOST_CHECK_EQUAL(-INFINITY, output); + } else { + BOOST_CHECK_MESSAGE(fabs(input - output) < sigma, str.str()); + } +} + +BOOST_AUTO_TEST_CASE(ieee_11073_20601_float_lots) { + c(36.4, 0xFF00016C); + c(1, 0xFF000270); + c(62.4, 0xFF000270); + c(25.8, 0xFF000102); + c(-0.2, 0xFFFFFFFE); + c(0.03, 0xFE000003); + c(15000000.0, 0x0116e360); + c(15000000.1, 0x0116e360, 0.11); + c(1500000.1, 0x0016e360, 0.11); + c(1499999.99, 0x0016e360, 0.02); + c(NAN, 0x007FFFFF); + c(1e300, 0x007FFFFE); + c(-1e300, 0x00800002); +} -- cgit v1.2.3