BROKEN's Advanced Vehicle Laboratory

Real-Time Linuxによるリアルタイム処理とOpenGL によるシミュレータの構築 (C)

index section 1 section 2 section 3 section 4 section 5 section 6
bibrography appendix A appendix B appendix C

C. リアルタイム処理とアニメーションの組み合わせプログラムのサンプル

サンプルプログラムCのソースコードのダウンロード

C.1 control.h


/*
  Simple program for RTLinux & OpenGL   
  control.h
 */

#if !defined ___CONTROL_H
#define ___CONTROL_H

#define COMMAND_FIFO    1
#define DATA_FIFO       3

#define START_THREAD    1
#define STOP_THREAD     2
#define RESET_THREAD    3

#define dt              0.01

// RT thread control message
typedef struct {
  int     command;
  double  period;
} STMsg;

// data from RT thread
typedef struct {
  double sec;
  double dummy;
} STData;

#endif

C.2 timer_module.c


/*
  Simple program for RTLinux & OpenGL
  timer_module.c
 */

#include <linux/errno.h>
#include <rtl.h>
#include <time.h>
#include <rtl_sched.h>
#include <rtl_fifo.h>
#include "control.h"

#define in2real(i)      ( (double)(i) / NSECS_PER_SEC )
#define real2in(r)      ( (long long)(NSECS_PER_SEC * (r) ) )

pthread_t theThread;

void *thread_code(void *t)
{
  STMsg   Msg;
  STData  Data = {0, 5.0};
  
  while (1) {
    int ret;
    int err;

    ret = pthread_wait_np();    
    err = rtf_get (COMMAND_FIFO + 1, &Msg, sizeof(STMsg));
    
    if( err == sizeof(STMsg) ){
      
      switch (Msg.command) {
      case START_THREAD :
        pthread_make_periodic_np(pthread_self(), 
                                 gethrtime(), 
                                 real2in(Msg.period) );
        break;

      case STOP_THREAD :
        pthread_suspend_np(pthread_self());
        break;

      case RESET_THREAD :
        Data.sec = 0.0;
        rtf_put(DATA_FIFO, (char *)(&Data), sizeof(STData) );
        pthread_suspend_np(pthread_self());
        break;

      default:
        rtl_printf("RTL task: bad command\n");
        return 0;
      }
    }

    // Write something here to execute periodicaly.
    
    Data.sec += dt;
    rtf_put(DATA_FIFO, (char *)(&Data), sizeof(STData) );
  } // end of while
  
  return 0;
}

int my_handler(unsigned int fifo)
{
  STMsg Msg;
  int   err;
  
  while( (err = rtf_get(COMMAND_FIFO, &Msg, sizeof(STMsg)))==sizeof(STMsg) ){
    rtf_put (COMMAND_FIFO + 1, &Msg, sizeof(STMsg));
    pthread_wakeup_np (theThread);
  }
  
  if (err != 0) {
    return -EINVAL;
  }
  return 0;
}


int init_module(void)
{
  pthread_attr_t attr;
  struct sched_param sched_param;
  int ret;
  int junk = 0;

  rtf_destroy(1);
  rtf_destroy(2);
  rtf_destroy(3);

  rtf_create(1, 50);
  rtf_create(2, 50);
  rtf_create(3, sizeof(STData));  // 送るデータが小さいときはバッファも小さく
  
  pthread_attr_init (&attr);
  sched_param.sched_priority = 4;
  pthread_attr_setschedparam (&attr, &sched_param);
  ret = pthread_create (&theThread,  &attr, thread_code, (void *)junk );

  pthread_setfp_np (theThread, 1);
  
  rtf_create_handler(COMMAND_FIFO, &my_handler); 

  return 0;
}


void cleanup_module(void)
{
  rtf_destroy(1);
  rtf_destroy(2);
  rtf_destroy(3);
 
  pthread_delete_np (theThread);  
}

C.3 model.h


/*
  sample program for RTLinux & OpenGL
  watch_model.h
  */

#if !defined ___MODEL_H
#define ___MODEL_H

#include <GL/glut.h>

#define OFFSET          0.05

#define BODY_RADIUS     3.0
#define BODY_HEIGHT     1.5

#define PLATE_RADIUS    2.5
#define PLATE_HEIGHT    1.5

#define HAND_RADIUS     0.02
#define HAND_LENGTH     2.6

#define TICK_BOX_LENGTH 0.1
#define TICK_BOX_WIDTH  0.03
#define TICK_BOX_HEIGHT 0.1

void CalcNormal(GLdouble *p0, GLdouble *p1, GLdouble *p2, GLdouble *normal);
void MySolidBox(GLdouble width,GLdouble height, GLdouble depth);
void MySolidCylinder(GLdouble radius, GLdouble height);

void watchModel(double sec);
void watchBody(void);
void watchPlate(void);
void watchSecondHand(double sec);
void watchKnob(void);

#endif

C.4 model.c


/*
  sample program for OpenGL animaion
  watch.c
 */

#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include <math.h>

#include "model.h"

// 法線を求める
void
CalcNormal(GLdouble *p0, GLdouble *p1, GLdouble *p2, GLdouble *normal)
{
  register int i;
  GLdouble norm;
  GLdouble v0[3],v1[3];
  
  // 2本のベクトルをつくる
  for(i=0; i<3; i++){
    v0[i] = p2[i] - p1[i];
    v1[i] = p0[i] - p1[i];
  }

  // 得られたベクトルの外積を計算
  normal[0] = v0[1]*v1[2] - v0[2]*v1[1];
  normal[1] = v0[2]*v1[0] - v0[0]*v1[2];
  normal[2] = v0[0]*v1[1] - v0[1]*v1[0];

  // 法線ベクトルは必ず正規化しておく
  norm = sqrt(normal[0]*normal[0] 
              + normal[1]*normal[1] 
              + normal[2]*normal[2]);

  for(i=0; i<3; i++)
    normal[i]/=norm;

}

void 
MySolidBox(GLdouble width,GLdouble height, GLdouble depth)
{
  double w = width/2.0;
  double h = height/2.0;
  double d = depth/2.0;
  GLdouble nv[3];
  
  // 頂点の配列 
  double vrtx[8][3] = { { w, h, d}, {-w, h, d}, {-w,-h, d}, { w,-h, d},
                        { w, h,-d}, {-w, h,-d}, {-w,-h,-d}, { w,-h,-d} };
    
  // ポリゴンを用いて各面を作る 
  glBegin(GL_POLYGON);    
    CalcNormal(vrtx[0],vrtx[1],vrtx[2],nv);
    glNormal3dv(nv);
    glVertex3dv(vrtx[0]);
    glVertex3dv(vrtx[1]);
    glVertex3dv(vrtx[2]);
    glVertex3dv(vrtx[3]);
    glVertex3dv(vrtx[0]);
  glEnd();

  glBegin(GL_POLYGON);
    CalcNormal(vrtx[4],vrtx[5],vrtx[6],nv);
    glNormal3dv(nv);
    glVertex3dv(vrtx[4]);
    glVertex3dv(vrtx[5]);
    glVertex3dv(vrtx[6]);
    glVertex3dv(vrtx[7]);
    glVertex3dv(vrtx[4]);
  glEnd();

  glBegin(GL_POLYGON);    
    CalcNormal(vrtx[0],vrtx[1],vrtx[5],nv);
    glNormal3dv(nv);
    glVertex3dv(vrtx[0]);
    glVertex3dv(vrtx[1]);
    glVertex3dv(vrtx[5]);
    glVertex3dv(vrtx[4]);
    glVertex3dv(vrtx[0]);
  glEnd();

  glBegin(GL_POLYGON);    
    CalcNormal(vrtx[3],vrtx[2],vrtx[6],nv);
    glNormal3dv(nv);
    glVertex3dv(vrtx[3]);
    glVertex3dv(vrtx[2]);
    glVertex3dv(vrtx[6]);
    glVertex3dv(vrtx[7]);
    glVertex3dv(vrtx[3]);
  glEnd();

  glBegin(GL_POLYGON);    
    CalcNormal(vrtx[0],vrtx[4],vrtx[7],nv);
    glNormal3dv(nv);
    glVertex3dv(vrtx[0]);
    glVertex3dv(vrtx[4]);
    glVertex3dv(vrtx[7]);
    glVertex3dv(vrtx[3]);
    glVertex3dv(vrtx[0]);
  glEnd();

  glBegin(GL_POLYGON);    
    CalcNormal(vrtx[1],vrtx[2],vrtx[6],nv);
    glNormal3dv(nv);
    glVertex3dv(vrtx[1]);
    glVertex3dv(vrtx[2]);
    glVertex3dv(vrtx[6]);
    glVertex3dv(vrtx[5]);
    glVertex3dv(vrtx[1]);
  glEnd();

}

// 円筒もないので作ってしまう 
#define SPLIT 36  
void 
MySolidCylinder(GLdouble radius, GLdouble height)
{
  GLUquadricObj *qobj = gluNewQuadric();

  glPushMatrix();
  glTranslatef(0.0, 0.0, -height/2.0 );
  gluDisk( qobj, 0.0, radius, SPLIT, 1 );               // 底の円盤 
  gluCylinder( qobj, radius, radius, height, SPLIT, 1 ); // 側面 
  glTranslatef(0.0, 0.0,  height );
  gluDisk( qobj, 0.0, radius, SPLIT, 1 );               // 上の円盤 
  glPopMatrix();

  gluDeleteQuadric(qobj);
}


void watchModel(double sec)
{
  glPushMatrix();

  watchBody();
  watchKnob();

  glTranslatef(0.0, 0.0, OFFSET);
  watchPlate();

  glTranslatef(0.0, 0.0, OFFSET);
  watchSecondHand(sec);

  glPopMatrix();
}

void watchBody(void)
{
  GLfloat blue_ambient[] = { 0.1, 0.1, 2.0, 1.0 };
  GLfloat blue_diffuse[] = { 0.1, 0.1, 2.0, 1.0 };
  GLfloat blue_specular[]= { 0.3, 0.3, 0.4, 1.0 };
  GLfloat blue_shininess[]={ 10.0 };

  glPushMatrix();

  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, blue_ambient);
  glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, blue_diffuse);
  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, blue_specular);  
  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, blue_shininess);
  
  MySolidCylinder(BODY_RADIUS, BODY_HEIGHT);
 
  glPopMatrix();
}

void watchPlate(void)
{
  int i;

  GLfloat white_ambient[] = { 0.5, 0.5, 0.5, 1.0 };
  GLfloat white_diffuse[] = { 0.6, 0.6, 0.6, 1.0 };
  GLfloat white_specular[]= { 0.5, 0.5, 0.5, 1.0 };
  GLfloat white_shininess[]={ 1.0 };

  GLfloat black_ambient[] = { 0.1, 0.1, 0.1, 1.0 };
  GLfloat black_diffuse[] = { 0.1, 0.1, 0.1, 1.0 };
  GLfloat black_specular[]= { 0.8, 0.8, 0.8, 1.0 };
  GLfloat black_shininess[]={ 1.0 };

  glPushMatrix();

  // 文字版
  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, white_ambient);
  glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, white_diffuse);
  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, white_specular);  
  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, white_shininess);
  
  MySolidCylinder(PLATE_RADIUS, PLATE_HEIGHT);
 
  // 秒刻み
  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, black_ambient);
  glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, black_diffuse);
  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, black_specular);  
  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, black_shininess);
  
  glTranslatef(0.0, 0.0, BODY_HEIGHT/2.0);
  for(i=0; i<360; i++){
    glPushMatrix();
    // 文字版の刻みは 0 秒から描く
    glRotatef(90.0, 0.0, 0.0, 1.0 );
    glRotatef( (GLfloat)i, 0.0, 0.0, 1.0 );
    glTranslatef( PLATE_RADIUS - TICK_BOX_LENGTH/2.0, 0.0, 0.0 );

  
    if( i % 30 == 0 )
      MySolidBox( 2.0*TICK_BOX_LENGTH, 2.0*TICK_BOX_WIDTH, TICK_BOX_HEIGHT);
    else if( i % 3 == 0 )
      MySolidBox( TICK_BOX_LENGTH, TICK_BOX_WIDTH, TICK_BOX_HEIGHT);

    glPopMatrix();
  }

  glPopMatrix();
}


void watchSecondHand(double sec)
{

  GLfloat yellow_ambient[] = { 0.8, 0.8, 0.0, 1.0 };
  GLfloat yellow_diffuse[] = { 0.8, 0.8, 0.0, 1.0 };
  GLfloat yellow_specular[]= { 1.0, 1.0, 1.0, 1.0 };
  GLfloat yellow_shininess[]={ 30.0 };

  glPushMatrix();
  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, yellow_ambient);
  glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, yellow_diffuse);
  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, yellow_specular);  
  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, yellow_shininess);

  glTranslatef(0.0, 0.0, BODY_HEIGHT/2.0 );
  MySolidCylinder(PLATE_RADIUS/20.0, HAND_RADIUS);  

  glRotatef( -sec*360.0/60.0, 0.0, 0.0, 1.0);
  glRotatef(-90.0, 1.0, 0.0, 0.0);
  glTranslatef( 0.0, 0.0, HAND_LENGTH/2.0 - OFFSET*4 );
  MySolidCylinder(HAND_RADIUS, HAND_LENGTH);  

  glPopMatrix();
}

void watchKnob(void)
{
  GLfloat green_ambient[] = { 0.1, 0.8, 0.1, 1.0 };
  GLfloat green_diffuse[] = { 0.0, 0.8, 0.0, 1.0 };
  GLfloat green_specular[]= { 0.3, 0.3, 0.3, 1.0 };
  GLfloat green_shininess[]={ 10.0 };

  glPushMatrix();

  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, green_ambient);
  glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, green_diffuse);
  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, green_specular);  
  glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, green_shininess);
  
  glRotatef(90.0, 1.0, 0.0, 0.0 );
  glTranslatef( 0.0, 0.0, -BODY_RADIUS);

  MySolidCylinder(BODY_RADIUS/10.0, BODY_HEIGHT/2.0);
 
  glPopMatrix();

}

C.5 watch.c


/*
  sample program for RTLinux & OpenGL
  watch.c
 */

#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <fcntl.h>
#include <unistd.h>
#include <rtl_fifo.h>
#include <rtl_time.h>
#include <glut.h>

#include "control.h"
#include "model.h"

// 視点の構造体
typedef struct{

  double pos[3];
  double pan[3];
  double zoom;

}STCamera;

STCamera Camera;
STCamera orgCamera;
STMsg    Msg;
STData   Data;
int      ctl, fd0;

void display(void)
{
  glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
  glLoadIdentity();

  // 視点を配置 
  gluLookAt(Camera.pos[0], Camera.pos[1], Camera.pos[2],
            0.0, 0.0, 0.0,
            0.0, 1.0, 0.0);

   // 視点を移動
  glRotatef( Camera.pan[0], 1.0, 0.0, 0.0 );
  glRotatef( Camera.pan[1], 0.0, 1.0, 0.0 );
  glRotatef( Camera.pan[2], 0.0, 0.0, 1.0 );

  glPushMatrix();

  watchModel(Data.sec);

  glPopMatrix();

  glutSwapBuffers();
}

void timeUpdate(void)
{
  fd_set         rfds;
  struct timeval tv;
  int            retval;

  FD_ZERO(&rfds);
  FD_SET(fd0, &rfds);
  tv.tv_sec =  dt*3; 
  tv.tv_usec = 0;
  
  retval = select(FD_SETSIZE, &rfds, NULL, NULL, &tv);
  if (retval > 0) {
    if (FD_ISSET(fd0, &rfds)) {
      read(fd0, &Data, sizeof(STData) );
      printf("time : %4.2f  \n", Data.sec );
    }
  }
  
  glutPostRedisplay();
}


void init(void)
{
  GLfloat light_ambient[] = { 0.8, 0.8, 0.8, 1.0 };
  GLfloat light_diffuse[] = { 0.8, 0.8, 0.8, 1.0 };
  GLfloat light_specular[]= { 0.5, 0.5, 0.5, 1.0 };
  GLfloat light_position[]= { 5.0, 5.0, 5.0, 0,0 };

  // 背景は黒
  glClearColor( 0.0, 0.0, 0.0, 0.0 );
  glShadeModel( GL_SMOOTH );

  glLightfv( GL_LIGHT0, GL_AMBIENT, light_ambient );
  glLightfv( GL_LIGHT0, GL_DIFFUSE, light_diffuse );
  glLightfv( GL_LIGHT0, GL_SPECULAR,light_specular);
  glLightfv( GL_LIGHT0, GL_POSITION,light_position);

  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  glEnable(GL_DEPTH_TEST);

  // 視点の初期設定
  Camera.pos[2] = 7.0;
  orgCamera = Camera;

  // Open RT-FIFO
  if ((ctl = open("/dev/rtf1", O_WRONLY)) < 0) {
    fprintf(stderr, "Error opening /dev/rtf1\n");
    exit(1);
  }  
  if ((fd0 = open("/dev/rtf3", O_RDONLY)) < 0) {
    fprintf(stderr, "Error opening /dev/rtf3\n");
    exit(1);
  }

}

void reshape(int w, int h)
{
  glViewport(0, 0, (GLsizei)w, (GLsizei)h );
  
  glMatrixMode( GL_PROJECTION );
  glLoadIdentity();
  gluPerspective( 60.0, (GLfloat)w/(GLfloat)h, 0.05, 100.0 );

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

}


void mouse(int button, int state, int x, int y)
{

  switch ( button ) {
    
  case GLUT_LEFT_BUTTON :
    if(state == GLUT_DOWN){

      if( START_THREAD != Msg.command ){
        // start the thread  
        Msg.command = START_THREAD;
        Msg.period = dt;
        if (write(ctl, &Msg, sizeof(STMsg)) < 0) {
          fprintf(stderr, "Can't send a command to RT-thread\n");
          exit(1);
        }
        
      }
      else{
        // stop the thread
        Msg.command = STOP_THREAD;
        if (write(ctl, &Msg, sizeof(STMsg)) < 0) {
          fprintf(stderr, "Can't send a command to RT-thread\n");
          exit(1);
        }
        
      }
    } 
    break;
    
  case GLUT_MIDDLE_BUTTON :
    if(state == GLUT_DOWN){
      // reset and stop the thread
      Msg.command = RESET_THREAD;
      if (write(ctl, &Msg, sizeof(STMsg)) < 0) {
        fprintf(stderr, "Can't send a command to RT-thread\n");
        exit(1);
      }
      
    }
    break;
    
  case GLUT_RIGHT_BUTTON :
    if(state == GLUT_DOWN){
      // stop the thread
      Msg.command = STOP_THREAD;
      if (write(ctl, &Msg, sizeof(STMsg)) < 0) {
        fprintf(stderr, "Can't send a command to RT-thread\n");
        exit(1);
      }
      exit(0);
    }  
  
  default :
    break;
    
  }
  
}

#define DPOS    0.1
#define DPAN    3.0
#define DZOOM   0.1
void specialKey(int key, int x, int y)
{
  switch( key ){

  case GLUT_KEY_UP :
    Camera.pan[0] += DPAN;
    break;

  case GLUT_KEY_DOWN :
    Camera.pan[0] -= DPAN;
    break;

  case GLUT_KEY_LEFT :
    Camera.pan[1] += DPAN;
    break;

  case GLUT_KEY_RIGHT :
    Camera.pan[1] -= DPAN;
    break;

  case GLUT_KEY_PAGE_UP :
    Camera.pos[2] += DZOOM;
    break;

  case GLUT_KEY_PAGE_DOWN :
    Camera.pos[2] -= DZOOM;
    break;

  case GLUT_KEY_HOME :
    Camera = orgCamera;
    break;

  } 
}

int main(int argc, char** argv)
{
   glutInit(&argc, argv);
   glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH );
   glutInitWindowSize (350, 350); 
   glutInitWindowPosition (100, 100);
   glutCreateWindow (argv[0]);

   init ();

   glutDisplayFunc(display); 
   glutReshapeFunc(reshape); 
   glutMouseFunc(mouse);
   glutSpecialFunc(specialKey);
   glutIdleFunc( timeUpdate );

   glutMainLoop();

   return 0;   /* ANSI C requires main to return int. */
}

C.6 Makefile


#
#  Makefile for simple RTLinux & OpenGL program
#


include rtl.mk

INCLUDE += -I/usr/local/include
GLLIB    = -L/usr/local/lib -lglut -lGL -lGLU
XLIB     = -L/usr/X11/lib -lX11 -lXext -lXmu -lXt -lXi -lSM -lICE

all: timer_module.o watch


watch: watch.c model.c
        $(CC) ${INCLUDE} -O2 -Wall watch.c model.c -o watch  ${GLLIB} ${XLIB} -lm

timer_module.o: timer_module.c
        $(CC) ${INCLUDE} ${CFLAGS} -c timer_module.c -o timer_module_tmp.o 
        ld -r -static timer_module_tmp.o -o timer_module.o -L/usr/lib -lm 
        rm -f timer_module_tmp.o

#test, remove any modules, load new ones and run app
test: all
        (cd ${RTL_DIR}; ./rmrtl)
        @echo "Now insert the fifo and scheduler"
        @echo "Type <return> to continue"
        @read junk
        (cd ${RTL_DIR}; ./insrtl)
        @echo "Now start the real-time tasks  module"
        @echo "Type <return> to continue"
        @read junk
        @insmod timer_module.o
        @echo "Now start the application"
        @echo "Type <return> to continue"
        @read junk
        @./watch
        rmmod timer_module
        (cd ${RTL_DIR}; ./rmrtl)

clean:
        rm -f watch *.o *~

insrtl :
        (cd ${RTL_DIR}; ./insrtl)

rmrtl :
        (cd ${RTL_DIR}; ./rmrtl)