/* This function receives data from a socket indicated to be ready
* to receive and submits the message received for processing.
* rgerhards, 2007-12-20
* Interface changed so that this function is passed the array index
* of the socket which is to be processed. This eases access to the
* growing number of properties. -- rgerhards, 2008-08-01
*/
static rsRetVal
readLog(int fd, uchar *pRcv, int iMaxLine)
{
DEFiRet;
struct strbuf data;
struct strbuf ctl;
struct log_ctl hdr;
int flags;
msg_t *pMsg;
int ret;
char errStr[1024];
data.buf = (char*)pRcv;
data.maxlen = iMaxLine;
ctl.maxlen = sizeof (struct log_ctl);
ctl.buf = (caddr_t)&hdr;
flags = 0;
ret = getmsg(fd, &ctl, &data, &flags);
if(ret < 0) {
if(errno == EINTR) {
FINALIZE;
} else {
int en = errno;
rs_strerror_r(errno, errStr, sizeof(errStr));
DBGPRINTF("imsolaris: stream input error on fd %d: %s.\n", fd, errStr);
errmsg.LogError(en, NO_ERRCODE, "imsolaris: stream input error: %s", errStr);
tryRecover();
}
} else {
DBGPRINTF("imsolaris: message from log stream %d: %s\n", fd, pRcv);
pRcv[data.len] = '\0'; /* make sure it is a valid C-String */
CHKiRet(msgConstruct(&pMsg));
MsgSetInputName(pMsg, pInputName);
MsgSetRawMsg(pMsg, (char*)pRcv, strlen((char*)pRcv));
MsgSetHOSTNAME(pMsg, glbl.GetLocalHostName(), ustrlen(glbl.GetLocalHostName()));
pMsg->iFacility = LOG_FAC(hdr.pri);
pMsg->iSeverity = LOG_PRI(hdr.pri);
pMsg->msgFlags = NEEDS_PARSING | NO_PRI_IN_RAW;
CHKiRet(submitMsg(pMsg));
}
finalize_it:
RETiRet;
}
/* once the system is fully initialized, we wait for new messages.
* We may think about replacing this with a read-loop, thus saving
* us the overhead of the poll.
* The timeout variable is the timeout to use for poll. During startup,
* it should be set to 0 (non-blocking) and later to -1 (infinit, blocking).
* This mimics the (strange) behaviour of the original syslogd.
* rgerhards, 2010-04-19
*/
static inline rsRetVal
getMsgs(thrdInfo_t *pThrd, int timeout)
{
DEFiRet;
int nfds;
int iMaxLine;
uchar *pRcv = NULL; /* receive buffer */
uchar bufRcv[4096+1];
char errStr[1024];
iMaxLine = glbl.GetMaxLine();
/* we optimize performance: if iMaxLine is below 4K (which it is in almost all
* cases, we use a fixed buffer on the stack. Only if it is higher, heap memory
* is used. We could use alloca() to achive a similar aspect, but there are so
* many issues with alloca() that I do not want to take that route.
* rgerhards, 2008-09-02
*/
if((size_t) iMaxLine < sizeof(bufRcv) - 1) {
pRcv = bufRcv;
} else {
CHKmalloc(pRcv = (uchar*) malloc(sizeof(uchar) * (iMaxLine + 1)));
}
while(pThrd->bShallStop != TRUE) {
DBGPRINTF("imsolaris: waiting for next message (timeout %d)...\n", timeout);
if(timeout == 0) {
nfds = poll(&sun_Pfd, 1, timeout); /* wait without timeout */
if(pThrd->bShallStop == TRUE) {
break;
}
if(nfds == 0) {
if(timeout == 0) {
DBGPRINTF("imsolaris: no more messages, getMsgs() terminates\n");
FINALIZE;
} else {
continue;
}
}
if(nfds < 0) {
if(errno != EINTR) {
int en = errno;
rs_strerror_r(en, errStr, sizeof(errStr));
DBGPRINTF("imsolaris: poll error: %d = %s.\n", errno, errStr);
errmsg.LogError(en, NO_ERRCODE, "imsolaris: poll error: %s",
errStr);
}
continue;
}
if(sun_Pfd.revents & POLLIN) {
readLog(sun_Pfd.fd, pRcv, iMaxLine);
} else if(sun_Pfd.revents & (POLLNVAL|POLLHUP|POLLERR)) {
tryRecover();
}
} else {
/* if we have an infinite wait, we do not use poll at all
* I'd consider this a waste of time. However, I do not totally
* remove the code, as it may be useful if we decide at some
* point to provide a capability to support multiple input streams
* at once (this may be useful for a jail). In that case, the poll()
* loop would be needed, and so it doesn't make much sense to change
* the code to not support it. -- rgerhards, 2010-04-20
*/
readLog(sun_Pfd.fd, pRcv, iMaxLine);
}
}
finalize_it:
if(pRcv != NULL && (size_t) iMaxLine >= sizeof(bufRcv) - 1)
free(pRcv);
RETiRet;
}
int Global_Plan(GP_FUNC_INPUT * pInput, GP_INFO * pData, GP_LOCAL_DATA * pLocal)
{
/* 0 全局规划预处理 */
/* 0.1 调用计数器自增 */
if(myFrameid==1)
MBUG("WARNING:this shell is for simulate only, no use any more\n");
++myFrameid;
pData->header.id = myFrameid;
MBUG("\n*************************FRAME_ID %d***********************************\n", myFrameid);
/* 0.2 检查规划器状态 */
if(RNDF_ERROR == gp.stat())
{
MBUG("GP@%d : GP is INVALID(RNDF STATUS ERROR, CHECK FILE task OR RECOVERY POINT)!\n", myFrameid);
MBUG("**********************************************************************\n");
return 1;
}
/* 0.3 预置全局规划信息，更新动态数据 */
gpInput = pInput;
gpOutput = pData;
gpEvent = pLocal;
memcpy(&pData->state, &pInput->state, sizeof(STATE));
gps[0] = pInput->state.pos.com_coord.x;
gps[1] = pInput->state.pos.com_coord.y;
gps[2] = pInput->state.pos.yaw*(180.0*1e-8)/3.1415926535898;
#ifdef USING_ROBIX4
rbxReadStatus(&robixStatus);
#else
if(myFrameid==1)
robixStatus=S_INVALID;
else
robixStatus=S_ROAD_NAV;
#endif
/* 1 执行全局规划 */
switch(robixStatus)
{
case S_INVALID:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "N/A", "S_INVALID");
tryRecover();
MBUG("**********************************************************************\n");
break;
case S_WAIT:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "N/A", "S_WAIT");
tryRecover();
MBUG("**********************************************************************\n");
break;
case S_WAIT_LIGHT:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_WAIT_LIGHT");
gp.context(gps,gpOutput);
MBUG("**********************************************************************\n");
break;
case S_ROAD_NAV:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_ROAD_NAV");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_CROSS_UND:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_CROSS_UND");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_STRAIGHT:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_STRAIGHT");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_LEFT:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_LEFT");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_RIGHT:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_RIGHT");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_UTURN:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_UTURN");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_PARKING:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_PARKING");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_FREE_RUN:
MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_FREE_RUN");
trySwitch();
MBUG("**********************************************************************\n");
break;
case S_TASK_OVER:
MBUG("GP@%d: gpState = {%s} (robixStatus = %s)\n", myFrameid, "Off", "S_TASK_OVER");
gpState = Off;
gp.context(gps,gpOutput);
MBUG("**********************************************************************\n");
break;
default:
break;
}
return 0;
}
