diff --git a/lib/pbio/include/pbio/color.h b/lib/pbio/include/pbio/color.h
index 20cfafebd..16897c0e8 100644
--- a/lib/pbio/include/pbio/color.h
+++ b/lib/pbio/include/pbio/color.h
@@ -13,6 +13,8 @@
 
 #include <stdint.h>
 
+#include <fixmath.h>
+
 /** @cond INTERNAL */
 
 /**
@@ -106,12 +108,24 @@ typedef struct __attribute__((__packed__)) {
     uint8_t v;
 } pbio_color_compressed_hsv_t;
 
+/** HSV color with fix16 components */
+typedef struct {
+    /** The hue component. radians. */
+    fix16_t h;
+    /** The saturation component. 0 to 1. */
+    fix16_t s;
+    /** The value component. 0 to 1. */
+    fix16_t v;
+} pbio_color_hsv_fix16_t;
+
 void pbio_color_rgb_to_hsv(const pbio_color_rgb_t *rgb, pbio_color_hsv_t *hsv);
 void pbio_color_hsv_to_rgb(const pbio_color_hsv_t *hsv, pbio_color_rgb_t *rgb);
 void pbio_color_to_hsv(pbio_color_t color, pbio_color_hsv_t *hsv);
 void pbio_color_to_rgb(pbio_color_t color, pbio_color_rgb_t *rgb);
 void pbio_color_hsv_compress(const pbio_color_hsv_t *hsv, pbio_color_compressed_hsv_t *compressed);
 void pbio_color_hsv_expand(const pbio_color_compressed_hsv_t *compressed, pbio_color_hsv_t *hsv);
+void pbio_color_hsv_to_fix16(const pbio_color_hsv_t *hsv, pbio_color_hsv_fix16_t *hsv_fix16);
+
 int32_t pbio_get_hsv_cost(const pbio_color_hsv_t *x, const pbio_color_hsv_t *c);
 int32_t pbio_get_cone_cost(const pbio_color_hsv_t *a, const pbio_color_hsv_t *b);
 
diff --git a/lib/pbio/src/color/conversion.c b/lib/pbio/src/color/conversion.c
index 212f6af4e..842552459 100644
--- a/lib/pbio/src/color/conversion.c
+++ b/lib/pbio/src/color/conversion.c
@@ -2,6 +2,8 @@
 // Copyright (c) 2020 The Pybricks Authors
 // Copyright (c) 2013 FastLED
 
+#include <fixmath.h>
+
 #include <pbio/color.h>
 
 /**
@@ -233,3 +235,18 @@ void pbio_color_hsv_expand(const pbio_color_compressed_hsv_t *compressed, pbio_c
     hsv->s = compressed->s;
     hsv->v = compressed->v;
 }
+
+/**
+ * Converts HSV to normalized HSV for fixed point operations.
+ *
+ * @param [in]  hsv         The source HSV color value.
+ * @param [out] hsv         The destination HSV_fix16 color value.
+ */
+void pbio_color_hsv_to_fix16(const pbio_color_hsv_t *hsv, pbio_color_hsv_fix16_t *hsv_fix16) {
+    const fix16_t by100 = F16C(0,0100);
+
+    // normalize h to radians, s/v to (0,1)
+    hsv_fix16->h = fix16_deg_to_rad(fix16_from_int(hsv->h));
+    hsv_fix16->s = fix16_mul(fix16_from_int(hsv->s), by100);
+    hsv_fix16->v = fix16_mul(fix16_from_int(hsv->v), by100);
+}
\ No newline at end of file
diff --git a/lib/pbio/src/color/util.c b/lib/pbio/src/color/util.c
index 87cb9a781..25c79fa86 100644
--- a/lib/pbio/src/color/util.c
+++ b/lib/pbio/src/color/util.c
@@ -1,5 +1,5 @@
 // SPDX-License-Identifier: MIT
-// Copyright (c) 2018-2020 The Pybricks Authors
+// Copyright (c) 2018-2022 The Pybricks Authors
 
 #include <fixmath.h>
 
@@ -32,47 +32,40 @@ int32_t pbio_get_hsv_cost(const pbio_color_hsv_t *x, const pbio_color_hsv_t *c)
     return hue_error * hue_error + 5 * saturation_error * saturation_error + 2 * value_error * value_error;
 }
 
-int32_t pbio_get_cone_cost(const pbio_color_hsv_t *a, const pbio_color_hsv_t *b) {
+int32_t pbio_get_cone_cost(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b) {
 
-    fix16_t by100 = fix16_div(fix16_one, fix16_from_int(100));
-
-    // normalize h to radians, s/v to (0,1)
-    fix16_t a_h = fix16_deg_to_rad(fix16_from_int(a->h));
-    fix16_t a_s = fix16_mul(fix16_from_int(a->s), by100);
-    fix16_t a_v = fix16_mul(fix16_from_int(a->v), by100);
-
-    fix16_t b_h = fix16_deg_to_rad(fix16_from_int(b->h));
-    fix16_t b_s = fix16_mul(fix16_from_int(b->s), by100);
-    fix16_t b_v = fix16_mul(fix16_from_int(b->v), by100);
+    pbio_color_hsv_fix16_t a, b;
+    pbio_color_hsv_to_fix16(hsv_a, &a);
+    pbio_color_hsv_to_fix16(hsv_b, &b);
 
     // x, y and z deltas between cartesian coordinates of a and b in HSV cone
     // delx = b_s*b_v*cos(b_h) - a_s*a_v*cos(a_h)
     fix16_t delx = fix16_sub(
         fix16_mul(
             fix16_mul(
-                fix16_cos(b_h),
-                b_v),
-            b_s),
+                fix16_cos(b.h),
+                b.v),
+            b.s),
         fix16_mul(
             fix16_mul(
-                fix16_cos(a_h),
-                a_v),
-            a_s));
+                fix16_cos(a.h),
+                a.v),
+            a.s));
 
     // dely = b_s*b_v*sin(b_h) - a_s*a_v*sin(a_h)
     fix16_t dely = fix16_sub(
         fix16_mul(
             fix16_mul(
-                fix16_sin(b_h),
-                b_v),
-            b_s),
+                fix16_sin(b.h),
+                b.v),
+            b.s),
         fix16_mul(
             fix16_mul(
-                fix16_sin(a_h),
-                a_v),
-            a_s));
+                fix16_sin(a.h),
+                a.v),
+            a.s));
     // delz = b_v - a_v
-    fix16_t delz = fix16_sub(b_v, a_v);
+    fix16_t delz = fix16_sub(b.v, a.v);
 
     // cdist = delx*delx + dely*dely + delz*delz
     fix16_t cdist = fix16_add(
diff --git a/lib/pbio/test/src/color.c b/lib/pbio/test/src/color.c
index c757af219..67be108d3 100644
--- a/lib/pbio/test/src/color.c
+++ b/lib/pbio/test/src/color.c
@@ -399,7 +399,6 @@ static void test_color_hsv_cost(void *env) {
     color_b.v = 70;
 
     dist = pbio_get_cone_cost(&color_a, &color_b);
-    printf("\n\nhello world %d\n", dist);
 
     color_a.h = 30;
     color_a.s = 40;