implemented tpCalibrate()

1 files changed, 50 insertions, 0 deletions

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