xenomai rt_sem 信号量

  
  

   
   #include <stdio.h>
  
  
  
  

   
   #include <signal.h>
  
  
  
  

   
   #include <unistd.h>
  
  
  
  

   
   #include <sys/mman.h>
  
  
  
  

   
    
  
  
  
  

   
   #include <native/task.h>
  
  
  
  

   
   #include <native/timer.h>
  
  
  
  

   
   #include <native/sem.h>
  
  
  
  

   
     
  
  
  
  

   
   #include  <rtdk.h>
  
  
  
  


  
  
  
  

   
   #include "program.h"
  
  
  
  


  
  
  
  


  
  
  
  

   
   RT_TASK 
   
   task
   
   ;
  
  
  
  


  
  
  
  

   
   RT_SEM 
   
   readDone
   
   ;
  
  
  
  

   
   RT_SEM 
   
   executionDone
   
   ;
  
  
  
  

   
   RT_SEM 
   
   writeDone
   
   ;
  
  
  
  


  
  
  
  

   
   short 
   
   stopped 
   
   = 
   
   0
   
   ;
  
  
  
  

   
   void
   
   * 
   
   sensors
   
   ;
  
  
  
  

   
   void
   
   * 
   
   actuators
   
   ;
  
  
  
  


  
  
  
  

   
   void 
   
   sense
   
   (
   
   void 
   
   *
   
   args
   
   );
  
  
  
  

   
   void 
   
   execute
   
   (
   
   void 
   
   *
   
   args
   
   );
  
  
  
  

   
   void 
   
   act
   
   (
   
   void 
   
   *
   
   agrs
   
   );
  
  
  
  


  
  
  
  

   
   void 
   
   sense
   
   (
   
   void 
   
   *
   
   args
   
   ){
  
  
  
  
	
   
   rt_task_set_periodic
   
   (
   
   NULL
   
   ,
   
   TM_NOW
   
   ,
   
   PLCperiod
   
   );
  
  
  
  
	
   
   while
   
   (
   
   !
   
   stopped
   
   ){
  
  
  
  
		
   
   /*
  
  
  
  

   
   		 *	Read inputs status
  
  
  
  

   
   		*/
  
  
  
  
		
   
   sensors 
   
   = 
   
   readInputs
   
   ();
  
  
  
  
		
   
   rt_sem_v
   
   (
   
   &
   
   readDone
   
   );
  
  
  
  
		
   
   rt_task_wait_period
   
   (
   
   NULL
   
   );
  
  
  
  
	
   
   }
  
  
  
  

   
   }
  
  
  
  


  
  
  
  

   
   void 
   
   act
   
   (
   
   void 
   
   *
   
   args
   
   ){
  
  
  
  
	
   
   rt_task_set_periodic
   
   (
   
   NULL
   
   ,
   
   TM_NOW
   
   ,
   
   PLCperiod
   
   );
  
  
  
  
	
   
   while
   
   (
   
   !
   
   stopped
   
   ){
  
  
  
  
		
   
   rt_sem_p
   
   (
   
   &
   
   executionDone
   
   ,
   
   0
   
   );
  
  
  
  
		
   
   /*
  
  
  
  

   
   		 *	Write outputs status
  
  
  
  

   
   		 */
  
  
  
  
		
   
   writeOutputs
   
   (
   
   actuators
   
   );
  
  
  
  
		
   
   rt_sem_v
   
   (
   
   &
   
   writeDone
   
   );
  
  
  
  
		
   
   rt_task_wait_period
   
   (
   
   NULL
   
   );
  
  
  
  
	
   
   }
  
  
  
  

   
   }
  
  
  
  


  
  
  
  

   
   void 
   
   executor
   
   (
   
   void 
   
   *
   
   args
   
   ){
  
  
  
  
	
   
   rt_task_set_periodic
   
   (
   
   NULL
   
   ,
   
   TM_NOW
   
   ,
   
   PLCperiod
   
   );
  
  
  
  
	
   
   short 
   
   step 
   
   = 
   
   (
   
   short
   
   )
   
   *
   
   args
   
   ;
  
  
  
  
	
   
   actuators 
   
   = 
   
   0
   
   ;	
   
   // how to initialize the temporary value of actuators?
  
  
  
  
	
   
   while
   
   (
   
   !
   
   stopped
   
   ){
  
  
  
  
		
   
   rt_sem_p
   
   (
   
   &
   
   readDone
   
   ,
   
   0
   
   );
  
  
  
  
		
   
   int 
   
   c
   
   ;
  
  
  
  
		
   
   for
   
   (
   
   c
   
   =
   
   0
   
   ;
   
   c
   
   <
   
   nStep
   
   ;
   
   c
   
   ++
   
   ){
  
  
  
  
			
   
   if
   
   (
   
   stepStatus
   
   [
   
   c
   
   ])
  
  
  
  
				
   
   step
   
   [
   
   c
   
   ](
   
   sensors
   
   );	
   
   // or equivalent
  
  
  
  
		
   
   }
  
  
  
  
		
   
   rt_sem_v
   
   (
   
   &
   
   executionDone
   
   );
  
  
  
  
		
   
   rt_sem_p
   
   (
   
   &
   
   writeDone
   
   ,
   
   0
   
   );
  
  
  
  
		
   
   for
   
   (
   
   c
   
   =
   
   0
   
   ;
   
   c
   
   <
   
   nStep
   
   ;
   
   c
   
   ++
   
   ){
  
  
  
  
			
   
   if
   
   (
   
   stepStatus
   
   [
   
   c
   
   ])
  
  
  
  
				
   
   condition
   
   [
   
   c
   
   ](
   
   sensor
   
   ,
   
   actuators
   
   );
  
  
  
  
		
   
   }
  
  
  
  
		
   
   rt_task_wait_period
   
   (
   
   NULL
   
   );
  
  
  
  
	
   
   }
  
  
  
  
	
   
   return
   
   ;
  
  
  
  

   
   }
  
  
  
  


  
  
  
  

   
   void 
   
   init_xenomai
   
   () 
   
   {
  
  
  
  
	
   
   // Perform auto-init of rt_print buffers if the task doesn't do so
  
  
  
  
	
   
   rt_print_auto_init
   
   (
   
   1
   
   );
  
  
  
  


  
  
  
  

   
    	
   
   // Avoids memory swapping for this program
  
  
  
  
	
   
   mlockall
   
   (
   
   MCL_CURRENT
   
   |
   
   MCL_FUTURE
   
   );
  
  
  
  

   
   }
  
  
  
  


  
  
  
  

   
   void 
   
   startup
   
   (){
  
  
  
  
	
   
   stepInitializzator
   
   ();
  
  
  
  


  
  
  
  
	
   
   rt_sem_create
   
   (
   
   &
   
   readDone
   
   , 
   
   "readSem"
   
   , 
   
   0
   
   , 
   
   S_PRIO
   
   );
  
  
  
  
	
   
   rt_sem_create
   
   (
   
   &
   
   executionDone
   
   , 
   
   "executionSem"
   
   , 
   
   0
   
   , 
   
   S_PRIO
   
   );
  
  
  
  
	
   
   rt_sem_create
   
   (
   
   &
   
   writeDone
   
   , 
   
   "writeSem"
   
   , 
   
   0
   
   , 
   
   S_PRIO
   
   );
  
  
  
  


  
  
  
  
	
   
   rt_task_create
   
   (
   
   &
   
   task
   
   , 
   
   "readerTask"
   
   , 
   
   0
   
   , 
   
   99
   
   , 
   
   0
   
   );
  
  
  
  
	
   
   rt_task_start
   
   (
   
   &
   
   task
   
   , 
   
   &
   
   reader
   
   , 
   
   NULL
   
   );
  
  
  
  


  
  
  
  
	
   
   rt_task_create
   
   (
   
   &
   
   task
   
   , 
   
   "executorTask"
   
   , 
   
   0
   
   , 
   
   98
   
   , 
   
   0
   
   );
  
  
  
  
	
   
   rt_task_start
   
   (
   
   &
   
   task
   
   , 
   
   &
   
   executor
   
   , 
   
   NULL
   
   );
  
  
  
  


  
  
  
  
	
   
   rt_task_create
   
   (
   
   &
   
   task
   
   , 
   
   "writerTask"
   
   , 
   
   0
   
   , 
   
   97
   
   , 
   
   0
   
   );
  
  
  
  
	
   
   rt_task_start
   
   (
   
   &
   
   task
   
   , 
   
   &
   
   writer
   
   , 
   
   NULL
   
   );
  
  
  
  

   
   }
  
  
  
  


  
  
  
  

   
   void 
   
   catch_signal
   
   (
   
   int 
   
   sig
   
   ) 
   
   {
  
  
  
  
	
   
   stopped
   
   =
   
   1
   
   ;
  
  
  
  
	
   
   rt_printf
   
   (
   
   "--should I exit?--
   
   \n
   
   "
   
   );
  
  
  
  
	
   
   //exit(0);
  
  
  
  

   
   }
  
  
  
  


  
  
  
  

   
   void 
   
   wait_for_ctrl_c
   
   () 
   
   {
  
  
  
  
	
   
   signal
   
   (
   
   SIGTERM
   
   , 
   
   catch_signal
   
   );
  
  
  
  
	
   
   signal
   
   (
   
   SIGINT
   
   , 
   
   catch_signal
   
   );
  
  
  
  
	
   
   // wait for SIGINT (CTRL-C) or SIGTERM signal
  
  
  
  
	
   
   pause
   
   ();
  
  
  
  

   
   }
  
  
  
  


  
  
  
  

   
   void 
   
   cleanup
   
   (){
  
  
  
  
	
   
   rt_printf
   
   (
   
   "cleaned
   
   \n
   
   "
   
   );
  
  
  
  

   
   }
  
  
  
  


  
  
  
  

   
   int 
   
   main
   
   (
   
   int 
   
   argn
   
   , 
   
   char
   
   ** 
   
   argv
   
   ){
  
  
  
  
	
   
   init_xenomai
   
   ();
  
  
  
  

   
   //	rt_printf("initialized.\n");
  
  
  
  

   
   //	rt_printf("starting up...\n");
  
  
  
  
	
   
   startup
   
   ();
  
  
  
  
	
   
   rt_printf
   
   (
   
   "waiting Ctrl+C
   
   \n
   
   "
   
   );
  
  
  
  
	
   
   wait_for_ctrl_c
   
   ();
  
  
  
  
	
   
   rt_printf
   
   (
   
   "closing...
   
   \n
   
   "
   
   );
  
  
  
  
	
   
   cleanup
   
   ();
  
  
  
  
	
   
   return 
   
   0
   
   ;
  
  
  
  

   
   }