diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp
index ab26d1b80..c2db78c34 100644
--- a/g2core/canonical_machine.cpp
+++ b/g2core/canonical_machine.cpp
@@ -2525,7 +2525,7 @@ static void _print_axis_coord_flt(nvObj_t *nv, const char *format)
 
 static void _print_pos(nvObj_t *nv, const char *format, uint8_t units)
 {
-    char axes[] = {"XYZABC"};
+    char axes[] = {"XYZUVWABC"};
     uint8_t axis = _axis(nv);
     if (axis >= AXIS_A) { units = DEGREES;}
     sprintf(cs.out_buf, format, axes[axis], nv->value_flt, GET_TEXT_ITEM(msg_units, units));
@@ -2534,7 +2534,7 @@ static void _print_pos(nvObj_t *nv, const char *format, uint8_t units)
 
 static void _print_hom(nvObj_t *nv, const char *format)
 {
-    char axes[] = {"XYZABC"};
+    char axes[] = {"XYZUVWABC"};
     uint8_t axis = _axis(nv);
     sprintf(cs.out_buf, format, axes[axis], nv->value_int);
     xio_writeline(cs.out_buf);
diff --git a/g2core/cycle_homing.cpp b/g2core/cycle_homing.cpp
index 69fc2f569..bf66073d7 100644
--- a/g2core/cycle_homing.cpp
+++ b/g2core/cycle_homing.cpp
@@ -440,142 +440,29 @@ static stat_t _homing_finalize_exit(int8_t axis)  // third part of return to hom
  */
 
 static int8_t _get_next_axis(int8_t axis) {
-#if (HOMING_AXES <= 4)
-    if (axis == -1) {  // inelegant brute force solution
-        if (hm.axis_flags[AXIS_Z]) {
-            return (AXIS_Z);
-        }
-        if (hm.axis_flags[AXIS_X]) {
-            return (AXIS_X);
-        }
-        if (hm.axis_flags[AXIS_Y]) {
-            return (AXIS_Y);
-        }
-        if (hm.axis_flags[AXIS_A]) {
-            return (AXIS_A);
-        }
-        if (hm.axis_flags[AXIS_B]) {
-            return (AXIS_B);
-        }
-        if (hm.axis_flags[AXIS_C]) {
-            return (AXIS_C);
-        }
-        return (-2);  // error
-    } else if (axis == AXIS_Z) {
-        if (hm.axis_flags[AXIS_X]) {
-            return (AXIS_X);
-        }
-        if (hm.axis_flags[AXIS_Y]) {
-            return (AXIS_Y);
-        }
-        if (hm.axis_flags[AXIS_A]) {
-            return (AXIS_A);
-        }
-        if (hm.axis_flags[AXIS_B]) {
-            return (AXIS_B);
-        }
-        if (hm.axis_flags[AXIS_C]) {
-            return (AXIS_C);
-        }
-    } else if (axis == AXIS_X) {
-        if (hm.axis_flags[AXIS_Y]) {
-            return (AXIS_Y);
-        }
-        if (hm.axis_flags[AXIS_A]) {
-            return (AXIS_A);
-        }
-        if (hm.axis_flags[AXIS_B]) {
-            return (AXIS_B);
-        }
-        if (hm.axis_flags[AXIS_C]) {
-            return (AXIS_C);
-        }
-    } else if (axis == AXIS_Y) {
-        if (hm.axis_flags[AXIS_A]) {
-            return (AXIS_A);
-        }
-        if (hm.axis_flags[AXIS_B]) {
-            return (AXIS_B);
-        }
-        if (hm.axis_flags[AXIS_C]) {
-            return (AXIS_C);
-        }
-    }
-    return (-1);  // done
+    const int8_t homing_order[9] = {AXIS_Z, AXIS_X, AXIS_Y, AXIS_U, AXIS_V, AXIS_W, AXIS_A, AXIS_B, AXIS_C};
 
-#else
     if (axis == -1) {
-        if (hm.axis_flags[AXIS_Z]) {
-            return (AXIS_Z);
-        }
-        if (hm.axis_flags[AXIS_X]) {
-            return (AXIS_X);
-        }
-        if (hm.axis_flags[AXIS_Y]) {
-            return (AXIS_Y);
-        }
-        if (hm.axis_flags[AXIS_A]) {
-            return (AXIS_A);
-        }
-        if (hm.axis_flags[AXIS_B]) {
-            return (AXIS_B);
-        }
-        if (hm.axis_flags[AXIS_C]) {
-            return (AXIS_C);
-        }
-        return (-2);  // error
-    } else if (axis == AXIS_Z) {
-        if (hm.axis_flags[AXIS_X]) {
-            return (AXIS_X);
-        }
-        if (hm.axis_flags[AXIS_Y]) {
-            return (AXIS_Y);
-        }
-        if (hm.axis_flags[AXIS_A]) {
-            return (AXIS_A);
-        }
-        if (hm.axis_flags[AXIS_B]) {
-            return (AXIS_B);
-        }
-        if (hm.axis_flags[AXIS_C]) {
-            return (AXIS_C);
-        }
-    } else if (axis == AXIS_X) {
-        if (hm.axis_flags[AXIS_Y]) {
-            return (AXIS_Y);
-        }
-        if (hm.axis_flags[AXIS_A]) {
-            return (AXIS_A);
-        }
-        if (hm.axis_flags[AXIS_B]) {
-            return (AXIS_B);
-        }
-        if (hm.axis_flags[AXIS_C]) {
-            return (AXIS_C);
-        }
-    } else if (axis == AXIS_Y) {
-        if (hm.axis_flags[AXIS_A]) {
-            return (AXIS_A);
-        }
-        if (hm.axis_flags[AXIS_B]) {
-            return (AXIS_B);
-        }
-        if (hm.axis_flags[AXIS_C]) {
-            return (AXIS_C);
-        }
-    } else if (axis == AXIS_A) {
-        if (hm.axis_flags[AXIS_B]) {
-            return (AXIS_B);
-        }
-        if (hm.axis_flags[AXIS_C]) {
-            return (AXIS_C);
+        // search the first axis in the order whose flag is set
+        for (int i=0; i<9; i++) {
+            if (hm.axis_flags[homing_order[i]]) {
+                return homing_order[i];
+            }
         }
-    } else if (axis == AXIS_B) {
-        if (hm.axis_flags[AXIS_C]) {
-            return (AXIS_C);
+        return -2; // no existing axis flag was set
+
+    } else {
+        // search the next axis in the order whose flag is set
+        for (int i=0; i<9; i++) {
+            if (axis == homing_order[i]) {
+                for (int j=i+1; j<9; j++) {
+                    if (hm.axis_flags[homing_order[j]]) {
+                        return homing_order[j];
+                    }
+                }
+                return -1; // no more axes, we are done
+            }
         }
+        return -1; // called with non-existing axis, should never happen (but compiler insists on some return value)
     }
-    return (-1);  // done
-
-#endif  //  (HOMING_AXES <= 4)
 }
diff --git a/g2core/cycle_probing.cpp b/g2core/cycle_probing.cpp
index 3f3db80f1..f8ab94235 100644
--- a/g2core/cycle_probing.cpp
+++ b/g2core/cycle_probing.cpp
@@ -138,6 +138,7 @@ uint8_t cm_straight_probe(float target[], bool flags[], bool trip_sense, bool al
 
     // error if no axes specified
     if (!(flags[AXIS_X] | flags[AXIS_Y] | flags[AXIS_Z] |
+          flags[AXIS_U] | flags[AXIS_V] | flags[AXIS_W] |
           flags[AXIS_A] | flags[AXIS_B] | flags[AXIS_C])) {
         return(cm_alarm(STAT_AXIS_IS_MISSING, "Axis is missing"));
     }
diff --git a/g2core/g2core.h b/g2core/g2core.h
index 9534ff7bf..798cc41e5 100644
--- a/g2core/g2core.h
+++ b/g2core/g2core.h
@@ -61,7 +61,7 @@ typedef uint16_t magic_t;		        // magic number size
 // Note: If you change COORDS you must adjust the entries in cfgArray table in config.c
 
 #define AXES 9          // number of axes supported in this version
-#define HOMING_AXES 4   // number of axes that can be homed (assumes Zxyabc sequence)
+#define HOMING_AXES 9   // number of axes that can be homed (assumes Zxyabc sequence)
 #define COORDS 6        // number of supported coordinate systems (index starts at 1)
 #define TOOLS 32        // number of entries in tool table (index starts at 1)
 
diff --git a/g2core/plan_line.cpp b/g2core/plan_line.cpp
index 5eecf976b..f69cbd2d2 100644
--- a/g2core/plan_line.cpp
+++ b/g2core/plan_line.cpp
@@ -639,8 +639,12 @@ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const floa
             bf->gm.feed_rate_mode = UNITS_PER_MINUTE_MODE;
         } else {
             // compute length of linear move in millimeters. Feed rate is provided as mm/min
+ #if (AXES == 9)
+            feed_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + axis_square[AXIS_Z] + axis_square[AXIS_U] + axis_square[AXIS_V] + axis_square[AXIS_W]) / bf->gm.feed_rate;
+ #else
             feed_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + axis_square[AXIS_Z]) / bf->gm.feed_rate;
-            // if no linear axes, compute length of multi-axis rotary move in degrees. 
+ #endif
+            // if no linear axes, compute length of multi-axis rotary move in degrees.
             // Feed rate is provided as degrees/min
             if (fp_ZERO(feed_time)) {
                 feed_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + axis_square[AXIS_C]) / bf->gm.feed_rate;
diff --git a/g2core/util.cpp b/g2core/util.cpp
index 61f4c4b0d..952bb0fe9 100644
--- a/g2core/util.cpp
+++ b/g2core/util.cpp
@@ -76,6 +76,9 @@ float get_axis_vector_length(const float a[], const float b[])
     return (sqrt(square(a[AXIS_X] - b[AXIS_X]) +
                  square(a[AXIS_Y] - b[AXIS_Y]) +
                  square(a[AXIS_Z] - b[AXIS_Z]) +
+                 square(a[AXIS_U] - b[AXIS_U]) +
+                 square(a[AXIS_V] - b[AXIS_V]) +
+                 square(a[AXIS_W] - b[AXIS_W]) +
                  square(a[AXIS_A] - b[AXIS_A]) +
                  square(a[AXIS_B] - b[AXIS_B]) +
                  square(a[AXIS_C] - b[AXIS_C])));