Check discard after tx

1 files changed, 8 insertions, 9 deletions

diff --git a/src/policy_engine_states.cpp b/src/policy_engine_states.cpp
index ffa4a4d..d3068a9 100644
--- a/src/policy_engine_states.cpp
+++ b/src/policy_engine_states.cpp
@@ -491,23 +491,15 @@ PolicyEngine::policy_engine_state PolicyEngine::pe_sink_wait_send_done() {
   uint32_t evt = currentEvents;
   clearEvents();
 
-  if ((uint32_t)evt & (uint32_t)Notifications::DISCARD) {
-    // increment the counter
-    _tx_messageidcounter = (_tx_messageidcounter + 1) % 8;
-    notify(Notifications::TX_ERR);
-    return postSendFailedState;
-  }
-
   /* If the message was sent successfully */
   if ((uint32_t)evt & (uint32_t)Notifications::I_TXSENT) {
 
-    clearEvents();
     if (incomingMessages.getOccupied()) {
       return pe_sink_wait_good_crc();
     } else {
       // No Good CRC has arrived, these should _normally_ come really fast, but users implementation may be lagging
       // Setup a callback for this state
-      return waitForEvent(PEWaitingMessageGoodCRC, (uint32_t)Notifications::MSG_RX, 100);
+      return waitForEvent(PEWaitingMessageGoodCRC, (uint32_t)Notifications::MSG_RX, 120);
     }
   }
   /* If the message failed to be sent */
@@ -515,6 +507,13 @@ PolicyEngine::policy_engine_state PolicyEngine::pe_sink_wait_send_done() {
     notify(Notifications::TX_ERR);
     return postSendFailedState;
   }
+  /* A discard was queued due to rx */
+  if ((uint32_t)evt & (uint32_t)Notifications::DISCARD) {
+    // increment the counter
+    _tx_messageidcounter = (_tx_messageidcounter + 1) % 8;
+    notify(Notifications::TX_ERR);
+    return postSendFailedState;
+  }
 
   /* Silence the compiler warning */
   notify(Notifications::TX_ERR);