dda.c/dda_maths: add int_f_sqrt for controller with FPU

very fast sqrt in hardware
also accurate dda->c for high steps/mm without overflowing

dda.c

@@ -414,8 +414,13 @@ void dda_create(DDA *dda, const TARGET *target) {
         if (dda->n == 0)
           dda->c = pgm_read_dword(&c0_P[dda->fast_axis]);
         else
+          #if __FPU_PRESENT
+          dda->c = (pgm_read_dword(&c0_P[dda->fast_axis]) /
+                    (2 * int_f_sqrt(dda->n)));
+          #else
           dda->c = (pgm_read_dword(&c0_P[dda->fast_axis]) *
                     int_inv_sqrt(dda->n)) >> 13;
+          #endif
         if (dda->c < dda->c_min)
           dda->c = dda->c_min;
       #else
@@ -874,8 +879,13 @@ void dda_clock() {
         // Explicit formula: c0 * (sqrt(n + 1) - sqrt(n)),
         // approximation here: c0 * (1 / (2 * sqrt(n))).
         // This >> 13 looks odd, but is verified with the explicit formula.
+        #if __FPU_PRESENT
+        move_c = (pgm_read_dword(&c0_P[dda->fast_axis]) /
+                  (2 * int_f_sqrt(move_n)));
+        #else
         move_c = (pgm_read_dword(&c0_P[dda->fast_axis]) *
                   int_inv_sqrt(move_n)) >> 13;
+        #endif
 
       // TODO: most likely this whole check is obsolete. It was left as a
       //       safety margin, only. Rampup steps calculation should be accurate

dda_maths.c

@@ -153,6 +153,19 @@ uint32_t approx_distance_3(uint32_t dx, uint32_t dy, uint32_t dz) {
   return (( approx + 512 ) >> 10 );
 }
 
+#if __FPU_PRESENT
+uint_fast16_t int_f_sqrt(uint32_t a) {
+  // using FPUs floating square root to return an unsigned fast16 bit result
+  float result;
+
+  __ASM volatile ("vcvt.f32.u32 %[result], %[a];\n"
+                  "  vsqrt.f32 %[result], %[result];"
+                  : [result]"=t" (result)
+                  : [a]"t" (a));
+
+  return (uint_fast16_t)(result);
+}
+#endif /* __FPU_PRESENT */
 /*!
   integer square root algorithm
   \param a find square root of this number

dda_maths.h

@@ -42,6 +42,9 @@ uint32_t approx_distance_3(uint32_t dx, uint32_t dy, uint32_t dz);
 
 // integer square root algorithm
 uint16_t int_sqrt(uint32_t a);
+#if __FPU_PRESENT
+uint_fast16_t int_f_sqrt(uint32_t a);
+#endif
 
 // integer inverse square root, 12bits precision
 uint16_t int_inv_sqrt(uint16_t a);