自学教程：C++ taskDelay函数代码示例

tpmode(int port){    char ttyName[16];    char chrStr[80],replyStr[80];    int len,i;    sprintf(ttyName,FpgaFormatStr,port);    printf("opening dev: '%s'/n",ttyName);    sfd = open(ttyName, O_RDWR, 0);    if(sfd < 1)     {       perror("open:");       printf("failed to open: '%s'/n",ttyName);       return(-1);    }    sprintf(ttyName,FpgaFormatStr,1);    sprintf(ttyName,FpgaFormatStr,2);    sprintf(ttyName,FpgaFormatStr,3);    while(1)    {    printf("string to issue: ");    fioRdString (STD_IN, &chrStr, 80);    printf("sending: '%s'/n",chrStr);    len = strlen(chrStr);    for(i=0;i<len;i++)          printf("char: '%c', 0x%x/n",chrStr[i],chrStr[i]);    printf("strlen: %d/n",len);    chrStr[len] = 13;     chrStr[len+1] = 10;    chrStr[len+2] = 0;    write(sfd,chrStr,len+2);    fioRdString(sfd,replyStr,80);    len = strlen(replyStr);    taskDelay(calcSysClkTicks(500));  /* 1/2 sec, taskDelay(30); */    printf("reply: %d bytes  '%s' /n",len,replyStr);    }}

STATIC s32 use_smem_test(int size){    u32* pret = (u32*)bsp_smalloc(size, MEM_ICC_DDR_POOL);    u32* praw = pret;    u32  rawsize = size;    if(pret)    {        mem_print_dbg("sucess BSP_Malloc addr:%x   size:%d", pret, size);        while(size > 0)        {            size -= sizeof(u32);            *pret = ACORE_MAGIC_NUM;            pret++;        }        taskDelay(200);        size = rawsize;        pret = praw;        while(size > 0)        {            size -= sizeof(u32);            if(*pret != ACORE_MAGIC_NUM)            {                mem_print_error("fail! Use smalloc fail addr:%x     size:%d", pret, size);                return MEM_TEST_ERROR;            }            pret++;        }        bsp_sfree(praw);        return MEM_TEST_OK;    }    else    {        mem_print_error("fail! BSP_Malloc addr:%x   size:%d", pret, size);        return MEM_TEST_ERROR;    }}

/*TASK 0*/void inputWatcher(){ 	char message[MAX_MESSAGE_LENGTH];	while (1)	{		newSensors = getSensorValues(); /*read the sensors*/		if(newSensors != oldSensors){			/*create message and send it through the queue*/			sprintf(message, "CHANGE %d", newSensors);			if((msgQSend(gateInQueue, message, MAX_MESSAGE_LENGTH, 					WAIT_FOREVER, MSG_PRI_NORMAL)) == ERROR)				printf("msgQSend to gate in failed/n");			/*both tasks wont work together - even with this message queue*/			if((msgQSend(gateOutQueue, message, MAX_MESSAGE_LENGTH, 					WAIT_FOREVER, MSG_PRI_NORMAL)) == ERROR)				printf("msgQSend to gate out failed/n");		}		/*if(newSensors != oldSensors){gateIn();gateOut();}		 */		taskDelay(6); /*Delay 6/60 -> 1/10 of a second*/		oldSensors = newSensors; /*update the sensors*/	}}

/**************************************************************************** rapiModeSet - Set new RAPI mode**	int rapiId     RAPI id*	int mode			New RAPI mode** RETURNS:* RAPI_OK on succes, or RAPI_ERR.*/int rapiModeSet(int rapiId, int mode){   int         i;   RAPI_HEADER *pRapiHeader;   if(rapiId < 0 || rapiId >= cRapi)                              /* Valid control module index ? */      return(RAPI_ERR);   if(mode < 0 || mode > 7)                                       /* Mode within valid range ? */      return(RAPI_ERR);   pRapiHeader = rapiModule[rapiId].pRapiHeader;                  /* Get pointer to RAPI_HEADER */   pRapiHeader->ctrlReg = (pRapiHeader->ctrlReg & ~0x07) | mode;  /* Read current status reg, mask mode, insert new mode and write ctrl reg */#if 0   for(i=0; i < mSecToTick(1000); i++)                            /* Wait a while for new mode to show up */#else   for(i=0; i < 10000000; i++)                                        /* Wait a while for new mode to show up */#endif   {      if((pRapiHeader->statReg & 0x07) == mode)                   /* New mode show up in status reg ? */      {#ifdef DEBUG         printf("Waited %d laps for mode to be set/n", i);        /* Try to get a grip on the time frame involved here ;-) */#endif         return(RAPI_OK);                                         /* Allright, it's set ! */      }      taskDelay(1);      /* (is it one millisecond ??) */         /* Sleep a short while bewteen status checking (nanosleep ??) */   }   return(RAPI_ERR);                                              /* We didn't succeed in setting new mode */}

pollTest(){    int i;    char Ichar;    SIO_CHAN *pSioChan, *pSioChan2; /* serial I/O channel */    char *text = "0123456789abcdefghijklmnopqurstuwxyz";    pSioChan = masterFpgaSerialChanGet(0);    /* ns16550InitChannel(pSioChan);  */    for (i = 0; i < 4; i++)    {	printf("output: %c, 0x%x/n",text[i],text[i]);        while (sioPollOutput (pSioChan, text[i]) == EAGAIN);        taskDelay(calcSysClkTicks(17));  /* taskDelay(1); */        if ( sioPollInput (pSioChan, &Ichar) != EAGAIN)           printf("got: '%c',  0x%x/n",Ichar,Ichar);    }/*    for (i = 0; i < 4; i++)    {        while (sioPollInput (pSioChan, &Ichar) == EAGAIN);	printf("got: %c/n",Ichar);    }*/}

/* * Runs the user operator control code. This function will be started in its own task with the * default priority and stack size whenever the robot is enabled via the Field Management System * or the VEX Competition Switch in the operator control mode. If the robot is disabled or * communications is lost, the operator control task will be stopped by the kernel. Re-enabling * the robot will restart the task, not resume it from where it left off. * * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will * run the operator control task. Be warned that this will also occur if the VEX Cortex is * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached. * * Code running in this task can take almost any action, as the VEX Joystick is available and * the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly * recommended to give other tasks (including system tasks such as updating LCDs) time to run. * * This task should never exit; it should end with some kind of infinite loop, even if empty. */void operatorControl() {	// start IR detection task	taskCreate(vTaskFilter, 2048, NULL, TASK_PRIORITY_DEFAULT);	// start PI controller	taskCreate(vTaskPIController, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);	//start polling the UltraSonic sensor	taskCreate(vTaskUltraSonic, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);	// show the detected IR levels	/*short leftIR_l, centerIR_l, rightIR_l;	while(1) {		if(mutexTake(irDetectorMutex, 10)) {			leftIR_l = leftIR;			centerIR_l = centerIR;			rightIR_l = rightIR;			mutexGive(irDetectorMutex);		}		printf("Left: %d Center: %d Right: %d /n", leftIR_l, centerIR_l, rightIR_l);		taskDelay(100);	}*/	// advance 1000, reverse 1000, every 2 seconds	int multiplier = 1;	while(1) {		// take PI Mutex		mutexTake(piSetpointMutex, -1);		leftMotor.position += 1000*multiplier;		rightMotor.position += 1000*multiplier;		mutexGive(piSetpointMutex);		multiplier = -1*multiplier;		taskDelay(2000);	}}

/****************************************************************************  cs4281_wrote_ac97() : write a word to the cs4281 address space      step-1: write ACCAD (Command Address Register) 46C h      step-2: write ACCDA (Command Data    Register) 470 h      step-3: write ACCTL (Control         Register) 460 h      step-4: read  ACCTL,  DCV should be reset and [460h] = 07h      step-5: if DCV not cleared, error*****************************************************************************/int cs4281_write_ac97(UINT32 offset, UINT32 value ){    UINT32 count, status;    /* write to the actual AC97 register */    writel(offset - BA0_AC97_RESET, CS4281_pBA0 + BA0_ACCAD);    writel(value, CS4281_pBA0 + BA0_ACCDA);    writel(ACCTL_DCV | ACCTL_VFRM |ACCTL_ESYN, CS4281_pBA0 + BA0_ACCTL);    /* Wait for write to finish ... */    for(count=0; count<10; count ++) {	taskDelay(25);	/*udelay(25);*/	/* check if write complete */	status = readl(CS4281_pBA0 + BA0_ACCTL);	if(!(status & ACCTL_DCV))	    break;    }    if(status & ACCTL_DCV)	return 1;    return 0;}

/*****************************************************************功能:控制CFG脚，发起初始化操作输入:无输出:无返回:无 *****************************************************************/LOCAL VOID StartDownLoadFpga_ALTERA(){    SetCfgPin_ALTERA(0);    taskDelay(1);    SetCfgPin_ALTERA(1);}

/** * Runs autonomous code. /n * Contains code to run our autonomous plans. /n */void autonomous() {	if (badPlan1) {		//Lift cube		motorSet(6, 127);		motorSet(7, 127);		motorSet(8, 127);		taskDelay(2000);		motorSet(6, 0);		motorSet(7, 0);		motorSet(8, 0);		//Turns robot		//Positive number = right; Negative number = left.		int Y1 = 127;		int X1 = 0;		int X2 = 0;		motorSet(2, Y1 + X2 + X1);		motorSet(3, Y1 - X2 - X1);		motorSet(4, Y1 + X2 - X1);		motorSet(5, Y1 - X2 + X1);		taskDelay(800);		//Go forward		X2 = -127;		Y1 = 0;		X1 = 0;		motorSet(2, Y1 + X2 + X1);		motorSet(3, Y1 - X2 - X1);		motorSet(4, Y1 + X2 - X1);		motorSet(5, Y1 - X2 + X1);		taskDelay(2000);		//Stop		motorSet(2, 0);		motorSet(5, 0);		motorSet(3, 0);		motorSet(4, 0);		//Drop Cube		motorSet(6, -127);		motorSet(7, -127);		motorSet(8, -127);		taskDelay(1500);		//Stop dropping		motorSet(6, 0);		motorSet(7, 0);		motorSet(8, 0);		//Go back		X2 = 127;		Y1 = 0;		X1 = 0;		motorSet(2, Y1 + X2 + X1);		motorSet(3, Y1 - X2 - X1);		motorSet(4, Y1 + X2 - X1);		motorSet(5, Y1 - X2 + X1);		taskDelay(1000);		//Stop		motorSet(2, 0);		motorSet(5, 0);		motorSet(3, 0);		motorSet(4, 0);	} else if (goodPlan1) {		//Raise Lift		motorSet(6, 127);		motorSet(7, 127);		motorSet(8, 127);		taskDelay(150);		//Stop lifting		motorSet(6, 0);		motorSet(7, 0);		motorSet(8, 0);		//Go forward		int X2 = -127;		int Y1 = 0;		int X1 = 0;		motorSet(2, Y1 + X2 + X1);		motorSet(3, Y1 - X2 - X1);		motorSet(4, Y1 + X2 - X1);		motorSet(5, Y1 - X2 + X1);		taskDelay(1100);		//Stop going forward		motorSet(2, 0);		motorSet(3, 0);		motorSet(4, 0);//.........这里部分代码省略.........

STATUS shellParserControl    (    UINT32 remoteEvent,	/* Starting or stopping a connection? */    UINT32 sessionId, 	/* Unique identifier for each session */    UINT32 slaveFd      /* File descriptor for character i/o  */    )    {    if ((taskNameToId ("tShell")) == ERROR)   /* Shell not started yet. */        return (ERROR);    if (remoteEvent == REMOTE_START)        {        /* Handle a new telnet or rlogin session. */        if (remoteId != 0)    /* Failed request - only one session allowed. */            return (ERROR);         if (!shellLock (TRUE)) 	/* Shell is not available. */            {	    fdprintf (slaveFd, "The shell is currently in use./n");            return (ERROR);            }        /* Let the user try to login */	if (shellLogin (slaveFd) != OK)	    {  	    shellLock (FALSE);            return (ERROR);            }        /* setup the slave device to act like a terminal */        (void) ioctl (slaveFd, FIOOPTIONS, OPT_TERMINAL);        shellLogoutInstall ((FUNCPTR) telnetdExit, sessionId);        /* get the shell's standard I/O fd's so we can restore them later */        shellInFd  = ioGlobalStdGet (STD_IN);        shellOutFd = ioGlobalStdGet (STD_OUT);        shellErrFd = ioGlobalStdGet (STD_ERR);        /* set shell's standard I/O to new device; add extra logging device */        shellOrigStdSet (STD_IN, slaveFd);        shellOrigStdSet (STD_OUT, slaveFd);        shellOrigStdSet (STD_ERR, slaveFd);        logFdAdd (slaveFd);        logFdFromRlogin = slaveFd;      /* store new fd for logFdSet() */        /* Store the session identifier. */        remoteId = sessionId;        /* notify the shell we have started a remote session */                shellIsRemoteConnectedSet (TRUE);        printErr ("/ntelnetd: This system *IN USE* via telnet./n");        /* Prevent network denial of service attacks by waiting a second */        taskDelay (sysClkRateGet() / 2);                /* Restart the shell to access the redirected file descriptors. */        excJobAdd (shellRestart, TRUE, 0, 0, 0, 0, 0);        return (OK);        }    else if (remoteEvent == REMOTE_STOP)        {        /*         * End an active telnet or rlogin session. This event occurs         * after the server closes the socket.         */        if (remoteId != sessionId)    /* Unknown remote session. */            return (ERROR);        shellLogoutInstall ((FUNCPTR) NULL, 0);  /* remove logout function */        if (logFdFromRlogin != NONE)            {            logFdDelete (logFdFromRlogin);       /* cancel extra log device */            logFdFromRlogin = NONE;              /* reset fd */            }        shellOrigStdSet (STD_IN,  shellInFd);    /* restore shell's stnd I/O */        shellOrigStdSet (STD_OUT, shellOutFd);        shellOrigStdSet (STD_ERR, shellErrFd);        shellLock (FALSE);                       /* unlock shell */        /*         * For typical remote sessions, restoring the standard I/O         * descriptors is enough to reconnect the shell to the console         * because closing the pty device will cause the shell to unblock//.........这里部分代码省略.........

int32 OS_mkfs (char *address, char *devname, char *volname, uint32 blocksize,                uint32 numblocks){    int         i;    int         status;    char        local_volname[OS_MAX_PATH_LEN];    uint32      ReturnCode;        BLK_DEV     *ramDev;    device_t    xbd;#ifdef USE_VXWORKS_ATA_DRIVER    BLK_DEV     *ataBlkDev;  #endif        if ( devname == NULL || volname == NULL )        return OS_FS_ERR_INVALID_POINTER;    /*     ** Find an open entry in the Volume Table     */    for (i = 0; i < NUM_TABLE_ENTRIES; i++)    {        if (OS_VolumeTable[i].FreeFlag == TRUE && OS_VolumeTable[i].IsMounted == FALSE            && strcmp(OS_VolumeTable[i].DeviceName, devname) == 0)            break;    }    if (i >= NUM_TABLE_ENTRIES)        return OS_FS_ERR_DEVICE_NOT_FREE;        /*     ** Now enter the info in the table     */    OS_VolumeTable[i].FreeFlag = FALSE;    strcpy(OS_VolumeTable[i].VolumeName, volname);    OS_VolumeTable[i].BlockSize = blocksize;        /*     ** Note we don't know the mount point/physical device name yet     */    strcpy(local_volname,volname);    if (OS_VolumeTable[i].VolumeType == RAM_DISK)    {        printf("OSAL: Making a RAM disk at: 0x%08X/n",(unsigned long)address );        /*        ** Create the ram disk device         ** The 32 is the number of blocks per track.         **  Other values dont seem to work here        */        ramDev = ramDevCreate (address, blocksize , 32 , numblocks,  0);        if (ramDev == NULL)        {            ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED;        }        /*        ** Connect the ram drive to the xbd block device         */        xbd = xbdBlkDevCreate(ramDev,local_volname);        if (xbd == NULLDEV)        {            ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED;        }        else        {           /*           ** Delay to allow the XBD operation to complete           */           (void) taskDelay(100);                     /*           ** Determine the vxWorks device name and store it            ** in the Physical Device field of the volume table.           ** It will be used for determining the path in OS_NameChange           */           strcat(local_volname,":0");           strncpy(OS_VolumeTable[i].PhysDevName, local_volname, 32 );                      /*           ** Call the dos format routine           */           status = dosFsVolFormat(OS_VolumeTable[i].PhysDevName, DOS_OPT_BLANK, NULL);           if ( status == -1 )           {              printf("OSAL: dosFsVolFormat failed. Errno = %d/n",errnoGet());              ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED;           }           else           {              ReturnCode = OS_FS_SUCCESS;           }        }    }    else if (OS_VolumeTable[i].VolumeType == FS_BASED)    {       /*       ** FS_BASED will map the cFE to an already mounted filesystem       */       //.........这里部分代码省略.........

int main(int argc, char *argv[]) {    int stat;    printf("/nJLAB TS Tests/n");    printf("----------------------------/n");    vmeOpenDefaultWindows();  /* Setup Address and data modes for DMA transfers   *      *  vmeDmaConfig(addrType, dataType, sstMode);   *   *  addrType = 0 (A16)    1 (A24)    2 (A32)   *  dataType = 0 (D16)    1 (D32)    2 (BLK32) 3 (MBLK) 4 (2eVME) 5 (2eSST)   *  sstMode  = 0 (SST160) 1 (SST267) 2 (SST320)   */    vmeDmaConfig(2,5,1);    /* INIT dmaPList */    dmaPFreeAll();    vmeIN  = dmaPCreate("vmeIN",1024,500,0);    vmeOUT = dmaPCreate("vmeOUT",0,0,0);        dmaPStatsAll();    dmaPReInitAll();    tsReload();/*     gefVmeSetDebugFlags(vmeHdl,0x0); */    /* Set the TS structure pointer */    tsPartInit(1,(21<<19),TS_READOUT_EXT_POLL,0);    if(tsCheckAddresses()==ERROR)      goto CLOSE;    tsPartLoadTriggerTable();    tsPartSetBlockBufferLevel(10);        tsSetBlockLevel(1);    stat = tsPartIntConnect(mytsISR, 0);    if (stat != OK)       {	printf("ERROR: tsIntConnect failed /n");	goto CLOSE;      }     else       {	printf("INFO: Attached TS Interrupt/n");      }    tsSetTriggerSource(6); // Pulser = 5, GTP/Ext/FP = 6    tsSetFPInput(0);    tsSetGenInput(0xffff);    tsSetGTPInput(0x0);    tsPartSetFPInput(1,2,3);    tsPartSetExtInput(1,2,3,4,5);    tsPartSetGTPInput(1,2,3,4,5);/*     tsSetBusySource(TS_BUSY_LOOPBACK,1); */    tsSetBusySource(0,1);/*     tsSetBlockBufferLevel(1); */    tsClockReset();    taskDelay(1);    tsTrigLinkReset();    taskDelay(1);    tsSyncReset();    taskDelay(1);        tsStatus();    printf("Hit enter to start triggers/n");    getchar();    tsPartIntEnable(0);    tsStatus();/* #define SOFTTRIG */#ifdef SOFTTRIG    tsSetRandomTrigger(1,0x7);    taskDelay(10);    tsSoftTrig(1,0x1,0x700,0);#endif    printf("Hit any key to Disable TID and exit./n");    getchar();    tsStatus();    tsPrintScalers(1);    tsPrintScalers(2);    tsPrintScalers(3);#ifdef SOFTTRIG//.........这里部分代码省略.........

void hang() {	liftTaskDelete();	// engage the winch	digitalWrite(winchPiston, 1);	// give it a chance to engage	taskDelay(100);	int leftTicks = analogRead(potLiftLeft);	int rightTicks = analogRead(potLiftRight);	int changesArrayLength = STALL_TIME/HANG_DT;	int changes[changesArrayLength]; // -1 indicates no data	fillArray(changes, changesArrayLength, -1);	int stallTicks = 0;	while (true) {		if (joystickGetDigital(1, 8, JOY_DOWN)) {			return;		}		int newLeftTicks = analogRead(potLiftLeft);		int newRightTicks = analogRead(potLiftRight);		if (newLeftTicks < HANG_HEIGHT && newRightTicks < HANG_HEIGHT) {			// we're at the right height, stop			break;		}		if ((stallTicks * HANG_DT) > STALL_DELAY) {			// do stall detection			int change = abs(leftTicks - newLeftTicks) + abs(rightTicks - newRightTicks);			pushArrayValue(changes, changesArrayLength, change);			int totalChanges = sumStallChanges(changes, changesArrayLength);			// if changes has data (>= 0) and it's stalled, stop motors			if (totalChanges >= 0 && totalChanges < STALL_THRESHOLD) {				// stalled, wait 3 seconds				hangMotors(0);				delay(3000);				// clear changes				fillArray(changes, changesArrayLength, -1);				continue;			}		}		leftTicks = newLeftTicks;		rightTicks = newRightTicks;		hangMotors(127);		stallTicks++;		taskDelay(HANG_DT);	}	hangMotors(0);}

VOID ResetFpga_ALTERA(){        BSP_AT91F_OR_PIO_ClearOutput(BSP_PIOC,FPGA_RESET);        taskDelay(2);        BSP_AT91F_OR_PIO_SetOutput(BSP_PIOC,FPGA_RESET);}

/*****************************************************************功能:从CF卡读取FPGA代码，然后加载到FPGA芯片中输入:无输出:无返回:0         成功            其他失败 *****************************************************************/SDWORD LoadFpga_ALTERA(CHAR filename[4][80],DWORD  fileno){    BYTE *data  = NULL;    DWORD bytes ;    BYTE *bootrom_p1 ;    FILE *bootrom_fd ;    BYTE  *pFpgaFile = NULL;    BYTE  dataTemp;    DWORD  i = 0;    DWORD  j = 0;    DWORD  count = 0 ;    DWORD  ulFileLength = 0;	DWORD  tempfileno = 0;	    InitFpgaPin_ALTERA();    StartDownLoadFpga_ALTERA();    /*检测FPGA_INIT信号，如果变为1    表示已经对FPGA里的memory清除完毕*/    while(0 == GpioValGet_ALTERA(FPGA_INIT_ALTERA))    {        taskDelay(2) ;        count++ ;        if( 10 == count)        {            printf("/r/n  FPGA INIT Signal error !!") ;             return (2);        }    }for(tempfileno=0;tempfileno<fileno;tempfileno++){	    pFpgaFile = (BYTE*)malloc(FPGA_FILE_MAX_LENGTH);    if(NULL == pFpgaFile)    {        printf( "/nERROR: NO memory!/n" ) ;        return (1);    }   bootrom_p1 = pFpgaFile ;       if((bootrom_fd = fopen( filename[tempfileno], "rb" )) == NULL )    {        printf( "/nERROR: can not open file %s/n",filename ) ;        free( pFpgaFile ) ;        return (1);    }    bytes = 0 ;    printf( "read FPGA file/n" ) ;    while( fscanf( bootrom_fd, "%c", &dataTemp ) != EOF )    {        *bootrom_p1++ = dataTemp ;        bytes = bytes + 1 ;        if( bytes >= FPGA_FILE_MAX_LENGTH )        break ;    }     fclose( bootrom_fd ) ;    if( bytes > FPGA_FILE_MAX_LENGTH )    {        printf( "ERROR: bootrom file is too long!/n" ) ;        return (1);    }		printf( "File %08x/n" ,bytes) ;	ulFileLength=bytes;    /*把文件存入到申请的内存块中*/    /*ulFileLength = ftpDownload("nsp2000.bin", pFpgaFile);   */    data = pFpgaFile;		/* 启始点*/    /*开始加载*/	    for(i = 0; i<ulFileLength; i++, data++)    {        for( j=0; j<8; j++)        {                /*置FPGA_CLK为0*/            GpioValSet_ALTERA(FPGA_CLK_ALTERA, 0);    				     /*一个字节的最高bit先打入FPGA，	     送到DIN脚上，在CLK上升沿打入*/            /*if(((*data) << j) & 0x80 )*/            if(((*data) >> j) & 0x01 )//.........这里部分代码省略.........

/*************************************************************************** cs4281_hw_init:   bring up the part**************************************************************************/int cs4281_hw_init( void ){    UINT32 ac97_slotid;    UINT32 temp1, temp2;    /****************************************************     *      set up the Sound System configuration     ****************************************************/    printf("/nCS4281 HardWare Initialization .../n");    /* ease the 'write protect' */    writel(0x4281, CS4281_pBA0 + BA0_CWPR);    /* Blast the clock control register to 0, so that PLL starts out       Blast the master serial port cntl register to 0, so that serial port       starts out    */    writel(0, CS4281_pBA0 + BA0_CLKCR1);    writel(0, CS4281_pBA0 + BA0_SERMC);    /***** <1> Make ESYN go to 0, to turn off the Sync pulse */    writel(0, CS4281_pBA0 + BA0_ACCTL);    taskDelay(50);    /*udelay(50);*/    /***** <2> Drive ARST# pin low for 1uS, then drive high, so that the	   external logic are reset    */    writel(0, CS4281_pBA0 + BA0_SPMC);    taskDelay(100);    /* udelay(100);*/    writel(SPMC_RSTN, CS4281_pBA0 + BA0_SPMC);    taskDelay(500);    /*delayus(50000);*/    /***** <3> Turn on the Sound System clocks */    writel(CLKCR1_PLLP, CS4281_pBA0 + BA0_CLKCR1);    taskDelay(500);    /*delayus(50000);*/    writel(CLKCR1_PLLP | CLKCR1_SWCE, CS4281_pBA0 + BA0_CLKCR1);    /***** <4> Power on everything for now */    writel(0x7e, CS4281_pBA0 + BA0_SSPM);    /***** <5> Wait for clock stabilization */    for(temp1=0; temp1<10000; temp1++) {	taskDelay(10);	/*udelay(1000);*/	if( readl(CS4281_pBA0 + BA0_CLKCR1) & CLKCR1_DLLRDY )	    break;    }    if(!(readl(CS4281_pBA0 + BA0_CLKCR1) & CLKCR1_DLLRDY)) {	printf("cs4281: DLLRDY failed! /n");	return -1;    }    /***** <6> Enable ASYNC generation */    writel(ACCTL_ESYN, CS4281_pBA0 +BA0_ACCTL);    /* wait for a while to start generating bit clock */    taskDelay(500);    /*dealyus(50000);*/    /* Set the serial port timing configuration */    writel( SERMC_PTC_AC97, CS4281_pBA0 + BA0_SERMC );    /***** <7> Wait for the codec ready signal from the AC97 codec */    for(temp1=0; temp1<1000; temp1++) {	taskDelay(1);	/*udelay(1000);*/	if(readl(CS4281_pBA0 + BA0_ACSTS) & ACSTS_CRDY )	    break;    }    if(!(readl(CS4281_pBA0 + BA0_ACSTS)& ACSTS_CRDY)) {	printf("cs4281: ACTST never came ready!/n");	return -1;    }    /***** <8> Assert the 'valid frame' signal to begin sending	   commands to AC97 codec */    writel(ACCTL_VFRM |ACCTL_ESYN, CS4281_pBA0 + BA0_ACCTL);    /***** <9> Wait until CODEC calibration is finished.*/    for(temp1=0; temp1<1000; temp1++) {	taskDelay(10);	/*	delayus(10000);*/	if(cs4281_read_ac97(BA0_AC97_POWERDOWN, &temp2) )	  return -1;	if( (temp2 & 0x0000000F) == 0x0000000F )	    break;    }    if( (temp2 & 0x0000000F) != 0x0000000F ) {	printf("cs4281: Codec failed to calibrate/n");	return -1;    }    /***** <12> Start digital data transfer of audio data to codec *///.........这里部分代码省略.........

/* emulate unix sleep * casey */void sleep(int seconds)	{	taskDelay(seconds*TICK);	}

voidusrAppInit(void){    printf("In usr usrAppInit/n");//    task_leds_start();    int i = 0;    uint32_t cnt = 0;    uint32_t heart_5[5];    uint32_t heart;    uint32_t max;    uint32_t min;    uint32_t total;    adc_init();    OSTaskChangePrio(0, 7);    uint32_t ad;    uint32_t pre_ad = get_v();    pre_tick = tickGet();    while (1)    {        ad = get_v();        if ((pre_ad < 570) && (ad >= 570))        {//            printf("%d pre_ad:%d ad:%d", i++, pre_ad, ad);            heart_tick = tickGet();            pre_tick = heart_tick - pre_tick;            heart = (1200000 / pre_tick);            if (heart > 35*10 && heart < 180*10)            {                memmove(&heart_5[0], &heart_5[1], 4*sizeof(uint32_t));                heart_5[4] = heart;                if (cnt < 5)                {                    cnt++;                }                else                {                    total = max = min = heart_5[0];                    for(int i = 1; i < 5; i++)                    {                        total += heart_5[i];                        if (heart_5[i] > max)                        {                            max = heart_5[i];                        }                        if (heart_5[i] < min)                        {                            min = heart_5[i];                        }                    }                    heart = (total - max - min) / 3;                    printf("心率:%d.%d/n", heart / 10,  heart % 10);//pre_tick                }            }            pre_tick = heart_tick;            sys_gpio_write(IO_LED2, E_LED_ON);        }        else if ((ad < 570) && (pre_ad >= 570))        {            sys_gpio_write(IO_LED2, E_LED_OFF);        }        pre_ad = ad;        taskDelay(2);    }}

int nanosleep    (    const struct timespec *rqtp,	/* time to delay                     */    struct timespec       *rmtp		/* premature wakeup (NULL=no result) */    )    {    int status;    /* int oldErrno; */    ULONG delayTicks;    struct timespec then;    struct timespec now;    int returnStatus;    int savtype;    if (rqtp == NULL || !TV_VALID(*rqtp))	{	errno = EINVAL;	return (ERROR);	}    if (TV_ISZERO(*rqtp))	return (OK);    if (_func_pthread_setcanceltype != NULL)        {        _func_pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &savtype);        }    (void)clockLibInit (); /* make sure clock "running" */    (void)clock_gettime (CLOCK_REALTIME, &then);    TV_CONVERT_TO_TICK (delayTicks, *rqtp);    /* return's 1 (RESTART) if interrupted sleep */    status = taskDelay (delayTicks);    if (status == 0)	returnStatus = 0;    else	returnStatus = -1;    if (rmtp != NULL)	{	(void)clock_gettime (CLOCK_REALTIME, &now);	TV_SUB (now, then);	/* make time relative to start */	if (TV_LT(now, *rqtp))	    {	    TV_SET(*rmtp, *rqtp);	    TV_SUB(*rmtp, now);	    }	else	    TV_ZERO((*rmtp));	}    if (_func_pthread_setcanceltype != NULL)        {        _func_pthread_setcanceltype(savtype, NULL);        }    return (returnStatus);    }

/********************************************************************************  Function:  CFE_PSP_Restart()****  Purpose:**    Provides a common interface to reset the processor.  This implementation**      may need to be customized for missions with custom reset requirements.****  Arguments:**    reset_type  : Type of reset.****  Return:**    (none)*/void CFE_PSP_Restart(uint32 reset_type){   OS_printf("/nWarning: CFE PSP Restart with reset type %u!/n/n", reset_type);   /*   ** Delay for second or two, allow the print statement to send   */   taskDelay(100);#ifndef _WRS_VX_SMP /* Changed for SMP API compatability. */   taskLock();   intLock();#else   /* Note: This only locks the current CPU core.  Other cores    *  are still active and may continue to access system resources    *  or service the watchdog on an SMP system.    */   taskCpuLock();   intCpuLock();#endif   if ( reset_type == CFE_ES_POWERON_RESET )   {      CFE_PSP_ReservedMemoryPtr->bsp_reset_type = CFE_ES_POWERON_RESET;      /*      ** The sysToMonitor function flushes caches for us      *      * reboot(BOOT_CLEAR);      */      /*      ** Use the watchdog timer to assert a reset      */      CFE_PSP_WatchdogEnable();      for(;;)      {         /*         ** Set the current count value to something small         */         CFE_PSP_WatchdogSet(CFE_PSP_WATCHDOG_MIN);         /*         ** Wait for the watchdog to expire...         */         taskDelay(100);      }   }   else   {      CFE_PSP_ReservedMemoryPtr->bsp_reset_type = CFE_ES_PROCESSOR_RESET;      /*      ** The sysToMonitor function flushes caches for us      *      * reboot(0);      */      /*      ** Use the watchdog timer to assert a reset      */      CFE_PSP_WatchdogEnable();      for(;;)      {         /*         ** Set the current count value to something small         */         CFE_PSP_WatchdogSet(CFE_PSP_WATCHDOG_MIN);         /*         ** Wait for the watchdog to expire...         */         taskDelay(100);      }   }}

static int dspIoctl (int devId, int function, int arg){  DSP_FD *pDsp = (DSP_FD *)devId;  SND_DEV *pDev = pDsp->dev.pDev;    AT91PS_SSC	pSSC = (AT91PS_SSC ) pDev->port;  union arg_union  {    int i;    long l;    int *pInt;    long *pLong;    snd_info_t *pInfo;  } u_arg;    u_arg.i = arg;  switch (function)  {    case SNDCTL_DSP_SYNC:      while (pDev->taskBusy) taskDelay (1);      return OK;    case SNDCTL_DSP_GETBLKSIZE:      *u_arg.pInt = MAX_DMA_SIZE;      return OK;    case SNDCTL_DSP_SPEED:      {	int i = *u_arg.pLong;	if (i < RATE_MIN) i = RATE_MIN;	if (i > RATE_MAX) i = RATE_MAX;	while (pDev->taskBusy) taskDelay (1);	pDsp->info.rate = i;	AT91F_SSC_SetBaudrate(pSSC, MCK, pDsp->info.rate*(BITS_BY_SLOT*SLOT_BY_FRAME));	return OK;      }    case SNDCTL_DSP_STEREO:      while (pDev->taskBusy) taskDelay (1);      pDsp->info.stereo = *u_arg.pInt;      return OK;    case SNDCTL_DSP_SAMPLESIZE:      if (*u_arg.pInt == 8 || *u_arg.pInt == 16)      {	while (pDev->taskBusy) taskDelay (1);	pDsp->info.sampleSize = *u_arg.pInt;	return OK;      }      break;    case SNDCTL_DSP_SETFORMAT:      pDsp->info.uLaw = *u_arg.pInt;      return OK;      break;    case SNDCTL_GET_INFO:      *u_arg.pInfo = pDsp->info;      return OK;    case SNDCTL_SET_INFO:      while (pDev->taskBusy) taskDelay (1);      pDsp->info = *u_arg.pInfo;      return OK;  }  errno = S_ioLib_UNKNOWN_REQUEST;  return ERROR;}

void autonSelectMenu() {	lcdSetBacklight(uart1, true);	while (button != LCD_BTN_CENTER) {		lcdClear(uart1);		lcdSetText(uart1, 1, "Alliance Color?");		lcdSetText(uart1, 2, "< Red     Blue >");		button = getLcdButtons();		if (button == LCD_BTN_LEFT) {			lcdClear(uart1);			lcdSetText(uart1, 1, "Starting Side?");			lcdSetText(uart1, 2, "< Middle  Hang >");			while (button != LCD_BTN_LEFT || button != LCD_BTN_RIGHT) {				button = getLcdButtons();				// Red Middle				if (button == LCD_BTN_LEFT) {					int screen = 0;					int minScreen = 0;					int maxScreen = 4;					lcdClear(uart1);					while (button != LCD_BTN_CENTER) {						switch (screen) {							case 0:								lcdSetText(uart1, 1, "auton one");								break;							case 1:								lcdSetText(uart1, 1, "auton two");								break;							case 2:								lcdSetText(uart1, 1, "auton three");								break;							case 3:								lcdSetText(uart1, 1, "auton four");								break;						}						button = getLcdButtons();						if (button == LCD_BTN_RIGHT) {							if (++screen > maxScreen)								screen = minScreen;						} else if (button == LCD_BTN_LEFT) {							if (--screen < minScreen)								screen = maxScreen;						}					}					autonSelection = screen;					lcdSetText(uart1, 1, "   AUTONOMOUS");					lcdSetText(uart1, 2, "      SET");					taskDelay(400);					return;				}				// Red Hang				if (button == LCD_BTN_RIGHT) {					int screen = 0;					int minScreen = 0;					int maxScreen = 4;					lcdClear(uart1);					while (button != LCD_BTN_CENTER) {						switch (screen) {							case 0:								lcdSetText(uart1, 1, "auton one");								break;							case 1:								lcdSetText(uart1, 1, "auton two");								break;							case 2:								lcdSetText(uart1, 1, "auton three");								break;							case 3:								lcdSetText(uart1, 1, "auton four");								break;						}						button = getLcdButtons();						if (button == LCD_BTN_RIGHT) {							if (++screen > maxScreen)							screen = minScreen;						} else if (button == LCD_BTN_LEFT) {							if (--screen < minScreen)								screen = maxScreen;						}					}					autonSelection = screen;					lcdSetText(uart1, 1, "   AUTONOMOUS");					lcdSetText(uart1, 2, "      SET");					taskDelay(400);//.........这里部分代码省略.........