Logo Search packages:      
Sourcecode: joystick version File versions  Download package

jscal.c

/*
 * jscal.c  Version 1.2
 *
 * Copyright (c) 1997-199 Vojtech Pavlik
 *
 * Sponsored by SuSE
 */

/*
 * This is a joystick calibration program for Linux joystick driver.
 */

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301 USA.
 */

#include <sys/ioctl.h>
#include <sys/time.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <math.h>
#include <getopt.h>
#include <string.h>
#include <fcntl.h>
#include <stdlib.h>

#include <asm/param.h>
#include <linux/joystick.h>

#define PIT_HZ 1193180L

#define NUM_POS 3
#define MAX_CORR 1

const char *pos_name[] = {"minimum", "center", "maximum"};
const char *corr_name[] = {"none (raw)", "broken line"};
const char corr_coef_num[] = {0,4};

00053 struct correction_data {
      int cmin[NUM_POS];
      int cmax[NUM_POS];
};

int fd;
struct js_corr corr[ABS_MAX + 1];
__u8 axmap[ABS_MAX + 1];
__u8 axmap2[ABS_MAX + 1];
__u16 buttonmap[(KEY_MAX - BTN_MISC + 1)];
char axes, buttons, fuzz;
int version;
struct correction_data corda[ABS_MAX + 1];

00067 struct js_info {
      int buttons;
      int axis[ABS_MAX + 1];
      } js;

void print_position(int i, int a)
{
      printf("Axis %d: %8d\r", i, a);
      fflush(stdout);

}

int get_time(void)
{
      struct timeval tv;

      gettimeofday(&tv, NULL);

      return tv.tv_sec * 1000 + tv.tv_usec / 1000;
}

void wait_for_event(int d, struct js_info *s)
{
      struct js_event ev;
      struct timeval tv;
      char buf;
      fd_set set;

      tv.tv_sec = 0;
      tv.tv_usec = 100000;

      FD_ZERO(&set);
      FD_SET(0, &set);
      FD_SET(d, &set);

      if (select(d+1, &set, NULL, NULL, &tv)) {

            if (FD_ISSET(d, &set)) {
                  if (read(d, &ev, sizeof(struct js_event)) == sizeof(struct js_event))
                  switch (ev.type & ~JS_EVENT_INIT) {
                        case JS_EVENT_AXIS:
                              s->axis[ev.number] = ev.value; break;
                        case JS_EVENT_BUTTON:
                              s->buttons = (s->buttons & ~(1 << ev.number)) | (ev.value << ev.number);
                  }
            }

            if (FD_ISSET(0, &set)) {
                  read(0, &buf, 1);
                  s->buttons |= (1 << 31);
            }

      } else {
            s->buttons &= ~(1 << 31);
      }
}

void putcs(char *s)
{
      int i;
      putchar('\r');
      for (i = 0; i < 78; i++) putchar(' ');
      putchar('\r');
      fputs(s, stdout);
      fflush(stdout);
}

int solve_broken(int *results, struct correction_data inputs)
{
      double a, b, c, d;

      a = inputs.cmin[1];
      b = inputs.cmax[1];
      c = 32767.0 / (inputs.cmin[1] - inputs.cmax[0]);
      d = 32767.0 / (inputs.cmin[2] - inputs.cmax[1]);

      results[0] = rint(a);
      results[1] = rint(b);
      results[2] = rint(c*16384.0);
      results[3] = rint(d*16384.0);

      return 1;
}

void help(void)
{
      putchar('\n');
      puts("Usage: jscal <device>");
      putchar('\n');
      puts("  -c             --calibrate         Calibrate the joystick");
      puts("  -h             --help              Display this help");
      puts("  -s <x,y,z...>  --set-correction    Sets correction to specified values");
      puts("  -t             --test-center       Tests if joystick is corectly calibrated");
      puts("                                       returns 0 on success, see the jscal");
      puts("                                       manpage for error values");
      puts("  -V             --version           Prints the version numbers");
      puts("  -p             --print-correction  Prints the current settings as a jscal");
      puts("                                       command line");
      puts("  -q             --print-mappings    Print the current axis and button");
  puts("                                       mappings as a jscal command line");
      puts("  -u <n_of_axes,axmap1,axmap2,...,");
  puts("      n_of_buttons,btnmap1,btnmap2,");
  puts("      ...>       --set-mappings      Sets axis and button mappings to the");
  puts("                                        specified values");
      putchar('\n');
}

void print_info()
{
      int i,j;

      if (ioctl(fd, JSIOCGAXES, &axes) < 0) {
            perror("jscal: error getting axes");
            exit(1);
      }
      if (ioctl(fd, JSIOCGBUTTONS, &buttons) < 0) {
            perror("jscal: error getting buttons");
            exit(1);
      }
      if (ioctl(fd, JSIOCGCORR, &corr) < 0) {
            perror("jscal: error getting correction");
            exit(1);
      }

      printf("Joystick has %d axes and %d buttons.\n", axes, buttons);
      for (i = 0; i < axes; i++) {
            printf("Correction for axis %d is %s, precision is %d.\n",
                  i, corr_name[(int)corr[i].type], corr[i].prec);
            if (corr_coef_num[(int)corr[i].type]) {
                  printf("Coeficients are:");
                  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(',');
                  }
            putchar('\n');
            }
      }
      putchar('\n');
}

void calibrate()
{
      int i, j, t, b;
      int axis, pos;

      for (i=0; i<ABS_MAX + 1; i++) {
            corr[i].type = JS_CORR_NONE;
            corr[i].prec = 0;
      }

      if (ioctl(fd, JSIOCSCORR, &corr) < 0) {
            perror("jscal: error setting correction");
            exit(1);
      }

      {

            int i;
            int amax[ABS_MAX + 1], amin[ABS_MAX + 1];

            puts("Calibrating precision: wait and don't touch the joystick.");

            wait_for_event(fd, &js);
            t = get_time();
            while (get_time() < t+50) wait_for_event(fd, &js);

            wait_for_event(fd, &js);
            t = get_time();
            for(i=0; i < axes; i++)
                  amin[i] = amax[i] = js.axis[i];

            do {
                  wait_for_event(fd, &js);
                  for(i=0; i < axes; i++) {
                        if (amin[i] > js.axis[i]) {
                              amin[i] = js.axis[i];
                              t = get_time();
                        }
                        if (amax[i] < js.axis[i]) {
                              amax[i] = js.axis[i];
                              t = get_time();
                        }
                        printf("Axis %d:%5d,%5d ", i, amin[i], amax[i]);
                  }
                  printf("\r");
                  fflush(stdout);
            } while (get_time() < t+4000);

            printf("Done. Precision is:                                             \n");

            for (i=0; i < axes; i++) {
                  corr[i].prec = amax[i] - amin[i];
                  printf("Axis: %d: %5d\n", i, corr[i].prec);
            }

            puts("");

      }


      b = js.buttons;

      for (axis = 0; axis < axes; axis++)
            for (pos = 0; pos < NUM_POS; pos++) {
                  while(b ^ js.buttons) wait_for_event(fd, &js);
                  printf("Move axis %d to %s position and push any button.\n", axis, pos_name[pos]);

                  while (!(b ^ js.buttons)) {
                        print_position(axis, js.axis[axis]);
                        wait_for_event(fd, &js);
                  }

                  putcs("Hold ... ");

                  corda[axis].cmin[pos] = js.axis[axis];
                  corda[axis].cmax[pos] = js.axis[axis];

                  t = get_time();

                  while (get_time() < t + 2000 && (b ^ js.buttons)) {
                        if (js.axis[axis] < corda[axis].cmin[pos]) {
                              corda[axis].cmin[pos] = js.axis[axis];
                              t = get_time();
                        }
                        if (js.axis[axis] > corda[axis].cmax[pos]) {
                              corda[axis].cmax[pos] = js.axis[axis];
                              t = get_time();
                        }
                        wait_for_event(fd, &js);
                  }
                  puts("OK.");
            }

      puts("");

      for (j = 0; j < axes; j++) {
            solve_broken(corr[j].coef, corda[j]);
            corr[j].type = JS_CORR_BROKEN;
      }

      puts("Setting correction to:");
      for (i = 0; i < axes; i++) {
            printf("Correction for axis %d: %s, precision: %d.\n",
                  i, corr_name[(int)corr[i].type], corr[i].prec);
            if (corr_coef_num[(int)corr[i].type]) {
                  printf("Coeficients:");
                  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(',');
                  }
            putchar('\n');
            }
      }

      putchar('\n');

      if (ioctl(fd, JSIOCSCORR, &corr) < 0) {
            perror("jscal: error setting correction");
            exit(1);
      }
}

void print_version()
{
      printf("JsCal was compiled for driver version: %d.%d.%d\n", JS_VERSION >> 16,
            (JS_VERSION >> 8) & 0xff, JS_VERSION & 0xff);
      printf("Current running driver version: %d.%d.%d\n", version >> 16,
            (version >> 8) & 0xff, version & 0xff);
}

void print_mappings(char *devicename)
{
      int i;

      if (ioctl(fd, JSIOCGAXES, &axes) < 0) {
            perror("jscal: error getting axes");
            exit(1);
      }
      if (ioctl(fd, JSIOCGBUTTONS, &buttons) < 0) {
            perror("jscal: error getting buttons");
            exit(1);
      }
      if (ioctl(fd, JSIOCGAXMAP, &axmap) < 0) {
            perror("jscal: error getting axis map");
            exit(1);
      }
      if (ioctl(fd, JSIOCGBTNMAP, &buttonmap) < 0) {
              buttons=0;
      }

      printf("jscal -u %d", axes);
      for (i = 0; i < axes; i++)
  {
            printf( ",%d", axmap[i]);
      }

  printf(",%d", buttons);
      for (i = 0; i < buttons; i++)
  {
            printf( ",%d", buttonmap[i]);
      }

      printf(" %s\n",devicename);
}


void get_axmap2(void)
{
        if (ioctl(fd, JSIOCGAXMAP, &axmap2) < 0) {
            perror("jscal: error getting axis map");
            exit(1);
      }
}

/*
 * Remap the calibration data to fit the (potentially) new axis map.
 * axmap2 stores the original axis map, axmap the new one.
 */
void correct_axes(void)
{
        int axmes[ABS_MAX + 1];
        struct js_corr corr_tmp[ABS_MAX + 1];
        int i;
        int ax[axes];
      //Create remapping table
        for(i=0;i<axes;++i){
              axmes[(axmap2[i])]=i;
      }
      for(i=0;i<axes;++i){
              ax[i]=axmes[(axmap[i])];
      }
      //Read again current callibration settings
      if (ioctl(fd, JSIOCGCORR, &corr) < 0) {
            perror("jscal: error getting correction");
            exit(1);
      }
      //Remap callibration settings
      for (i = 0; i < axes; i++) {
              corr_tmp[i]=corr[(ax[i])];
      }
      if (ioctl(fd, JSIOCSCORR, &corr_tmp) < 0) {
            perror("jscal: error setting correction");
            exit(1);
      }

}

void print_settings(char *devicename)
{
      int i,j;

      if (ioctl(fd, JSIOCGAXES, &axes) < 0) {
            perror("jscal: error getting axes");
            exit(1);
      }
      if (ioctl(fd, JSIOCGBUTTONS, &buttons) < 0) {
            perror("jscal: error getting buttons");
            exit(1);
      }
      if (ioctl(fd, JSIOCGCORR, &corr) < 0) {
            perror("jscal: error getting correction");
            exit(1);
      }

      printf("jscal -s %d", axes);
      for (i = 0; i < axes; i++) {
            printf( ",%d,%d", corr[i].type, corr[i].prec);
            for (j = 0; j < corr_coef_num[(int)corr[i].type]; j++)
                  printf(",%d", corr[i].coef[j]);
      }
      printf(" %s\n",devicename);
}

// n axes                      n buttons
// 10,0,1,2,5,6,16,17,40,41,42:13,288,289,290,291,292,293,294,295,296,297,298,299,300
void set_mappings(char *p)
{
      int i;
      int axes_on_cl = 0;
      int btns_on_cl = 0;
  int axis_mapping = 0;
  int btn_mapping = 0;

      if (ioctl(fd, JSIOCGAXES, &axes) < 0) {
            perror("jscal: error getting axes");
            exit(1);
      }
      if (ioctl(fd, JSIOCGBUTTONS, &buttons) < 0) {
            perror("jscal: error getting buttons");
            exit(1);
      }

      if (axes > ABS_MAX + 1) axes = ABS_MAX + 1;

      if (!p) {
            fprintf(stderr, "jscal: missing argument for --set-mappings\n");
            exit(1);
      }

   //axes
      sscanf(p, "%d", &axes_on_cl);
      p = strstr(p, ",");

      if (axes_on_cl != axes) {
            fprintf(stderr, "jscal: joystick has %d axes and not %d as specified on command line\n", 
                  axes, axes_on_cl);
            exit(1);
      }


      for (i = 0; i < axes; i++)
  {
            if (!p) {
                  fprintf(stderr, "jscal: missing mapping for axis %d\n", i);
                  exit(1);
            }
            sscanf(++p, "%d", &axis_mapping);
            p = strstr(p, ",");


            if (axis_mapping > ABS_MAX + 1) {
                  fprintf(stderr, "jscal: invalid axis mapping for axis %d (max is %d)\n", i, ABS_MAX + 1);
                  exit(1);
            }
            axmap[i] = axis_mapping;
      }

  //buttons
      sscanf(++p, "%d", &btns_on_cl);
      p = strstr(p, ",");

      if ((btns_on_cl != buttons)&&(btns_on_cl!=0)) {
            fprintf(stderr, "jscal: joystick has %d buttons and not %d as specified on command line\n", 
                  buttons, btns_on_cl);
            exit(1);
      }


      for (i = 0; i < btns_on_cl; i++)
        {
            if (!p) {
                  fprintf(stderr, "jscal: missing mapping for button %d\n", i);
                  exit(1);
            }
            sscanf(++p, "%d", &btn_mapping);
            p = strstr(p, ",");


            if (btn_mapping > KEY_MAX) {
                  fprintf(stderr, "jscal: invalid button mapping for button %d (max is %d)\n", i, KEY_MAX);
                  exit(1);
            }
            if (btn_mapping < BTN_MISC) {
                  fprintf(stderr, "jscal: invalid button mapping for button %d (min is %d)\n", i, BTN_MISC);
                  exit(1);
            }
            buttonmap[i] = btn_mapping;
        }

      if (p) {
            fprintf(stderr, "jscal: too many values\n");
            exit(1);
      }

      // Save the current axis map
      get_axmap2();
      
      // Apply the new axis map
      if (ioctl(fd, JSIOCSAXMAP, &axmap) < 0) {
            perror("jscal: error setting axis map");
            exit(1);
      }

      // Move the calibration data accordingly
      correct_axes();

      if (btns_on_cl!=0){
            if (ioctl(fd, JSIOCSBTNMAP, &buttonmap) < 0) {
                   perror("jscal: error setting button map");
                     exit(1);
            }
       }
}

void set_correction(char *p)
{
      int i,j;
      int t = 0;

      if (ioctl(fd, JSIOCGAXES, &axes) < 0) {
            perror("jscal: error getting axes");
            exit(1);
      }

      if (axes > ABS_MAX + 1) axes = ABS_MAX + 1;

      if (!p) {
            fprintf(stderr, "jscal: missing number of axes\n");
            exit(1);
      }
      sscanf(p, "%d", &t);
      p = strstr(p, ",");

      if (t != axes) {
            fprintf(stderr, "jscal: joystick has different number of axes (%d) than specified in command line (%d)\n", 
                  axes, t);
            exit(1);
      }


      for (i = 0; i < axes; i++) {

            if (!p) {
                  fprintf(stderr, "jscal: missing correction type for axis %d\n", i);
                  exit(1);
            }
            sscanf(++p, "%d", &t);
            p = strstr(p, ",");


            if (t > MAX_CORR) {
                  fprintf(stderr, "jscal: unknown correction type for axis %d\n", i);
                  exit(1);
            }
            corr[i].type = t;

            if (!p) {
                  fprintf(stderr, "jscal: missing precision for axis %d\n", i);
                  exit(1);
            }
            sscanf(++p, "%d", &t);
            p = strstr(p, ",");

            corr[i].prec = t;

            for(j = 0; j < corr_coef_num[corr[i].type]; j++) {
                  if (!p) {
                        fprintf(stderr, "jscal: missing coefficient %d for axis %d\n", j, i);
                        exit(1);
                  }
                  sscanf(++p, "%d", (int*) &corr[i].coef[j]);
                  p = strstr(p, ",");
            }
      }

      if (p) {
            fprintf(stderr, "jscal: too many values\n");
            exit(1);
      }
      
      if (ioctl(fd, JSIOCSCORR, &corr) < 0) {
            perror("jscal: error setting correction");
            exit(1);
      }
}

void test_center()
{
      int i;
      struct js_event ev;

      if (ioctl(fd, JSIOCGAXES, &axes) < 0) {
            perror("jscal: error getting axes");
            exit(1);
      }

      if (ioctl(fd, JSIOCGBUTTONS, &buttons) < 0) {
            perror("jscal: error getting buttons");
            exit(1);
      }

      if (fcntl(fd, F_SETFL, O_NONBLOCK)) {
            perror("jscal: cannot set nonblocking mode");
            exit(1);
      }

      while (read(fd, &ev, sizeof(struct js_event)) == sizeof(struct js_event)) {
            switch (ev.type & ~JS_EVENT_INIT) {
                  case JS_EVENT_AXIS:
                        js.axis[ev.number] = ev.value; break;
                  case JS_EVENT_BUTTON:
                        js.buttons = (js.buttons & ~(1 << ev.number)) | (ev.value << ev.number);
            }
      }

      for (i = 0; i < axes; i++) if (js.axis[i]) {
            fprintf(stderr, "jscal: axes not calibrated\n");
            exit(2);
      }
      if (js.buttons) {
            fprintf(stderr, "jscal: buttons pressed\n");
            exit(3);
      }
}

int action = 0;

int main(int argc, char **argv)
{
      int option_index = 0;
      char *parameter = NULL;
      int t;

  // /usr/include/getopt.h
      static struct option long_options[] =
      {
            {"calibrate", no_argument, NULL, 'c'},
            {"help", no_argument, NULL, 'h'},
            {"set-correction", required_argument, NULL, 's'},
            {"set-mappings", required_argument, NULL, 'u'},
            {"test-center", no_argument, NULL, 't'},
            {"version", no_argument, NULL, 'V'},
            {"print-correction", no_argument, NULL, 'p'},
            {"print-mappings", no_argument, NULL, 'q'},
    {NULL, no_argument, NULL, 0 }
      };

      if (argc == 1) {
            help();
            exit(1);
      }

      do {
            t = getopt_long(argc, argv, "chpqu:s:vVt", long_options, &option_index);
            switch (t) {
                  case 'p':
                  case 'q':
                  case 's':
                  case 'u':
                  case 'c':
                  case 't':
                  case 'V':
                        if (action) {
                              fprintf(stderr, "jscal: more than one action specified\n");
                              exit(1);
                        } else {
                              action = t;
                              if ((parameter=optarg)) strcpy(parameter,optarg);
                        }
                        break;
                  case 'h':
                        help();
                        exit(0);
                  case 0:
                  case EOF:
                        break;
                  case ':':
                        fprintf(stderr, "jscal: missing parameter\n");
                        exit(1);
                  case '?':
                        fprintf(stderr, "jscal: unknown option\n");
                        exit(1);
                  default:
                        fprintf(stderr, "jscal: option parsing error\n");
            }
      } while (t != EOF);

      if (argc != optind + 1) {
            fprintf(stderr, "jscal: missing devicename\n");
            exit(1);
      }

      if ((fd = open(argv[argc - 1], O_RDONLY)) < 0) {
            perror("jscal: can't open joystick device");
            exit(1);
      }

      if (ioctl(fd, JSIOCGVERSION, &version) < 0) {
            perror("jscal: error getting version");
            exit(1);
      }
      if (version != JS_VERSION) {
            fprintf(stderr, "jscal: wrong version\n");
            print_version();
            exit(1);
      }

      switch (action) {
            case 0:
                  print_info();
                  break;
            case 'c':
                  print_info();
                  calibrate();
                  break;
            case 'p':
                  print_settings(argv[argc -1]);
                  break;
            case 'q':
                  print_mappings(argv[argc -1]);
                  break;
            case 's':
                  set_correction(parameter);
                  break;
            case 'u':
                  set_mappings(parameter);
                  break;
            case 't':
                  test_center();
                  break;
            case 'V':
                  print_version();
                  break;
            default:
                  fprintf(stderr, "jscal: this cannot happen\n");
                  exit(1);
      }

      close(fd);
      return 0;
}

Generated by  Doxygen 1.6.0   Back to index