Fix for other char sizes too

src/libCom/SerialCom.cpp

@@ -215,11 +215,21 @@ std::string SerialCom::receive() {
         std::string response = std::string(m_tmpbuf, n_read);
 
         // Convert response if communication uses charsize different from 8
-        if (m_charsize == mapCHARSIZE.at("CS7")) {
+        if (m_charsize != mapCHARSIZE.at("CS8")) {
             std::string decoded_response;
             for (size_t i = 0; i < response.length(); i++) {
-                unsigned char original = static_cast<unsigned char>(response[i]);
-                unsigned char fixed = original & 0x7F;
+                unsigned char original =
+                    static_cast<unsigned char>(response[i]);
+                unsigned char fixed;
+                if (m_charsize == mapCHARSIZE.at("CS7"))
+                    fixed = original & 0x7F;
+                else if (m_charsize == mapCHARSIZE.at("CS6"))
+                    fixed = original & 0x6F;
+                else if (m_charsize == mapCHARSIZE.at("CS5"))
+                    fixed = original & 0x5F;
+                else
+                    throw std::runtime_error(
+                        "CharSize not recognized! Unable to decode response");
                 decoded_response.push_back(fixed);
             }
             response = decoded_response;