// 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;
}
