Listing 6 (lidar_graph.c)

/* ---
  lidar_graph task.  This program processes
  the raw frame buffer.  Frame buffers are received
  through the graph q, when processing is finished,
  the frame buffer is placed on the available queue.
  
  For the sample program, processing and graphing
  are replaced by a call to delay which uses
  DELAY_TIME cpu time.
--- */

#include <stdio.h>
#include <signal.h>
#include "lidar.h"

#define DELAY_TIME 0.3

main ()
{
   int     terminate = 0;
   int     graph_qid, avail_qid;
   char    *fb_shm;
   int     alive_sem;
   message mptr;
   
   /* --- set process priority --- */
   setpriority (PRIO_PROCESS, 0, LIDAR_GRAPH);
   
   /* --- open message queues --- */
   graph_qid = msgget (GRAPHICS_Q, 0666 | IPC_CREAT);
   avail_qid = msgget (AVAILABLE_Q, 0666 | IPC_CREAT);
   
   /* --- create alive semaphore --- */
   alive_sem = sem_get (ALIVE_KEY, 0);
   
   /* -- create shared memory buffers -- */
   fb_shm = shmat (shmget (FB_KEY, FRAME_SIZE * N_FRAMES,
                0666 | IPC_CREAT), 0, 0);
   
   /* --- tell the world we're open for business --- */
   sem_signal (alive_sem);
   
   /* --- while lidar program running --- */
   while (!terminate) {
      /* --- wait for the next buffer --- */
      msgrcv (graph_qid, &mptr, MSG_SIZE, ANYTYPE, NOFLAGS);
      /* --- check for termination --- */
      if (mptr.key == QUIT)
         terminate =1;
      else
         delay (DELAY_TIME);
      
      /* --- place buffer on the availaible queue --- */
      msgsnd (avail_qid, &mptr, MSG_SIZE, IPC_NOWAIT);
      }
   
   /* -- release memory -- */
   shmdt (fb_shm);
}

int cpu_hog_func ();
int take_cpu_time = 1;
#define SETTIMER(t,p) \
     t.it_value.tv_sec = (int)delta; \
     t.it_value.tv_usec = (p - (int)p) * 1000000; \
     setitimer (ITIMER_PROF, &t, 0);

/* --- delay and grab cpu time --- */
delay (delta)
double delta;
{
   struct itimerval t;
   
   /* --- signal handler for profiler interrupt --- */
   signal (SIGPROF, cpu_hog_func);
   
   /* --- turn on timer --- */
   SETTIMER(t,delta);
   
   /* --- loop until stop condition --- */
   while (take_cpu_time)
      /* do nothing */;
   
   /* --- reset loop condition --- */
   take_cpu_time++;
   
   /* --- turn off timer --- */
   SETTIMER(t,0);
}

cpu_hog_func ()
{
   take_cpu_time--;
}