Include sqrtll approximation for more than 48 valid bits.

Signed-off-by: Pavel Pisa <ppisa@pikron.com>

diff --git a/sw/app/rocon/math_sqrtll.c b/sw/app/rocon/math_sqrtll.c
index b63659de252fd05d0a8d3bbe0816df070ae8329e..b05123a0c4d62b1118ca85cadd52ab51e7d04f1f 100644 (file)
--- a/sw/app/rocon/math_sqrtll.c
+++ b/sw/app/rocon/math_sqrtll.c
@@ -57,7 +57,7 @@ uint32_t
 unsigned long sqrtll(unsigned long long x)
 {
   uint32_t y,t1,t2;
-  uint32_t x0=x, x1=x>>32;
+  uint32_t x0 = x, x1 = x >> 32;
 
  #ifndef __GNUC__
 
@@ -82,8 +82,8 @@ unsigned long sqrtll(unsigned long long x)
     }
   }
 
-  while(!(t1&0x80000000)){
-    t1<<=1;
+  while (!(t1 & 0x80000000)) {
+    t1 <<= 1;
     t2--;
   }
  #else /*__GNUC__*/
@@ -93,7 +93,7 @@ unsigned long sqrtll(unsigned long long x)
     t2 = __builtin_clz(x1);
     t1 = (x1 << t2) | ((x0 >> (32 - t2)));
     t2 = 63 - t2;
-  }else{
+  } else {
     if (!x0)
       return 0;
     t2 = __builtin_clz(x0);
@@ -121,7 +121,7 @@ unsigned long sqrtll(unsigned long long x)
   if (!x1) {
     y= (y + x0 / y + 1) / 2;
     y= (y + x0 / y + 1) / 2;
-  } else if (t2<24) {
+  } else if (x1 < 0x00010000) {
     uint32_t y0, y1, y2, xt, r;
 
     xt = (x1 << 16) | (x0 >> 16);
@@ -153,6 +153,24 @@ unsigned long sqrtll(unsigned long long x)
   return y;
 }
 
+#ifdef __GNUC__
+
+unsigned long sqrtll_approx(unsigned long long x)
+{
+  unsigned int r;
+  uint32_t x1 = x >> 32;
+  if (x1 < 0x00010000)
+    return sqrtll(x);
+
+  r = __builtin_clz(x1);
+  r = (17 - r) / 2;
+  x1 = sqrtll(x >> (r * 2));
+
+  return x1 << r;
+}
+
+#endif /*__GNUC__*/
+
 #else
 
 unsigned long sqrtll(unsigned long long x)

diff --git a/sw/app/rocon/math_sqrtll_test.c b/sw/app/rocon/math_sqrtll_test.c
index 8436cb230be1c3062c64c4c6cd1784531db5e3cf..4ff6e4cec1a943fa81eeecaf31281a6e1db9eaa7 100644 (file)
--- a/sw/app/rocon/math_sqrtll_test.c
+++ b/sw/app/rocon/math_sqrtll_test.c
@@ -6,6 +6,7 @@
  #include <system_def.h>
  #include <lt_timer.h>
  unsigned long sqrtll(unsigned long long x);
+ unsigned long sqrtll_approx(unsigned long long x);
  #define main sqrtll_main
  #ifdef LPC_TIM0
   #define sqrtll_ticks() (LPC_TIM0->TC)
@@ -38,27 +39,32 @@ int main(int argc, char *argv[])
     sqr=(unsigned long long int)inp*inp;
     cli();
     spent=sqrtll_ticks();
-    res=sqrtll(sqr);
+    res=sqrtll_approx(sqr);
     spent=sqrtll_ticks()-spent;
     sti();
     printf("sqrtll: inp=%lu, res=%lu, spent=%u\n",inp,res,(int)spent);
   }else{
     inp=0; incr=1; cnt=0;
+    int eps=1;
     lt_mstime_update();
     time=actual_msec;
     do{
-      sqr=(unsigned long long int)inp*inp;
-      res=sqrtll(sqr);
-      if(0||(inp!=res)){
-        printf("sqrt error %lu -> %lu diff %ld sqr 0x%llx\n",inp,res,inp-res,sqr);
-        res=sqrtll(sqr);
+      sqr=(unsigned long long int)inp * inp;
+      res=sqrtll_approx(sqr);
+      if(0||(labs(inp-res)) >= eps){
+        printf("\nsqrt error %lu -> %lu diff %ld sqr 0x%llx\n",inp,res,inp-res,sqr);
+        eps<<=1;
+        res=sqrtll_approx(sqr);
       }
-      inp+=incr; cnt++;
+      cnt++;
+      if(inp>0xffffffff-incr)
+        break;
+      inp+=incr;
       if(inp&0xfff) continue;
       if(inp&((incr<<19)-1)) continue;
       incr<<=1;
       printf(".");fflush(NULL);
-    }while(inp);
+    }while(1);
     lt_mstime_update();
     time=actual_msec-time;
     printf("\nsqrtll OK, spent %ld cnt %ld\n",time,cnt);