Add conversions and mixer tests. Work in progress

src/modules/systemlib/pwm_limit/pwm_limit.c

@@ -47,7 +47,7 @@
 
 void pwm_limit_init(pwm_limit_t *limit)
 {
-	limit->state = LIMIT_STATE_OFF;
+	limit->state = PWM_LIMIT_STATE_OFF;
 	limit->time_armed = 0;
 	return;
 }
@@ -56,26 +56,26 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 {
 	/* first evaluate state changes */
 	switch (limit->state) {
-		case LIMIT_STATE_OFF:
+		case PWM_LIMIT_STATE_OFF:
 			if (armed)
-				limit->state = LIMIT_STATE_RAMP;
+				limit->state = PWM_LIMIT_STATE_RAMP;
 			limit->time_armed = hrt_absolute_time();
 			break;
-		case LIMIT_STATE_INIT:
+		case PWM_LIMIT_STATE_INIT:
 			if (!armed)
-				limit->state = LIMIT_STATE_OFF;
+				limit->state = PWM_LIMIT_STATE_OFF;
 			else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US)
-				limit->state = LIMIT_STATE_RAMP;
+				limit->state = PWM_LIMIT_STATE_RAMP;
 			break;
-		case LIMIT_STATE_RAMP:
+		case PWM_LIMIT_STATE_RAMP:
 			if (!armed)
-				limit->state = LIMIT_STATE_OFF;
+				limit->state = PWM_LIMIT_STATE_OFF;
 			else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US)
-				limit->state = LIMIT_STATE_ON;
+				limit->state = PWM_LIMIT_STATE_ON;
 			break;
-		case LIMIT_STATE_ON:
+		case PWM_LIMIT_STATE_ON:
 			if (!armed)
-				limit->state = LIMIT_STATE_OFF;
+				limit->state = PWM_LIMIT_STATE_OFF;
 			break;
 		default:
 			break;
@@ -86,14 +86,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 
 	/* then set effective_pwm based on state */
 	switch (limit->state) {
-		case LIMIT_STATE_OFF:
-		case LIMIT_STATE_INIT:
+		case PWM_LIMIT_STATE_OFF:
+		case PWM_LIMIT_STATE_INIT:
 			for (unsigned i=0; i<num_channels; i++) {
 				effective_pwm[i] = disarmed_pwm[i];
 				output[i] = 0.0f;
 			}
 			break;
-		case LIMIT_STATE_RAMP:
+		case PWM_LIMIT_STATE_RAMP:
 
 			progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
 			for (unsigned i=0; i<num_channels; i++) {
@@ -120,7 +120,7 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 				output[i] = (float)progress/10000.0f * output[i];
 			}
 			break;
-		case LIMIT_STATE_ON:
+		case PWM_LIMIT_STATE_ON:
 			for (unsigned i=0; i<num_channels; i++) {
 				effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
 				/* effective_output stays the same */

src/modules/systemlib/pwm_limit/pwm_limit.h

@@ -46,6 +46,8 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+__BEGIN_DECLS
+
 /*
  * time for the ESCs to initialize
  * (this is not actually needed if PWM is sent right after boot)
@@ -56,18 +58,18 @@
  */
 #define RAMP_TIME_US 2500000
 
+enum pwm_limit_state {
+	PWM_LIMIT_STATE_OFF = 0,
+	PWM_LIMIT_STATE_INIT,
+	PWM_LIMIT_STATE_RAMP,
+	PWM_LIMIT_STATE_ON
+};
+
 typedef struct {
-	enum {
-		LIMIT_STATE_OFF = 0,
-		LIMIT_STATE_INIT,
-		LIMIT_STATE_RAMP,
-		LIMIT_STATE_ON
-	} state;
+	enum pwm_limit_state state;
 	uint64_t time_armed;
 } pwm_limit_t;
 
-__BEGIN_DECLS
-
 __EXPORT void pwm_limit_init(pwm_limit_t *limit);
 
 __EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit);

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

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;
+}

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;
 }

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.
  *
  */
 

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
 

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}
 };