diff --git a/include/touchpad.h b/include/touchpad.h
index 445d3202..55a83a9d 100644
--- a/include/touchpad.h
+++ b/include/touchpad.h
@@ -70,7 +70,7 @@ struct cal {
 extern "C" {
 #endif
 
-void tpInit(TOUCHPADDriver *tp);
+void tpInit(const TOUCHPADDriver *tp);
 uint16_t tpReadX(void);
 uint16_t tpReadY(void);
 
diff --git a/src/touchpad.c b/src/touchpad.c
index 977bb31c..a18930da 100644
--- a/src/touchpad.c
+++ b/src/touchpad.c
@@ -69,13 +69,13 @@ static uint16_t _tpReadRealX(void) {
     uint32_t results = 0;
     uint16_t i, x;
 
+    /* Median filtering is already done in LLD */
     for(i = 0; i < CONVERSIONS; i++) {
-        tp_lld_read_x(); /* dummy, reduce noise on SPI */
         results += tp_lld_read_x();
-    }   
+    }
 
-	// 12-bit
-    x = (((SCREEN_WIDTH-1) * (results/CONVERSIONS)) / 2048);
+	/* Take the average of the readings */
+    x = results / CONVERSIONS;
 
     return x;
 }
@@ -89,13 +89,13 @@ static uint16_t _tpReadRealY(void) {
     uint32_t results = 0;
     uint16_t i, y;
 
+    /* Median filtering is already done in LLD */
     for(i = 0; i < CONVERSIONS; i++) {
-        tp_lld_read_y(); /* dummy, reduce noise on SPI */
         results += tp_lld_read_y();
-    }   
+    }
 
-	// 12-bit
-    y = (((SCREEN_HEIGHT-1) * (results/CONVERSIONS)) / 2048);
+    /* Take the average of the readings */
+    y = results / CONVERSIONS;
 
     return y;
 }
@@ -135,7 +135,7 @@ static void _tpDrawCross(uint16_t x, uint16_t y) {
  *
  * @api
  */
-void tpInit(TOUCHPADDriver *tp) {
+void tpInit(const TOUCHPADDriver *tp) {
 	/* Initialise Mutex */
 	//MUTEX_INIT