diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/Makefile b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/Makefile
index f98cbed3..c3a7a62b 100644
--- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/Makefile
+++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/Makefile
@@ -144,6 +144,11 @@ ifdef GEN_DEMO
 
     CFLAGS := -DmainCREATE_GEN_DEMO=1
 else
+ifeq ($(COPTER_DEMO), 1)
+    SOURCE_FILES += main_copter.c
+
+    CFLAGS := -DmainCOPTER_DEMO=1
+else
 ifeq ($(RELEASE_DEMO), 1)
     SOURCE_FILES += main_release.c
 
@@ -171,6 +176,7 @@ endif
 endif
 endif
 endif
+endif
 
 ifeq ($(INTERRUPT_ACTIVATION), 1)
     CFLAGS += -D INTERRUPT_ACTIVATION=1
diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/arbitrary_loads.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/arbitrary_loads.c
index c6b7e35a..41ba5c31 100644
--- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/arbitrary_loads.c
+++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/arbitrary_loads.c
@@ -34,7 +34,7 @@ volatile long _NONSENSE_VAR = 0;
 #define C 12345ull
 static unsigned long long rng_seed = 2345745ull;
 #define RNG rng_seed+=((A*((rng_seed+=C)-C)+C) % M)
-#define RNG_FROM(X) ((A*X+C) % M)
+#define RNG_FROM(X) ((A*(X)+C) % M)
 
 // Challanges =======
 #define CHANCE_1_IN_POWOF2(X,Y) (RNG_FROM(X)<(M>>Y))    // assume the type of x has more than y bits
diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/copter.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/copter.c
new file mode 100644
index 00000000..ce9a4704
--- /dev/null
+++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/copter.c
@@ -0,0 +1,157 @@
+// #include "os.h"
+// #include "test/test.h"
+// #include "timing.h"
+// #include "machine.h"
+
+#include "de-osek.h"
+
+//extern "C" volatile uint32_t random_source =0 ;
+DeclareTask(SignalGatherInitiateTask);
+DeclareTask(SignalGatherFinishedTask);
+DeclareTask(SignalGatherTimeoutTask);
+DeclareTask(SignalProcessingActuateTask);
+DeclareTask(SignalProcessingAttitudeTask);
+DeclareTask(ActuateTask);
+DeclareTask(FlightControlAttitudeTask);
+DeclareTask(FlightControlActuateTask);
+DeclareTask(MavlinkSendTask);
+DeclareTask(CopterControlTask);
+DeclareTask(MavlinkRecvHandler);
+
+DeclareResource(SPIBus);
+
+DeclareAlarm(CopterControlWatchdogAlarm);
+
+TIMING_MAKE_OS_MAIN( StartOS(0) )
+
+//GENERATE_TIME_CONSUMER(calculation, 100)
+//GENERATE_TIME_CONSUMER(calculation_short, 10)
+// #define calculation() do {} while(0)
+// #define calculation_short() do {} while(0)
+
+int round;
+
+TASK(SignalGatherInitiateTask) {
+    timing_start(0);
+	GetResource(SPIBus);
+	calculation();
+	if ((round % 2) == 0) {
+            ActivateTask(SignalGatherTimeoutTask);
+	} else {
+            ActivateTask(SignalGatherFinishedTask);
+	}
+	round ++;
+	calculation();
+	ReleaseResource(SPIBus);
+	calculation();
+	TerminateTask();
+}
+
+TASK(SignalGatherFinishedTask) {
+    calculation();
+    ActivateTask(SignalProcessingAttitudeTask);
+    calculation();
+    ActivateTask(SignalProcessingActuateTask);
+    calculation();
+    TerminateTask();
+}
+
+TASK(SignalGatherTimeoutTask) {
+    calculation();
+    GetResource(SPIBus);
+    calculation();
+    ReleaseResource(SPIBus);
+    calculation();
+    ChainTask(SignalGatherFinishedTask);
+}
+
+volatile int calculate;
+
+TASK(SignalProcessingActuateTask) {
+	calculation();
+	TerminateTask();
+}
+
+TASK(SignalProcessingAttitudeTask) {
+	calculation();
+        timing_end(0);
+	TerminateTask();
+}
+
+TASK(FlightControlTask) {
+    timing_start(1 | TIMING_POINT_IS_HIGHEST_PRIO);
+    calculation();
+    ActivateTask(FlightControlAttitudeTask);
+    ActivateTask(FlightControlActuateTask);
+    ActivateTask(MavlinkSendTask);
+    calculation();
+    TerminateTask();
+}
+
+TASK(FlightControlAttitudeTask) {
+	calculation();
+	TerminateTask();
+}
+
+TASK(FlightControlActuateTask) {
+	calculation();
+	TerminateTask();
+}
+
+TASK(MavlinkSendTask) {
+    calculation();
+    GetResource(SPIBus);
+    calculation_short();
+    Machine::trigger_interrupt_from_user(37);
+    ReleaseResource(SPIBus);
+    calculation();
+    timing_end(1);
+    TerminateTask();
+}
+
+TASK(CopterControlTask) {
+
+    calculation();
+    SuspendAllInterrupts();
+    calculation_short();
+    ResumeAllInterrupts();
+    calculation();
+
+    /* Saves 8000 states
+    if (round < 5) {
+        CancelAlarm(CopterControlWatchdogAlarm);
+        SetRelAlarm(CopterControlWatchdogAlarm, 100, 100);
+    }
+    calculation();
+    */
+
+    timing_end(2);
+
+    TerminateTask();
+}
+
+ISR2(MavlinkRecvHandler) {
+    timing_start(2 | TIMING_POINT_START_INTERRUPT_FROM_IDLE);
+    calculation_short();
+    ActivateTask(CopterControlTask);
+    calculation_short();
+}
+
+TASK(CopterControlWatchdogTask) {
+    calculation();
+    TerminateTask();
+}
+
+
+void PreIdleHook() {
+    static int count = 0;
+    kout << "---\n";
+    timing_print();
+    if (++count > 5) {
+    }
+
+    if (count == 100) {
+        timing_dump();
+    }
+}
+
diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/de-osek.h b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/de-osek.h
new file mode 100644
index 00000000..fabb6460
--- /dev/null
+++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/de-osek.h
@@ -0,0 +1,40 @@
+// Macros that allow for a crude translation of OSEK to FreeRTOS
+
+// #include <FreeRTOS.h>
+// #include <task.h>
+// #include <queue.h>
+// #include <stdio.h>
+// #include <semphr.h>
+// #include "arbitrary_loads.c"
+
+#define DeclareTask(task_name) static void prv_##task_name( void * pvParameters ); \
+    static TaskHandle_t xTask_##task_name = NULL
+
+#define DeclareResource(resource_name) static SemaphoreHandle_t xSemaphore_##resource_name 
+
+#define DeclareAlarm(alarm_name) static TickType_t xFrequency_##alarm_name;
+
+#define TIMING_MAKE_OS_MAIN(os_start) void main_osek( void ) { \
+}
+#define TASK(task_name) static void prv_##task_name( void * pvParameters )
+
+#define GetResource(resource_name) xSemaphoreTake(xSemaphore_##resource_name, 1)
+
+#define ReleaseResource(resource_name) xSemaphoreGive(xSemaphore_##resource_name)
+
+#define calculation() WASTE_MSEC(1)
+#define calculation_short() WASTE_USEC(500)
+
+#define TerminateTask() do { ulTaskNotifyTake(pdTRUE, 0);} while (1)
+
+#define ActivateTask(task_name) xTaskNotify(xTask_##task_name, 0, eNoAction)
+
+#define ChainTask(task_name) ActivateTask(task_name); TerminateTask()
+
+#define SuspendAllInterrupts() vTaskSuspendAll()
+#define ResumeAllInterrupts() xTaskResumeAll()
+
+#define Machine
+#define trigger_interrupt_from_user(interrupt_number) NVIC_SetPendingIRQ((interrupt_number))
+
+#define ISR2(isr_name) void ISR_2_Handler( void )
\ No newline at end of file
diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/init/startup.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/init/startup.c
index bd245f7a..d20b743b 100644
--- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/init/startup.c
+++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/init/startup.c
@@ -218,12 +218,28 @@ void _start( void )
     NVIC_EnableIRQ(3);
     NVIC_EnableIRQ(4);
     NVIC_EnableIRQ(5);
+    NVIC_EnableIRQ(6);
+    NVIC_EnableIRQ(7);
+    NVIC_EnableIRQ(8);
+    NVIC_EnableIRQ(9);
+    NVIC_EnableIRQ(10);
+    NVIC_EnableIRQ(11);
+    NVIC_EnableIRQ(12);
+    NVIC_EnableIRQ(13);
     NVIC_SetPriority (0, 6);    // need to stay above configMAX_SYSCALL_INTERRUPT_PRIORITY
     NVIC_SetPriority (1, 6);
     NVIC_SetPriority (2, 6);
     NVIC_SetPriority (3, 6);
     NVIC_SetPriority (4, 6);
     NVIC_SetPriority (5, 6);
+    NVIC_SetPriority (6, 6);
+    NVIC_SetPriority (7, 6);
+    NVIC_SetPriority (8, 6);
+    NVIC_SetPriority (9, 6);
+    NVIC_SetPriority (10, 6);
+    NVIC_SetPriority (11, 6);
+    NVIC_SetPriority (12, 6);
+    NVIC_SetPriority (13, 6);
     main( 0, 0 );
     exit( 0 );
 }
diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main.c
index 53331ce6..bfd1e058 100644
--- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main.c
+++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main.c
@@ -119,6 +119,10 @@ int main()
         {
             main_release();
         }
+    #elif ( mainCOPTER_DEMO == 1 )
+        {
+            main_osek();
+        }
     #else
 
         {
diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_copter.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_copter.c
new file mode 100644
index 00000000..b6451542
--- /dev/null
+++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_copter.c
@@ -0,0 +1,347 @@
+#include <FreeRTOS.h>
+#include <queue.h>
+#include <semphr.h>
+#include <stdio.h>
+#include <task.h>
+#include <CMSDK_CM3.h>
+#define GLOBAL_WCET_MULT                                                       \
+  1 // Multiplier to increase all waiting periods to make the schedule more
+     // tight an force preemptions
+#include "arbitrary_loads.c"
+
+#define NUM_TASKS 15
+#define MAX_INPUT_BYTES 4096
+volatile unsigned char FUZZ_INPUT[MAX_INPUT_BYTES] = {};
+volatile int INPUT_POINTERS[NUM_TASKS] = {};
+volatile uint32_t FUZZ_LENGTH = MAX_INPUT_BYTES;// ignored
+
+__attribute__((noinline)) static void trigger_Qemu_break( void )
+{
+    puts("Trigger");
+    while (1) {
+    }
+}
+__attribute__((noinline)) static void trigger_job_done( void )
+{
+    puts("Job Done");
+}
+
+static unsigned char fuzz_char_next(int tasknum) {
+  if (INPUT_POINTERS[tasknum] == 0) {
+      INPUT_POINTERS[tasknum] = tasknum * (MAX_INPUT_BYTES / NUM_TASKS);
+  }
+  if (INPUT_POINTERS[tasknum] >= (tasknum+1) * (MAX_INPUT_BYTES / NUM_TASKS)) {
+      trigger_Qemu_break();
+  }
+  FUZZ_INPUT[INPUT_POINTERS[tasknum]++];
+}
+
+volatile int RNG_STATES[NUM_TASKS] = {};
+static unsigned char next(int tasknum) {
+  RNG_STATES[tasknum] = RNG_FROM(RNG_STATES[tasknum]);
+  unsigned char t = fuzz_char_next(tasknum);
+  return RNG_STATES[tasknum] ^ (((int)t << 8) + (int)t);
+}
+
+// #define SHORT_CALC 50
+// #define LONG_CALC 200
+
+#define SHORT_CALC (25+next(task_id)%25)
+#define LONG_CALC (150+next(task_id)%100)
+
+#define HYPER_PERIOD 9
+#define SIMULATE_PERIODS 4
+static TickType_t initial_release_time = 0;
+static TaskHandle_t  xTaskTimeSupervisor = NULL;
+static void timing_supervisor_task( void * pvParameters ) {
+    initial_release_time = xTaskGetTickCount(); // The highest priority task sets the initial time
+    TickType_t xLastWakeTime = initial_release_time;
+    const TickType_t xFrequency = (SIMULATE_PERIODS * HYPER_PERIOD) / portTICK_PERIOD_MS;
+    trigger_job_done();
+    xTaskDelayUntil( &xLastWakeTime, xFrequency );
+    trigger_job_done();
+    trigger_Qemu_break();
+    for( ;; ){}
+}
+
+static void prv_SignalGatherInitiateTask(void *pvParameters);
+static TaskHandle_t xTask_SignalGatherInitiateTask = NULL;
+static void prv_SignalGatherFinishedTask(void *pvParameters);
+static TaskHandle_t xTask_SignalGatherFinishedTask = NULL;
+static void prv_SignalGatherTimeoutTask(void *pvParameters);
+static TaskHandle_t xTask_SignalGatherTimeoutTask = NULL;
+static void prv_SignalProcessingActuateTask(void *pvParameters);
+static TaskHandle_t xTask_SignalProcessingActuateTask = NULL;
+static void prv_SignalProcessingAttitudeTask(void *pvParameters);
+static TaskHandle_t xTask_SignalProcessingAttitudeTask = NULL;
+// static void prv_ActuateTask(void *pvParameters);
+// static TaskHandle_t xTask_ActuateTask = NULL;
+static void prv_FlightControlTask(void *pvParameters);
+static TaskHandle_t xTask_FlightControlTask = NULL;
+static void prv_FlightControlAttitudeTask(void *pvParameters);
+static TaskHandle_t xTask_FlightControlAttitudeTask = NULL;
+static void prv_FlightControlActuateTask(void *pvParameters);
+static TaskHandle_t xTask_FlightControlActuateTask = NULL;
+static void prv_MavlinkSendTask(void *pvParameters);
+static TaskHandle_t xTask_MavlinkSendTask = NULL;
+static void prv_CopterControlTask(void *pvParameters);
+static TaskHandle_t xTask_CopterControlTask = NULL;
+static void prv_MavlinkRecvHandler(void *pvParameters);
+static TaskHandle_t xTask_MavlinkRecvHandler = NULL;
+
+static SemaphoreHandle_t xSemaphore_SPIBus;
+
+static TickType_t xFrequency_CopterControlWatchdogAlarm;
+
+void main_osek(void) {
+  xSemaphore_SPIBus = xSemaphoreCreateBinary();
+  xSemaphoreGive(xSemaphore_SPIBus);
+
+  xTaskCreate(prv_SignalGatherInitiateTask, "SGInitiate",
+              configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 9, // 24
+              &xTask_SignalGatherInitiateTask);
+  xTaskCreate(prv_SignalGatherFinishedTask, "SGFinished",
+              configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 10, // 25
+              &xTask_SignalGatherFinishedTask);
+  xTaskCreate(prv_SignalGatherTimeoutTask, "SGTimeout", 
+              configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 8, // 23
+              &xTask_SignalGatherTimeoutTask);
+  xTaskCreate(prv_SignalProcessingActuateTask, "SPActuate", 
+              configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 7, // 22
+              &xTask_SignalProcessingActuateTask);
+  xTaskCreate(prv_SignalProcessingAttitudeTask, "SPAttitude",
+              configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 6, // 21
+              &xTask_SignalProcessingAttitudeTask);
+  // xTaskCreate(prv_ActuateTask, "ActuateTask", configMINIMAL_STACK_SIZE, NULL,
+              // tskIDLE_PRIORITY + 1, &xTask_ActuateTask);
+  xTaskCreate(prv_FlightControlTask, "FC",
+              configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 3, // 11
+              &xTask_FlightControlTask);
+  xTaskCreate(prv_FlightControlAttitudeTask, "FCAttitude",
+              configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 4, // 12
+              &xTask_FlightControlAttitudeTask);
+  xTaskCreate(prv_FlightControlActuateTask, "FCActuate",
+              configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 5, // 13
+              &xTask_FlightControlActuateTask);
+  xTaskCreate(prv_MavlinkSendTask, "MSend", configMINIMAL_STACK_SIZE,
+              NULL, tskIDLE_PRIORITY + 2, &xTask_MavlinkSendTask); // 10
+  xTaskCreate(prv_CopterControlTask, "CControl",
+              configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 1, // 1
+              &xTask_CopterControlTask);
+
+  xTaskCreate( timing_supervisor_task,
+              "supervisor",
+              configMINIMAL_STACK_SIZE,
+              NULL,
+              configMAX_PRIORITIES - 1,
+              &xTaskTimeSupervisor );
+
+  vTaskStartScheduler();
+  for( ; ; )
+  {
+  }
+}
+
+int round;
+
+static void prv_SignalGatherInitiateTask(void *pvParameters) {
+  const int task_id = 0;
+  TickType_t xLastWakeTime = initial_release_time;
+  const TickType_t xFrequency = 3 / portTICK_PERIOD_MS;
+  do {
+    // timing_start(0);
+    xSemaphoreTake(xSemaphore_SPIBus, 1);
+    WASTE_USEC(LONG_CALC);
+    if ((round % 2) == 0) {
+      xTaskNotify(xTask_SignalGatherTimeoutTask, 0, eNoAction);
+    } else {
+      xTaskNotify(xTask_SignalGatherFinishedTask, 0, eNoAction);
+    }
+    round++;
+    WASTE_USEC(LONG_CALC);
+    xSemaphoreGive(xSemaphore_SPIBus);
+    WASTE_USEC(LONG_CALC);
+    trigger_job_done();
+    xTaskDelayUntil( &xLastWakeTime, xFrequency );
+  } while (1);
+}
+
+static void prv_SignalGatherFinishedTask(void *pvParameters) {
+  const int task_id = 1;
+  trigger_job_done();
+  do {
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+    WASTE_USEC(LONG_CALC);
+    xTaskNotify(xTask_SignalProcessingAttitudeTask, 0, eNoAction);
+    WASTE_USEC(LONG_CALC);
+    xTaskNotify(xTask_SignalProcessingActuateTask, 0, eNoAction);
+    WASTE_USEC(LONG_CALC);
+    trigger_job_done();
+  } while (1);
+}
+
+static void prv_SignalGatherTimeoutTask(void *pvParameters) {
+  const int task_id = 2;
+  trigger_job_done();
+  do {
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+    WASTE_USEC(LONG_CALC);
+    xSemaphoreTake(xSemaphore_SPIBus, 1);
+    WASTE_USEC(LONG_CALC);
+    xSemaphoreGive(xSemaphore_SPIBus);
+    WASTE_USEC(LONG_CALC);
+    xTaskNotify(xTask_SignalGatherFinishedTask, 0, eNoAction);
+    trigger_job_done();
+  } while (1);
+}
+
+volatile int calculate;
+
+static void prv_SignalProcessingActuateTask(void *pvParameters) {
+  const int task_id = 3;
+  trigger_job_done();
+  do {
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+    WASTE_USEC(LONG_CALC);
+    trigger_job_done();
+  } while (1);
+}
+
+static void prv_SignalProcessingAttitudeTask(void *pvParameters) {
+  const int task_id = 4;
+  trigger_job_done();
+  do {
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+    WASTE_USEC(LONG_CALC);
+    // timing_end(0);
+    trigger_job_done();
+  } while (1);
+}
+
+static void prv_FlightControlTask(void *pvParameters) {
+  const int task_id = 5;
+  TickType_t xLastWakeTime = initial_release_time;
+  const TickType_t xFrequency = 9 / portTICK_PERIOD_MS;
+  do {
+    // timing_start(1 | TIMING_POINT_IS_HIGHEST_PRIO);
+    WASTE_USEC(LONG_CALC);
+    xTaskNotify(xTask_FlightControlAttitudeTask, 0, eNoAction);
+    xTaskNotify(xTask_FlightControlActuateTask, 0, eNoAction);
+    xTaskNotify(xTask_MavlinkSendTask, 0, eNoAction);
+    WASTE_USEC(LONG_CALC);
+    trigger_job_done();
+    xTaskDelayUntil( &xLastWakeTime, xFrequency );
+  } while (1);
+}
+
+static void prv_FlightControlAttitudeTask(void *pvParameters) {
+  const int task_id = 6;
+  trigger_job_done();
+  do {
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+    WASTE_USEC(LONG_CALC);
+    trigger_job_done();
+  } while (1);
+}
+
+static void prv_FlightControlActuateTask(void *pvParameters) {
+  const int task_id = 7;
+  trigger_job_done();
+  do {
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+    WASTE_USEC(LONG_CALC);
+    trigger_job_done();
+  } while (1);
+}
+
+static void prv_MavlinkSendTask(void *pvParameters) {
+  const int task_id = 8;
+  trigger_job_done();
+  do {
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+    WASTE_USEC(LONG_CALC);
+    xSemaphoreTake(xSemaphore_SPIBus, 1);
+    WASTE_USEC(SHORT_CALC);
+    // NVIC_SetPendingIRQ(1);
+    xSemaphoreGive(xSemaphore_SPIBus);
+    WASTE_USEC(LONG_CALC);
+    // timing_end(1);
+    trigger_job_done();
+  } while (1);
+}
+
+static void prv_CopterControlTask(void *pvParameters) {
+  const int task_id = 9;
+  trigger_job_done();
+  do {
+    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+    WASTE_USEC(LONG_CALC);
+    vTaskSuspendAll();
+    WASTE_USEC(SHORT_CALC);
+    xTaskResumeAll();
+    WASTE_USEC(LONG_CALC);
+    // timing_end(2);
+
+    trigger_job_done();
+  } while (1);
+}
+
+void ISR_0_Handler(void) {
+  const int task_id = 10;
+  // timing_start(2 | TIMING_POINT_START_INTERRUPT_FROM_IDLE);
+  WASTE_USEC(SHORT_CALC);
+  xTaskNotifyFromISR(xTask_CopterControlTask, 0, eNoAction, NULL);
+  WASTE_USEC(SHORT_CALC);
+}
+
+static void prv_CopterControlWatchdogTask(void *pvParameters) {
+  const int task_id = 11;
+  TickType_t xLastWakeTime = initial_release_time;
+  const TickType_t xFrequency = 10 / portTICK_PERIOD_MS;
+  do {
+    WASTE_USEC(LONG_CALC);
+    trigger_job_done();
+    xTaskDelayUntil( &xLastWakeTime, xFrequency );
+  } while (1);
+}
+
+// void PreIdleHook() {
+//   static int count = 0;
+//   kout << "---\n";
+//   timing_print();
+//   if (++count > 5) {
+//   }
+
+//   if (count == 100) {
+//     timing_dump();
+//   }
+// }
+
+// void ISR_1_Handler(void) {
+//   // timing_start(2 | TIMING_POINT_START_INTERRUPT_FROM_IDLE);
+//   WASTE_USEC(SHORT_CALC);
+// }
+// void ISR_3_Handler(void) {
+//   // timing_start(2 | TIMING_POINT_START_INTERRUPT_FROM_IDLE);
+//   WASTE_USEC(SHORT_CALC);
+// }
+// void ISR_4_Handler(void) {
+//   // timing_start(2 | TIMING_POINT_START_INTERRUPT_FROM_IDLE);
+//   WASTE_USEC(SHORT_CALC);
+// }
+// void ISR_5_Handler(void) {
+//   // timing_start(2 | TIMING_POINT_START_INTERRUPT_FROM_IDLE);
+//   WASTE_USEC(SHORT_CALC);
+// }
+// void ISR_6_Handler(void) {
+//   // timing_start(2 | TIMING_POINT_START_INTERRUPT_FROM_IDLE);
+//   WASTE_USEC(SHORT_CALC);
+// }
+// void ISR_7_Handler(void) {
+//   // timing_start(2 | TIMING_POINT_START_INTERRUPT_FROM_IDLE);
+//   WASTE_USEC(SHORT_CALC);
+// }
+// void ISR_8_Handler(void) {
+//   // timing_start(2 | TIMING_POINT_START_INTERRUPT_FROM_IDLE);
+//   WASTE_USEC(SHORT_CALC);
+// }
\ No newline at end of file
diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_release.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_release.c
index 1436a05c..5aa3d7b2 100644
--- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_release.c
+++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_release.c
@@ -149,6 +149,20 @@ static TickType_t initial_release_time = 0;
 
 static SemaphoreHandle_t xMutex;
 
+#define HYPER_PERIOD 25
+#define SIMULATE_PERIODS 2
+static TaskHandle_t  xTaskTimeSupervisor = NULL;
+static void timing_supervisor_task( void * pvParameters ) {
+    initial_release_time = xTaskGetTickCount(); // The highest priority task sets the initial time
+    TickType_t xLastWakeTime = initial_release_time;
+    const TickType_t xFrequency = (SIMULATE_PERIODS * HYPER_PERIOD) / portTICK_PERIOD_MS;
+    trigger_job_done();
+    xTaskDelayUntil( &xLastWakeTime, xFrequency );
+    trigger_job_done();
+    trigger_Qemu_break();
+    for( ;; ){}
+}
+
 void main_release( void )
 {
     xMutex = xSemaphoreCreateBinary();
@@ -193,6 +207,13 @@ void main_release( void )
                     mainTASK_5_PRIO,
                     &xTask5 );
 
+    xTaskCreate( timing_supervisor_task,
+                    "supervisor",
+                    configMINIMAL_STACK_SIZE,
+                    NULL,
+                    configMAX_PRIORITIES - 1,
+                    &xTaskTimeSupervisor );
+
     /* Start the tasks and timer running. */
     // puts("Start scheduler");
     vTaskStartScheduler();
diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_waters.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_waters.c
index c56fe155..d52cf747 100644
--- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_waters.c
+++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_waters.c
@@ -75,7 +75,6 @@ __attribute__((noinline)) static void trigger_job_done( void )
 #endif
 
 // Begin Input Stuff
-#define PARTITION_INPUT
 #define NUM_TASKS 10
 #define MAX_INPUT_BYTES 4096
 volatile unsigned char FUZZ_INPUT[MAX_INPUT_BYTES] = {};
diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_watersv2.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_watersv2.c
index 1d31eb81..9efe0a91 100644
--- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_watersv2.c
+++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_watersv2.c
@@ -78,7 +78,6 @@ __attribute__((noinline)) static void trigger_job_done( void )
 // #define INTERRUPT_ACTIVATION 1
 
 // Begin Input Stuff
-#define PARTITION_INPUT
 #define NUM_TASKS 10
 #define MAX_INPUT_BYTES 4096
 volatile unsigned char FUZZ_INPUT[MAX_INPUT_BYTES] = {};