1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
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);
|
