aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Tools/tests-host/Makefile10
-rw-r--r--Tools/tests-host/hrt.cpp13
-rw-r--r--Tools/tests-host/mixer_test.cpp2
-rw-r--r--Tools/tests-host/queue.h133
-rw-r--r--src/modules/systemlib/pwm_limit/pwm_limit.c30
-rw-r--r--src/modules/systemlib/pwm_limit/pwm_limit.h18
-rw-r--r--src/systemcmds/tests/module.mk3
-rw-r--r--src/systemcmds/tests/test_conv.cpp76
-rw-r--r--src/systemcmds/tests/test_mixer.cpp52
-rw-r--r--src/systemcmds/tests/test_rc.c4
-rw-r--r--src/systemcmds/tests/tests.h1
-rw-r--r--src/systemcmds/tests/tests_main.c1
12 files changed, 315 insertions, 28 deletions
diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile
index 97410ff47..7ab1454f0 100644
--- a/Tools/tests-host/Makefile
+++ b/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)
diff --git a/Tools/tests-host/hrt.cpp b/Tools/tests-host/hrt.cpp
new file mode 100644
index 000000000..dc9fa23de
--- /dev/null
+++ b/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;
+} \ No newline at end of file
diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp
index 042322aad..e311617f9 100644
--- a/Tools/tests-host/mixer_test.cpp
+++ b/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);
} \ No newline at end of file
diff --git a/Tools/tests-host/queue.h b/Tools/tests-host/queue.h
new file mode 100644
index 000000000..0fdb170db
--- /dev/null
+++ b/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_ */
+
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
index cac3dc82a..992a7024b 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.c
+++ b/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 */
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h
index 9974770be..bc8709398 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.h
+++ b/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);
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}
};
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-05 17:40:50 +0000