xenomai queue測試程序

同樣的，記錄下xenomai的queue通信的測試程序。程序的現象是，程序運行以後，從命令行中輸入的數據，會通過queue發送到另一個任務，並打印出來。

#include <errno.h>
#include <signal.h>
#include <string.h>
#include <sys/resource.h>
#include <unistd.h>
#include <sys/mman.h>
#include <native/task.h>
#include <native/timer.h>
#include <stdio.h>
RT_TASK task_desc,task_command_receive;
static int run=1;

RT_QUEUE q_command;
void ethercat(void *cookie)
{
    int err;
    int time=0;
	ssize_t len;
    void *msg;
    err=rt_task_set_periodic(NULL,TM_NOW,1000000);
    while(run) {
        rt_task_wait_period(NULL);
//        time++;
//       rt_printf("tims is %d\n",time);
		    err = rt_queue_bind(&q_command,"command_queue",TM_INFINITE);
			if (err)
				fail();
			/* Collect each message sent to the queue by the queuer() routine,
			   until the queue is eventually removed from the system by a call
			   to rt_queue_delete(). */
			while ((len = rt_queue_receive(&q_command,&msg,TM_INFINITE)) > 0)
				{
				printf("received message> len=%d bytes, ptr=%p, s=%s\n",
					   len,msg,(const char *)msg);
				rt_queue_free(&q_command,msg);
				}
			/* We need to unbind explicitly from the queue in order to
			   properly release the underlying memory mapping. Exiting the
			   process unbinds all mappings automatically. */
			rt_queue_unbind(&q_command);
			if (len != -EIDRM)
				/* We received some unexpected error notification. */
				fail();
    }
}

void command(void * cookie)
{
	      int n, len;
          void *msg;
		  char receiveCommand[50]={0};
		  err=rt_task_set_periodic(NULL,TM_NOW,1000000);
		  while(run)
		  {
			  rt_task_wait_period(NULL);
			  while(fgets(receiveCommand,50,stdin)!=NULL)
              {
//                      printf("str is %s,length is %d",receiveCommand,strlen(receiveCommand)); 
                        if(strstr(receiveCommand,"LED=1"))
                        {
                                printf("LED=1 in\n");
                        }
                        else if(strstr(receiveCommand,"LED=0"))
                        {
                             //   printf("LED=0 in..\n");
								
								

                        }
                        else
                        {
                               // printf("invalid command!\n");
							   len = strlen(receiveCommand) + 1;
							   msg = rt_queue_alloc(&q_command,len);
								if (!msg)
									/* No memory available. */
									fail();
								strcpy(msg,receiveCommand);
								rt_queue_send(&q_command,msg,len,Q_NORMAL);
                        }
                        memset(receiveCommand,50,0);
               }
			  
		  }

}

void signal_handler(int sig)
{
        run=0;
}

void cleanup ()
{
    rt_task_delete(&task_desc);
	rt_task_delete(&task_command_receive);
}
int main (int argc, char *argv[])
{
   int err,ret;
   rt_print_auto_init(1);
   printf("start run.\n");
   signal(SIGTERM,signal_handler);
   signal(SIGINT,signal_handler);
   mlockall(MCL_CURRENT|MCL_FUTURE);
   //create msg queue
   ret=rt_queue_create(&q_command,"command_queue",20,1,Q_FIFO);
   if (ret < 0) {
        fprintf(stderr, "Failed to create commnd_queue: %s\n", strerror(-ret));
        return -1;
    }
   
   
 //ethercat 任務
    ret = rt_task_create(&task_desc, "ethercat", 0, 99, T_FPU);
    if (ret < 0) {
        fprintf(stderr, "Failed to create task: %s\n", strerror(-ret));
        return -1;
    }

    printf("Starting ethercat...\n");
    ret = rt_task_start(&task_desc, &ethercat, NULL);
    if (ret < 0) {
        fprintf(stderr, "Failed to start task: %s\n", strerror(-ret));
        return -1;
    }
	//解析命令行任務
	ret = rt_task_create(&task_command_receive, "command_receive", 0, 80, T_FPU);
    if (ret < 0) {
        fprintf(stderr, "Failed to create task: %s\n", strerror(-ret));
        return -1;
    }

    printf("Starting command_receive...\n");
    ret = rt_task_start(&task_command_receive, &command, NULL);
    if (ret < 0) {
        fprintf(stderr, "Failed to start task: %s\n", strerror(-ret));
        return -1;
    }
   while(run)
        {
                sched_yield();
        }
    cleanup();
    printf("end\n");
    return 0;

}
//sudo  gcc taskTest.c -I/usr/xenomai/include  -lpthread -lrt -lnative -lxenomai -L/usr/xenomai/lib -o taskTest