home *** CD-ROM | disk | FTP | other *** search
/ Stars of Shareware: Raytrace & Morphing / SOS-RAYTRACE.ISO / programm / source / devel5 / logiptr.c < prev    next >
Encoding:
C/C++ Source or Header  |  1993-04-23  |  10.9 KB  |  526 lines
  1. /*
  2.  LOGITECH 6D head tracker: overhead, no recenter
  3.  by Dave Stampe, April 1993
  4.  (c) 1993 Bernie Roehl, Dave Stampe
  5. */
  6.  
  7. #include <stdlib.h>
  8. #include <dos.h>
  9. #include <bios.h>
  10. #include <stdio.h>
  11. #include <conio.h>
  12. #include <signal.h>
  13. #include <mem.h>
  14.  
  15. #include "rend386.h"
  16. #include "userint.h"
  17. #include "pointer.h"
  18.  
  19. /******************* INTERRUPT CONSTANTS  ****************/
  20.  
  21.  
  22. #define PIC_CMD     0x20     /* constants for PIC EOI command */
  23. #define NONSPEC_EOI 0x20
  24. #define PIC_MASK    0x21
  25.  
  26. extern int logitech_port;             /* 1: default of com2, 0 for com1 */
  27.  
  28. static int serirqnum = 3;
  29. static int serirqvect = 11;
  30. static int serirqmask = 0x08;
  31. static unsigned int address = 0x2F8;
  32.  
  33. #define DIVILOW (address + 0)    /* latch bit 1 */
  34. #define DIVIHIGH (address + 1)   /* latch bit 1 */
  35. #define INTENABLE (address + 1)  /* latch bit 0 */
  36. #define LINECTRL (address + 3)
  37. #define MODEMCTRL (address + 4)
  38. #define LINESTAT (address + 5)
  39. #define MODEMSTAT (address + 6)
  40.  
  41. #define BAUD_19200 6             /* baud rate constants */
  42. #define BAUD_1200  96
  43.  
  44. static void interrupt (*old_int)();     /* old timer ISR vector */
  45. static void interrupt serirq_handler();
  46.  
  47. static int schar = 0;
  48. static int errstat = 0;
  49.  
  50. #define PACKETSIZE 16
  51. static unsigned char buff1[20], buff2[20];
  52. static unsigned char *rcvbuff = buff1, *currbuff = buff2;
  53. static int indx = -1;
  54. static int received = 0;  /* positive if packet received */
  55.  
  56.  
  57. /***************** SERIAL SUPPORT *****************/
  58.  
  59.  
  60. static void set_baud(int divisor)
  61. {
  62.  int save = inportb(LINECTRL);
  63.  outportb(LINECTRL, 0x83);     /* enable latch */
  64.  outportb(DIVILOW, divisor);   /* baud rate: 6 = 19200, 96=1200 */
  65.  outportb(DIVIHIGH, 0);
  66.  outportb(LINECTRL, 3|save);        /* 8 bits, no parity, one stop bit */
  67. }
  68.  
  69.  
  70. static void pulse_rts()
  71. {
  72.  outportb(MODEMCTRL,0x00);     /* clear RTS */
  73.  delay(500);
  74.  outportb(MODEMCTRL,0x02);     /* set RTS   */
  75.  delay(500);
  76. }
  77.  
  78.  
  79. static void outch(unsigned char c)
  80. {
  81.  int i;
  82.  
  83. /* printf("%c\n", c); */
  84.  
  85.  while ((inportb(LINESTAT) & 0x40) == 0)
  86.  if (bioskey(1)) { getch(); return; }
  87.  outportb(address, c);
  88.  for (i = 0; i < 100; i++);        /* settling time for fast PCs */
  89. }
  90.  
  91.  
  92. static unsigned char inch(void)
  93. {
  94.  while ((inportb(LINESTAT) & 0x01) == 0)
  95.  if (bioskey(1)) { getch(); return 0; }
  96.  return inportb(address);
  97. }
  98.  
  99. static void flush_port()
  100. {
  101.  while ((inportb(LINESTAT) & 0x01))
  102.      inportb(address);
  103. }
  104.  
  105.  
  106. /***************** 6D MOUSE SUPPORT *****************/
  107.  
  108. static void reset_unit()
  109. {
  110.  outch('*');  outch('D');   /* reset */
  111.  delay(200);
  112.  outch('*');  outch('R');   /* reset */
  113.  delay(1000);
  114.  flush_port();
  115. }
  116.  
  117.  
  118. static int diagnostics()
  119. {
  120.  char c;
  121.  
  122. return 0;
  123.  outch('*');  outch(0x05);                  /* diagnostics */
  124.  if ((c = inch()) != 0xBF)
  125.   {
  126.    printf("Diagnostics failed -- first byte = 0x%02.2X\n", c);
  127.    return 2;
  128.   }
  129.  if ((c = inch()) != 0x3F)
  130.   {
  131.    printf("Diagnostics failed -- second byte = 0x%02.2X\n", c);
  132.    return 3;
  133.   }
  134.  return 0;
  135. }
  136.  
  137.             /* 0: enter hi-res mouse mode */
  138. static void head_mode(int hm)  /* 1: enter head track mode   */
  139. {
  140.  received = -1;        /* force discard of packets */
  141.  indx = -1;
  142.  
  143.  outch('*');           /* demand mode */
  144.  outch('D');
  145.  delay(150);
  146.  
  147.  outch('*');
  148.  if (hm)
  149.     outch('H');
  150.  else
  151.     outch('h');
  152.  delay(150);
  153.  
  154.  outch('*');
  155.  outch('I');           /* incremental reporting */
  156.  delay(150);
  157. }
  158.  
  159.  
  160. static int init_6D()
  161. {
  162.  set_baud(BAUD_19200);
  163.  pulse_rts();
  164.  
  165.  inportb(address);
  166.  if ((inportb(MODEMSTAT) & 0x10) == 0)
  167.   {
  168.    printf("No CTS\n");
  169.    exit(1);
  170.   }
  171.  
  172.  reset_unit();
  173.  if (diagnostics()) exit(1);
  174.  
  175.  outch('*');
  176.  outch('I');   /* incremental reporting */
  177.  delay(300);
  178.  return 1;
  179. }
  180.  
  181.  
  182.  
  183. /***************** 6D MOUSE READING *****************/
  184.  
  185.  
  186. static void poll_packet(char *buf)
  187. {
  188.  unsigned char c;
  189.  char *b = buf;
  190.  int i;
  191.  
  192.  for (i = 0; i < PACKETSIZE; i++)
  193.   {
  194. wstart:
  195.    while ((inportb(LINESTAT) & 0x01) == 0)  if (bioskey(1)) { getch(); return; }
  196.             /* data received */
  197.    c = inportb(address);
  198.    if (i == 0 && (c & 0x80) == 0) goto wstart;
  199.    *b++ = c;
  200.   }
  201. }
  202.  
  203.  
  204. static void dump_packet(char *buf)
  205. {
  206.  unsigned char buttons;
  207.  long x, y, z;
  208.  int rx, ry, rz;
  209.  
  210.  if (buf[0] & 0x40) printf("(FRI)");
  211.  if (buf[0] & 0x20) printf("(OUT)");
  212.  
  213.  buttons = buf[0] & 0x0F;
  214.  printf("%02.2X", buttons);
  215.  
  216.     /* sign extend if needed */
  217.  x = (buf[1] & 0x40) ? 0xFFE00000 : 0;
  218.  x |= (long)(buf[1] & 0x7f) << 14;
  219.  x |= (long)(buf[2] & 0x7f) << 7;
  220.  x |= (buf[3] & 0x7f);
  221.  
  222.  y = (buf[4] & 0x40) ? 0xFFE00000 : 0;
  223.  y |= (long)(buf[4] & 0x7f) << 14;
  224.  y |= (long)(buf[5] & 0x7f) << 7;
  225.  y |= (buf[6] & 0x7f);
  226.  
  227.  z = (buf[7] & 0x40) ? 0xFFE00000 : 0;
  228.  z |= (long)(buf[7] & 0x7f) << 14;
  229.  z |= (long)(buf[8] & 0x7f) << 7;
  230.  z |= (buf[9] & 0x7f);
  231.  
  232.  printf("%f,%f,%f", ((float) x) / 1000.0, ((float) y) / 1000.0, ((float) z) / 1000.0);
  233.  
  234.  rx  = (buf[10] & 0x7f) << 7;
  235.  rx += (buf[11] & 0x7f);
  236.  
  237.  ry  = (buf[12] & 0x7f) << 7;
  238.  ry += (buf[13] & 0x7f);
  239.  
  240.  rz  = (buf[14] & 0x7f) << 7;
  241.  rz += (buf[15] & 0x7f);
  242.  
  243.  printf("\t%f,%f,%f\n", ((float) rx) / 40.0, ((float) ry) / 40.0, ((float) rz) / 40.0);
  244. }
  245.  
  246.  
  247. /***************** INTERRUPT ROUTINES *****************/
  248.  
  249.  
  250.  
  251. static void reset_serirq()
  252. {                                /* RESET PC TO NORMAL */
  253.  disable();
  254.  
  255.  outportb(PIC_MASK,(inportb(PIC_MASK)|serirqmask)); /* mask serial IRQ */
  256.  
  257.  outportb(INTENABLE,0);
  258.  outportb(MODEMCTRL,0);               /* disable data available int */
  259.  inportb(address);
  260.  
  261.  setvect(serirqvect, old_int);        /* reset ISR vector */
  262.  outportb(PIC_CMD, NONSPEC_EOI);      /* reset any pending interrupts */
  263.  
  264.  enable();
  265. }
  266.  
  267.  
  268. static void init_serirq()                /* SET UP IRQ3 (COM2) */
  269. {
  270.  
  271.  old_int = getvect(serirqvect);       /* save old ISR vector */
  272.  set_baud(BAUD_19200);
  273.  
  274.  disable();
  275.  outportb(MODEMCTRL,0x0b);               /* enable data available int */
  276.  outportb(INTENABLE,0x05);
  277.  
  278.  setvect(serirqvect, serirq_handler);       /* new ISR handler */
  279.  outportb(PIC_MASK,(inportb(PIC_MASK)&(!serirqmask)));   /* unmask serial IRQ */
  280.  outportb(PIC_CMD, NONSPEC_EOI);    /* clear pending interrupts */
  281.  
  282.  enable();
  283. }
  284.  
  285.  
  286.  
  287. /****************** READ POINTS FROM TRACKER ****************/
  288.  
  289. static void interrupt serirq_handler()      /* COM2 input ISR */
  290. {
  291.  unsigned char c;
  292.  
  293.  disable();
  294. ++received;  /* DEBUG ONLY */
  295.  if (inportb(LINESTAT) & 0x01) /* data received */
  296.   {
  297.    c = inportb(address);
  298.    if (c & 0x80) indx = 0;
  299.    if (indx >= 0) rcvbuff[indx++] = c;
  300.    if (indx >= 16)
  301.     {
  302.      unsigned char *t;         /* buffer ready! */
  303.  
  304.      t = currbuff;
  305.      currbuff = rcvbuff;
  306.      rcvbuff = t;
  307.      indx = -1;
  308.      received++;
  309.     }
  310.   }
  311.  else
  312.   {
  313.    if ((inportb(LINESTAT) & 0x0E)!=0)    /* correct overrun errors */
  314.     {
  315.      inportb(address);
  316.      inportb(address);
  317.      inportb(address);
  318.     }
  319.   }
  320.  outportb(INTENABLE, 5);
  321.  outportb(PIC_CMD, NONSPEC_EOI);    /* reset int at PIC */
  322.  enable();
  323. }
  324.  
  325.  
  326. static int copy_int_packet(char *dest)
  327. {
  328.  disable();
  329.  if (received < 1)
  330.   {
  331.    enable();
  332.    return 0;
  333.   }
  334.  received = 0;
  335.  memcpy(dest, rcvbuff, PACKETSIZE);
  336.  enable();
  337.  return 1;
  338. }
  339.  
  340.  
  341. static long rx_6d=0, ry_6d=0, rz_6d=0;
  342. static long x_6d=0, y_6d=0, z_6d=0;
  343. static int buttons_6d=0;
  344. static int status_6d= -1;
  345.  
  346. /* returns status_6d:    */
  347. /* -1 : no data read yet */
  348. /* 40h: out of range     */
  349. /* 20h: fringe of range  */
  350. /* 1  : no new data      */
  351. /* 0  : new data         */
  352.  
  353. static int get_6D_data()
  354. {
  355.  int but, i;
  356.  char buf[PACKETSIZE];
  357.  
  358.  i = copy_int_packet(buf);
  359.  if (i == 0)
  360.   {
  361.    status_6d |= 1;
  362.    return status_6d;
  363.   }
  364.  
  365.  status_6d  = buf[0] & 0x60;
  366.  buttons_6d = buf[0] & 0x0F;
  367.  
  368.     /* sign extend if needed */
  369.  x_6d = (buf[1] & 0x40) ? 0xFFE00000 : 0;
  370.  x_6d |= (long)(buf[1] & 0x7f) << 14;
  371.  x_6d |= (long)(buf[2] & 0x7f) << 7;
  372.  x_6d |= (buf[3] & 0x7f);
  373.  
  374.  y_6d = (buf[4] & 0x40) ? 0xFFE00000 : 0;
  375.  y_6d |= (long)(buf[4] & 0x7f) << 14;
  376.  y_6d |= (long)(buf[5] & 0x7f) << 7;
  377.  y_6d |= (buf[6] & 0x7f);
  378.  
  379.  z_6d = (buf[7] & 0x40) ? 0xFFE00000 : 0;
  380.  z_6d |= (long)(buf[7] & 0x7f) << 14;
  381.  z_6d |= (long)(buf[8] & 0x7f) << 7;
  382.  z_6d |= (buf[9] & 0x7f);
  383.  
  384.  rx_6d  = (buf[10] & 0x7f) << 7;
  385.  rx_6d += (buf[11] & 0x7f);
  386.  
  387.  ry_6d  = (buf[12] & 0x7f) << 7;
  388.  ry_6d += (buf[13] & 0x7f);
  389.  
  390.  rz_6d  = (buf[14] & 0x7f) << 7;
  391.  rz_6d += (buf[15] & 0x7f);
  392.  
  393.  return status_6d;
  394. }
  395.  
  396.  
  397.  
  398. /*****************/
  399.  
  400. static int tbuf[20];      /* working copy buffer */
  401.  
  402. static logitech_init()
  403. {
  404.  if (logitech_port)
  405.   {
  406.    serirqnum = 3;           /* COM2 select */
  407.    serirqvect = 11;
  408.    serirqmask = 0x08;
  409.    address = 0x2f8;
  410.   }
  411.  else
  412.   {
  413.    serirqnum = 4;           /* COM1 select */
  414.    serirqvect = 12;
  415.    serirqmask = 0x10;
  416.    address = 0x3f8;
  417.   }
  418.  
  419.  init_6D();
  420.  head_mode(1);
  421.  
  422.  init_serirq();   /* assume pointer shutdown will handle */
  423. /* ldtest(0); */
  424. }
  425.  
  426.  
  427. static p_dump(int i)
  428. {
  429.  char c[100];
  430.  
  431.  sprintf(c,"%d: %d %.0f,%.0f,%.0f %.0f,%.0f,%.0f", i, status_6d,
  432.  ((float) x_6d) / 40.0, ((float) y_6d) / 40.0, ((float) z_6d) / 40.0,
  433.  ((float) rx_6d) / 40.0, ((float) ry_6d) / 40.0, ((float) rz_6d) / 40.0);
  434.  
  435.  popmsg(c);
  436. }
  437.  
  438.  
  439. static ldtest(int i)              /* debug: interrupt report */
  440. {
  441.  int t;
  442.  while (!kbhit())
  443.   {
  444.    if (!get_6D_data(tbuf)) p_dump(i);
  445.   }
  446.  getch();
  447. }
  448.  
  449. static ddtest(int i)
  450. {
  451.  poll_packet(tbuf);
  452.  p_dump(i);
  453. }
  454.  
  455. static idtest(int i)
  456. {
  457.  get_6D_data(tbuf);
  458.  p_dump(i);
  459. }
  460.  
  461.  
  462.  
  463. /***************** LOGITECH POINTER DRIVER *************/
  464.  
  465. static pconfig logi_pconfig = {
  466.     1640L, 1640L, 1640L,            /* position res: mm/tick in <16.16>  */
  467.     1048575L, 1048575L, 1048575L,   /* xyz ranges:                */
  468.     -1048575L, -1048575L, -1048575L,
  469.     1640L, 1640L, 1640L,        /* rotation res: <ticks per */
  470.     7200L, 7200L, 7200L,
  471.     -7200L, -7200L, -7200L,         /* rotation range      */
  472.     320, 200, /* mouse emulation limits (writable) */
  473.     P_HASX | P_HASY | P_HASZ | P_HASRX | P_HASRY | P_HASRZ | P_HASB1 | P_HASB2 | P_HASB3,    /* databits  */
  474.     P_POINTER, 0, 0,                 /* modes, nullkey, flexnum           */
  475.     40, 50, 25,                     /* delay, idelay, reads/sec          */
  476.     P_IS6D | P_IS6H | P_IS3D,       /* uses  */
  477.     "Logitech Red Baron Head Tracker"
  478. };
  479.  
  480.  
  481. pconfig *logitech_driver(int op, POINTER *p, int mode)
  482. {
  483.     int type = FP_OFF(p);         /* way to get integer arg. */
  484.     int ft, fi, fm, fp;
  485.  
  486.     switch(op)
  487.     {
  488.     case DRIVER_CMD:
  489.     case DRIVER_RESET:
  490.         break;
  491.  
  492.     case DRIVER_INIT:
  493.         logitech_init();
  494.         break;
  495.  
  496.     case DRIVER_READ:        /* head pointer (6DP) read */
  497.         if (mode == P_POINTER)
  498.          {
  499.           if (get_6D_data(tbuf))
  500.            {
  501.            /*        popmsg("none"); */
  502.             return NULL;
  503.            }
  504.           /*      p_dump(9); */
  505.           p->x = -x_6d;
  506.           p->z = y_6d;
  507.           p->y = -z_6d;
  508.           p->rx = rx_6d<<16;
  509.           p->rz = -ry_6d<<16;
  510.           p->ry = rz_6d<<16;
  511.           p->buttons = buttons_6d;
  512.           p->keys = 0;
  513.           p->gesture = -1 ;
  514.          }
  515.         break;
  516.  
  517.     case DRIVER_CHECK:
  518.         break;
  519.     case DRIVER_QUIT:
  520.         reset_serirq();
  521.         break;
  522.     }
  523.     return &logi_pconfig;
  524. }
  525.  
  526.