diff --git a/app/benchmark/timing/copter.cc b/app/benchmark/timing/copter.cc
index 688a15b187760ba995bd7673392cb8150416f909..2fde270692ac2ac6eb3e52b4e500ef5188745448 100644
--- a/app/benchmark/timing/copter.cc
+++ b/app/benchmark/timing/copter.cc
@@ -125,7 +125,7 @@ TASK(CopterControlTask) {
 }
 
 ISR2(MavlinkRecvHandler) {
-    timing_start(2);
+    timing_start(2 | TIMING_POINT_IS_HIGHEST_PRIO);
     calculation_short();
     ActivateTask(CopterControlTask);
     calculation_short();
diff --git a/generator/transform/GeneratePML.py b/generator/transform/GeneratePML.py
index 7a383151e2b657d5c615fd64f99d1381337921f5..0d35949141e9ed1b8b40f3d6e77830921547168c 100644
--- a/generator/transform/GeneratePML.py
+++ b/generator/transform/GeneratePML.py
@@ -66,6 +66,17 @@ class GeneratePML(Analysis, GraphObject):
 
 
     def get_circuit_states(self, start_abbs, end_abbs):
+        def priority_map(state):
+            ret = {}
+            for subtask in self.system_graph.subtasks:
+                abb = state.get_continuation(subtask) or state.current_abb
+                if not state.is_surely_ready(subtask):
+                    continue
+                if subtask.conf.is_isr or subtask.is_idle_thread():
+                    continue
+                ret[subtask] = (subtask.conf.static_priority, abb.dynamic_priority)
+            return ret
+
         # 1. Find all starting states
         sse = self.system_graph.get_pass("sse")
         start_states, end_states = {}, {}
@@ -79,22 +90,19 @@ class GeneratePML(Analysis, GraphObject):
                         if not nstate.current_subtask.conf.is_isr:
                             continue
                         start_states[id(nstate)] = nstate
-                elif is_TIMING_POINT_IS_HIGHEST_PRIO(start_abbs.get(state.current_abb,0)) and \
-                     not state.current_subtask.conf.preemptable:
-                    # The current subtask must be of the highest priority
-                    # to be a good starting point
-                    def dynamic_priority(subtask):
-                        abb = state.get_continuation(subtask) or state.current_abb
-                        return abb.dynamic_priority
-
-                    prio = state.current_subtask.conf.static_priority
-                    ignore = False
-                    for subtask in self.system_graph.subtasks:
-                        if subtask == state.current_subtask:
-                            continue
-                        if state.is_surely_ready(subtask) and dynamic_priority(subtask) > prio:
-                            ignore = True
-                    if ignore is False:
+                elif is_TIMING_POINT_IS_HIGHEST_PRIO(start_abbs.get(state.current_abb,0)):
+                    # The current running subtask must be of the
+                    # highest priority to be a good starting point
+
+                    # Find the highest possible subtask, excluding interrupts
+                    prios = list(sorted(priority_map(state).items(),
+                                        key=lambda x: x[1][1],
+                                        reverse=True))
+                    if prios \
+                       and not prios[0][0].conf.preemptable \
+                       and any([x[1][0] > prios[0][1][0] for x in prios[1:]]):
+                        pass # Ignore state
+                    else:
                         start_states[id(state)] = state
                 else:
                     start_states[id(state)] = state