kingkevin
/
web-u2
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
web-u2/u2_bt.c

931 lines
34 KiB
C
Raw Permalink Blame History

/* Copyright 2019-2020 King Kévin <kingkevin@cuvoodoo.info>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
/* this program connects to the Qway U2 USB power meter over Bluetooth and reads out the measurements
*
* this uses BlueZ 5.
* with BlueZ 5 the main interface is D-Bus, but D-Bus is a pain and there are not a lot of C examples.
* here a list of useful resources:
* https://dbus.freedesktop.org/doc/api/html/group__DBusMessage.html
* http://www.cplusplus.com/forum/unices/127440/
* https://leonardoce.wordpress.com/2015/03/17/dbus-tutorial-part-3/
* https://github.com/jjjsmit/BluetoothBLEClient/blob/master/bleClient.c
*/
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdarg.h>
#include <unistd.h>
#include <assert.h>
#include <signal.h>
#include <dbus/dbus.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
// the prefix for Qway bluwtooth device names
static const char* device_prefix = "QWAY_U2";
// GATT characteristic UUID for data exchange
static const char* uart_uuid = "0000ffe1-0000-1000-8000-00805f9b34fb"; // for HM-10 UART port
// if debug messages should be output
static bool debug = false;
/* print message to standard error and
* @param[in] format format string
*/
static void printf_debug(const char* format, ...)
{
if (!debug) {
return;
}
va_list args;
va_start(args, format);
vfprintf(stdout, format, args);
va_end(args);
}
/* print message to standard error and quit program
* @param[in] format format string
*/
static void die(const char* format, ...)
{
va_list args;
va_start(args, format);
vfprintf(stderr, format, args);
va_end(args);
exit(1);
}
/* watchdog called after alarm timeout
* @param[in] sig alarm signal
*/
static void watchdog(int sig)
{
(void)sig; // argument not used
die("exiting due to inactivity\n");
}
/* get all D-Bus object paths with matching interface
* @param[in] connection D-Bus connection
* @param[in] bus_name bus to get objects from
* @param[in] interface interface string to match
* @param[out] error D-Bus error set on error
* @param[out] object_paths array of object paths with matching interface
* @return number of objects found
* @note D-Bus error is set in case of error
* @warning it is up to the caller to free the object paths
*/
static unsigned int dbus_get_object_paths_by_interface(DBusConnection* connection, const char* bus_name, const char* interface, DBusError* error, char*** object_paths) {
assert(connection);
assert(bus_name);
assert(interface);
unsigned int paths_count = 0; // number of matching paths
DBusMessage* query = dbus_message_new_method_call(bus_name, "/", "org.freedesktop.DBus.ObjectManager", "GetManagedObjects"); // prepare the call to get all bus objects
assert(query);
DBusMessage* reply = dbus_connection_send_with_reply_and_block(connection, query, -1, error); // request all bus objects
if (dbus_error_is_set(error)) {
return 0;
}
assert(reply);
if (query) {
dbus_message_unref(query);
query = NULL;
}
// check D-Bus objects array
DBusMessageIter iter_root;
if (!dbus_message_iter_init(reply, &iter_root)) {
dbus_set_error_const(error, "iter_init_error", "Could not initialise message iterator");
return 0;
}
if (DBUS_TYPE_ARRAY != dbus_message_iter_get_arg_type(&iter_root)) {
dbus_set_error_const(error, "arg_type_error", "Expected array type");
return 0;
}
// go through D-Bus objects array
DBusMessageIter iter_array;
dbus_message_iter_recurse(&iter_root, &iter_array); // got through array
while(true) { // iterate through array (will exit using break)
if (DBUS_TYPE_DICT_ENTRY == dbus_message_iter_get_arg_type(&iter_array)) { // it should be an array of entries (e.g. dictionary)
DBusMessageIter iter_entry;
dbus_message_iter_recurse(&iter_array, &iter_entry); // get dictionary entry
if (DBUS_TYPE_OBJECT_PATH == dbus_message_iter_get_arg_type(&iter_entry)) { // expect a path
char* path = NULL;
dbus_message_iter_get_basic(&iter_entry, &path); // remember path (key of entry)
dbus_message_iter_next(&iter_entry); // go to value if entry
if (DBUS_TYPE_ARRAY == dbus_message_iter_get_arg_type(&iter_entry)) { // value should be an array
DBusMessageIter iter_subarray;
dbus_message_iter_recurse(&iter_entry, &iter_subarray); // go through array
while (true) { // iterate through array (will exit using break)
if (DBUS_TYPE_DICT_ENTRY == dbus_message_iter_get_arg_type(&iter_subarray)) { // we are looking for an entry
DBusMessageIter iter_subentry;
dbus_message_iter_recurse(&iter_subarray, &iter_subentry); // get entry
if (DBUS_TYPE_STRING == dbus_message_iter_get_arg_type(&iter_subentry)) { // expect interface name as string
char* name = NULL;
dbus_message_iter_get_basic(&iter_subentry, &name); // get interface name
if (0 == strcmp(name, interface)) { // check if it is the wished interface
paths_count++;
//assert(paths_count < SIZE_MAX / sizeof(char*));
*object_paths = realloc(*object_paths, paths_count * sizeof(char*));
if (NULL == *object_paths) {
dbus_set_error_const(error, "memory_error", "Unable to allocated memory for paths");;
return paths_count - 1;
}
assert(strlen(path) < SIZE_MAX - 1);
(*object_paths)[paths_count - 1] = malloc(strlen(path) + 1);
if (NULL == (*object_paths)[paths_count - 1]) {
dbus_set_error_const(error, "memory_error", "Unable to allocated memory for path");;
return paths_count - 1;
}
strcpy((*object_paths)[paths_count - 1], path);
}
}
}
if (dbus_message_iter_has_next(&iter_subarray)) {
dbus_message_iter_next(&iter_subarray); // go to next element
} else {
break;
}
} // end going through inner array
} // end if array
} // end if path
} // end if entry
if(dbus_message_iter_has_next(&iter_array)) {
dbus_message_iter_next(&iter_array);
} else {
break;
}
} // end of adapter search
// clean buffers
if (reply) {
dbus_message_unref(reply);
reply = NULL;
}
return paths_count;
}
/* get property from D-Bus object
* @param[in] connection D-Bus connection
* @param[in] bus_name bus to get object from
* @param[in] path object patch to get get property from
* @param[in] interface interface to get property from
* @param[out] error D-Bus error set on error
* @param[in] property_type property type to get
* @param[out] property were to write to property to
* @note D-Bus error is set in case of error
*/
static void dbus_get_property(DBusConnection *connection, const char *bus_name, const char *path, const char *interface, const char *property_name, DBusError *error, int property_type, void* property) {
assert(connection);
assert(bus_name);
assert(path);
assert(interface);
assert(property_name);
assert(error);
assert(property);
// create Get query to read property
DBusMessage* query = dbus_message_new_method_call(bus_name, path, "org.freedesktop.DBus.Properties", "Get");
assert(query);
dbus_message_append_args(query, DBUS_TYPE_STRING, &interface, DBUS_TYPE_STRING, &property_name, DBUS_TYPE_INVALID);
DBusMessage* reply = dbus_connection_send_with_reply_and_block(connection, query, -1, error);
if (query) {
dbus_message_unref(query);
query = NULL;
}
if (dbus_error_is_set(error)) {
goto end;
}
// get property from response (stored in a variant)
DBusMessageIter iter;
dbus_message_iter_init(reply, &iter);
if (DBUS_TYPE_VARIANT != dbus_message_iter_get_arg_type(&iter)) {
dbus_set_error_const(error, "reply_should_be_variant", "This message hasn't a variant response type");
goto end;
}
DBusMessageIter sub;
dbus_message_iter_recurse(&iter, &sub);
if (property_type != dbus_message_iter_get_arg_type(&sub)) {
dbus_set_error_const(error, "variant_type_mismatch", "This variant reply message does not have the right content type");
goto end;
}
dbus_message_iter_get_basic(&sub, property);
end:
if (reply) {
dbus_message_unref(reply);
reply = NULL;
}
}
/* set property of D-Bus object
* @param[in] connection D-Bus connection
* @param[in] bus_name bus to get object from
* @param[in] path object patch to get get property from
* @param[in] interface interface to get property from
* @param[out] error D-Bus error set on error
* @param[in] container_type type of the container/variant
* @param[in] property_type property type to get
* @param[in] property property value to set
*/
static void dbus_set_property(DBusConnection *connection, const char *bus_name, const char *path, const char *interface, const char *property_name, DBusError *error, const char* container_type, int property_type, const void* property) {
assert(connection);
assert(bus_name);
assert(path);
assert(interface);
assert(property_name);
assert(error);
assert(property);
// create Set query to write property
DBusMessage* query = dbus_message_new_method_call(bus_name, path, "org.freedesktop.DBus.Properties", "Set");
assert(query);
DBusMessageIter iter, sub_iter;
dbus_message_iter_init_append(query, &iter);
dbus_message_iter_append_basic(&iter, DBUS_TYPE_STRING, &interface);
dbus_message_iter_append_basic(&iter, DBUS_TYPE_STRING, &property_name);
dbus_message_iter_open_container(&iter, DBUS_TYPE_VARIANT, container_type, &sub_iter);
dbus_message_iter_append_basic(&sub_iter, property_type, property);
dbus_message_iter_close_container(&iter, &sub_iter);
DBusMessage* reply = dbus_connection_send_with_reply_and_block(connection, query, -1, error);
if (query) {
dbus_message_unref(query);
query = NULL;
}
if (reply) {
dbus_message_unref(reply);
reply = NULL;
}
}
/* call D-Bus object method
* @param[in] connection D-Bus connection
* @param[in] bus_name bus to get object from
* @param[in] path object patch to get get property from
* @param[in] interface interface to get property from
* @param[in] method_name object method to call
* @param[out] error D-Bus error set on error
*/
static void dbus_call_method(DBusConnection *connection, const char *bus_name, const char *path, const char *interface, const char *method_name, DBusError *error) {
assert(connection);
assert(bus_name);
assert(path);
assert(interface);
assert(method_name);
assert(error);
DBusMessage* query = dbus_message_new_method_call(bus_name, path, interface, method_name);
assert(query);
DBusMessage* reply = dbus_connection_send_with_reply_and_block(connection, query, -1, error);
if (query) {
dbus_message_unref(query);
query = NULL;
}
if (reply) {
dbus_message_unref(reply);
reply = NULL;
}
}
/* get file descriptor from GATT characteristic
* @param[in] connection D-Bus connection
* @param[in] path object patch to get get property from
* @param[in] method acquire method returning wished file descriptor
* @param[out] error D-Bus error set on error
* @return file descriptor
* @note sets dbus_error on error
*/
static int bluez_gatt_acquire(DBusConnection *connection, const char *path, const char *method, DBusError *error) {
assert(connection);
assert(path);
assert(error);
int fd = 0;
uint16_t mtu;
DBusMessage* query = dbus_message_new_method_call("org.bluez", path, "org.bluez.GattCharacteristic1", method);
assert(query);
DBusMessageIter iter, dict;
dbus_message_iter_init_append(query, &iter);
dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY, DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING DBUS_TYPE_STRING_AS_STRING DBUS_TYPE_VARIANT_AS_STRING DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &dict);
dbus_message_iter_close_container(&iter, &dict);
DBusMessage*reply = dbus_connection_send_with_reply_and_block(connection, query, -1, error);
if (query) {
dbus_message_unref(query);
query = NULL;
}
if (dbus_error_is_set(error)) {
return 0;
}
if (!dbus_message_get_args(reply, NULL, DBUS_TYPE_UNIX_FD, &fd, DBUS_TYPE_UINT16, &mtu, DBUS_TYPE_INVALID)) {
dbus_set_error_const(error, "invalid_argument", "invalid acquire response");
}
if (reply) {
dbus_message_unref(reply);
reply = NULL;
}
return fd;
}
/* write data to GATT characteristic
* @param[in] connection D-Bus connection
* @param[in] path object patch to get get property from
* @param[out] error D-Bus error set on error
* @param[in] bytes data to write
* @param[in] length length of data to write
* @note sets dbus_error on error
*/
static void bluez_gatt_write(DBusConnection *connection, const char *path, DBusError *error, const uint8_t** bytes, size_t length) {
if (!bytes || 0 == length) {
return;
}
assert(connection);
assert(path);
assert(error);
DBusMessage* query = dbus_message_new_method_call("org.bluez", path, "org.bluez.GattCharacteristic1", "WriteValue");
assert(query);
DBusMessageIter iter, array, dict;
dbus_message_iter_init_append(query, &iter);
dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY, "y", &array);
dbus_message_iter_append_fixed_array(&array, DBUS_TYPE_BYTE, bytes, length);
dbus_message_iter_close_container(&iter, &array);
dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY, DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING DBUS_TYPE_STRING_AS_STRING DBUS_TYPE_VARIANT_AS_STRING DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &dict);
dbus_message_iter_close_container(&iter, &dict);
DBusMessage*reply = dbus_connection_send_with_reply_and_block(connection, query, -1, error);
if (query) {
dbus_message_unref(query);
query = NULL;
}
if (dbus_error_is_set(error)) {
return;
}
if (reply) {
dbus_message_unref(reply);
reply = NULL;
}
}
struct u2_values_t {
float voltage_vbus;
float current_vbus;
float voltage_dp;
float voltage_dm;
float charge;
float energy;
float temperature_internal;
float temperature_external;
int8_t accel_x;
int8_t accel_y;
int8_t accel_z;
uint32_t time_on;
uint32_t time_rec;
uint8_t group;
uint8_t messages; // bit mask of which messages have been received
};
int main(int argc, char* argv[])
{
if (argc >= 2) {
if (0 == strcmp(argv[1],"-h")) {
printf("%s [-d|-h] [host port]\n", argv[0]);
printf("connect to QWay U2 over Bluetooth and output measurements as CSV\n\n");
printf("\t-h\tprint help\n");
printf("\t-d\tenable debug output\n");
printf("\thost port\tinfluxDB UDP host and port to send energy measurements to\n");
exit(0);
} if (0 == strcmp(argv[1],"-d")) {
debug = true;
}
}
signal(SIGALRM, watchdog); // setup watchdog
alarm(30); // start watchdog (in case connecting to the U2 gets stuck)
// UDP socket to send data to influxdb
int influxdb_fd = 0;
struct addrinfo* res = 0;
if (argc >= 3) {
struct addrinfo hints;
memset(&hints, 0, sizeof(hints));
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_DGRAM;
hints.ai_protocol = 0;
hints.ai_flags = AI_ADDRCONFIG;
if (0 != getaddrinfo(argv[argc -2], argv[argc - 1], &hints, &res)) {
die("failed to resolve remote socket address\n");
}
influxdb_fd = socket(res->ai_family,res->ai_socktype,res->ai_protocol);
if (0 == influxdb_fd) {
die("could not create UDP socket\n");
}
}
// D-Bus variables
DBusConnection *connection = NULL;
DBusError error;
char** paths = NULL; // to get the object paths
// connect to system D-Bus
dbus_error_init(&error);
connection = dbus_bus_get(DBUS_BUS_SYSTEM, &error);
if (dbus_error_is_set(&error)) {
die("dbus: can't connect to system bus (%s)\n", error.message);
dbus_error_free(&error);
} else if (NULL == connection) {
die("dbus: can't connect to system bus\n");
}
printf_debug("connected to D-BUS\n");
// get adapter
unsigned int paths_count = dbus_get_object_paths_by_interface(connection, "org.bluez", "org.bluez.Adapter1", &error, &paths);
if (dbus_error_is_set(&error)) {
die("dbus: can't get bus objects (%s)\n", error.message);
dbus_error_free(&error);
}
if (paths_count < 1) {
die("dbus: no bluetooth adapter found\n");
}
if (!paths) {
die("dbus: no bluetooth adapter paths found\n");
}
if (!paths[0]) {
die("dbus: no bluetooth adapter path found\n");
}
char* adapter_path = paths[0]; // pick first adapter path
for (unsigned int i = 1; i < paths_count; i++) {
if (paths[i]) {
free(paths[i]);
paths[i] = NULL;
}
}
if (paths) {
free(paths);
paths = NULL;
}
printf_debug("BlueZ adapter found: %s\n", adapter_path);
// power on adapter
bool powered;
dbus_get_property(connection, "org.bluez", adapter_path, "org.bluez.Adapter1", "Powered", &error, DBUS_TYPE_BOOLEAN, &powered);
if (dbus_error_is_set(&error)) {
die("dbus: can't read adapter Powered property from %s (%s)\n", adapter_path, error.message);
dbus_error_free(&error);
}
printf_debug("adapter is %spowered\n", powered ? "" : "not ");
if (!powered) {
printf("powering adapter\n");
powered = true;
dbus_set_property(connection, "org.bluez", adapter_path, "org.bluez.Adapter1", "Powered", &error, DBUS_TYPE_BOOLEAN_AS_STRING, DBUS_TYPE_BOOLEAN, &powered);
if (dbus_error_is_set(&error)) {
die("dbus: can't write adapter Powered property to %s (%s)\n", adapter_path, error.message);
dbus_error_free(&error);
}
dbus_get_property(connection, "org.bluez", adapter_path, "org.bluez.Adapter1", "Powered", &error, DBUS_TYPE_BOOLEAN, &powered);
if (dbus_error_is_set(&error)) {
die("dbus: can't read adapter Powered property from %s (%s)\n", adapter_path, error.message);
dbus_error_free(&error);
}
if (!powered) {
die("could not power adapter\n");
}
}
// start discovering devices
bool already_discovering;
dbus_get_property(connection, "org.bluez", adapter_path, "org.bluez.Adapter1", "Discovering", &error, DBUS_TYPE_BOOLEAN, &already_discovering);
if (dbus_error_is_set(&error)) {
die("dbus: can't read adapter Discovering property from %s (%s)\n", adapter_path, error.message);
dbus_error_free(&error);
}
printf_debug("adapter is %sdiscovering\n", already_discovering ? "" : "not ");
if (!already_discovering) {
printf_debug("start discovery\n");
dbus_call_method(connection, "org.bluez", adapter_path, "org.bluez.Adapter1", "StartDiscovery", &error);
if (dbus_error_is_set(&error)) {
die("dbus: can't start discovery (%s)\n", error.message);
dbus_error_free(&error);
}
bool discovering;
dbus_get_property(connection, "org.bluez", adapter_path, "org.bluez.Adapter1", "Discovering", &error, DBUS_TYPE_BOOLEAN, &discovering);
if (dbus_error_is_set(&error)) {
die("dbus: can't read adapter Discovering property from %s (%s)\n", adapter_path, error.message);
dbus_error_free(&error);
}
if (!discovering) {
die("cannot discover devices\n");
}
}
// wait until Qway device is discovered (search by name and check if active)
char* device_path = NULL; // D-Bus path of the Qway device
while (!device_path) {
unsigned int device_count = dbus_get_object_paths_by_interface(connection, "org.bluez", "org.bluez.Device1", &error, &paths);
if (dbus_error_is_set(&error)) {
die("dbus: can't get bus objects (%s)\n", error.message);
dbus_error_free(&error);
}
for (unsigned int i = 0; paths && i < device_count; i++) {
if (!device_path && paths[i]) {
char* name = NULL;
dbus_get_property(connection, "org.bluez", paths[i], "org.bluez.Device1", "Name", &error, DBUS_TYPE_STRING, &name);
if (dbus_error_is_set(&error)) { // ignore device with unknown names
dbus_error_free(&error);
} else if (0 == strncmp(name, device_prefix, strlen(device_prefix))) { // check if it is a U2
printf_debug("device %s found: %s\n", name, paths[i]);
bool reachable = false;
bool connected = false;
dbus_get_property(connection, "org.bluez", paths[i], "org.bluez.Device1", "Connected", &error, DBUS_TYPE_BOOLEAN, &connected);
if (dbus_error_is_set(&error)) {
die("dbus: can't read device Connected property from %s (%s)\n", paths[i], error.message);
dbus_error_free(&error);
} else if (connected) {
reachable = true;
}
int16_t rssi;
dbus_get_property(connection, "org.bluez", paths[i], "org.bluez.Device1", "RSSI", &error, DBUS_TYPE_INT16, &rssi);
if (dbus_error_is_set(&error)) { // actual error not checked
dbus_error_free(&error);
} else {
reachable = true;
}
if (reachable) {
device_path = paths[i]; // found active device
} else {
printf_debug("device unreachable\n");
}
}
}
if (paths[i] && !(device_path && 0 == strcmp(device_path, paths[i]))) {
free(paths[i]);
paths[i] = NULL;
}
}
if (paths) {
free(paths);
paths = NULL;
}
if (!device_path) {
printf_debug("device not found, waiting a bit\n");
sleep(3);
}
}
if (!device_path) {
die("no device found\n");
}
if (!already_discovering) {
bool discovering;
dbus_get_property(connection, "org.bluez", adapter_path, "org.bluez.Adapter1", "Discovering", &error, DBUS_TYPE_BOOLEAN, &discovering);
if (dbus_error_is_set(&error)) {
die("dbus: can't read adapter Discovering property from %s (%s)\n", adapter_path, error.message);
dbus_error_free(&error);
}
if (discovering) {
printf_debug("stop discovery\n");
dbus_call_method(connection, "org.bluez", adapter_path, "org.bluez.Adapter1", "StopDiscovery", &error);
if (dbus_error_is_set(&error)) {
die("dbus: can't stop discovery (%s)\n", error.message);
dbus_error_free(&error);
}
dbus_get_property(connection, "org.bluez", adapter_path, "org.bluez.Adapter1", "Discovering", &error, DBUS_TYPE_BOOLEAN, &discovering);
if (dbus_error_is_set(&error)) {
die("dbus: can't read adapter Discovering property from %s (%s)\n", adapter_path, error.message);
dbus_error_free(&error);
}
if (discovering) {
die("cannot stop device discovery\n");
}
}
}
// connect to device
bool already_connected;
dbus_get_property(connection, "org.bluez", device_path, "org.bluez.Device1", "Connected", &error, DBUS_TYPE_BOOLEAN, &already_connected);
if (dbus_error_is_set(&error)) {
die("dbus: can't read device Connected property from %s (%s)\n", device_path, error.message);
dbus_error_free(&error);
}
printf_debug("device is %sconnected\n", already_connected ? "" : "not ");
if (!already_connected) {
printf_debug("connecting to device\n");
dbus_call_method(connection, "org.bluez", device_path, "org.bluez.Device1", "Connect", &error);
if (dbus_error_is_set(&error)) {
die("dbus: can't connect to %s (%s)\n", device_path, error.message);
dbus_error_free(&error);
}
bool connected;
dbus_get_property(connection, "org.bluez", device_path, "org.bluez.Device1", "Connected", &error, DBUS_TYPE_BOOLEAN, &connected);
if (dbus_error_is_set(&error)) {
die("dbus: can't read device Connected property from %s (%s)\n", device_path, error.message);
dbus_error_free(&error);
}
if (!connected) {
die("cannot connect to device\n");
}
}
// wait until the services are resolved
bool resolved = false;
while (!resolved) {
dbus_get_property(connection, "org.bluez", device_path, "org.bluez.Device1", "ServicesResolved", &error, DBUS_TYPE_BOOLEAN, &resolved);
if (dbus_error_is_set(&error)) {
die("dbus: can't read device ServicesResolved property from %s (%s)\n", device_path, error.message);
dbus_error_free(&error);
}
bool connected;
dbus_get_property(connection, "org.bluez", device_path, "org.bluez.Device1", "Connected", &error, DBUS_TYPE_BOOLEAN, &connected);
if (dbus_error_is_set(&error)) {
die("dbus: can't read device Connected property from %s (%s)\n", device_path, error.message);
dbus_error_free(&error);
}
if (!connected) {
die("connection to device lost\n");
}
if (!resolved) {
printf_debug("waiting for services to be resolved\n");
sleep(1);
}
}
// get path to HM-10 custom characteristic to exchange data 0000FFE1-0000-1000-8000-00805F9B34FB
char* char_path = NULL;
unsigned int char_count = dbus_get_object_paths_by_interface(connection, "org.bluez", "org.bluez.GattCharacteristic1", &error, &paths);
if (dbus_error_is_set(&error)) {
die("dbus: can't get GATT characteristics (%s)\n", error.message);
dbus_error_free(&error);
}
for (unsigned int i = 0; paths && i < char_count; i++) {
if (paths[i]) {
if (0 == strncmp(paths[i], device_path, strlen(device_path))) { // only check characteristic from our device
char* uuid = NULL;
dbus_get_property(connection, "org.bluez", paths[i], "org.bluez.GattCharacteristic1", "UUID", &error, DBUS_TYPE_STRING, &uuid);
if (dbus_error_is_set(&error)) {
die("dbus: can't read UUID from %s (%s)\n", paths[i], error.message);
dbus_error_free(&error);
} else if (0 == strcmp(uuid, uart_uuid)) { // check if it is the characteristic we need
printf_debug("characteristic found: %s\n", paths[i]);
char_path = paths[i];
}
}
if (!char_path || strcmp(char_path, paths[i])) {
free(paths[i]);
paths[i] = NULL;
}
}
}
if (paths) {
free(paths);
paths = NULL;
}
if (!char_path) {
printf_debug("device characteristic not found\n");
goto close;
}
// open read
bool notifying;
dbus_get_property(connection, "org.bluez", char_path, "org.bluez.GattCharacteristic1", "Notifying", &error, DBUS_TYPE_BOOLEAN, &notifying);
if (dbus_error_is_set(&error)) {
die("dbus: can't read GATT Notifying property from %s (%s)\n", char_path, error.message);
dbus_error_free(&error);
}
if (notifying) {
printf("device characteristic notification is already used by someone else\n");
goto close;
}
bool notify_acquired;
dbus_get_property(connection, "org.bluez", char_path, "org.bluez.GattCharacteristic1", "Notifying", &error, DBUS_TYPE_BOOLEAN, &notify_acquired);
if (dbus_error_is_set(&error)) {
die("dbus: can't read GATT Notifying property from %s (%s)\n", char_path, error.message);
dbus_error_free(&error);
}
if (notifying) {
printf_debug("device characteristic notification is already used by someone else\n");
goto close;
}
int notify_fd = bluez_gatt_acquire(connection, char_path, "AcquireNotify", &error);
if (dbus_error_is_set(&error)) {
die("dbus: can't acquire write file descriptor from %s (%s)\n", char_path, error.message);
dbus_error_free(&error);
}
// sadly this characteristic does not support AcquireWrite, thus we will hat to write through D-Bus
printf_debug("ready to receive data\n");
const uint8_t get_conf[] = {0xf1, 0x01, 0x00, 0x00, 0x00, 0xfe}; // message to get the configuration (also starts the measurements)
const uint8_t* packet_pointer = get_conf; // this is because &array == array
bluez_gatt_write(connection, char_path, &error, &packet_pointer, sizeof(get_conf));
if (dbus_error_is_set(&error)) {
fprintf(stderr, "dbus: can't write to %s (%s)\n", char_path, error.message);
// don't die since write error could be solved by disconnecting
dbus_error_free(&error);
goto close;
}
printf_debug("device information requested\n");
// read and parse 2 messages
uint8_t packet[20]; // to store the message read from U2
uint8_t packet_len = 0; // number of bytes read
struct u2_values_t u2_values; // to store latest U2 values
u2_values.messages = 0; // remember no message has been received
u2_values.group = 0xff; // to know it is not initialized
// print CSV header
printf("VBUS voltage (V), VBUS current (A), D+ voltage (V), D- voltage (V), temperature internal (°C), temperature external (°C), on time (s), recording time (s), acceleration x-axis, acceleration Y-axis, acceleration Z-axis, charge (Ah), energy (Wh), group\n");
while (true) {
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(notify_fd, &readfds);
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 100000;
int activity = select(notify_fd + 1, &readfds, NULL, NULL, &timeout);
if (activity < 0) { // error received
die("select error\n");
} else if (activity > 0) { // data received
uint8_t rbuff[20]; // 20 bytes is the maximum BLE packet size
ssize_t count = read(notify_fd, rbuff, sizeof(rbuff));
if (count <= 0) {
fprintf(stderr, "can't read from device\n");
goto close;
}
for (ssize_t i = 0; i < count; i++) {
printf_debug("%02x ", rbuff[i]);
}
printf_debug("\n");
// copy data to packet
if (0xf1 == rbuff[0] || 0xfe == rbuff[0]) { // message header detected
for (packet_len = 0; packet_len < count && packet_len < sizeof(packet); packet_len++) {
packet[packet_len] = rbuff[packet_len];
}
} else if (packet_len < sizeof(packet)) { // copy rest of packet
for (uint8_t i = 0; i < count && packet_len + i < sizeof(packet); i++) {
packet[packet_len++] = rbuff[i];
}
}
// line containing values for influxdb
char line[22 * 4];
int line_len;
if (packet_len == sizeof(packet)) { // message is complete
if (0xf1 == packet[0] && 0x01 == packet[1]) { // device information packet
printf_debug("device serial: %c%c%c%c%c%c\n", packet[2], packet[3], packet[4], packet[5], packet[6], packet[7]);
printf_debug("firmware version: %u.%u\n", packet[8] / 16, packet[8] % 16);
float fvalue; // to store floating values
uint32_t uvalue; // to store unsigned integer values
memcpy(&uvalue, &packet[9], 4);
printf_debug("run: %u\n", uvalue);
memcpy(&uvalue, &packet[13], 1);
printf_debug("current threshold: %.04f A\n", uvalue / 1000.0);
memcpy(&fvalue, &packet[14], 4);
printf_debug("energy: %.04f Wh\n", fvalue);
printf_debug("group: %u\n", packet[18] + 1);
} else if (0xfe == packet[0] && packet[1] < 6 && packet[18] < 6 && 0x00 == packet[19]) { // measurement packet (use group limit and last byte to somehow detect transmission errors)
alarm(10); // restart watchdog
u2_values.messages |= (1 << packet[1]);
if (u2_values.group != (packet[18] + 1)) { // new group
u2_values.messages = 0; // remember not all message has been received
u2_values.group = (packet[18] + 1); // remember new group
printf_debug("new group\n");
}
// VBUS voltage and current are in every message
memcpy(&u2_values.voltage_vbus, &packet[2], 4);
memcpy(&u2_values.current_vbus, &packet[6], 4);
switch (packet[1]) {
case 1:
memcpy(&u2_values.voltage_dp, &packet[10], 4);
memcpy(&u2_values.voltage_dm, &packet[14], 4);
break;
case 2:
memcpy(&u2_values.temperature_internal, &packet[10], 4);
memcpy(&u2_values.temperature_external, &packet[14], 4);
break;
case 3:
memcpy(&u2_values.time_on, &packet[10], 4);
memcpy(&u2_values.time_rec, &packet[14], 4);
break;
case 4:
u2_values.accel_x = (uint8_t)packet[10];
u2_values.accel_y = (uint8_t)packet[11];
u2_values.accel_z = (uint8_t)packet[12];
memcpy(&u2_values.time_rec, &packet[14], 4);
break;
case 5:
memcpy(&u2_values.charge, &packet[10], 4);
memcpy(&u2_values.energy, &packet[14], 4);
// send values to influxDB
if (influxdb_fd) {
line_len = snprintf(line, sizeof(line), "voltage value=%.04f\ncurrent value=%.04f\ncharge value=%.04f\nenergy value=%.04f\n", u2_values.voltage_vbus, u2_values.current_vbus, u2_values.charge, u2_values.energy);
if (-1 == sendto(influxdb_fd, line, line_len, 0, res->ai_addr, res->ai_addrlen)) {
printf("could not send UDP packet\n");
goto close;
}
}
break;
default:
printf("unknown type %u\n", packet[1]);
}
if (0x3e == u2_values.messages) { // all messages received all least once
// print CSV
printf("%.04f,", u2_values.voltage_vbus);
printf("%.04f,", u2_values.current_vbus);
printf("%.04f,", u2_values.voltage_dp);
printf("%.04f,", u2_values.voltage_dm);
printf("%.01f,", u2_values.temperature_internal);
printf("%.01f,", u2_values.temperature_external);
printf("%02u:%02u:%02u,", u2_values.time_on / 60 / 60, (u2_values.time_on / 60) % 60, u2_values.time_on % 60);
printf("%02u:%02u:%02u,", u2_values.time_rec / 60 / 60, (u2_values.time_rec / 60) % 60, u2_values.time_rec % 60);
printf("%d,", u2_values.accel_x);
printf("%d,", u2_values.accel_y);
printf("%d,", u2_values.accel_z);
printf("%.04f,", u2_values.charge);
printf("%.04f,", u2_values.energy);
printf("%u", u2_values.group);
printf("\n");
}
}
packet_len = 0; // mark packet as read
}
} else if (0 == activity) { // timeout reached
const uint8_t start_meas[] = {0xf1, 0x02, 0x00, 0x00, 0x00, 0xfe}; // message to get measurements
packet_pointer = start_meas; // this is because &array == array
bluez_gatt_write(connection, char_path, &error, &packet_pointer, sizeof(start_meas));
if (dbus_error_is_set(&error)) {
fprintf(stderr, "dbus: can't write to %s (%s)\n", char_path, error.message);
dbus_error_free(&error);
goto close;
}
printf_debug("resume measurements\n");
}
}
close:
// close file descriptors
if (notify_fd) {
close(notify_fd);
notify_fd = 0;
}
if (influxdb_fd) {
close(influxdb_fd);
influxdb_fd = 0;
}
// close device connection
if (!already_connected) {
bool connected = false;
dbus_get_property(connection, "org.bluez", device_path, "org.bluez.Device1", "Connected", &error, DBUS_TYPE_BOOLEAN, &connected);
if (dbus_error_is_set(&error)) {
die("dbus: can't read device Connected property from %s (%s)\n", device_path, error.message);
dbus_error_free(&error);
}
if (connected) {
printf_debug("disconnecting from device\n");
dbus_call_method(connection, "org.bluez", device_path, "org.bluez.Device1", "Disconnect", &error);
if (dbus_error_is_set(&error)) {
die("dbus: can't disconnect from %s (%s)\n", device_path, error.message);
dbus_error_free(&error);
}
dbus_get_property(connection, "org.bluez", device_path, "org.bluez.Device1", "Connected", &error, DBUS_TYPE_BOOLEAN, &connected);
if (dbus_error_is_set(&error)) {
die("dbus: can't read device Connected property from %s (%s)\n", device_path, error.message);
dbus_error_free(&error);
}
if (connected) {
die("cannot disconnect from device\n");
}
}
}
// close dbus connection
if (connection) {
dbus_connection_unref(connection); // unref instead of close for system bus connections
connection = NULL;
}
}
Reference in New Issue View Git Blame Copy Permalink