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. --- ble/Bluetooth.cpp | 19 ++++++--- ble/BluetoothImpl.h | 18 ++++---- ble/ByteBuffer.cpp | 111 ++++++++++++++++++++++++++++++++++++++++++++++-- ble/LinuxBluetooth.cpp | 112 +++++++++++++++++++++++++------------------------ 4 files changed, 187 insertions(+), 73 deletions(-) (limited to 'ble') 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; -// } } } -- cgit v1.2.3