Merge branch 'vs/MTX-3251/modem-does-not-work' into 'master'

Vs/mtx 3251/modem does not work

See merge request !20

2 files changed, 81 insertions, 6 deletions

diff --git a/include/mts/MTS_IO_SerialConnection.h b/include/mts/MTS_IO_SerialConnection.h
index a4ca011..b0ce663 100644
--- a/include/mts/MTS_IO_SerialConnection.h
+++ b/include/mts/MTS_IO_SerialConnection.h
@@ -173,6 +173,8 @@ namespace MTS {
                 virtual int doWrite(const char* pBuffer, const uint32_t& iSize, int32_t& timeoutMillis);
 
                 void cleanup();
+                void printPortSetting(const termios *options);
+                const char* humanSpeed(speed_t speed);
         };
     }
 }
diff --git a/src/MTS_IO_SerialConnection.cpp b/src/MTS_IO_SerialConnection.cpp
index a9f3d6e..936d28d 100644
--- a/src/MTS_IO_SerialConnection.cpp
+++ b/src/MTS_IO_SerialConnection.cpp
@@ -50,6 +50,7 @@ SerialConnection::Builder::Builder(const std::string& sPortName)
 , m_eParity(OFF)
 , m_eDataBits(B8)
 , m_eStopBits(B1)
+, m_eFlowControl(NONE)
 , m_bBuilt(false)
 {
 
@@ -254,6 +255,77 @@ int SerialConnection::getFileDescriptor() {
     return h;
 }
 
+const char* SerialConnection::humanSpeed(speed_t speed) {
+  const char *hspeed;
+  switch (speed) {
+    case B0:       hspeed = "B0";
+                   break;
+    case B50:      hspeed = "B50";
+                   break;
+    case B75:      hspeed = "B75";
+                   break;
+    case B110:     hspeed = "B110";
+                   break;
+    case B134:     hspeed = "B134";
+                   break;
+    case B150:     hspeed = "B150";
+                   break;
+    case B200:     hspeed = "B200";
+                   break;
+    case B300:     hspeed = "B300";
+                   break;
+    case B600:     hspeed = "B600";
+                   break;
+    case B1200:    hspeed = "B1200";
+                   break;
+    case B1800:    hspeed = "B1800";
+                   break;
+    case B2400:    hspeed = "B2400";
+                   break;
+    case B4800:    hspeed = "B4800";
+                   break;
+    case B9600:    hspeed = "B9600";
+                   break;
+    case B19200:   hspeed = "B19200";
+                   break;
+    case B57600:   hspeed = "B57600";
+                   break;
+    case B115200:  hspeed = "B115200";
+                   break;
+    default:       hspeed = "unknown";
+  }
+  return hspeed;
+}
+
+void SerialConnection::printPortSetting(const termios *options){
+    printDebug("SERIAL| port settings:");
+    printDebug("SERIAL| in speed:%s out speed:%s",
+               humanSpeed(cfgetispeed(options)),
+               humanSpeed(cfgetospeed(options)));
+    int data_length=0;
+    if ((options->c_cflag&CSIZE) == CS5) {
+        data_length = 5;
+    }
+    if ((options->c_cflag&CSIZE) == CS6) {
+        data_length = 6;
+    }
+    if ((options->c_cflag&CSIZE) == CS7) {
+        data_length = 7;
+    }
+    if ((options->c_cflag&CSIZE) == CS8) {
+        data_length = 8;
+    }
+    printDebug("SERIAL| data:%d stop bits:%d", data_length,options->c_cflag&CSTOPB?2:1);
+    if (!(options->c_cflag&PARENB)) {
+        printDebug("SERIAL| parity: NONE");
+    } else if (options->c_cflag&PARODD) {
+        printDebug("SERIAL| parity: ODD");
+    } else {
+        printDebug("SERIAL| parity: EVEN");
+    }
+    printDebug("SERIAL| CRTSCTS:%d", options->c_cflag&CRTSCTS?1:0);
+}
+
 bool SerialConnection::doOpen(const int32_t& timeoutMillis) {
     m_apHandleLock->lock();
 
@@ -353,6 +425,7 @@ bool SerialConnection::doOpen(const int32_t& timeoutMillis) {
     }
 
     //Set Parity, Stop Bits, and Data Length
+    options.c_cflag &= ~(PARODD | PARENB);
     switch(m_eParity) {
         case ODD:
             options.c_cflag |= ( PARODD | PARENB );
@@ -364,7 +437,6 @@ bool SerialConnection::doOpen(const int32_t& timeoutMillis) {
 
         case OFF:
         default:
-            options.c_cflag &= ~PARENB;
             break;
     }
 
@@ -390,7 +462,7 @@ bool SerialConnection::doOpen(const int32_t& timeoutMillis) {
 
     switch(m_eStopBits) {
         case B2:
-            options.c_cflag &= CSTOPB;
+            options.c_cflag |= CSTOPB;
             break;
 
         case B1:
@@ -401,19 +473,20 @@ bool SerialConnection::doOpen(const int32_t& timeoutMillis) {
 
 	switch (m_eFlowControl) {
 		case RTS_CTS:
-			options.c_cflag &= CRTSCTS;
+			options.c_cflag |= CRTSCTS;
 			break;
 		case NONE:
 		default:
-			options.c_cflag &= ~(IXON | IXOFF | IXANY);
-            break;
+			options.c_cflag &= ~CRTSCTS;
+			break;
     }
 
 	//Set Control Modes
     options.c_cc[VMIN] = 0;
     options.c_cc[VTIME] = 2; // tenths of seconds
-
+    printPortSetting(&options);
     result = tcsetattr(m_iHandle, TCSANOW, &options);
+
     if (result == -1) {
         printWarning("SERIAL| Failed to set configurations on port [%s] [%d]%s", m_sPortName.c_str(), errno,
                      errno == 13 ? " (Permission Denied)" : "");