00001
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <stdio.h>
00038 #include <assert.h>
00039 #include <stdlib.h>
00040 #include <string.h>
00042 #include <nsnet.h>
00043 #include <nsutil.h>
00045 #include <nsser.h>
00047 #include <openedf.h>
00049 #include <config.h>
00051 #define PROTOWINDOW 17
00052
00053 char buf[PROTOWINDOW];
00054 sock_t sock_fd;
00055 int bufCount;
00056 static int failCount = 0;
00057 static struct OutputBuffer ob;
00058 static struct InputBuffer ib;
00059
00060 int hasMatchedProtocol;
00061 int isP2, isP3;
00062 int isValidPacket(unsigned short nchan, unsigned short *samples);
00063 int doesMatchP3(unsigned char c, unsigned short *vals,int *nsamples);
00064 int doesMatchP2(unsigned char c, unsigned short *vals,int *nsamples);
00065 int mGetOK(sock_t fd, struct InputBuffer *ib);
00066
00067
00068 int mGetOK(sock_t fd, struct InputBuffer *ib)
00069 {
00070 return 0;
00071 }
00072
00073 static struct EDFDecodedConfig modEEGCfg = {
00074 { 0,
00075 -1,
00076 2,
00077 "0",
00078 "Mr. Default Patient",
00079 "Ms. Default Recorder",
00080 "date",
00081 "time",
00082 "manufacturer",
00083 1.0,
00084 }, {
00085 {
00086 256,
00087 -512, 512,
00088 0, 1023,
00089 "electrode 0",
00090 "metal electrode",
00091 "uV",
00092 "LP:59Hz",
00093 ""
00094 },
00095 {
00096 256, -512, 512, 0, 1023,
00097 "electrode 1", "metal electrode", "uV", "LP:59Hz", ""
00098 }
00099 }
00100 };
00101
00102 static struct EDFDecodedConfig current;
00103
00104 int isValidPacket(unsigned short chan, unsigned short *samples)
00105 {
00106 int i;
00107 if (chan != 2 && chan != 4 && chan != 6) return 0;
00108 for (i = 0; i < chan; i++) {
00109 if (samples[i] > 1023) return 0;
00110 }
00111 return 1;
00112 }
00113
00114 void resetBuffer(void)
00115 {
00116 bufCount = 0;
00117 }
00118
00119 void gobbleChars(int howMany)
00120 {
00121 assert(howMany <= bufCount);
00122 if (bufCount > howMany)
00123 memmove(buf, buf+howMany, bufCount-howMany);
00124 bufCount -= howMany;
00125 }
00126
00127 void eatCharacter(unsigned char c)
00128 {
00129 #define MINGOOD 6
00130 int oldHasMatched;
00131 static int goodCount;
00132 unsigned short vals[MAXCHANNELS];
00133 int ns;
00134 int didMatch = 0;
00135 oldHasMatched = hasMatchedProtocol;
00136 if (bufCount == PROTOWINDOW - 1)
00137 memmove(buf, buf+1, PROTOWINDOW - 1);
00138 buf[bufCount] = c;
00139 if (bufCount < PROTOWINDOW - 1)
00140 bufCount += 1;
00141 if (hasMatchedProtocol) {
00142 if (isP2)
00143 didMatch = doesMatchP2(c, vals, &ns);
00144 if (isP3)
00145 didMatch = doesMatchP3(c, vals, &ns);
00146 } else {
00147 if ( (didMatch = doesMatchP2(c, vals, &ns)) ) {
00148 if (goodCount > MINGOOD) {
00149 hasMatchedProtocol = 1;
00150 isP2 = 1;
00151 }
00152 } else {
00153 if ( (didMatch = doesMatchP3(c, vals, &ns)) ) {
00154 if (goodCount > MINGOOD) {
00155 hasMatchedProtocol = 1;
00156 isP3 = 1;
00157 }
00158 }
00159 }
00160 }
00161 if (didMatch) {
00162 char *pstr = "xx";
00163 if (isValidPacket(ns, vals)) {
00164 goodCount += 1;
00165 }
00166 else {
00167 rprintf("Warning: invalid serial packet ahead!\n");
00168 }
00169 if (isP2)
00170 pstr = "P2";
00171 if (isP3)
00172 pstr = "P3";
00173
00174 failCount = 0;
00175
00176
00177
00178
00179
00180 }
00181 else {
00182 failCount += 1;
00183 if (failCount % PROTOWINDOW == 0) {
00184 rprintf("Serial packet sync error -- missed window.\n");
00185 goodCount = 0;
00186 }
00187 }
00188 }
00189
00190 void handleSamples(int packetCounter, int chan, unsigned short *vals)
00191 {
00192 char buf[MAXLEN];
00193 int bufPos = 0;
00194 int i;
00195
00196 if (chan > current.hdr.dataRecordChannels)
00197 chan = current.hdr.dataRecordChannels;
00198 bufPos += sprintf(buf+bufPos, "! %d %d", packetCounter, chan);
00199 for (i = 0; i < chan; ++i)
00200 bufPos += sprintf(buf+bufPos, " %d", vals[i]);
00201 bufPos += sprintf(buf+bufPos, "\r\n");
00202 writeString(sock_fd, buf, &ob);
00203 mGetOK(sock_fd, &ib);
00204 }
00205
00206 int doesMatchP3(unsigned char c, unsigned short *vals,int *nsamples)
00207 {
00208 static int i = 0;
00209 static int j = 0;
00210 static int packetCounter = 0;
00211 static int auxChan = 0;
00212 static short samples[MAXCHANNELS];
00213 static int needsHeader = 1;
00214 static int needsAux;
00215 if (needsHeader) {
00216 if (c & 0x80) goto resetMachine;
00217 packetCounter = c >> 1;
00218 auxChan = c & 1;
00219 needsHeader = 0;
00220 needsAux = 1;
00221 return 0;
00222 }
00223 if (needsAux) {
00224 if (c & 0x80) goto resetMachine;
00225 auxChan += (int) c * 2;
00226 needsAux = 0;
00227 return 0;
00228 }
00229 if (2*i < MAXCHANNELS) {
00230 switch (j) {
00231 case 0: samples[2*i] = (int) c; break;
00232 case 1: samples[2*i+1] = (int) c; break;
00233 case 2:
00234 samples[2*i] += (int) (c & 0x70) << 3;
00235 samples[2*i+1] += (int) (c & 0x07) << 7;
00236 i += 1;
00237 break;
00238 }
00239 }
00240 j = (j+1) % 3;
00241 if (c & 0x80) {
00242 if (j == 0) {
00243 int k;
00244 memcpy(vals, samples, sizeof(vals[0]) * i * 2);
00245 *nsamples = i * 2;
00246 for (k = 0; k < *nsamples; ++k)
00247 if (vals[k] > 1023)
00248 goto resetMachine;
00249 i = 0;
00250 j = 0;
00251 needsHeader = 1;
00252 handleSamples(packetCounter, *nsamples, vals);
00253 return 1;
00254 }
00255 else {
00256 if (isP3)
00257 rprintf("P3 sync error:i=%d,j=%d,c=%d.\n",i,j,c);
00258 }
00259 goto resetMachine;
00260 }
00261 return 0;
00262
00263 resetMachine:
00264 i = 0;
00265 j = 0;
00266 needsHeader = 1;
00267 return 0;
00268 }
00269
00270 int doesMatchP2(unsigned char c, unsigned short *vals,int *nsamples)
00271 {
00272 #define P2CHAN 6
00273 enum P2State { waitingA5, waiting5A, waitingVersion, waitingCounter,
00274 waitingData, waitingSwitches };
00275 static int packetCounter = 0;
00276 static int chan = 0;
00277 static int i = 0;
00278 static enum P2State state = waitingA5;
00279 static unsigned short s[MAXCHANNELS];
00280 switch (state) {
00281 case waitingA5:
00282 if (c == 0xa5) {
00283 failCount = 0;
00284 state = waiting5A;
00285 }
00286 else {
00287 failCount += 1;
00288 if (failCount % PROTOWINDOW == 0) {
00289 rprintf("Sync error, packet lost.\n");
00290 }
00291 }
00292 break;
00293
00294 case waiting5A:
00295 if (c == 0x5a)
00296 state = waitingVersion;
00297 else
00298 state = waitingA5;
00299 break;
00300
00301 case waitingVersion:
00302 if (c == 0x02)
00303 state = waitingCounter;
00304 else
00305 state = waitingA5;
00306 break;
00307
00308 case waitingCounter:
00309 packetCounter = c;
00310 state = waitingData;
00311 i = 0;
00312 break;
00313
00314 case waitingData:
00315 if ((i%2) == 0)
00316 s[i/2] = c;
00317 else
00318 s[i/2] = s[i/2]* 256 + c;
00319 i += 1;
00320 if (i == P2CHAN * 2)
00321 state = waitingSwitches;
00322 break;
00323
00324 case waitingSwitches:
00325 chan = P2CHAN;
00326 chan = 2;
00327 *nsamples = chan;
00328 memcpy(vals, s, chan * sizeof(s[0]));
00329 handleSamples(packetCounter, *nsamples, vals);
00330 state = waitingA5;
00331 return 1;
00332
00333 default:
00334 rprintf("Error: unknown state in P2!\n");
00335 rexit(0);
00336 }
00337 return 0;
00338 }
00339 void printHelp()
00340 {
00341 rprintf("ModEEG Driver interfaces the ModEEG system to the NeuroServer TCP/IP protocol\n");
00342 rprintf("Usage: modeegdriver [-d serialDeviceName] [hostname] [portno]\n");
00343 rprintf("The default options are modeegdriver -d %s %s %d\n",
00344 DEFAULTDEVICE, DEFAULTHOST, DEFAULTPORT);
00345 }
00346
00347 int main(int argc, char **argv)
00348 {
00349 char responseBuf[MAXLEN];
00350 int i;
00351 int retval;
00352 ser_t serport;
00353 char EDFPacket[MAXHEADERLEN];
00354 int EDFLen = MAXHEADERLEN;
00356 char smallbuf[PROTOWINDOW];
00357 char *hostname = NULL;
00358 char *deviceName = NULL;
00360 unsigned short portno = 0;
00362 struct timeval when;
00363
00364 rprintf("ModEEG Driver v. %s-%s\n", VERSION, OSTYPESTR);
00365
00366 rinitNetworking();
00368 for (i = 1; argv[i]; ++i) {
00369 if (argv[i][0] == '-' && argv[i][1] == 'h') {
00370 printHelp();
00371 exit(0);
00372 }
00373 if (argv[i][0] == '-' && argv[i][1] == 'd') {
00374 if (argv[i+1] == NULL) {
00375 rprintf("Bad devicename option: %s\nExpected device name as next argument.\n", argv[i]);
00376 exit(1);
00377 }
00378 deviceName = argv[i+1];
00379 i += 1;
00380 continue;
00381 }
00382 if (hostname == NULL) {
00383 hostname = argv[i];
00384 continue;
00385 }
00386 if (portno == 0) {
00387 portno = atoi(argv[i]);
00388 continue;
00389 }
00390 }
00391 if (deviceName == NULL)
00392 deviceName = DEFAULTDEVICE;
00393 if (portno == 0)
00394 portno = DEFAULTPORT;
00395 if (hostname == NULL)
00396 hostname = DEFAULTHOST;
00397
00398 makeREDFConfig(¤t, &modEEGCfg);
00399 writeEDFString(¤t, EDFPacket, &EDFLen);
00400
00401
00402 sock_fd = rsocket();
00403 if (sock_fd < 0) {
00404 perror("socket");
00405 rexit(1);
00406 }
00407 setblocking(sock_fd);
00408
00409 rprintf("Attempting to connect to nsd at %s:%d\n", hostname, portno);
00410 retval = rconnectName(sock_fd, hostname, portno);
00411 if (retval != 0) {
00412 rprintf("connect error\n");
00413 rexit(1);
00414 }
00415 rprintf("Socket connected.\n");
00416 fflush(stdout);
00417
00418 serport = openSerialPort(deviceName,57600);
00419 rprintf("Serial port %s opened.\n", deviceName);
00420
00421 writeString(sock_fd, "eeg\n", &ob);
00422 mGetOK(sock_fd, &ib);
00423 writeString(sock_fd, "setheader ", &ob);
00424 writeBytes(sock_fd, EDFPacket, EDFLen, &ob);
00425 writeString(sock_fd, "\n", &ob);
00426 mGetOK(sock_fd, &ib);
00427 updateMaxFd(sock_fd);
00428 #ifndef __MINGW32__
00429 updateMaxFd(serport);
00430 #endif
00431 when.tv_usec = (1000000L / modEEGCfg.chan[0].sampleCount);
00432 rprintf("Polling at %d Hertz or %d usec\n", modEEGCfg.chan[0].sampleCount, when.tv_usec);
00433 for (;;) {
00434 int i, readSerBytes;
00435 int retval;
00436 fd_set toread;
00437 when.tv_sec = 0;
00438 when.tv_usec = (1000000L / modEEGCfg.chan[0].sampleCount);
00439 FD_ZERO(&toread);
00440 FD_SET(sock_fd, &toread);
00441 #ifndef __MINGW32__
00442 FD_SET(serport, &toread);
00443 #endif
00444 retval = rselect_timed(max_fd, &toread, NULL, NULL, &when);
00445 readSerBytes = readSerial(serport, smallbuf, PROTOWINDOW);
00446 if (readSerBytes < 0) {
00447 rprintf("Serial error.\n");
00448 }
00449 if (readSerBytes > 0) {
00450 for (i = 0; i < readSerBytes; ++i)
00451 eatCharacter(smallbuf[i]);
00452 }
00453 if (FD_ISSET(sock_fd, &toread)) {
00454 my_read(sock_fd, responseBuf, MAXLEN, &ib);
00455 }
00456 if (isEOF(sock_fd, &ib)) {
00457 rprintf("Server died, exitting.\n");
00458 exit(0);
00459 }
00460 }
00461
00462 return 0;
00463 }
00464