From 56c2238fef8138c7cd744b0963a90323f245bf35 Mon Sep 17 00:00:00 2001
From: Nico Tonnhofer <wurstnase.reprap@gmail.com>
Date: Tue, 18 Oct 2016 19:50:25 +0200
Subject: [PATCH] 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       | 10 ++++++++++
 dda_maths.c | 13 +++++++++++++
 dda_maths.h |  3 +++
 3 files changed, 26 insertions(+)

diff --git a/dda.c b/dda.c
index d08bc3a..f890945 100644
--- a/dda.c
+++ b/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
diff --git a/dda_maths.c b/dda_maths.c
index cb5c05e..d364940 100644
--- a/dda_maths.c
+++ b/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
diff --git a/dda_maths.h b/dda_maths.h
index c0fb7b6..ccb571e 100644
--- a/dda_maths.h
+++ b/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);