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 From 464b5b045ceb8dab976c0e4232f4112ad0db48bb Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 25 Apr 2018 23:31:24 -0400 Subject: Fix LibSurvive poser for use with a single point (when you're totally strapped) --- src/poser_charlesrefine.c | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 12c2b9c..5357600 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,8 @@ 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) { + printf( ":::%d\n", validpoints ); + if (validpoints > 1) { LinmathQuat correction; quatcopy(correction, LinmathQuat_Identity); for (i = 0; i < pts; i++) { -- cgit v1.2.3 From 5d4f2e2d9eb0548d880b130cc15733662fc57158 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 25 Apr 2018 23:40:13 -0400 Subject: remove printf, process multiple UDP packets per slice. --- src/poser_charlesrefine.c | 1 - src/survive_driver_udp.c | 5 +++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 5357600..b35a878 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -350,7 +350,6 @@ 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. - printf( ":::%d\n", validpoints ); if (validpoints > 1) { LinmathQuat correction; quatcopy(correction, LinmathQuat_Identity); diff --git a/src/survive_driver_udp.c b/src/survive_driver_udp.c index 9c0904a..8000749 100644 --- a/src/survive_driver_udp.c +++ b/src/survive_driver_udp.c @@ -37,6 +37,8 @@ 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"); @@ -53,8 +55,7 @@ static int UDP_poll(struct SurviveContext *ctx, void *_driver) { 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: -- cgit v1.2.3