int main(void) { /* Enable GPIO block clock */ SYSCON_AHBPeriphClockCmd(SYSCON_AHBPeriph_GPIO, ENABLE); CoInitOS(); /* Create Blink Task 0 */ CoCreateTask( taskBlink0, (void *)0, PRIORITY_BLINK_TASK0, &stkBlink0[SIZE_BLINK_TASK0-1], SIZE_BLINK_TASK0 ); /* Create Blink Task 1 */ CoCreateTask( taskBlink1, (void *)0, PRIORITY_BLINK_TASK1, &stkBlink1[SIZE_BLINK_TASK1-1], SIZE_BLINK_TASK1 ); CoStartOS(); while(1); }
static void sem2_execute(void) { int i; StatusType err; for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++) { Task_Id[i] = 0; } sem1ID = CoCreateSem(0,5,EVENT_SORT_TYPE_PRIO); testAssert(sem1ID != (OS_EventID)-1,"#3");; Task_Id[0] = CoCreateTask(task1, "A", MAINTEST_PRIMARY_PRIORITY - 3, &Task_Stack[0][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE); Task_Id[1] = CoCreateTask(task1, "B", MAINTEST_PRIMARY_PRIORITY - 4, &Task_Stack[1][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE); Task_Id[2] = CoCreateTask(task1, "C", MAINTEST_PRIMARY_PRIORITY - 1, &Task_Stack[2][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE); Task_Id[3] = CoCreateTask(task1, "D", MAINTEST_PRIMARY_PRIORITY - 5, &Task_Stack[3][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE); Task_Id[4] = CoCreateTask(task1, "E", MAINTEST_PRIMARY_PRIORITY - 2, &Task_Stack[4][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE); for(i = 0; i < 5; i++) { err = CoPostSem(sem1ID); } testAssertSequence("DBAEC"); err = CoDelSem(sem1ID,OPT_DEL_ANYWAY); testAssert(err == E_OK,"#4"); }
/** ******************************************************************************* * @brief main function * @param[in] None * @param[out] None * @retval None ******************************************************************************* */ int main (){ CoInitOS (); /*!< Initial CooCox CoOS */ /*!< Create three tasks */ CoCreateTask ((FUNCPtr)taskA,(void *)123,0,&taskA_stk[STACK_SIZE_TASKA-1],STACK_SIZE_TASKA); CoCreateTask ((FUNCPtr)taskB,(void *)&par,1,&taskB_stk[STACK_SIZE_TASKB-1],STACK_SIZE_TASKB); CoCreateTask ((FUNCPtr)taskC,(void *)0,2,&taskC_stk[STACK_SIZE_TASKC-1],STACK_SIZE_TASKC); CoStartOS (); /*!< Start multitask */ while (1); /*!< The code don''t reach here */ }
int main(void) { initializeBoard(); CoInitOS(); //CoCreateTask(USART_rx,0,0,&USART1_stk[STACK_SIZE_DEFAULT-1],STACK_SIZE_DEFAULT); CoCreateTask(blinkLED,0,0,&blinkLED_stk[STACK_SIZE_DEFAULT-1],STACK_SIZE_DEFAULT); CoCreateTask(btPRESS,0,1,&btPRESS_stk[STACK_SIZE_DEFAULT-1],STACK_SIZE_DEFAULT); //CoCreateTask(GPIO_LCD1602,0,0,&updateLCD_stk[STACK_SIZE_DEFAULT-1],STACK_SIZE_DEFAULT); CoStartOS(); while(1); }
/** * ----------------------------------------------------------------------------- * @brief mutex1_execute * @param[in] None * @param[out] None * @retval None * @par Description * @details The code of mutex test example 1. * ----------------------------------------------------------------------------- */ static void mutex1_execute (void) { unsigned char i; Cnt = 0; Del_Flag = 0; Exit_Flag = 0; CoTickDelay (1); for (i=0; i<MUTEX_NUM; i++) { Mutex_Id[i] = CoCreateMutex (); if (Mutex_Id[i] == E_CREATE_FAIL) { printf ("\r Create the %d mutex fail. \n",i+1); } } for (i=0; i< MAX_SLAVE_TEST_TASKS; i++) { Task_Id [i] = CoCreateTask (mutexTask01,(void*)i,MAINTEST_PRIMARY_PRIORITY-(i+1),&Task_Stack[i][SLAVE_TASK_STK_SIZE-1],SLAVE_TASK_STK_SIZE); testAssert((Task_Id[i] != E_CREATE_FAIL)," Create task fail #3 "); } while (Exit_Flag == 0) ; Del_Flag = 0; Exit_Flag = 0; for (i=0; i<MAX_SLAVE_TEST_TASKS; i++) { testAssert((Flag[i] == Sequence[i])," Mutex #1 "); } }
/** * ----------------------------------------------------------------------------- * @brief mutex2_execute * @param[in] None * @param[out] None * @retval None * @par Description * @details The code of mutex test example 3. * ----------------------------------------------------------------------------- */ static void mutex2_execute (void) { unsigned char i; Cnt = 0; Exit_Flag = 0; CoTickDelay (1); for (i=0;i<MAX_SLAVE_TEST_TASKS;i++ ){ Flag [i] = 0; } for (i=0; i< MAX_SLAVE_TEST_TASKS; i++) { Task_Id [i] = CoCreateTask (mutexTask02,(void*)i,MAINTEST_PRIMARY_PRIORITY-(i+1),&Task_Stack[i][SLAVE_TASK_STK_SIZE-1],SLAVE_TASK_STK_SIZE); if (Task_Id[i] == E_CREATE_FAIL) { printf ("\r Create the %d mutex task fail. \n",i+1); } } while (Exit_Flag == 0) ; Exit_Flag = 0; for (i=0; i<MAX_SLAVE_TEST_TASKS; i++) { testAssert((Flag[i] == Sequence2[i])," Mutex #2 "); } }
/** * ----------------------------------------------------------------------------- * @brief mutex3_execute * @param[in] None * @param[out] None * @retval None * @par Description * @details The code of mutex test example 2. * ----------------------------------------------------------------------------- */ static void mutex3_execute (void) { unsigned char i; Cnt = 0; Exit_Flag = 0; CoTickDelay (1); for (i=0;i<MAX_SLAVE_TEST_TASKS;i++ ){ Flag [i] = 0; } for (i=0; i< MAX_SLAVE_TEST_TASKS; i++) { Task_Id [i] = CoCreateTask (mutexTask03,(void*)i,MAINTEST_PRIMARY_PRIORITY-(i+1),&Task_Stack[i][SLAVE_TASK_STK_SIZE-1],SLAVE_TASK_STK_SIZE); testAssert((Task_Id[i] != E_CREATE_FAIL)," Mutex #3: Create mutex fail "); } while (Exit_Flag == 0) ; Exit_Flag = 0; for (i=0; i<MAX_SLAVE_TEST_TASKS; i++) { testAssert((Flag[i] == Sequence3[i])," Mutex #3: Test fail. "); } }
void controlInit(void) { AQ_NOTICE("Control init\n"); memset((void *)&controlData, 0, sizeof(controlData)); #ifdef USE_QUATOS quatosInit(AQ_INNER_TIMESTEP, AQ_OUTER_TIMESTEP); #endif utilFilterInit3(controlData.userPitchFilter, AQ_INNER_TIMESTEP, 0.1f, 0.0f); utilFilterInit3(controlData.userRollFilter, AQ_INNER_TIMESTEP, 0.1f, 0.0f); utilFilterInit3(controlData.navPitchFilter, AQ_INNER_TIMESTEP, 0.125f, 0.0f); utilFilterInit3(controlData.navRollFilter, AQ_INNER_TIMESTEP, 0.125f, 0.0f); controlData.pitchRatePID = pidInit(&p[CTRL_TLT_RTE_P], &p[CTRL_TLT_RTE_I], &p[CTRL_TLT_RTE_D], &p[CTRL_TLT_RTE_F], &p[CTRL_TLT_RTE_PM], &p[CTRL_TLT_RTE_IM], &p[CTRL_TLT_RTE_DM], &p[CTRL_TLT_RTE_OM], 0, 0, 0, 0); controlData.rollRatePID = pidInit(&p[CTRL_TLT_RTE_P], &p[CTRL_TLT_RTE_I], &p[CTRL_TLT_RTE_D], &p[CTRL_TLT_RTE_F], &p[CTRL_TLT_RTE_PM], &p[CTRL_TLT_RTE_IM], &p[CTRL_TLT_RTE_DM], &p[CTRL_TLT_RTE_OM], 0, 0, 0, 0); controlData.yawRatePID = pidInit(&p[CTRL_YAW_RTE_P], &p[CTRL_YAW_RTE_I], &p[CTRL_YAW_RTE_D], &p[CTRL_YAW_RTE_F], &p[CTRL_YAW_RTE_PM], &p[CTRL_YAW_RTE_IM], &p[CTRL_YAW_RTE_DM], &p[CTRL_YAW_RTE_OM], 0, 0, 0, 0); controlData.pitchAnglePID = pidInit(&p[CTRL_TLT_ANG_P], &p[CTRL_TLT_ANG_I], &p[CTRL_TLT_ANG_D], &p[CTRL_TLT_ANG_F], &p[CTRL_TLT_ANG_PM], &p[CTRL_TLT_ANG_IM], &p[CTRL_TLT_ANG_DM], &p[CTRL_TLT_ANG_OM], 0, 0, 0, 0); controlData.rollAnglePID = pidInit(&p[CTRL_TLT_ANG_P], &p[CTRL_TLT_ANG_I], &p[CTRL_TLT_ANG_D], &p[CTRL_TLT_ANG_F], &p[CTRL_TLT_ANG_PM], &p[CTRL_TLT_ANG_IM], &p[CTRL_TLT_ANG_DM], &p[CTRL_TLT_ANG_OM], 0, 0, 0, 0); controlData.yawAnglePID = pidInit(&p[CTRL_YAW_ANG_P], &p[CTRL_YAW_ANG_I], &p[CTRL_YAW_ANG_D], &p[CTRL_YAW_ANG_F], &p[CTRL_YAW_ANG_PM], &p[CTRL_YAW_ANG_IM], &p[CTRL_YAW_ANG_DM], &p[CTRL_YAW_ANG_OM], 0, 0, 0, 0); controlTaskStack = aqStackInit(CONTROL_STACK_SIZE, "CONTROL"); controlData.controlTask = CoCreateTask(controlTaskCode, (void *)0, CONTROL_PRIORITY, &controlTaskStack[CONTROL_STACK_SIZE-1], CONTROL_STACK_SIZE); }
/** ******************************************************************************* * Main function ******************************************************************************* */ int main() { RCC_Configuration(); /*!< Configure the system clocks */ NVIC_Configuration(); /*!< NVIC Configuration */ UART_Configuration (); /*!< UART configuration */ uart_printf ("\n\r\n\r"); uart_printf ("\r System initial... "); uart_printf (" [OK]. \n"); uart_printf ("\r System Clock have been configured as 60MHz!\n\n"); uart_printf ("\r +------------------------------------------------+ \n"); uart_printf ("\r | CooCox RTOS Demo for Cortex-M3 MCU(STM32F10x). | \n"); uart_printf ("\r | Demo in Keil MCBSTM32 Board. | \n"); uart_printf ("\r +------------------------------------------------+ \n\n"); uart_printf ("\r Initial CooCox RTOS... "); CoInitOS(); /*!< Initial CooCox RTOS */ uart_printf (" [OK]. \n"); uart_printf ("\r Create a \"task_init\" task... "); CoCreateTask(task_init, (void *)0, 10,&task_init_Stk[TASK_STK_SIZE-1], TASK_STK_SIZE); uart_printf (" [OK]. \n"); uart_printf ("\r Start multi-task. "); CoStartOS(); while (1); }
void runInit(void) { float acc[3], mag[3]; float pres; int i; memset((void *)&runData, 0, sizeof(runData)); runData.runFlag = CoCreateFlag(1, 0); // auto reset runTaskStack = aqStackInit(RUN_TASK_SIZE, "RUN"); runData.runTask = CoCreateTask(runTaskCode, (void *)0, RUN_PRIORITY, &runTaskStack[RUN_TASK_SIZE-1], RUN_TASK_SIZE); acc[0] = IMU_ACCX; acc[1] = IMU_ACCY; acc[2] = IMU_ACCZ; mag[0] = IMU_MAGX; mag[1] = IMU_MAGY; mag[2] = IMU_MAGZ; pres = AQ_PRESSURE; // initialize sensor history for (i = 0; i < RUN_SENSOR_HIST; i++) { runData.accHist[0][i] = acc[0]; runData.accHist[1][i] = acc[1]; runData.accHist[2][i] = acc[2]; runData.magHist[0][i] = mag[0]; runData.magHist[1][i] = mag[1]; runData.magHist[2][i] = mag[2]; runData.presHist[i] = pres; runData.sumAcc[0] += acc[0]; runData.sumAcc[1] += acc[1]; runData.sumAcc[2] += acc[2]; runData.sumMag[0] += mag[0]; runData.sumMag[1] += mag[1]; runData.sumMag[2] += mag[2]; runData.sumPres += pres; } runData.sensorHistIndex = 0; runData.bestHacc = 1000.0f; runData.accMask = 1000.0f; // use altUkf altitude & vertical velocity estimates to start with runData.altPos = &ALT_POS; runData.altVel = &ALT_VEL; #ifdef USE_MAVLINK // configure px4flow sensor // mavlinkSendParameter(81, 50, "BFLOW_V_THLD", 2500.0f); // mavlinkSendParameter(81, 50, "BFLOW_F_THLD", 100.0f); mavlinkSendParameter(81, 50, "BFLOW_GYRO_COM", 0.0f); mavlinkSendParameter(81, 50, "USB_SEND_VIDEO", 0.0f); #endif }
int main() { CoInitOS(); CoCreateTask(task_led, (void *)0, TASK_LED_PRI, &task_led_stack[TASK_LED_STACK_SIZE - 1], TASK_LED_STACK_SIZE); CoStartOS(); return 0; }
int main(void) { cpuInit(); uartInit(BAUDRATE); i2cInit(); TASKHANDLES System; System.flight_control.armed=0; System.flight_control.error=0; System.lock.i2c = CoCreateMutex ( ); CoInitOS(); CoCreateTask( ledTask, &System, PRIORITY_LED_TASK, &stkLED[SIZE_LED_TASK-1], SIZE_LED_TASK ); CoCreateTask( radioTask, &System, PRIORITY_RADIO_TASK, &stkRADIO[SIZE_RADIO_TASK-1], SIZE_RADIO_TASK ); CoCreateTask( flightTask, &System, PRIORITY_FLIGHT_TASK, &stkFLIGHT[SIZE_FLIGHT_TASK-1], SIZE_FLIGHT_TASK ); printf("Starting OS!!\n"); CoStartOS(); printf("ERROR"); while(1); }
void supervisorInit(void) { memset((void *)&supervisorData, 0, sizeof(supervisorData)); supervisorData.readyLed = digitalInit(SUPERVISOR_READY_PORT, SUPERVISOR_READY_PIN, 0); #ifdef SUPERVISOR_DEBUG_PORT supervisorData.debugLed = digitalInit(SUPERVISOR_DEBUG_PORT, SUPERVISOR_DEBUG_PIN, 0); #endif supervisorData.gpsLed = digitalInit(GPS_LED_PORT, GPS_LED_PIN, 0); supervisorData.state = STATE_INITIALIZING; supervisorTaskStack = aqStackInit(SUPERVISOR_STACK_SIZE, "SUPERVISOR"); supervisorData.supervisorTask = CoCreateTask(supervisorTaskCode, (void *)0, SUPERVISOR_PRIORITY, &supervisorTaskStack[SUPERVISOR_STACK_SIZE-1], SUPERVISOR_STACK_SIZE); }
void gpsInit(void) { const uint8_t conf[] = { 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A }; // set rate to 5Hz const uint8_t conf_[] = { 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47 }; // set POSLLH MSG const uint8_t conf__[] = { 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41 }; // set EGNOS #if 0 timerDelay(1000); confGPS(conf, sizeof(conf)); timerDelay(100); confGPS(conf_, sizeof(conf_)); confGPS(conf__, sizeof(conf__)); #endif // STATUS, SOL, VELNED are ON by default CoCreateTask(gpsTask, 0, 20, &gpsStack[GPS_STACK_SIZE - 1], GPS_STACK_SIZE); }
sys_thread_t sys_thread_new(const char *name, lwip_thread_fn thread, void *arg, int stacksize, int prio) { OS_TID tsk; OS_STK *stack; if (LWIP_TASK_MAX > current_task) { stack = (OS_STK*)CoKmalloc(stacksize); if (stack) { tsk = CoCreateTask(thread, arg, prio, &stack[(stacksize / sizeof(OS_STK)) - 1], stacksize); if (E_CREATE_FAIL != tsk) { current_task++; return tsk; } } } LWIP_DEBUGF(NETIF_DEBUG,("Lwip task created failed, maybe out of range !\n\r")); return E_CREATE_FAIL; }
// ------------------------------------------------------------------------ // main( ) - Main program creates task0 and then starts CoOS kernel. // // ------------------------------------------------------------------------ int main(void) { // initialize the LPCXpresso board by: // turning on clock signal for the GPIO peripheral. // setting GPIO port 0, bit22 for output (to drive LED2) initializeBoard(); // initialize the CoOS real time kernel CoInitOS(); // create task1 CoCreateTask(task1,0,0,&task1_stk[STACK_SIZE_DEFAULT-1],STACK_SIZE_DEFAULT); // start the CoOS real time kernel CoStartOS(); // you should never get here! while(1); }
/** ******************************************************************************* * @brief Initialize OS * @param[in] None * @param[out] None * @retval None * * @par Description * @details This function is called to initialize OS. * * @note You must call this function first,before any other OS API function * * @code There is a example for useage of this function,as follows: * e.g. * ... // Your target initial code. * * OsInit(); // Initial OS. * CreateTask(...); // Create tasks. * ... * OsStart(); // Start multitask. * @endcode ******************************************************************************* */ void CoInitOS(void) { InitSysTick(); /* Initialize system tick. */ InitInt(); /* Initialize PendSV,SVC,SysTick interrupt */ CreateTCBList(); /* Create TCB list. */ #if CFG_EVENT_EN > 0 CreateEventList(); /* Create event control list. */ #endif #if CFG_KHEAP_EN > 0 CoCreateKheap(); /* Create kernel heap within user define */ #endif OsSchedLock(); /* Lock Schedule */ /* Create first task -- IDLE task. */ CoCreateTask( CoIdleTask, Co_NULL, CFG_LOWEST_PRIO, &idle_stk[CFG_IDLE_STACK_SIZE-1], CFG_IDLE_STACK_SIZE ); /* Set PSP for CoIdleTask coming in */ SetEnvironment(&idle_stk[CFG_IDLE_STACK_SIZE-1]); }
static void sem3_execute(void) { StatusType err; U64 expectTime; int i; sem1ID = CoCreateSem(0,5,EVENT_SORT_TYPE_FIFO); // Test 1 AcceptSem expectTime = CoGetOSTime(); err = CoAcceptSem(sem1ID); testAssert(err == E_SEM_EMPTY,"#5"); testAssertTimeWindow(expectTime,expectTime + ALLOWED_DELAY_TICK); // Test 2 PendSem with timeout //printf("#5"); expectTime = CoGetOSTime() + (MAX_SLAVE_TEST_TASKS * 50); for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++) { err = CoPendSem(sem1ID,50); testAssert(err == E_TIMEOUT,"#6"); } testAssertTimeWindow(expectTime,expectTime + ALLOWED_DELAY_TICK); //printf("#6"); // Test 3 PendSem without timeout Task_Id[0] = CoCreateTask(task2, "A", MAINTEST_PRIMARY_PRIORITY - 3, &Task_Stack[0][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE); err = CoPendSem(sem1ID,100); testAssert(err == E_OK,"#8"); // PostSem only inc 1 && PendSem dec 1 //printf("#7"); err = CoPendSem(sem1ID,100); testAssert(err == E_TIMEOUT,"#9"); err = CoDelSem(sem1ID,OPT_DEL_NO_PEND); testAssert(err == E_OK,"#7"); }
/** ******************************************************************************* * @brief Set time task * @param[in] pdata A pointer to parameter passed to task. * @param[out] None * @retval None * * @details This task use to set time. ******************************************************************************* */ void time_set(void *pdata) { static unsigned char bct = 0; /* Button calc */ U16 evtmp = 0; pdata = pdata; for (;;) { evtmp = CoWaitForMultipleFlags(EVT_BUTTON_SEL|EVT_BUTTON_ADD,OPT_WAIT_ANY,0,&errinfo[20]); if(errinfo[20] != E_OK) uart_printf("\r Flag ERROR:\n"); if (evtmp == EVT_BUTTON_SEL) { bct++; switch (bct) { case 1: timeflag = 1; if(lcd_blink_id == 0) lcd_blink_id = CoCreateTask (lcd_blink,(void *)0, LCD_BLINK_PRI ,&lcd_blink_Stk[TASK_STK_SIZE - 1], TASK_STK_SIZE ); DisableRTCInterrupt(); CoClearFlag(time_display_flg); CoSetFlag(lcd_blink_flg); break; case 2: timeflag = 2; break; case 3: timeflag = 3; break; case 4: bct = 0; CoClearFlag(lcd_blink_flg); CoSetFlag(time_display_flg); EnableRTCInterrupt(); break; default: break; }; CoTickDelay(40); CoClearFlag(button_sel_flg); } else if(evtmp == EVT_BUTTON_ADD) { CoEnterMutexSection(mut_lcd); switch (bct) { case 1: time[0]++; if (time[0] >= 60) time[0] = 0; chart[6] = time[0]/10 + '0'; chart[7] = time[0]%10 + '0'; break; case 2: time[1]++; if (time[1] >= 60) time[1] = 0; chart[3] = time[1]/10 + '0'; chart[4] = time[1]%10 + '0'; break; case 3: time[2]++; if (time[2] >= 24) time[2] = 0; chart[0] = time[2]/10 + '0'; chart[1] = time[2]%10 + '0'; break; default: break; } set_cursor(7, 0); lcd_print (chart); CoLeaveMutexSection(mut_lcd); CoTickDelay(40); CoClearFlag(button_add_flg); } } }
void radioInit(void) { uint16_t radioType = (uint16_t)p[RADIO_TYPE]; int i; AQ_NOTICE("Radio init\n"); memset((void *)&radioData, 0, sizeof(radioData)); radioData.mode = (radioType>>12) & 0x0f; for (i = 0; i < RADIO_NUM; i++) { radioInstance_t *r = &radioData.radioInstances[i]; USART_TypeDef *uart; // determine UART switch (i) { case 0: uart = RC1_UART; break; #ifdef RC2_UART case 1: uart = RC2_UART; break; #endif #ifdef RC3_UART case 2: uart = RC3_UART; break; #endif default: uart = 0; break; } r->radioType = (radioType>>(i*4)) & 0x0f; r->channels = &radioData.allChannels[RADIO_MAX_CHANNELS * i]; utilFilterInit(&r->qualityFilter, (1.0f / 50.0f), 0.75f, 0.0f); switch (r->radioType) { case RADIO_TYPE_SPEKTRUM11: case RADIO_TYPE_SPEKTRUM10: case RADIO_TYPE_DELTANG: if (uart) { spektrumInit(r, uart); radioRCSelect(i, 0); AQ_PRINTF("Spektrum on RC port %d\n", i); } break; case RADIO_TYPE_SBUS: if (uart) { futabaInit(r, uart); radioRCSelect(i, 1); AQ_PRINTF("Futaba on RC port %d\n", i); } break; case RADIO_TYPE_PPM: ppmInit(r); AQ_PRINTF("PPM on RC port %d\n", i); break; case RADIO_TYPE_SUMD: if (uart) { grhottInit(r, uart); radioRCSelect(i, 0); AQ_PRINTF("GrHott on RC port %d\n", i); } break; case RADIO_TYPE_MLINK: if (uart) { mlinkrxInit(r, uart); radioRCSelect(i, 0); AQ_PRINTF("Mlink on RC port %d\n", i); } break; case RADIO_TYPE_NONE: break; default: AQ_NOTICE("WARNING: Invalid radio type!\n"); break; } } switch (radioData.mode) { case RADIO_MODE_DIVERSITY: // select first available radio to start with for (i = 0; i < RADIO_NUM; i++) { if (radioData.radioInstances[i].radioType > RADIO_TYPE_NONE) { radioMakeCurrent(&radioData.radioInstances[i]); break; } } break; case RADIO_MODE_SPLIT: radioMakeCurrent(&radioData.radioInstances[0]); break; } // set mode default radioData.channels[(int)p[RADIO_FLAP_CH]] = -700; radioTaskStack = aqStackInit(RADIO_STACK_SIZE, "RADIO"); radioData.radioTask = CoCreateTask(radioTaskCode, (void *)0, RADIO_PRIORITY, &radioTaskStack[RADIO_STACK_SIZE-1], RADIO_STACK_SIZE); }
void adcInit(void) { GPIO_InitTypeDef GPIO_InitStructure; DMA_InitTypeDef DMA_InitStructure; ADC_CommonInitTypeDef ADC_CommonInitStructure; ADC_InitTypeDef ADC_InitStructure; NVIC_InitTypeDef NVIC_InitStructure; AQ_NOTICE("ADC init\n"); memset((void *)&adcData, 0, sizeof(adcData)); // energize mag's set/reset circuit adcData.magSetReset = digitalInit(GPIOE, GPIO_Pin_10, 1); // use auto-zero function of gyros adcData.rateAutoZero = digitalInit(GPIOE, GPIO_Pin_8, 0); // bring ACC's SELF TEST line low adcData.accST = digitalInit(GPIOE, GPIO_Pin_12, 0); // bring ACC's SCALE line low (ADXL3X5 requires this line be tied to GND or left floating) adcData.accScale = digitalInit(GPIOC, GPIO_Pin_15, 0); GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7; GPIO_Init(GPIOA, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4 | GPIO_Pin_5; GPIO_Init(GPIOC, &GPIO_InitStructure); RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | RCC_APB2Periph_ADC3, ENABLE); adcData.sample = ADC_SAMPLES - 1; // Use STM32F4's Triple Regular Simultaneous Mode capable of ~ 6M samples per second DMA_DeInit(ADC_DMA_STREAM); DMA_InitStructure.DMA_Channel = ADC_DMA_CHANNEL; DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)adcDMAData.adc123Raw1; DMA_InitStructure.DMA_PeripheralBaseAddr = ((uint32_t)0x40012308); DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; DMA_InitStructure.DMA_BufferSize = ADC_CHANNELS * 3 * 2; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh; DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Enable; DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; DMA_Init(ADC_DMA_STREAM, &DMA_InitStructure); DMA_ITConfig(ADC_DMA_STREAM, DMA_IT_HT | DMA_IT_TC, ENABLE); DMA_ClearITPendingBit(ADC_DMA_STREAM, ADC_DMA_FLAGS); DMA_Cmd(ADC_DMA_STREAM, ENABLE); NVIC_InitStructure.NVIC_IRQChannel = ADC_DMA_IRQ; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); // ADC Common Init ADC_CommonStructInit(&ADC_CommonInitStructure); ADC_CommonInitStructure.ADC_Mode = ADC_TripleMode_RegSimult; ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2; ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1; ADC_CommonInit(&ADC_CommonInitStructure); // ADC1 configuration ADC_StructInit(&ADC_InitStructure); ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ScanConvMode = ENABLE; ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = 16; ADC_Init(ADC1, &ADC_InitStructure); ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGX, 1, ADC_SAMPLE_TIME); // magX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGX, 2, ADC_SAMPLE_TIME); // magX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGX, 3, ADC_SAMPLE_TIME); // magX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGX, 4, ADC_SAMPLE_TIME); // magX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGY, 5, ADC_SAMPLE_TIME); // magY ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGY, 6, ADC_SAMPLE_TIME); // magY ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGY, 7, ADC_SAMPLE_TIME); // magY ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGY, 8, ADC_SAMPLE_TIME); // magY ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGZ, 9, ADC_SAMPLE_TIME); // magZ ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGZ, 10, ADC_SAMPLE_TIME); // magZ ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGZ, 11, ADC_SAMPLE_TIME); // magZ ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGZ, 12, ADC_SAMPLE_TIME); // magZ ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_RATEX, 13, ADC_SAMPLE_TIME); // rateX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_RATEX, 14, ADC_SAMPLE_TIME); // rateX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_RATEX, 15, ADC_SAMPLE_TIME); // rateX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_RATEX, 16, ADC_SAMPLE_TIME); // rateX // Enable ADC1 DMA since ADC1 is the Master ADC_DMACmd(ADC1, ENABLE); // ADC2 configuration ADC_StructInit(&ADC_InitStructure); ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ScanConvMode = ENABLE; ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = 16; ADC_Init(ADC2, &ADC_InitStructure); ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_RATEY, 1, ADC_SAMPLE_TIME); // rateY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_RATEY, 2, ADC_SAMPLE_TIME); // rateY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_RATEY, 3, ADC_SAMPLE_TIME); // rateY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_RATEY, 4, ADC_SAMPLE_TIME); // rateY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCX, 5, ADC_SAMPLE_TIME); // accX ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCX, 6, ADC_SAMPLE_TIME); // accX ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCX, 7, ADC_SAMPLE_TIME); // accX ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCX, 8, ADC_SAMPLE_TIME); // accX ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCY, 9, ADC_SAMPLE_TIME); // accY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCY, 10, ADC_SAMPLE_TIME); // accY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCY, 11, ADC_SAMPLE_TIME); // accY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCY, 12, ADC_SAMPLE_TIME); // accY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCZ, 13, ADC_SAMPLE_TIME); // accZ ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCZ, 14, ADC_SAMPLE_TIME); // accZ ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCZ, 15, ADC_SAMPLE_TIME); // accZ ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCZ, 16, ADC_SAMPLE_TIME); // accZ // ADC3 configuration ADC_StructInit(&ADC_InitStructure); ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ScanConvMode = ENABLE; ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = 16; ADC_Init(ADC3, &ADC_InitStructure); ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_RATEZ, 1, ADC_SAMPLE_TIME); // rateZ ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_RATEZ, 2, ADC_SAMPLE_TIME); // rateZ ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_RATEZ, 3, ADC_SAMPLE_TIME); // rateZ ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_RATEZ, 4, ADC_SAMPLE_TIME); // rateZ ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_TEMP1, 5, ADC_SAMPLE_TIME); // temp1 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_TEMP2, 6, ADC_SAMPLE_TIME); // temp2 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES1, 7, ADC_SAMPLE_TIME); // pressure1 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES1, 8, ADC_SAMPLE_TIME); // pressure1 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES1, 9, ADC_SAMPLE_TIME); // pressure1 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES1, 10, ADC_SAMPLE_TIME); // pressure1 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_VIN, 11, ADC_SAMPLE_TIME); // Vin ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_TEMP3, 12, ADC_SAMPLE_TIME); // temp3 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES2, 13, ADC_SAMPLE_TIME); // pressure2 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES2, 14, ADC_SAMPLE_TIME); // pressure2 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES2, 15, ADC_SAMPLE_TIME); // pressure2 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES2, 16, ADC_SAMPLE_TIME); // pressure2 // Enable DMA request after last transfer (Multi-ADC mode) ADC_MultiModeDMARequestAfterLastTransferCmd(ENABLE); // Enable ADC_Cmd(ADC1, ENABLE); ADC_Cmd(ADC2, ENABLE); ADC_Cmd(ADC3, ENABLE); adcData.adcFlag = CoCreateFlag(1, 0); adcTaskStack = aqStackInit(ADC_STACK_SIZE, "ADC"); adcData.adcTask = CoCreateTask(adcTaskCode, (void *)0, ADC_PRIORITY, &adcTaskStack[ADC_STACK_SIZE-1], ADC_STACK_SIZE); // Start ADC1 Software Conversion ADC_SoftwareStartConv(ADC1); yield(100); // set initial temperatures adcData.temp1 = adcIDGVoltsToTemp(adcData.voltages[ADC_VOLTS_TEMP1]); adcData.temp2 = adcIDGVoltsToTemp(adcData.voltages[ADC_VOLTS_TEMP2]); adcData.temp3 = adcT1VoltsToTemp(adcData.voltages[ADC_VOLTS_TEMP3]); analogData.vIn = adcVsenseToVin(adcData.voltages[ADC_VOLTS_VIN]); adcCalibOffsets(); }
void menusTask(void * pdata) { opentxInit(); #if defined(PCBTARANIS) && defined(REV9E) while (1) { uint32_t pwr_check = pwrCheck(); if (pwr_check == e_power_off) { break; } else if (pwr_check == e_power_press) { continue; } #else while (pwrCheck() != e_power_off) { #endif U64 start = CoGetOSTime(); perMain(); // TODO remove completely massstorage from sky9x firmware U32 runtime = (U32)(CoGetOSTime() - start); // deduct the thread run-time from the wait, if run-time was more than // desired period, then skip the wait all together if (runtime < MENU_TASK_PERIOD_TICKS) { CoTickDelay(MENU_TASK_PERIOD_TICKS - runtime); } } #if defined(REV9E) topLcdOff(); #endif BACKLIGHT_OFF(); #if defined(PCBTARANIS) displaySleepBitmap(); #else lcd_clear(); displayPopup(STR_SHUTDOWN); #endif opentxClose(); boardOff(); // Only turn power off if necessary } extern void audioTask(void* pdata); void tasksStart() { CoInitOS(); #if defined(CLI) cliStart(); #endif #if defined(BLUETOOTH) btTaskId = CoCreateTask(btTask, NULL, 15, &btStack[BT_STACK_SIZE-1], BT_STACK_SIZE); #endif mixerTaskId = CoCreateTask(mixerTask, NULL, 5, &mixerStack[MIXER_STACK_SIZE-1], MIXER_STACK_SIZE); menusTaskId = CoCreateTask(menusTask, NULL, 10, &menusStack[MENUS_STACK_SIZE-1], MENUS_STACK_SIZE); audioTaskId = CoCreateTask(audioTask, NULL, 7, &audioStack[AUDIO_STACK_SIZE-1], AUDIO_STACK_SIZE); #if !defined(SIMU) audioMutex = CoCreateMutex(); mixerMutex = CoCreateMutex(); #endif CoStartOS(); }
/** * In this function, the hardware should be initialized. * Called from ethernetif_init(). * * @param netif the already initialized lwip network interface structure * for this ethernetif */ static void low_level_init(struct netif *netif) { StatusType result; int data; /* set MAC hardware address length */ netif->hwaddr_len = ETHARP_HWADDR_LEN; /* set MAC hardware address */ netif->hwaddr[0] = MAC_ADDR0; netif->hwaddr[1] = MAC_ADDR1; netif->hwaddr[2] = MAC_ADDR2; netif->hwaddr[3] = MAC_ADDR3; netif->hwaddr[4] = MAC_ADDR4; netif->hwaddr[5] = MAC_ADDR5; /* maximum transfer unit */ netif->mtu = ENET_MAX_PKT_SIZE - 20; /* device capabilities */ /* don't set NETIF_FLAG_ETHARP if this device is not an ethernet one */ netif->flags = NETIF_FLAG_BROADCAST | NETIF_FLAG_ETHARP | NETIF_FLAG_LINK_UP; /* enable the ENET clock */ SIM_SCGC2 |= SIM_SCGC2_ENET_MASK; /* allow concurrent access to MPU controller. Example: ENET uDMA to SRAM, otherwise bus error */ MPU_CESR = 0; init_enet_bufs(); /* create semaphores */ sem_rx = CoCreateSem(NUM_ENET_TX_BUFS, NUM_ENET_TX_BUFS, 1); sem_tx = CoCreateSem(0, NUM_ENET_RX_BUFS, 1); /* Set the Reset bit and clear the Enable bit */ ENET_ECR = ENET_ECR_RESET_MASK; /* Wait at least 8 clock cycles */ for (data = 0; data < 10; data++) { asm("NOP"); } /* start MII interface */ mii_init(50/*BUS_CLOCK*/); // enet tx interrupt NVIC_SetPriority(ENET_Transmit_IRQn, 6); NVIC_EnableIRQ(ENET_Transmit_IRQn); // enet rx interrupt NVIC_SetPriority(ENET_Receive_IRQn, 6); NVIC_EnableIRQ(ENET_Receive_IRQn); //enet error interrupt NVIC_SetPriority(ENET_Error_IRQn, 6); NVIC_EnableIRQ(ENET_Error_IRQn); /* Make sure the external interface signals are enabled */ PORTB_PCR0 = PORT_PCR_MUX(4); //RMII0_MDIO/MII0_MDIO PORTB_PCR1 = PORT_PCR_MUX(4); //RMII0_MDC/MII0_MDC #if USE_MII_MODE PORTA_PCR14 = PORT_PCR_MUX(4); //RMII0_CRS_DV/MII0_RXDV //PORTA_PCR5 = PORT_PCR_MUX(4); //RMII0_RXER/MII0_RXER PORTA_PCR12 = PORT_PCR_MUX(4); //RMII0_RXD1/MII0_RXD1 PORTA_PCR13 = PORT_PCR_MUX(4); //RMII0_RXD0/MII0_RXD0 PORTA_PCR15 = PORT_PCR_MUX(4); //RMII0_TXEN/MII0_TXEN PORTA_PCR16 = PORT_PCR_MUX(4); //RMII0_TXD0/MII0_TXD0 PORTA_PCR17 = PORT_PCR_MUX(4); //RMII0_TXD1/MII0_TXD1 PORTA_PCR11 = PORT_PCR_MUX(4); //MII0_RXCLK PORTA_PCR25 = PORT_PCR_MUX(4); //MII0_TXCLK PORTA_PCR9 = PORT_PCR_MUX(4); //MII0_RXD3 PORTA_PCR10 = PORT_PCR_MUX(4); //MII0_RXD2 PORTA_PCR28 = PORT_PCR_MUX(4); //MII0_TXER PORTA_PCR24 = PORT_PCR_MUX(4); //MII0_TXD2 PORTA_PCR26 = PORT_PCR_MUX(4); //MII0_TXD3 PORTA_PCR27 = PORT_PCR_MUX(4); //MII0_CRS PORTA_PCR29 = PORT_PCR_MUX(4); //MII0_COL #else PORTA_PCR14 = PORT_PCR_MUX(4); //RMII0_CRS_DV/MII0_RXDV // PORTA_PCR5 = PORT_PCR_MUX(4); //RMII0_RXER/MII0_RXER PORTA_PCR12 = PORT_PCR_MUX(4); //RMII0_RXD1/MII0_RXD1 PORTA_PCR13 = PORT_PCR_MUX(4); //RMII0_RXD0/MII0_RXD0 PORTA_PCR15 = PORT_PCR_MUX(4); //RMII0_TXEN/MII0_TXEN PORTA_PCR16 = PORT_PCR_MUX(4); //RMII0_TXD0/MII0_TXD0 PORTA_PCR17 = PORT_PCR_MUX(4); //RMII0_TXD1/MII0_TXD1 #endif /* USE_MII_MODE */ /* Can we talk to the PHY? */ do { CoTickDelay(PHY_LINK_DELAY); data = 0xffff; mii_read(PHY_ADDR, MII_PHYID1, (uint16_t*)&data); } while (data == 0xffff); /* Start auto negotiate. */ mii_write(PHY_ADDR, MII_BMCR, (MII_BMCR_ANRESTART | MII_BMCR_ANENABLE)); /* Wait for auto negotiate to complete. */ do { CoTickDelay(PHY_LINK_DELAY); mii_read(PHY_ADDR, MII_BMSR, (uint16_t*)&data); } while (!(data & MII_BMSR_ANEGCOMPLETE)); /* When we get here we have a link - find out what has been negotiated. */ data = 0; mii_read(PHY_ADDR, PHY_STATUS, (uint16_t*)&data); /* Set the Physical Address for the selected ENET */ ENET_PALR = (uint32_t)((netif->hwaddr[0] << 24) | (netif->hwaddr[1] << 16) | (netif->hwaddr[2] << 8) | netif->hwaddr[3]); ENET_PAUR = (uint32_t)((netif->hwaddr[4] << 24) | (netif->hwaddr[5] << 16)); /* Clear the Individual and Group Address Hash registers */ ENET_IALR = 0; ENET_IAUR = 0; ENET_GALR = 0; ENET_GAUR = 0; #if USE_MII_MODE /* various mode/status setup */ ENET_RCR = ENET_RCR_MAX_FL(ENET_MAX_PKT_SIZE) | ENET_RCR_MII_MODE_MASK | ENET_RCR_CRCFWD_MASK; #else ENET_RCR = ENET_RCR_MAX_FL(ENET_MAX_PKT_SIZE) | ENET_RCR_MII_MODE_MASK | ENET_RCR_CRCFWD_MASK | ENET_RCR_RMII_MODE_MASK; #endif /* clear rx/tx control registers */ ENET_TCR = 0; /* setup half or full duplex */ if (data & PHY_DUPLEX_STATUS) { /* full duplex */ ENET_RCR &= ~ENET_RCR_DRT_MASK; ENET_TCR |= ENET_TCR_FDEN_MASK; } else { /* half duplex */ ENET_RCR |= ENET_RCR_DRT_MASK; ENET_TCR &= ~ENET_TCR_FDEN_MASK; } /* Setup speed */ if (data & PHY_SPEED_STATUS) { /* 10Mbps */ ENET_RCR |= ENET_RCR_RMII_10T_MASK; } #if USE_PROMISCUOUS_MODE ENET_RCR |= ENET_RCR_PROM_MASK; #endif #ifdef ENHANCED_BD ENET_ECR = ENET_ECR_EN1588_MASK; #else ENET_ECR = 0; #endif #if ENET_HARDWARE_CHECKSUM /* enable discard on wrong protocol checksum and other nice features */ ENET_RACC = ENET_RACC_IPDIS_MASK | ENET_RACC_PRODIS_MASK | ENET_RACC_LINEDIS_MASK | ENET_RACC_IPDIS_MASK | ENET_RACC_PADREM_MASK; /* enable protocol checksum insertion */ ENET_TACC = ENET_TACC_IPCHK_MASK | ENET_TACC_PROCHK_MASK; #endif ENET_TFWR = ENET_TFWR_STRFWD_MASK; #if ENET_HARDWARE_SHIFT /* enable ethernet header alignment for rx */ ENET_RACC |= ENET_RACC_SHIFT16_MASK; /* enable ethernet header alignment for tx */ ENET_TACC |= ENET_TACC_SHIFT16_MASK; #endif /* set rx buffer size */ ENET_MRBR = ENET_RX_BUF_SIZE; /* point to the start of the circular rx buffer descriptor queue */ ENET_RDSR = (uint32_t)rx_bd; /* point to the start of the circular tx buffer descriptor queue */ ENET_TDSR = (uint32_t)tx_bd; /* clear all ENET interrupt events */ ENET_EIR = -1u; /* enable interrupts */ ENET_EIMR = 0 /* rx irqs */ | ENET_EIMR_RXF_MASK /* only for complete frame, not partial buffer descriptor */ /* tx irqs */ | ENET_EIMR_TXF_MASK /* only for complete frame, not partial buffer descriptor */ /* others enet irqs */ | ENET_EIMR_UN_MASK | ENET_EIMR_RL_MASK | ENET_EIMR_LC_MASK | ENET_EIMR_BABT_MASK | ENET_EIMR_BABR_MASK | ENET_EIMR_EBERR_MASK; /* create the task that handles the MAC ENET */ result = CoCreateTask((FUNCPtr)ethernetif_input, (void *)netif, TASK_ETHIF_PRIO, &stack[TASK_ETHIF_STACK_SIZE - 1], TASK_ETHIF_STACK_SIZE); if (result == E_CREATE_FAIL) { LWIP_DEBUGF(NETIF_DEBUG,("Task for ETH recive created failed !\n\r ")); }; /* Enable the MAC itself */ ENET_ECR |= ENET_ECR_ETHEREN_MASK; /* Indicate that there have been empty receive buffers produced */ ENET_RDAR = ENET_RDAR_RDAR_MASK; }
int main() { UART_CFG_Type uart_config; UART_FIFO_CFG_Type uart_fifo_config; TIM_TIMERCFG_Type timer_config; TIM_MATCHCFG_Type timer_match; OS_TID uart_task_id = 0; OS_TID activity_task_id = 0; uint32_t reset_flags = 0; reset_flags = LPC_SYSCON->SYSRSTSTAT; SEQ_Initialize(); PROTO_Reset(); PROTO_SetHandlers(g_protocol_handlers); SYSCON_AHBPeriphClockCmd(SYSCON_AHBPeriph_GPIO, ENABLE); // Reset pin IOCON_SetPinFunc(IOCON_PIO0_0, PIO0_0_FUN_RESET); // Status LED pin ACTIVITY_SET_PIN(); GPIO_SetDir(ACTIVITY_PORT, ACTIVITY_PIN, 1); GPIO_ResetBits(ACTIVITY_PORT, ACTIVITY_PIN); // Timer activity LED pin TIMER_ACTIVITY_SET_PIN(); GPIO_SetDir(TIMER_ACTIVITY_PORT, TIMER_ACTIVITY_PIN, 1); GPIO_ResetBits(TIMER_ACTIVITY_PORT, TIMER_ACTIVITY_PIN); // RGB control RED_SET_PIN(); GREEN_SET_PIN(); BLUE_SET_PIN(); GPIO_SetDir(RED_PORT, RED_PIN, 1); GPIO_SetDir(GREEN_PORT, GREEN_PIN, 1); GPIO_SetDir(BLUE_PORT, BLUE_PIN, 1); GPIO_ResetBits(RED_PORT, RED_PIN); GPIO_ResetBits(GREEN_PORT, GREEN_PIN); GPIO_ResetBits(BLUE_PORT, BLUE_PIN); timer_config.PrescaleOption = TIM_PRESCALE_TICKVAL; timer_config.PrescaleValue = 1; TIM_Init(LPC_TMR32B0, TIM_TIMER_MODE, &timer_config); timer_match.MatchChannel = 0; timer_match.IntOnMatch = ENABLE; timer_match.StopOnMatch = DISABLE; timer_match.ResetOnMatch = ENABLE; timer_match.ExtMatchOutputType = 0; timer_match.MatchValue = SystemCoreClock / (TICKS_PER_SECOND * 256); TIM_ConfigMatch(LPC_TMR32B0, &timer_match); TIM_Cmd(LPC_TMR32B0, ENABLE); NVIC_EnableIRQ(TIMER_32_0_IRQn); // UART IOCON_SetPinFunc(IOCON_PIO1_6, PIO1_6_FUN_RXD); /* UART RXD - PIO1_6 */ IOCON_SetPinFunc(IOCON_PIO1_7, PIO1_7_FUN_TXD); /* UART RXD - PIO1_7 */ uart_config.Baud_rate = 115200; uart_config.Databits = UART_DATABIT_8; uart_config.Parity = UART_PARITY_NONE; uart_config.Stopbits = UART_STOPBIT_1; UART_Init(LPC_UART, &uart_config); uart_fifo_config.FIFO_Level = UART_FIFO_TRGLEV0; uart_fifo_config.FIFO_ResetRxBuf = ENABLE; uart_fifo_config.FIFO_ResetTxBuf = ENABLE; UART_FIFOConfig(LPC_UART, &uart_fifo_config); UART_TxCmd(LPC_UART, ENABLE); // SPI CL632_Init(); // Select page 0 and no paging access CL632_SpiWriteByte(0x00, 0x00); CL632_SpiWriteByte(0x00, 0x00); // LCD // LCD backlite control LCD_BACKLITE_SET_PIN(); GPIO_SetDir(LCD_BACKLITE_PORT, LCD_BACKLITE_PIN, 1); GPIO_ResetBits(LCD_BACKLITE_PORT, LCD_BACKLITE_PIN); // LCD Data bus LCD_DATA_SET_PINS(); GPIO_SetDir(LCD_DATA_PORT, LCD_DATA_BUS, 1); GPIO_ResetBits(LCD_DATA_PORT, LCD_DATA_BUS); LCD_RS_SET_PIN(); GPIO_SetDir(LCD_RS_PORT, LCD_RS_PIN, 1); GPIO_ResetBits(LCD_RS_PORT, LCD_RS_PIN); LCD_RW_SET_PIN(); GPIO_SetDir(LCD_RW_PORT, LCD_RW_PIN, 1); GPIO_ResetBits(LCD_RW_PORT, LCD_RW_PIN); LCD_E_SET_PIN(); GPIO_SetDir(LCD_E_PORT, LCD_E_PIN, 1); GPIO_ResetBits(LCD_E_PORT, LCD_E_PIN); KS0066_PowerUpDelay(); KS0066_FunctionSet(); KS0066_WaitForIdle(); KS0066_DisplayOnOffControl(KS0066_DISPCTL_DISPLAY_ON); KS0066_WaitForIdle(); KS0066_ClearDisplay(); KS0066_WaitForIdle(); CoInitOS(); GPIO_SetBits(ACTIVITY_PORT, ACTIVITY_PIN); uart_task_id = CoCreateTask(uartTask, NULL, UART_TASK_PRIORITY, GetStackTop(uartTaskStack), GetStackSize(uartTaskStack)); activity_task_id = CoCreateTask(activityTask, NULL, ACTIVITY_TASK_PRIORITY, GetStackTop(activityTaskStack), GetStackSize(activityTaskStack)); if (uart_task_id == E_CREATE_FAIL || activity_task_id == E_CREATE_FAIL) { UART_PrintString("INIT ERROR"); UART_PrintString(kNewLine); } if (reset_flags & 0x01) UART_PrintString("RST:PU"); else if (reset_flags & 0x02) UART_PrintString("RST:RST"); else if (reset_flags & 0x04) UART_PrintString("RST:WDT"); else if (reset_flags & 0x08) UART_PrintString("RST:BOD"); else if (reset_flags & 0x10) UART_PrintString("RST:SOFT"); else UART_PrintString("RST"); UART_PrintString(kNewLine); PrintVersionString(UART_WriteChar); UART_PrintString(kNewLine); func_printf_nofloat(UART_WriteChar, "COOS:%d\r\n", CoGetOSVersion()); KS0066_WriteString("V:" __DATE__ " " __TIME__, KS0066_WRAP_FLAG); CoStartOS(); //while (1) { //} return 0; }
void dIMUInit(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; NVIC_InitTypeDef NVIC_InitStructure; #ifdef DIMU_HAVE_MPU6000 mpu6000PreInit(); #endif #ifdef DIMU_HAVE_MAX21100 max21100PreInit(); #endif #ifdef DIMU_HAVE_EEPROM eepromPreInit(); #endif #ifdef DIMU_HAVE_HMC5983 hmc5983PreInit(); #endif #ifdef DIMU_HAVE_MS5611 ms5611PreInit(); #endif #ifdef DIMU_HAVE_MPU6000 mpu6000Init(); #endif #ifdef DIMU_HAVE_MAX21100 max21100Init(); #endif #ifdef DIMU_HAVE_EEPROM eepromInit(); // dIMUWriteCalib(); dIMUReadCalib(); #endif #ifdef DIMU_HAVE_HMC5983 if (hmc5983Init() == 0) AQ_NOTICE("DIMU: MAG sensor init failed!\n"); #endif #ifdef DIMU_HAVE_MS5611 if (ms5611Init() == 0) AQ_NOTICE("DIMU: PRES sensor init failed!\n"); #endif dIMUTaskStack = aqStackInit(DIMU_STACK_SIZE, "DIMU"); dImuData.flag = CoCreateFlag(1, 0); dImuData.task = CoCreateTask(dIMUTaskCode, (void *)0, DIMU_PRIORITY, &dIMUTaskStack[DIMU_STACK_SIZE-1], DIMU_STACK_SIZE); // setup digital IMU timer DIMU_EN; // Time base configuration for 1MHz (us) TIM_TimeBaseStructure.TIM_Period = 0xffff; TIM_TimeBaseStructure.TIM_Prescaler = (DIMU_CLOCK / 1000000) - 1; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(DIMU_TIM, &TIM_TimeBaseStructure); // reset TIM_SetCounter(DIMU_TIM, 0); // Output Compare for alarms TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Inactive; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OC1Init(DIMU_TIM, &TIM_OCInitStructure); TIM_OC1PreloadConfig(DIMU_TIM, TIM_OCPreload_Disable); TIM_OC2Init(DIMU_TIM, &TIM_OCInitStructure); TIM_OC2PreloadConfig(DIMU_TIM, TIM_OCPreload_Disable); // Enable the global Interrupt NVIC_InitStructure.NVIC_IRQChannel = DIMU_IRQ_CH; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); // reset TIM_SetCounter(DIMU_TIM, 0); dIMUCancelAlarm1(); // go... TIM_Cmd(DIMU_TIM, ENABLE); #ifdef DIMU_HAVE_MPU6000 mpu6000Enable(); #endif #ifdef DIMU_HAVE_MAX21100 max21100Enable(); #endif #ifdef DIMU_HAVE_HMC5983 hmc5983Enable(); #endif #ifdef DIMU_HAVE_MS5611 ms5611Enable(); #endif // setup IMU timestep alarm dImuData.nextPeriod = DIMU_TIM->CCR2 + DIMU_INNER_PERIOD; DIMU_TIM->CCR2 = dImuData.nextPeriod; DIMU_TIM->DIER |= TIM_IT_CC2; #ifdef DIMU_HAVE_MPU6000 mpu6600InitialBias(); #endif #ifdef DIMU_HAVE_MAX21100 max21100InitialBias(); #endif #ifdef DIMU_HAVE_MS5611 ms5611InitialBias(); #endif }
static void task1_execute(void) { int i; StatusType err; char expectSequence[MAX_SLAVE_TEST_TASKS+1]; for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++) { expectSequence[i] = 'A' + i; } expectSequence[i] = '\0'; dbgPassCnt = 0; // Test 1 CreateTask && argv && ExitTask && Preemptive Priority for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++) { Task_Id[i] = CoCreateTask(exitTaskTestTask,(void*)expectSequence[i],MAINTEST_PRIMARY_PRIORITY-1,&Task_Stack[i][SLAVE_TASK_STK_SIZE-1],SLAVE_TASK_STK_SIZE); testAssert(Task_Id[i] != (OS_TID)-1,"#1"); } testAssert(dbgPassCnt == MAX_SLAVE_TEST_TASKS,"#2"); testAssertSequence(expectSequence); // Test 2 SetPriority for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++) { Task_Id[i] = 0; } dbgPassCnt = 0; for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++) { Task_Id[i] = CoCreateTask(prioTestTask,NULL,MAINTEST_PRIMARY_PRIORITY+(MAX_SLAVE_TEST_TASKS-i),&Task_Stack[i][SLAVE_TASK_STK_SIZE-1],SLAVE_TASK_STK_SIZE); testAssert(Task_Id[i] != (OS_TID)-1,"#3"); err = CoSetPriority(mainTestTaskID,MAINTEST_PRIMARY_PRIORITY + 2 + (MAX_SLAVE_TEST_TASKS-i)); testAssert(err == E_OK,"#4"); } testAssert(dbgPassCnt == MAX_SLAVE_TEST_TASKS,"#5"); for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++) { err = CoDelTask(Task_Id[i]); testAssert(err == E_OK,"#6"); } // Delete invaild task for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++) { err = CoDelTask(Task_Id[i]); testAssert(err == E_INVALID_ID,"#7"); } // Test 3 SuspendTask && AwakeTask dbgPassCnt = 0; // Awake a running task err = CoAwakeTask(mainTestTaskID); testAssert(err == E_TASK_NOT_WAITING,"#8"); for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++) { Task_Id[i] = CoCreateTask(awakeTestTask,NULL,MAINTEST_PRIMARY_PRIORITY+(MAX_SLAVE_TEST_TASKS-i),&Task_Stack[i][SLAVE_TASK_STK_SIZE-1],SLAVE_TASK_STK_SIZE); testAssert(Task_Id[i] != (OS_TID)-1,"#9"); // Suspend a ready task err = CoSuspendTask(Task_Id[i]); testAssert(err == E_OK,"#10"); // Awake a wait task err = CoAwakeTask(Task_Id[i]); testAssert(err == E_OK,"#11"); // Awake a ready task err = CoAwakeTask(Task_Id[i]); testAssert(err == E_TASK_NOT_WAITING,"#12"); // Suspend the running task err = CoSuspendTask(mainTestTaskID); if (err != E_OK) testAssert(err == E_OK,"#13"); } testAssert(dbgPassCnt == MAX_SLAVE_TEST_TASKS,"#14"); for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++) { err = CoDelTask(Task_Id[i]); testAssert(err == E_OK,"#15"); } }
/** ******************************************************************************* * @brief Initialization task * @param[in] pdata A pointer to parameter passed to task. * @param[out] None * @retval None * * @details This task is called to initial hardware and created tasks. ******************************************************************************* */ void task_init(void *pdata) { uart_printf (" [OK]. \n\r\n\r"); uart_printf ("\r \"task_init\" task enter. \n\r\n\r "); pdata = pdata; /* Initiate Time buffer for LCD display */ chart[0] = time[2]/10 + '0'; chart[1] = time[2]%10 + '0'; chart[3] = time[1]/10 + '0'; chart[4] = time[1]%10 + '0'; chart[6] = time[0]/10 + '0'; chart[7] = time[0]%10 + '0'; uart_printf ("\r Create the \"mut_uart\" mutex... "); mut_uart = CoCreateMutex(); if(mut_uart != E_CREATE_FAIL) uart_printf (" [OK]. \n"); else uart_printf (" [Fail]. \n"); uart_printf ("\r Create the \"mut_lcd\" mutex... "); mut_lcd = CoCreateMutex(); if(mut_lcd != E_CREATE_FAIL) uart_printf (" [OK]. \n"); else uart_printf (" [Fail]. \n"); uart_printf ("\r Create the \"button_sel_flg\" flag... "); /*!< Manual reset flag,initial state:0 */ button_sel_flg = CoCreateFlag(Co_FALSE,0); if(button_sel_flg != E_CREATE_FAIL) uart_printf (" [OK]. \n"); else uart_printf (" [Fail]. \n"); uart_printf ("\r Create the \"button_add_flag\" flag..."); /*!< Manual reset flag,initial state:0 */ button_add_flg = CoCreateFlag(Co_FALSE,0); if(button_add_flg != E_CREATE_FAIL) uart_printf (" [OK]. \n\n"); else uart_printf (" [Fail]. \n\n"); uart_printf ("\r Create the \"lcd_blink_flg\" flag... "); lcd_blink_flg = CoCreateFlag(Co_FALSE,0); /*!< Manual reset flag,initial state:0 */ if(lcd_blink_flg != E_CREATE_FAIL) uart_printf (" [OK]. \n"); else uart_printf (" [Fail]. \n"); uart_printf ("\r Create the \"time_display_flg\" flag..."); /*!< Manual reset flag,initial state:0 */ time_display_flg = CoCreateFlag(Co_FALSE,0); if(time_display_flg != E_CREATE_FAIL) uart_printf (" [OK]. \n"); else uart_printf (" [Fail]. \n"); /*!< Set flag to allow "time_display_flg" task run. */ CoSetFlag(time_display_flg); uart_printf ("\r Create the first mailbox... "); mbox0 = CoCreateMbox(EVENT_SORT_TYPE_FIFO); if(mbox0 == E_CREATE_FAIL) uart_printf (" [Fail]. \n\n"); else uart_printf (" [OK]. \n\n"); /* Configure Peripheral */ uart_printf ("\r Initial hardware in Board : \n\r"); uart_printf ("\r ADC initial... "); ADC_Configuration (); uart_printf (" [OK]. \n"); uart_printf ("\r RTC initial... "); RTC_Configuration (); uart_printf (" [OK]. \n"); uart_printf ("\r GPIO initial... "); GPIO_Configuration (); uart_printf (" [OK]. \n"); uart_printf ("\r External interrupt initial... "); EXIT_Configuration (); uart_printf (" [OK]. \n"); uart_printf ("\r LCD initial... "); LCD_Configuration (); uart_printf (" [OK]. \n\n"); /* Create Tasks */ CoCreateTask( lcd_display_adc, (void *)0 , LCD_DISPLAY_PRI , &lcd_display_adc_Stk[TASK_STK_SIZE-1] , TASK_STK_SIZE ); CoCreateTask( uart_print , (void *)0 , UART_PRINT_PRI , &uart_print_Stk[TASK_STK_SIZE-1], TASK_STK_SIZE ); CoCreateTask( led_blink , (void *)0 , LCD_BLINK_PRI , &led_display_Stk[TASK_STK_SIZE-1], TASK_STK_SIZE ); time_display_id = CoCreateTask( time_display, (void *)0, TIME_DISRPLAY_PRI, &time_display_Stk[TASK_STK_SIZE - 1], TASK_STK_SIZE ); CoCreateTask( time_set , (void *)0 , TIME_SET_PRI , &time_set_Stk[TASK_STK_SIZE-1], TASK_STK_SIZE ); CoExitTask(); /*!< Delete 'task_init' task. */ }