2 files changed, 27 insertions, 17 deletions
diff --git a/software/mpu/heartbeat.c b/software/mpu/heartbeat.c
index 21a3375..5838c11 100644
--- a/software/mpu/heartbeat.c
+++ b/software/mpu/heartbeat.c
@@ -50,6 +50,7 @@ static msg_t Heartbeat(void *arg)
     palSetPad(GPIOA, GPIOA_LED);
     chThdSleepMilliseconds(850);
   }
+  return 0; /* Never executed.*/
 }
 
 void heartbeatInit(void)
diff --git a/software/mpu/main.c b/software/mpu/main.c
index a737d9b..9224531 100644
--- a/software/mpu/main.c
+++ b/software/mpu/main.c
@@ -47,6 +47,28 @@ static void reset2bootloader(void)
       /* TODO: not just reset but go to bootloader */
 }
 
+static void shutdown(void)
+{
+  /*  Safely stop critical threads, aka call shutdown functions of all subsystems */
+     /* nothing here. */
+
+  /*  Invoke the xxxStop() method on all the active device drivers */
+     /* nothing here. */
+
+  chSysDisable();
+
+  /* Stop the system timer whose service routine invokes chSysTimerHandlerI(). */
+  SysTick->CTRL &= ~(SysTick_CTRL_ENABLE_Msk);
+
+  /* Disable any other interrupt source that may invoke OS APIs */
+     /* nothing here. */
+
+  /* Perform any application related de-initialization. */
+     /* nothing here. */
+
+  chSysEnable();
+}
+
 /*
  * Application entry point.
  */
@@ -61,28 +83,15 @@ int main(void)
    */
   halInit();
   chSysInit();
+  /* the base os is ready now */
 
+  /* call init functions of all subsystems which should spawn threads for their task */
   heartbeatInit();
-  int8_t ret = usbCDCShellRun();
 
+  int8_t ret = usbCDCShellRun();
   /* the debug shell returned - we have to halt, reboot or goto bootloader */
 
-  /*  Safely stop critical threads */
-     /* nothing here. */
-
-  /*  Invoke the xxxStop() method on all the active device drivers */
-  chSysDisable();
-
-  /* Stop the system timer whose service routine invokes chSysTimerHandlerI(). */
-  SysTick->CTRL &= ~(SysTick_CTRL_ENABLE_Msk);
-
-  /* Disable any other interrupt source that may invoke OS APIs */
-     /* nothing here. */
-
-  /* Perform any application related de-initialization. */
-     /* nothing here. */
-
-  chSysEnable();
+  shutdown();
   /* the OS is stopped now */
 
   switch(ret) {