1 files changed, 68 insertions, 0 deletions
diff --git a/lib.cpp b/lib.cpp
new file mode 100644
index 0000000..fbbf338
--- /dev/null
+++ b/lib.cpp
@@ -0,0 +1,68 @@
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/socket.h>
+#include <string.h>
+#include <sys/un.h>
+#include <time.h>
+
+#include "lib.h"
+
+using namespace cv;
+
+#define BUFSIZE 67108864
+
+static void die(const char* msg){perror(msg); exit(1);}
+static void suicide(const char* msg){ fprintf(stderr, "%s\n", msg); exit(1); }
+
+static int read_completely(int fd, void* buf, size_t len)
+{
+	size_t n_read;
+	for (n_read = 0; n_read < len;)
+	{
+		size_t tmp = read(fd, buf, len-n_read);
+		n_read+=tmp;
+		buf = ((char*)buf)+tmp;
+	}
+	return n_read;
+}
+
+
+DroneConnection::DroneConnection(const char* sockpath)
+{
+	struct sockaddr_un my_sockaddr;
+	buffer = new unsigned char[BUFSIZE];
+
+	my_sockaddr.sun_family=AF_UNIX;
+	strcpy(my_sockaddr.sun_path, sockpath);
+	int sockaddrlen = strlen(my_sockaddr.sun_path) + sizeof(my_sockaddr.sun_family);
+
+	sockfd = socket(AF_UNIX, SOCK_STREAM, 0);
+	if (sockfd == -1) die("socket");
+
+
+	if (connect(sockfd, (struct sockaddr*) &my_sockaddr, sockaddrlen) == -1)
+		die("connect");
+}
+
+DroneConnection::~DroneConnection()
+{
+	close(sockfd);
+	delete [] buffer;
+}
+
+void DroneConnection::get(Mat& frame, navdata_t* nav)
+{
+	write(sockfd,"get\n",4);
+
+	read_completely(sockfd, buffer, 4);
+	int framelen = ((buffer[0]*256+buffer[1])*256+buffer[2])*256+buffer[3];
+	if (framelen + sizeof(navdata_t) > BUFSIZE) suicide("buffer too small");
+	
+	read_completely(sockfd, buffer, framelen+sizeof(navdata_t));
+	
+	const navdata_t* navdata = (navdata_t*)(buffer + framelen);
+	memcpy(nav, navdata, sizeof(*navdata));
+
+	frame = Mat(768,1280,CV_8UC3, buffer);
+}