// FreeRTOS Task to test the Motors driver with a rampup of each motor alone. void motorsTestTask(void* params) { int step=0; float rampup = 0.01; motorsSetupMinMaxPos(); motorsSetRatio(MOTOR_LEFT, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_REAR, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_RIGHT, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_FRONT, 1*(1<<16) * 0.0); vTaskDelay(M2T(1000)); while(1) { vTaskDelay(M2T(100)); motorsSetRatio(MOTOR_LEFT, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_REAR, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_RIGHT, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_FRONT, 1*(1<<16) * rampup); rampup += 0.001; if (rampup >= 0.1) { if(++step>3) step=0; rampup = 0.01; } } }
// FreeRTOS Task to test the Motors driver with a rampup of each motor alone. void motorsTestTask(void* params) { int step=0; float rampup = 0.01; motorsSetRatio(MOTOR_M4, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_M3, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_M2, 1*(1<<16) * 0.0); motorsSetRatio(MOTOR_M1, 1*(1<<16) * 0.0); vTaskDelay(M2T(1000)); while(1) { vTaskDelay(M2T(100)); motorsSetRatio(MOTOR_M4, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_M3, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_M2, 1*(1<<16) * rampup); motorsSetRatio(MOTOR_M1, 1*(1<<16) * rampup); rampup += 0.001; if (rampup >= 0.1) { if(++step>3) step=0; rampup = 0.01; } } }
void vApplicationIdleHook( void ) { extern size_t debugPrintTCBInfo(void); static uint32_t timeToPrint = M2T(5000); if (xTaskGetTickCount() - timeToPrint > M2T(10000)) { timeToPrint = xTaskGetTickCount(); debugPrintTCBInfo(); } // Enter sleep mode // { __asm volatile ("wfi"); } }
rt_bool_t motorsTest(void) { #ifndef BRUSHLESS_MOTORCONTROLLER int i; for (i = 0; i < sizeof(MOTORS) / sizeof(*MOTORS); i++) { motorsSetRatio(MOTORS[i], MOTORS_TEST_RATIO); rt_thread_delay(M2T(MOTORS_TEST_ON_TIME_MS)); motorsSetRatio(MOTORS[i], 0); rt_thread_delay(M2T(MOTORS_TEST_DELAY_TIME_MS)); } #endif return isInit; }
rt_err_t crtpReceivePacketWait(CRTPPort portId, CRTPPacket *p, int wait) { RT_ASSERT(queues[portId]); RT_ASSERT(p); // return xQueueReceive(queues[portId], p, M2T(wait)); return rt_mq_recv(queues[portId], p, sizeof(CRTPPacket), M2T(wait)); }
static int logCreateBlock(unsigned char id, struct ops_setting * settings, int len) { int i; for (i=0; i<LOG_MAX_BLOCKS; i++) if (id == logBlocks[i].id) return EEXIST; for (i=0; i<LOG_MAX_BLOCKS; i++) if (logBlocks[i].id == BLOCK_ID_FREE) break; if (i == LOG_MAX_BLOCKS) return ENOMEM; logBlocks[i].id = id; //logBlocks[i].timer = xTimerCreate( (const signed char *)"logTimer", M2T(1000), // pdRT_TRUE, &logBlocks[i], logBlockTimed ); logBlocks[i].timer = rt_timer_create("logTimer", logBlockTimed, &logBlocks[i], M2T(1000), RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER); logBlocks[i].ops = RT_NULL; if (logBlocks[i].timer == RT_NULL) { logBlocks[i].id = BLOCK_ID_FREE; return ENOMEM; } DEBUG("Added block ID %d\n", id); return logAppendBlock(id, settings, len); }
void crtpRxTask(void *param) { CRTPPacket p; static unsigned int droppedPacket=0; while (true) { if (link != &nopLink) { if (!link->receivePacket(&p)) { if (queues[p.port]) { // The queue is only 1 long, so if the last packet hasn't been processed, we just replace it xQueueOverwrite(queues[p.port], &p); } else { droppedPacket++; } if (callbacks[p.port]) callbacks[p.port](&p); //Dangerous? } } else { vTaskDelay(M2T(10)); } } }
bool usbSendData(uint32_t size, uint8_t* data) { outStage.size = size; memcpy(outStage.data, data, size); // Dont' block when sending return (xQueueSend(usbDataTx, &outStage, M2T(100)) == pdTRUE); }
static int logStartBlock(int id, unsigned int period) { int i; for (i=0; i<LOG_MAX_BLOCKS; i++) if (logBlocks[i].id == id) break; if (i >= LOG_MAX_BLOCKS) { ERROR("Trying to start block id %d that doesn't exist.", id); return ENOENT; } DEBUG("Starting block %d with period %dms\n", id, period); if (period>0) { xTimerChangePeriod(logBlocks[i].timer, M2T(period), 100); xTimerStart(logBlocks[i].timer, 100); } else { // single-shoot run workerSchedule(logRunBlock, &logBlocks[i]); } return 0; }
static int logCreateBlock(unsigned char id, struct ops_setting * settings, int len) { int i; for (i=0; i<LOG_MAX_BLOCKS; i++) if (id == logBlocks[i].id) return EEXIST; for (i=0; i<LOG_MAX_BLOCKS; i++) if (logBlocks[i].id == BLOCK_ID_FREE) break; if (i == LOG_MAX_BLOCKS) return ENOMEM; logBlocks[i].id = id; logBlocks[i].timer = xTimerCreate( (const signed char *)"logTimer", M2T(1000), pdTRUE, &logBlocks[i], logBlockTimed ); logBlocks[i].ops = NULL; if (logBlocks[i].timer == NULL) { logBlocks[i].id = BLOCK_ID_FREE; return ENOMEM; } DEBUG("Added block ID %d\n", id); return logAppendBlock(id, settings, len); }
static bool owSyslinkTransfer(uint8_t type, uint8_t length) { SyslinkPacket slp; ASSERT(length <= SYSLINK_MTU); slp.type = type; slp.length = length; memcpy(slp.data, &owCmdBuf, length); syslinkSendPacket(&slp); // Wait for reply if (xSemaphoreTake(waitForReply, M2T(5000)) == pdTRUE) //if (xSemaphoreTake(waitForReply, portMAX_DELAY)) { // We have now got a reply and *owCmd has been filled with data if (owDataIsValid) { owDataIsValid = false; return true; } } else { DEBUG_PRINT("Cmd 0x%X timeout.\n", slp.type); } return false; }
int crtpReceivePacketWait(CRTPPort portId, CRTPPacket *p, int wait) { ASSERT(queues[portId]); ASSERT(p); return xQueueReceive(queues[portId], p, M2T(wait)); }
static void eskylinkInitPairing(void) { int i; //Power the radio, Enable the DR interruption, set the radio in PRX mode with 2bytes CRC nrfWrite1Reg(REG_CONFIG, 0x3F); vTaskDelay(M2T(2)); //Wait for the chip to be ready //Set the radio channel, pairing channel is 50 nrfSetChannel(50); //Set the radio data rate nrfSetDatarate(RADIO_RATE_1M); nrfWrite1Reg(REG_SETUP_AW, VAL_SETUP_AW_3B); // 3 bytes address address[0] = address[1] = address[2] = 0; nrfWriteReg(REG_RX_ADDR_P0, address, 3); // Pipe address == 0 nrfWrite1Reg(REG_EN_RXADDR, 0x01); nrfWrite1Reg(REG_FEATURE, 0x00); // No dynamic size payload nrfWrite1Reg(REG_DYNPD, 0x00); nrfWrite1Reg(REG_RX_PW_P0, 13); //13 bytes payload nrfWrite1Reg(REG_EN_AA, 0); //Disable shockburst //Flush RX for(i=0;i<3;i++) nrfFlushRx(); //Flush TX for(i=0;i<3;i++) nrfFlushTx(); }
void imuInit(void) { if(isInit) return; // Wait for sensors to startup while (xTaskGetTickCount() < M2T(IMU_STARTUP_TIME_MS)); /* Initialize the IMU 6-axes */ BSP_IMU_6AXES_Init(); BSP_IMU_6AXES_X_Set_ODR(500.0f); BSP_IMU_6AXES_X_Set_FS(8.0f); BSP_IMU_6AXES_G_Set_ODR(500.0f); BSP_IMU_6AXES_G_Set_FS(2000.0f); BSP_MAGNETO_Init(); BSP_PRESSURE_Init(); imuBiasInit(&gyroBias); #ifdef IMU_TAKE_ACCEL_BIAS imuBiasInit(&accelBias); #endif varianceSampleTime = -GYRO_MIN_BIAS_TIMEOUT_MS + 1; imuAccLpfAttFactor = IMU_ACC_IIR_LPF_ATT_FACTOR; isInit = true; }
void vApplicationIdleHook( void ) { static uint32_t tickOfLatestWatchdogReset = M2T(0); portTickType tickCount = xTaskGetTickCount(); if (tickCount - tickOfLatestWatchdogReset > M2T(WATCHDOG_RESET_PERIOD_MS)) { tickOfLatestWatchdogReset = tickCount; watchdogReset(); } // Enter sleep mode. Does not work when debugging chip with SWD. // Currently saves about 20mA STM32F405 current consumption (~30%). #ifndef DEBUG { __asm volatile ("wfi"); } #endif }
static int radiolinkReceiveCRTPPacket(CRTPPacket *p) { if (xQueueReceive(crtpPacketDelivery, p, M2T(100)) == pdTRUE) { return 0; } return -1; }
static void usdInit(DeckInfo *info) { isInit = true; FATFS_AddDriver(&fatDrv, 0); timer = xTimerCreate( "usdTimer", M2T(SD_DISK_TIMER_PERIOD_MS), pdTRUE, NULL, usdTimer); xTimerStart(timer, 0); }
static int usblinkReceiveCRTPPacket(CRTPPacket *p) { if (xQueueReceive(crtpPacketDelivery, p, M2T(100)) == pdTRUE) { ledseqRun(LINK_LED, seq_linkup); return 0; } return -1; }
// FreeRTOS Task to test the Motors driver void motorsTestTask(void* params) { static const int sequence[] = {0.1*(1<<16), 0.15*(1<<16), 0.2*(1<<16), 0.25*(1<<16)}; int step=0; //Wait 3 seconds before starting the motors vTaskDelay(M2T(3000)); while(1) { motorsSetRatio(MOTOR_LEFT, sequence[step%4]); motorsSetRatio(MOTOR_REAR, sequence[(step+1)%4]); motorsSetRatio(MOTOR_RIGHT, sequence[(step+2)%4]); motorsSetRatio(MOTOR_FRONT, sequence[(step+3)%4]); if(++step>3) step=0; vTaskDelay(M2T(1000)); } }
bool ledTest(void) { ledSet(LED_GREEN_L, 1); ledSet(LED_GREEN_R, 1); ledSet(LED_RED_L, 0); ledSet(LED_RED_R, 0); vTaskDelay(M2T(250)); ledSet(LED_GREEN_L, 0); ledSet(LED_GREEN_R, 0); ledSet(LED_RED_L, 1); ledSet(LED_RED_R, 1); vTaskDelay(M2T(250)); // LED test end ledClearAll(); ledSet(LED_BLUE_L, 1); return isInit; }
static void commanderCacheSelectorUpdate(void) { uint32_t tickNow = xTaskGetTickCount(); /* Check inputs and prioritize. CHANGED BY Carlitos: crtp higher than extrx */ if ((tickNow - crtpCache.timestamp) < M2T(commanderStabilizeTimeout)) { activeCache = &crtpCache; } else if ((tickNow - extrxCache.timestamp) < M2T(commanderStabilizeTimeout)) { activeCache = &extrxCache; } else if ((tickNow - crtpCache.timestamp) < M2T(commanderShutdownTimeout)) { activeCache = &crtpCache; commanderLevelRPY(); } else if ((tickNow - extrxCache.timestamp) < M2T(commanderShutdownTimeout)) { activeCache = &extrxCache; commanderLevelRPY(); } else { activeCache = &crtpCache; commanderDropToGround(); } }
// FreeRTOS Task to test the Motors driver void motorsTestTask(void* params) { static const int sequence[] = {0.1*(1<<16), 0.15*(1<<16), 0.2*(1<<16), 0.25*(1<<16)}; int step=0; //Wait 3 seconds before starting the motors rt_thread_delay(M2T(3000)); while(1) { motorsSetRatio(MOTOR_M4, sequence[step%4]); motorsSetRatio(MOTOR_M3, sequence[(step+1)%4]); motorsSetRatio(MOTOR_M2, sequence[(step+2)%4]); motorsSetRatio(MOTOR_M1, sequence[(step+3)%4]); if(++step>3) step=0; rt_thread_delay(M2T(1000)); } }
static void buzzerInit(DeckInfo *info) { piezoInit(); neffect = sizeof(effects)/sizeof(effects[0])-1; nmelody = sizeof(melodies)/sizeof(melodies[0])-1; timer = xTimerCreate( "buzztimer", M2T(10), pdTRUE, NULL, buzzTimer ); xTimerStart(timer, 100); }
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 soundInit(void) { if (isInit) return; neffect = sizeof(effects)/sizeof(effects[0])-1; timer = xTimerCreate("SoundTimer", M2T(10), pdTRUE, NULL, soundTimer); xTimerStart(timer, 100); isInit = true; }
bool lps25hInit(I2C_Dev *i2cPort) { if (isInit) return true; I2Cx = i2cPort; devAddr = LPS25H_I2C_ADDR; vTaskDelay(M2T(5)); isInit = true; return true; }
void crtpTxTask(void *param) { CRTPPacket p; while (true) { if (link != &nopLink) { if (xQueueReceive(txQueue, &p, portMAX_DELAY) == pdTRUE) { // Keep testing, if the link changes to USB it will go though while (link->sendPacket(&p) == false) { // Relaxation time vTaskDelay(M2T(10)); } } } else { vTaskDelay(M2T(10)); } } }
void systemTask(void *arg) { bool pass = true; /* Init the high-levels modules */ systemInit(); uartInit(); commInit(); stabilizerInit(); //Test the modules pass &= systemTest(); pass &= commTest(); // pass &= commanderTest(); pass &= stabilizerTest(); 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); } } pmSetChargeState(charge500mA); //Should never reach this point! while(1) vTaskDelay(portMAX_DELAY); }
static int radiolinkSendCRTPPacket(CRTPPacket *p) { static SyslinkPacket slp; ASSERT(p->size <= CRTP_MAX_DATA_SIZE); slp.type = SYSLINK_RADIO_RAW; slp.length = p->size + 1; memcpy(slp.data, &p->header, p->size + 1); if (xQueueSend(txQueue, &slp, M2T(100)) == pdTRUE) { return true; } return false; }
bool sensorsTest(void) { bool testStatus = true; if (!isInit) { DEBUG_PRINT("Error while initializing sensor task\r\n"); testStatus = false; } // Try for 3 seconds so the quad has stabilized enough to pass the test for (int i = 0; i < 300; i++) { if(mpu6500SelfTest() == true) { isMpu6500TestPassed = true; break; } else { vTaskDelay(M2T(10)); } } testStatus &= isMpu6500TestPassed; #ifdef SENSORS_ENABLE_MAG_AK8963 testStatus &= isMagnetometerPresent; if (testStatus) { isAK8963TestPassed = ak8963SelfTest(); testStatus = isAK8963TestPassed; } #endif #ifdef SENSORS_ENABLE_PRESSURE_LPS25H testStatus &= isBarometerPresent; if (testStatus) { isLPS25HTestPassed = lps25hSelfTest(); testStatus = isLPS25HTestPassed; } #endif return testStatus; }