Add gdispContrastColor() and remove divides from gdispBlendColors()

2 files changed, 23 insertions, 3 deletions

diff --git a/src/gdisp/gdisp.c b/src/gdisp/gdisp.c
index 9f3ff11b..e9ede8ab 100644
--- a/src/gdisp/gdisp.c
+++ b/src/gdisp/gdisp.c
@@ -3020,13 +3020,23 @@ color_t gdispBlendColor(color_t fg, color_t bg, uint8_t alpha)
 	g += GREEN_OF(bg) * bg_ratio;
 	b += BLUE_OF(bg) * bg_ratio;
 	
-	r /= 256;
-	g /= 256;
-	b /= 256;
+	r >>= 8;
+	g >>= 8;
+	b >>= 8;
 	
 	return RGB2COLOR(r, g, b);
 }
 
+color_t gdispContrastColor(color_t color) {
+	uint16_t r, g, b;
+
+	r = RED_OF(color) > 128 ? 0 : 255;
+	g = GREEN_OF(color) > 128 ? 0 : 255;
+	b = BLUE_OF(color) > 128 ? 0 : 255;
+
+	return RGB2COLOR(r, g, b);
+}
+
 #if (!defined(gdispPackPixels) && !defined(GDISP_PIXELFORMAT_CUSTOM))
 	void gdispPackPixels(pixel_t *buf, coord_t cx, coord_t x, coord_t y, color_t color) {
 		/* No mutex required as we only read static data */
diff --git a/src/gdisp/sys_defs.h b/src/gdisp/sys_defs.h
index a40d6e80..72fdc621 100644
--- a/src/gdisp/sys_defs.h
+++ b/src/gdisp/sys_defs.h
@@ -211,6 +211,16 @@ extern "C" {
 color_t gdispBlendColor(color_t fg, color_t bg, uint8_t alpha);
 
 /**
+ * @brief   Find a contrasting color
+ * @return	The contrasting color
+ *
+ * @param[in] color		The color to contrast
+ *
+ * @api
+ */
+color_t gdispContrastColor(color_t color);
+
+/**
  * @brief   Get the specified display
  * @return	The pointer to the display or NULL if the display doesn't exist
  * @note	The GDISP variable contains the display used by the gdispXxxx routines