Merge pull request #130 from abhijeetvhotkar/master

Adding UDP support

2 files changed, 171 insertions, 2 deletions

diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c
index 12c2b9c..b35a878 100644
--- a/src/poser_charlesrefine.c
+++ b/src/poser_charlesrefine.c
@@ -166,12 +166,25 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 		FLT dist = dot3d(sensorpos_rel_lh, sweep_normal);
 
 		if ((i = dd->ptsweep) < MAX_PT_PER_SWEEP) {
+			int repeat = 0;
+			int k;
+
+			//Detect repeated hits.  a rare problem but can happen with lossy sources of pose.
+			for( k = 0; k < dd->ptsweep; k++ )
+			{
+				if( dd->sensor_ids[k] == sensor_id )
+				{
+					repeat = 1;
+					i = k;
+				}
+			}
 			memcpy(dd->normal_at_errors[i], sweep_normal, sizeof(FLT) * 3);
 			dd->quantity_errors[i] = dist;
 			dd->angles_at_pts[i] = angle;
 			dd->sensor_ids[i] = sensor_id;
 			memcpy(&dd->object_pose_at_hit[i], &dd->InteralPoseUsedForCalc, sizeof(SurvivePose));
-			dd->ptsweep++;
+			if( !repeat )
+				dd->ptsweep++;
 		}
 
 		dd->last_angle_lh_axis[lhid][axis] = inangle;
@@ -337,7 +350,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 			// Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov.
 			// If we were able to determine likliehood of a hit in the sweep instead of afterward
 			// we would actually be able to perform this on a per-hit basis.
-			if (1) {
+			if (validpoints > 1) {
 				LinmathQuat correction;
 				quatcopy(correction, LinmathQuat_Identity);
 				for (i = 0; i < pts; i++) {
diff --git a/src/survive_driver_udp.c b/src/survive_driver_udp.c
new file mode 100644
index 0000000..8000749
--- /dev/null
+++ b/src/survive_driver_udp.c
@@ -0,0 +1,156 @@
+// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL
+// or LGPL licenses.
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <survive.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <time.h>
+#include "survive_config.h"
+#include "os_generic.h"
+
+#define EXAMPLE_PORT 2333
+#define EXAMPLE_GROUP "226.5.1.32"
+
+struct __attribute__((packed)) sendbuf_t{
+		uint32_t header;
+		uint8_t dev_no;
+		uint16_t sensor_no;
+		uint16_t length_event;
+		uint32_t ccount_struc;
+} sendbuf;
+
+struct SurviveDriverUDP {
+	SurviveContext * ctx;
+	SurviveObject * so;
+	struct sockaddr_in addr;
+	int sock;
+	socklen_t addrlen;
+	struct ip_mreq mreq;
+};
+typedef struct SurviveDriverUDP SurviveDriverUDP;
+
+static int UDP_poll(struct SurviveContext *ctx, void *_driver) {
+	SurviveDriverUDP *driver = _driver;
+	int cnt;
+	for( ;; )
+	{
+		cnt = recvfrom(driver->sock, (char *) &sendbuf, sizeof(sendbuf), MSG_DONTWAIT | MSG_NOSIGNAL , (struct sockaddr *) &driver->addr, &driver->addrlen);
+		 if (cnt < 0) {
+				// perror("recvfrom");
+				//  exit(1);
+				return 0;
+				} else if (cnt == 0) {
+				return 0;
+					// break;
+				}
+		 // printf("On %s: Header - %s | Device_Number - %d | Sensor_no - %d | Event_length - %5d | Ccount - %u \n", inet_ntoa(driver->addr.sin_addr), (char *) &sendbuf.header, sendbuf.dev_no, sendbuf.sensor_no, sendbuf.length_event, sendbuf.ccount_struc);
+
+		 	LightcapElement le;
+	 		le.sensor_id = 0;		//8 bits
+	 		le.length = sendbuf.length_event * 48 / 160;		//16 bits
+	 		le.timestamp = sendbuf.ccount_struc * 48 / 160; 		//32 bits
+	 		handle_lightcap(driver->so, &le);
+	}
+
+/*
+	To emit an IMU event, send this:
+		driver->ctx->imuproc(so, mask, accelgyro, timecode, id);
+
+	To emit light data, send this:
+		LightcapElement le;
+		le.sensor_id = X		//8 bits
+		le.length = Z			//16 bits
+		le.timestamp = Y		//32 bits
+		handle_lightcap(so, &le);
+*/
+
+	return 0;
+}
+
+static int UDP_close(struct SurviveContext *ctx, void *_driver) {
+	SurviveDriverUDP *driver = _driver;
+
+/*
+	If you need to handle any cleanup here, like closing handles, etc.
+	you can perform it here.
+*/
+
+	return 0;
+}
+
+int UDP_haptic( SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount )
+{
+/*
+	If your device has haptics, you can add the control for them here.
+*/
+	return 0;
+}
+
+int DriverRegUDP(SurviveContext *ctx)
+{
+
+	int enable_UDP_driver = survive_configi( ctx, "UDP_driver_enable", SC_GET, 0 );
+
+	if( !enable_UDP_driver ) return 0;
+
+	SurviveDriverUDP *sp = calloc(1, sizeof(SurviveDriverUDP));
+	sp->ctx = ctx;
+
+	SV_INFO("Setting up UDP driver.");
+
+	//Create a new SurviveObject...
+	SurviveObject *device = calloc(1, sizeof(SurviveObject));
+	device->ctx = ctx;
+	device->driver = sp;
+	memcpy(device->codename, "UD0", 4);
+	memcpy(device->drivername, "UDP", 4);
+	device->sensor_ct = 1;
+	device->sensor_locations = malloc( sizeof(FLT)*3 );
+	device->sensor_normals = malloc( sizeof(FLT)*3 );
+	device->sensor_locations[0] = 0;
+	device->sensor_locations[1] = 0;
+	device->sensor_locations[2] = 0;
+	device->sensor_normals[0] = 0;
+	device->sensor_normals[1] = 0;
+	device->sensor_normals[2] = 1;
+
+	device->timebase_hz = 48000000;
+	device->imu_freq = 1000.0f;
+	device->haptic = UDP_haptic;
+
+	sp->so = device;
+	survive_add_object( ctx, device );
+	survive_add_driver( ctx, sp, UDP_poll, UDP_close, 0 );
+
+	sp->sock = socket(AF_INET, SOCK_DGRAM, 0);
+
+	if (sp->sock < 0) {
+		perror("socket");
+		exit(1);
+	}
+	bzero((char *)&sp->addr, sizeof(sp->addr));
+	sp->addr.sin_family = AF_INET;
+	sp->addr.sin_addr.s_addr = htonl(INADDR_ANY);
+	sp->addr.sin_port = htons(EXAMPLE_PORT);
+	sp->addrlen = sizeof(sp->addr);
+
+	 if (bind(sp->sock, (struct sockaddr *) &sp->addr, sizeof(sp->addr)) < 0) {
+				 perror("bind");
+				 exit(1);
+		 }
+		 sp->mreq.imr_multiaddr.s_addr = inet_addr(EXAMPLE_GROUP);
+		 sp->mreq.imr_interface.s_addr = htonl(INADDR_ANY);
+	 if (setsockopt(sp->sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &sp->mreq, sizeof(sp->mreq)) < 0) {
+				perror("setsockopt mreq");
+				exit(1);
+		 }
+
+	return 0;
+}
+
+REGISTER_LINKTIME(DriverRegUDP);