diff --git a/main.cpp b/main.cpp
index fea5744..d69b346 100644
--- a/main.cpp
+++ b/main.cpp
@@ -33,38 +33,11 @@ static void printUsage()
  -s, --sinkless   run without serial port\n";
 }
 
-static void printRates()
-{
-std::cout<<"Rates:\n\
-    B50      0000001\n\
-    B75      0000002\n\
-    B110     0000003\n\
-    B134     0000004\n\
-    B150     0000005\n\
-    B200     0000006\n\
-    B300     0000007\n\
-    B600     0000010\n\
-    B1200    0000011\n\
-    B1800    0000012\n\
-    B2400    0000013\n\
-    B4800    0000014\n\
-    B9600    0000015\n\
-    B19200   0000016\n\
-    B38400   0000017\n\
-    B57600   0010001\n\
-    B115200  0010002\n\
-    B500000  0010005\n\
-    B1000000 0010010\n\
-    B1152000 0010011\n\
-    B1500000 0010012\n\
-    B2000000 0010013\n";
-}
-
 struct Config
 {
     std::string portFileName = "/dev/ttyUSB0";
     unsigned short port = 6856;
-    int baud = 0000017;
+    int baud = B9600;
     bool noSerial = false;
 };
 
@@ -135,6 +108,7 @@ int main(int argc, char* argv[])
         std::cout<<"Using serial port: "<<config.portFileName<<" at "<<config.baud<<" baud\n";
         serial = serialport_init(config.portFileName.c_str(), config.baud);
         if(serial == -1) return 1;
+		tcflush(serial, TCIOFLUSH);
     }
     else std::cout<<"Sinkless mode\n";
 
diff --git a/serial_io.cpp b/serial_io.cpp
index 42fa2d1..d7f8107 100644
--- a/serial_io.cpp
+++ b/serial_io.cpp
@@ -1,7 +1,5 @@
 #include "serial_io.h"
 
-struct termios toptions;
-
 void sWrite(int port, char string[], size_t length)
 {
     if(port != -1) write(port, string, length);
@@ -18,9 +16,35 @@ ssize_t sRead(int port, void *buf, size_t count)
     return (port != -1) ? read(port, buf, count) : 0;
 }
 
+#ifdef __cplusplus
+void printRates()
+{
+std::cout<<"Rates:\n"\
+         <<"B50      "<<B50<<'\n'\
+         <<"B75      "<<B75<<'\n'\
+         <<"B110     "<<B110<<'\n'\
+         <<"B134     "<<B134<<'\n'\
+         <<"B150     "<<B150<<'\n'\
+         <<"B200     "<<B200<<'\n'\
+         <<"B300     "<<B300<<'\n'\
+         <<"B600     "<<B600<<'\n'\
+         <<"B1200    "<<B1200<<'\n'\
+         <<"B1800    "<<B1800<<'\n'\
+         <<"B2400    "<<B2400<<'\n'\
+         <<"B4800    "<<B4800<<'\n'\
+         <<"B9600    "<<B9600<<'\n'\
+         <<"B19200   "<<B19200<<'\n'\
+         <<"B38400   "<<B38400<<'\n'\
+         <<"B57600   "<<B57600<<'\n'\
+         <<"B115200  "<<B115200<<'\n'\
+         <<"B230400  "<<B230400<<'\n';
+}
+#endif
+
 int serialport_init(const char* device, int baud)
 {
     int fd;
+    struct termios toptions;
     fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY);
     if (fd == -1)
     {
@@ -44,19 +68,20 @@ int serialport_init(const char* device, int baud)
 
     //Make Raw
     toptions.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
-    toptions.c_oflag &= ~OPOST;
+    toptions.c_oflag &= ~OPOST; //hmm examine
     toptions.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
     toptions.c_cflag &= ~(CSIZE | PARENB);
     toptions.c_cflag |= CS8;
     
-    cfsetispeed(&toptions, baud);
-    cfsetospeed(&toptions, baud);
-
+    if(cfsetispeed(&toptions, baud) < 0 || cfsetospeed(&toptions, baud) < 0)
+    {
+        perror("init_serialport: Couldn't set baud rate");
+        return -1;
+    }
     // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
     toptions.c_cc[VMIN]  = 0;
     toptions.c_cc[VTIME] = 40;
     fcntl(fd, F_SETFL, FNDELAY);
-
     if( tcsetattr(fd, TCSANOW, &toptions) < 0)
     {
         perror("init_serialport: Couldn't set term attributes");
diff --git a/serial_io.h b/serial_io.h
index d88c676..c41ef28 100644
--- a/serial_io.h
+++ b/serial_io.h
@@ -3,7 +3,10 @@
 #include <termios.h>
 #include <fcntl.h>
 #include <unistd.h>
+
+#ifdef __cplusplus
 #include <iostream>
+#endif
 
 #define BAUDRATE B38400
 
@@ -13,6 +16,9 @@ void sWrite(int port, const char string[], size_t length);
 
 ssize_t sRead(int port, void *buf, size_t count);
 
+#ifdef __cplusplus
+void printRates();
+#endif
 
 int serialport_init(const char* device, int baud = BAUDRATE);