From b2f7a6fcd46871b411f74ab7fdcd12784355a653 Mon Sep 17 00:00:00 2001 From: jiung-yu Date: Fri, 1 Oct 2021 01:20:27 +0900 Subject: [PATCH] Add test for writing aoa data Change-Id: I90f211e777f509654af3f43ab21480aec6727543 Signed-off-by: Yu jiung --- tests/CMakeLists.txt | 2 +- tests/capi-network-uwb-test.c | 423 ++++++++++++++++++++++++++++++++++ 2 files changed, 424 insertions(+), 1 deletion(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f7e6703..6a02386 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -57,7 +57,7 @@ ADD_EXECUTABLE(${TARGET_UWB_TEST} ) TARGET_LINK_LIBRARIES(${TARGET_UWB_TEST} - ${TARGET_UWB_TEST_REQ_PKGS_LIBRARIES} ${PROJECT_NAME}) + ${TARGET_UWB_TEST_REQ_PKGS_LIBRARIES} ${PROJECT_NAME} -lpthread) #Install INSTALL(TARGETS ${TARGET_UWB_TEST} DESTINATION ${BIN_DIR}) \ No newline at end of file diff --git a/tests/capi-network-uwb-test.c b/tests/capi-network-uwb-test.c index 55f9de0..d7838df 100644 --- a/tests/capi-network-uwb-test.c +++ b/tests/capi-network-uwb-test.c @@ -6,6 +6,13 @@ #include #include +#include +#include + +#include + +#include + #include #include @@ -26,6 +33,269 @@ } \ } while (0) +#define RETV_IF(expr, val) do { \ + if (expr) { \ + printf("(%s)\n", #expr); \ + return (val); \ + } \ + } while (0) + +#define DEFAULT_NODE_UPDATE_INTERVAL 1 +#define DEFAULT_DATA_PATH "/tmp/aoa_data/" +#define DEFAULT_FILE_PREFIX "test_data" +#define DEFAULT_FILE_EXT ".csv" + +#define DEFAULT_FILE_NAME_LEN 80 +#define DEFAULT_DATA_LEN 80 + +typedef struct { + int node_id; + int range; + int aoa; + int pdoa; +} aoa_data_s; + +typedef void (*free_data_func)(void * user_data); + +typedef struct _node_s { + struct _node_s *next; + void *data; + free_data_func free_data; +} node_s; + +typedef struct { + pthread_mutex_t queue_lock; + pthread_cond_t queue_cond; + node_s *head; + node_s *tail; +} sync_queue_s; + +int sync_queue_init(sync_queue_s *queue, free_data_func free_node_func) +{ + if (!queue) + return -1; + + if (pthread_mutex_init(&queue->queue_lock, NULL) != 0) { + printf("mutex init failed\n"); + return -1; + } + if (pthread_cond_init(&queue->queue_cond, NULL) != 0) { + printf("cont init failed\n"); + return -1; + } + queue->head = NULL; + queue->tail = NULL; + + return 0; +} + +void sync_queue_push(sync_queue_s *queue, void *data, free_data_func free_func) +{ + if (!queue || !data) + return; + + pthread_mutex_lock(&queue->queue_lock); + + node_s *new_node = calloc(1, sizeof(node_s)); + new_node->next = NULL; + new_node->data = data; + new_node->free_data = free_func; + + if (queue->head == NULL) { + queue->head = new_node; + queue->tail = new_node; + } else { + queue->tail->next = new_node; + queue->tail = new_node; + } + + pthread_cond_broadcast(&queue->queue_cond); + pthread_mutex_unlock(&queue->queue_lock); +} + +node_s *sync_queue_wait_pop(sync_queue_s *queue) +{ + pthread_mutex_lock(&queue->queue_lock); + + if (queue->head == NULL) { + pthread_cond_wait(&queue->queue_cond, &queue->queue_lock); + } + + node_s *pop_node = queue->head; + if (queue->head == queue->tail) { + queue->head = NULL; + queue->tail = NULL; + } else { + queue->head = queue->head->next; + } + + pthread_mutex_unlock(&queue->queue_lock); + + return pop_node; +} + +void sync_queue_flush(sync_queue_s *queue) +{ + if (!queue) + return; + + pthread_mutex_lock(&queue->queue_lock); + node_s *current = queue->head; + while(current) { + node_s *next = current->next; + if (current->free_data) + current->free_data(current->data); + free(current); + current = next; + } + + pthread_cond_broadcast(&queue->queue_cond); + pthread_mutex_unlock(&queue->queue_lock); +} + +void sync_queue_deinit(sync_queue_s *queue) +{ + if (!queue) + return; + + sync_queue_flush(queue); + queue->head = NULL; + queue->tail = NULL; +} + +typedef struct { + pthread_t file_writer_thread; + sync_queue_s aoa_data_queue; + int report_position_fd; + bool is_running; +} monitor_ctx_s; + +void free_aoa_data(void *user_data) +{ + if (!user_data) + return; + + aoa_data_s *aoa_data_ptr = (aoa_data_s *)user_data; + free(aoa_data_ptr); + + return; +} + +int init_monitor_ctx(monitor_ctx_s *monitor) +{ + monitor->report_position_fd = -1; + if (sync_queue_init(&monitor->aoa_data_queue, free_aoa_data) != 0) + return -1; + + monitor->is_running = false; + + return 0; +} + +void __create_data_directory(void) +{ + struct stat st = {0}; + + if (stat(DEFAULT_DATA_PATH, &st) == -1) { + mkdir(DEFAULT_DATA_PATH, 0644); + } +} + +int __open_file_with_data_time(void) +{ + char filename[DEFAULT_FILE_NAME_LEN]; + struct tm *timenow; + int fd = -1; + + time_t now = time(NULL); + timenow = gmtime(&now); + + strftime(filename, sizeof(filename), DEFAULT_DATA_PATH DEFAULT_FILE_PREFIX "_%F-%T" DEFAULT_FILE_EXT, timenow); + fd = open(filename, O_RDWR|O_CREAT, 0755); + + return fd; +} + +static void __write_aoa_data(int aoa_data_file, aoa_data_s *aoa_data) +{ + char buffer[DEFAULT_DATA_LEN] = {0,}; + int ret; + if (aoa_data_file == -1) + return; + + ret = snprintf(buffer, sizeof(buffer), "%d,%d,%d,%d\n", aoa_data->node_id, aoa_data->range, aoa_data->aoa, aoa_data->pdoa); + if (ret == 0) + return; + + ret = write(aoa_data_file, buffer, ret + 1); + if(ret < 0) + printf("file write error\n"); + + return; +} + +static void *__file_writer(void *user_data) +{ + monitor_ctx_s *monitor = (monitor_ctx_s *)user_data; + if (!monitor) + return NULL; + + while(monitor->is_running) { + node_s *current_node = sync_queue_wait_pop(&monitor->aoa_data_queue); + if (current_node == NULL) + continue; + if (current_node->data == NULL) { + free(current_node); + continue; + } + + aoa_data_s *aoa_data = (aoa_data_s *)current_node->data; + __write_aoa_data(monitor->report_position_fd, aoa_data); + } + + return NULL; +} + +int start_monitor(monitor_ctx_s *monitor) +{ + int pthread_create_ret = 0; + + __create_data_directory(); + monitor->report_position_fd = __open_file_with_data_time(); + if (monitor->report_position_fd == -1) { + printf("Failed to open file\n"); + return -1; + } + + monitor->is_running = true; + pthread_create_ret = pthread_create(&monitor->file_writer_thread, NULL, __file_writer, monitor); + if (pthread_create_ret != 0) { + printf("Failed to create pthread\n"); + return -1; + } + + return 0; +} + +void stop_monitor(monitor_ctx_s *monitor) +{ + int ret; + monitor->is_running = false; + sync_queue_deinit(&monitor->aoa_data_queue); + ret = pthread_join(monitor->file_writer_thread, NULL); + printf("File writer thread returned with %d\n", ret); + + return; +} + +void deinit_monitor_ctx(monitor_ctx_s *monitor) +{ + if (monitor->report_position_fd != -1) { + close(monitor->report_position_fd); + monitor->report_position_fd = -1; + } + monitor->is_running = false; +} enum { CMD_QUIT, @@ -45,6 +315,8 @@ enum { CMD_SEND_MESSAGE_TO, CMD_GET_CONFIGURATION, CMD_SET_CONFIGURATION, + CMD_START_MONITOR_NODES, + CMD_STOP_MONITOR_NODES, CMD_INVALID, }; @@ -85,6 +357,10 @@ char *g_menu_str[] = { = "GET_CONFIGURATION", [CMD_SET_CONFIGURATION] = "SET_CONFIGURATION", + [CMD_START_MONITOR_NODES] + = "START_MONITOR_NODES", + [CMD_STOP_MONITOR_NODES] + = "STOP_MONITOR_NODES", [CMD_INVALID] = NULL, }; @@ -617,6 +893,149 @@ out: free(string_data); } +static int __start_uwb(void) +{ + int ret; + ret = uwb_initialize(); + RETV_IF(ret != UWB_ERROR_NONE, -1); + + ret = uwb_network_enable(); + RETV_IF(ret != UWB_ERROR_NONE, -1); + + ret = uwb_location_engine_start("mqtt.eclipse.org", 1883); + RETV_IF(ret != UWB_ERROR_NONE, -1); + + return 0; +} + +static bool __uwb_network_foreach_remote_node_cb( + uwb_node_h remote_node, void *user_data) +{ + uint64_t node_id; + int range, aoa, pdoa; + + if (remote_node == NULL || user_data == NULL) + return true; + + uwb_node_get_node_id(remote_node, &node_id); + uwb_node_get_range(remote_node, &range); + uwb_node_get_aoa(remote_node, &aoa); + uwb_node_get_pdoa(remote_node, &pdoa); + printf("AoA related Node ID [%lld] range: %d aoa: %d pdoa: %d\n", node_id, range, aoa, pdoa); + + monitor_ctx_s *monitor = (monitor_ctx_s *)user_data; + aoa_data_s *data = (aoa_data_s *)calloc(1, sizeof(aoa_data_s)); + if (!data) + return true; + + data->node_id = node_id; + data->range = range; + data->aoa = aoa; + data->pdoa = pdoa; + + sync_queue_push(&monitor->aoa_data_queue, data, free_aoa_data); + + return true; +} + +static void __run_update_node_timer(monitor_ctx_s *monitor_ctx); + +static void __uwb_get_network_finished_cb(int result, + uwb_network_h network, void *user_data) +{ + int ret; + if (result == UWB_ERROR_NONE) { + ret = uwb_network_foreach_remote_node(network, + __uwb_network_foreach_remote_node_cb, user_data); + if (ret != UWB_ERROR_NONE) + printf("uwb_network_foreach_remote_node failed : %d\n", ret); + } else { + printf("failed error %d\n", ret); + } + + uwb_network_destroy(network); + __run_update_node_timer((monitor_ctx_s *)user_data); +} + +static gboolean __update_nodes(gpointer user_data) +{ + int ret; + monitor_ctx_s * monitor_ctx = (monitor_ctx_s *)user_data; + if (!monitor_ctx || !monitor_ctx->is_running) { + return FALSE; + } + + ret = uwb_get_network(__uwb_get_network_finished_cb, (void *)user_data); + if (ret != UWB_ERROR_NONE) + printf("uwb_get_network failed : %d\n", ret); + + return FALSE; +} + +static void __stop_uwb(void) +{ + int ret; + ret = uwb_location_engine_stop(); + __print_result(ret, "uwb_location_engine_stop"); + + ret = uwb_network_disable(); + __print_result(ret, "uwb_network_disable"); + + ret = uwb_deinitialize(); + __print_result(ret, "uwb_deinitialize"); +} + +static guint g_update_node_timer; + +static void __run_update_node_timer(monitor_ctx_s *monitor_ctx) +{ + g_update_node_timer = g_timeout_add_seconds(DEFAULT_NODE_UPDATE_INTERVAL, + __update_nodes, (gpointer)monitor_ctx); +} + +static void __stop_update_node_timer(void) +{ + if (g_update_node_timer > 0) { + g_source_remove(g_update_node_timer); + g_update_node_timer = 0; + } +} + +monitor_ctx_s g_monitor_ctx; + +void test_start_monitor_nodes(void) +{ + + if (__start_uwb() != 0) { + printf("Starting UWB is failed\n"); + return; + } + + if (init_monitor_ctx(&g_monitor_ctx) != 0) { + printf("Failed to initialize monitor\n"); + __stop_uwb(); + return; + } + + if (start_monitor(&g_monitor_ctx) != 0) { + printf("Failed to start monitor\n"); + __stop_uwb(); + return; + } + __run_update_node_timer(&g_monitor_ctx); + + return; +} + +void test_stop_monitor_nodes(void) +{ + __stop_update_node_timer(); + stop_monitor(&g_monitor_ctx); + deinit_monitor_ctx(&g_monitor_ctx); + __stop_uwb(); + + return; +} typedef void (*test_func)(void); test_func g_menu_func[] = { @@ -658,6 +1077,10 @@ test_func g_menu_func[] = { = test_get_configuration, [CMD_SET_CONFIGURATION] = test_set_configuration, + [CMD_START_MONITOR_NODES] + = test_start_monitor_nodes, + [CMD_STOP_MONITOR_NODES] + = test_stop_monitor_nodes, [CMD_INVALID] = NULL, }; -- 2.34.1