home *** CD-ROM | disk | FTP | other *** search
/ Virtual Reality Homebrewer's Handbook / vr.iso / vr386 / logiptr.c < prev    next >
C/C++ Source or Header  |  1996-03-19  |  13KB  |  581 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. // Bernie originally worked on this code, but couldn't get it
  8. // to Work.  Dave reworked it to get it functioning, then
  9. // added 80% of the code  you see here.
  10.  
  11.  
  12. // DO NOT COMPILE TO 386 CODE!  BC 3.1 has a bug for interrupt handlers
  13. // in that mode!
  14.  
  15. // BY USING THE pushad/popad fix in this file, it will work.
  16.  
  17. /*
  18.  This code is part of the VR-386 project, created by Dave Stampe.
  19.  VR-386 is a desendent of REND386, created by Dave Stampe and
  20.  Bernie Roehl.  Almost all the code has been rewritten by Dave
  21.  Stampre for VR-386.
  22.  
  23.  Copyright (c) 1994 by Dave Stampe:
  24.  May be freely used to write software for release into the public domain
  25.  or for educational use; all commercial endeavours MUST contact Dave Stampe
  26.  (dstampe@psych.toronto.edu) for permission to incorporate any part of
  27.  this software or source code into their products!  Usually there is no
  28.  charge for under 50-100 items for low-cost or shareware products, and terms
  29.  are reasonable.  Any royalties are used for development, so equipment is
  30.  often acceptable payment.
  31.  
  32.  ATTRIBUTION:  If you use any part of this source code or the libraries
  33.  in your projects, you must give attribution to VR-386 and Dave Stampe,
  34.  and any other authors in your documentation, source code, and at startup
  35.  of your program.  Let's keep the freeware ball rolling!
  36.  
  37.  DEVELOPMENT: VR-386 is a effort to develop the process started by
  38.  REND386, improving programmer access by rewriting the code and supplying
  39.  a standard API.  If you write improvements, add new functions rather
  40.  than rewriting current functions.  This will make it possible to
  41.  include you improved code in the next API release.  YOU can help advance
  42.  VR-386.  Comments on the API are welcome.
  43.  
  44.  CONTACT: dstampe@psych.toronto.edu
  45. */
  46.  
  47.  
  48.  
  49. #pragma inline   // for pushad, popad
  50.  
  51. #include <stdlib.h>
  52. #include <dos.h>
  53. #include <bios.h>
  54. #include <stdio.h>
  55. #include <conio.h>
  56. #include <signal.h>
  57. #include <mem.h>
  58.  
  59. #include "pointint.h"
  60. #include "vr_api.h"
  61. #include "pcdevice.h"
  62.  
  63. /******************* INTERRUPT CONSTANTS  ****************/
  64.  
  65.  
  66. #define PIC_CMD     0x20     /* constants for PIC EOI command */
  67. #define NONSPEC_EOI 0x20
  68. #define PIC_MASK    0x21
  69.  
  70. int logitech_port = 1;             /* 1: default of com2, 0 for com1 */
  71.  
  72. static int serirqnum = 3;
  73. static int serirqvect = 11;
  74. static int serirqmask = 0x08;
  75. static unsigned int address = 0x2F8;
  76.  
  77. #define DIVILOW (address + 0)    /* latch bit 1 */
  78. #define DIVIHIGH (address + 1)   /* latch bit 1 */
  79. #define INTENABLE (address + 1)  /* latch bit 0 */
  80. #define LINECTRL (address + 3)
  81. #define MODEMCTRL (address + 4)
  82. #define LINESTAT (address + 5)
  83. #define MODEMSTAT (address + 6)
  84.  
  85. #define BAUD_19200 6             /* baud rate constants */
  86. #define BAUD_1200  96
  87.  
  88. static void interrupt (*old_int)();     /* old timer ISR vector */
  89. static void interrupt serirq_handler();
  90.  
  91. static int schar = 0;
  92. static int errstat = 0;
  93.  
  94. #define PACKETSIZE 16
  95. static unsigned char buff1[20], buff2[20];
  96. static unsigned char *rcvbuff = buff1, *currbuff = buff2;
  97. static int indx = -1;
  98. static int received = 0;  /* positive if packet received */
  99.  
  100.  
  101. /***************** SERIAL SUPPORT *****************/
  102.  
  103.  
  104. static void set_baud(int divisor)
  105. {
  106.  int save = inportb(LINECTRL);
  107.  outportb(LINECTRL, 0x83);     /* enable latch */
  108.  outportb(DIVILOW, divisor);   /* baud rate: 6 = 19200, 96=1200 */
  109.  outportb(DIVIHIGH, 0);
  110.  outportb(LINECTRL, 3|save);        /* 8 bits, no parity, one stop bit */
  111. }
  112.  
  113.  
  114. static void pulse_rts()
  115. {
  116.  outportb(MODEMCTRL,0x00);     /* clear RTS */
  117.  delay(500);
  118.  outportb(MODEMCTRL,0x02);     /* set RTS   */
  119.  delay(500);
  120. }
  121.  
  122.  
  123. static void outch(unsigned char c)
  124. {
  125.  int i;
  126.  
  127. /* printf("%c\n", c); */
  128.  
  129.  while ((inportb(LINESTAT) & 0x40) == 0)
  130.  if (bioskey(1)) { getch(); return; }
  131.  outportb(address, c);
  132.  for (i = 0; i < 100; i++);        /* settling time for fast PCs */
  133. }
  134.  
  135.  
  136. static unsigned char inch(void)
  137. {
  138.  while ((inportb(LINESTAT) & 0x01) == 0)
  139.  if (bioskey(1)) { getch(); return 0; }
  140.  return inportb(address);
  141. }
  142.  
  143. static void flush_port()
  144. {
  145.  while ((inportb(LINESTAT) & 0x01))
  146.      inportb(address);
  147. }
  148.  
  149.  
  150. /***************** 6D MOUSE SUPPORT *****************/
  151.  
  152. static void reset_unit()
  153. {
  154.  outch('*');  outch('D');   /* reset */
  155.  delay(200);
  156.  outch('*');  outch('R');   /* reset */
  157.  delay(1000);
  158.  flush_port();
  159. }
  160.  
  161.  
  162. static int diagnostics()
  163. {
  164.  char c;
  165.  
  166. return 0;
  167.  outch('*');  outch(0x05);                  /* diagnostics */
  168.  if ((c = inch()) != 0xBF)
  169.   {
  170.    printf("Diagnostics failed -- first byte = 0x%02.2X\n", c);
  171.    return 2;
  172.   }
  173.  if ((c = inch()) != 0x3F)
  174.   {
  175.    printf("Diagnostics failed -- second byte = 0x%02.2X\n", c);
  176.    return 3;
  177.   }
  178.  return 0;
  179. }
  180.  
  181.             /* 0: enter hi-res mouse mode */
  182. static void head_mode(int hm)  /* 1: enter head track mode   */
  183. {
  184.  received = -1;        /* force discard of packets */
  185.  indx = -1;
  186.  
  187.  outch('*');           /* demand mode */
  188.  outch('D');
  189.  delay(150);
  190.  
  191.  outch('*');
  192.  if (hm)
  193.     outch('H');
  194.  else
  195.     outch('h');
  196.  delay(150);
  197.  
  198.  outch('*');
  199.  outch('I');           /* incremental reporting */
  200.  delay(150);
  201. }
  202.  
  203.  
  204. static int init_6D()
  205. {
  206.  set_baud(BAUD_19200);
  207.  pulse_rts();
  208.  
  209.  inportb(address);
  210.  if ((inportb(MODEMSTAT) & 0x10) == 0)
  211.   {
  212.    printf("No CTS\n");
  213.    exit(1);
  214.   }
  215.  
  216.  reset_unit();
  217.  if (diagnostics()) exit(1);
  218.  
  219.  outch('*');
  220.  outch('I');   /* incremental reporting */
  221.  delay(300);
  222.  return 1;
  223. }
  224.  
  225.  
  226.  
  227. /***************** 6D MOUSE READING *****************/
  228.  
  229.  
  230. static void poll_packet(char *buf)
  231. {
  232.  unsigned char c;
  233.  char *b = buf;
  234.  int i;
  235.  
  236.  for (i = 0; i < PACKETSIZE; i++)
  237.   {
  238. wstart:
  239.    while ((inportb(LINESTAT) & 0x01) == 0)  if (bioskey(1)) { getch(); return; }
  240.             /* data received */
  241.    c = inportb(address);
  242.    if (i == 0 && (c & 0x80) == 0) goto wstart;
  243.    *b++ = c;
  244.   }
  245. }
  246.  
  247.  
  248. static void dump_packet(char *buf)
  249. {
  250.  unsigned char buttons;
  251.  long x, y, z;
  252.  int rx, ry, rz;
  253.  
  254.  if (buf[0] & 0x40) printf("(FRI)");
  255.  if (buf[0] & 0x20) printf("(OUT)");
  256.  
  257.  buttons = buf[0] & 0x0F;
  258.  printf("%02.2X", buttons);
  259.  
  260.     /* sign extend if needed */
  261.  x = (buf[1] & 0x40) ? 0xFFE00000 : 0;
  262.  x |= (long)(buf[1] & 0x7f) << 14;
  263.  x |= (long)(buf[2] & 0x7f) << 7;
  264.  x |= (buf[3] & 0x7f);
  265.  
  266.  y = (buf[4] & 0x40) ? 0xFFE00000 : 0;
  267.  y |= (long)(buf[4] & 0x7f) << 14;
  268.  y |= (long)(buf[5] & 0x7f) << 7;
  269.  y |= (buf[6] & 0x7f);
  270.  
  271.  z = (buf[7] & 0x40) ? 0xFFE00000 : 0;
  272.  z |= (long)(buf[7] & 0x7f) << 14;
  273.  z |= (long)(buf[8] & 0x7f) << 7;
  274.  z |= (buf[9] & 0x7f);
  275.  
  276.  printf("%f,%f,%f", ((float) x) / 1000.0, ((float) y) / 1000.0, ((float) z) / 1000.0);
  277.  
  278.  rx  = (buf[10] & 0x7f) << 7;
  279.  rx += (buf[11] & 0x7f);
  280.  
  281.  ry  = (buf[12] & 0x7f) << 7;
  282.  ry += (buf[13] & 0x7f);
  283.  
  284.  rz  = (buf[14] & 0x7f) << 7;
  285.  rz += (buf[15] & 0x7f);
  286.  
  287.  printf("\t%f,%f,%f\n", ((float) rx) / 40.0, ((float) ry) / 40.0, ((float) rz) / 40.0);
  288. }
  289.  
  290.  
  291. /***************** INTERRUPT ROUTINES *****************/
  292.  
  293.  
  294.  
  295. static void reset_serirq()
  296. {                                /* RESET PC TO NORMAL */
  297.  disable();
  298.  
  299.  outportb(PIC_MASK,(inportb(PIC_MASK)|serirqmask)); /* mask serial IRQ */
  300.  
  301.  outportb(INTENABLE,0);
  302.  outportb(MODEMCTRL,0);               /* disable data available int */
  303.  inportb(address);
  304.  
  305.  setvect(serirqvect, old_int);        /* reset ISR vector */
  306.  outportb(PIC_CMD, NONSPEC_EOI);      /* reset any pending interrupts */
  307.  
  308.  enable();
  309. }
  310.  
  311.  
  312. static void init_serirq()                /* SET UP IRQ3 (COM2) */
  313. {
  314.  
  315.  old_int = getvect(serirqvect);       /* save old ISR vector */
  316.  set_baud(BAUD_19200);
  317.  
  318.  disable();
  319.  outportb(MODEMCTRL,0x0b);               /* enable data available int */
  320.  outportb(INTENABLE,0x05);
  321.  
  322.  setvect(serirqvect, serirq_handler);       /* new ISR handler */
  323.  outportb(PIC_MASK,(inportb(PIC_MASK)&(!serirqmask)));   /* unmask serial IRQ */
  324.  outportb(PIC_CMD, NONSPEC_EOI);    /* clear pending interrupts */
  325.  
  326.  enable();
  327. }
  328.  
  329.  
  330.  
  331. /****************** READ POINTS FROM TRACKER ****************/
  332.  
  333. static void interrupt serirq_handler()      /* COM2 input ISR */
  334. {
  335.  unsigned char c;
  336.  
  337.  asm {
  338.     .386
  339.     pushad
  340.      }
  341.  
  342.  disable();
  343. ++received;  /* DEBUG ONLY */
  344.  if (inportb(LINESTAT) & 0x01) /* data received */
  345.   {
  346.    c = inportb(address);
  347.    if (c & 0x80) indx = 0;
  348.    if (indx >= 0) rcvbuff[indx++] = c;
  349.    if (indx >= 16)
  350.     {
  351.      unsigned char *t;         /* buffer ready! */
  352.  
  353.      t = currbuff;
  354.      currbuff = rcvbuff;
  355.      rcvbuff = t;
  356.      indx = -1;
  357.      received++;
  358.     }
  359.   }
  360.  else
  361.   {
  362.    if ((inportb(LINESTAT) & 0x0E)!=0)    /* correct overrun errors */
  363.     {
  364.      inportb(address);
  365.      inportb(address);
  366.      inportb(address);
  367.     }
  368.   }
  369.  outportb(INTENABLE, 5);
  370.  outportb(PIC_CMD, NONSPEC_EOI);    /* reset int at PIC */
  371.  enable();
  372.  
  373.  asm {
  374.      popad;
  375.      }
  376. }
  377.  
  378.  
  379. static int copy_int_packet(char *dest)
  380. {
  381.  disable();
  382.  if (received < 1)
  383.   {
  384.    enable();
  385.    return 0;
  386.   }
  387.  received = 0;
  388.  memcpy(dest, rcvbuff, PACKETSIZE);
  389.  enable();
  390.  return 1;
  391. }
  392.  
  393.  
  394. static long rx_6d=0, ry_6d=0, rz_6d=0;
  395. static long x_6d=0, y_6d=0, z_6d=0;
  396. static int buttons_6d=0;
  397. static int status_6d= -1;
  398.  
  399. /* returns status_6d:    */
  400. /* -1 : no data read yet */
  401. /* 40h: out of range     */
  402. /* 20h: fringe of range  */
  403. /* 1  : no new data      */
  404. /* 0  : new data         */
  405.  
  406. static int get_6D_data()
  407. {
  408.  int but, i;
  409.  char buf[PACKETSIZE];
  410.  
  411.  i = copy_int_packet(buf);
  412.  if (i == 0)
  413.   {
  414.    status_6d |= 1;
  415.    return status_6d;
  416.   }
  417.  
  418.  status_6d  = buf[0] & 0x60;
  419.  buttons_6d = buf[0] & 0x0F;
  420.  
  421.     /* sign extend if needed */
  422.  x_6d = (buf[1] & 0x40) ? 0xFFE00000 : 0;
  423.  x_6d |= (long)(buf[1] & 0x7f) << 14;
  424.  x_6d |= (long)(buf[2] & 0x7f) << 7;
  425.  x_6d |= (buf[3] & 0x7f);
  426.  
  427.  y_6d = (buf[4] & 0x40) ? 0xFFE00000 : 0;
  428.  y_6d |= (long)(buf[4] & 0x7f) << 14;
  429.  y_6d |= (long)(buf[5] & 0x7f) << 7;
  430.  y_6d |= (buf[6] & 0x7f);
  431.  
  432.  z_6d = (buf[7] & 0x40) ? 0xFFE00000 : 0;
  433.  z_6d |= (long)(buf[7] & 0x7f) << 14;
  434.  z_6d |= (long)(buf[8] & 0x7f) << 7;
  435.  z_6d |= (buf[9] & 0x7f);
  436.  
  437.  rx_6d  = (buf[10] & 0x7f) << 7;
  438.  rx_6d += (buf[11] & 0x7f);
  439.  
  440.  ry_6d  = (buf[12] & 0x7f) << 7;
  441.  ry_6d += (buf[13] & 0x7f);
  442.  
  443.  rz_6d  = (buf[14] & 0x7f) << 7;
  444.  rz_6d += (buf[15] & 0x7f);
  445.  
  446.  return status_6d;
  447. }
  448.  
  449.  
  450.  
  451. /*****************/
  452.  
  453. static int tbuf[20];      /* working copy buffer */
  454.  
  455. static logitech_init()
  456. {
  457.  if (logitech_port)
  458.   {
  459.    serirqnum = 3;           /* COM2 select */
  460.    serirqvect = 11;
  461.    serirqmask = 0x08;
  462.    address = 0x2f8;
  463.   }
  464.  else
  465.   {
  466.    serirqnum = 4;           /* COM1 select */
  467.    serirqvect = 12;
  468.    serirqmask = 0x10;
  469.    address = 0x3f8;
  470.   }
  471.  
  472.  init_6D();
  473.  head_mode(1);
  474.  
  475.  init_serirq();   /* assume pointer shutdown will handle */
  476. /* ldtest(0); */
  477. }
  478.  
  479.  
  480. // REAL RES IS 0.5 DEGREE!
  481.  
  482. static p_dump(int i)
  483. {
  484.  char c[100];
  485.  
  486.  sprintf(c,"%d: %d %.1f,%.1f,%.1f %.1f,%.1f,%.1f", i, status_6d,
  487.  ((float) x_6d) / 40.0, ((float) y_6d) / 40.0, ((float) z_6d) / 40.0,
  488.  ((float) rx_6d) / 40.0, ((float) ry_6d) / 40.0, ((float) rz_6d) / 40.0);
  489.  
  490.  popmsg(c);
  491. }
  492.  
  493.  
  494. static ldtest(int i)              /* debug: interrupt report */
  495. {
  496.  int t;
  497.  while (!kbhit())
  498.   {
  499.    if (!get_6D_data(tbuf)) p_dump(i);
  500.   }
  501.  getch();
  502. }
  503.  
  504. static ddtest(int i)
  505. {
  506.  poll_packet(tbuf);
  507.  p_dump(i);
  508. }
  509.  
  510. static idtest(int i)
  511. {
  512.  get_6D_data(tbuf);
  513.  p_dump(i);
  514. }
  515.  
  516.  
  517.  
  518. /***************** LOGITECH POINTER DRIVER *************/
  519.  
  520. static pconfig logi_pconfig = {
  521.     1640L, 1640L, 1640L,            /* position res: mm/tick in <16.16>  */
  522.     1048575L, 1048575L, 1048575L,   /* xyz ranges:                */
  523.     -1048575L, -1048575L, -1048575L,
  524.     1640L, 1640L, 1640L,        /* rotation res: <ticks per */
  525.     7200L, 7200L, 7200L,
  526.     -7200L, -7200L, -7200L,         /* rotation range      */
  527.     320, 200, /* mouse emulation limits (writable) */
  528.     P_HASX | P_HASY | P_HASZ | P_HASRX | P_HASRY | P_HASRZ | P_HASB1 | P_HASB2 | P_HASB3,    /* databits  */
  529.     P_POINTER, 0, 0,                 /* modes, nullkey, flexnum           */
  530.     40, 50, 25,                     /* delay, idelay, reads/sec          */
  531.     P_IS6D | P_IS6H | P_IS3D,       /* uses  */
  532.     "Logitech Red Baron Head Tracker"
  533. };
  534.  
  535.  
  536. pconfig *logitech_driver(int op, POINTER *p, int mode)
  537. {
  538.     int type = FP_OFF(p);         /* way to get integer arg. */
  539.     int ft, fi, fm, fp;
  540.  
  541.     switch(op)
  542.     {
  543.     case DRIVER_CMD:
  544.     case DRIVER_RESET:
  545.         break;
  546.  
  547.     case DRIVER_INIT:
  548.         logitech_init();
  549.         break;
  550.  
  551.     case DRIVER_READ:        /* head pointer (6DP) read */
  552.         if (mode == P_POINTER)
  553.          {
  554.           if (get_6D_data(tbuf))
  555.            {
  556.            /*        popmsg("none"); */
  557.             return NULL;
  558.            }
  559. ///  DEBUG: DUMP DATA      p_dump(9);
  560.           p->x = -x_6d;
  561.           p->z = y_6d;
  562.           p->y = -z_6d;
  563.           p->rx = rx_6d<<16;
  564.           p->rz = -ry_6d<<16;
  565.           p->ry = rz_6d<<16;
  566.           p->buttons = buttons_6d;
  567.           p->keys = 0;
  568.           p->gesture = -1 ;
  569.          }
  570.         break;
  571.  
  572.     case DRIVER_CHECK:
  573.         break;
  574.     case DRIVER_QUIT:
  575.         reset_serirq();
  576.         break;
  577.     }
  578.     return &logi_pconfig;
  579. }
  580.  
  581.