aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/SoilMoisture.cpp64
-rw-r--r--apps/SoilMoisture.h15
-rw-r--r--apps/ble-inspect-device.cpp10
-rw-r--r--apps/ble-scan.cpp15
-rw-r--r--apps/sm-get-value.cpp21
-rw-r--r--ble/Bluetooth.cpp19
-rw-r--r--ble/BluetoothImpl.h18
-rw-r--r--ble/ByteBuffer.cpp111
-rw-r--r--ble/LinuxBluetooth.cpp112
-rw-r--r--include/ble/Bluetooth.h18
-rw-r--r--include/ble/ByteBuffer.h34
-rw-r--r--test/ByteBufferTest.cpp62
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 <bitset>
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<uint8_t>(sm_cmd_code::SM_CMD_GET_SENSOR_COUNT));
}
+static
ByteBuffer createGetValue(uint8_t sensor) {
return ByteBuffer::alloc(100) //
.write8(static_cast<uint8_t>(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<uint8_t>(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<uint8_t>(sm_cmd_code::SM_CMD_SET_SENSOR_NAME))
@@ -39,10 +48,12 @@ ByteBuffer createSetSensorName(uint8_t sensor, string name) {
.write(reinterpret_cast<const uint8_t *>(name.c_str()), name.length());
}
+static
ByteBuffer createGetSensorName(uint8_t sensor) {
return ByteBuffer::alloc(100).write8(static_cast<uint8_t>(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<uint8_t>(sm_cmd_code::SM_CMD_SET_UPDATE_INTERVAL))
@@ -65,13 +76,18 @@ SoilMoisture SoilMoisture::create(shared_ptr<BluetoothGatt> 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<BluetoothGatt> gatt, BluetoothGattService &s, BluetoothGattCharacteristic &c)
- : gatt(std::move(gatt)), s(s), c(c) {}
+SoilMoisture::SoilMoisture(const shared_ptr<BluetoothGatt> &gatt, BluetoothGattService &s,
+ const BluetoothGattCharacteristic &soilMoistureCharacteristic,
+ const o<const BluetoothGattCharacteristic &> 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<double> SoilMoisture::readTemperature() {
+ if (!temperatureCharacteristic) {
+ return o<double>();
+ }
+
+ 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<double>(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 <typename T>
+using o = boost::optional<T>;
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<double> readTemperature();
+
private:
- SoilMoisture(shared_ptr<BluetoothGatt> gatt, BluetoothGattService &s, BluetoothGattCharacteristic &c);
+ SoilMoisture(const shared_ptr<BluetoothGatt> &gatt, BluetoothGattService &s,
+ const BluetoothGattCharacteristic &soilMoistureCharacteristic,
+ const o<const BluetoothGattCharacteristic &> temperatureCharacteristic);
- ByteBuffer writeAndRead(ByteBuffer &requestBytes);
+ ByteBuffer writeAndRead(const BluetoothGattCharacteristic &c, ByteBuffer &requestBytes);
shared_ptr<BluetoothGatt> gatt;
BluetoothGattService &s;
- BluetoothGattCharacteristic &c;
+ const BluetoothGattCharacteristic &soilMoistureCharacteristic;
+ const o<const BluetoothGattCharacteristic &> 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<string>()->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<BluetoothDevice> &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<BluetoothAdapter> 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<int>()->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<int>();
-
- adapter = getAdapter(adapter_index);
+ adapter = bluetoothSystem.getAdapter(adapter_name);
set<Mac> 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<BluetoothAdapter> getAdapter(int hciDevice) {
- return getAdapterImpl(hciDevice);
-}
+shared_ptr<BluetoothAdapter> 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<BluetoothGattCharacteristic &> findCharacteristic(uuid_t uuid) const {
+ virtual const o<const BluetoothGattCharacteristic &> findCharacteristic(uuid_t uuid) const {
for (auto c: characteristics) {
if (memcmp(c->getUuid().data, uuid.data, 16) == 0) {
- return o<BluetoothGattCharacteristic &>(*c);
+ return o<const BluetoothGattCharacteristic &>(*c);
}
}
- return o<BluetoothGattCharacteristic &>();
+ return o<const BluetoothGattCharacteristic &>();
}
protected:
@@ -187,7 +187,7 @@ protected:
};
template<class A>
-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<BluetoothAdapter> getAdapterImpl(int hciDevice);
-
-void shutdownImpl();
+shared_ptr<BluetoothAdapter> 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 <cassert>
#include <cstring>
#include <iomanip>
+#include <cmath>
+#include <iostream>
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<uint32_t>(FLOAT::NaN);
+ } else if (d > FLOAT::max) {
+ result = static_cast<uint32_t>(FLOAT::positive_infinity);
+ } else if (d < FLOAT::min) {
+ result = static_cast<uint32_t>(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<void(BluetoothDevice &device)>) override;
+ LinuxBluetoothAdapter(const LinuxBluetoothAdapter &) = delete;
- BluetoothDevice &getDevice(Mac &mac) override;
+ LinuxBluetoothAdapter& operator=(const LinuxBluetoothAdapter&) = delete;
+
+ void runScan(std::function<void(const shared_ptr<BluetoothDevice> &device)>) override;
+
+ shared_ptr <BluetoothDevice> getDevice(Mac &mac) override;
private:
void startScan() override;
@@ -48,25 +52,29 @@ private:
struct hci_filter hciFilter;
bool scanning;
- map<Mac, LinuxBluetoothDevice *> devices;
+ map<Mac, shared_ptr<LinuxBluetoothDevice>> devices;
};
-class LinuxBluetoothDevice : public DefaultBluetoothDevice<LinuxBluetoothAdapter> {
+class LinuxBluetoothDevice final : public DefaultBluetoothDevice<LinuxBluetoothAdapter>,
+ public std::enable_shared_from_this<LinuxBluetoothDevice> {
public:
LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac &mac);
- ~LinuxBluetoothDevice();
+ virtual ~LinuxBluetoothDevice();
+
+ LinuxBluetoothDevice(const LinuxBluetoothDevice &) = delete;
+
+ LinuxBluetoothDevice& operator=(const LinuxBluetoothDevice&) = delete;
shared_ptr<BluetoothGatt> connectGatt() override;
private:
- weak_ptr<LinuxBluetoothGatt> gatt;
- int l2cap;
+ LinuxBluetoothGatt *gatt;
};
-class LinuxBluetoothGatt : public DefaultBluetoothGatt<LinuxBluetoothDevice> {
+class LinuxBluetoothGatt final : public DefaultBluetoothGatt<LinuxBluetoothDevice> {
public:
- LinuxBluetoothGatt(LinuxBluetoothDevice &device, int l2cap);
+ LinuxBluetoothGatt(LinuxBluetoothDevice &device);
~LinuxBluetoothGatt();
@@ -94,8 +102,6 @@ private:
ByteBuffer writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> 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<BluetoothGatt> 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<LinuxBluetoothGatt>(*this, l2cap);
- gatt = ref;
+ gatt = new LinuxBluetoothGatt(*this);
- return ref;
+ return shared_ptr<LinuxBluetoothGatt>(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<Mac, LinuxBluetoothDevice *>::iterator it = devices.find(mac);
+shared_ptr<BluetoothDevice> LinuxBluetoothAdapter::getDevice(Mac &mac) {
+ auto it = devices.find(mac);
if (it == devices.end()) {
- LinuxBluetoothDevice *device = new LinuxBluetoothDevice(*this, mac);
+ auto device = make_shared<LinuxBluetoothDevice>(*this, mac);
devices[mac] = device;
- return *device;
+ return device;
}
- return *it->second;
+ return it->second;
}
-void LinuxBluetoothAdapter::runScan(std::function<void(BluetoothDevice &device)> callback) {
+void LinuxBluetoothAdapter::runScan(std::function<void(const shared_ptr<BluetoothDevice> &device)> callback) {
fd_set rfds;
FD_ZERO(&rfds);
@@ -559,7 +565,7 @@ void LinuxBluetoothAdapter::runScan(std::function<void(BluetoothDevice &device)>
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<int, shared_ptr<LinuxBluetoothAdapter>> adapters;
-
-shared_ptr<BluetoothAdapter> getAdapterImpl(int hciDevice) {
- map<int, shared_ptr<LinuxBluetoothAdapter>>::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<BluetoothAdapter> 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<LinuxBluetoothAdapter>(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<BluetoothAdapter, LinuxBluetoothAdapter>(it->second);
+ auto adapter = make_shared<LinuxBluetoothAdapter>(hciDevice, mac);
+
+ return std::static_pointer_cast<BluetoothAdapter, LinuxBluetoothAdapter>(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 <stdexcept>
#include <vector>
#include <cstdint>
+#include <map>
#include "ByteBuffer.h"
@@ -115,7 +116,7 @@ public:
virtual void addCharacteristic(BluetoothGattCharacteristic *characteristic) = 0;
- virtual const boost::optional<BluetoothGattCharacteristic &> findCharacteristic(boost::uuids::uuid uuid) const = 0;
+ virtual const boost::optional<const BluetoothGattCharacteristic &> findCharacteristic(boost::uuids::uuid uuid) const = 0;
};
class BluetoothGatt {
@@ -165,9 +166,9 @@ public:
virtual void stopScan() = 0;
- virtual void runScan(std::function<void(BluetoothDevice &device)>) = 0;
+ virtual void runScan(std::function<void(const shared_ptr<BluetoothDevice> &device)>) = 0;
- virtual BluetoothDevice &getDevice(Mac &mac) = 0;
+ virtual shared_ptr <BluetoothDevice> getDevice(Mac &mac) = 0;
protected:
BluetoothAdapter();
@@ -185,6 +186,11 @@ public:
BluetoothSystem();
~BluetoothSystem();
+
+ shared_ptr<BluetoothAdapter> getAdapter(string name);
+
+private:
+ map<string, shared_ptr<BluetoothAdapter>> 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<AttributeData> parse(ByteBuffer &bytes, AttPduType type);
@@ -247,10 +253,6 @@ private:
AttributeData(uint16_t handle, ByteBuffer value);
};
-shared_ptr<BluetoothAdapter> 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 <iosfwd>
#include <memory>
#include <stdexcept>
+#include <math.h>
+
+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 <iomanip>
#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);
+}
generated by cgit v1.2.3 (git 2.46.0) at 2024-12-22 21:49:49 +0000