pybricks/util_pb: modify rgb to hsv conversion

This adds in a few modifications to make HSV match reality somewhat better. In the long run, this needs to be done properly similar to how HSV to RGB is calibrated. That way, we would also distinguish orange from red better.

At the moment, all sensors have the exact same mismatch, so we use a shared correction function. Better results may be obtained by tweaking it per sensor.

pybricks/nxtdevices/pb_type_nxtdevices_colorsensor.c

@@ -97,7 +97,7 @@ STATIC mp_obj_t nxtdevices_ColorSensor_hsv(mp_obj_t self_in) {
     pb_type_Color_obj_t *color = pb_type_Color_new_empty();
 
     // Convert and store RGB as HSV
-    pbio_color_rgb_to_hsv(&rgb, &color->hsv);
+    color_map_rgb_to_hsv(&rgb, &color->hsv);
 
     // Return color
     return MP_OBJ_FROM_PTR(color);
@@ -117,7 +117,7 @@ STATIC mp_obj_t nxtdevices_ColorSensor_color(mp_obj_t self_in) {
         .g = all[1],
         .b = all[2],
     };
-    pbio_color_rgb_to_hsv(&rgb, &hsv);
+    color_map_rgb_to_hsv(&rgb, &hsv);
 
     // Get and return discretized color based on HSV
     return pb_color_map_get_color(&self->color_map, &hsv);

pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c

@@ -50,7 +50,7 @@ STATIC void pupdevices_ColorDistanceSensor__hsv(pupdevices_ColorDistanceSensor_o
     pbio_color_rgb_t rgb;
     raw_to_rgb(raw, &rgb);
 
-    pbio_color_rgb_to_hsv(&rgb, hsv);
+    color_map_rgb_to_hsv(&rgb, hsv);
 }
 
 // pybricks.pupdevices.ColorDistanceSensor.__init__

pybricks/pupdevices/pb_type_pupdevices_colorsensor.c

@@ -38,7 +38,7 @@ STATIC void pupdevices_ColorSensor__get_hsv_reflected(pb_device_t *pbdev, pbio_c
     };
 
     // Convert to HSV
-    pbio_color_rgb_to_hsv(&rgb, hsv);
+    color_map_rgb_to_hsv(&rgb, hsv);
 }
 
 // pybricks._common.ColorSensor._get_hsv_ambient

pybricks/util_pb/pb_color_map.c

@@ -15,6 +15,26 @@
 #include <pybricks/util_pb/pb_color_map.h>
 #include <pybricks/util_pb/pb_error.h>
 
+// This expands pbio_color_rgb_to_hsv with additional calibration steps that
+// ultimately must be properly done in pbio_color_rgb_to_hsv, just like
+// pbio_color_hsv_to_rgb, by adjusting RGB instead of hacking at the HSV value.
+void color_map_rgb_to_hsv(const pbio_color_rgb_t *rgb, pbio_color_hsv_t *hsv) {
+
+    // Standard conversion
+    pbio_color_rgb_to_hsv(rgb, hsv);
+
+    // Slight shift for lower hues to make yellow somewhat more accurate
+    if (hsv->h < 40) {
+        uint8_t offset = ((hsv->h - 20) << 8) / 20;
+        int32_t scale = 200 - ((100 * (offset * offset)) >> 16);
+        hsv->h = hsv->h * scale / 100;
+    }
+
+    // Value and saturation correction
+    hsv->s = hsv->s * (200 - hsv->s) / 100;
+    hsv->v = hsv->v * (200 - hsv->v) / 100;
+}
+
 STATIC const mp_rom_obj_tuple_t pb_color_map_default = {
     {&mp_type_tuple},
     7,

pybricks/util_pb/pb_color_map.h

@@ -8,6 +8,8 @@
 
 #include "py/obj.h"
 
+void color_map_rgb_to_hsv(const pbio_color_rgb_t *rgb, pbio_color_hsv_t *hsv);
+
 void pb_color_map_save_default(mp_obj_t *color_map);
 
 mp_obj_t pb_color_map_get_color(mp_obj_t *color_map, pbio_color_hsv_t *hsv);