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. */
}
