joystick.c

linux 下svgalib编的一个界面程序示例

   if (__svgalib_driver_report)
     printf("svgalib, joystick %d: Setting correction to: %s\n", joydev, corr_name[JS_CORR_NONE]);
   
   if (-1 == ioctl(joydesc[joydev].fd, JSIOCSCORR, &corr)) 
     {
   	if (__svgalib_driver_report)
	  printf("svgalib, joystick %d: error setting correction\n", joydev);
	return -1;
     }
   
   
   for (i=0; i<NUM_POS; i++) 
     {
	do 
	  {
	     while(js.buttons)
	       wait_for_event(joydesc[joydev].fd, &js);

	     sprintf(msg, "Position all joystick axes at their %s positions and push any button.\n"
	     	"Move them slightly around then, still holding the button.\n", pos_name[i]);
	     jo(msg);

	     while (!js.buttons) 
	       {
		  msg[0] = 0;
		  for(j=0; j < joydesc[joydev].joydata->axes; j++)
		    sprintf(strchr(msg, 0), "Axis %d:%5d ", j, js.axis[j]);
		  strcat(msg, "\r");
	     	  jo(msg);
		  
		  wait_for_event(joydesc[joydev].fd, &js);
	       }
	     jo("\nHold ... ");
	     for (j = 0; j < joydesc[joydev].joydata->axes; j++) 
	       {
		  corda[j].cmin[i] = js.axis[j];
		  corda[j].cmax[i] = 0;
	       }
	     t = js.time;
	     while (js.time < t+2000 && js.buttons) 
	       {
		  for (j = 0; j < joydesc[joydev].joydata->axes; j++) 
		    {
		       if (js.axis[j] < corda[j].cmin[i]) 
			 corda[j].cmin[i] = js.axis[j];
		       if (js.axis[j] > corda[j].cmax[i]) 
			 corda[j].cmax[i] = js.axis[j];
		    }
		wait_for_event(joydesc[joydev].fd, &js);
	       }
	     if (js.time < t+2000) {
 		jo("released too soon. Try again:\n\n");
	     }
	  } while (js.time < t+2000);
	jo("OK.\n");
     }
   
   for (j = 0; j < joydesc[joydev].joydata->axes; j++) 
     {
	int temp;
	
	/* This used to use fp math, not any more */
	corr[j].coef[0] = corda[j].cmin[1];
	corr[j].coef[1] = corda[j].cmax[1];
	corr[j].coef[2] = (temp=32768 / (corda[j].cmin[1] - 
				      corda[j].cmax[0]))*16384;
	corr[j].coef[3] = -temp * corda[j].cmax[0];
	corr[j].coef[4] = (temp=32767 / (corda[j].cmin[2] - 
				      corda[j].cmax[1]))*16384;
	corr[j].coef[5] = -temp * corda[j].cmin[1] + 32768;
	
	
	corr[j].type = JS_CORR_BROKEN;
	corr[j].prec = 10;
     }

   if (__svgalib_driver_report)
     {
	printf("svgalib, joystick %d: Setting correction to: %s\n", joydev,
		 corr_name[(int)corr[0].type]);
	for (i = 0; i < joydesc[joydev].joydata->axes; i++) 
	  {
	     printf("  Coefs for axis %d:", i);
	     for(j = 0; j < corr_coef_num[(int)corr[i].type]; j++) 
	       {
		  printf(" %d", corr[i].coef[j]); 
		  if (j < corr_coef_num[(int)corr[i].type] - 1) putchar(',');
	       }
	     puts("");
	  }
	puts("");
     }
   
   if (-1 == ioctl(joydesc[joydev].fd, JSIOCSCORR, &corr)) 
     {
   	if (__svgalib_driver_report)
	  printf("svgalib, joystick %d: error setting correction\n", joydev);
	return -1;
     }
   
   return joydesc[joydev].fd;
}

int joystick_update(void) {
   int i, retval = 0, status, mask;

   for (i = 0, mask = 1; i < NUM_JOYSTICKS; i++, mask <<= 1) {
	if ((joydesc[i].fd >= 0) && joydesc[i].joydata) {
	    do {
		status = joydesc[i].joydata->update(i);
		if (status & 1)
		    retval |= mask;
	    } while(status & 2);
	}
   }
   return retval & 1;
}

/* temporarily release joystick devices.. */
void __joystick_flip_vc(int acquire) {
   int i;

   for (i = 0; i < NUM_JOYSTICKS; i++)
	if (joydesc[i].joydata && joydesc[i].joydata->flip_vc)
	    if (joydesc[i].joydata->flip_vc(i, acquire)) {
		puts("svgalib: Fatal, cannot reopen joystick after VC switch.\n");
		exit(1);
	    }
}

static void joy_stdout(const char *msg)
{
   fputs(msg, stdout);
   fflush(stdout);
}

int joystick_init(int joydev, __joystick_output jo)
{
   if ((joydev < 0) || (joydev >= NUM_JOYSTICKS))
	return -1;
   
   if (jo == JOY_CALIB_STDOUT) 
	jo = joy_stdout;

   joystick_close(joydev);

   if ((joydesc[joydev].fd = open(__joystick_devicenames[joydev] ?
	 __joystick_devicenames[joydev] : defnames[joydev], O_RDONLY|O_NONBLOCK)) < 0)
     {
	return -1;
     }
   else
     {
	u_int32_t version;

	if (-1 == ioctl(joydesc[joydev].fd, JSIOCGVERSION, &version)) 
	  {
	     if (__svgalib_driver_report)
	       printf("svgalib: Initializing joystick %d: assuming old 0.x driver protocol\n", joydev);
	     
	     /* Old 0.x protocol */
	     joystick_initdefhandler(joydev, 4, 2);
	     joydesc[joydev].joydata->update = joystick_upd0;
	     joydesc[joydev].joydata->flip_vc = joystick_flp0;
	     joydesc[joydev].joydata->p.prot0.b = 0;
	     joydesc[joydev].joydata->p.prot0.x = 0;
	     joydesc[joydev].joydata->p.prot0.y = 0;
	     joydesc[joydev].joydata->p.prot0.xdif = 0x80;
	     joydesc[joydev].joydata->p.prot0.ydif = 0x80;
	     
	     if (__svgalib_driver_report)
	       printf("   assuming %d axes and %d buttons\n",
		      joydesc[joydev].joydata->axes, joydesc[joydev].joydata->buttons);
	     
	     /* Now calibrate ... */
	     if (jo)
		jscal0(joydev, jo);

	     if (-1 == ioctl(joydesc[joydev].fd, JS_GET_CAL,
			 &joydesc[joydev].joydata->p.prot0.caldata)) {
	       if (__svgalib_driver_report)
		  printf("svgalib, joystick%d: Failed to read calibration data\n", joydev);
	       joystick_close(joydev);
	       return -1;
	     }
	     return 1;
	  }
	else
	  {
	     char axes, buttons;

	     if (__svgalib_driver_report)
	       printf("svgalib: Initializing joystick %d: driver version %x.%x.%x (new protocol)\n",
		      joydev,
		      (version & 0xff0000) >> 16, (version & 0xff00)>> 8,
		      version & 0xff);
	     
	     if (-1 == ioctl(joydesc[joydev].fd, JSIOCGAXES, &axes)) 
	       {
	          if (__svgalib_driver_report)
		    printf("svgalib, joystick%d: error getting number of axes\n", joydev);
	          joystick_close(joydev);
		  return -1;
	       }
	     
	     if (-1 == ioctl(joydesc[joydev].fd, JSIOCGBUTTONS, &buttons)) 
	       {
	          if (__svgalib_driver_report)
		    printf("svgalib, joystick%d: error getting number of buttons\n", joydev);
	          joystick_close(joydev);
		  return -1;
	       }

	     if (__svgalib_driver_report)
	       printf("joystick %d has %d axes and %d buttons\n", joydev, axes, buttons);
	     joystick_initdefhandler(joydev, buttons, axes);
	     
	     /* Now calibrate ... */
#if 0 /* assume joystick is calibrated */
	     if (jo)
		jscal1(joydev, jo);

	     if (-1 == ioctl(joydesc[joydev].fd, JSIOCGCORR,
			&joydesc[joydev].joydata->p.prot1.corrdata)) {
	       if (__svgalib_driver_report)
		  printf("svgalib, joystick%d: Failed to read calibration data\n", joydev);
	       joystick_close(joydev);
	       return -1;
	     }
#endif
	     joydesc[joydev].joydata->update = joystick_upd1;
	     joydesc[joydev].joydata->flip_vc = joystick_flp1;
	     return 1;
	  }
     }  
   return -1;
}

void joystick_close(int joydev)
{
   if (joydev >= NUM_JOYSTICKS)
	return;
   if (joydev < 0) {
	for (joydev = 0; joydev < NUM_JOYSTICKS; joydev++)
	     joystick_close(joydev);
	return;
   }
   if (joydesc[joydev].fd >= 0)
	close(joydesc[joydev].fd);
   joydesc[joydev].fd = -1;
   if (joydesc[joydev].joydata) {
	if (joydesc[joydev].joydata->dh_buttons)
	    free(joydesc[joydev].joydata->dh_buttons);
	if (joydesc[joydev].joydata->dh_axes)
	    free(joydesc[joydev].joydata->dh_axes);
	free(joydesc[joydev].joydata);
	joydesc[joydev].joydata = NULL;
   }
}

char joystick_getnumaxes(int joydev)
{
   if ((joydev >= 0) && (joydev < NUM_JOYSTICKS) && joydesc[joydev].joydata)
	return joydesc[joydev].joydata->axes;
   return 0;
}

char joystick_getnumbuttons(int joydev)
{
   if ((joydev >= 0) && (joydev < NUM_JOYSTICKS) && joydesc[joydev].joydata)
	return joydesc[joydev].joydata->buttons;
   return 0;
}

static void joystick_initdefhandler(int joydev, int buttons, int axes)
{
   joydesc[joydev].joydata = malloc(sizeof(joydat_t));
   if (!joydesc[joydev].joydata) {
     nomem:
	printf("svgalib: Fatal, out of memory\n");
	exit(1);
   }
   joydesc[joydev].joydata->axes = axes;
   joydesc[joydev].joydata->buttons = buttons;
   joydesc[joydev].joydata->dh_buttons = malloc(buttons * sizeof(int));
   joydesc[joydev].joydata->dh_axes = malloc(axes * sizeof(int));
  
   if ((!joydesc[joydev].joydata->dh_buttons) ||
	(!joydesc[joydev].joydata->dh_axes))
	    goto nomem; 
  
   memset(joydesc[joydev].joydata->dh_buttons, 0, buttons * sizeof(int));
   memset(joydesc[joydev].joydata->dh_axes, 0, axes * sizeof(int));
   joydesc[joydev].joydata->handler = joystick_defaulthandler;
   joydesc[joydev].joydata->update = joystick_updx;
   joydesc[joydev].joydata->flip_vc = NULL;
}

static void joystick_defaulthandler(int event, int num, char val, int joydev)
{
#ifdef DEBUG
   printf("%d: %d %d %d\n", joydev, event, num, val);
#endif
   switch (event)
     {
      case JOY_EVENTAXIS:
	joydesc[joydev].joydata->dh_axes[num]=val;
	break;

      case JOY_EVENTBUTTONDOWN:
	joydesc[joydev].joydata->dh_buttons[num]=1;
	break;

      case JOY_EVENTBUTTONUP:
	joydesc[joydev].joydata->dh_buttons[num]=0;
	break;	
     }
}

void joystick_sethandler(int joydev, __joystick_handler jh)
{
   if (joydev >= NUM_JOYSTICKS)
	return;
   if (joydev < 0) {
	for (joydev = 0; joydev < NUM_JOYSTICKS; joydev++)
	     joystick_sethandler(joydev, jh);
	return;
   }
   if (joydesc[joydev].joydata)
	joydesc[joydev].joydata->handler=jh;
}

void joystick_setdefaulthandler(int joydev)
{
   joystick_sethandler(joydev, joystick_defaulthandler);
}

char joystick_getaxis(int joydev, int a)
{
   if ((joydev >= 0) && (joydev < NUM_JOYSTICKS) && joydesc[joydev].joydata &&
		(a >= 0) && (a < joydesc[joydev].joydata->axes))
	return joydesc[joydev].joydata->dh_axes[a];
   return 0;
}

char joystick_getbutton(int joydev, int b)
{
   if ((joydev >= 0) && (joydev < NUM_JOYSTICKS) && joydesc[joydev].joydata &&
		(b >= 0) && (b < joydesc[joydev].joydata->buttons))
	return joydesc[joydev].joydata->dh_buttons[b];
   return 0;
}

#endif