diff --git a/touchpad.c b/touchpad.c
index 0c2fbf8c..0118952a 100644
--- a/touchpad.c
+++ b/touchpad.c
@@ -1,6 +1,8 @@
 #include "touchpad.h"
 #include "glcd.h"
 
+static int16_t x_cal, y_cal;
+
 static void spicb(SPIDriver *spip);
 static const SPIConfig spicfg = {
 	NULL,
@@ -47,8 +49,12 @@ static __inline uint16_t readY(void) {
 	return y;
 }
 
+uint8_t __inline tpIRQ(void) {
+	return (!palReadPad(TP_PORT, TP_IRQ));
+}
+
 uint16_t tpReadX(void) {
-	uint32_t results;
+	uint32_t results = 0;
 	uint16_t i, x;
 
 	for(i=0; i<CONVERSIONS; i++) {
@@ -58,11 +64,11 @@ uint16_t tpReadX(void) {
 
 	x = (((lcdGetHeight()-1) * (results/CONVERSIONS)) / 2048);
 
-	return x;
+	return x + x_cal;
 }
 
 uint16_t tpReadY(void) {
-	uint32_t results;
+	uint32_t results = 0;
 	uint16_t i, y;
 
 	for(i=0; i<CONVERSIONS; i++) {
@@ -72,7 +78,60 @@ uint16_t tpReadY(void) {
 
 	y = (((lcdGetWidth()-1) * (results/CONVERSIONS)) / 2048);
 
-	return y;
+	return y + y_cal;
 }
 
+void tpDrawCross(uint16_t x, uint16_t y) {
+	lcdDrawLine(x-15,y,x-2,y,0xffff);
+	lcdDrawLine(x+2,y,x+15,y,0xffff);
+	lcdDrawLine(x,y-15,x,y-2,0xffff);
+	lcdDrawLine(x,y+2,x,y+15,0xffff);
+  
+	lcdDrawLine(x-15,y+15,x-7,y+15,RGB565CONVERT(184,158,131));
+	lcdDrawLine(x-15,y+7,x-15,y+15,RGB565CONVERT(184,158,131));
+
+	lcdDrawLine(x-15,y-15,x-7,y-15,RGB565CONVERT(184,158,131));
+	lcdDrawLine(x-15,y-7,x-15,y-15,RGB565CONVERT(184,158,131));
+
+	lcdDrawLine(x+7,y+15,x+15,y+15,RGB565CONVERT(184,158,131));
+	lcdDrawLine(x+15,y+7,x+15,y+15,RGB565CONVERT(184,158,131));
+
+	lcdDrawLine(x+7,y-15,x+15,y-15,RGB565CONVERT(184,158,131));
+	lcdDrawLine(x+15,y-15,x+15,y-7,RGB565CONVERT(184,158,131));    
+}
+
+void tpCalibrate(void) {
+	uint16_t cross[3][2] = {{20,40}, {220,160}, {50,300}};
+	uint16_t cal[3][2];
+	uint8_t i, j;
+	int16_t a, b;
+	unsigned char buffer[32];
+
+	lcdClear(Red);
+	lcdDrawString(40, 10, "Touchpad Calibration", White, Red);
+	
+	for(i=0; i<3; i++) {
+		tpDrawCross(cross[i][0], cross[i][1]);
+		while(!tpIRQ());
+		cal[i][0] = tpReadX();
+		cal[i][1] = tpReadY();
+		while(tpIRQ());
+	}
+
+	for(i=0, j=0; i<3; i++) {
+		sprintf(buffer, "X: %d", cal[i][0]);
+		lcdDrawString(100, 100+(i*20)+j, buffer, White, Red);
+		sprintf(buffer, "Y: %d", cal[i][1]);
+		lcdDrawString(100, 120+(i*20)+j, buffer, White, Red);
+		j += 40;
+	}
+
+	for(a=0, b=0, i=0; i<3; i++) {
+		a += (cross[i][0] - cal[i][0]);
+		b += (cross[i][1] - cal[i][1]);
+	}
+
+	x_cal = (a / 3);
+	y_cal = (b / 3);
+}
 
diff --git a/touchpad.h b/touchpad.h
index 0dc6d7fc..88e397a8 100644
--- a/touchpad.h
+++ b/touchpad.h
@@ -10,6 +10,7 @@
 
 void tpInit(void);
 void tpWriteData(uint8_t data);
+void tpDrawCross(uint16_t x, uint16_t y);
 uint16_t tpReadData(void);
 uint16_t tpReadX(void);
 uint16_t tpReadY(void);