mart/xenomai/ex11/ex11.c

   1 #include <stdio.h>
   2 #include <signal.h>
   3 #include <unistd.h>
   4 #include <sys/mman.h>
   5 #include <sys/io.h>
   6
   7 #include <native/task.h>
   8 #include <native/timer.h>
   9 #include <native/intr.h>
  10
  11 #include <rtdk.h>
  12
  13 #define LPT1IRQ 7
  14 #define TICKS 5
  15 #define MAXX 16
  16
  17 RT_TASK intrtask, drawtask;
  18 RTIME ticks[TICKS+1];
  19 RTIME tocks[TICKS+1];
  20
  21 unsigned char data[MAXX] = {
  22         1, 2, 4, 8, 16, 32, 64, 128, 256,
  23         1, 2, 4, 8, 16, 32, 64, 128, 256
  24 };
  25
  26 void add_tick(RTIME *t, RTIME tnew)
  27 {
  28         unsigned int i = 0;
  29         t[TICKS] = 0;
  30         for(i=TICKS-1; i>0; i--){
  31                 t[i] = t[i-1];
  32                 t[TICKS] += t[i];
  33         }
  34         t[0] = tnew;
  35         t[TICKS] = t[TICKS]/TICKS;
  36 }
  37
  38 void taskd(void *arg)
  39 {
  40         RTIME time = *(RTIME *)arg;
  41         RTIME step = time/(2*MAXX+2);
  42         unsigned int i;
  43
  44         //To the right
  45         outb(0x00, 0x378);
  46         rt_task_sleep(step*2);
  47         for(i=0; i<MAXX; i++){
  48                 outb(data[i], 0x378);
  49                 rt_task_sleep(step/2);
  50                 outb(0x00, 0x378);
  51                 rt_task_sleep(step/2);
  52         }
  53         rt_task_sleep(step);
  54         outb(0x00, 0x378);
  55
  56         //rt_printf("Every x has %.2fms\n", step/1000/1000.0);
  57 }
  58
  59 void taski(void *arg)
  60 {
  61         //Register as interrupt handler
  62         RT_INTR intr;
  63         rt_intr_create(&intr, NULL, LPT1IRQ, 0);
  64         rt_printf("Interrupt created\n");
  65
  66         //Initialize and declare the timer variables
  67         RTIME tick = rt_timer_read();
  68         RTIME tock;
  69         RTIME period;
  70         while(1){
  71                 tock = rt_timer_read();
  72                 add_tick(ticks, tock-tick);
  73
  74                 // Tocks waiting period
  75                 if(tocks[TICKS] < ticks[TICKS]){
  76                         // sleep until the arm is most left
  77                         rt_task_sleep(tocks[TICKS]/2);
  78                         rt_task_create(&drawtask, NULL, 0, 50, 0);
  79                         period = ticks[TICKS]+tocks[TICKS];
  80                         rt_task_start(&drawtask, &taskd, &period);
  81                         rt_printf("tocks is the short stretch\n");
  82                 }
  83                 rt_intr_wait(&intr, TM_INFINITE);
  84
  85                 //Tick waiting period
  86                 tick = rt_timer_read();
  87                 add_tick(tocks, tick-tock);
  88
  89                 if(ticks[TICKS] < tocks[TICKS]){
  90                         // sleep until the arm is most left
  91                         rt_task_sleep(ticks[TICKS]/2);
  92                         rt_task_create(&drawtask, NULL, 0, 50, 0);
  93                         period = ticks[TICKS]+tocks[TICKS];
  94                         rt_task_start(&drawtask, &taskd, &period);
  95                         rt_printf("ticks is the short stretch\n");
  96                 }
  97                 rt_intr_wait(&intr, TM_INFINITE);
  98                 //Print average
  99                 //rt_printf("avgtick: %lluns, avgtock: %lluns, avgticktock: %lluns\n",
 100                 //              ticks[TICKS], tocks[TICKS], (ticks[TICKS]+tocks[TICKS])/2);
 101         }
 102 }
 103
 104 int main(int argc, char* argv[])
 105 {
 106         rt_print_auto_init(1);
 107         mlockall(MCL_CURRENT | MCL_FUTURE);
 108
 109         // Get permission to write to parallel port
 110         ioperm(0x37A, 1, 1);
 111         ioperm(0x378, 1, 1);
 112         outb(inb(0x37A) | 0x10, 0x37A);
 113
 114         rt_task_create(&intrtask, NULL, 0, 50, 0);
 115         rt_task_start(&intrtask, &taski, 0);
 116
 117         pause();
 118 }