From b8e412e066db3f3f231a6873e5f4da15ff297c2d Mon Sep 17 00:00:00 2001
From: Tectu <joel@unormal.org>
Date: Thu, 9 Aug 2012 03:43:30 +0200
Subject: [PATCH] implemented tpCalibrate()

---
 halext/include/gdisp_lld.h |  2 ++
 halext/src/touchpad.c      | 50 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 52 insertions(+)

diff --git a/halext/include/gdisp_lld.h b/halext/include/gdisp_lld.h
index 970fd712..e9edbe96 100644
--- a/halext/include/gdisp_lld.h
+++ b/halext/include/gdisp_lld.h
@@ -296,6 +296,8 @@
 	#define RED_OF(c)			(((c) & 0xF800)>>8)
 	#define GREEN_OF(c)			(((c)&0x007E)>>3)
 	#define BLUE_OF(c)			(((c)&0x001F)<<3)
+	#define RGB565CONVERT(red, green, blue) (uint16_t)( (( red   >> 3 ) << 11 ) | (( green >> 2 ) << 5  ) | ( blue  >> 3 ))
+
 
 #elif defined(GDISP_PIXELFORMAT_RGB888)
 	typedef uint32_t color_t;
diff --git a/halext/src/touchpad.c b/halext/src/touchpad.c
index 2b51504a..5274e350 100644
--- a/halext/src/touchpad.c
+++ b/halext/src/touchpad.c
@@ -98,6 +98,30 @@ static uint16_t _tpReadRealY(void) {
     return y;
 }
 
+/**
+ * @brief	draws a cross. Used for calibration.
+ *
+ * @noapi
+ */
+static void _tpDrawCross(uint16_t x, uint16_t y) {
+    gdispDrawLine(x-15, y, x-2, y, 0xffff);
+    gdispDrawLine(x+2, y, x+15, y, 0xffff);
+    gdispDrawLine(x, y-15, x, y-2, 0xffff);
+    gdispDrawLine(x, y+2, x, y+15, 0xffff);
+  
+    gdispDrawLine(x-15, y+15, x-7, y+15, RGB565CONVERT(184,158,131));
+    gdispDrawLine(x-15, y+7, x-15, y+15, RGB565CONVERT(184,158,131));
+
+    gdispDrawLine(x-15, y-15, x-7, y-15, RGB565CONVERT(184,158,131));
+    gdispDrawLine(x-15, y-7, x-15, y-15, RGB565CONVERT(184,158,131));
+
+    gdispDrawLine(x+7, y+15, x+15, y+15, RGB565CONVERT(184,158,131));
+    gdispDrawLine(x+15, y+7, x+15, y+15, RGB565CONVERT(184,158,131));
+
+    gdispDrawLine(x+7, y-15, x+15, y-15, RGB565CONVERT(184,158,131));
+    gdispDrawLine(x+15, y-15, x+15, y-7, RGB565CONVERT(184,158,131));    
+}
+
 /*===========================================================================*/
 /* Driver exported functions.                                                */
 /*===========================================================================*/
@@ -177,6 +201,32 @@ uint16_t tpReadY(void) {
 	return y;
 }
 
+void tpCalibrate(void) {
+	uint16_t cross[2][2] = {{40,50}, {200, 280}};
+	uint16_t points[2][2];
+	uint8_t i;
+
+    //gdispSetOrientation(portrait);
+    gdispClear(Red);
+    gdispDrawString(40, 10, "Calibration", &fontUI1Double, White);
+
+    for(i = 0; i < 2; i++) {
+        _tpDrawCross(cross[i][0], cross[i][1]);
+        while(!tpIRQ());
+        points[i][0] = _tpReadRealX();
+        points[i][1] = _tpReadRealY();
+		chThdSleepMilliseconds(100);
+        while(tpIRQ());
+        gdispFillArea(cross[i][0]-15, cross[i][1]-15, 42, 42, Red);
+	}
+
+    cal.xm = ((float)cross[1][0] - (float)cross[0][0]) / ((float)points[1][0] - (float)points[0][0]);
+    cal.ym = ((float)cross[1][1] - (float)cross[0][1]) / ((float)points[1][1] - (float)points[0][1]);
+
+    cal.xn = (float)cross[0][0] - cal.xm * (float)points[0][0];
+    cal.yn = (float)cross[0][1] - cal.ym * (float)points[0][1];
+}
+
 #if TOUCHPAD_HAS_IRQ || defined(__DOXYGEN__)
 	/**
 	 * @brief	returns if touchpad is pressed or not