2020-01-09 01:01:38 +01:00
/* Copyright 2019-2020 King Kévin <kingkevin@cuvoodoo.info>
2019-09-30 13:16:49 +02:00
*
* 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>
2020-01-09 01:01:38 +01:00
# include <signal.h>
2019-09-30 13:16:49 +02:00
# 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 ) ;
}
2020-01-09 01:01:38 +01:00
/* 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 " ) ;
}
2019-09-30 13:16:49 +02:00
/* 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 \t print help \n " ) ;
printf ( " \t -d \t enable debug output \n " ) ;
printf ( " \t host port \t influxDB UDP host and port to send energy measurements to \n " ) ;
exit ( 0 ) ;
} if ( 0 = = strcmp ( argv [ 1 ] , " -d " ) ) {
debug = true ;
}
}
2020-01-09 01:01:38 +01:00
signal ( SIGALRM , watchdog ) ; // setup watchdog
alarm ( 30 ) ; // start watchdog (in case connecting to the U2 gets stuck)
2019-09-30 13:16:49 +02:00
// 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 ) {
2020-01-09 11:19:43 +01:00
die ( " could not create UDP socket \n " ) ;
2019-09-30 13:16:49 +02:00
}
}
// 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
2020-01-09 11:31:11 +01:00
char line [ 22 * 4 ] ;
2019-09-30 13:16:49 +02:00
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)
2020-01-09 01:01:38 +01:00
alarm ( 10 ) ; // restart watchdog
2019-09-30 13:16:49 +02:00
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 \n current value=%.04f \n charge value=%.04f \n energy 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 ;
}
}