works

diff --git a/src/dev_smag.c b/src/dev_smag.c
index f96ae5d..5d0e6cd 100644 (file)
--- a/src/dev_smag.c
+++ b/src/dev_smag.c
@@ -76,16 +76,20 @@ static int opendev(const char *dev)
                return -1;      /* failed to get a response */
        }
 
-       ser_printf(fd, "vQ\r");
+       ser_block(fd);
 
-       while(ser_getline_block(fd, buf, sizeof buf) && buf[0] != 'v');
+       ser_printf(fd, "vQ\r");
+       do {
+               ser_getline(fd, buf, sizeof buf);
+       } while(buf[0] != 'v');
 
-       printf("DBG: \"%s\"\n", buf);
        if(buf[0] != 'v' || !strstr(buf, "MAGELLAN")) {
                fprintf(stderr, "unknown device: \"%s\"\n", buf + 1);
                ser_close(fd);
                return -1;
        }
+
+       ser_nonblock(fd);
        return fd;
 }
 

diff --git a/src/main.c b/src/main.c
index e5a87d8..a936a74 100644 (file)
--- a/src/main.c
+++ b/src/main.c
@@ -317,13 +317,13 @@ int handle_dev_event(device_event *ev)
                        float axis_len = sqrt(RX(ev) * RX(ev) + RY(ev) * RY(ev) + RZ(ev) * RZ(ev));
                        if(axis_len != 0.0) {
                                rot = quat_rotate(rot, axis_len * 0.001, -RX(ev) / axis_len,
-                                               -RY(ev) / axis_len, RZ(ev) / axis_len);
+                                               -RY(ev) / axis_len, -RZ(ev) / axis_len);
                        }
                }
 
                pos.x += TX(ev) * 0.001;
                pos.y += TY(ev) * 0.001;
-               pos.z -= TZ(ev) * 0.001;
+               pos.z += TZ(ev) * 0.001;
                redisplay = 1;
                break;
 

diff --git a/src/serial.h b/src/serial.h
index f920461..d21b60d 100644 (file)
--- a/src/serial.h
+++ b/src/serial.h
@@ -8,6 +8,9 @@
 int ser_open(const char *port, int baud, unsigned int mode);
 void ser_close(int fd);
 
+int ser_block(int fd);
+int ser_nonblock(int fd);
+
 int ser_pending(int fd);
 /* if msec < 0: wait for ever */
 int ser_wait(int fd, long msec);
@@ -17,6 +20,5 @@ int ser_read(int fd, char *buf, int count);
 
 void ser_printf(int fd, const char *fmt, ...);
 char *ser_getline(int fd, char *buf, int bsz);
-char *ser_getline_block(int fd, char *buf, int bsz);
 
 #endif /* SERIAL_H_ */

diff --git a/src/unix/serial.c b/src/unix/serial.c
index cdcdb88..22978e4 100644 (file)
--- a/src/unix/serial.c
+++ b/src/unix/serial.c
@@ -22,11 +22,10 @@ int ser_open(const char *port, int baud, unsigned int mode)
                return -1;
        }
 
-       if((fd = open(port, O_RDWR | O_NONBLOCK | O_NOCTTY)) == -1) {
+       if((fd = open(port, O_RDWR | O_NOCTTY)) == -1) {
                return -1;
        }
 
-       /*memset(&term, 0, sizeof term);*/
        tcgetattr(fd, &term);
 
        term.c_oflag = 0;
@@ -34,10 +33,13 @@ int ser_open(const char *port, int baud, unsigned int mode)
        term.c_cc[VMIN] = 0;
        term.c_cc[VTIME] = 0;
 
-       term.c_cflag = CLOCAL | CREAD | CS8 | HUPCL | CRTSCTS;
+       term.c_cflag = CLOCAL | CREAD | CS8 | HUPCL;
        if(mode & SER_8N2) {
                term.c_cflag |= CSTOPB;
        }
+       if(mode & SER_HWFLOW) {
+               term.c_cflag |= CRTSCTS;
+       }
 
        term.c_iflag = IGNBRK | IGNPAR;
 
@@ -50,7 +52,9 @@ int ser_open(const char *port, int baud, unsigned int mode)
                return -1;
        }
 
-       if(mode & SER_HWFLOW) {
+#ifdef TIOCM_RTS
+       /* assert DTR/RTS lines */
+       {
                int st;
                if(ioctl(fd, TIOCMGET, &st) == -1) {
                        perror("ser_open: failed to get modem status");
@@ -64,6 +68,7 @@ int ser_open(const char *port, int baud, unsigned int mode)
                        return -1;
                }
        }
+#endif
 
        return fd;
 }
@@ -73,6 +78,16 @@ void ser_close(int fd)
        close(fd);
 }
 
+int ser_block(int fd)
+{
+       return fcntl(fd, F_SETFL, fcntl(fd, F_GETFL) & ~O_NONBLOCK);
+}
+
+int ser_nonblock(int fd)
+{
+       return fcntl(fd, F_SETFL, fcntl(fd, F_GETFL) | O_NONBLOCK);
+}
+
 int ser_pending(int fd)
 {
        static struct timeval tv_zero;
@@ -136,7 +151,7 @@ char *ser_getline(int fd, char *buf, int bsz)
 {
        static char linebuf[512];
        static int widx;
-       int i, rd, size;
+       int i, rd, size, offs;
 
        size = sizeof linebuf - widx;
        while(size && (rd = read(fd, linebuf + widx, size)) > 0) {
@@ -144,28 +159,23 @@ char *ser_getline(int fd, char *buf, int bsz)
                size -= rd;
        }
 
+       linebuf[widx] = 0;
+
        for(i=0; i<widx; i++) {
                if(linebuf[i] == '\r' || linebuf[i] == '\n') {
                        size = i >= bsz ? bsz - 1 : i;
                        memcpy(buf, linebuf, size);
                        buf[size] = 0;
 
-                       memmove(linebuf, linebuf + i + 1, sizeof linebuf - i - 1);
-                       printf("ser_getline-> \"%s\"\n", buf);
+                       offs = i + 1;
+                       memmove(linebuf, linebuf + offs, widx - offs);
+                       widx -= offs;
                        return buf;
                }
        }
        return 0;
 }
 
-char *ser_getline_block(int fd, char *buf, int bsz)
-{
-       if(!ser_wait(fd, -1)) {
-               return 0;
-       }
-       return ser_getline(fd, buf, bsz);
-}
-
 static int baud_id(int baud)
 {
        switch(baud) {