aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorAbhijeet <abv1@umbc.edu>2018-04-25 23:17:27 -0400
committerAbhijeet <abv1@umbc.edu>2018-04-25 23:17:27 -0400
commit35db8cc98e365b2cf01cf5deddee3ff3ec9c4851 (patch)
treee7e9fd136021c1e2c3d8b7174e2f603400c4c629 /src
parent824811185187d2941eff054ec7be1aa2aa6ace50 (diff)
downloadlibsurvive-35db8cc98e365b2cf01cf5deddee3ff3ec9c4851.tar.gz
libsurvive-35db8cc98e365b2cf01cf5deddee3ff3ec9c4851.tar.bz2
Trying to connect to libsurvive
Diffstat (limited to 'src')
-rw-r--r--src/survive_driver_udp.c155
1 files changed, 155 insertions, 0 deletions
diff --git a/src/survive_driver_udp.c b/src/survive_driver_udp.c
new file mode 100644
index 0000000..9c0904a
--- /dev/null
+++ b/src/survive_driver_udp.c
@@ -0,0 +1,155 @@
+// 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;
+ 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);