merged upstream/master into sbus2_sensorssbus2_sensors

18 files changed, 647 insertions, 75 deletions

diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index beb9ad13d..622a0faf3 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -24,7 +24,9 @@ SRCS			 = test_adc.c \
 			   test_uart_loopback.c \
 			   test_uart_send.c \
 			   test_mixer.cpp \
+			   test_mathlib.cpp \
 			   test_file.c \
+			   test_file2.c \
 			   tests_main.c \
 			   test_param.c \
 			   test_ppm_loopback.c \
diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c
index 030ac6e23..03391b851 100644
--- a/src/systemcmds/tests/test_adc.c
+++ b/src/systemcmds/tests/test_adc.c
@@ -66,8 +66,8 @@ int test_adc(int argc, char *argv[])
 	}
 
 	for (unsigned i = 0; i < 5; i++) {
-		/* make space for a maximum of eight channels */
-		struct adc_msg_s data[8];
+		/* make space for a maximum of twelve channels */
+		struct adc_msg_s data[12];
 		/* read all channels available */
 		ssize_t count = read(fd, data, sizeof(data));
 
diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c
index 6130fe763..12d598df4 100644
--- a/src/systemcmds/tests/test_bson.c
+++ b/src/systemcmds/tests/test_bson.c
@@ -40,6 +40,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
+#include <math.h>
 
 #include <systemlib/err.h>
 #include <systemlib/bson/tinybson.h>
@@ -123,7 +124,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
 			warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE);
 			return 1;
 		}
-		if (node->d != sample_double) {
+		if (fabs(node->d - sample_double) > 1e-12) {
 			warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double);
 			return 1;
 		}
diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp
index 50dece816..fda949c61 100644
--- a/src/systemcmds/tests/test_conv.cpp
+++ b/src/systemcmds/tests/test_conv.cpp
@@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[])
 		float f = i/10000.0f;
 		float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
 		if (fabsf(f - fres) > 0.0001f) {
-			warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres);
+			warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres);
 			return 1;
 		}
 	}
diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c
new file mode 100644
index 000000000..1f844a97d
--- /dev/null
+++ b/src/systemcmds/tests/test_dataman.c
@@ -0,0 +1,182 @@
+/****************************************************************************
+ * px4/sensors/test_dataman.c
+ *
+ *  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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_led.h>
+#include <systemlib/systemlib.h>
+#include <drivers/drv_hrt.h>
+#include <semaphore.h>
+
+
+#include "tests.h"
+
+#include "dataman/dataman.h"
+
+static sem_t *sems;
+
+static int
+task_main(int argc, char *argv[])
+{
+	char buffer[DM_MAX_DATA_SIZE];
+	hrt_abstime wstart, wend, rstart, rend;
+
+	warnx("Starting dataman test task %s", argv[1]);
+	/* try to read an invalid item */
+	int my_id = atoi(argv[1]);
+	/* try to read an invalid item */
+	if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) {
+		warnx("%d read an invalid item failed", my_id);
+		goto fail;
+	}
+	/* try to read an invalid index */
+	if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
+		warnx("%d read an invalid index failed", my_id);
+		goto fail;
+	}
+	srand(hrt_absolute_time() ^ my_id);
+	unsigned hit = 0, miss = 0;
+	wstart = hrt_absolute_time();
+	for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		memset(buffer, my_id, sizeof(buffer));
+		buffer[1] = i;
+		unsigned hash = i ^ my_id;
+		unsigned len = (hash & 63) + 2;
+
+		int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
+		warnx("ret: %d", ret);
+		if (ret != len) {
+			warnx("%d write failed, index %d, length %d", my_id, hash, len);
+			goto fail;
+		}
+		usleep(rand() & ((64 * 1024) - 1));
+	}
+	rstart = hrt_absolute_time();
+	wend = rstart;
+
+	for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		unsigned hash = i ^ my_id;
+		unsigned len2, len = (hash & 63) + 2;
+		if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) {
+			warnx("%d read failed length test, index %d", my_id, hash);
+			goto fail;
+		}
+		if (buffer[0] == my_id) {
+			hit++;
+			if (len2 != len) {
+				warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2);
+				goto fail;
+			}
+			if (buffer[1] != i) {
+				warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]);
+				goto fail;
+			}
+		}
+		else
+			miss++;
+	}
+	rend = hrt_absolute_time();
+	warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.",
+		my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000);
+	sem_post(sems + my_id);
+	return 0;
+fail:
+	warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x",
+		my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
+	sem_post(sems + my_id);
+	return -1;
+}
+
+int test_dataman(int argc, char *argv[])
+{
+	int i, num_tasks = 4;
+	char buffer[DM_MAX_DATA_SIZE];
+
+	if (argc > 1)
+		num_tasks = atoi(argv[1]);
+	
+	sems = (sem_t *)malloc(num_tasks * sizeof(sem_t));
+	warnx("Running %d tasks", num_tasks);
+	for (i = 0; i < num_tasks; i++) {
+		int task;
+		char a[16];
+		sprintf(a, "%d", i);
+		const char *av[2];
+		av[0] = a;
+		av[1] = 0;
+		sem_init(sems + i, 1, 0);
+		/* start the task */
+		if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) {
+			warn("task start failed");
+		}
+	}
+	for (i = 0; i < num_tasks; i++) {
+		sem_wait(sems + i);
+		sem_destroy(sems + i);
+	}
+	free(sems);
+	dm_restart(DM_INIT_REASON_IN_FLIGHT);
+	for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0)
+			break;
+	}
+	if (i >= NUM_MISSIONS_SUPPORTED) {
+		warnx("Restart in-flight failed");
+		return -1;
+
+	}
+	dm_restart(DM_INIT_REASON_POWER_ON);
+	for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
+			warnx("Restart power-on failed");
+			return -1;
+		}
+	}
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c
index 96be1e8df..570583dee 100644
--- a/src/systemcmds/tests/test_file.c
+++ b/src/systemcmds/tests/test_file.c
@@ -102,7 +102,7 @@ test_file(int argc, char *argv[])
 	}
 
 	/* perform tests for a range of chunk sizes */
-	unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500};
+	int chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500};
 
 	for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
 
@@ -116,7 +116,7 @@ test_file(int argc, char *argv[])
 			uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
 
 			/* fill write buffer with known values */
-			for (int i = 0; i < sizeof(write_buf); i++) {
+			for (size_t i = 0; i < sizeof(write_buf); i++) {
 				/* this will wrap, but we just need a known value with spacing */
 				write_buf[i] = i+11;
 			}
@@ -149,6 +149,8 @@ test_file(int argc, char *argv[])
 			}
 			end = hrt_absolute_time();
 
+			warnx("write took %llu us", (end - start));
+
 			close(fd);
 			fd = open("/fs/microsd/testfile", O_RDONLY);
 
@@ -192,7 +194,6 @@ test_file(int argc, char *argv[])
 
 			warnx("testing aligned writes - please wait.. (CTRL^C to abort)");
 
-			start = hrt_absolute_time();
 			for (unsigned i = 0; i < iterations; i++) {
 				int wret = write(fd, write_buf, chunk_sizes[c]);
 
@@ -224,9 +225,6 @@ test_file(int argc, char *argv[])
 					return 1;
 				}
 				
-				/* compare value */
-				bool compare_ok = true;
-
 				for (int j = 0; j < chunk_sizes[c]; j++) {
 					if (read_buf[j] != write_buf[j]) {
 						warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]);
diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c
new file mode 100644
index 000000000..8db3ea5ae
--- /dev/null
+++ b/src/systemcmds/tests/test_file2.c
@@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ *   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_file2.c
+ *
+ * File write test.
+ */
+
+#include <sys/stat.h>
+#include <dirent.h>
+#include <stdio.h>
+#include <stddef.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+#include <stdlib.h>
+#include <getopt.h>
+
+#include "tests.h"
+
+#define FLAG_FSYNC 1
+#define FLAG_LSEEK 2
+
+/*
+  return a predictable value for any file offset to allow detection of corruption
+ */
+static uint8_t get_value(uint32_t ofs)
+{
+	union {
+		uint32_t ofs;
+		uint8_t buf[4];
+	} u;
+	u.ofs = ofs;
+	return u.buf[ofs % 4];
+}
+
+static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags)
+{
+	printf("Testing on %s with write_chunk=%u write_size=%u\n", 
+	       filename, (unsigned)write_chunk, (unsigned)write_size);
+	
+	uint32_t ofs = 0;
+	int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC);
+	if (fd == -1) {
+		perror(filename);
+		exit(1);
+	}
+	
+	// create a file of size write_size, in write_chunk blocks
+	uint8_t counter = 0;
+	while (ofs < write_size) {
+        uint8_t buffer[write_chunk];
+        for (uint16_t j=0; j<write_chunk; j++) {
+		buffer[j] = get_value(ofs);
+		ofs++;
+        }
+        if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
+            printf("write failed at offset %u\n", ofs);
+            exit(1);
+        }
+	if (flags & FLAG_FSYNC) {
+		fsync(fd);
+	}
+        if (counter % 100 == 0) {
+		printf("write ofs=%u\r", ofs);
+        }
+        counter++;
+	}
+	close(fd);
+	
+	printf("write ofs=%u\n", ofs);
+	
+	// read and check
+	fd = open(filename, O_RDONLY);
+	if (fd == -1) {
+		perror(filename);
+		exit(1);
+	}
+
+	counter = 0;
+	ofs = 0;
+	while (ofs < write_size) {
+		uint8_t buffer[write_chunk];
+		if (counter % 100 == 0) {
+			printf("read ofs=%u\r", ofs);
+		}
+		counter++;
+		if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
+			printf("read failed at offset %u\n", ofs);
+			exit(1);
+		}
+		for (uint16_t j=0; j<write_chunk; j++) {
+			if (buffer[j] != get_value(ofs)) {
+				printf("corruption at ofs=%u got %u\n", ofs, buffer[j]);
+				exit(1);
+			}
+			ofs++;
+		}
+		if (flags & FLAG_LSEEK) {
+			lseek(fd, 0, SEEK_CUR);
+		}
+	}
+	printf("read ofs=%u\n", ofs);
+	close(fd);
+	unlink(filename);
+	printf("All OK\n");    
+}
+
+static void usage(void)
+{
+	printf("test file2 [options] [filename]\n");
+	printf("\toptions:\n");
+	printf("\t-s SIZE     set file size\n");
+	printf("\t-c CHUNK    set IO chunk size\n");
+	printf("\t-F          fsync on every write\n");
+	printf("\t-L          lseek on every read\n");
+}
+
+int test_file2(int argc, char *argv[])
+{
+	int opt;
+	uint16_t flags = 0;
+	const char *filename = "/fs/microsd/testfile2.dat";
+	uint32_t write_chunk = 64;
+	uint32_t write_size = 5*1024;
+
+	while ((opt = getopt(argc, argv, "c:s:FLh")) != EOF) {
+		switch (opt) {
+		case 'F':
+			flags |= FLAG_FSYNC;
+			break;
+		case 'L':
+			flags |= FLAG_LSEEK;
+			break;
+		case 's':
+			write_size = strtoul(optarg, NULL, 0);
+			break;
+		case 'c':
+			write_chunk = strtoul(optarg, NULL, 0);
+			break;
+		case 'h':
+		default:
+			usage();
+			exit(1);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+
+	if (argc > 0) {
+		filename = argv[0];
+	}
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+        test_corruption(filename, write_chunk, write_size, flags);
+        return 0;
+}
+
diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c
index 4921c9bbb..bb8b6c7f5 100644
--- a/src/systemcmds/tests/test_float.c
+++ b/src/systemcmds/tests/test_float.c
@@ -68,7 +68,7 @@ int test_float(int argc, char *argv[])
 	float sinf_one = sinf(1.0f);
 	float sqrt_two = sqrt(2.0f);
 
-	if (sinf_zero == 0.0f) {
+	if (fabsf(sinf_zero) < FLT_EPSILON) {
 		printf("\t success: sinf(0.0f) == 0.0f\n");
 
 	} else {
@@ -94,7 +94,7 @@ int test_float(int argc, char *argv[])
 		printf("\t success: asinf(1.0f) == 1.57079f\n");
 
 	} else {
-		printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one);
+		printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one);
 		ret = -1;
 	}
 
@@ -128,7 +128,7 @@ int test_float(int argc, char *argv[])
 
 	float sinf_zero_one = sinf(0.1f);
 
-	if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
+	if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
 		printf("\t success: sinf(0.1f) == 0.09983f\n");
 
 	} else {
@@ -136,7 +136,7 @@ int test_float(int argc, char *argv[])
 		ret = -2;
 	}
 
-	if (sqrt_two == 1.41421356f) {
+	if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) {
 		printf("\t success: sqrt(2.0f) == 1.41421f\n");
 
 	} else {
@@ -155,7 +155,7 @@ int test_float(int argc, char *argv[])
 	}
 
 	char sbuf[30];
-	sprintf(sbuf, "%8.4f", 0.553415f);
+	sprintf(sbuf, "%8.4f", (double)0.553415f);
 
 	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
 		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
@@ -166,7 +166,7 @@ int test_float(int argc, char *argv[])
 		ret = -5;
 	}
 
-	sprintf(sbuf, "%8.4f", -0.553415f);
+	sprintf(sbuf, "%8.4f", (double)-0.553415f);
 
 	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
 		sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
@@ -188,7 +188,7 @@ int test_float(int argc, char *argv[])
 
 	double d1d2 = d1 * d2;
 
-	if (d1d2 == 2.022200000000000219557705349871) {
+	if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) {
 		printf("\t success: 1.0111 * 2.0 == 2.0222\n");
 
 	} else {
@@ -201,11 +201,11 @@ int test_float(int argc, char *argv[])
 	// Assign value of f1 to d1
 	d1 = f1;
 
-	if (f1 == (float)d1) {
+	if (fabsf(f1 - (float)d1) < FLT_EPSILON) {
 		printf("\t success: (float) 1.55f == 1.55 (double)\n");
 
 	} else {
-		printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1);
+		printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1);
 		ret = -8;
 	}
 
@@ -216,7 +216,7 @@ int test_float(int argc, char *argv[])
 	double sin_one = sin(1.0);
 	double atan2_ones = atan2(1.0, 1.0);
 
-	if (sin_zero == 0.0) {
+	if (fabs(sin_zero - 0.0) < DBL_EPSILON) {
 		printf("\t success: sin(0.0) == 0.0\n");
 
 	} else {
@@ -224,7 +224,7 @@ int test_float(int argc, char *argv[])
 		ret = -9;
 	}
 
-	if (sin_one == 0.841470984807896504875657228695) {
+	if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) {
 		printf("\t success: sin(1.0) == 0.84147098480\n");
 
 	} else {
@@ -232,7 +232,7 @@ int test_float(int argc, char *argv[])
 		ret = -10;
 	}
 
-	if (atan2_ones != 0.785398) {
+	if (fabs(atan2_ones - 0.785398) < DBL_EPSILON) {
 		printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
 
 	} else {
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
new file mode 100644
index 000000000..70d173fc9
--- /dev/null
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -0,0 +1,160 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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_mathlib.cpp
+ *
+ * Mathlib test
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <time.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include "tests.h"
+
+#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); }
+
+using namespace math;
+
+int test_mathlib(int argc, char *argv[])
+{
+	warnx("testing mathlib");
+
+	{
+		Vector<2> v;
+		Vector<2> v1(1.0f, 2.0f);
+		Vector<2> v2(1.0f, -1.0f);
+		float data[2] = {1.0f, 2.0f};
+		TEST_OP("Constructor Vector<2>()", Vector<2> v3);
+		TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1));
+		TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data));
+		TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f));
+		TEST_OP("Vector<2> = Vector<2>", v = v1);
+		TEST_OP("Vector<2> + Vector<2>", v + v1);
+		TEST_OP("Vector<2> - Vector<2>", v - v1);
+		TEST_OP("Vector<2> += Vector<2>", v += v1);
+		TEST_OP("Vector<2> -= Vector<2>", v -= v1);
+		TEST_OP("Vector<2> * Vector<2>", v * v1);
+		TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
+	}
+
+	{
+		Vector<3> v;
+		Vector<3> v1(1.0f, 2.0f, 0.0f);
+		Vector<3> v2(1.0f, -1.0f, 2.0f);
+		float data[3] = {1.0f, 2.0f, 3.0f};
+		TEST_OP("Constructor Vector<3>()", Vector<3> v3);
+		TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1));
+		TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data));
+		TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f));
+		TEST_OP("Vector<3> = Vector<3>", v = v1);
+		TEST_OP("Vector<3> + Vector<3>", v + v1);
+		TEST_OP("Vector<3> - Vector<3>", v - v1);
+		TEST_OP("Vector<3> += Vector<3>", v += v1);
+		TEST_OP("Vector<3> -= Vector<3>", v -= v1);
+		TEST_OP("Vector<3> * float", v1 * 2.0f);
+		TEST_OP("Vector<3> / float", v1 / 2.0f);
+		TEST_OP("Vector<3> *= float", v1 *= 2.0f);
+		TEST_OP("Vector<3> /= float", v1 /= 2.0f);
+		TEST_OP("Vector<3> * Vector<3>", v * v1);
+		TEST_OP("Vector<3> %% Vector<3>", v1 % v2);
+		TEST_OP("Vector<3> length", v1.length());
+		TEST_OP("Vector<3> length squared", v1.length_squared());
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+		// Need pragma here intead of moving variable out of TEST_OP and just reference because
+		// TEST_OP measures performance of vector operations.
+		TEST_OP("Vector<3> element read", volatile float a = v1(0));
+		TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]);
+#pragma GCC diagnostic pop
+		TEST_OP("Vector<3> element write", v1(0) = 1.0f);
+		TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
+	}
+
+	{
+		Vector<4> v;
+		Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
+		Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f);
+		float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
+		TEST_OP("Constructor Vector<4>()", Vector<4> v3);
+		TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1));
+		TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data));
+		TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f));
+		TEST_OP("Vector<4> = Vector<4>", v = v1);
+		TEST_OP("Vector<4> + Vector<4>", v + v1);
+		TEST_OP("Vector<4> - Vector<4>", v - v1);
+		TEST_OP("Vector<4> += Vector<4>", v += v1);
+		TEST_OP("Vector<4> -= Vector<4>", v -= v1);
+		TEST_OP("Vector<4> * Vector<4>", v * v1);
+	}
+
+	{
+		Vector<10> v1;
+		v1.zero();
+		float data[10];
+		TEST_OP("Constructor Vector<10>()", Vector<10> v3);
+		TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1));
+		TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
+	}
+
+	{
+		Matrix<3, 3> m1;
+		m1.identity();
+		Matrix<3, 3> m2;
+		m2.identity();
+		Vector<3> v1(1.0f, 2.0f, 0.0f);
+		TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1);
+		TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
+		TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
+	}
+
+	{
+		Matrix<10, 10> m1;
+		m1.identity();
+		Matrix<10, 10> m2;
+		m2.identity();
+		Vector<10> v1;
+		v1.zero();
+		TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
+		TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
+		TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
+	}
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index df382e2c6..8ab8fa2d6 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -81,7 +81,7 @@ int test_mixer(int argc, char *argv[])
 
 	warnx("testing mixer");
 
-	char *filename = "/etc/mixers/IO_pass.mix";
+	const char *filename = "/etc/mixers/IO_pass.mix";
 
 	if (argc > 1)
 		filename = argv[1];
@@ -100,8 +100,6 @@ int test_mixer(int argc, char *argv[])
 	 * e.g. on PX4IO.
 	 */
 
-	unsigned nused = 0;
-
 	const unsigned chunk_size = 64;
 
 	MixerGroup mixer_group(mixer_callback, 0);
@@ -124,7 +122,6 @@ int test_mixer(int argc, char *argv[])
 		return 1;
 
 	/* FIRST mark the mixer as invalid */
-	bool mixer_ok = false;
 	/* THEN actually delete it */
 	mixer_group.reset();
 	char mixer_text[256];		/* large enough for one mixer */
@@ -140,7 +137,6 @@ int test_mixer(int argc, char *argv[])
 
 		/* check for overflow - this would be really fatal */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
-			bool mixer_ok = false;
 			return 1;
 		}
 
@@ -156,15 +152,6 @@ int test_mixer(int argc, char *argv[])
 
 		/* if anything was parsed */
 		if (resid != mixer_text_length) {
-
-			/* only set mixer ok if no residual is left over */
-			if (resid == 0) {
-				mixer_ok = true;
-			} else {
-				/* not yet reached the end of the mixer, set as not ok */
-				mixer_ok = false;
-			}
-
 			warnx("used %u", mixer_text_length - resid);
 
 			/* copy any leftover text to the base of the buffer for re-use */
@@ -192,7 +179,7 @@ int test_mixer(int argc, char *argv[])
 	should_arm = true;
 
 	/* run through arming phase */
-	for (int i = 0; i < output_max; i++) {
+	for (unsigned i = 0; i < output_max; i++) {
 		actuator_controls[i] = 0.1f;
 		r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
 		r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
@@ -213,7 +200,7 @@ int test_mixer(int argc, char *argv[])
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
-		for (int i = 0; i < mixed; i++)
+		for (unsigned i = 0; i < mixed; i++)
 		{
 			/* check mixed outputs to be zero during init phase */
 			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
@@ -228,7 +215,7 @@ int test_mixer(int argc, char *argv[])
 				return 1;
 			}
 
-			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]);
 		}
 		usleep(sleep_quantum_us);
 		sleepcount++;
@@ -244,7 +231,7 @@ int test_mixer(int argc, char *argv[])
 
 	for (int j = -jmax; j <= jmax; j++) {
 
-		for (int i = 0; i < output_max; i++) {
+		for (unsigned i = 0; i < output_max; i++) {
 			actuator_controls[i] = j/10.0f + 0.1f * i;
 			r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
 			r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
@@ -257,11 +244,11 @@ int test_mixer(int argc, char *argv[])
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		warnx("mixed %d outputs (max %d)", mixed, output_max);
-		for (int i = 0; i < mixed; i++)
+		for (unsigned i = 0; i < mixed; i++)
 		{
 			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
 			if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
-				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
+				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
 				warnx("mixer violated predicted value");
 				return 1;
 			}
@@ -282,7 +269,7 @@ int test_mixer(int argc, char *argv[])
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
-		for (int i = 0; i < mixed; i++)
+		for (unsigned i = 0; i < mixed; i++)
 		{
 			/* check mixed outputs to be zero during init phase */
 			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
@@ -316,7 +303,7 @@ int test_mixer(int argc, char *argv[])
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
-		for (int i = 0; i < mixed; i++)
+		for (unsigned i = 0; i < mixed; i++)
 		{
 			/* predict value */
 			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
@@ -333,7 +320,7 @@ int test_mixer(int argc, char *argv[])
 			/* check post ramp phase */
 			if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
 				fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
-				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
+				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
 				warnx("mixer violated predicted value");
 				return 1;
 			}
@@ -372,6 +359,7 @@ int test_mixer(int argc, char *argv[])
 	}
 
 	warnx("SUCCESS: No errors in mixer test");
+	return 0;
 }
 
 static int
diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c
index 44e34d9ef..4b6303cfb 100644
--- a/src/systemcmds/tests/test_mount.c
+++ b/src/systemcmds/tests/test_mount.c
@@ -141,8 +141,8 @@ test_mount(int argc, char *argv[])
 			/* announce mode switch */
 			if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) {
 				warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC");
-				fsync(stdout);
-				fsync(stderr);
+				fsync(fileno(stdout));
+				fsync(fileno(stderr));
 				usleep(20000);
 			}
 
@@ -162,7 +162,7 @@ test_mount(int argc, char *argv[])
 	}
 
 	char buf[64];
-	int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
+	(void)sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
 	lseek(cmd_fd, 0, SEEK_SET);
 	write(cmd_fd, buf, strlen(buf) + 1);
 	fsync(cmd_fd);
@@ -174,8 +174,8 @@ test_mount(int argc, char *argv[])
 
 		printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC");
 		printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n");
-		fsync(stdout);
-		fsync(stderr);
+		fsync(fileno(stdout));
+		fsync(fileno(stderr));
 		usleep(50000);
 
 		for (unsigned a = 0; a < alignments; a++) {
@@ -185,22 +185,20 @@ test_mount(int argc, char *argv[])
 			uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
 
 			/* fill write buffer with known values */
-			for (int i = 0; i < sizeof(write_buf); i++) {
+			for (unsigned i = 0; i < sizeof(write_buf); i++) {
 				/* this will wrap, but we just need a known value with spacing */
 				write_buf[i] = i+11;
 			}
 
 			uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
-			hrt_abstime start, end;
 
 			int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
 
-			start = hrt_absolute_time();
 			for (unsigned i = 0; i < iterations; i++) {
 
 				int wret = write(fd, write_buf + a, chunk_sizes[c]);
 
-				if (wret != chunk_sizes[c]) {
+				if (wret != (int)chunk_sizes[c]) {
 					warn("WRITE ERROR!");
 
 					if ((0x3 & (uintptr_t)(write_buf + a)))
@@ -214,8 +212,8 @@ test_mount(int argc, char *argv[])
 					fsync(fd);
 				} else {
 					printf("#");
-					fsync(stdout);
-					fsync(stderr);
+					fsync(fileno(stdout));
+					fsync(fileno(stderr));
 				}
 			}
 
@@ -224,12 +222,10 @@ test_mount(int argc, char *argv[])
 			}
 
 			printf(".");
-			fsync(stdout);
-			fsync(stderr);
+			fsync(fileno(stdout));
+			fsync(fileno(stderr));
 			usleep(200000);
 
-			end = hrt_absolute_time();
-
 			close(fd);
 			fd = open("/fs/microsd/testfile", O_RDONLY);
 
@@ -237,7 +233,7 @@ test_mount(int argc, char *argv[])
 			for (unsigned i = 0; i < iterations; i++) {
 				int rret = read(fd, read_buf, chunk_sizes[c]);
 
-				if (rret != chunk_sizes[c]) {
+				if (rret != (int)chunk_sizes[c]) {
 					warnx("READ ERROR!");
 					return 1;
 				}
@@ -245,7 +241,7 @@ test_mount(int argc, char *argv[])
 				/* compare value */
 				bool compare_ok = true;
 
-				for (int j = 0; j < chunk_sizes[c]; j++) {
+				for (unsigned j = 0; j < chunk_sizes[c]; j++) {
 					if (read_buf[j] != write_buf[j + a]) {
 						warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a);
 						compare_ok = false;
@@ -271,16 +267,16 @@ test_mount(int argc, char *argv[])
 		}
 	}
 
-	fsync(stdout);
-	fsync(stderr);
+	fsync(fileno(stdout));
+	fsync(fileno(stderr));
 	usleep(20000);
 
 
 
 	/* we always reboot for the next test if we get here */
 	warnx("Iteration done, rebooting..");
-	fsync(stdout);
-	fsync(stderr);
+	fsync(fileno(stdout));
+	fsync(fileno(stderr));
 	usleep(50000);
 	systemreset(false);
 
diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c
index bac9efbdb..43231ccad 100644
--- a/src/systemcmds/tests/test_mtd.c
+++ b/src/systemcmds/tests/test_mtd.c
@@ -57,6 +57,8 @@
 #define PARAM_FILE_NAME "/fs/mtd_params"
 
 static int check_user_abort(int fd);
+static void print_fail(void);
+static void print_success(void);
 
 int check_user_abort(int fd) {
 	/* check if user wants to abort */
@@ -126,7 +128,7 @@ test_mtd(int argc, char *argv[])
 		uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
 
 		/* fill write buffer with known values */
-		for (int i = 0; i < sizeof(write_buf); i++) {
+		for (unsigned i = 0; i < sizeof(write_buf); i++) {
 			/* this will wrap, but we just need a known value with spacing */
 			write_buf[i] = i+11;
 		}
@@ -137,11 +139,14 @@ test_mtd(int argc, char *argv[])
 		int fd = open(PARAM_FILE_NAME, O_RDONLY);
 		int rret = read(fd, read_buf, chunk_sizes[c]);
 		close(fd);
+		if (rret <= 0) {
+			err(1, "read error");
+		}
 
 		fd = open(PARAM_FILE_NAME, O_WRONLY);
 
 		printf("printing 2 percent of the first chunk:\n");
-		for (int i = 0; i < sizeof(read_buf) / 50; i++) {
+		for (unsigned i = 0; i < sizeof(read_buf) / 50; i++) {
 			printf("%02X", read_buf[i]);
 		}
 		printf("\n");
@@ -166,14 +171,16 @@ test_mtd(int argc, char *argv[])
 		}
 		end = hrt_absolute_time();
 
+		warnx("write took %llu us", (end - start));
+
 		close(fd);
 		fd = open(PARAM_FILE_NAME, O_RDONLY);
 
 		/* read back data for validation */
 		for (unsigned i = 0; i < iterations; i++) {
-			int rret = read(fd, read_buf, chunk_sizes[c]);
+			int rret2 = read(fd, read_buf, chunk_sizes[c]);
 
-			if (rret != chunk_sizes[c]) {
+			if (rret2 != (int)chunk_sizes[c]) {
 				warnx("READ ERROR!");
 				print_fail();
 				return 1;
@@ -182,7 +189,7 @@ test_mtd(int argc, char *argv[])
 			/* compare value */
 			bool compare_ok = true;
 
-			for (int j = 0; j < chunk_sizes[c]; j++) {
+			for (unsigned j = 0; j < chunk_sizes[c]; j++) {
 				if (read_buf[j] != write_buf[j]) {
 					warnx("COMPARISON ERROR: byte %d", j);
 					print_fail();
@@ -211,7 +218,7 @@ test_mtd(int argc, char *argv[])
 	char ffbuf[64];
 	memset(ffbuf, 0xFF, sizeof(ffbuf));
 	int fd = open(PARAM_FILE_NAME, O_WRONLY);
-	for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
+	for (unsigned i = 0; i < file_size / sizeof(ffbuf); i++) {
 		int ret = write(fd, ffbuf, sizeof(ffbuf));
 
 		if (ret != sizeof(ffbuf)) {
diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c
index b9041b013..addd57bea 100644
--- a/src/systemcmds/tests/test_ppm_loopback.c
+++ b/src/systemcmds/tests/test_ppm_loopback.c
@@ -65,7 +65,6 @@ int test_ppm_loopback(int argc, char *argv[])
 	int _rc_sub = orb_subscribe(ORB_ID(input_rc));
 
 	int servo_fd, result;
-	servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
 	servo_position_t pos;
 
 	servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c
index 57c0e7f4c..c9f9ef439 100644
--- a/src/systemcmds/tests/test_rc.c
+++ b/src/systemcmds/tests/test_rc.c
@@ -52,6 +52,7 @@
 #include <arch/board/board.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_rc_input.h>
+#include <drivers/drv_hrt.h>
 #include <systemlib/err.h>
 
 #include "tests.h"
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index 096106ff3..a4f17eebd 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -139,7 +139,14 @@ accel(int argc, char *argv[])
 	}
 
 	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
-		warnx("ACCEL1 acceleration values out of range!");
+		warnx("ACCEL acceleration values out of range!");
+		return ERROR;
+	}
+
+	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+	if (len < 8.0f || len > 12.0f) {
+		warnx("ACCEL scale error!");
 		return ERROR;
 	}
 
@@ -186,6 +193,13 @@ accel1(int argc, char *argv[])
 		return ERROR;
 	}
 
+	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+	if (len < 8.0f || len > 12.0f) {
+		warnx("ACCEL1 scale error!");
+		return ERROR;
+	}
+
 	/* Let user know everything is ok */
 	printf("\tOK: ACCEL1 passed all tests successfully\n");
 
@@ -225,6 +239,13 @@ gyro(int argc, char *argv[])
 		printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
+	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+	if (len > 0.3f) {
+		warnx("GYRO scale error!");
+		return ERROR;
+	}
+
 	/* Let user know everything is ok */
 	printf("\tOK: GYRO passed all tests successfully\n");
 	close(fd);
@@ -263,6 +284,13 @@ gyro1(int argc, char *argv[])
 		printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
+	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+	if (len > 0.3f) {
+		warnx("GYRO1 scale error!");
+		return ERROR;
+	}
+
 	/* Let user know everything is ok */
 	printf("\tOK: GYRO1 passed all tests successfully\n");
 	close(fd);
@@ -301,6 +329,13 @@ mag(int argc, char *argv[])
 		printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
+	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
+
+	if (len < 1.0f || len > 3.0f) {
+		warnx("MAG scale error!");
+		return ERROR;
+	}
+
 	/* Let user know everything is ok */
 	printf("\tOK: MAG passed all tests successfully\n");
 	close(fd);
diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c
index ef8512bf5..9c6951ca2 100644
--- a/src/systemcmds/tests/test_servo.c
+++ b/src/systemcmds/tests/test_servo.c
@@ -51,6 +51,7 @@
 
 #include <arch/board/board.h>
 #include <drivers/drv_pwm_output.h>
+#include <systemlib/err.h>
 
 #include <nuttx/spi.h>
 
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index 82de05dff..ad55e1410 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -107,11 +107,13 @@ extern int	test_jig_voltages(int argc, char *argv[]);
 extern int	test_param(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
+extern int	test_file2(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
 extern int	test_rc(int argc, char *argv[]);
 extern int	test_conv(int argc, char *argv[]);
 extern int	test_mount(int argc, char *argv[]);
 extern int	test_mtd(int argc, char *argv[]);
+extern int	test_mathlib(int argc, char *argv[]);
 
 __END_DECLS
 
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 77a4df618..e3f26924f 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -104,11 +104,13 @@ const struct {
 	{"param",		test_param,	0},
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
+	{"file2",		test_file2,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mount",		test_mount,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mtd",			test_mtd,	0},
+	{"mathlib",		test_mathlib,	0},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };
@@ -233,7 +235,7 @@ test_perf(int argc, char *argv[])
 	printf("perf: expect count of 1\n");
 	perf_print_counter(ec);
 	printf("perf: expect at least two counters\n");
-	perf_print_all();
+	perf_print_all(0);
 
 	perf_free(cc);
 	perf_free(ec);