From 35db8cc98e365b2cf01cf5deddee3ff3ec9c4851 Mon Sep 17 00:00:00 2001 From: Abhijeet Date: Wed, 25 Apr 2018 23:17:27 -0400 Subject: Trying to connect to libsurvive --- Makefile | 14 ++--- src/survive_driver_udp.c | 155 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 161 insertions(+), 8 deletions(-) create mode 100644 src/survive_driver_udp.c diff --git a/Makefile b/Makefile index 2f9063e..6a7dcd7 100644 --- a/Makefile +++ b/Makefile @@ -33,10 +33,10 @@ endif SBA:=redist/sba/sba_chkjac.c redist/sba/sba_crsm.c redist/sba/sba_lapack.c redist/sba/sba_levmar.c redist/sba/sba_levmar_wrap.c redist/minimal_opencv.c src/poser_epnp.c src/poser_sba.c src/epnp/epnp.c src/poser_general_optimizer.c LIBSURVIVE_CORE+=src/survive.c src/survive_process.c src/ootx_decoder.c src/survive_driverman.c src/survive_default_devices.c src/survive_playback.c src/survive_config.c src/survive_cal.c src/poser.c src/survive_sensor_activations.c src/survive_disambiguator.c src/survive_imu.c src/survive_kalman.c -MINIMAL_NEEDED+=src/survive_usb.c src/survive_charlesbiguator.c src/survive_vive.c src/survive_reproject.c -AUX_NEEDED+=src/survive_turveybiguator.c src/survive_statebased_disambiguator.c src/survive_driver_dummy.c +MINIMAL_NEEDED+=src/survive_usb.c src/survive_charlesbiguator.c src/survive_vive.c src/survive_reproject.c +AUX_NEEDED+=src/survive_turveybiguator.c src/survive_statebased_disambiguator.c src/survive_driver_dummy.c src/survive_driver_udp.c POSERS:=src/poser_dummy.c src/poser_imu.c src/poser_charlesrefine.c -EXTRA_POSERS:=src/poser_daveortho.c src/poser_charlesslow.c src/poser_octavioradii.c src/poser_turveytori.c +EXTRA_POSERS:=src/poser_daveortho.c src/poser_charlesslow.c src/poser_octavioradii.c src/poser_turveytori.c REDISTS:=redist/json_helpers.c redist/linmath.c redist/jsmn.c @@ -86,16 +86,16 @@ test : test.c $(LIBRARY) $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS_TOOLS) simple_pose_test : simple_pose_test.c $(DRAWFUNCTIONS) $(LIBRARY) - $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS_TOOLS) + $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS_TOOLS) data_recorder : data_recorder.c $(LIBRARY) - $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS_TOOLS) + $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS_TOOLS) calibrate : calibrate.c $(DRAWFUNCTIONS) $(LIBRARY) $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS_TOOLS) calibrate_client : calibrate_client.c $(GRAPHICS_LOFI) $(LIBRARY) - $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS_TOOLS) + $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS_TOOLS) #### Testers. @@ -189,5 +189,3 @@ help : @echo " clean Erase build and incremental files." @echo " buildfolders Produce build file structure." @echo " $(LIBRARY) Produce libsurvive.so" - - 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 +#include +#include +#include +#include +#include +#include +#include +#include +#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); -- cgit v1.2.3