Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

Add conversions and mixer tests. Work in progress

Browse Source
This commit is contained in:
Lorenz Meier
2013-12-31 14:45:38 +01:00
parent c367959d28
commit 7f14f1f7de
12 changed files with 315 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/tests-host/Makefile
+9 -1
View File
@@ -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
+13
View File
@@ -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
+2
View File
@@ -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
+133
View File
@@ -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
+15 -15
View File
@@ -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
+10 -8
View File
@@ -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
+2 -1
View File
@@ -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
+76
View File
@@ -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 -1
View File
@@ -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
+2 -2
View File
@@ -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
+1
View File
@@ -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
+1
View File
@@ -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}
};