Changed the Treply_delay wait to be smaller when the next message in the queue is does not match.

bacnet-stack/ports/at91sam7s/dlmstp.c

@@ -1118,7 +1118,8 @@ static bool MSTP_Master_Node_FSM(
                 Master_State = MSTP_MASTER_STATE_IDLE;
                 /* clear our flag we were holding for comparison */
                 MSTP_Flag.ReceivedValidFrame = false;
-            } else if (rs485_silence_time_elapsed(Treply_delay)) {
+            } else if (rs485_silence_time_elapsed(Treply_delay) ||
+                (pkt != NULL)) {
                 /* DeferredReply */
                 /* If no reply will be available from the higher layers */
                 /* within Treply_delay after the reception of the */

bacnet-stack/ports/bdk-atxx4-mstp/dlmstp.c

@@ -1145,7 +1145,8 @@ static bool MSTP_Master_Node_FSM(
                 Master_State = MSTP_MASTER_STATE_IDLE;
                 /* clear our flag we were holding for comparison */
                 MSTP_Flag.ReceivedValidFrame = false;
-            } else if (rs485_silence_time_elapsed(Treply_delay)) {
+            } else if (rs485_silence_time_elapsed(Treply_delay) ||
+                (pkt != NULL)) {
                 /* DeferredReply */
                 /* If no reply will be available from the higher layers */
                 /* within Treply_delay after the reception of the */

bacnet-stack/ports/stm32f10x/dlmstp.c

@@ -1234,7 +1234,8 @@ static bool MSTP_Master_Node_FSM(
                 Master_State = MSTP_MASTER_STATE_IDLE;
                 /* clear our flag we were holding for comparison */
                 MSTP_Flag.ReceivedValidFrame = false;
-            } else if (rs485_silence_time_elapsed(Treply_delay)) {
+            } else if (rs485_silence_time_elapsed(Treply_delay) ||
+                (pkt != NULL)) {
                 /* DeferredReply */
                 /* If no reply will be available from the higher layers */
                 /* within Treply_delay after the reception of the */

bacnet-stack/src/mstp.c

@@ -1051,6 +1051,9 @@ bool MSTP_Master_Node_FSM(
             /* The ANSWER_DATA_REQUEST state is entered when a  */
             /* BACnet Data Expecting Reply, a Test_Request, or  */
             /* a proprietary frame that expects a reply is received. */
+            /* FIXME: MSTP_Get_Reply waits for a matching reply, but
+               if the next queued message doesn't match, then we
+               sit here for Treply_delay doing nothing */
             length = (unsigned) MSTP_Get_Reply(mstp_port, 0);
             if (length > 0) {
                 /* Reply */