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;
}
int
fadcFirmwareZeroSRAM (int id)
{
int ii, value=0, value_1=0, value_2=0;
int ErrorCount=0, stopPrint=0;
if(id==0) id=fadcID[0];
if((id<=0) || (id>21) || (FAp[id] == NULL))
{
printf("%s: ERROR : ADC in slot %d is not initialized \n",
__FUNCTION__,id);
return ERROR;
}
/* set address = 0; allow increment on mem2 access */
FALOCK;
vmeWrite32(&FAp[id]->mem_adr, 0x0000 | FA_MEM_ADR_INCR_MEM2);
for( ii = 0; ii < 0x80000; ii++) /* write ZERO to entire memory */
{
vmeWrite32(&FAp[id]->mem1_data, 0);
vmeWrite32(&FAp[id]->mem2_data, 0);
}
/* reset address = 0; allow increment on mem2 access */
vmeWrite32(&FAp[id]->mem_adr, 0x0000 | FA_MEM_ADR_INCR_MEM2);
FAUNLOCK;
/* read and test expected memory data */
for( ii = 0; ii < 0x80000; ii++)
{
FALOCK;
value_1 = vmeRead32(&FAp[id]->mem1_data);
value_2 = vmeRead32(&FAp[id]->mem2_data);
FAUNLOCK;
if( (value_1 != 0) || (value_2 != 0) )
{
ErrorCount++;
FALOCK;
value = vmeRead32(&FAp[id]->mem_adr) & 0xFFFFF;
FAUNLOCK;
if(!stopPrint)
{
printf("%s: ERROR: FADC %2d address = %8X mem1 read = %8X mem2 read = %8X\n",
__FUNCTION__,id,
value, value_1, value_2);
taskDelay(1); /* wait */
}
if(ErrorCount==80)
{
printf("%s: Further errors for FADC %2d will not be displayed\n",
__FUNCTION__,id);
stopPrint=1;
}
}
}
if(ErrorCount)
return ERROR;
return OK;
}
int
fadcFirmwareVerifyDownload (int id)
{
unsigned int ArraySize;
unsigned int ByteCount, ByteIndex, ByteNumber;
unsigned int ExpWord32Bits, SramAdr, RdWord32Bits;
int ErrorCount=0, stopPrint=0;
int value;
if(id==0) id=fadcID[0];
if((id<=0) || (id>21) || (FAp[id] == NULL))
{
printf("%s: ERROR : ADC in slot %d is not initialized \n",
__FUNCTION__,id);
return ERROR;
}
if(MSC_loaded != 1)
{
printf("%s: ERROR : Firmware was not loaded\n",
__FUNCTION__);
return ERROR;
}
/* ArraySize = sizeof(MSC_ARRAY); */
ArraySize = MSC_arraySize;
ByteIndex = 0;
SramAdr = 0;
/* write SRAM address register */
/* start at 0 and increment address after read from mem1 data register */
FALOCK;
vmeWrite32(&FAp[id]->mem_adr, 0x0000 | FA_MEM_ADR_INCR_MEM1);
value = vmeRead32(&FAp[id]->mem_adr);
FAUNLOCK;
#ifdef DEBUG
printf("%s: FADC %2d memory address at start of read = 0x%08x\n\n",
__FUNCTION__,id,value);
#endif
taskDelay(2); /* wait */
for (ByteCount = 0; ByteCount < ArraySize; ByteCount += 4)
{
/* get expected value */
ExpWord32Bits = 0;
for (ByteNumber = 0; ByteNumber < 4; ++ByteNumber)
{
ExpWord32Bits = (MSC_ARRAY[ByteIndex] << (8 * ByteNumber)) | ExpWord32Bits;
++ByteIndex;
}
/* read 32-bit data word from mem1 data register */
FALOCK;
RdWord32Bits = (unsigned int)vmeRead32(&FAp[id]->mem1_data);
FAUNLOCK;
#ifdef DEBUG
if(ByteCount<40)
printf("RdWord32Bits = 0x%08x\n",RdWord32Bits);
#endif
/* test if read value = expected value */
if (RdWord32Bits != ExpWord32Bits)
{
ErrorCount++;
if(!stopPrint)
printf("%s: ERROR: FADC %2d ByteCount %8d Expect %08X Read %08X\n",
__FUNCTION__,id,
ByteCount, ExpWord32Bits, RdWord32Bits);
if( ErrorCount==80 )
{
printf("%s: Further errors for FADC %2d will not be displayed\n",
__FUNCTION__,id);
/* getchar(); */
stopPrint=1;
}
/* if( ErrorCount>1000 ) */
/* stopPrint=1; */
}
}
FALOCK;
value = vmeRead32(&FAp[id]->mem_adr);
FAUNLOCK;
#ifdef DEBUG
printf("%s: memory address after read = 0x%08x\n\n",
__FUNCTION__,value);
#endif
if(ErrorCount)
printf("%s: ErrorCount = %d\n",__FUNCTION__,ErrorCount);
taskDelay(1); /* wait */
if(ErrorCount)
return ERROR;
return OK;
}
int
fadcFirmwareLoad(int id, int chip, int pFlag)
{
if(id==0) id=fadcID[0];
if((id<=0) || (id>21) || (FAp[id] == NULL))
{
printf("%s: ERROR : ADC in slot %d is not initialized \n",
__FUNCTION__,id);
return ERROR;
}
if(chip<0 || chip>2)
{
printf("%s: ERROR: Invalid chip parameter %d\n",
__FUNCTION__,chip);
return ERROR;
}
/* Perform a hardware and software reset */
FALOCK;
vmeWrite32(&FAp[id]->reset, 0xFFFF);
FAUNLOCK;
taskDelay(60);
/* Check if FADC is Ready */
if(fadcFirmwareTestReady(id, 60, pFlag) != OK)
{
printf("%s: ERROR: FADC %2d not ready after reset\n",
__FUNCTION__,id);
return ERROR;
}
/* Data to SRAM */
printf("%s: Loading SRAM with data \n",__FUNCTION__);
fadcFirmwareDownloadConfigData(id);
if(fadcFirmwareVerifyDownload(id) != OK)
{
printf("%s: ERROR: FADC %2d Failed data verification at SRAM\n",
__FUNCTION__,id);
return ERROR;
}
/* SRAM TO PROM */
taskDelay(1);
printf("%s: Loading PROM with SRAM data \n",__FUNCTION__);
FALOCK;
if(chip==FADC_FIRMWARE_LX110)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_SRAM_TO_PROM1);
else if(chip==FADC_FIRMWARE_FX70T)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_SRAM_TO_PROM2);
FAUNLOCK;
taskDelay(1);
if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */
{
printf("%s: ERROR: FADC %2d ready timeout SRAM -> PROM\n",
__FUNCTION__,id);
return ERROR;
}
/* PROM TO SRAM (For verification) */
printf("%s: Loading SRAM with PROM data \n",__FUNCTION__);
fadcFirmwareZeroSRAM(id);
FALOCK;
if(chip==FADC_FIRMWARE_LX110)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_PROM1_TO_SRAM);
else if(chip==FADC_FIRMWARE_FX70T)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_PROM2_TO_SRAM);
FAUNLOCK;
taskDelay(1);
if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */
{
printf("%s: ERROR: FADC %2d ready timeout PROM -> SRAM\n",
__FUNCTION__,id);
return ERROR;
}
/* Compare SRAM to Data Array */
printf("%s: Verifying data \n",__FUNCTION__);
if(fadcFirmwareVerifyDownload(id) != OK)
{
printf("%s: ERROR: FADC %d PROM data not verified\n",
__FUNCTION__,id);
return ERROR;
}
/* PROM to FPGA (Reboot FPGA) */
printf("%s: Rebooting FPGA \n",__FUNCTION__);
FALOCK;
if(chip==FADC_FIRMWARE_LX110)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_REBOOT_FPGA1);
else if(chip==FADC_FIRMWARE_FX70T)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_REBOOT_FPGA2);
FAUNLOCK;
taskDelay(1);
if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */
{
printf("%s: ERROR: FADC %2d ready timeout PROM -> FPGA\n",
__FUNCTION__,id);
return ERROR;
}
printf("%s: Done programming FADC %2d\n",
__FUNCTION__,id);
return OK;
}
/*****************************************************************
功能:控制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);
motorSet(5, 0);
//Lift cube & stop lifting cube
motorSet(6, 127);
motorSet(7, 127);
motorSet(8, 127);
taskDelay(2000);
motorSet(6, 0);
motorSet(7, 0);
motorSet(8, 0);
/*
//Turn left
Y1 = -127;
X1 = 0;
X2 = 0;
motorSet(2, Y1 + X2 + X1);
motorSet(3, Y1 - X2 - X1);
motorSet(4, Y1 + X2 - X1);
motorSet(5, Y1 - X2 + X1);
taskDelay(800);
//Stop turning left
motorSet(2, 0);
motorSet(3, 0);
motorSet(4, 0);
motorSet(5, 0);
//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(1100);
//Stop going forward
motorSet(2, 0);
motorSet(3, 0);
motorSet(4, 0);
motorSet(5, 0);
//Turn left
Y1 = -127;
X1 = 0;
X2 = 0;
motorSet(2, Y1 + X2 + X1);
motorSet(3, Y1 - X2 - X1);
motorSet(4, Y1 + X2 - X1);
motorSet(5, Y1 - X2 + X1);
taskDelay(1100);
//Stop turning left
motorSet(2, 0);
motorSet(3, 0);
motorSet(4, 0);
motorSet(5, 0);
//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(2200);
//Stop going forward
motorSet(2, 0);
motorSet(3, 0);
motorSet(4, 0);
motorSet(5, 0);
//Drop Cube
motorSet(6, -127);
motorSet(7, -127);
motorSet(8, -127);
taskDelay(1000);
//Stop dropping
motorSet(6, 0);
motorSet(7, 0);
motorSet(8, 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
* from its read() and use the restored descriptors. However,
* problems can occur upon logout if the remote user has disabled
* the line editor and/or put the pty device in raw mode, so the
* shell is restarted in all cases.
*/
remoteId = 0; /* Allow a new session. */
/* notify the shell we have ended a remote session */
shellIsRemoteConnectedSet (FALSE);
excJobAdd (shellRestart, FALSE, 0, 0, 0, 0, 0);
return (OK);
}
return (ERROR); /* Ignore unknown control operations. */
}
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
*/
/*
** now enter the info in the table
*/
OS_VolumeTable[i].FreeFlag = FALSE;
strcpy(OS_VolumeTable[i].VolumeName, volname);
OS_VolumeTable[i].BlockSize = blocksize;
ReturnCode = OS_FS_SUCCESS;
}
#ifdef USE_VXWORKS_ATA_DRIVER
/*
** Format an ATA disk
** This code requires an ATA driver in the BSP, so it must be
** left out of the compilation BSPs without.
*/
/* --------------------------------------------- */
else if (OS_VolumeTable[i].VolumeType == ATA_DISK)
{
printf("OSAL: Formatting a FLASH DISK\n");
/*
** Create the Flash disk device
*/
if( (ataBlkDev = ataDevCreate( 0, 0, 0, 0)) == NULL)
{
printf("OSAL: Error Creating flash disk device.\n");
ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED;
}
else
{
printf("OSAL: FLASH device initialized.\n");
/*
** Attach to the XBD layer
*/
xbd = xbdBlkDevCreate(ataBlkDev,local_volname);
if (xbd == NULLDEV)
{
printf("OSAL: Error Creating XBD device on FLASH disk.\n");
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
*/
status = OS_GetPhysDeviceName(OS_VolumeTable[i].PhysDevName, local_volname);
if ( status == OS_FS_ERROR )
{
ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED;
}
else
{
/*
** Format the Device with the DOS file system
*/
if ( dosFsVolFormat(OS_VolumeTable[i].PhysDevName,DOS_OPT_BLANK,NULL) == ERROR )
{
printf("OSAL: DOS format error on flash disk.\n");
ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED;
}
else
{
ReturnCode = OS_FS_SUCCESS;
}
}
}
}
}/* if ATA_DISK */
#endif
else
{
/*
** VolumeType is something else that is not supported right now
*/
ReturnCode = OS_FS_ERROR;
}
return ReturnCode;
} /* end OS_mkfs */
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
/* No more soft triggers */
/* tidSoftTrig(0x0,0x8888,0); */
tsSoftTrig(1,0,0x700,0);
tsDisableRandomTrigger();
#endif
tsPartIntDisable();
tsPartIntDisconnect();
CLOSE:
vmeCloseDefaultWindows();
exit(0);
}
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 )
{
GpioValSet_ALTERA(FPGA_DIN_ALTERA, 1);
}
else
{
GpioValSet_ALTERA(FPGA_DIN_ALTERA, 0);
}
mydelay();
/*置FPGA_CLK为1，因为上升沿把DIN
上的电平值打入FPGA，此时打入*/
GpioValSet_ALTERA(FPGA_CLK_ALTERA, 1);
mydelay();
}
if(0 == GpioValGet_ALTERA(FPGA_INIT_ALTERA))
{
free(pFpgaFile);
printf("\r\n FPGA_INIT_ALTERA in middle!\n") ;
return(3);
}
}
free(pFpgaFile);
for(i = 0; i <100; i++)
{
GpioValSet_ALTERA(FPGA_CLK_ALTERA , 0 );
mydelay();
GpioValSet_ALTERA(FPGA_CLK_ALTERA , 1 );
mydelay();
}
}
milliseconds_t Sleep(milliseconds_t milliDuration)
{
int ticks = TimeUtils::Milli2Ticks(milliDuration);
taskDelay(ticks);
return TimeUtils::Ticks2Milli(ticks);
}
/***************************************************************************
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 */
writel(ACOSV_SLV3 | ACOSV_SLV4, CS4281_pBA0 + BA0_ACOSV );
writel(ACISV_ISV3 | ACISV_ISV4, CS4281_pBA0 + BA0_ACISV );
/************************************************
* Unmute the Master and
* Alternate (headphone) volumes, to max.
************************************************/
cs4281_write_ac97(BA0_AC97_MASTER_VOLUME, 0x0000);
cs4281_write_ac97(BA0_AC97_HEADPHONE_VOLUME, 0x0000);
cs4281_write_ac97(BA0_AC97_MASTER_VOLUME_MONO, 0x0010);
cs4281_write_ac97(BA0_AC97_PC_BEEP_VOLUME, 0x0010);
cs4281_write_ac97(BA0_AC97_PHONE_VOLUME, 0x0008);
cs4281_write_ac97(BA0_AC97_MIC_VOLUME, 0x0008);
cs4281_write_ac97(BA0_AC97_CD_VOLUME, 0x0808);
cs4281_write_ac97(BA0_AC97_VIDEO_VOLUME, 0x0808);
cs4281_write_ac97(BA0_AC97_AUX_VOLUME, 0x0808);
cs4281_write_ac97(BA0_AC97_PCM_OUT_VOLUME, 0x0000);
cs4281_write_ac97(BA0_AC97_RECORD_GAIN, 0x0b0b);
cs4281_write_ac97(BA0_AC97_RECORD_GAIN_MIC, 0x0b0b);
/************************************************
* POWER on the DAC
************************************************/
cs4281_read_ac97(BA0_AC97_POWERDOWN, &temp1);
cs4281_write_ac97(BA0_AC97_POWERDOWN, temp1 &= 0xfdff);
/* Wait until we sample a DAC ready state */
for(temp2=0; temp2<32; temp2++) {
taskDelay(1);
/* delayus(1000);*/
cs4281_read_ac97(BA0_AC97_POWERDOWN, &temp1);
if(temp1 & 0x2)
break;
}
/************************************************
* POWER on the ADC
************************************************/
cs4281_read_ac97(BA0_AC97_POWERDOWN, &temp1);
cs4281_write_ac97(BA0_AC97_POWERDOWN, temp1 &= 0xfeff);
/* Wait until we sample a DAC ready state */
for(temp2=0; temp2<32; temp2++) {
taskDelay(1);
/* delayus(1000);*/
cs4281_read_ac97(BA0_AC97_POWERDOWN, &temp1);
if(temp1 & 0x1)
break;
}
/*
For playback, we map AC97 slot 3 and 4(Left
& Right PCM playback) to DMA Channel 0.
Set the fifo to be 15 bytes at offset zero.
*/
ac97_slotid = 0x01000f00;
writel(ac97_slotid, CS4281_pBA0 + BA0_FCR0);
writel(ac97_slotid | FCRn_FEN, CS4281_pBA0 + BA0_FCR0);
/*
For record, we map AC97 slots 3 and 4(Left
& Right Record) to DMA Channel 1. Set the
fifo to be 15 bytes at offset 63.
*/
ac97_slotid = 0x0b0a0f3f;
writel(ac97_slotid, CS4281_pBA0 + BA0_FCR1);
writel(ac97_slotid | FCRn_FEN, CS4281_pBA0 + BA0_FCR1);
/*
Map the Playback SRC to the same AC97 slots(3 & 4--
--Playback left & right)as DMA channel 0.
Map the record SRC to the same AC97 slots(10 & 11--
-- Record left & right) as DMA channel 1.
*/
ac97_slotid = 0x0b0a0100;
writel(ac97_slotid, CS4281_pBA0 + BA0_SRCSA);
/*
Set 'Half Terminal Count Interrupt Enable' and 'Terminal
Count Interrupt Enable' in DMA Control Registers 0 & 1.
Set 'MSK' flag to 1 to keep the DMA engines paused.
*/
/* temp1 = (DCRn_TCIE | DCRn_MSK);
writel(temp1, CS4281_pBA0 + BA0_DCR0);*/
temp1 = (DCRn_HTCIE | DCRn_TCIE | DCRn_MSK);
writel(temp1, CS4281_pBA0 + BA0_DCR0);
/* temp1 = (DCRn_HTCIE | DCRn_TCIE | DCRn_MSK);
writel(temp1, CS4281_pBA0 + BA0_DCR1); */
temp1 = (DCRn_TCIE | DCRn_MSK);
writel(temp1, CS4281_pBA0 + BA0_DCR1);
/*
Set 'Auto-Initialize Control' to 'enabled'; For playback,
set 'Transfer Type Control'(TR[1:0]) to 'read transfer',
for record, set Transfer Type Control to 'write transfer'.
All other bits set to zero; Some will be changed @ transfer start.
*/
temp1 = (DMRn_DMA | DMRn_AUTO | DMRn_TR_READ);
writel(temp1, CS4281_pBA0 + BA0_DMR0);
temp1 = (DMRn_DMA | DMRn_AUTO | DMRn_TR_WRITE);
writel(temp1, CS4281_pBA0 + BA0_DMR1);
/*
Enable DMA interrupts generally, and
DMA0 & DMA1 interrupts specifically.
*/
temp1 = readl(CS4281_pBA0 + BA0_HIMR) & 0xfffbfcff;
writel(temp1, CS4281_pBA0+BA0_HIMR);
printf("\nCS4281 Hardware Initialization Complete!!\n");
return 0;
}
void Suspend()
{
taskDelay(0);
}
/* emulate unix sleep
* casey
*/
void sleep(int seconds)
{
taskDelay(seconds*TICK);
}
void
usrAppInit(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);
return;
}
}
}
if (button == LCD_BTN_RIGHT) {
lcdClear(uart1);
lcdSetText(uart1, 1, "Starting Side?");
lcdSetText(uart1, 2, "< Middle Hang >");
while (button != LCD_BTN_LEFT || button != LCD_BTN_RIGHT) {
button = getLcdButtons();
// Blue 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;
}
// Blue 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);
return;
}
}
}
}
}
/**
* To talk to an ET system on another computer, we need
* to find the TCP port number the server thread of that
* ET system is listening on. There are a number of ways to
* do this.<p>
*
* The remote client (who is running this routine) either
* says he doesn't know where the ET is (config->host =
* ET_HOST_REMOTE or ET_HOST_ANYWHERE), or he specifies
* the host that the ET system is running on.<p>
*
* If we don't know the host name, we can broadcast and/or
* multicast (UDP) to addresses which the ET system is
* listening on (each address on a separate thread). In
* this broad/multicast, we send the name of the ET system
* that we want to find. The system which receives the
* packet and has the same name, responds with quite a bit of information
* including its TCP port #, uname, canonical host name, and
* all of its IP addresses in dotted-decimal form. This
* info is sent from each interface or broad/multicast address
* that received the packet.<p>
*
* Notice that if broad/multicasting to an unknown host was successful,
* only 1 structure in passed back in the allETinfo arg even though it
* is a linked list.<p>
*
* If we know the hostname, however, we send a udp packet
* to ET systems listening on that host and UDP port. The
* proper system should be the only one to respond with its
* server TCP port.<p>
*
* When a server responds to a broad/multicast and we've
* specified ET_HOST_REMOTE, this routine must see whether
* the server is really on a remote host and not the local one.
* To make this determination, the server has also sent
* his host name obtained by running "uname". That is compared
* to the result of running "uname" on this host.
*
* @param etname ET system file name
* @param ethost returns name of ET system's host
* @param port returns ET system's TCP port
* @param inetaddr returns host address as network-byte-ordered, 32-bit unsigned int
* @param allETinfo returns a pointer to structure (linked list) with all of the ET system's
* response information;
* may be null if all relevant info given in ethost, port, and inetaddr args
* @param config configuration passed to et_open
* @param trys max # of times to broadcast UDP packet
* @param waittime wait time for response to broad/multicast
*
* @return ET_OK if successful
* @return ET_ERROR failure in select statement
* @return ET_ERROR_BADARG NULL arg for ethost, port, or inetaddr
* @return ET_ERROR_NOMEM cannot allocate memory
* @return ET_ERROR_NETWORK error find IP addresses or resolving host name;
* error converting dotted-decimal IP addr to binary;
* error create socket; failure reading socket
* @return ET_ERROR_SOCKET error setting socket option
* @return ET_ERROR_TOOMANY if multiple ET systems responded when policy is to return an error when
* more than one responds.
* @return ET_ERROR_TIMEOUT if no responses received in allotted time
*/
int et_findserver2(const char *etname, char *ethost, int *port, uint32_t *inetaddr,
et_response **allETinfo, et_open_config *config, int trys, struct timeval *waittime)
{
int i, j, k, l, m, n, err, version, addrCount, castType, gotMatch=0, subnetCount=0, ipAddrCount=0;
int length, len_net, lastdelay, maxtrys=6, serverport=0, debug=1, magicInts[3];
const int on=1, timeincr[]={0,1,2,3,4,5};
uint32_t addr;
codaIpList *baddr;
/* encoding & decoding packets */
char *pbuf, buffer[4096]; /* should be way more than enough */
char outbuf[ET_FILENAME_LENGTH+1+5*sizeof(int)];
char localhost[ET_MAXHOSTNAMELEN];
char localuname[ET_MAXHOSTNAMELEN];
char **ipAddrs;
char specifiedhost[ET_MAXHOSTNAMELEN];
char unqualifiedhost[ET_MAXHOSTNAMELEN];
int numresponses=0, remoteresponses=0;
et_response *answer, *answer_first=NULL, *answer_prev=NULL, *first_remote=NULL;
/* socket & select stuff */
struct in_addr castaddr;
struct sockaddr_in servaddr, cliaddr;
socklen_t len;
int sockfd, numsockets=0, biggestsockfd=-1;
struct timespec delaytime;
struct timeval tv;
fd_set rset;
/* structure for holding info to send out a packet on a socket */
struct senddata {
int sockfd;
struct sockaddr_in servaddr;
};
struct senddata *send;
/* check args */
if ((ethost == NULL) || (port == NULL) || (inetaddr == NULL)) {
fprintf(stderr, "et_findserver: invalid (null) arguments\n");
return ET_ERROR_BADARG;
}
/* count # of subnet addrs */
baddr = config->bcastaddrs;
while (baddr != NULL) {
subnetCount++;
baddr = baddr->next;
}
/* allocate space to store all our outgoing stuff */
send = (struct senddata *) calloc(subnetCount + config->mcastaddrs.count,
sizeof(struct senddata));
if (send == NULL) {
fprintf(stderr, "et_findserver: cannot allocate memory\n");
return ET_ERROR_NOMEM;
}
/* find local uname */
if (etNetGetUname(localuname, ET_MAXHOSTNAMELEN) != ET_OK) {
/* nothing will match this */
strcpy(localuname, "...");
}
/* If the host is local or we know its name... */
if ((strcmp(config->host, ET_HOST_REMOTE) != 0) &&
(strcmp(config->host, ET_HOST_ANYWHERE) != 0)) {
/* if it's local, find dot-decimal IP addrs */
if ((strcmp(config->host, ET_HOST_LOCAL) == 0) ||
(strcmp(config->host, "localhost") == 0)) {
if ((etNetGetIpAddrs(&ipAddrs, &ipAddrCount, NULL) != ET_OK) || ipAddrCount < 1) {
fprintf(stderr, "et_findserver: cannot find local IP addresses\n");
return ET_ERROR_NETWORK;
}
}
/* else if we know its name, find dot-decimal IP addrs */
else {
if ((etNetGetIpAddrs(&ipAddrs, &ipAddrCount, config->host) != ET_OK) || ipAddrCount < 1) {
fprintf(stderr, "et_findserver: cannot find IP addresses for %s\n", config->host);
return ET_ERROR_NETWORK;
}
}
}
/* else if the host name is not specified, and it's either
* remote or anywhere out there, broad/multicast to find it */
else { }
/* We need 1 socket for each subnet if broadcasting */
if ((config->cast == ET_BROADCAST) ||
(config->cast == ET_BROADANDMULTICAST)) {
printf("et_findserver2 1\n");
/* for each listed broadcast address ... */
baddr = config->bcastaddrs;
while (baddr != NULL) {
/* put address into net-ordered binary form */
if (inet_aton(baddr->addr, &castaddr) == INET_ATON_ERR) {
fprintf(stderr, "et_findserver: inet_aton error net_addr = %s\n", baddr->addr);
for (j=0; j<numsockets; j++) {
close(send[j].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
return ET_ERROR_NETWORK;
}
if (debug) {
printf("et_findserver: send broadcast packet to %s on port %d\n", baddr->addr, config->udpport);
}
/* server's location */
bzero((void *)&servaddr, sizeof(servaddr));
servaddr.sin_family = AF_INET;
servaddr.sin_addr = castaddr;
servaddr.sin_port = htons((unsigned short)config->udpport);
/* create socket */
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) {
fprintf(stderr, "et_findserver: socket error\n");
for (j=0; j<numsockets; j++) {
close(send[j].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
return ET_ERROR_NETWORK;
}
/* make this a broadcast socket */
err = setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, (void *) &on, sizeof(on));
if (err < 0) {
fprintf(stderr, "et_findserver: setsockopt SO_BROADCAST error\n");
close(sockfd);
for (j=0; j<numsockets; j++) {
close(send[j].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
return ET_ERROR_SOCKET;
}
/* for sending packet and for select */
send[numsockets].sockfd = sockfd;
send[numsockets].servaddr = servaddr;
numsockets++;
if (biggestsockfd < sockfd) biggestsockfd = sockfd;
FD_SET(sockfd, &rset);
/* go to next broadcast address */
baddr = baddr->next;
}
}
/* if also configured for multicast, send that too */
if ((config->cast == ET_MULTICAST) ||
(config->cast == ET_BROADANDMULTICAST)) {
printf("et_findserver2 2\n");
/* for each listed address ... */
for (i=0; i < config->mcastaddrs.count; i++) {
/* put address into net-ordered binary form */
if (inet_aton(config->mcastaddrs.addr[i], &castaddr) == INET_ATON_ERR) {
fprintf(stderr, "et_findserver: inet_aton error\n");
for (j=0; j<numsockets; j++) {
close(send[j].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
return ET_ERROR_NETWORK;
}
if (debug) {
printf("et_findserver: send multicast packet to %s on port %d\n",
config->mcastaddrs.addr[i], config->multiport);
}
/* server's location */
bzero((void *)&servaddr, sizeof(servaddr));
servaddr.sin_family = AF_INET;
servaddr.sin_addr = castaddr;
servaddr.sin_port = htons((unsigned short)config->multiport);
/* create socket */
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) {
fprintf(stderr, "et_findserver: socket error\n");
for (j=0; j<numsockets; j++) {
close(send[j].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
return ET_ERROR_NETWORK;
}
/* Set the scope of the multicast, but don't bother
* if ttl = 1 since that's the default.
*/
if (config->ttl != 1) {
unsigned char ttl = config->ttl;
err = setsockopt(sockfd, IPPROTO_IP, IP_MULTICAST_TTL, (void *) &ttl, sizeof(ttl));
if (err < 0){
fprintf(stderr, "et_findserver: setsockopt IP_MULTICAST_TTL error\n");
close(sockfd);
for (j=0; j<numsockets; j++) {
close(send[j].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
return ET_ERROR_SOCKET;
}
}
/* for sending packet and for select */
send[numsockets].sockfd = sockfd;
send[numsockets].servaddr = servaddr;
numsockets++;
if (biggestsockfd < sockfd) biggestsockfd = sockfd;
FD_SET(sockfd, &rset);
}
}
/* time to wait for responses to broad/multicasting */
if (waittime != NULL) {
delaytime.tv_sec = waittime->tv_sec;
delaytime.tv_nsec = 1000*waittime->tv_usec;
}
else {
delaytime.tv_sec = 0;
delaytime.tv_nsec = 0;
}
lastdelay = delaytime.tv_sec;
/* make select return after 0.01 seconds */
tv.tv_sec = 0;
tv.tv_usec = 10000; /* 0.01 sec */
/* max # of broadcasts is maxtrys */
if ((trys > maxtrys) || (trys < 1)) {
trys = maxtrys;
}
printf("et_findserver2 9\n");
/* Prepare output buffer that we send to servers:
* (1) ET magic numbers (3 ints),
* (2) ET version #,
* (3) ET file name length (includes null)
* (4) ET file name
*/
pbuf = outbuf;
/* magic numbers */
magicInts[0] = htonl(ET_MAGIC_INT1);
magicInts[1] = htonl(ET_MAGIC_INT2);
magicInts[2] = htonl(ET_MAGIC_INT3);
memcpy(pbuf, magicInts, sizeof(magicInts));
pbuf += sizeof(magicInts);
/* ET major version number */
version = htonl(ET_VERSION);
memcpy(pbuf, &version, sizeof(version));
pbuf += sizeof(version);
/* length of ET system name string */
length = strlen(etname)+1;
len_net = htonl(length);
memcpy(pbuf, &len_net, sizeof(len_net));
pbuf += sizeof(len_net);
/* name of the ET system we want to open */
memcpy(pbuf, etname, length);
/* zero socket-set for select */
FD_ZERO(&rset);
for (i=0; i < trys; i++) {
/* send out packets */
for (j=0; j<numsockets; j++) {
sendto(send[j].sockfd, (void *) outbuf, sizeof(outbuf), 0,
(SA *) &send[j].servaddr, sizeof(servaddr));
}
/* Increase waiting time for response each round. */
delaytime.tv_sec = lastdelay + timeincr[i];
lastdelay = delaytime.tv_sec;
/* reset "rset" value each time select is called */
for (j=0; j<numsockets; j++) {
FD_SET(send[j].sockfd, &rset);
}
/* Linux reportedly changes tv in "select" so reset it each round */
tv.tv_sec = 0;
tv.tv_usec = 10000; /* 0.01 sec */
/* wait for responses from ET systems */
#ifdef VXWORKS
taskDelay(lastdelay*60);
#else
nanosleep(&delaytime, NULL);
#endif
/* select */
err = select(biggestsockfd + 1, &rset, NULL, NULL, &tv);
/* if select error ... */
if (err == -1) {
fprintf(stderr, "et_findserver: select error\n");
for (j=0; j<numsockets; j++) {
close(send[j].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
return ET_ERROR;
}
/* else if select times out, rebroadcast with longer waiting time */
else if (err == 0) {
continue;
}
/* else if we have some response(s) ... */
else {
for(j=0; j < numsockets; j++) {
int bytes;
if (FD_ISSET(send[j].sockfd, &rset) == 0) {
/* this socket is not ready for reading */
continue;
}
anotherpacket:
/* get back a packet from a server */
len = sizeof(cliaddr);
n = recvfrom(send[j].sockfd, (void *) buffer, sizeof(buffer), 0, (SA *) &cliaddr, &len);
/* if error ... */
if (n < 0) {
fprintf(stderr, "et_findserver: recvfrom error, %s\n", strerror(errno));
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
/* free up all answers */
et_freeAnswers(answer_first);
return ET_ERROR_NETWORK;
}
/* allocate space for single response */
answer = (et_response *) calloc(1, sizeof(et_response));
if (answer == NULL) {
fprintf(stderr, "et_findserver: out of memory\n");
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
et_freeAnswers(answer_first);
return ET_ERROR_NOMEM;
}
/* string answer structs in linked list */
if (answer_prev != NULL) {
answer_prev->next = answer;
}
else {
answer_first = answer;
}
/* see if there's another packet to be read on this socket */
bytes = 0;
#ifdef VXWORKS
ioctl(send[j].sockfd, FIONREAD, (int) &bytes);
#else
ioctl(send[j].sockfd, FIONREAD, (void *) &bytes);
#endif
/* decode packet from ET system:
*
* (0) ET magic numbers (3 ints)
* (1) ET version #
* (2) port of tcp server thread (not udp config->port)
* (3) ET_BROADCAST or ET_MULTICAST (int)
* (4) length of next string
* (5) broadcast address (dotted-dec) if broadcast received or
* multicast address (dotted-dec) if multicast received
* (see int #3)
* (6) length of next string
* (7) hostname given by "uname" (used as a general
* identifier of this host no matter which interface is used)
* (8) length of next string
* (9) canonical name of host
* (10) number of IP addresses
* (11) 32bit, net-byte ordered IPv4 address assoc with following address
* (12) length of next string
* (13) first dotted-decimal IPv4 address
* (14) 32bit, net-byte ordered IPv4 address assoc with following address
* (15) length of next string
* (16) second dotted-decimal IPv4 address ...
*
* All known IP addresses are sent here both in numerical & dotted-decimal forms.
*/
pbuf = buffer;
do {
/* get ET magic #s */
memcpy(magicInts, pbuf, sizeof(magicInts));
pbuf += sizeof(magicInts);
/* if the wrong magic numbers, reject it */
if (ntohl(magicInts[0]) != ET_MAGIC_INT1 ||
ntohl(magicInts[1]) != ET_MAGIC_INT2 ||
ntohl(magicInts[2]) != ET_MAGIC_INT3) {
free(answer);
break;
}
/* get ET system's major version # */
memcpy(&version, pbuf, sizeof(version));
version = ntohl(version);
pbuf += sizeof(version);
/* if the wrong version ET system is responding, reject it */
if (version != ET_VERSION) {
free(answer);
break;
}
/* get ET system's TCP port */
memcpy(&serverport, pbuf, sizeof(serverport));
serverport = ntohl(serverport);
if ((serverport < 1) || (serverport > 65536)) {
free(answer);
break;
}
answer->port = serverport;
pbuf += sizeof(serverport);
/* broad or multi cast? might be both for java. */
memcpy(&castType, pbuf, sizeof(castType));
castType = ntohl(castType);
if ((castType != ET_BROADCAST) &&
(castType != ET_MULTICAST) &&
(castType != ET_BROADANDMULTICAST)) {
free(answer);
break;
}
answer->castType = castType;
pbuf += sizeof(castType);
if (debug) {
if (castType == ET_BROADCAST) {
printf("et_findserver: reply to broadcast\n");
}
else if (castType != ET_MULTICAST) {
printf("et_findserver: reply to multicast\n");
}
else if (castType != ET_BROADANDMULTICAST) {
printf("et_findserver: reply to broad & multicast\n");
}
else {
printf("et_findserver: reply -> don't know if broad or multi casting\n");
}
}
/* get broad/multicast IP original packet sent to */
memcpy(&length, pbuf, sizeof(length));
length = ntohl(length);
if ((length < 1) || (length > ET_IPADDRSTRLEN)) {
free(answer);
break;
}
pbuf += sizeof(length);
memcpy(answer->castIP, pbuf, length);
pbuf += length;
/* get ET system's uname */
memcpy(&length, pbuf, sizeof(length));
length = ntohl(length);
if ((length < 1) || (length > ET_MAXHOSTNAMELEN)) {
free(answer);
break;
}
pbuf += sizeof(length);
memcpy(answer->uname, pbuf, length);
pbuf += length;
/* get ET system's canonical name */
memcpy(&length, pbuf, sizeof(length));
length = ntohl(length);
if (length > ET_MAXHOSTNAMELEN) {
free(answer);
break;
}
pbuf += sizeof(length);
if (length > 0) {
memcpy(answer->canon, pbuf, length);
pbuf += length;
}
/* get number of IP addrs */
memcpy(&addrCount, pbuf, sizeof(addrCount));
addrCount = ntohl(addrCount);
if ((addrCount < 0) || (addrCount > 20)) {
free(answer);
break;
}
answer->addrCount = addrCount;
pbuf += sizeof(addrCount);
/* allocate mem - arrays of ints & char *'s */
answer->addrs = (uint32_t *)calloc(addrCount, sizeof(uint32_t));
answer->ipaddrs = (char **)calloc(addrCount, sizeof(char *));
if (answer->addrs == NULL || answer->ipaddrs == NULL) {
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
free(answer);
et_freeAnswers(answer_first);
return ET_ERROR_NOMEM;
}
/* read in all the addrs */
for (k=0; k < addrCount; k++) {
memcpy(&addr, pbuf, sizeof(addr));
/* do not swap the addr as it must be network byte ordered */
answer->addrs[k] = addr;
pbuf += sizeof(addr);
memcpy(&length, pbuf, sizeof(length));
length = ntohl(length);
if ((length < 1) || (length > ET_MAXHOSTNAMELEN)) {
for (l=0; l < k; l++) {
free(answer->ipaddrs[l]);
}
free(answer->ipaddrs);
free(answer->addrs);
free(answer);
break;
}
pbuf += sizeof(length);
answer->ipaddrs[k] = strdup(pbuf); /* ending NULL is sent so we're OK */
if (answer->ipaddrs[k] == NULL) {
for (l=0; l<numsockets; l++) {
close(send[l].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
for (l=0; l < k; l++) {
free(answer->ipaddrs[l]);
}
free(answer->ipaddrs);
free(answer->addrs);
free(answer);
et_freeAnswers(answer_first);
return ET_ERROR_NOMEM;
}
answer->ipaddrs[k][length-1] = '\0';
pbuf += length;
}
if (debug) {
printf("et_findserver: RESPONSE from %s at %d with cast addr = %s and uname = %s\n",
answer->ipaddrs[0], answer->port, answer->castIP, answer->uname);
}
numresponses++;
} while(0);
/* Now that we have a real answer, make that a part of the list */
answer_prev = answer;
/* see if there's another packet to be read on this socket */
if (bytes > 0) {
goto anotherpacket;
}
} /* for each socket (j) */
/* If host is local or we know its name. There may be many responses.
* Look only at the response that matches the host's address.
*/
if ((strcmp(config->host, ET_HOST_REMOTE) != 0) &&
(strcmp(config->host, ET_HOST_ANYWHERE) != 0)) {
/* analyze the answers/responses */
answer = answer_first;
while (answer != NULL) {
/* The problem here is another ET system of the
* same name may respond, but we're interested
* only in the one on the specified host.
*/
for (n=0; n < answer->addrCount; n++) {
for (m=0; m<ipAddrCount; m++) {
if (debug) printf("et_findserver: comparing %s to incoming %s\n", ipAddrs[m], answer->ipaddrs[n]);
if (strcmp(ipAddrs[m], answer->ipaddrs[n]) == 0) {
gotMatch = 1;
break;
}
}
if (gotMatch) break;
}
if (gotMatch) {
if (debug) printf("et_findserver: got a match to local or specific: %s\n", answer->ipaddrs[n]);
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
strcpy(ethost, answer->ipaddrs[n]);
*port = answer->port;
*inetaddr = answer->addrs[n];
et_freeAnswers(answer_first);
return ET_OK;
}
answer = answer->next;
}
}
/* If the host may be anywhere (local or remote) ... */
else if (strcmp(config->host, ET_HOST_ANYWHERE) == 0) {
/* if our policy is to return an error for more than 1 response ... */
if ((config->policy == ET_POLICY_ERROR) && (numresponses > 1)) {
/* print error message and return */
if (debug) printf("et_findserver: %d responses to broad/multicast -\n", numresponses);
j = 1;
answer = answer_first;
while (answer != NULL) {
fprintf(stderr, " RESPONSE %d from %s at %d\n",
j++, answer->ipaddrs[0], answer->port);
answer = answer->next;
}
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
et_freeAnswers(answer_first);
return ET_ERROR_TOOMANY;
}
/* analyze the answers/responses */
j = 0;
answer = answer_first;
while (answer != NULL) {
/* If our policy is to take the first response do so. If our
* policy is ET_POLICY_ERROR, we can also return the first as
* we can only be here if there's been just 1 response.
*/
if ((config->policy == ET_POLICY_FIRST) ||
(config->policy == ET_POLICY_ERROR)) {
if (debug) printf("et_findserver: got a match to .anywhere (%s), first or error policy\n", answer->ipaddrs[0]);
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
strcpy(ethost, answer->ipaddrs[0]);
*port = answer->port;
*inetaddr = answer->addrs[0];
if (allETinfo != NULL) {
/* remove answer from linked list & return the first of new list */
answer_first = removeResponseFromList(answer_first, answer);
*allETinfo = answer;
}
/* free the rest of the linked list */
et_freeAnswers(answer_first);
return ET_OK;
}
/* else if our policy is to take the first local response ... */
else if (config->policy == ET_POLICY_LOCAL) {
if (strcmp(localuname, answer->uname) == 0) {
if (debug) printf("et_findserver: got a uname match to .anywhere, local policy\n");
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
strcpy(ethost, answer->ipaddrs[0]);
*port = answer->port;
*inetaddr = answer->addrs[0];
if (allETinfo != NULL) {
answer_first = removeResponseFromList(answer_first, answer);
*allETinfo = answer;
}
et_freeAnswers(answer_first);
return ET_OK;
}
/* If we went through all responses without finding
* a local one, pick the first one we received.
*/
if (++j == numresponses-1) {
if (debug) printf("et_findserver: got a match to .anywhere, nothing local available\n");
answer = answer_first;
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
strcpy(ethost, answer->ipaddrs[0]);
*port = answer->port;
*inetaddr = answer->addrs[0];
if (allETinfo != NULL) {
answer_first = removeResponseFromList(answer_first, answer);
*allETinfo = answer;
}
et_freeAnswers(answer_first);
return ET_OK;
}
}
answer = answer->next;
}
}
/* if user did not specify host name, and it must be remote ... */
else if (strcmp(config->host, ET_HOST_REMOTE) == 0) {
/* analyze the answers/responses */
answer = answer_first;
while (answer != NULL) {
/* The problem here is if we are broadcasting, a local ET
* system of the same name may respond, but we're interested
* only in the remote systems. Weed out the local system.
*/
if (strcmp(localuname, answer->uname) == 0) {
answer = answer->next;
continue;
}
remoteresponses++;
if (first_remote == NULL) {
first_remote = answer;
}
/* The ET_POLICY_LOCAL doesn't make any sense here so
* default to ET_POLICY_FIRST
*/
if ((config->policy == ET_POLICY_FIRST) ||
(config->policy == ET_POLICY_LOCAL)) {
if (debug) printf("et_findserver: got a match to .remote, first or local policy\n");
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
strcpy(ethost, answer->ipaddrs[0]);
*port = answer->port;
*inetaddr = answer->addrs[0];
if (allETinfo != NULL) {
answer_first = removeResponseFromList(answer_first, answer);
*allETinfo = answer;
}
et_freeAnswers(answer_first);
return ET_OK;
}
answer = answer->next;
}
/* If we're here, we do NOT have a first/local policy with at least
* 1 remote response.
*/
if (config->policy == ET_POLICY_ERROR) {
if (remoteresponses == 1) {
if (debug) printf("et_findserver: got a match to .remote, error policy\n");
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
strcpy(ethost, first_remote->ipaddrs[0]);
*port = first_remote->port;
*inetaddr = first_remote->addrs[0];
if (allETinfo != NULL) {
answer_first = removeResponseFromList(answer_first, first_remote);
*allETinfo = first_remote;
}
et_freeAnswers(answer_first);
return ET_OK;
}
else if (remoteresponses > 1) {
/* too many proper responses, return error */
fprintf(stderr, "et_findserver: %d responses to broad/multicast -\n",
numresponses);
j = 0;
answer = answer_first;
while (answer != NULL) {
if (strcmp(localuname, answer->uname) == 0) {
answer = answer->next;
continue;
}
fprintf(stderr, " RESPONSE %d from %s at %d\n",
j++, answer->ipaddrs[0], answer->port);
answer = answer->next;
}
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
et_freeAnswers(answer_first);
return ET_ERROR_TOOMANY;
}
}
} /* else if host is remote */
} /* else if we have some response(s) ... */
} /* try another broadcast */
/* if we're here, we got no response from any matching ET system */
if (debug) printf("et_findserver: no valid response received\n");
for (k=0; k<numsockets; k++) {
close(send[k].sockfd);
}
free(send);
freeIpAddrs(ipAddrs, ipAddrCount);
et_freeAnswers(answer_first);
return ET_ERROR_TIMEOUT;
}
void
fssrGainScan(int id, char *filename, \
int beg_chip, int end_chip, \
int beg_chan, int end_chan,
int start_thr, int chan_mult)
{
FILE *fd;
char fname[256], reg27[20];
int thr, ichip, ich;
int i, j;
int zero_count, amp, min_hits;
int base_thr = start_thr;
int temp_start_thr, zero_status, a;
uint32_t pulser_rate = vscmGetPulserRate(id);
uint32_t scaler[128];
/* Pulser Settings */
const int start_amp = 75;
const int end_amp = 125;
const int step_amp = 25;
int steps = ((end_amp - start_amp) / step_amp) + 1;
/* Get current time for file saving */
time_t now;
time(&now);
threshold_scan *data;
#ifdef VXWORKS
int ticks = 1;
#else
struct timespec ts;
ts.tv_sec = 0;
ts.tv_nsec = 16666667; /* ~16.6 ms (similar to 1 tick on VxWorks) */
#endif
if (vscmIsNotInit(&id, __func__))
return;
/*
if ((end_chip - beg_chip) > 3) {
logMsg("ERROR: %s: Chip range must be 4 chips or less\n", __func__);
logMsg("ERROR: %s: Output files will be overwritten if >4\n", __func__);
return;
}
*/
if ((chan_mult < 1) || ((chan_mult & (chan_mult - 1)) != 0)) {
logMsg("ERROR: %s: Channel multiplier (%d) ", __func__, chan_mult);
logMsg("must be a power of 2\n");
return;
}
data = malloc(steps * chan_mult * sizeof(threshold_scan));
/* Temporarily disable both pulser outputs */
vscmPulser(id, 0, 0, 1);
#ifdef VXWORKS
min_hits = (int)(pulser_rate * (ticks / (float)sysClkRateGet()) * 0.99);
#else
min_hits = (int)(pulser_rate * (float)(ts.tv_nsec * 1.0e-9) * 0.99);
#endif
logMsg("INFO: %s: Min Hits = %d\n", __func__, min_hits);
for (ichip = beg_chip; ichip <= end_chip; ichip++) {
#ifdef DEBUG
fssrStatus(id, ichip);
#endif
if(filename)
{
logMsg("%s\n", filename);
logMsg("cd %d\n", chdir(filename));
logMsg("%s\n", getcwd());
char datetime[] = "YYYYMMDD_HHMM";
strftime(datetime, sizeof(datetime), "%Y%m%d_%H%M", localtime(&now));
struct stat sb;
if (stat(datetime, &sb) != 0)
mkdir(datetime, S_IRWXU | S_IRWXG | S_IROTH);
char host[255];
gethostname(host, 255);
sprintf(fname,"%s/%s_s%02d_c%1d_u%1d", datetime, host, id, ((ichip > 3) ? 2 : 1), ((ichip % 4) + 1));
if( (fd = fopen(fname, "w")) <= NULL )
{
logMsg("ERROR: %s: Opening file: %s\n", __func__, fname);
free(data);
return;
}
else
{
logMsg("INFO: %s: Output file: %s\n", __func__, fname);
}
}
else
{
logMsg("Chip %d\n", ichip);
}
fssrParseControl(id, ichip, reg27);
if (filename) {
fprintf(fd, "%s\n", reg27);
}
else
printf("%s\n", reg27);
/* Set all discriminators - start of loop*/
for (i = 0; i < 8; i++) {
fssrSetThreshold(id, ichip, i, base_thr + (i * 30));
}
for (i = 0; i < (128 / chan_mult); i++)
{
start_thr = base_thr;
thr = start_thr;
temp_start_thr = 1;
fssrKillMaskDisableAll(id, ichip);
fssrInjectMaskDisableAll(id, ichip);
/* printf("[%d] Chan: ",id);*/
for (j = 0; j < chan_mult; j++)
{
ich = i + (j * (128 / chan_mult));
/* printf("%d ", ich);*/
fssrKillMaskEnableSingle(id, ichip, ich);
fssrInjectMaskEnableSingle(id, ichip, ich);
}
/* printf("\n"); fflush(stdout);*/
for (a = 0; a < steps; a++)
{
amp = start_amp + (a * step_amp);
thr = start_thr;
zero_count = 0;
/* printf("[%d] Amp: %d Start: %d\n",id, amp, thr); fflush(stdout);*/
vscmPulser(id, ((ichip > 3) ? 2 : 1), amp, (int)(min_hits / 0.99));
while(thr < 256 && zero_count < 2)
{
zero_status = 0;
temp_start_thr = 0;
fssrSetThreshold(id, ichip, 0, thr);
vscmDisableChipScaler(id, ichip);
vscmClearStripScalers(id, ichip);
vscmEnableChipScaler(id, ichip);
vscmDisableChipScaler(id, ichip);
vscmEnableChipScaler(id, ichip);
vscmPulserStart(id);
/* Gather data by sleeping */
#ifdef VXWORKS
taskDelay(ticks);
#else
nanosleep(&ts, NULL);
#endif
vscmDisableChipScaler(id, ichip);
vscmReadStripScalers(id, ichip, scaler);
for (j = 0; j < chan_mult; j++)
{
ich = i + (j * (128 / chan_mult));
if (thr == start_thr)
{
data[(a * chan_mult) + j].amp = amp;
data[(a * chan_mult) + j].start = start_thr;
}
data[(a * chan_mult) + j].hits[thr] = scaler[ich];
if (scaler[ich] > min_hits)
temp_start_thr++;
if (scaler[ich] == 0)
zero_status++;
}
if (zero_status == chan_mult)
zero_count++;
else
zero_count = 0;
if (temp_start_thr == chan_mult)
start_thr = thr;
thr++;
}
}
/* Save the data struct to file */
if (filename)
{
for (j = 0; j < chan_mult; j++)
{
ich = i + (j * (128 / chan_mult));
fprintf(fd, "%d\n", ich);
for (a = 0; a < steps; a++)
{
fprintf(fd, "%d, ", data[(a * chan_mult) + j].amp);
fprintf(fd, "%d ", data[(a * chan_mult) + j].start);
zero_count = 0;
thr = data[(a *chan_mult) + j].start;
while(thr < 256 && zero_count < 2)
{
fprintf(fd, ",%d ", data[(a * chan_mult) + j].hits[thr]);
if (data[(a * chan_mult) + j].hits[thr] == 0)
zero_count++;
else
zero_count = 0;
thr++;
}
fprintf(fd, "\n");
}
fprintf(fd, "\n");
}
fflush(fd);
}
} /* End of Channel loop */
if (filename)
fclose(fd);
else
printf("\n");
/* Set all discriminators - end of loop*/
for (i = 0; i < 8; i++) {
fssrSetThreshold(id, ichip, i, base_thr + (i * 30));
}
} /* End of Chip loop */
free(data);
}
/*************************************************************
* fadcFirmwareGLoad
* - load up firmware for all initialized modules
*
* NOTE: Make call to
* fadcFirmwareSetFilename(...);
* if not using firmware from the default
*/
int
fadcFirmwareGLoad(int chip, int pFlag)
{
int ifadc=0, id=0, step=0;
unsigned int passed[FA_MAX_BOARDS+1], stepfail[FA_MAX_BOARDS+1];
if(chip<0 || chip>2)
{
printf("%s: ERROR: Invalid chip parameter %d\n",
__FUNCTION__,chip);
return ERROR;
}
/* Perform a hardware and software reset */
step=0;
FALOCK;
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
if((id<=0) || (id>21) || (FAp[id] == NULL))
{
printf("%s: ERROR : ADC in slot %d is not initialized \n",
__FUNCTION__,id);
passed[id] = 0;
stepfail[id] = step;
}
else
{
passed[id] = 1;
vmeWrite32(&FAp[id]->reset, 0xFFFF);
}
}
FAUNLOCK;
taskDelay(60);
/* Check if FADC is Ready */
step=1;
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
if(fadcFirmwareTestReady(id, 60, pFlag) != OK)
{
printf("%s: ERROR: FADC %2d not ready after reset\n",
__FUNCTION__,id);
passed[id] = 0;
stepfail[id] = step;
}
}
/* Data to SRAM */
step=2;
printf("%s: Loading SRAM with data \n",__FUNCTION__);
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
printf("%2d: ",id);
fflush(stdout);
if(passed[id]) /* Skip the ones that have previously failed */
{
fadcFirmwareDownloadConfigData(id);
if(fadcFirmwareVerifyDownload(id) != OK)
{
printf("%s: ERROR: FADC %2d Failed data verification at SRAM\n",
__FUNCTION__,id);
passed[id] = 0;
stepfail[id] = step;
}
else
printf(" Done\n");
}
}
/* SRAM TO PROM */
step=3;
taskDelay(1);
printf("%s: Loading PROM with SRAM data \n",__FUNCTION__);
FALOCK;
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
if(passed[id]) /* Skip the ones that have previously failed */
{
if(chip==FADC_FIRMWARE_LX110)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_SRAM_TO_PROM1);
else if(chip==FADC_FIRMWARE_FX70T)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_SRAM_TO_PROM2);
}
}
FAUNLOCK;
taskDelay(1);
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
if(passed[id]) /* Skip the ones that have previously failed */
{
if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */
{
printf("%s: ERROR: FADC %2d ready timeout SRAM -> PROM\n",
__FUNCTION__,id);
passed[id] = 0;
stepfail[id] = step;
}
}
}
/* PROM TO SRAM (For verification) */
printf("%s: Loading SRAM with PROM data \n",__FUNCTION__);
step=4;
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
if(passed[id]) /* Skip the ones that have previously failed */
{
fadcFirmwareZeroSRAM(id);
FALOCK;
if(chip==FADC_FIRMWARE_LX110)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_PROM1_TO_SRAM);
else if(chip==FADC_FIRMWARE_FX70T)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_PROM2_TO_SRAM);
FAUNLOCK;
}
}
taskDelay(1);
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
if(passed[id]) /* Skip the ones that have previously failed */
{
if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */
{
printf("%s: ERROR: FADC %2d ready timeout PROM -> SRAM\n",
__FUNCTION__,id);
passed[id] = 0;
stepfail[id] = step;
}
}
}
/* Compare SRAM to Data Array */
printf("%s: Verifying data \n",__FUNCTION__);
step=5;
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
printf("%2d: ",id);
fflush(stdout);
if(passed[id]) /* Skip the ones that have previously failed */
{
if(fadcFirmwareVerifyDownload(id) != OK)
{
printf("%s: ERROR: FADC %d PROM data not verified\n",
__FUNCTION__,id);
passed[id] = 0;
stepfail[id] = step;
}
else
printf(" Done\n");
}
}
/* PROM to FPGA (Reboot FPGA) */
printf("%s: Rebooting FPGA \n",__FUNCTION__);
step=6;
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
if(passed[id]) /* Skip the ones that have previously failed */
{
FALOCK;
if(chip==FADC_FIRMWARE_LX110)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_REBOOT_FPGA1);
else if(chip==FADC_FIRMWARE_FX70T)
vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_REBOOT_FPGA2);
FAUNLOCK;
}
}
taskDelay(1);
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
if(passed[id]) /* Skip the ones that have previously failed */
{
if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */
{
printf("%s: ERROR: FADC %2d ready timeout PROM -> FPGA\n",
__FUNCTION__,id);
passed[id] = 0;
stepfail[id] = step;
}
}
}
for(ifadc=0 ; ifadc<nfadc; ifadc++)
{
id = fadcID[ifadc];
if(passed[id]) /* Skip the ones that have previously failed */
{
printf("%s: Done programming FADC %2d\n",
__FUNCTION__,id);
}
else
{
printf("%s: FAILED programming FADC %2d at step %d\n",
__FUNCTION__,id,stepfail[id]);
}
}
return OK;
}
