#include <time.h>
#include <assert.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <fcntl.h>
+
+#include <pthread.h>
+
#include <glib.h>
#include <glib-object.h>
} \
} 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,
CMD_SEND_MESSAGE_TO,
CMD_GET_CONFIGURATION,
CMD_SET_CONFIGURATION,
+ CMD_START_MONITOR_NODES,
+ CMD_STOP_MONITOR_NODES,
CMD_INVALID,
};
= "GET_CONFIGURATION",
[CMD_SET_CONFIGURATION]
= "SET_CONFIGURATION",
+ [CMD_START_MONITOR_NODES]
+ = "START_MONITOR_NODES",
+ [CMD_STOP_MONITOR_NODES]
+ = "STOP_MONITOR_NODES",
[CMD_INVALID]
= NULL, };
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[] = {
= 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, };
projects / platform / core / api / uwb.git / commitdiff