void systemTask(void *arg) {
    bool pass = true;

    //Init the high-levels modules
    systemInit();

#ifndef USE_UART_CRTP
#ifdef UART_OUTPUT_TRACE_DATA
    debugInitTrace();
#endif
#ifdef HAS_UART
    uartInit();
#endif
#endif //ndef USE_UART_CRTP
    commInit();

    DEBUG_PRINT("Crazyflie is up and running!\n");
    DEBUG_PRINT("Build %s:%s (%s) %s\n", V_SLOCAL_REVISION, V_SREVISION, V_STAG, (V_MODIFIED) ? "MODIFIED" : "CLEAN");
    DEBUG_PRINT("I am 0x%X%X%X and I have %dKB of flash!\n", *((int* )(0x1FFFF7E8 + 8)), *((int* )(0x1FFFF7E8 + 4)), *((int* )(0x1FFFF7E8 + 0)), *((short* )(0x1FFFF7E0)));

    commanderInit();
    stabilizerInit();

    //Test the modules
    pass &= systemTest();
    pass &= commTest();
    pass &= commanderTest();
    pass &= stabilizerTest();

    //Start the firmware
    if (pass) {
        systemStart();
        ledseqRun(LED_RED, seq_alive);
        ledseqRun(LED_GREEN, seq_testPassed);
    } else {
        if (systemTest()) {
            while (1) {
                ledseqRun(LED_RED, seq_testPassed); //Red passed == not passed!
                vTaskDelay(M2T(2000) );
            }
        } else {
            ledInit();
            ledSet(LED_RED, true);
        }
    }

    workerLoop();

    //Should never reach this point!
    while (1)
        vTaskDelay(portMAX_DELAY);
}
void systemTask(void *arg)
{
  bool pass = true;
  
  ledInit();
  ledSet(CHG_LED, 1);

  uartInit();
  //Init the high-levels modules
  systemInit();

#ifndef USE_RADIOLINK_CRTP
#ifdef UART_OUTPUT_TRACE_DATA
  //debugInitTrace();
#endif
#ifdef ENABLE_UART
//  uartInit();
#endif
#endif //ndef USE_RADIOLINK_CRTP

  commInit();

  DEBUG_PRINT("----------------------------\n");
  DEBUG_PRINT("Crazyflie is up and running!\n");
  DEBUG_PRINT("Build %s:%s (%s) %s\n", V_SLOCAL_REVISION,
              V_SREVISION, V_STAG, (V_MODIFIED)?"MODIFIED":"CLEAN");
  DEBUG_PRINT("I am 0x%X%X%X and I have %dKB of flash!\n",
              *((int*)(MCU_ID_ADDRESS+8)), *((int*)(MCU_ID_ADDRESS+4)),
              *((int*)(MCU_ID_ADDRESS+0)), *((short*)(MCU_FLASH_SIZE_ADDRESS)));

  commanderInit();
  stabilizerInit();
  expbrdInit();
  memInit();
  
  //Test the modules
  pass &= systemTest();
  pass &= configblockTest();
  pass &= commTest();
  pass &= commanderTest();
  pass &= stabilizerTest();
  pass &= expbrdTest();
  pass &= memTest();
  
  //Start the firmware
  if(pass)
  {
    selftestPassed = 1;
    systemStart();
    ledseqRun(SYS_LED, seq_alive);
    ledseqRun(LINK_LED, seq_testPassed);
  }
  else
  {
    selftestPassed = 0;
    if (systemTest())
    {
      while(1)
      {
        ledseqRun(SYS_LED, seq_testPassed); //Red passed == not passed!
        vTaskDelay(M2T(2000));
        // System can be forced to start by setting the param to 1 from the cfclient
        if (selftestPassed)
        {
	        DEBUG_PRINT("Start forced.\n");
          systemStart();
          break;
        }
      }
    }
    else
    {
      ledInit();
      ledSet(SYS_LED, true);
    }
  }
  DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize());
  
  workerLoop();
  
  //Should never reach this point!
  while(1)
    vTaskDelay(portMAX_DELAY);
}
void systemTask(void *arg)
{
  bool pass = true;

  ledInit();
  ledSet(CHG_LED, 1);

#ifdef DEBUG_QUEUE_MONITOR
  queueMonitorInit();
#endif

#ifdef ENABLE_UART1
  uart1Init();
#endif
#ifdef ENABLE_UART2
  uart2Init();
#endif

  //Init the high-levels modules
  systemInit();
  commInit();
  commanderInit();

  StateEstimatorType estimator = anyEstimator;
  deckInit();
  estimator = deckGetRequiredEstimator();
  stabilizerInit(estimator);
  if (deckGetRequiredLowInterferenceRadioMode())
  {
    platformSetLowInterferenceRadioMode();
  }
  soundInit();
  memInit();

#ifdef PROXIMITY_ENABLED
  proximityInit();
#endif

  //Test the modules
  pass &= systemTest();
  pass &= configblockTest();
  pass &= commTest();
  pass &= commanderTest();
  pass &= stabilizerTest();
  pass &= deckTest();
  pass &= soundTest();
  pass &= memTest();
  pass &= watchdogNormalStartTest();

  //Start the firmware
  if(pass)
  {
    selftestPassed = 1;
    systemStart();
    soundSetEffect(SND_STARTUP);
    ledseqRun(SYS_LED, seq_alive);
    ledseqRun(LINK_LED, seq_testPassed);
  }
  else
  {
    selftestPassed = 0;
    if (systemTest())
    {
      while(1)
      {
        ledseqRun(SYS_LED, seq_testPassed); //Red passed == not passed!
        vTaskDelay(M2T(2000));
        // System can be forced to start by setting the param to 1 from the cfclient
        if (selftestPassed)
        {
	        DEBUG_PRINT("Start forced.\n");
          systemStart();
          break;
        }
      }
    }
    else
    {
      ledInit();
      ledSet(SYS_LED, true);
    }
  }
  DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize());

  workerLoop();

  //Should never reach this point!
  while(1)
    vTaskDelay(portMAX_DELAY);
}
void systemTask(void *arg)
{
  bool pass = true;

  ledInit();
  ledSet(CHG_LED, 1);

#ifdef DEBUG_QUEUE_MONITOR
  queueMonitorInit();
#endif

#ifdef PLATFORM_CF1
  uartInit();
#endif

#ifdef ENABLE_UART1
  uart1Init();
#endif
#ifdef ENABLE_UART2
  uart2Init();
#endif

  //Init the high-levels modules
  systemInit();

#ifndef USE_RADIOLINK_CRTP
#ifdef UART_OUTPUT_TRACE_DATA
  //debugInitTrace();
#endif
#ifdef ENABLE_UART
//  uartInit();
#endif
#endif //ndef USE_RADIOLINK_CRTP

  commInit();
  commanderInit();
  stabilizerInit();
#ifdef PLATFORM_CF2
  deckInit();
  #endif
  soundInit();
  memInit();

#ifdef PROXIMITY_ENABLED
  proximityInit();
#endif

  //Test the modules
  pass &= systemTest();
  pass &= configblockTest();
  pass &= commTest();
  pass &= commanderTest();
  pass &= stabilizerTest();
#ifdef PLATFORM_CF2
  pass &= deckTest();
  #endif
  pass &= soundTest();
  pass &= memTest();
  pass &= watchdogNormalStartTest();

  //Start the firmware
  if(pass)
  {
    selftestPassed = 1;
    systemStart();
    soundSetEffect(SND_STARTUP);
    ledseqRun(SYS_LED, seq_alive);
    ledseqRun(LINK_LED, seq_testPassed);
  }
  else
  {
    selftestPassed = 0;
    if (systemTest())
    {
      while(1)
      {
        ledseqRun(SYS_LED, seq_testPassed); //Red passed == not passed!
        vTaskDelay(M2T(2000));
        // System can be forced to start by setting the param to 1 from the cfclient
        if (selftestPassed)
        {
	        DEBUG_PRINT("Start forced.\n");
          systemStart();
          break;
        }
      }
    }
    else
    {
      ledInit();
      ledSet(SYS_LED, true);
    }
  }
  DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize());

  workerLoop();

  //Should never reach this point!
  while(1)
    vTaskDelay(portMAX_DELAY);
}