tests/ts_calibrate.c

   1 /*
   2  *  tslib/tests/ts_calibrate.c
   3  *
   4  *  Copyright (C) 2001 Russell King.
   5  *
   6  * This file is placed under the GPL.  Please see the file
   7  * COPYING for more details.
   8  *
   9  * $Id: ts_calibrate.c,v 1.8 2004/10/19 22:01:27 dlowder Exp $
  10  *
  11  * Basic test program for touchscreen library.
  12  */
  13 #include "config.h"
  14
  15 #include <stdio.h>
  16 #include <stdlib.h>
  17 #include <signal.h>
  18 #include <string.h>
  19 #include <unistd.h>
  20 #include <sys/fcntl.h>
  21 #include <sys/ioctl.h>
  22 #include <sys/mman.h>
  23 #include <sys/time.h>
  24 #include <sys/stat.h>
  25 #include <linux/kd.h>
  26 #include <linux/vt.h>
  27 #include <linux/fb.h>
  28
  29 #include "tslib.h"
  30
  31 #include "fbutils.h"
  32 #include "testutils.h"
  33
  34 static int palette [] =
  35 {
  36         0x000000, 0xffe080, 0xffffff, 0xe0c0a0
  37 };
  38 #define NR_COLORS (sizeof (palette) / sizeof (palette [0]))
  39
  40 typedef struct {
  41         int x[5], xfb[5];
  42         int y[5], yfb[5];
  43         int a[7];
  44 } calibration;
  45
  46 static void sig(int sig)
  47 {
  48         close_framebuffer ();
  49         fflush (stderr);
  50         printf ("signal %d caught\n", sig);
  51         fflush (stdout);
  52         exit (1);
  53 }
  54
  55 int perform_calibration(calibration *cal) {
  56         int j;
  57         float n, x, y, x2, y2, xy, z, zx, zy;
  58         float det, a, b, c, e, f, i;
  59         float scaling = 65536.0;
  60
  61 // Get sums for matrix
  62         n = x = y = x2 = y2 = xy = 0;
  63         for(j=0;j<5;j++) {
  64                 n += 1.0;
  65                 x += (float)cal->x[j];
  66                 y += (float)cal->y[j];
  67                 x2 += (float)(cal->x[j]*cal->x[j]);
  68                 y2 += (float)(cal->y[j]*cal->y[j]);
  69                 xy += (float)(cal->x[j]*cal->y[j]);
  70         }
  71
  72 // Get determinant of matrix -- check if determinant is too small
  73         det = n*(x2*y2 - xy*xy) + x*(xy*y - x*y2) + y*(x*xy - y*x2);
  74         if(det < 0.1 && det > -0.1) {
  75                 printf("ts_calibrate: determinant is too small -- %f\n",det);
  76                 return 0;
  77         }
  78
  79 // Get elements of inverse matrix
  80         a = (x2*y2 - xy*xy)/det;
  81         b = (xy*y - x*y2)/det;
  82         c = (x*xy - y*x2)/det;
  83         e = (n*y2 - y*y)/det;
  84         f = (x*y - n*xy)/det;
  85         i = (n*x2 - x*x)/det;
  86
  87 // Get sums for x calibration
  88         z = zx = zy = 0;
  89         for(j=0;j<5;j++) {
  90                 z += (float)cal->xfb[j];
  91                 zx += (float)(cal->xfb[j]*cal->x[j]);
  92                 zy += (float)(cal->xfb[j]*cal->y[j]);
  93         }
  94
  95 // Now multiply out to get the calibration for framebuffer x coord
  96         cal->a[0] = (int)((a*z + b*zx + c*zy)*(scaling));
  97         cal->a[1] = (int)((b*z + e*zx + f*zy)*(scaling));
  98         cal->a[2] = (int)((c*z + f*zx + i*zy)*(scaling));
  99
 100         printf("%f %f %f\n",(a*z + b*zx + c*zy),
 101                                 (b*z + e*zx + f*zy),
 102                                 (c*z + f*zx + i*zy));
 103
 104 // Get sums for y calibration
 105         z = zx = zy = 0;
 106         for(j=0;j<5;j++) {
 107                 z += (float)cal->yfb[j];
 108                 zx += (float)(cal->yfb[j]*cal->x[j]);
 109                 zy += (float)(cal->yfb[j]*cal->y[j]);
 110         }
 111
 112 // Now multiply out to get the calibration for framebuffer y coord
 113         cal->a[3] = (int)((a*z + b*zx + c*zy)*(scaling));
 114         cal->a[4] = (int)((b*z + e*zx + f*zy)*(scaling));
 115         cal->a[5] = (int)((c*z + f*zx + i*zy)*(scaling));
 116
 117         printf("%f %f %f\n",(a*z + b*zx + c*zy),
 118                                 (b*z + e*zx + f*zy),
 119                                 (c*z + f*zx + i*zy));
 120
 121 // If we got here, we're OK, so assign scaling to a[6] and return
 122         cal->a[6] = (int)scaling;
 123         return 1;
 124 /*
 125 // This code was here originally to just insert default values
 126         for(j=0;j<7;j++) {
 127                 c->a[j]=0;
 128         }
 129         c->a[1] = c->a[5] = c->a[6] = 1;
 130         return 1;
 131 */
 132
 133 }
 134
 135 static void get_sample (struct tsdev *ts, calibration *cal,
 136                         int index, int x, int y, char *name)
 137 {
 138         static int last_x = -1, last_y;
 139
 140         if (last_x != -1) {
 141 #define NR_STEPS 10
 142                 int dx = ((x - last_x) << 16) / NR_STEPS;
 143                 int dy = ((y - last_y) << 16) / NR_STEPS;
 144                 int i;
 145                 last_x <<= 16;
 146                 last_y <<= 16;
 147                 for (i = 0; i < NR_STEPS; i++) {
 148                         put_cross (last_x >> 16, last_y >> 16, 2 | XORMODE);
 149                         usleep (1000);
 150                         put_cross (last_x >> 16, last_y >> 16, 2 | XORMODE);
 151                         last_x += dx;
 152                         last_y += dy;
 153                 }
 154         }
 155
 156         put_cross(x, y, 2 | XORMODE);
 157         getxy (ts, &cal->x [index], &cal->y [index]);
 158         put_cross(x, y, 2 | XORMODE);
 159
 160         last_x = cal->xfb [index] = x;
 161         last_y = cal->yfb [index] = y;
 162
 163         printf("%s : X = %4d Y = %4d\n", name, cal->x [index], cal->y [index]);
 164 }
 165
 166 static int clearbuf(struct tsdev *ts)
 167 {
 168         int fd = ts_fd(ts);
 169         fd_set fdset;
 170         struct timeval tv;
 171         int nfds;
 172         struct ts_sample sample;
 173
 174         while (1) {
 175                 FD_ZERO(&fdset);
 176                 FD_SET(fd, &fdset);
 177
 178                 tv.tv_sec = 0;
 179                 tv.tv_usec = 0;
 180
 181                 nfds = select(fd + 1, &fdset, NULL, NULL, &tv);
 182                 if (nfds == 0) break;
 183
 184                 if (ts_read_raw(ts, &sample, 1) < 0) {
 185                         perror("ts_read");
 186                         exit(1);
 187                 }
 188         }
 189 }
 190
 191 int main()
 192 {
 193         struct tsdev *ts;
 194         calibration cal;
 195         int cal_fd;
 196         char cal_buffer[256];
 197         char *tsdevice = NULL;
 198         char *calfile = NULL;
 199         unsigned int i;
 200
 201         signal(SIGSEGV, sig);
 202         signal(SIGINT, sig);
 203         signal(SIGTERM, sig);
 204
 205         if( (tsdevice = getenv("TSLIB_TSDEVICE")) != NULL ) {
 206                 ts = ts_open(tsdevice,0);
 207         } else {
 208                 if (!(ts = ts_open("/dev/input/event0", 0)))
 209                         ts = ts_open("/dev/touchscreen/ucb1x00", 0);
 210         }
 211
 212         if (!ts) {
 213                 perror("ts_open");
 214                 exit(1);
 215         }
 216         if (ts_config(ts)) {
 217                 perror("ts_config");
 218                 exit(1);
 219         }
 220
 221         if (open_framebuffer()) {
 222                 close_framebuffer();
 223                 exit(1);
 224         }
 225
 226         for (i = 0; i < NR_COLORS; i++)
 227                 setcolor (i, palette [i]);
 228
 229         put_string_center (xres / 2, yres / 4,
 230                            "TSLIB calibration utility", 1);
 231         put_string_center (xres / 2, yres / 4 + 20,
 232                            "Touch crosshair to calibrate", 2);
 233
 234         printf("xres = %d, yres = %d\n", xres, yres);
 235
 236         // Clear the buffer
 237         clearbuf(ts);
 238
 239         get_sample (ts, &cal, 0, 50,        50,        "Top left");
 240         clearbuf(ts);
 241         get_sample (ts, &cal, 1, xres - 50, 50,        "Top right");
 242         clearbuf(ts);
 243         get_sample (ts, &cal, 2, xres - 50, yres - 50, "Bot right");
 244         clearbuf(ts);
 245         get_sample (ts, &cal, 3, 50,        yres - 50, "Bot left");
 246         clearbuf(ts);
 247         get_sample (ts, &cal, 4, xres / 2,  yres / 2,  "Center");
 248
 249         if (perform_calibration (&cal)) {
 250                 printf ("Calibration constants: ");
 251                 for (i = 0; i < 7; i++) printf("%d ", cal.a [i]);
 252                 printf("\n");
 253                 if ((calfile = getenv("TSLIB_CALIBFILE")) != NULL) {
 254                         cal_fd = open (calfile, O_CREAT | O_RDWR,
 255                                        S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
 256                 } else {
 257                         cal_fd = open (TS_POINTERCAL, O_CREAT | O_RDWR,
 258                                        S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
 259                 }
 260                 sprintf (cal_buffer,"%d %d %d %d %d %d %d",
 261                          cal.a[1], cal.a[2], cal.a[0],
 262                          cal.a[4], cal.a[5], cal.a[3], cal.a[6]);
 263                 write (cal_fd, cal_buffer, strlen (cal_buffer) + 1);
 264                 close (cal_fd);
 265                 i = 0;
 266         } else {
 267                 printf("Calibration failed.\n");
 268                 i = -1;
 269         }
 270
 271         close_framebuffer();
 272         return i;
 273 }