Add conversions and mixer tests. Work in progress

6 files changed, 133 insertions, 4 deletions

diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 68a080c77..576caff99 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -28,4 +28,5 @@ SRCS			 = test_adc.c \
 			   tests_main.c \
 			   test_param.c \
 			   test_ppm_loopback.c \
-			   test_rc.c
+			   test_rc.c \
+			   test_conv.cpp
diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp
new file mode 100644
index 000000000..50dece816
--- /dev/null
+++ b/src/systemcmds/tests/test_conv.cpp
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 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_conv.cpp
+ * Tests conversions used across the system.
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+#include <systemlib/err.h>
+#include <unit_test/unit_test.h>
+#include <px4iofirmware/protocol.h>
+
+int test_conv(int argc, char *argv[])
+{
+	warnx("Testing system conversions");
+
+	for (int i = -10000; i <= 10000; i+=1) {
+		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);
+			return 1;
+		}
+	}
+
+	warnx("All conversions clean");
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index 4da86042d..d330da39f 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -51,6 +51,7 @@
 
 #include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
 
 #include "tests.h"
 
@@ -59,6 +60,18 @@ static int	mixer_callback(uintptr_t handle,
 			       uint8_t control_index,
 			       float &control);
 
+const unsigned output_max = 8;
+static float actuator_controls[output_max];
+static bool should_arm = false;
+uint16_t r_page_servo_disarmed[output_max];
+uint16_t r_page_servo_control_min[output_max];
+uint16_t r_page_servo_control_max[output_max];
+uint16_t r_page_servos[output_max];
+/*
+ * PWM limit structure
+ */
+pwm_limit_t pwm_limit;
+
 int test_mixer(int argc, char *argv[])
 {
 	warnx("testing mixer");
@@ -164,6 +177,40 @@ int test_mixer(int argc, char *argv[])
 	if (mixer_group.count() != 8)
 		return 1;
 
+	/* execute the mixer */
+	
+	float	outputs_unlimited[output_max];
+	float	outputs[output_max];
+	unsigned mixed;
+	const int jmax = 50;
+
+	pwm_limit_init(&pwm_limit);
+	pwm_limit.state = PWM_LIMIT_STATE_ON;
+	should_arm = true;
+
+	for (int j = -jmax; j <= jmax; j++) {
+
+		for (int i = 0; i < output_max; i++) {
+			actuator_controls[i] = j/100.0f + 0.1f * i;
+			r_page_servo_disarmed[i] = 900;
+			r_page_servo_control_min[i] = 1000;
+			r_page_servo_control_max[i] = 2000;
+		}
+
+		/* mix */
+		mixed = mixer_group.mix(&outputs_unlimited[0], output_max);
+
+		memcpy(outputs, outputs_unlimited, sizeof(outputs));
+
+		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++)
+		{
+			printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+	}
+
 	/* load multirotor at once test */
 	mixer_group.reset();
 
@@ -193,7 +240,10 @@ mixer_callback(uintptr_t handle,
 	if (control_group != 0)
 		return -1;
 
-	control = 0.0f;
+	if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0])))
+		return -1;
+
+	control = actuator_controls[control_index];
 
 	return 0;
 }
diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c
index 72619fc8b..6a602ecfc 100644
--- a/src/systemcmds/tests/test_rc.c
+++ b/src/systemcmds/tests/test_rc.c
@@ -32,8 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file test_ppm_loopback.c
- * Tests the PWM outputs and PPM input
+ * @file test_rc.c
+ * Tests RC input.
  *
  */
 
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index a57d04be3..321e91e60 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -109,6 +109,7 @@ extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(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[]);
 
 __END_DECLS
 
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 1088a4407..1c717d3ea 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -106,6 +106,7 @@ const struct {
 	{"file",		test_file,	0},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };