Better joystick calibration

Contributed by Sergio Presa Fontan.

1 files changed, 103 insertions, 45 deletions

diff --git a/src/joy.c b/src/joy.c
index 31c8982..c1b433a 100644
--- a/src/joy.c
+++ b/src/joy.c
@@ -16,26 +16,44 @@
 #define AXISX   1
 #define AXISY   2
 
-#define TIMEOUT 16000
+#define TIMEOUT 9999
+#define CALIBRATING_LOOPS 10
 
-static int16_t j_up, j_down, j_left, j_right;
+static uint16_t j_up, j_down, j_left, j_right;
 
-static int16_t read_axis(uint8_t axis)
+static uint16_t read_axis(uint16_t *x, uint16_t *y)
 {
-    uint16_t i;
+    uint16_t i, value;
+    uint16_t mask = AXISX | AXISY;
+
+    *x = *y = 0;
 
     disable();
-    outportb(PORT, 0xff);
-    for (i = 0; i < TIMEOUT; i++)
-        if (!(inportb(PORT) & axis))
-            break;
+    outportb(PORT, 0);
+    for (i = 1; mask && i < TIMEOUT; i++)
+    {
+        value = inportb(PORT) ^ mask;
+        if (value & AXISX)
+        {
+            *x = i;
+            mask ^= AXISX;
+        }
+        if (value & AXISY)
+        {
+            *y = i;
+            mask ^= AXISY;
+        }
+    }
     enable();
 
     return i;
 }
 
-static uint8_t calibrate(int16_t *v, int16_t *h)
+static uint8_t calibrate_release(uint16_t *x, uint16_t *y)
 {
+    uint8_t i;
+    uint16_t xacc, yacc;
+
     while (1)
     {
         if (keys[KEY_ESC])
@@ -45,57 +63,99 @@ static uint8_t calibrate(int16_t *v, int16_t *h)
             return 0;
         }
 
-        *v = read_axis(AXISY);
-        *h = read_axis(AXISX);
+        xacc = yacc = 0;
+        for (i = 0; i < CALIBRATING_LOOPS; i++)
+        {
+            read_axis(x, y);
+            xacc += *x;
+            yacc += *y;
+        }
 
-        outportb(PORT, 0xff);
         if (!(inportb(PORT) & BUTTON1))
         {
             while (!(inportb(PORT) & BUTTON1))
                 wait_vsync();
+
+            *x = xacc / CALIBRATING_LOOPS;
+            *y = yacc / CALIBRATING_LOOPS;
+
             return 1;
         }
+        wait_vsync();
+    }
+}
 
+static uint8_t calibrate_circle(uint16_t joy_values[4])
+{
+    uint8_t i;
+    uint16_t x, y;
+
+    while (1)
+    {
+        if (keys[KEY_ESC])
+        {
+            while (keys[KEY_ESC])
+                wait_vsync();
+            return 0;
+        }
+
+        for (i = 0; i < CALIBRATING_LOOPS; i++)
+        {
+            read_axis(&x, &y);
+            // up
+            if (y < joy_values[0])
+                joy_values[0] = y;
+            // down
+            if (y > joy_values[1])
+                joy_values[1] = y;
+            // left
+            if (x < joy_values[2])
+                joy_values[2] = x;
+            // right
+            if (x > joy_values[3])
+                joy_values[3] = x;
+        }
+
+        if (!(inportb(PORT) & BUTTON1))
+        {
+            while (!(inportb(PORT) & BUTTON1))
+                wait_vsync();
+
+            return 1;
+        }
         wait_vsync();
     }
 }
 
 uint8_t joy_detect()
 {
-    if (read_axis(AXISX) == TIMEOUT)
+    uint16_t xcenter, ycenter;
+
+    // this read doesn't matter
+    if (read_axis(&xcenter, &ycenter) == TIMEOUT)
         return 0;
 
     printf("Joystick detected!\n");
 
-    int16_t cx, cy;
     printf("Please center the joystick and press BUTTON 1 (ESC to abort)\n");
-    if (!calibrate(&cy, &cx))
-        return 0;
-    printf("Please move UP + LEFT and press BUTTON 1 (ESC to abort)\n");
-    if (!calibrate(&j_up, &j_left))
-        return 0;
-    printf("Please move DOWN + RIGHT and press BUTTON 1 (ESC to abort)\n");
-    if (!calibrate(&j_down, &j_right))
+    if (!calibrate_release(&xcenter, &ycenter))
         return 0;
 
-#ifdef DEBUG
-    printf("Joystick reads: up=%04d, down=%04d, left=%04d, right=%04d\n",
-           j_up, j_down, j_left, j_right);
-#endif
+    // up, down, left, right
+    uint16_t joy_values[4] = { ycenter, ycenter, xcenter, xcenter };
 
-    j_up = cy - ((cy - j_up) / 2);
-    j_down = cy + ((j_down - cy) / 2);
-    j_left = cx - ((cx - j_left) / 2);
-    j_right = cx + ((j_right - cx) / 2);
+    printf("Please move the joystick in circles and then press BUTTON 1 (ESC to abort)\n");
+    if (!calibrate_circle(joy_values))
+        return 0;
 
-#ifdef DEBUG
-    printf("      Adjusted: up=%04d, down=%04d, left=%04d, right=%04d\n",
-           j_up, j_down, j_left, j_right);
+    printf("Please center the joystick and press BUTTON 1 (ESC to abort)\n");
+    if (!calibrate_release(&xcenter, &ycenter))
+        return 0;
 
-    printf("(press X to continue)\n");
-    while (!keys[KEY_X])
-        wait_vsync();
-#endif
+    j_up = ycenter - ((ycenter - joy_values[0]) / 2);
+    j_down = ycenter + ((joy_values[1] - ycenter) / 2);
+    j_left = xcenter - ((xcenter - joy_values[2]) / 2);
+    j_right = xcenter + ((joy_values[3] - xcenter) / 2);
 
     return 1;
 }
@@ -104,25 +164,23 @@ uint8_t joy_read()
 {
     uint8_t r = CTL_NONE;
     uint8_t b;
-    int16_t v;
 
-    outportb(PORT, 0xff);
     b = inportb(PORT);
     if (!(b & BUTTON1))
         r |= CTL_FIRE1;
     if (!(b & BUTTON2))
         r |= CTL_FIRE2;
 
-    v = read_axis(AXISY);
-    if (v < j_up)
+    uint16_t x, y;
+
+    read_axis(&x, &y);
+    if (y < j_up)
         r |= CTL_UP;
-    if (v > j_down)
+    if (y > j_down)
         r |= CTL_DOWN;
-
-    v = read_axis(AXISX);
-    if (v < j_left)
+    if (x < j_left)
         r |= CTL_LEFT;
-    if (v > j_right)
+    if (x > j_right)
         r |= CTL_RIGHT;
 
     return r;