implement bt_connect threading

Examples/Linux/drone_control/Sources/bt_connect.c

+#include <stdio.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <sys/time.h>
+#include <time.h>
+
+#include <config.h>
+#include <VP_Os/vp_os_print.h>
+
+#include <sys/socket.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/rfcomm.h>
+
+#include "bt_connect.h"
+
+char bt_data[2048];
+
+void update_data(char *line) {
+    memcpy(bt_data, line, sizeof(bt_data));
+    printf("received: [%s]\n", line);
+}
+
+bdaddr_t find_client(char *target) {
+    inquiry_info *ii = NULL;
+    int max_rsp, num_rsp;
+    int dev_id, sock, len, flags;
+    int i;
+    char addr[19] = { 0 };
+    char name[248] = { 0 };
+    bdaddr_t target_channel = {0};
+
+    dev_id = hci_get_route(NULL);
+    sock = hci_open_dev( dev_id );
+    if (dev_id < 0 || sock < 0) {
+        perror("opening socket");
+        exit(1);
+    }
+
+    len  = 8;
+    max_rsp = 255;
+    flags = IREQ_CACHE_FLUSH;
+    ii = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));
+    
+    num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
+    if( num_rsp < 0 ) perror("hci_inquiry");
+
+    for (i = 0; i < num_rsp; i++) {
+        target_channel = (ii+i)->bdaddr;
+        ba2str(&target_channel, addr);
+        memset(name, 0, sizeof(name));
+        if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), 
+                                 name, 0) < 0)
+            strcpy(name, "[unknown]");
+        int j = strlen(name);
+        while (name[j-1] == ' ' || name[j-1] == '\n' || name[j-1] == '\r')
+            name[--j] = '\0';
+        if (strcmp(name, target))
+            printf("skipped:  %s  %s\n", addr, name);
+        else {
+            printf("find:     %s  %s\n", addr, name);
+            break;
+        }
+    }
+
+    free( ii );
+    close( sock );
+    return target_channel;
+}
+
+DEFINE_THREAD_ROUTINE( bluetooth_connect, data )
+{
+    printf("\n Bluetooth connection init\n\n");
+
+    struct sockaddr_rc addr = {
+        AF_BLUETOOTH, // rc_family
+        0, //find_client("dronermctrl"), // rc_bdaddr
+        (uint8_t) 1 // rc_channel
+    };
+
+    char dest[18] = "98:D3:31:70:26:22";
+    str2ba( dest, &addr.rc_bdaddr );
+
+    int s, status, bytes_read, buf_end = 0, i;
+    char buf[2048] = {0}, bak[2048] = {0};
+
+    // allocate a socket
+    s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+
+    // connect to server
+    status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
+    if( status < 0 )
+        perror("connect failed");
+    
+    while (1) {
+        bytes_read = read(s, buf + buf_end, sizeof(buf));
+        buf_end += bytes_read;
+        for (i = buf_end - bytes_read; i < buf_end; ++i)
+            if (buf[i] == '\n') {
+                buf[i-1] = '\0';
+                update_data(buf);
+                memcpy(bak, buf, sizeof(buf));
+                memset(buf, 0, sizeof(buf));
+                memcpy(buf, bak + i + 1, sizeof(buf) - i - 1);
+                buf_end -= i + 1;
+            }
+    }
+
+    close(s);
+
+    PRINT("Bluetooth connect thread ended\n\n");
+
+    return (THREAD_RET)0;
+}
\ No newline at end of file

Examples/Linux/drone_control/Sources/bt_connect.h

+#ifndef _BT_CONNECT_H_
+#define _BT_CONNECT_H_
+
+#include <config.h>
+#include <VP_Api/vp_api_thread_helper.h>
+
+extern char bt_data[2048];
+
+PROTO_THREAD_ROUTINE( bluetooth_connect, data );
+
+#endif // _BT_CONNECT_H_
\ No newline at end of file

Examples/Linux/drone_control/Sources/main.c

@@ -25,8 +25,9 @@
 #include <VP_Os/vp_os_signal.h>
 
 //Local project
-#include <Control/control.h>
-#include <Video/video_stage.h>
+#include "control.h"
+#include "video_stage.h"
+#include "bt_connect.h"
 
 static int32_t exit_ihm_program = 1;
 
@@ -76,6 +77,7 @@ C_RESULT ardrone_tool_init_custom(void)
 
   /* Start all threads of your application */
   START_THREAD( video_stage, NULL );
+  START_THREAD( bluetooth_connect, NULL );
   
   return C_OK;
 }
@@ -85,6 +87,7 @@ C_RESULT ardrone_tool_shutdown_custom(void)
 {
   /* Relinquish all threads of your application */
   JOIN_THREAD( video_stage );
+  JOIN_THREAD( bluetooth_connect );
 
   /* Unregistering for the current device */
   ardrone_tool_input_remove(&control_device);
@@ -102,6 +105,7 @@ bool_t ardrone_tool_exit()
 BEGIN_THREAD_TABLE
   THREAD_TABLE_ENTRY( ardrone_control, 20 )
   THREAD_TABLE_ENTRY( navdata_update, 20 )
+  THREAD_TABLE_ENTRY( bluetooth_connect, 20 )
   THREAD_TABLE_ENTRY( video_stage, 20 )
 END_THREAD_TABLE
 

Examples/Linux/drone_control/Sources/video_stage.c

@@ -44,7 +44,7 @@
 
 #include <ardrone_tool/Video/video_com_stage.h>
 
-#include "Video/video_stage.h"
+#include "video_stage.h"
 
 #define NB_STAGES 10