maths: aproximation of sqrt

example/hsp_ag_demo.c

@@ -55,7 +55,6 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <math.h>
 #include <unistd.h>
 
 #include "btstack.h"

example/hsp_hs_demo.c

@@ -55,7 +55,6 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <math.h>
 #include <unistd.h>
 
 #include "btstack.h"

src/classic/btstack_cvsd_plc.c

@@ -45,7 +45,6 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <math.h>
 
 #include "btstack_cvsd_plc.h"
 #include "btstack_debug.h"
@@ -56,6 +55,30 @@ static float rcos[CVSD_OLAL] = {
     0.45386582,0.36316850,0.27713082,0.19868268, 
     0.13049554,0.07489143,0.03376389,0.00851345};
 
+// taken from http://www.codeproject.com/Articles/69941/Best-Square-Root-Method-Algorithm-Function-Precisi
+// Algorithm: Babylonian Method + some manipulations on IEEE 32 bit floating point representation
+static float sqrt3(const float x){
+    union {
+        int i;
+        float x;
+    } u;
+    u.x = x;
+    u.i = (1<<29) + (u.i >> 1) - (1<<22); 
+
+    // Two Babylonian Steps (simplified from:)
+    // u.x = 0.5f * (u.x + x/u.x);
+    // u.x = 0.5f * (u.x + x/u.x);
+    u.x =       u.x + x/u.x;
+    u.x = 0.25f*u.x + x/u.x;
+
+    return u.x;
+}
+
+static float absolute(float x){
+     if (x < 0) x = -x;
+     return x;
+}
+
 static float CrossCorrelation(int8_t *x, int8_t *y){
     float num = 0;
     float den = 0;
@@ -67,7 +90,7 @@ static float CrossCorrelation(int8_t *x, int8_t *y){
         x2+=((float)x[m])*x[m];
         y2+=((float)y[m])*y[m];
     }
-    den = (float)sqrt(x2*y2);
+    den = (float)sqrt3(x2*y2);
     return num/den;
 }
 
@@ -93,8 +116,8 @@ static float AmplitudeMatch(int8_t *y, int8_t bestmatch) {
     float sf;
     
     for (i=0;i<CVSD_FS;i++){
-        sumx += abs(y[CVSD_LHIST-CVSD_FS+i]);
-        sumy += abs(y[bestmatch+i]);
+        sumx += absolute(y[CVSD_LHIST-CVSD_FS+i]);
+        sumy += absolute(y[bestmatch+i]);
     }
     sf = sumx/sumy;
     // This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts 

src/classic/btstack_sbc_plc.c

@@ -44,7 +44,6 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <math.h>
 
 #include "btstack_sbc_plc.h"
 
@@ -61,6 +60,30 @@ static float rcos[SBC_OLAL] = {
     0.45386582f,0.36316850f,0.27713082f,0.19868268f, 
     0.13049554f,0.07489143f,0.03376389f,0.00851345f};
 
+// taken from http://www.codeproject.com/Articles/69941/Best-Square-Root-Method-Algorithm-Function-Precisi
+// Algorithm: Babylonian Method + some manipulations on IEEE 32 bit floating point representation
+static float sqrt3(const float x){
+    union {
+        int i;
+        float x;
+    } u;
+    u.x = x;
+    u.i = (1<<29) + (u.i >> 1) - (1<<22); 
+
+    // Two Babylonian Steps (simplified from:)
+    // u.x = 0.5f * (u.x + x/u.x);
+    // u.x = 0.5f * (u.x + x/u.x);
+    u.x =       u.x + x/u.x;
+    u.x = 0.25f*u.x + x/u.x;
+
+    return u.x;
+}
+
+static float absolute(float x){
+     if (x < 0) x = -x;
+     return x;
+}
+
 static float CrossCorrelation(int16_t *x, int16_t *y){
     float num = 0;
     float den = 0;
@@ -72,7 +95,7 @@ static float CrossCorrelation(int16_t *x, int16_t *y){
         x2+=((float)x[m])*x[m];
         y2+=((float)y[m])*y[m];
     }
-    den = (float)sqrt(x2*y2);
+    den = (float)sqrt3(x2*y2);
     return num/den;
 }
 
@@ -99,8 +122,8 @@ static float AmplitudeMatch(int16_t *y, int16_t bestmatch) {
     float sf;
     
     for (i=0;i<SBC_FS;i++){
-        sumx += abs(y[SBC_LHIST-SBC_FS+i]);
-        sumy += abs(y[bestmatch+i]);
+        sumx += absolute(y[SBC_LHIST-SBC_FS+i]);
+        sumy += absolute(y[bestmatch+i]);
     }
     sf = sumx/sumy;
     /* This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts */

test/maths/.gitignore

@@ -0,0 +1 @@
+sqrt_test

test/maths/Makefile

@@ -0,0 +1,23 @@
+CC=g++
+
+# Requirements: cpputest.github.io
+
+BTSTACK_ROOT =  ../..
+CPPUTEST_HOME = ${BTSTACK_ROOT}/test/cpputest
+
+CFLAGS  = -g -Wall -I. -I../ -I${BTSTACK_ROOT}/src -I${BTSTACK_ROOT}/include
+LDFLAGS += -lCppUTest -lCppUTestExt
+
+COMMON_OBJ = $(COMMON:.c=.o)
+
+all: sqrt_test
+
+btstack_linked_list_test: ${COMMON_OBJ} sqrt_test.c
+	${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@
+
+test: all
+	./sqrt_test
+	
+clean:
+	rm -fr sqrt_test *.dSYM *.o ../src/*.o
+	

test/maths/sqrt_test.c

@@ -0,0 +1,104 @@
+#include <math.h>
+#include <sys/time.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "CppUTest/TestHarness.h"
+#include "CppUTest/CommandLineTestRunner.h"
+
+static struct timeval init_tv;
+
+union {
+    int i;
+    float x;
+} u;
+
+// taken from http://www.codeproject.com/Articles/69941/Best-Square-Root-Method-Algorithm-Function-Precisi
+// Algorithm: Babylonian Method + some manipulations on IEEE 32 bit floating point representation
+float sqrt1(const float x){
+    u.x = x;
+    u.i = (1<<29) + (u.i >> 1) - (1<<22); 
+
+    // Two Babylonian Steps (simplified from:)
+    // u.x = 0.5f * (u.x + x/u.x);
+    // u.x = 0.5f * (u.x + x/u.x);
+    u.x =       u.x + x/u.x;
+    u.x = 0.25f*u.x + x/u.x;
+
+    return u.x;
+}  
+
+// Algorithm: Log base 2 approximation and Newton's Method
+float sqrt3(const float x){
+    u.x = x;
+    u.i = (1<<29) + (u.i >> 1) - (1<<22); 
+    return u.x;
+}
+
+float sqrt2(const float n) {
+    /*using n itself as initial approximation => improve */
+    float x = n;
+    float y = 1;
+    float e = 0.001; /* e decides the accuracy level*/
+    while(x - y > e){
+        x = (x + y)/2;
+        y = n/x;
+    }
+    return x;
+}
+
+static uint32_t get_time_ms(void){
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+    uint32_t time_ms = (uint32_t)((tv.tv_sec  - init_tv.tv_sec) * 1000) + (tv.tv_usec / 1000);
+    return time_ms;
+}
+
+static int values_len = 100000;
+
+TEST_GROUP(SqrtTest){
+    void setup(void){
+    }
+
+    void test_method(float (*my_sqrt)(const float x)){
+        int i, j;
+        float precision = 0;
+        int ta = 0;
+        int te = 0;
+
+        for (j=0; j<100; j++){
+            for (i=0; i<values_len; i++){
+                int t1 = get_time_ms();
+                float expected = sqrt(i);
+                int t2 = get_time_ms();
+                float actual = my_sqrt(i);
+                int t3 = get_time_ms();
+                te += t2 - t1;
+                ta += t3 - t2;    
+                precision += fabs(expected - actual);
+            }
+        }
+        printf("Precision: %f, Time: (%d, %d)ms\n", precision/values_len, te, ta);
+    }
+};
+
+TEST(SqrtTest, Sqrt1){
+    printf("\nsqrt1: ");
+    test_method(sqrt1);
+}
+
+TEST(SqrtTest, Sqrt2){
+    printf("\nsqrt2: ");
+    test_method(sqrt2);
+}
+
+TEST(SqrtTest, Sqrt3){
+    printf("\nsqrt3: ");
+    test_method(sqrt3);
+}
+
+int main (int argc, const char * argv[]){
+    return CommandLineTestRunner::RunAllTests(argc, argv);
+}
+
+// TODO: check http://www.embedded.com/electronics-blogs/programmer-s-toolbox/4219659/Integer-Square-Roots

test/pts/hsp_hs_test.c

@@ -45,7 +45,6 @@
 
 #include <errno.h>
 #include <fcntl.h>
-#include <math.h>
 #include <net/if_arp.h>
 #include <stdint.h>
 #include <stdio.h>