diff --git a/halext/drivers/touchpad/touchpadXPT2046/xpt2046_lld.c.h b/halext/drivers/touchpad/touchpadXPT2046/xpt2046_lld.c.h
index 97adc1ad..2b33034a 100644
--- a/halext/drivers/touchpad/touchpadXPT2046/xpt2046_lld.c.h
+++ b/halext/drivers/touchpad/touchpadXPT2046/xpt2046_lld.c.h
@@ -62,6 +62,8 @@ __inline uint16_t lld_tpReadY(void) {
 }
 
 __inline uint16_t lld_tpReadZ(void) {
+	/* ToDo */
+
     return 0;
 }
 
diff --git a/halext/include/touchpad.h b/halext/include/touchpad.h
index c6e44b55..222411cf 100644
--- a/halext/include/touchpad.h
+++ b/halext/include/touchpad.h
@@ -45,6 +45,16 @@
 /* Type definitions                                                          */
 /*===========================================================================*/
 
+/**
+ * @brief	Struct used for calibration
+ */
+struct cal {
+    float xm; 
+    float ym; 
+    float xn; 
+    float yn; 
+};
+
 /*===========================================================================*/
 /* External declarations.                                                    */
 /*===========================================================================*/
@@ -53,29 +63,14 @@
 extern "C" {
 #endif
 
-#if TOUCHPAD_NEED_MULTITHREAD
-
-	void tpInit(TOUCHPADDriver *tp);
-	uint16_t tpReadX(void);
-	uint16_t tpReadY(void);
-
-	#if TOUCHPAD_PRESSURE
-		uint16_t tpReadZ(void);
-	#endif
-
-#else
-
-	#define tpInit(tp)		tp_lld_init(tp)
-	#define	tpReadX()		tp_lld_read_x()
-	#define tpReadY()		tp_lld_read_y()
-
-	#if TOUCHPAD_PRESSURE
-		#define tpReadZ()	tp_lld_read_z()
-	#endif
+void tpInit(TOUCHPADDriver *tp);
+uint16_t tpReadX(void);
+uint16_t tpReadY(void);
 
+#if TOUCHPAD_PRESSURE
+	uint16_t tpReadZ(void);
 #endif
 
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/halext/src/touchpad.c b/halext/src/touchpad.c
index f65ca0cd..4adb25fe 100644
--- a/halext/src/touchpad.c
+++ b/halext/src/touchpad.c
@@ -27,17 +27,11 @@
  */
 #include "ch.h"
 #include "hal.h"
+#include "gdisp.h"
 #include "touchpad.h"
 
 #if HAL_USE_TOUCHPAD || defined(__DOXYGEN__)
 
-#if GDISP_NEED_MULTITHREAD
-    #warning "TOUCHPAD: Multithread support not complete"
-    #define MUTEX_INIT      /* Not defined yet */
-    #define MUTEX_ENTER     /* Not defined yet */
-    #define MUTEX_EXIT      /* Not defined yet */
-#endif
-
 /*===========================================================================*/
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
@@ -58,6 +52,9 @@
 /*===========================================================================*/
 /* Driver local variables.                                                   */
 /*===========================================================================*/
+volatile static struct cal cal = {
+    1, 1, 0, 0
+};
 
 /*===========================================================================*/
 /* Driver local functions.                                                   */
@@ -67,52 +64,62 @@
 /* Driver exported functions.                                                */
 /*===========================================================================*/
 
-#if TOUCHPAD_NEED_MULTITHREAD || defined(__DOXYGEN__)
-    /**
-     * @brief	Touchpad Driver initialization.
-     * @note    This function is NOT currently implicitly invoked by @p halInit().
-     *          It must be called manually.
-     *
-     * @init
-     */
-    void tpInit(TOUCHPADDriver * UNUSED(tp)) {
-        /* Initialise Mutex */
-        //MUTEX_INIT
+/**
+ * @brief	Touchpad Driver initialization.
+ * @note    This function is NOT currently implicitly invoked by @p halInit().
+ *          It must be called manually.
+ *
+ * @init
+ */
+void tpInit(TOUCHPADDriver *tp) {
+	/* Initialise Mutex */
+	//MUTEX_INIT
 
-        /* Initialise driver */
-        //MUTEX_ENTER
-        tp_lld_init();
-        //MUTEX_EXIT
-    }
-#endif
+	/* Initialise driver */
+	//MUTEX_ENTER
+	tp_lld_init(tp);
+	//MUTEX_EXIT
+}
 
-#if TOUCHPAD_NEED_MULTITHREAD || defined(__DOXYGEN__)
-	/**
-	 * @brief	Get the X-Coordinate, relative to screen zero point.
-	 *
-	 * @return	The X position in pixels.
-	 *
-	 * @api
-	 */
-	uint16_t tpReadX(void) {
-		return (tp_lld_read_x());
-	}
-#endif
+/**
+ * @brief	Get the X-Coordinate, relative to screen zero point.
+ *
+ * @return	The X position in pixels.
+ *
+ * @api
+ */
+uint16_t tpReadX(void) {
+	uint16_t x, y;
 
-#if TOUCHPAD_NEED_MULTITHREAD || defined(__DOXYGEN__)
-	/**
-	 * @brief	Get the X-Coordinate, relative to screen zero point.
-	 *
-	 * @return	The Y position in pixels.
-	 *
-	 * @api
-	 */
-	uint16_t tpReadY(void) {
-		return (tp_lld_read_y());
-	}
-#endif
+	x = cal.xm * _tpReadRealX() + cal.xn;
+	y = cal.ym * _tpReadRealY() + cal.yn;
 
-#if TOUCHPAD_NEED_MULTITHREAD || defined(__DOXYGEN__)
+	//switch(gdispGetOrientation()) {
+	switch(portrait) { // implement gdispGetOrientation()  
+		case portrait:
+			return x;
+		case landscape:
+			return SCREEN_HEIGHT - y;
+		case portraitInv:
+			return SCREEN_WIDTH - x;
+		case landscapeInv:
+			return y;
+	}   
+		return x;
+}
+	
+/**
+ * @brief	Get the X-Coordinate, relative to screen zero point.
+ *
+ * @return	The Y position in pixels.
+ *
+ * @api
+ */
+uint16_t tpReadY(void) {
+	return (tp_lld_read_y());
+}
+
+#if TOUCHPAD_PRESSURE || defined(__DOXYGEN__)
 	/**
 	 * @brief	Get the pressure.
 	 *