STATUS wdDestroy(
WDOG_ID wdId,
BOOL dealloc
)
{
STATUS status;
int level;
/* Not callable from interrupts */
if (INT_RESTRICT() != OK)
{
status = ERROR;
}
else
{
/* Lock interrupts */
INT_LOCK(level);
/* Verify object */
if (OBJ_VERIFY(wdId, wdClassId) != OK )
{
INT_UNLOCK(level);
status = ERROR;
}
else
{
/* Terminate object */
objCoreTerminate(&wdId->objCore);
/* Enter kernel */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Cancel watchdog timer */
vmxWdCancel(wdId);
wdId->status = WDOG_DEAD;
taskSafe();
/* Exit kernel */
vmxExit();
/* Deallocate if requested */
if (dealloc == TRUE)
{
objFree(wdClassId, wdId);
}
taskUnsafe();
status = OK;
}
}
return status;
}
STATUS semDestroy(
SEM_ID semId,
BOOL deallocate
)
{
STATUS status;
int level;
if (INT_RESTRICT() != OK)
{
errnoSet(S_intLib_NOT_ISR_CALLABLE);
status = ERROR;
}
else
{
INT_LOCK(level);
if (OBJ_VERIFY(semId, semClassId) != OK)
{
INT_UNLOCK(level);
status = ERROR;
}
else
{
objCoreTerminate(&semId->objCore);
/* Delete it */
kernelState = TRUE;
INT_UNLOCK(level);
vmxSemDelete(semId);
taskSafe();
vmxExit();
if (deallocate == TRUE)
{
objFree(semClassId, semId);
}
taskUnsafe();
status = OK;
}
}
return status;
}
STATUS taskDestroy(
int taskId,
BOOL freeStack,
unsigned timeout,
BOOL forceDestroy
)
{
STATUS status;
int i, level;
TCB_ID tcbId;
if (INT_RESTRICT() != OK)
{
errnoSet (S_intLib_NOT_ISR_CALLABLE);
return ERROR;
}
/* Get task context */
tcbId = taskTcb(taskId);
if (tcbId == NULL)
return ERROR;
/* If task self destruct and excption lib installed */
if (tcbId == taskIdCurrent) {
/* Wait for safe to destroy */
while (tcbId->safeCount > 0)
taskUnsafe();
/* Kill it */
status = excJobAdd(
(VOIDFUNCPTR) taskDestroy,
(ARG) tcbId,
(ARG) freeStack,
(ARG) WAIT_NONE,
(ARG) FALSE,
(ARG) 0,
(ARG) 0
);
/* Block here and suspend */
while(status == OK)
taskSuspend(0);
} /* End if task self destruct and exception lib installed */
taskDestroyLoop:
/* Lock interrupts */
INT_LOCK(level);
/* Check id */
if (TASK_ID_VERIFY(tcbId) != OK)
{
/* errno set by taskIdVerify() */
/* Unlock interrupts */
INT_UNLOCK(level);
return ERROR;
}
/* Mask all signals */
if (tcbId->pSignalInfo != NULL)
tcbId->pSignalInfo->sigt_blocked = 0xffffffff;
/* Block here for safe and running locked tasks */
while ( (tcbId->safeCount > 0) ||
( (tcbId->status == TASK_READY) && (tcbId->lockCount > 0) )
)
{
/* Enter kernel mode */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Check if force deletion, or suicide */
if (forceDestroy || (tcbId == taskIdCurrent))
{
/* Remove protections */
tcbId->safeCount = 0;
tcbId->lockCount = 0;
/* Check if flush of safety queue is needed */
if (Q_FIRST(&tcbId->safetyQ) != NULL)
vmxPendQFlush(&tcbId->safetyQ);
/* Exit trough kernel */
vmxExit();
}
else
{
/* Not forced deletion or suicide */
/* Put task on safe queue */
if (vmxPendQPut(&tcbId->safetyQ, timeout) != OK)
{
/* Exit trough kernel */
vmxExit();
errnoSet (S_taskLib_INVALID_TIMEOUT);
return ERROR;
}
/* Exit trough kernel */
status = vmxExit();
/* Check for restart */
if (status == SIG_RESTART)
{
timeout = (sigTimeoutRecalc)(timeout);
goto taskDestroyLoop;
}
/* Check if unsuccessful */
if (status == ERROR)
{
/* timer should have set errno to S_objLib_TIMEOUT */
return ERROR;
}
} /* End else forced or suicide */
/* Lock interrupts */
INT_LOCK(level);
/* Now verify class id again */
if (TASK_ID_VERIFY(tcbId) != OK)
{
/* errno set by taskIdVerify() */
/* Unlock interrupts */
INT_UNLOCK(level);
return ERROR;
}
} /* End while blocked by safety */
/* Now only one cadidate is selected for deletion */
/* Make myself safe */
taskSafe();
/* Protet deletion cadidate */
tcbId->safeCount++;
/* Check if not suicide */
if (tcbId != taskIdCurrent)
{
/* Enter kernel mode */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Suspend victim */
vmxSuspend(tcbId);
/* Exit trough kernel */
vmxExit();
}
else
{
/* Unlock interrupts */
INT_UNLOCK(level);
}
/* Run deletion hooks */
for (i = 0; i < MAX_TASK_DELETE_HOOKS; i++)
if (taskDeleteHooks[i] != NULL)
(*taskDeleteHooks[i])(tcbId);
/* Lock task */
taskLock();
/* If dealloc and options dealloc stack */
if ( freeStack && (tcbId->options & TASK_OPTIONS_DEALLOC_STACK) ) {
#if (_STACK_DIR == _STACK_GROWS_DOWN)
objFree(taskClassId, tcbId->pStackEnd);
#else /* _STACK_GROWS_UP */
objFree(taskClassId, tbcId - TASK_EXTRA_BYTES);
#endif /* _STACK_DIR */
}
/* Lock interrupts */
INT_LOCK(level);
/* Invalidate id */
objCoreTerminate(&tcbId->objCore);
/* Enter kernel mode */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Delete task */
status = vmxDelete(tcbId);
/* Check if safe quque needs to be flushed */
if (Q_FIRST(&tcbId->safetyQ) != NULL)
vmxPendQFlush(&tcbId->safetyQ);
/* Exit trough kernel */
vmxExit();
/* Unprotect */
taskUnlock();
taskUnsafe();
return OK;
}
STATUS taskRestart(
int taskId
)
{
TCB_ID tcbId;
char *name, *rename;
int len;
unsigned priority;
int options;
char *pStackBase;
unsigned stackSize;
FUNCPTR entry;
ARG args[MAX_TASK_ARGS];
STATUS status;
if (INT_RESTRICT() != OK)
{
errnoSet(S_intLib_NOT_ISR_CALLABLE);
return ERROR;
}
/* If self restart */
if ( (taskId == 0) || (taskId == (int) taskIdCurrent) )
{
/* Task must be unsafe */
while (taskIdCurrent->safeCount > 0)
taskSafe();
/* Spawn a task that will restart this task */
taskSpawn(restartTaskName, restartTaskPriority, restartTaskOptions,
restartTaskStackSize, taskRestart, (ARG) taskIdCurrent,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0);
/* Wait for restart */
while (1)
taskSuspend(0);
} /* End if self restart */
/* Get task context */
tcbId = taskTcb(taskId);
if (tcbId == NULL)
return ERROR;
/* TASK_ID_VERIFY() already done by taskTcb() */
/* Copy task data */
priority = tcbId->priority;
options = tcbId->options;
entry = tcbId->entry;
pStackBase = tcbId->pStackBase;
stackSize = (tcbId->pStackEnd - tcbId->pStackBase) * _STACK_DIR;
taskArgGet(tcbId, pStackBase, args);
/* Copy name if needed */
name = tcbId->name;
rename = NULL;
if (name != NULL) {
len = strlen(name) + 1;
rename = malloc(len);
if (rename != NULL)
strcpy(rename, name);
name = rename;
}
/* Prevent deletion */
taskSafe();
if (taskTerminate((int) tcbId) != OK)
{
taskUnsafe();
/* errno set by taskTerminate() */
return ERROR;
}
/* Initialize task with same data */
status = taskInit(tcbId, name, priority, options, pStackBase,
stackSize, entry, args[0], args[1], args[2],
args[3], args[4], args[5], args[6], args[7],
args[8], args[9]);
if (status != OK)
{
/* errno set by taskInit() */
return ERROR;
}
/* And start it */
status = taskActivate((int) tcbId);
if (status != OK)
{
/* errno set by taskActivate() */
return ERROR;
}
/* Make me mortal */
taskUnsafe();
/* Free rename buffer if needed */
if (rename != NULL)
free(rename);
return OK;
}
/**
* @brief Initialize the socket and serve images to the PC.
* This is the task that serves images to the PC in a loop. This runs
* as a separate task.
*/
int PCVideoServer::ServerTask()
{
/* Setup to PC sockets */
struct sockaddr_in serverAddr;
int sockAddrSize = sizeof(serverAddr);
int pcSock = ERROR;
bzero ((char *) &serverAddr, sockAddrSize);
serverAddr.sin_len = (u_char) sockAddrSize;
serverAddr.sin_family = AF_INET;
serverAddr.sin_port = htons (VIDEO_TO_PC_PORT);
serverAddr.sin_addr.s_addr = htonl (INADDR_ANY);
int success;
while (true)
{
taskSafe();
// Create the socket.
if ((pcSock = socket (AF_INET, SOCK_STREAM, 0)) == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to create the PCVideoServer socket");
continue;
}
// Set the TCP socket so that it can be reused if it is in the wait state.
int reuseAddr = 1;
setsockopt(pcSock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast<char*>(&reuseAddr), sizeof(reuseAddr));
// Bind socket to local address.
if (bind (pcSock, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to bind the PCVideoServer port");
close (pcSock);
continue;
}
// Create queue for client connection requests.
if (listen (pcSock, 1) == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to listen on the PCVideoServer port");
close (pcSock);
continue;
}
struct sockaddr_in clientAddr;
int clientAddrSize;
int newPCSock = accept (pcSock, reinterpret_cast<sockaddr*>(&clientAddr), &clientAddrSize);
if (newPCSock == ERROR)
{
close(pcSock);
continue;
}
//TODO: check camera error
int numBytes = 0;
int imageDataSize = 0;
char* imageData = NULL;
while(!m_stopServer)
{
success = semTake(m_newImageSem, 1000);
if (success == ERROR)
{
// If the semTake timed out, there are no new images from the camera.
continue;
}
success = AxisCamera::GetInstance().CopyJPEG(&imageData, numBytes, imageDataSize);
if (!success)
{
// No point in running too fast -
Wait(1.0);
// If camera is not initialzed you will get failure and
// the timestamp is invalid. Reset this value and try again.
continue;
}
// Write header to PC
static const char header[4]={1,0,0,0};
int headerSend = write(newPCSock, const_cast<char*>(header), 4);
// Write image length to PC
int lengthSend = write(newPCSock, reinterpret_cast<char*>(&numBytes), 4);
// Write image to PC
int sent = write (newPCSock, imageData, numBytes);
// The PC probably closed connection. Get out of here
// and try listening again.
if (headerSend == ERROR || lengthSend == ERROR || sent == ERROR)
{
break;
}
}
// Clean up
delete [] imageData;
close (newPCSock);
newPCSock = ERROR;
close (pcSock);
pcSock = ERROR;
taskUnsafe();
Wait(0.1);
}
return (OK);
}
static int __wind_task_safe(struct task_struct *curr, struct pt_regs *regs)
{
taskSafe();
return 0;
}
/* Function: UploadServerWork =================================================
* Abstract:
* Upload model signals to host for a single upInfo.
*/
void UploadServerWork(int32_T upInfoIdx, int_T numSampTimes)
{
int_T i;
ExtBufMemList upList;
boolean_T error = EXT_NO_ERROR;
#ifdef VXWORKS
/*
* Don't spin the CPU unless we've got data to upload.
* The upload.c/UploadBufAddTimePoint function gives the sem
* each time that data is added.
*/
taskUnsafe();
semTake(uploadSem, WAIT_FOREVER);
taskSafe();
#endif
if (!connected) goto EXIT_POINT;
UploadBufGetData(&upList, upInfoIdx, numSampTimes);
while(upList.nActiveBufs > 0) {
for (i=0; i<upList.nActiveBufs; i++) {
const BufMem *bufMem = &upList.bufs[i];
/*
* We call SendPktDataToHost() instead of SendPktToHost() because
* the packet header is combined with packet payload. We do this
* to avoid the overhead of making two calls for each upload
* packet - one for the head and one for the payload.
*/
error = SendPktDataToHost(
bufMem->section1,
bufMem->nBytes1);
if (error != EXT_NO_ERROR) {
#ifndef EXTMODE_DISABLEPRINTF
fprintf(stderr,"SendPktDataToHost() failed on data upload.\n");
#endif
goto EXIT_POINT;
}
if (bufMem->nBytes2 > 0) {
error = SendPktDataToHost(
bufMem->section2,
bufMem->nBytes2);
if (error != EXT_NO_ERROR) {
#ifndef EXTMODE_DISABLEPRINTF
fprintf(stderr,"SendPktDataToHost() failed on data upload.\n");
#endif
goto EXIT_POINT;
}
}
/* confirm that the data was sent */
UploadBufDataSent(upList.tids[i], upInfoIdx);
}
UploadBufGetData(&upList, upInfoIdx, numSampTimes);
}
EXIT_POINT:
if (error != EXT_NO_ERROR) {
/* An error in this function is caused by a physical failure in the
* external mode connection. We assume this failure caused the host
* to disconnect. The target must be disconnected and returned to a
* state where it is running and can be re-connected to by the host.
*/
ForceDisconnectFromHost(numSampTimes);
}
}
/* Function: rt_PktServerWork ==================================================
* Abstract:
* If not connected, establish communication of the packet line and the
* data upload line. If connected, send/receive packets and parameters
* on the packet line.
*/
PUBLIC void rt_PktServerWork(RTWExtModeInfo *ei,
int_T numSampTimes,
boolean_T *stopReq)
{
PktHeader pktHdr;
boolean_T hdrAvail;
boolean_T error = EXT_NO_ERROR;
boolean_T disconnectOnError = false;
/*
* If not connected, attempt to make connection to host.
*/
if (!connected) {
rtExtModeTestingKillIfOrphaned(false);
error = ExtOpenConnection(extUD,&connected);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
}
/*
* If ExtOpenConnection is not blocking and there are no pending
* requests to open a connection, we'll still be unconnected.
*/
if (!connected) goto EXIT_POINT; /* nothing to do */
/*
* Process packets.
*/
/* Wait for a packet. */
error = GetPktHdr(&pktHdr, &hdrAvail);
if (error != EXT_NO_ERROR) {
#ifndef EXTMODE_DISABLEPRINTF
fprintf(stderr, "\nError occurred getting packet header.\n");
#endif
disconnectOnError = true;
goto EXIT_POINT;
}
rtExtModeTestingKillIfOrphaned(hdrAvail);
if (!hdrAvail) goto EXIT_POINT; /* nothing to do */
/*
* This is the first packet. Should contain the string:
* 'ext-mode'. Its contents are not important to us.
* It is used as a flag to start the handshaking process.
*/
if (!commInitialized) {
pktHdr.type = EXT_CONNECT;
}
/*
* At this point we know that we have a packet: process it.
*/
#ifdef VXWORKS
taskSafe();
#endif
switch(pktHdr.type) {
case EXT_GET_TIME:
{
/* Skip verbosity print out - we get too many of these */
/*PRINT_VERBOSE(("got EXT_GET_TIME packet.\n"));*/
time_T t = rteiGetT(ei);
error = SendPktToHost(
EXT_GET_TIME_RESPONSE,sizeof(time_T),
(char_T *)&t);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
}
case EXT_ARM_TRIGGER:
{
#ifndef EXTMODE_DISABLESIGNALMONITORING
error = ProcessCancelLoggingArmTriggerPkt(EXT_ARM_TRIGGER_RESPONSE, pktHdr.size, numSampTimes);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#else
error = AcknowledgeSignalActionPkt(pktHdr.size, EXT_ARM_TRIGGER_RESPONSE);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
break;
}
case EXT_SELECT_SIGNALS:
{
#ifndef EXTMODE_DISABLESIGNALMONITORING
error = ProcessSelectTriggerSignalPkt(EXT_SELECT_SIGNALS_RESPONSE, ei, pktHdr.size, numSampTimes, ERRMSG_PROCESSSELECTSIGNAL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#else
error = AcknowledgeSignalActionPkt(pktHdr.size, EXT_SELECT_SIGNALS_RESPONSE);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
break;
}
case EXT_SELECT_TRIGGER:
{
#ifndef EXTMODE_DISABLESIGNALMONITORING
error = ProcessSelectTriggerSignalPkt(EXT_SELECT_TRIGGER_RESPONSE, ei, pktHdr.size, -1, ERRMSG_PROCESSSELECTTRIGGER);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#else
error = AcknowledgeSignalActionPkt(pktHdr.size, EXT_SELECT_TRIGGER_RESPONSE);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
break;
}
case EXT_CONNECT:
{
PRINT_VERBOSE(("got EXT_CONNECT packet.\n"));
error = ProcessConnectPkt(ei);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
}
case EXT_SETPARAM:
{
#ifndef EXTMODE_DISABLEPARAMETERTUNING
PRINT_VERBOSE(("got EXT_SETPARAM packet.\n"));
error = ProcessSetParamPkt(ei, pktHdr.size);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#else
PRINT_VERBOSE(("discard EXT_SETPARAM packet.\n"));
error = AcknowledgeSetParamPkt(pktHdr.size);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
break;
}
case EXT_GETPARAMS:
{
#ifndef EXTMODE_DISABLEPARAMETERTUNING
PRINT_VERBOSE(("got EXT_GETPARAMS packet.\n"));
error = ProcessGetParamsPkt(ei);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
break;
}
case EXT_DISCONNECT_REQUEST:
{
PRINT_VERBOSE(("got EXT_DISCONNECT_REQUEST packet.\n"));
/*
* Note that from the target's point of view this is
* more a "notify" than a "request". The host needs to
* have this acknowledged before it can begin closing
* the connection.
*/
error = SendPktToHost(EXT_DISCONNECT_REQUEST_RESPONSE, 0, NULL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
DisconnectFromHost(numSampTimes);
break;
}
case EXT_DISCONNECT_REQUEST_NO_FINAL_UPLOAD:
{
PRINT_VERBOSE(("got EXT_DISCONNECT_REQUEST_NO_FINAL_UPLOAD packet.\n"));
/*
* The target receives this packet when the host is
* immediately terminating the extmode communication due
* to some error. The target should not send back a
* response or a final upload of data because the host is
* expecting neither. The target must be disconnected and
* returned to a state where it is running and can be
* re-connected to by the host.
*/
ForceDisconnectFromHost(numSampTimes);
break;
}
case EXT_MODEL_START:
PRINT_VERBOSE(("got EXT_MODEL_START packet.\n"));
#ifdef VXWORKS
{
extern SEM_ID startStopSem;
semGive(startStopSem);
}
#endif
startModel = true;
error = SendPktToHost(EXT_MODEL_START_RESPONSE, 0, NULL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
case EXT_MODEL_STOP:
PRINT_VERBOSE(("got EXT_MODEL_STOP packet.\n"));
*stopReq = true;
break;
#ifndef EXTMODE_DISABLETESTING
case EXT_MODEL_PAUSE:
PRINT_VERBOSE(("got EXT_MODEL_PAUSE packet.\n"));
modelStatus = TARGET_STATUS_PAUSED;
startModel = false;
error = SendPktToHost(EXT_MODEL_PAUSE_RESPONSE, 0, NULL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
case EXT_MODEL_STEP:
PRINT_VERBOSE(("got EXT_MODEL_STEP packet.\n"));
if ((modelStatus == TARGET_STATUS_PAUSED) && !startModel) {
startModel = true;
}
error = SendPktToHost(EXT_MODEL_STEP_RESPONSE, 0, NULL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
case EXT_MODEL_CONTINUE:
PRINT_VERBOSE(("got EXT_MODEL_CONTINUE packet.\n"));
if (modelStatus == TARGET_STATUS_PAUSED) {
modelStatus = TARGET_STATUS_RUNNING;
startModel = false;
}
error = SendPktToHost(EXT_MODEL_CONTINUE_RESPONSE, 0, NULL);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
break;
case EXT_CANCEL_LOGGING:
{
#ifndef EXTMODE_DISABLESIGNALMONITORING
error = ProcessCancelLoggingArmTriggerPkt(EXT_CANCEL_LOGGING_RESPONSE, pktHdr.size, numSampTimes);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#else
error = AcknowledgeSignalActionPkt(pktHdr.size, EXT_CANCEL_LOGGING_RESPONSE);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
break;
}
default:
#ifndef EXTMODE_DISABLEPRINTF
fprintf(stderr,"received invalid packet.\n");
#endif
break;
} /* end switch */
EXIT_POINT:
if (error != EXT_NO_ERROR) {
if (disconnectOnError) {
#ifndef EXTMODE_DISABLEPRINTF
fprintf(stderr,
"Error occurred in rt_PktServerWork.\n"
"Disconnecting from host!\n");
#endif
/* An error in this function which causes disconnectOnError to be
* set to true is caused by a physical failure in the external mode
* connection. We assume this failure caused the host to disconnect.
* The target must be disconnected and returned to a state
* where it is running and can be re-connected to by the host.
*/
ForceDisconnectFromHost(numSampTimes);
}
}
#ifdef VXWORKS
taskUnsafe();
#endif
} /* end rt_PktServerWork */
// Shamelessly stolen from PCVideoServer
int DashboardCommandServer()
{
/* Setup to PC sockets */
struct sockaddr_in serverAddr;
int sockAddrSize = sizeof(serverAddr);
int pcSock = ERROR;
bzero ((char *) &serverAddr, sockAddrSize);
serverAddr.sin_len = (u_char) sockAddrSize;
serverAddr.sin_family = AF_INET;
serverAddr.sin_port = htons (kDashboardCommandPort);
serverAddr.sin_addr.s_addr = htonl (INADDR_ANY);
while (true)
{
taskSafe();
// Create the socket.
if ((pcSock = socket (AF_INET, SOCK_STREAM, 0)) == ERROR)
{
perror ("socket");
continue;
}
// Set the TCP socket so that it can be reused if it is in the wait state.
int reuseAddr = 1;
setsockopt(pcSock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast<char*>(&reuseAddr), sizeof(reuseAddr));
// Bind socket to local address.
if (bind (pcSock, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR)
{
perror ("bind");
close (pcSock);
continue;
}
// Create queue for client connection requests.
if (listen (pcSock, 1) == ERROR)
{
perror ("listen");
close (pcSock);
continue;
}
struct sockaddr_in clientAddr;
int clientAddrSize;
int newPCSock = accept (pcSock, reinterpret_cast<sockaddr*>(&clientAddr), &clientAddrSize);
if (newPCSock == ERROR)
{
close(pcSock);
continue;
}
char cmdBuffer[32];
char *pBuffer;
while(1)
{
int numBytes = 0;
pBuffer = cmdBuffer;
while (numBytes < 2 || (*(pBuffer-2) != '\r' && *(pBuffer-1) != '\n'))
{
numBytes += read(newPCSock, pBuffer++, 1);
}
char command = cmdBuffer[0];
switch (command)
{
case 'E':
speedJag.EnableControl();
//printf("Enable\n");
break;
case 'D':
speedJag.DisableControl();
//printf("Disable\n");
break;
case 'G':
{
double P, I, D;
memcpy((char*)&P, cmdBuffer+1, sizeof(double));
memcpy((char*)&I, cmdBuffer+9, sizeof(double));
memcpy((char*)&D, cmdBuffer+17, sizeof(double));
speedJag.SetPID(P, I, D);
//printf("Set- P: %f I: %f D: %f\n", P, I, D);
//P = speedJag.GetP();
//I = speedJag.GetI();
//D = speedJag.GetD();
//printf("Get- P: %f I: %f D: %f\n", P, I, D);
}
break;
}
//no point in running too fast -
Wait(0.01);
}
// Clean up
close (newPCSock);
newPCSock = ERROR;
close (pcSock);
pcSock = ERROR;
taskUnsafe();
Wait(0.1);
}
return (OK);
}