void* workerFunction(void* arg) { struct worker* worker = arg; printf("Creating worker on tid %u\n", (unsigned int)pthread_self()); /* int s; cpu_set_t cpuset; pthread_t thread; thread = pthread_self(); */ // sgenrand(worker->cpu_num, worker->config->random_seed); struct timeval timestamp; gettimeofday(×tamp, NULL); int seed=(timestamp.tv_usec+worker->cpu_num)%2309; sgenrand(seed, &(worker->myMT19937p)); // sgenrand(worker->config->random_seed, &(worker->myMT19937p)); /* Set affinity mask to include CPUs 0 to 7 */ /* CPU_ZERO(&cpuset); CPU_SET(worker->cpu_num, &cpuset); s = pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset); if (s != 0) { printf("Couldn't set CPU affinity\n"); } else { printf("Set tid %u affinity to CPU %d\n", (unsigned int)pthread_self(), worker->cpu_num); } */ workerLoop(worker); return NULL; }//End workerFunction()
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 uartInit(); #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(); commanderAdvancedInit(); stabilizerInit(); #ifdef PLATFORM_CF2 deckInit(); #endif soundInit(); memInit(); #ifdef PROXIMITY_ENABLED proximityInit(); #endif //Test the modules pass &= systemTest(); pass &= configblockTest(); pass &= commTest(); pass &= commanderAdvancedTest(); 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); }