Add conversions and mixer tests. Work in progress

Tools/tests-host/Makefile

@@ -10,11 +10,13 @@ LIBS=-lm
 #_DEPS = test.h
 #DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
 
-_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o
+_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \
+	mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o
 OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
 
 #$(DEPS)
 $(ODIR)/%.o: %.cpp
+	mkdir -p obj
 	$(CC) -c -o $@ $< $(CFLAGS)
 
 $(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
@@ -26,6 +28,12 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
 $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
 	$(CC) -c -o $@ $< $(CFLAGS)
 
+$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c
+	$(CC) -c -o $@ $< $(CFLAGS)
+
 $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
 	$(CC) -c -o $@ $< $(CFLAGS)
 

Tools/tests-host/hrt.cpp

@@ -0,0 +1,13 @@
+#include <sys/time.h>
+#include <inttypes.h>
+#include <drivers/drv_hrt.h>
+#include <stdio.h>
+
+uint64_t hrt_absolute_time()
+{
+    struct timeval te; 
+    gettimeofday(&te, NULL); // get current time
+    unsigned long long us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us
+    printf("us: %lld\n", us);
+    return us;
+}

Tools/tests-host/mixer_test.cpp

@@ -9,4 +9,6 @@ int main(int argc, char *argv[]) {
 				 "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
 
 	test_mixer(3, args);
+
+	test_conv(1, args);
 }

Tools/tests-host/queue.h

@@ -0,0 +1,133 @@
+/************************************************************************
+ * include/queue.h
+ *
+ *   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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.
+ *
+ ************************************************************************/
+
+#ifndef __INCLUDE_QUEUE_H
+#define __INCLUDE_QUEUE_H
+
+#ifndef FAR
+#define FAR
+#endif
+
+/************************************************************************
+ * Included Files
+ ************************************************************************/
+
+#include <sys/types.h>
+
+/************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************/
+
+#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
+#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
+
+#define sq_next(p) ((p)->flink)
+#define dq_next(p) ((p)->flink)
+#define dq_prev(p) ((p)->blink)
+
+#define sq_empty(q) ((q)->head == NULL)
+#define dq_empty(q) ((q)->head == NULL)
+
+#define sq_peek(q)  ((q)->head)
+#define dq_peek(q)  ((q)->head)
+
+/************************************************************************
+ * Global Type Declarations
+ ************************************************************************/
+
+struct sq_entry_s
+{
+  FAR struct sq_entry_s *flink;
+};
+typedef struct sq_entry_s sq_entry_t;
+
+struct dq_entry_s
+{
+  FAR struct dq_entry_s *flink;
+  FAR struct dq_entry_s *blink;
+};
+typedef struct dq_entry_s dq_entry_t;
+
+struct sq_queue_s
+{
+  FAR sq_entry_t *head;
+  FAR sq_entry_t *tail;
+};
+typedef struct sq_queue_s  sq_queue_t;
+
+struct dq_queue_s
+{
+  FAR dq_entry_t *head;
+  FAR dq_entry_t *tail;
+};
+typedef struct dq_queue_s dq_queue_t;
+
+/************************************************************************
+ * Global Function Prototypes
+ ************************************************************************/
+
+#ifdef __cplusplus
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+EXTERN void  sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void  dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue);
+EXTERN void  sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void  dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue);
+EXTERN void  sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
+                         sq_queue_t *queue);
+EXTERN void  dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node,
+                         dq_queue_t *queue);
+EXTERN void  dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node,
+                          dq_queue_t *queue);
+
+EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void            sq_rem(FAR sq_entry_t *node, sq_queue_t *queue);
+EXTERN void            dq_rem(FAR dq_entry_t *node, dq_queue_t *queue);
+EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue);
+EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue);
+EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue);
+EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue);
+
+#undef EXTERN
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __INCLUDE_QUEUE_H_ */
+

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 PWM_LIMIT_STATE_OFF:
-		case LIMIT_STATE_INIT:
+		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 {
+	enum pwm_limit_state state;
-		LIMIT_STATE_OFF = 0,
-		LIMIT_STATE_INIT,
-		LIMIT_STATE_RAMP,
-		LIMIT_STATE_ON
-	} 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
+ * @file test_rc.c
- * Tests the PWM outputs and PPM input
+ * 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}
 };