Added math library to build.

6 files changed, 186 insertions, 12 deletions

diff --git a/apps/mk/app.mk b/apps/mk/app.mk
index 3948ef354..f1cbb87f0 100644
--- a/apps/mk/app.mk
+++ b/apps/mk/app.mk
@@ -101,7 +101,9 @@ INCLUDES	+= $(APPDIR)
 
 ASRCS		?= $(wildcard $(SRCDIR)/*.S)
 CSRCS		?= $(wildcard $(SRCDIR)/*.c)
+CHDRS		?= $(wildcard $(SRCDIR)/*.h)
 CXXSRCS		?= $(wildcard $(SRCDIR)/*.cpp)
+CXXHDRS		?= $(wildcard $(SRCDIR)/*.hpp)
 
 # if APPNAME is not set, this is a library
 ifeq ($(APPNAME),)
@@ -179,8 +181,8 @@ all:		.built
 #
 depend:		.depend
 .depend:	$(MAKEFILE_LIST) $(SRCS)
-	@$(MKDEP) --dep-path . $(CC) -- $(CFLAGS) -- $(CSRCS) >Make.dep
-	@$(MKDEP) --dep-path . $(CXX) -- $(CXXFLAGS) -- $(CXXSRCS) >>Make.dep
+	@$(MKDEP) --dep-path . $(CC) -- $(CFLAGS) -- $(CSRCS) $(CHDRS) >Make.dep
+	@$(MKDEP) --dep-path . $(CXX) -- $(CXXFLAGS) -- $(CXXSRCS) $(CXXHDRS) >>Make.dep
 	@touch $@
 
 ifneq ($(APPNAME),)
diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile
index 5778c552e..b3e7f6ac9 100644
--- a/apps/systemlib/Makefile
+++ b/apps/systemlib/Makefile
@@ -48,14 +48,40 @@ CSRCS		 = err.c \
 #		   ppm_decode.c \
 
 
+CXXSRCS   =  test/test.cpp \
+			 math/Vector.cpp \
+			 math/Vector3.cpp \
+			 math/EulerAngles.cpp \
+			 math/Quaternion.cpp \
+			 math/Dcm.cpp \
+			 math/Matrix.cpp
+
+CXXHDRS   =  test/test.hpp \
+			 math/Vector.hpp \
+			 math/Vector3.hpp \
+			 math/EulerAngles.hpp \
+			 math/Quaternion.hpp \
+			 math/Dcm.hpp \
+			 math/Matrix.hpp
 
 #
 # XXX this really should be a CONFIG_* test
 #
 ifeq ($(TARGET),px4fmu)
+INCLUDES += math/arm
 CSRCS		+= systemlib.c \
-		   pid/pid.c \
-		   geo/geo.c
+		pid/pid.c \
+		geo/geo.c
+CXXSRCS     += math/arm/Vector.cpp \
+		math/arm/Matrix.cpp
+CXXHDRS     += math/arm/Vector.hpp \
+	    math/arm/Matrix.hpp
+else
+INCLUDES += math/generic
+CXXSRCS     += math/generic/Vector.cpp \
+		math/generic/Matrix.cpp
+CXXHDRS     += math/generic/Vector.hpp \
+	    math/generic/Matrix.hpp
 endif
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/systemlib/test/test.cpp b/apps/systemlib/test/test.cpp
new file mode 100644
index 000000000..aa00e66fa
--- /dev/null
+++ b/apps/systemlib/test/test.cpp
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test.cpp
+ *
+ * Test library code
+ */
+
+#include <stdio.h>
+#include <math.h>
+
+#include "test.hpp"
+
+bool __EXPORT equal(float a, float b, float epsilon)
+{
+    float diff = fabsf(a-b);
+    if (diff>epsilon)
+    {
+        printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); 
+        return false;
+    }
+    else return true;
+}
+
+void __EXPORT float2SigExp(
+        const float & num,
+        float & sig,
+        int & exp)
+{
+    if (isnan(num) || isinf(num))
+    {
+        sig = 0.0f;
+        exp = -99;
+        return;
+    }
+    if (fabsf(num) < 1.0e-38f)
+    {
+        sig = 0;
+        exp = 0;
+        return;
+    }
+    exp = log10f(fabsf(num));
+    if (exp>0) {
+        exp = ceil(exp);
+    } else {
+        exp = floor(exp);
+    }
+    sig = num;
+    // cheap power since it is integer
+    if (exp>0) {
+        for (int i=0;i<abs(exp);i++) sig /= 10;
+    } else {
+        for (int i=0;i<abs(exp);i++) sig *= 10;
+    }
+}
+
+
diff --git a/apps/systemlib/test/test.hpp b/apps/systemlib/test/test.hpp
new file mode 100644
index 000000000..860758e7d
--- /dev/null
+++ b/apps/systemlib/test/test.hpp
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test.hpp
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <assert.h>
+#include <time.h>
+#include <stdlib.h>
+
+bool equal(float a, float b, float eps = 1e-5);
+void float2SigExp(
+        const float & num,
+        float & sig,
+        int & exp);
diff --git a/nuttx/configs/px4fmu/common/Make.defs b/nuttx/configs/px4fmu/common/Make.defs
index ff2e4c5fa..44082f64e 100644
--- a/nuttx/configs/px4fmu/common/Make.defs
+++ b/nuttx/configs/px4fmu/common/Make.defs
@@ -55,7 +55,7 @@ NM			 = $(CROSSDEV)nm
 OBJCOPY			 = $(CROSSDEV)objcopy
 OBJDUMP			 = $(CROSSDEV)objdump
 
-MAXOPTIMIZATION		 = -O3
+MAXOPTIMIZATION		 = -O3 # disable for debugging
 ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
 			   -mthumb \
 			   -march=armv7e-m \
@@ -67,6 +67,11 @@ ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
 INSTRUMENTATIONDEFINES	 = -finstrument-functions \
 			   -ffixed-r10
 
+#CMSIS
+CMSIS_INCLUDE = $(TOPDIR)/../CMSIS/Include
+CMSIS_LIB = $(TOPDIR)/../CMSIS/Lib/GCC/libarm_cortexM4lf_math.a
+EXTRA_LIBS		+= $(CMSIS_LIB)
+
 # pull in *just* libm from the toolchain ... this is grody
 LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
 EXTRA_LIBS		+= $(LIBM)
@@ -79,8 +84,8 @@ ifeq ($(WINTOOL),y)
   DIRLINK		 = $(TOPDIR)/tools/winlink.sh
   DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
   MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
-  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
-  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(CMSIS_INCLUDE)}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" -isystem "${shell cygpath -w $(CMSIS_INCLUDE)}"
   ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}"
 else
   ifeq ($(PX4_WINTOOL),y)
@@ -88,14 +93,14 @@ else
     DIRLINK		 = $(TOPDIR)/tools/winlink.sh
     DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
     MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
-    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
-    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(CMSIS_INCLUDE)
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx -isystem $(CMSIS_INCLUDE)
     ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)
   else
     # Linux/Cygwin-native toolchain 
     MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
-    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
-    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(CMSIS_INCLUDE)
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx -isystem $(CMSIS_INCLUDE)
     ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)
   endif
 endif
@@ -144,7 +149,7 @@ ARCHCWARNINGS		 = $(ARCHWARNINGS) \
 			   -Wnested-externs \
 			   -Wunsuffixed-float-constants
 ARCHWARNINGSXX		 = $(ARCHWARNINGS)
-ARCHDEFINES		 = 
+ARCHDEFINES		 = -DARM_MATH_CM4 -D__FPU_PRESENT
 ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
 
 # this seems to be the only way to add linker flags
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index 7f1a93df9..7048ef82f 100644
--- a/nuttx/configs/px4fmu/nsh/appconfig
+++ b/nuttx/configs/px4fmu/nsh/appconfig
@@ -65,6 +65,9 @@ CONFIGURED_APPS += systemcmds/delay_test
 # https://pixhawk.ethz.ch/px4/dev/deamon
 # CONFIGURED_APPS += examples/px4_deamon_app
 
+# Math library
+CONFIGURED_APPS += examples/math_demo
+
 # Shared object broker; required by many parts of the system.
 CONFIGURED_APPS += uORB