diff --git a/sw/airborne/math/Makefile b/sw/airborne/math/Makefile
index ab90c6a2090..11cba79adb2 100644
--- a/sw/airborne/math/Makefile
+++ b/sw/airborne/math/Makefile
@@ -31,7 +31,7 @@ all:
 	@cat README
 
 shared_lib: $(OBJ)
-	$(CC) -shared -o $(BUILDDIR)/$(LIBNAME).so $(OBJ)
+	$(CC) -shared -o $(BUILDDIR)/$(LIBNAME).so $(OBJ) -lm
 
 install_shared_lib: shared_lib
 	test -d $(LIB_INSTALLDIR) || mkdir -p $(LIB_INSTALLDIR)
diff --git a/tests/Makefile b/tests/Makefile
index 37c10dca345..ba27ec065cc 100644
--- a/tests/Makefile
+++ b/tests/Makefile
@@ -26,6 +26,7 @@ else
 endif
 
 test:
+	$(Q)make -C math test
 	$(Q)$(PERLENV) $(PERL) "-e" "$(RUNTESTS)"
 
 clean:
diff --git a/tests/math/.gitignore b/tests/math/.gitignore
new file mode 100644
index 00000000000..5ad50ca5138
--- /dev/null
+++ b/tests/math/.gitignore
@@ -0,0 +1 @@
+test_pprz_math.run
diff --git a/tests/math/Makefile b/tests/math/Makefile
new file mode 100644
index 00000000000..3806678baf1
--- /dev/null
+++ b/tests/math/Makefile
@@ -0,0 +1,64 @@
+# Copyright (C) 2014 Piotr Esden-Tempski
+#
+# This file is part of paparazzi.
+#
+# paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with paparazzi; see the file COPYING.  If not, see
+# <http://www.gnu.org/licenses/>.
+
+# The default is to produce a quiet echo of compilation commands
+# Launch with "make Q=''" to get full echo
+
+# Make sure all our environment is set properly in case we run make not from toplevel director.
+Q ?= @
+
+PAPARAZZI_SRC ?= $(shell pwd)/../..
+ifeq ($(PAPARAZZI_HOME),)
+PAPARAZZI_HOME=$(PAPARAZZI_SRC)
+endif
+
+# export the PAPARAZZI environment to sub-make
+export PAPARAZZI_SRC
+export PAPARAZZI_HOME
+
+MATHSRC_PATH=$(PAPARAZZI_SRC)/sw/airborne/math
+MATHLIB_PATH=$(PAPARAZZI_SRC)/var/build/math
+
+#####################################################
+# If you add more test files you add their names here
+TESTS = test_pprz_math.run
+
+###################################################
+# You should not need to touch the rest of the file
+
+all: test
+
+math_shlib:
+	$(Q)cd $(MATHSRC_PATH); make shared_lib
+
+build_tests: math_shlib $(TESTS)
+
+test: build_tests
+	$(Q)for i in $(TESTS); do \
+	./$$i;	\
+	done
+
+%.run: %.c
+	$(CC) -L$(MATHLIB_PATH) -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/sw/include tap.c $< -lpprzmath -o $@
+
+clean:
+	$(Q)rm -f $(MATHLIB_PATH)/*.o $(MATHLIB_PATH)/libpprzmath.so
+	$(Q)rm -f $(TESTS)
+
+
+.PHONY: math_shlib build_tests test clean all
diff --git a/tests/math/tap.c b/tests/math/tap.c
new file mode 100644
index 00000000000..e037ba63369
--- /dev/null
+++ b/tests/math/tap.c
@@ -0,0 +1,372 @@
+/*
+libtap - Write tests in C
+Copyright 2012 Jake Gelbman <gelbman@gmail.com>
+This file is licensed under the GPLv2 or any later version
+*/
+
+#define _BSD_SOURCE 1
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <string.h>
+#include "tap.h"
+
+static int expected_tests = NO_PLAN;
+static int failed_tests;
+static int current_test;
+static char *todo_mesg;
+
+static char *
+vstrdupf (const char *fmt, va_list args) {
+    char *str;
+    int size;
+    va_list args2;
+    va_copy(args2, args);
+    if (!fmt)
+        fmt = "";
+    size = vsnprintf(NULL, 0, fmt, args2) + 2;
+    str = malloc(size);
+    if (!str) {
+        perror("malloc error");
+        exit(1);
+    }
+    vsprintf(str, fmt, args);
+    va_end(args2);
+    return str;
+}
+
+void
+tap_plan (int tests, const char *fmt, ...) {
+    expected_tests = tests;
+    if (tests == SKIP_ALL) {
+        char *why;
+        va_list args;
+        va_start(args, fmt);
+        why = vstrdupf(fmt, args);
+        va_end(args);
+        printf("1..0 ");
+        note("SKIP %s\n", why);
+        exit(0);
+    }
+    if (tests != NO_PLAN) {
+        printf("1..%d\n", tests);
+    }
+}
+
+int
+vok_at_loc (const char *file, int line, int test, const char *fmt,
+            va_list args)
+{
+    char *name = vstrdupf(fmt, args);
+    if (!test)
+        printf("not ");
+    printf("ok %d", ++current_test);
+    if (*name)
+        printf(" - %s", name);
+    if (todo_mesg) {
+        printf(" # TODO");
+        if (*todo_mesg)
+            printf(" %s", todo_mesg);
+    }
+    printf("\n");
+    if (!test) {
+        fprintf(stderr, "#   Failed ");
+        if (todo_mesg)
+            fprintf(stderr, "(TODO) ");
+        fprintf(stderr, "test ");
+        if (*name)
+            fprintf(stderr, "'%s'\n#   ", name);
+        fprintf(stderr, "at %s line %d.\n", file, line);
+        if (!todo_mesg)
+            failed_tests++;
+    }
+    free(name);
+    return test;
+}
+
+int
+ok_at_loc (const char *file, int line, int test, const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    vok_at_loc(file, line, test, fmt, args);
+    va_end(args);
+    return test;
+}
+
+static int
+mystrcmp (const char *a, const char *b) {
+    return a == b ? 0 : !a ? -1 : !b ? 1 : strcmp(a, b);
+}
+
+#define eq(a, b) (!mystrcmp(a, b))
+#define ne(a, b) (mystrcmp(a, b))
+
+int
+is_at_loc (const char *file, int line, const char *got, const char *expected,
+           const char *fmt, ...)
+{
+    int test = eq(got, expected);
+    va_list args;
+    va_start(args, fmt);
+    vok_at_loc(file, line, test, fmt, args);
+    va_end(args);
+    if (!test) {
+        diag("         got: '%s'", got);
+        diag("    expected: '%s'", expected);
+    }
+    return test;
+}
+
+int
+isnt_at_loc (const char *file, int line, const char *got, const char *expected,
+             const char *fmt, ...)
+{
+    int test = ne(got, expected);
+    va_list args;
+    va_start(args, fmt);
+    vok_at_loc(file, line, test, fmt, args);
+    va_end(args);
+    if (!test) {
+        diag("         got: '%s'", got);
+        diag("    expected: anything else");
+    }
+    return test;
+}
+
+int
+cmp_ok_at_loc (const char *file, int line, int a, const char *op, int b,
+               const char *fmt, ...)
+{
+    int test = eq(op, "||") ? a || b
+             : eq(op, "&&") ? a && b
+             : eq(op, "|")  ? a |  b
+             : eq(op, "^")  ? a ^  b
+             : eq(op, "&")  ? a &  b
+             : eq(op, "==") ? a == b
+             : eq(op, "!=") ? a != b
+             : eq(op, "<")  ? a <  b
+             : eq(op, ">")  ? a >  b
+             : eq(op, "<=") ? a <= b
+             : eq(op, ">=") ? a >= b
+             : eq(op, "<<") ? a << b
+             : eq(op, ">>") ? a >> b
+             : eq(op, "+")  ? a +  b
+             : eq(op, "-")  ? a -  b
+             : eq(op, "*")  ? a *  b
+             : eq(op, "/")  ? a /  b
+             : eq(op, "%")  ? a %  b
+             : diag("unrecognized operator '%s'", op);
+    va_list args;
+    va_start(args, fmt);
+    vok_at_loc(file, line, test, fmt, args);
+    va_end(args);
+    if (!test) {
+        diag("    %d", a);
+        diag("        %s", op);
+        diag("    %d", b);
+    }
+    return test;
+}
+
+static int
+find_mem_diff (const char *a, const char *b, size_t n, size_t *offset) {
+    size_t i;
+    if (a == b)
+        return 0;
+    if (!a || !b)
+        return 2;
+    for (i = 0; i < n; i++) {
+        if (a[i] != b[i]) {
+            *offset = i;
+            return 1;
+        }
+    }
+    return 0;
+}
+
+int
+cmp_mem_at_loc (const char *file, int line, const void *got,
+                const void *expected, size_t n, const char *fmt, ...)
+{
+    size_t offset;
+    int diff = find_mem_diff(got, expected, n, &offset);
+    va_list args;
+    va_start(args, fmt);
+    vok_at_loc(file, line, !diff, fmt, args);
+    va_end(args);
+    if (diff == 1) {
+        diag("    Difference starts at offset %d", offset);
+        diag("         got: 0x%02x", ((unsigned char *)got)[offset]);
+        diag("    expected: 0x%02x", ((unsigned char *)expected)[offset]);
+    }
+    else if (diff == 2) {
+        diag("         got: %s", got ? "not NULL" : "NULL");
+        diag("    expected: %s", expected ? "not NULL" : "NULL");
+    }
+    return !diff;
+}
+
+static void
+vdiag_to_fh (FILE *fh, const char *fmt, va_list args) {
+    char *mesg, *line;
+    int i;
+    if (!fmt)
+        return;
+    mesg = vstrdupf(fmt, args);
+    line = mesg;
+    for (i = 0; *line; i++) {
+        char c = mesg[i];
+        if (!c || c == '\n') {
+            mesg[i] = '\0';
+            fprintf(fh, "# %s\n", line);
+            if (!c)
+                break;
+            mesg[i] = c;
+            line = mesg + i + 1;
+        }
+    }
+    free(mesg);
+    return;
+}
+
+int
+diag (const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    vdiag_to_fh(stderr, fmt, args);
+    va_end(args);
+    return 0;
+}
+
+int
+note (const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    vdiag_to_fh(stdout, fmt, args);
+    va_end(args);
+    return 0;
+}
+
+int
+exit_status () {
+    int retval = 0;
+    if (expected_tests == NO_PLAN) {
+        printf("1..%d\n", current_test);
+    }
+    else if (current_test != expected_tests) {
+        diag("Looks like you planned %d test%s but ran %d.",
+            expected_tests, expected_tests > 1 ? "s" : "", current_test);
+        retval = 255;
+    }
+    if (failed_tests) {
+        diag("Looks like you failed %d test%s of %d run.",
+            failed_tests, failed_tests > 1 ? "s" : "", current_test);
+        if (expected_tests == NO_PLAN)
+            retval = failed_tests;
+        else
+            retval = expected_tests - current_test + failed_tests;
+    }
+    return retval;
+}
+
+int
+bail_out (int ignore, const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    printf("Bail out!  ");
+    vprintf(fmt, args);
+    printf("\n");
+    va_end(args);
+    exit(255);
+    return 0;
+}
+
+void
+tap_skip (int n, const char *fmt, ...) {
+    char *why;
+    va_list args;
+    va_start(args, fmt);
+    why = vstrdupf(fmt, args);
+    va_end(args);
+    while (n --> 0) {
+        printf("ok %d ", ++current_test);
+        note("skip %s\n", why);
+    }
+    free(why);
+}
+
+void
+tap_todo (int ignore, const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    todo_mesg = vstrdupf(fmt, args);
+    va_end(args);
+}
+
+void
+tap_end_todo () {
+    free(todo_mesg);
+    todo_mesg = NULL;
+}
+
+#ifndef _WIN32
+#include <sys/mman.h>
+#include <sys/param.h>
+#include <regex.h>
+
+#if defined __APPLE__ || defined BSD
+#define MAP_ANONYMOUS MAP_ANON
+#endif
+
+/* Create a shared memory int to keep track of whether a piece of code executed
+dies. to be used in the dies_ok and lives_ok macros.  */
+int
+tap_test_died (int status) {
+    static int *test_died = NULL;
+    int prev;
+    if (!test_died) {
+        test_died = mmap(0, sizeof (int), PROT_READ | PROT_WRITE,
+                         MAP_SHARED | MAP_ANONYMOUS, -1, 0);
+        *test_died = 0;
+    }
+    prev = *test_died;
+    *test_died = status;
+    return prev;
+}
+
+int
+like_at_loc (int for_match, const char *file, int line, const char *got,
+             const char *expected, const char *fmt, ...)
+{
+    int test;
+    regex_t re;
+    va_list args;
+    int err = regcomp(&re, expected, REG_EXTENDED);
+    if (err) {
+        char errbuf[256];
+        regerror(err, &re, errbuf, sizeof errbuf);
+        fprintf(stderr, "Unable to compile regex '%s': %s at %s line %d\n",
+                        expected, errbuf, file, line);
+        exit(255);
+    }
+    err = regexec(&re, got, 0, NULL, 0);
+    regfree(&re);
+    test = for_match ? !err : err;
+    va_start(args, fmt);
+    vok_at_loc(file, line, test, fmt, args);
+    va_end(args);
+    if (!test) {
+        if (for_match) {
+            diag("                   '%s'", got);
+            diag("    doesn't match: '%s'", expected);
+        }
+        else {
+            diag("                   '%s'", got);
+            diag("          matches: '%s'", expected);
+        }
+    }
+    return test;
+}
+#endif
+
diff --git a/tests/math/tap.h b/tests/math/tap.h
new file mode 100644
index 00000000000..302e6d85aec
--- /dev/null
+++ b/tests/math/tap.h
@@ -0,0 +1,117 @@
+/*
+libtap - Write tests in C
+Copyright 2012 Jake Gelbman <gelbman@gmail.com>
+This file is licensed under the GPLv2 or any later version
+*/
+
+#ifndef __TAP_H__
+#define __TAP_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef va_copy
+#ifdef __va_copy
+#define va_copy __va_copy
+#else
+#define va_copy(d, s) ((d) = (s))
+#endif
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+
+int     vok_at_loc      (const char *file, int line, int test, const char *fmt,
+                         va_list args);
+int     ok_at_loc       (const char *file, int line, int test, const char *fmt,
+                         ...);
+int     is_at_loc       (const char *file, int line, const char *got,
+                         const char *expected, const char *fmt, ...);
+int     isnt_at_loc     (const char *file, int line, const char *got,
+                         const char *expected, const char *fmt, ...);
+int     cmp_ok_at_loc   (const char *file, int line, int a, const char *op,
+                         int b, const char *fmt, ...);
+int     cmp_mem_at_loc  (const char *file, int line, const void *got,
+                         const void *expected, size_t n, const char *fmt, ...);
+int     bail_out        (int ignore, const char *fmt, ...);
+void    tap_plan        (int tests, const char *fmt, ...);
+int     diag            (const char *fmt, ...);
+int     note            (const char *fmt, ...);
+int     exit_status     (void);
+void    tap_skip        (int n, const char *fmt, ...);
+void    tap_todo        (int ignore, const char *fmt, ...);
+void    tap_end_todo    (void);
+
+#define NO_PLAN          -1
+#define SKIP_ALL         -2
+#define ok(...)          ok_at_loc(__FILE__, __LINE__, __VA_ARGS__, NULL)
+#define is(...)          is_at_loc(__FILE__, __LINE__, __VA_ARGS__, NULL)
+#define isnt(...)        isnt_at_loc(__FILE__, __LINE__, __VA_ARGS__, NULL)
+#define cmp_ok(...)      cmp_ok_at_loc(__FILE__, __LINE__, __VA_ARGS__, NULL)
+#define cmp_mem(...)     cmp_mem_at_loc(__FILE__, __LINE__, __VA_ARGS__, NULL);
+#define plan(...)        tap_plan(__VA_ARGS__, NULL)
+#define done_testing()   return exit_status()
+#define BAIL_OUT(...)    bail_out(0, "" __VA_ARGS__, NULL)
+#define pass(...)        ok(1, "" __VA_ARGS__)
+#define fail(...)        ok(0, "" __VA_ARGS__)
+
+#define skip(test, ...)  do {if (test) {tap_skip(__VA_ARGS__, NULL); break;}
+#define end_skip         } while (0)
+
+#define todo(...)        tap_todo(0, "" __VA_ARGS__, NULL)
+#define end_todo         tap_end_todo()
+
+#define dies_ok(...)     dies_ok_common(1, __VA_ARGS__)
+#define lives_ok(...)    dies_ok_common(0, __VA_ARGS__)
+
+#ifdef _WIN32
+#define like(...)        tap_skip(1, "like is not implemented on Windows")
+#define unlike           tap_skip(1, "unlike is not implemented on Windows")
+#define dies_ok_common(...) \
+                         tap_skip(1, "Death detection is not supported on Windows")
+#else
+#define like(...)        like_at_loc(1, __FILE__, __LINE__, __VA_ARGS__, NULL)
+#define unlike(...)      like_at_loc(0, __FILE__, __LINE__, __VA_ARGS__, NULL)
+int     like_at_loc     (int for_match, const char *file, int line,
+                         const char *got, const char *expected,
+                         const char *fmt, ...);
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/wait.h>
+int tap_test_died (int status);
+#define dies_ok_common(for_death, code, ...)                \
+    do {                                                    \
+        int cpid;                                           \
+        int it_died;                                        \
+        tap_test_died(1);                                   \
+        cpid = fork();                                      \
+        switch (cpid) {                                     \
+        case -1:                                            \
+            perror("fork error");                           \
+            exit(1);                                        \
+        case 0:                                             \
+            close(1);                                       \
+            close(2);                                       \
+            code                                            \
+            tap_test_died(0);                               \
+            exit(0);                                        \
+        }                                                   \
+        if (waitpid(cpid, NULL, 0) < 0) {                   \
+            perror("waitpid error");                        \
+            exit(1);                                        \
+        }                                                   \
+        it_died = tap_test_died(0);                         \
+        if (!it_died)                                       \
+            {code}                                          \
+        ok(for_death ? it_died : !it_died, "" __VA_ARGS__); \
+    } while (0)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/tests/math/test_pprz_math.c b/tests/math/test_pprz_math.c
new file mode 100644
index 00000000000..feb23f866cd
--- /dev/null
+++ b/tests/math/test_pprz_math.c
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2014 Felix Ruess <felix.ruess@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file test_pprz_math.c
+ * @brief Tests for Paparazzi math libary.
+ *
+ * Using libtap to create a TAP (TestAnythingProtocol) producer:
+ * https://github.com/zorgnax/libtap
+ *
+ */
+
+#include "tap.h"
+#include "math/pprz_algebra_int.h"
+
+int main()
+{
+  plan(1);
+
+  /* test int32_vect2_normalize */
+  struct Int32Vect2 v = {2300, -4200};
+  int32_vect2_normalize(&v, 10);
+
+  int int32_vect2_normalize_ok = FALSE;
+  if (v.x == 491 && v.y == -898)
+    int32_vect2_normalize_ok = TRUE;
+  ok(int32_vect2_normalize_ok, "int32_vect2_normalize([2300, -4200]) returned [%d, %d]", v.x, v.y);
+
+
+  done_testing();
+}