/*Thread to watch the sensors and send a message of events to the correct gate*/
void inputWatcher(){
char message[MAX_MESSAGE_LENGTH];
while(1){
newSensors = getSensorValues(); /*consider the read sensor values new*/
if(newSensors != oldSensors){ /*if the sensors have changed from the old one then continue*/
if( ( newSensors & IN_SENSORS ) != ( oldSensors & IN_SENSORS ) ){ /*if in gate sensors have changed:*/
sprintf(message, "IN:CHANGE FROM %d TO %d\n", oldSensors & IN_SENSORS, newSensors & IN_SENSORS); /*create message*/
if((msgQSend(inGateQueue, message, MAX_MESSAGE_LENGTH, WAIT_FOREVER, MSG_PRI_NORMAL)) == ERROR){ /*send message to in gate*/
printf("msgQSend to gate in failed\n");
}
}
if ( (newSensors & OUT_SENSORS) != (oldSensors & OUT_SENSORS) ){
sprintf(message, "OUT:CHANGE FROM %d TO %d\n", oldSensors & OUT_SENSORS, newSensors & OUT_SENSORS); /*create message*/
if((msgQSend(outGateQueue, message, MAX_MESSAGE_LENGTH, WAIT_FOREVER, MSG_PRI_NORMAL)) == ERROR){ /*send message to in gate*/
printf("msgQSend to gate out failed\n");
}
}
}
taskDelay(6); /*one tenth of a second*/
oldSensors = newSensors; /*consider the read sensor values old*/
}
}
int task1( int dummy0, int dummy1, int dummy2, int dummy3, int dummy4,
int dummy5, int dummy6, int dummy7, int dummy8, int dummy9 )
{
STATUS err;
int i;
char *buffer;
taskDelay( 50 );
for (;;) {
puts( "\r\ntask1 waiting for ready signal from either of SEM2 or SEM3" );
err = semTake( sema42_id, NO_WAIT );
if ( err == OK )
{
/*
** Received ready signal from task 2...
*/
buffer = ts_malloc( 128 );
if ( buffer != (char *)NULL )
{
for (i = 0; i < 10; i++)
buffer[i] = 'A' + i;
buffer[i] = 0;
printf("\r\ntask1's message for task2: %s\n", buffer );
err = msgQSend( queue2_id, buffer, 11, NO_WAIT, MSG_PRI_NORMAL );
ts_free( buffer );
}
else
puts( "\r\nNo memory for message to task 2" );
}
err = semTake( sema43_id, NO_WAIT );
if ( err == OK )
{
/*
** Received ready signal from task 3...
*/
buffer = ts_malloc( 128 );
if ( buffer != (char *)NULL )
{
for (i = 0; i < 10; i++)
buffer[i] = 'Z' - i;
buffer[i] = 0;
printf("\r\ntask1's message for task3: %s\n", buffer );
err = msgQSend( queue3_id, buffer, 11, NO_WAIT, MSG_PRI_NORMAL );
ts_free( buffer );
}
else
puts( "\r\nNo memory for message to task 3" );
}
taskDelay( 1 );
}
}
void startRead(){
char type;
int nbPack1;
int nbPack2;
message answer;
int batchNumber=0;
int commandNumber=0;
batch toDo;
pack toGet;
for (;;)
{
//First read the message type :
read(sock, &type, sizeof(char));
switch(type){
case 'a':
#ifdef test
printf("reveived a start/stop command");
#endif
//answer
read(sock,answer,sizeof(char));//get answer type
//send it to mere.
if (answer[0]=='s')
{
sprintf(answer+1,"Terminating application");
}else{
sprintf(answer+1,"Restarting application");
}
msgQSend(mid_actions,answer,sizeof(message),WAIT_FOREVER,MSG_PRI_URGENT);
break;
case 'i':
//Init
#ifdef test
printf("reveived an init command");
#endif
read(sock,(char*)&nbPack1,sizeof(int));
read(sock,(char*)&nbPack2,sizeof(int));
toDo.batchNumber=batchNumber++;
toDo.nbPack1=nbPack1;
toDo.nbPack2=nbPack2;
msgQSend(mid_batch,(char*)&toDo,sizeof(toDo),WAIT_FOREVER,MSG_PRI_NORMAL);
break;
case 'c':
//command
#ifdef test
printf("reveived an order command");
#endif
read(sock,(char*)&nbPack1,sizeof(int));
read(sock,(char*)&nbPack2,sizeof(int));
toGet.number=commandNumber++;
toGet.size=0;
toGet.nbType1=-1*nbPack1;
toGet.nbType2=-1*nbPack2;
msgQSend(mid_packaging,(char*)&toDo,sizeof(toDo),WAIT_FOREVER,MSG_PRI_URGENT);
//Message urgent because we don't want to wait for all the incomming
//packs to be handled by the warehouse before they handle incomming orders
break;
}
}
}
int Zprintf0(int force, char *buf)
{
int l;
int n;
int ret;
int msgret;
char *str;
char junk;
str = buf;
l = sipx_min(ABSOLUTE_MAX_LOG_MSG_LEN, strlen(buf));
ret = l;
if (0 == MpMisc.LogQ) {
ret = force ? fwrite(str, 1, ret, stderr) : 0;
return ret;
}
if (force || logging) {
if ((ret > MpMisc.logMsgSize) && !intContext()) {
taskLock();
}
while (l > 0) {
n = sipx_min(l, MpMisc.logMsgSize);
msgret = msgQSend(MpMisc.LogQ, buf, n,
VX_NO_WAIT, MSG_PRI_NORMAL);
if (ERROR == msgret) {
// int r2 =
msgQReceive(MpMisc.LogQ, &junk, 1, VX_NO_WAIT);
// osPrintf("discard message; r1=%d, r2=%d, q=0x%X: '%s'\n",
// msgret, r2, MpMisc.LogQ, buf);
numDiscarded++;
msgret = msgQSend(MpMisc.LogQ, buf, n,
VX_NO_WAIT, MSG_PRI_NORMAL);
}
if ((ERROR == msgret) && force) {
fwrite(str, 1, ret, stderr);
l = 0;
} else {
l -= n;
buf += n;
}
}
if ((ret > MpMisc.logMsgSize) && !intContext()) {
taskUnlock();
}
} else {
ret = 0;
}
return ret;
}
//------------------------------------------------------------------------------
tOplkError timeru_delInstance(void)
{
ULONG msg;
tTimeruData* pTimer;
/* send message to timer task to signal shutdown */
msg = 0;
msgQSend(timeruInstance_l.msgQueue, (char*)&msg, sizeof(ULONG),
NO_WAIT, MSG_PRI_NORMAL);
/* wait for timer task to end */
while (taskIdVerify(timeruInstance_l.taskId) == OK)
taskDelay(sysClkRateGet());
/* free up timer list */
resetTimerList();
while ((pTimer = getNextTimer()) != NULL)
{
hrtimer_delete (pTimer->timer);
removeTimer(pTimer);
OPLK_FREE(pTimer);
}
/* cleanup resources */
semDelete(timeruInstance_l.mutex);
msgQDelete(timeruInstance_l.msgQueue);
timeruInstance_l.pFirstTimer = NULL;
timeruInstance_l.pLastTimer = NULL;
return kErrorOk;
}
static void taskTwo(void)
{
char message[] = "Received Message from taskTwo.";
char msgBuf[MAX_MESSAGE_LENGTH];
int i;
for (i = 0; i < MAX_MESSAGES; i++)
{
if (msgQReceive(msgQueueId, msgBuf, MAX_MESSAGE_LENGTH,
WAIT_FOREVER) == ERROR)
{
printf("msgQReceive in taskTwo failed.\n");
}
else
{
printf("taskTwo %s\n",msgBuf);
}
if (msgQSend(msgQueueIdNew, message, MAX_MESSAGE_LENGTH,
WAIT_FOREVER, MSG_PRI_NORMAL) == ERROR)
{
printf("msgQsend in taskTwo failed.\n");
}
else
{
printf("taskTwo sent a message.\n");
}
}
msgQDelete(msgQueueId);
}
/*
* This routine is called from the context of the NDDS task n_rtu7400
*
* Author: Greg Brissey 8/04/2004
*/
static void ExceptionPub_CallBack(Cntlr_Comm *recvIssue)
{
/* in the future we might do a few test and call differnet routines but for
now we just relay the info on to a seperate Task to handle exceptions
so NDDS can get on with it's business of receiving issue
*/
/* if this was a exception message initiated from this controller then a msge was
* already directly sent to the phandler, thus we can safely ignore this message
*/
if (strcmp(recvIssue->cntlrId,hostName) != 0)
{
CNTLR_COMM_MSG excMsg;
strncpy(excMsg.cntlrId, recvIssue->cntlrId,16);
excMsg.cmd = recvIssue->cmd;
excMsg.errorcode = recvIssue->errorcode;
excMsg.warningcode = recvIssue->warningcode;
excMsg.arg1 = recvIssue->arg1;
excMsg.arg2 = recvIssue->arg2;
excMsg.arg3 = recvIssue->arg3;
excMsg.crc32chksum = recvIssue->crc32chksum;
strncpy(excMsg.msgstr,recvIssue->msgstr,COMM_MAX_STR_SIZE);
/* send error to exception handler task, it knows what to do */
msgQSend(pMsgesToPHandlr, (char*) &excMsg,
sizeof(CNTLR_COMM_MSG), NO_WAIT, MSG_PRI_NORMAL);
}
else
{
DPRINT1(-1,"NDDS callback for Local Exception from '%s', ignored.\n",recvIssue->cntlrId);
}
}
UGL_STATUS uglOSMsgQPost (
UGL_OS_MSG_Q_ID qId,
UGL_TIMEOUT timeout,
void *pMsg,
UGL_SIZE msgSize
) {
UGL_STATUS status;
int ern;
if (timeout != UGL_WAIT_FOREVER) {
timeout = timeout * sysClockRateGet() / 1000;
}
ern = errnoGet();
if (msgQSend(qId, (char *) pMsg, msgSize, timeout, 0)) {
switch(ern) {
case S_objLib_UNAVAILABLE:
errnoSet(ern);
status = UGL_STATUS_Q_FULL;
break;
default:
status = UGL_STATUS_ERROR;
break;
}
}
else {
status = UGL_STATUS_OK;
}
return status;
}
/* 3x callback */
RTIBool Monitor_CmdCallback(const NDDSRecvInfo *issue, NDDSInstance *instance,
void *callBackRtnParam)
{
Monitor_Cmd *recvIssue;
MONITOR_MSG monIssue;
void decode(MONITOR_MSG *issue);
if (issue->status == NDDS_FRESH_DATA)
{
recvIssue = (Monitor_Cmd *) instance;
monIssue.cmd = recvIssue->cmd;
monIssue.arg1 = recvIssue->arg1;
monIssue.arg2 = recvIssue->arg2;
monIssue.arg3 = recvIssue->arg3;
monIssue.arg4 = recvIssue->arg4;
monIssue.arg5 = recvIssue->arg5;
monIssue.crc32chksum = recvIssue->crc32chksum;
monIssue.msgstr.len = recvIssue->msgstr.len;
strncpy(monIssue.msgstr.val,recvIssue->msgstr.val,MAX_MONITOR_MSG_STR);
DPRINT6(+1,"Monitor_Cmd CallBack: cmd: %d, arg1: %d, arg2: %d, arg3: %d, crc: 0x%lx, msgstr: '%s'\n",
recvIssue->cmd,recvIssue->arg1, recvIssue->arg2, recvIssue->arg3, recvIssue->crc32chksum, recvIssue->msgstr.val);
msgQSend(pMsgesToMonitor,(char*) &monIssue, sizeof(MONITOR_MSG), NO_WAIT, MSG_PRI_NORMAL);
/* decode(recvIssue); */
}
return RTI_TRUE;
}
sendFidCtStatus()
{
FidCt_Stat fstat;
if (pFidCtStatusPub != NULL)
{
fstat.FidCt = fid_count;
fstat.Ct = fid_ct;
/*
pFidCtStat->FidCt = fidnum;
pFidCtStat->Ct = ct;
*/
}
/*
if (pFidCtPubSem != NULL)
semGive(pFidCtPubSem);
*/
if (pFidCtPubMsgQ == NULL)
return;
// If the msg Q is full msgQSend will not post the new fid status - this just means
// we are running too fast to keep up with the (real time) fid/ct update rate
// and some fid/ct status values will not show up in vj's exp bar.
msgQSend(pFidCtPubMsgQ, (char *)&fstat, sizeof(FidCt_Stat), NO_WAIT, MSG_PRI_NORMAL);
/* DPRINT1(-1,"sendFidCtStatus: MsqSend: 0x%lx\n",pFidCtPubMsgQ); */
}
STATUS pcmciaJobAdd
(
VOIDFUNCPTR func, /* function pointer */
int arg1, /* argument 1 */
int arg2, /* argument 2 */
int arg3, /* argument 3 */
int arg4, /* argument 4 */
int arg5, /* argument 5 */
int arg6 /* argument 6 */
)
{
PCMCIA_MSG msg;
msg.func = func;
msg.arg[0] = arg1;
msg.arg[1] = arg2;
msg.arg[2] = arg3;
msg.arg[3] = arg4;
msg.arg[4] = arg5;
msg.arg[5] = arg6;
if (msgQSend (pcmciaMsgQId, (char *) &msg, sizeof (msg),
INT_CONTEXT() ? NO_WAIT : WAIT_FOREVER, MSG_PRI_NORMAL) != OK)
{
++pcmciaMsgsLost;
return (ERROR);
}
return (OK);
}
/*
* Board Specific ISR for harderrors of the system
*
* Author Greg Brissey 9/23/04
*
*/
static void Brd_Specific_Fail_ISR(int int_status, int errorcode)
{
msgQSend(pMsgs2FifoIST,(char*) &errorcode, sizeof(int), NO_WAIT, MSG_PRI_NORMAL);
if (DebugLevel > -1)
logMsg("Brd_Specific_Fail_ISR: status: 0x%lx, errorcode: %d\n",int_status, errorcode);
return;
}
STATUS excJobAdd(
VOIDFUNCPTR func,
ARG arg0,
ARG arg1,
ARG arg2,
ARG arg3,
ARG arg4,
ARG arg5
)
{
STATUS status;
int wt;
EXC_MSG message;
if (excLibInstalled != TRUE)
{
status = ERROR;
}
else
{
/* Setup work structure */
message.func = func;
message.arg[0] = arg0;
message.arg[1] = arg1;
message.arg[2] = arg2;
message.arg[3] = arg3;
message.arg[4] = arg4;
message.arg[5] = arg5;
/* Select wait method */
if (INT_CONTEXT() == TRUE)
{
wt = WAIT_NONE;
}
else
{
wt = WAIT_FOREVER;
}
/* Send message to queue */
if (msgQSend(
excMsgQId,
&message,
sizeof(message),
wt,
MSG_PRI_NORMAL
) != OK)
{
++excMsgsLost;
status = ERROR;
}
else
{
status = OK;
}
}
return status;
}
void isrCapteur()
{
//some new part has been detected
part newPart;
partType=1; //part type is 1
newPart.type=1;
msgQSend(mid_received_part,(char*)&newPart,sizeof(newPart),NO_WAIT,MSG_PRI_NORMAL);
}
static void rootTask(long a0, long a1, long a2, long a3, long a4,
long a5, long a6, long a7, long a8, long a9)
{
int ret, msg, n;
traceobj_enter(&trobj);
traceobj_mark(&trobj, 1);
qid = msgQCreate(NMESSAGES, sizeof(msg), MSG_Q_FIFO);
traceobj_assert(&trobj, qid != 0);
traceobj_mark(&trobj, 2);
for (msg = 0; msg < NMESSAGES; msg++) {
ret = msgQSend(qid, (char *)&msg, sizeof(msg), NO_WAIT, MSG_PRI_NORMAL);
traceobj_assert(&trobj, ret == OK);
}
traceobj_mark(&trobj, 3);
ret = msgQSend(qid, (char *)&msg, sizeof(msg), WAIT_FOREVER, MSG_PRI_URGENT);
traceobj_assert(&trobj, ret == OK);
traceobj_mark(&trobj, 4);
ret = msgQReceive(qid, (char *)&msg, sizeof(int), WAIT_FOREVER);
traceobj_assert(&trobj, ret == sizeof(int) && msg == 10);
traceobj_mark(&trobj, 5);
for (n = 1; n < NMESSAGES; n++) { /* peer task read #0 already. */
ret = msgQReceive(qid, (char *)&msg, sizeof(int), WAIT_FOREVER);
traceobj_assert(&trobj, ret == sizeof(int) && msg == n);
}
traceobj_mark(&trobj, 6);
ret = msgQReceive(qid, (char *)&msg, sizeof(int), WAIT_FOREVER);
traceobj_assert(&trobj, ret == ERROR && errno == S_objLib_OBJ_DELETED);
traceobj_mark(&trobj, 7);
traceobj_exit(&trobj);
}
/*
* Sensors handling functions : isr and msgDispatch
*/
void isr()
{
char value[4];
memcpy(value,®,4);//copy register to local variable
msgQSend(isrmq,value,4,NO_WAIT,MSG_PRI_NORMAL); //queue msg
//If msgQSend fails it probably means that the msg queue hasn't been created
//yet. But we don't really care in here whether there is someone to listen to
//us or we just shouting in the wind, thus no return code test.
}
int logMsg(
char *fmt,
ARG arg0,
ARG arg1,
ARG arg2,
ARG arg3,
ARG arg4,
ARG arg5
)
{
int ret;
int timeout;
LOG_MSG logMsg;
/* If called from interupt */
if (INT_CONTEXT() == TRUE)
{
logMsg.id = -1;
timeout = WAIT_NONE;
}
else
{
logMsg.id = taskIdSelf();
timeout = WAIT_FOREVER;
}
/* Setup struct */
logMsg.fmt = fmt;
logMsg.arg[0] = arg0;
logMsg.arg[1] = arg1;
logMsg.arg[2] = arg2;
logMsg.arg[3] = arg3;
logMsg.arg[4] = arg4;
logMsg.arg[5] = arg5;
/* If message got error when sending */
if (msgQSend(
logMsgQId,
&logMsg,
sizeof(logMsg),
timeout,
MSG_PRI_NORMAL
) != OK)
{
logMsgsLost++;
ret = EOF;
}
else
{
ret = sizeof(logMsg);
}
return ret;
}
tstLkMsgQ()
{
Lock_Cmd tstIssue;
tstIssue.cmd = LK_SET_PHASE;
tstIssue.arg1 = 180;
tstIssue.arg2 = 0;
tstIssue.arg3 = 0.0;
tstIssue.arg4 = 0.0;
msgQSend(pMsgesToLockParser, (char*) &tstIssue, sizeof(Lock_Cmd), NO_WAIT, MSG_PRI_NORMAL);
}
// unpack udp data and msgq send
static uint32 UDP_UnpackRecvBtmInfoData(MSG_Q_ID id)
{
sint32 i = 0;
sint32 j = 0;
STATUS rs = ERROR;
uint8 btmRst = 0;
uint8 ret = 0U; // 临时变量，返回值
uint32 offset = UDP_MSG_HEAD_LEN; // 临时变量，地址偏移
ret = ArrayToVariable_uint32(&(udpRecvBtmInfo.btmId), udpRecvBuf + offset);
offset += ret;
ret = ArrayToVariable_uint32(&(udpRecvBtmInfo.triggerPulse), udpRecvBuf + offset);
offset += ret;
ret = ArrayToVariable_uint32(&(udpRecvBtmInfo.dataLength), udpRecvBuf + offset);
offset += ret;
if (udpRecvBtmInfo.dataLength > UDP_BTM_DATA_BUF_LEN)
{
udpRecvBtmInfo.dataLength = UDP_BTM_DATA_BUF_LEN;
}
for (j = 0; j < udpRecvBtmInfo.dataLength; j++)
{
ret = ArrayToVariable_uint8(&(udpRecvBtmInfo.dataBuf[j]), udpRecvBuf + offset);
offset += ret;
}
if (SPD_GetLeftSpdPluseNum() < udpRecvBtmInfo.triggerPulse)
{
btmRst = UDP_CheckBtmIdList(id);
if (BTM_ID_CHECKRST_OK == btmRst)
{
// send btm data to msg queue
rs = msgQSend(id, (char *)&udpRecvBtmInfo, sizeof(udpRecvBtmInfo), NO_WAIT, MSG_PRI_NORMAL);
if (rs == ERROR)
{
DBG_ERRO("[UDP] msgQSend error! msgQnum=%d\n", msgQNumMsgs(id));
}
}
else
{
DBG_INFO("[UDP] btm id have been in list\n");
}
}
else
{
DBG_INFO("[UDP] recv old btm trigger pulse\n");
}
return offset;
}
/**
* Cette fonction est appelée quand un message est disponible sur le réseau.
* Elle numérote et date le message, et le place dans le buffer correspondant a son
* addresse, si un device logiciel est associé à un capteur.
* Sinon, le message est perdu.
*/
void pe_interrupt_handler()
{
/* Les id des messages sont globales au driver */
static unsigned id_msg = 0;
Message msg;
msg.id = id_msg++;
// On fait passer l'adresse par la date non initialisé pour éviter des champs de mémoire supplémentaire inutile.
msg.date.tv_nsec = mock_can_device.address;
strncpy(msg.msg, mock_can_device.data, MAX_DATA_SIZE);
msgQSend( msgQ_dispatcher, (char*)&msg, sizeof(msg), WAIT_FOREVER, MSG_PRI_NORMAL);
}
/*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*/
}
}
static int __wind_msgq_send(struct pt_regs *regs)
{
xnhandle_t handle = __xn_reg_arg1(regs);
char tmp_buf[128], *msgbuf;
wind_msgq_t *msgq;
int timeout, prio;
unsigned nbytes;
STATUS err;
nbytes = __xn_reg_arg3(regs);
timeout = __xn_reg_arg4(regs);
prio = __xn_reg_arg5(regs);
if (timeout != NO_WAIT && !xnpod_primary_p())
return -EPERM;
msgq = (wind_msgq_t *)xnregistry_fetch(handle);
if (!msgq)
return S_objLib_OBJ_ID_ERROR;
if (nbytes > msgq->msg_length)
return S_msgQLib_INVALID_MSG_LENGTH;
if (nbytes <= sizeof(tmp_buf))
msgbuf = tmp_buf;
else {
msgbuf = (char *)xnmalloc(nbytes);
if (!msgbuf)
return S_memLib_NOT_ENOUGH_MEMORY;
}
/* This is sub-optimal since we end up copying the data twice. */
if (__xn_safe_copy_from_user(msgbuf, (void __user *)__xn_reg_arg2(regs), nbytes))
err = -EFAULT;
else {
if (msgQSend((MSG_Q_ID)msgq, msgbuf, nbytes, timeout, prio) == ERROR)
err = wind_errnoget();
else
err = 0;
}
if (msgbuf != tmp_buf)
xnfree(msgbuf);
return err;
}
epicsShareFunc int epicsShareAPI epicsMessageQueueSendWithTimeout(
epicsMessageQueueId id,
void *message,
unsigned int messageSize,
double timeout)
{
int ticks;
if (timeout<=0.0) {
ticks = 0;
} else {
ticks = (int)(timeout*sysClkRateGet());
if(ticks<=0) ticks = 1;
}
return msgQSend((MSG_Q_ID)id, (char *)message, messageSize, ticks, MSG_PRI_NORMAL);
}
static void rootTask(long a0, long a1, long a2, long a3, long a4,
long a5, long a6, long a7, long a8, long a9)
{
TASK_ID tid;
int ret, n;
traceobj_enter(&trobj);
qid = msgQCreate(NMESSAGES, sizeof(int), MSG_Q_PRIORITY);
traceobj_assert(&trobj, qid != 0);
traceobj_mark(&trobj, 3);
ret = taskPrioritySet(taskIdSelf(), 10);
traceobj_assert(&trobj, ret == OK);
traceobj_mark(&trobj, 4);
tid = taskSpawn("peerTask",
11,
0, 0, peerTask, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
traceobj_assert(&trobj, tid != ERROR);
traceobj_mark(&trobj, 5);
n = 0;
do
ret = msgQSend(qid, (char *)&messages[n], sizeof(int), NO_WAIT, MSG_PRI_URGENT);
while(n++ < NMESSAGES && ret != ERROR);
traceobj_assert(&trobj, ret == ERROR && errno == S_objLib_OBJ_UNAVAILABLE && n == NMESSAGES + 1);
traceobj_mark(&trobj, 6);
ret = taskDelay(10);
traceobj_assert(&trobj, ret == OK);
traceobj_mark(&trobj, 7);
ret = msgQNumMsgs(qid);
traceobj_assert(&trobj, ret == 0);
traceobj_verify(&trobj, tseq, sizeof(tseq) / sizeof(int));
traceobj_exit(&trobj);
}
/*
* =====================================================================
* Function:MsgQSend()
* Description: send a message to a message queue
* Input: msgQId -- msgQ id
* msgBuffer -- message to send
* msgLength -- length of the message to be sent
* timeTick -- wait time (in ticks)
* WAIT_FOREVER, this routine block until queue not full
* NO_WAIT, this routine return immediately with errorno
* wait time, this routine block until queue not full or time up
* msgPriority -- MSG_PRI_NORMAL or MSG_PRI_URGENT
* MSG_PRI_NORMAL(0) add the message to the tail of the queue
* MSG_PRI_URGENT(1) add the message to the head of the queue
* Output: N/A
* Return: OK on success or ERROR otherwise.
*======================================================================
*/
int MsgQSend(MSG_QUEUE_ID msgQId, const char* msgBuffer, int msgLength, int timeTick, int msgPriority)
{
#ifdef LINUX_OS
if (msgQId == AII_NULL || msgBuffer == AII_NULL)
{
return (-1);
}
return Mq_send((mqd_t)(*msgQId), msgBuffer, msgLength, timeTick, msgPriority);
#elif VXWORKS_OS
if (msgQId == AII_NULL || msgBuffer == AII_NULL)
{
return (-1);
}
return msgQSend(msgQId, msgBuffer, msgLength, timeTick, msgPriority);
#endif
}
/*
* The controller sends it's exception directly to it's phandler via this call
* the NDDS callback Exception subscription handler will ignore the Exception coming
* from itself
*/
sendExceptionDirectly(Cntlr_Comm *recvIssue)
{
CNTLR_COMM_MSG excMsg;
strncpy(excMsg.cntlrId, recvIssue->cntlrId,16);
excMsg.cmd = recvIssue->cmd;
excMsg.errorcode = recvIssue->errorcode;
excMsg.warningcode = recvIssue->warningcode;
excMsg.arg1 = recvIssue->arg1;
excMsg.arg2 = recvIssue->arg2;
excMsg.arg3 = recvIssue->arg3;
excMsg.crc32chksum = recvIssue->crc32chksum;
strncpy(excMsg.msgstr,recvIssue->msgstr,COMM_MAX_STR_SIZE);
/* send error to exception handler task, it knows what to do */
msgQSend(pMsgesToPHandlr, (char*) &excMsg,
sizeof(CNTLR_COMM_MSG), NO_WAIT, MSG_PRI_NORMAL);
}
/*|><|************************************************************************
*
* Method Name: Post
*
*
* Inputs: CMessage *
*
* Outputs: None
*
* Returns: bool
*
* Logic Notes:
*
* Caveats:
*
************************************************************************|<>|*/
bool CMsgQueue::Post(CMessage *poMessage)
{
#ifdef WIN32 /* [ */
// A client has requested us to post this message on the queue.
// Let's check whether the MessageEvent is still valid. It may become
// invalid at the initial stages of startup or at termination.
if(m_hMessageEvent == NULL)
return(FALSE);
// Presumably all is well. Let's add the message to the list
if(!AddEntry(poMessage))
return(FALSE);
// The Entry has been successfully added. Let's set the message event
// to actuate its being processed.
SetEvent(m_hMessageEvent);
#elif defined(_VXWORKS) /* ] [ */
// Check whether the Message Queue was successfully initialized
if(m_ulMsgQID == NULL)
return(FALSE);
// Use the vxWorks Message Posting routine to place the message on the FIFO
if(msgQSend(m_ulMsgQID,
(char *)(&poMessage),
sizeof(CMessage *),
NO_WAIT_TIME,
MSG_PRI_NORMAL) == ERROR)
{
return(FALSE);
}
#elif defined(__pingtel_on_posix__) /* ] [ */
if(m_pMsgQ == NULL)
return(FALSE);
#ifdef RTCP_LINUX_DEBUG
osPrintf("DEBUG: RTCP Message queue queueing message! (type %X)\n", poMessage->GetMsgType());
#endif
if(m_pMsgQ->send(*(OsMsg *)poMessage, OsTime::NO_WAIT_TIME))
return(FALSE);
poMessage->releaseMsg();
#endif /* ] */
return(TRUE);
}
RTIBool Lock_CmdCallback(const NDDSRecvInfo *issue, NDDSInstance *instance,
void *callBackRtnParam)
{
Lock_Cmd *recvIssue;
void LockCmdParser(Lock_Cmd *issue);
int *param;
if (issue->status == NDDS_FRESH_DATA)
{
recvIssue = (Lock_Cmd *) instance;
DPRINT5(-1,"Lock_Cmd CallBack: cmd: %d, arg1: %d, arg2: %d, arg3: %lf, arg4: %lf crc: 0x%lx\n",
recvIssue->cmd,recvIssue->arg1, recvIssue->arg2, recvIssue->arg3, recvIssue->arg4);
msgQSend(pMsgesToLockParser, (char*) recvIssue, sizeof(Lock_Cmd), NO_WAIT, MSG_PRI_NORMAL);
/* LockCmdParser(recvIssue); */
}
return RTI_TRUE;
}
/**
******************************************************************************
* @brief 日志输出
* @param[in] fmt : 日志信息与printf相同
*
* @retval ERROR: 发送日志消息失败
* @retval OK : 发送日志消息成功
******************************************************************************
*/
status_t
logmsg(const char *fmt, ...)
{
log_msg_t *msg;
if (the_logmsg_taskid == NULL)
{
va_list args;
va_start( args, fmt );
print( 0, fmt, args );
return OK;
}
if (strlen(fmt) > (MAX_BYTES_IN_A_MSG - 1))
{
return ERROR;
}
msg = malloc(sizeof(log_msg_t));
/* 判断是否在中断中调用 */
msg->id = (intContext() == TRUE) ? -1 : (int32_t)taskIdSelf();
char *out = (char *)msg->buf;
va_list args;
va_start( args, fmt );
msg->len = print( &out, fmt, args );
va_end(args);
if (msg->len > (MAX_BYTES_IN_A_MSG - 1))
{
the_logmsgs_outoflen++; /* shit out of buf length */
}
msg->buf[msg->len] = '\0';
if (msgQSend(the_logmsg_qid, (void *) msg) != OK)
{
++the_logmsgs_lost;
return ERROR;
}
return OK;
}
void state_photo_check_timer_isr(void)
{
photo_check_time_loop_count++;
/*
當是底下 MSG 馬上跳離
MSG_APQ_CONNECT_TO_PC / MSG_APQ_MENU_KEY_ACTIVE / MSG_APQ_MODE
*/
if((pic_flag == 2)||(pic_flag == 3)||(pic_flag == 4))
{
photo_check_time_loop_count = photo_check_time_preview_count;
}
if(photo_check_time_loop_count >= photo_check_time_preview_count)
{
msgQSend(ApQ, MSG_APQ_CAPTURE_PREVIEW_ON, NULL, NULL, MSG_PRI_NORMAL);
timer_stop(TIMER_C);
}
}