diff --git a/pd.py b/pd.py
index e8fdddc..99ec3cb 100644
--- a/pd.py
+++ b/pd.py
@@ -160,6 +160,12 @@ def decode(self):
         dshot_value = DshotCmd(self.dshot_cfg)
         telem_value = DshotTelem(self.dshot_cfg)
 
+        last_dshot_value = DshotCmd(self.dshot_cfg)
+        last_telem_value = DshotTelem(self.dshot_cfg)
+
+        max_time_before_telem = 40e-6
+        max_samples_before_telem = int(max_time_before_telem / (1 / self.samplerate))
+
         #bitseq = BitDshot()
         while True:
 
@@ -167,6 +173,8 @@ def decode(self):
                 case State.CMD:
                     match self.state_dshot:
                         case State_Dshot.RESET:
+                            if dshot_value.crc_ok:
+                                last_dshot_value = dshot_value
                             dshot_value = DshotCmd(self.dshot_cfg)
                             self.state_dshot = State_Dshot.START
 
@@ -175,6 +183,7 @@ def decode(self):
                                 pins = self.wait([{0: 'r'}, {0: 'f'}, {'skip': self.dshot_cfg.samples_after_motorcmd}])
                             else:
                                 pins = self.wait([{0: 'f'}, {0: 'r'}, {'skip': self.dshot_cfg.samples_after_motorcmd}])
+
                             #TODO: Increase skip to maximum time for effiency
                             #TODO: Mark any changes in this time as errors?  Option to reduce load?
 
@@ -195,6 +204,11 @@ def decode(self):
                                 if result:
                                     self.display_dshot(dshot_value)
                                     self.state_dshot = State_Dshot.RESET
+                                    #TODO: Change??
+                                    dshot_value.packet.es = self.samplenum
+                                else:
+                                    #error, reset
+                                    self.state_dshot = State_Dshot.RESET
                                 if result and self.dshot_cfg.bidirectional:
                                     self.state = State.TELEM
 
@@ -221,10 +235,15 @@ def decode(self):
                             self.state_telem = State_Dshot.START
 
                         case State_Dshot.START:
+                            if last_dshot_value.packet.es is not None:
+                                if self.samplenum >= last_dshot_value.packet.es + max_samples_before_telem:
+                                    self.state_telem = State_Dshot.RESET
+                                    self.state = State.CMD
+                                continue
                             # First wait for falling edge (idle high)
                             pins = self.wait([{0: 'f'}])
                             # Save start pulse
-                            tlm_start = self.samplenum
+                            telem_value.packet.ss = self.samplenum
                             # Switch to receiving state
                             self.state_telem = State_Dshot.RECV
                             # TODO: Check if still low after 1/8 bitlength for error det?
diff --git a/protoDshot/protocols_motor.py b/protoDshot/protocols_motor.py
index d2ff316..f962e06 100644
--- a/protoDshot/protocols_motor.py
+++ b/protoDshot/protocols_motor.py
@@ -1,5 +1,6 @@
 from functools import reduce
 from enum import Enum
+from .dshot_bits import Sequence
 
 gcr_tables = {
     "0b11001": 0x0,
@@ -53,6 +54,7 @@ def __init__(self,settings_Dshot=DshotSettings()):
         self.crc_ok = False
         self.bits = 0
         self.results = []
+        self.packet = Sequence()
 
     def checkCRC(self,data,crc_recv):
         self.crc_recv = crc_recv