diff --git a/.gitignore b/.gitignore
index b8bd026..4a0fc91 100644
--- a/.gitignore
+++ b/.gitignore
@@ -26,3 +26,7 @@
 *.exe
 *.out
 *.app
+.pioenvs
+.piolibdeps
+.clang_complete
+.gcc-flags.json
diff --git a/src/ModbusSlave.cpp b/src/ModbusSlave.cpp
index 629e9a9..0c8f721 100644
--- a/src/ModbusSlave.cpp
+++ b/src/ModbusSlave.cpp
@@ -141,39 +141,75 @@ int Modbus::poll() {
         return 0;
     }
 
+    // check unit-id
+    if (bufIn[0] != unitID) return 0;
+
     /**
      * Validate buffer.
      */
     // check minimum length.
     if (lengthIn < 8) return 0;
 
-    // this removes the null character that sometimes
-    // is get from the SoftwareSerial on the "nano"
-    if(lengthIn > 8 && bufIn[lengthIn-1] == 0) lengthIn--;
-
-    // check unit-id
-    if (bufIn[0] != unitID) return 0;
+    /**
+     * Get the Function code.
+     */
+    fc = bufIn[1];
 
-    // check crc.
-    crc = word(bufIn[lengthIn - 1], bufIn[lengthIn - 2]);
-    if (calcCRC(bufIn, lengthIn - 2) != crc) return 0;
+    /**
+     * Get message address and length/status.
+     */
+    address = word(bufIn[2], bufIn[3]); // first register.
+    length = word(bufIn[4], bufIn[5]);  // number of registers to act upone or status.
 
     /**
-     * Parse command
+     * Output length sanity check, and remove trailing noise from message.
      */
-    fc = bufIn[1];
     switch (fc) {
         case FC_READ_COILS: // read coils (digital out state)
         case FC_READ_DISCRETE_INPUT: // read input state (digital in)
-            address = word(bufIn[2], bufIn[3]); // coil to set.
-            length = word(bufIn[4], bufIn[5]);
+        case FC_READ_HOLDING_REGISTERS: // read holding registers (analog out state)
+        case FC_READ_INPUT_REGISTERS: // read input registers (analog in)
+            // sanity check.
+            if (length > MAX_BUFFER) return 0;
+
+            // ignore tailing nulls.
+            lengthIn = 8;
+
+            break;
+        case FC_WRITE_COIL:
+            status = length; // 0xff00 - on, 0x0000 - off
 
+            // ignore tailing nulls.
+            lengthIn = 8;
+
+            break;
+        case FC_WRITE_MULTIPLE_REGISTERS:
             // sanity check.
             if (length > MAX_BUFFER) return 0;
 
-            // check command length.
-            if (lengthIn != 8) return 0;
+            // check buffer in size.
+            if (lengthIn < (int)(7 + length * 2 + 2)) return 0;
 
+            // ignore tailing nulls.
+            lengthIn = (int)(7 + length * 2 + 2);
+
+            break;
+        default:
+            // unknown command
+            return 0;
+            break;
+    }
+
+    // check crc.
+    crc = word(bufIn[lengthIn - 1], bufIn[lengthIn - 2]);
+    if (calcCRC(bufIn, lengthIn - 2) != crc) return 0;
+
+    /**
+     * Parse command
+     */
+    switch (fc) {
+        case FC_READ_COILS: // read coils (digital out state)
+        case FC_READ_DISCRETE_INPUT: // read input state (digital in)
             // build valid empty answer.
             lengthOut = 3 + (length - 1) / 8 + 1 + 2;
             bufOut[2] = (length - 1) / 8 + 1;
@@ -188,15 +224,6 @@ int Modbus::poll() {
             break;
         case FC_READ_HOLDING_REGISTERS: // read holding registers (analog out state)
         case FC_READ_INPUT_REGISTERS: // read input registers (analog in)
-            address = word(bufIn[2], bufIn[3]); // first register.
-            length = word(bufIn[4], bufIn[5]); // number of registers to read.
-
-            // sanity check.
-            if (length > MAX_BUFFER) return 0;
-
-            // check command length.
-            if (lengthIn != 8) return 0;
-
             // build valid empty answer.
             lengthOut = 3 + 2 * length + 2;
             bufOut[2] = 2 * length;
@@ -210,12 +237,6 @@ int Modbus::poll() {
             }
             break;
         case FC_WRITE_COIL: // write one coil (digital out)
-            address = word(bufIn[2], bufIn[3]); // coil to set
-            status = word(bufIn[4], bufIn[5]); // 0xff00 - on, 0x0000 - off
-
-            // check command length.
-            if (lengthIn != 8) return 0;
-
             // build valid empty answer.
             lengthOut = 8;
             memcpy(bufOut + 2, bufIn + 2, 4);
@@ -226,15 +247,6 @@ int Modbus::poll() {
             }
             break;
         case FC_WRITE_MULTIPLE_REGISTERS: // write holding registers (analog out)
-            address = word(bufIn[2], bufIn[3]); // first register
-            length = word(bufIn[4], bufIn[5]); // number of registers to set
-
-            // sanity check.
-            if (length > MAX_BUFFER) return 0;
-
-            // check command length
-            if (lengthIn != (7 + length * 2 + 2)) return 0;
-
             // build valid empty answer.
             lengthOut = 8;
             memcpy(bufOut + 2, bufIn + 2, 4);
@@ -244,10 +256,6 @@ int Modbus::poll() {
                 cbVector[CB_WRITE_MULTIPLE_REGISTERS](fc, address, length);
             }
             break;
-        default:
-            // unknown command
-            return 0;
-            break;
     }
 
     /**