client lib

4 files changed, 104 insertions, 58 deletions

diff --git a/Makefile b/Makefile
index 9f87474..a917452 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,4 @@
-client2: client2.c
-	g++ client2.c `pkg-config --libs opencv` -lm  -o client2
+client2: client2.c lib.cpp
+	g++ client2.c lib.cpp `pkg-config --libs opencv` -lm  -o client2
 client: client.c
 	gcc client.c -lX11 -lXi -lXmu -lglut -lGL -lGLU -lm  -o client
diff --git a/client2.c b/client2.c
index 1e04755..1066739 100644
--- a/client2.c
+++ b/client2.c
@@ -7,63 +7,20 @@
 #include <time.h>
 
 #include <opencv2/opencv.hpp>
+#include "lib.h"
 
 using namespace cv;
 
 #define SOCKETPATH "/home/flo/uds_socket"
 
-void die(const char* msg){perror(msg); exit(1);}
-void suicide(const char* msg){ fprintf(stderr, "%s\n", msg); exit(1); }
-
-
-
-unsigned char buffer[67108864]; // must be unsigned. because reasons -_-
-
-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;
-}
-
 int main(int argc, const char** argv)
 {
-	struct sockaddr_un my_sockaddr;
-	struct navdata_t
-	{
-		double phi;
-		double theta;
-		double psi;
-		double batt;
-	};
-
-	my_sockaddr.sun_family=AF_UNIX;
-	strcpy(my_sockaddr.sun_path, SOCKETPATH);
-	int sockaddrlen = strlen(my_sockaddr.sun_path) + sizeof(my_sockaddr.sun_family);
-
-	int sockfd = socket(AF_UNIX, SOCK_STREAM, 0);
-	if (sockfd == -1) die("socket");
-
-
-	if (connect(sockfd, (struct sockaddr*) &my_sockaddr, sockaddrlen) == -1)
-		die("connect");
+	DroneConnection drone(SOCKETPATH);
 
 	printf("GO!!\n");
 	time_t t = time(NULL);
 	int nframes=0;
 
-	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) > sizeof(buffer)) suicide("buffer too small");
-	read_completely(sockfd, buffer, framelen+sizeof(navdata_t));
-	const navdata_t* navdata = (navdata_t*)(buffer + framelen);
-	printf("%lf\t%lf\t%lf\t%lf\n", navdata->phi, navdata->theta, navdata->psi, navdata->batt);
 	char key;
 	bool go=true;
 	bool new_vals=true;
@@ -93,6 +50,8 @@ int main(int argc, const char** argv)
 	//col[0]=-0.01;
 	//col[1]=0.0;
 	//col[2]=0.01;
+	
+	Mat dingens;
 
 	while ((key = waitKey(1)) != 'x')
 	{
@@ -146,18 +105,10 @@ int main(int argc, const char** argv)
 		}
 
 		if (go) {
-			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) > sizeof(buffer)) suicide("buffer too small");
-			read_completely(sockfd, buffer, framelen+sizeof(navdata_t));
-	
-			const navdata_t* navdata = (navdata_t*)(buffer + framelen);
-			printf("%lf\t%lf\t%lf\t%lf\n", navdata->phi, navdata->theta, navdata->psi, navdata->batt);
+			navdata_t navdata;
+			drone.get(dingens, &navdata);
 		}
 
-		//Mat dingens=Mat::eye(100,100,CV_8UC1) * 244;
-		Mat dingens(768,1280,CV_8UC3, buffer);
 		for (int i=0; i< 1280; i+=50)
 			dingens.col(i)=Vec3b(255,192,128);
 		for (int i=0; i< 768; i+=50)
@@ -197,6 +148,5 @@ int main(int argc, const char** argv)
 		nframes++;
 	}
 	
-	close(sockfd);
 	return 0;
 }
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);
+}
diff --git a/lib.h b/lib.h
new file mode 100644
index 0000000..da622ed
--- /dev/null
+++ b/lib.h
@@ -0,0 +1,28 @@
+#ifndef __LIB_H__
+#define __LIB_H__
+
+#include <opencv2/opencv.hpp>
+
+struct navdata_t
+{
+	double phi;
+	double theta;
+	double psi;
+	double batt;
+};
+
+
+class DroneConnection
+{
+	public:
+		DroneConnection(const char* sockpath);
+		~DroneConnection();
+		void get(cv::Mat& frame, navdata_t* navdata);
+	
+	private:
+		unsigned char* buffer;
+		int sockfd = -1;
+
+};
+
+#endif