diff --git a/drivers/ginput/touch/FT5x06/gmouse_lld_FT5x06.c b/drivers/ginput/touch/FT5x06/gmouse_lld_FT5x06.c
index ba3db50b..0359ecf3 100644
--- a/drivers/ginput/touch/FT5x06/gmouse_lld_FT5x06.c
+++ b/drivers/ginput/touch/FT5x06/gmouse_lld_FT5x06.c
@@ -72,10 +72,25 @@ static bool_t MouseXYZ(GMouse* m, GMouseReading* pdr)
 		pdr->y = (coord_t)read_word(m, FT5x06_TOUCH1_YH);
 		pdr->z = 1;
 
+		// Rescale X,Y if we are using self-calibration
 		#if GMOUSE_FT5x06_SELF_CALIBRATE
-			// Rescale X,Y,Z - If we are using self-calibration
-			pdr->x = gdispGGetWidth(m->display) - pdr->x / (4096/gdispGGetWidth(m->display));
-			pdr->y = pdr->y / (4096/gdispGGetHeight(m->display));
+			#if GDISP_NEED_CONTROL
+				switch(gdispGGetOrientation(m->display)) {
+				case GDISP_ROTATE_0:
+				case GDISP_ROTATE_180:
+					pdr->x = gdispGGetWidth(m->display) - pdr->x / (4096/gdispGGetWidth(m->display));
+					pdr->y = pdr->y / (4096/gdispGGetHeight(m->display));
+					break;
+				case GDISP_ROTATE_90:
+				case GDISP_ROTATE_270:
+					pdr->x = gdispGGetHeight(m->display) - pdr->x / (4096/gdispGGetHeight(m->display));
+					pdr->y = pdr->y / (4096/gdispGGetWidth(m->display));
+					break;
+				}
+			#else
+				pdr->x = gdispGGetWidth(m->display) - pdr->x / (4096/gdispGGetWidth(m->display));
+				pdr->y = pdr->y / (4096/gdispGGetHeight(m->display));
+			#endif
 		#endif
 	}
 
diff --git a/drivers/ginput/touch/STMPE610/gmouse_lld_STMPE610.c b/drivers/ginput/touch/STMPE610/gmouse_lld_STMPE610.c
index 2175da94..6458c87d 100644
--- a/drivers/ginput/touch/STMPE610/gmouse_lld_STMPE610.c
+++ b/drivers/ginput/touch/STMPE610/gmouse_lld_STMPE610.c
@@ -207,7 +207,6 @@ static bool_t read_xyz(GMouse* m, GMouseReading* pdr)
 	#endif
 
 	// Rescale X,Y if we are using self-calibration
-	// Not working currently.
 	#if GMOUSE_STMPE610_SELF_CALIBRATE
 		#if GDISP_NEED_CONTROL
 			switch(gdispGGetOrientation(m->display)) {
@@ -215,14 +214,16 @@ static bool_t read_xyz(GMouse* m, GMouseReading* pdr)
 			case GDISP_ROTATE_180:
 			default:
 				pdr->x = pdr->x / (4096/gdispGGetWidth(m->display));
-				pdr->y = gdispGGetHeight(m->display) - pdr->y / (4096/gdispGGetHeight(m->display));
+				pdr->y = pdr->y / (4096/gdispGGetHeight(m->display));
+				break;
 			case GDISP_ROTATE_90:
 			case GDISP_ROTATE_270:
 				pdr->x = pdr->x / (4096/gdispGGetHeight(m->display));
-				pdr->y = gdispGGetWidth(m->display) - pdr->y / (4096/gdispGGetWidth(m->display));
+				pdr->y = pdr->y / (4096/gdispGGetWidth(m->display));
+				break;
 			}
 		#else
-			pdr->x = gdispGGetWidth(m->display) - pdr->x / (4096/gdispGGetWidth(m->display));
+			pdr->x = pdr->x / (4096/gdispGGetWidth(m->display));
 			pdr->y = pdr->y / (4096/gdispGGetHeight(m->display));
 		#endif
 	#endif