Merge pull request #13 from abhishek-kakkar/master

Touchscreen 3 point calibration support

include/touchpad.h

@@ -49,6 +49,9 @@
 /* Include the low level driver information */
 #include "touchpad_lld.h"
 
+/* For definitions of coord_t, we require gdisp.h */
+#include "gdisp.h"
+
 /*===========================================================================*/
 /* Type definitions                                                          */
 /*===========================================================================*/
@@ -57,10 +60,12 @@
  * @brief	Struct used for calibration
  */
 typedef struct cal_t {
-    float xm; 
+    float ax;
-    float ym; 
+    float bx;
-    float xn; 
+    float cx;
-    float yn; 
+    float ay;
+    float by;
+    float cy;
 } cal_t;
 
 /*===========================================================================*/
@@ -72,12 +77,12 @@ extern "C" {
 #endif
 
 void tpInit(const TOUCHPADDriver *tp);
-uint16_t tpReadX(void);
+coord_t tpReadX(void);
-uint16_t tpReadY(void);
+coord_t tpReadY(void);
 void tpCalibrate(void);
 
 #if TOUCHPAD_HAS_IRQ
-	uint8_t tpIRQ(void);
+	bool_t tpIRQ(void);
 #endif
 
 #if TOUCHPAD_HAS_PRESSURE

src/touchpad.c

@@ -54,11 +54,11 @@ static struct cal_t *cal;
 /* Driver local functions.                                                   */
 /*===========================================================================*/
 
-static uint16_t _tpReadRealX(void) {
+static coord_t _tpReadRealX(void) {
-    uint32_t results = 0;
+    int32_t results = 0;
-    uint16_t i, x;
+    int16_t i;
+    coord_t x;
 
-    /* Median filtering is already done in LLD */
     for(i = 0; i < CONVERSIONS; i++) {
         results += tp_lld_read_x();
     }
@@ -69,11 +69,11 @@ static uint16_t _tpReadRealX(void) {
     return x;
 }
 
-static uint16_t _tpReadRealY(void) {
+static coord_t _tpReadRealY(void) {
-    uint32_t results = 0;
+    int32_t results = 0;
-    uint16_t i, y;
+    int16_t i;
+    coord_t y;
 
-    /* Median filtering is already done in LLD */
     for(i = 0; i < CONVERSIONS; i++) {
         results += tp_lld_read_y();
     }
@@ -103,6 +103,50 @@ static void _tpDrawCross(uint16_t x, uint16_t y) {
     gdispDrawLine(x+15, y-15, x+15, y-7, RGB565CONVERT(184,158,131));    
 }
 
+static void _tpTransform(coord_t *x, coord_t *y) {
+  *x = (coord_t) (cal->ax * (*x) + cal->bx * (*y) + cal->cx);
+  *y = (coord_t) (cal->ay * (*x) + cal->by * (*y) + cal->cy);
+}
+
+static void _tpDo3PointCalibration(const coord_t (*cross)[2], coord_t (*points)[2],
+                                   cal_t *c)
+{
+  float dx, dx0, dx1, dx2, dy0, dy1, dy2;
+
+  /* Compute all the required determinants */
+  dx = ((float)(points[0][0] - points[2][0])) * ((float)(points[1][1] - points[2][1]))
+          - ((float)(points[1][0] - points[2][0])) * ((float)(points[0][1] - points[2][1]));
+
+  dx0 = ((float)(cross[0][0] - cross[2][0])) * ((float)(points[1][1] - points[2][1]))
+          - ((float)(cross[1][0] - cross[2][0])) * ((float)(points[0][1] - points[2][1]));
+
+  dx1 = ((float)(points[0][0] - points[2][0])) * ((float)(cross[1][0] - cross[2][0]))
+          - ((float)(points[1][0] - points[2][0])) * ((float)(cross[0][0] - cross[2][0]));
+
+  dx2 = cross[0][0] * ((float)points[1][0] * (float)points[2][1] - (float)points[2][0] * (float)points[1][1]) -
+        cross[1][0] * ((float)points[0][0] * (float)points[2][1] - (float)points[2][0] * (float)points[0][1]) +
+        cross[2][0] * ((float)points[0][0] * (float)points[1][1] - (float)points[1][0] * (float)points[0][1]);
+
+  dy0 = ((float)(cross[0][1] - cross[2][1]))  *  ((float)(points[1][1] - points[2][1]))
+          - ((float)(cross[1][1] - cross[2][1]))  *  ((float)(points[0][1] - points[2][1]));
+
+  dy1  = ((float)(points[0][0] - points[2][0]))  *  ((float)(cross[1][1] - cross[2][1]))
+          - ((float)(points[1][0] - points[2][0]))  *  ((float)(cross[0][1] - cross[2][1]));
+
+  dy2  = cross[0][1] * ((float)points[1][0] * (float)points[2][1] - (float)points[2][0] * (float)points[1][1]) -
+         cross[1][1] * ((float)points[0][0] * (float)points[2][1] - (float)points[2][0] * (float)points[0][1]) +
+         cross[2][1] * ((float)points[0][0] * (float)points[1][1] - (float)points[1][0] * (float)points[0][1]);
+
+  /* Now, calculate all the required coefficients */
+  c->ax = dx0 / dx;
+  c->bx = dx1 / dx;
+  c->cx = dx2 / dx;
+
+  c->ay = dy0 / dx;
+  c->by = dy1 / dx;
+  c->cy = dy2 / dx;
+}
+
 /*===========================================================================*/
 /* Driver exported functions.                                                */
 /*===========================================================================*/
@@ -145,17 +189,19 @@ void tpInit(const TOUCHPADDriver *tp) {
  *
  * @api
  */
-uint16_t tpReadX(void) {
+coord_t tpReadX(void) {
-	uint16_t x, y;
+    coord_t x, y;
 
 #if TOUCHPAD_XY_INVERTED == TRUE
-	x = cal->xm * _tpReadRealY() + cal->xn;
+    x = _tpReadRealY();
-	y = cal->ym * _tpReadRealX() + cal->yn;
+    y = _tpReadRealX();
 #else
-	x = cal->xm * _tpReadRealX() + cal->xn;
+    x = _tpReadRealX();
-	y = cal->ym * _tpReadRealY() + cal->yn;
+    y = _tpReadRealY();
 #endif
 
+    _tpTransform(&x, &y);
+
     switch(gdispGetOrientation()) {
         case GDISP_ROTATE_0:
             return x;
@@ -177,17 +223,19 @@ uint16_t tpReadX(void) {
  *
  * @api
  */
-uint16_t tpReadY(void) {
+coord_t tpReadY(void) {
-	uint16_t x, y;
+    coord_t x, y;
 
 #if TOUCHPAD_XY_INVERTED == TRUE
-	x = cal->xm * _tpReadRealY() + cal->xn;
+    x = _tpReadRealY();
-	y = cal->ym * _tpReadRealX() + cal->yn;
+    y = _tpReadRealX();
 #else
-	x = cal->xm * _tpReadRealX() + cal->xn;
+    x = _tpReadRealX();
-	y = cal->ym * _tpReadRealY() + cal->yn;
+    y = _tpReadRealY();
 #endif
 
+    _tpTransform(&x, &y);
+
     switch(gdispGetOrientation()) {
         case GDISP_ROTATE_0:
             return y;
@@ -217,45 +265,106 @@ uint16_t tpReadY(void) {
 #endif
 
 /**
- * @brief	returns if touchpad is pressed or not
+ * @brief	Returns if touchpad is pressed or not
  *
- * @return	1 if pressed, 0 otherwise
+ * @return	TRUE if pressed, FALSE otherwise
  *
  * @api
  */
 #if TOUCHPAD_HAS_IRQ || defined(__DOXYGEN__)
-	uint8_t tpIRQ(void) {
+	bool_t tpIRQ(void) {
 		return tp_lld_irq();
 	}
 #endif
 
+/* Define maximum no. of times to sample the calibration point */
+#define MAX_CAL_SAMPLES     10
+
+/**
+ * @brief   This function interactively performs calibration of the touchscreen
+ *          using 3-point calibration algorithm. Optionally, it also verifies
+ *          the accuracy of the calibration coefficients obtained if the symbol
+ *          TOUCHPAD_VERIFY_CALIBRATION is defined in the configuration.
+ *
+ * @api
+ */
 void tpCalibrate(void) {
-	const uint16_t h = gdispGetHeight();
+    const uint16_t height  =  gdispGetHeight();
-	const uint16_t w = gdispGetWidth();
+    const uint16_t width  =  gdispGetWidth();
-	const uint16_t cross[2][2] = {{(w/8), (h/8)}, {(w-(w/8)) , (h-(h/8))}};
+    const coord_t cross[][2]  =  {{(width / 4), (height / 4)},
-	uint16_t points[2][2];
+                                    {(width - (width / 4)) , (height / 4)},
-	uint8_t i;
+                                    {(width - (width / 4)) , (height - (height / 4))},
+                                    {(width / 2), (height / 2)}}; /* Check point */
+    coord_t points[4][2];
+    int32_t px, py;
+    uint8_t i, j;
 
     gdispSetOrientation(GDISP_ROTATE_0);
-	gdispClear(Red);
+    gdispClear(Blue);
-	gdispFillStringBox(0, 10, gdispGetWidth(), 30, "Calibration", &fontUI2Double,  White, Red, justifyCenter);
 
-	for(i = 0; i < 2; i++) {
+    gdispFillStringBox(0, 5, gdispGetWidth(), 30, "Calibration", &fontUI2Double,  White, Blue, justifyCenter);
+
+#if defined(TOUCHPAD_VERIFY_CALIBRATION)
+calibrate:
+    for(i = 0; i < 4; i++) {
+#else
+    for(i = 0; i < 3; i++) {
+#endif
       _tpDrawCross(cross[i][0], cross[i][1]);
-		while(!tpIRQ());
+
-		points[i][0] = _tpReadRealX();
+      while(!tpIRQ())
-		points[i][1] = _tpReadRealY();
+        chThdSleepMilliseconds(2);        /* Be nice to other threads*/
-		chThdSleepMilliseconds(100);
+
-		while(tpIRQ());
+      chThdSleepMilliseconds(20);         /* Allow screen to settle */
-		gdispFillArea(cross[i][0]-15, cross[i][1]-15, 42, 42, Red);
+
+      /* Take a little more samples per point and their average
+       * for precise calibration */
+      px = py = 0;
+
+      j = 0;
+      while (j < MAX_CAL_SAMPLES) {
+        if (tpIRQ()) {
+          /* We have valid pointer data */
+          px += _tpReadRealX();
+          py += _tpReadRealY();
+
+          j++;
+        }
       }
 
-	cal->xm = ((float)cross[1][0] - (float)cross[0][0]) / ((float)points[1][0] - (float)points[0][0]);
+      points[i][0] = px / j;
-	cal->ym = ((float)cross[1][1] - (float)cross[0][1]) / ((float)points[1][1] - (float)points[0][1]);
+      points[i][1] = py / j;
 
-	cal->xn = (float)cross[0][0] - cal->xm * (float)points[0][0];
+      chThdSleepMilliseconds(100);
-	cal->yn = (float)cross[0][1] - cal->ym * (float)points[0][1];
 
+      while(tpIRQ())
+        chThdSleepMilliseconds(2);        /* Be nice to other threads*/
+
+      gdispFillArea(cross[i][0] - 15, cross[i][1] - 15, 42, 42, Blue);
+    }
+
+    /* Apply 3 point calibration algorithm */
+    _tpDo3PointCalibration(cross, points, cal);
+
+#if defined(TOUCHPAD_VERIFY_CALIBRATION)
+    /* Verification of correctness of calibration (optional) :
+     *  See if the 4th point (Middle of the screen) coincides with the calibrated
+     *  result. If point is with +/- 2 pixel margin, then successful calibration
+     *  Else, start from the beginning.
+     */
+
+    /* Transform the co-ordinates */
+    _tpTransform(&points[3][0], &points[3][1]);
+
+    /* Calculate the delta */
+    px = (points[3][0] - cross[3][0]) * (points[3][0] - cross[3][0]) +
+         (points[3][1] - cross[3][1]) * (points[3][1] - cross[3][1]);
+
+    if (px > 4)
+      goto calibrate;
+#endif
+
+    /* If enabled, serialize the calibration values for storage */
       #if TOUCHPAD_STORE_CALIBRATION
         tp_store_calibration_lld(cal);
       #endif