Anton Babushkin
12 years ago
28 changed files with 584 additions and 127 deletions
Binary file not shown.
@ -0,0 +1,2 @@
@@ -0,0 +1,2 @@
|
||||
./obj/* |
||||
mixer_test |
||||
@ -0,0 +1,39 @@
@@ -0,0 +1,39 @@
|
||||
|
||||
CC=g++
|
||||
CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0"
|
||||
|
||||
ODIR=obj
|
||||
LDIR =../lib
|
||||
|
||||
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 = $(patsubst %,$(ODIR)/%,$(_OBJ))
|
||||
|
||||
#$(DEPS)
|
||||
$(ODIR)/%.o: %.cpp |
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp |
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp |
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp |
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c |
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
#
|
||||
mixer_test: $(OBJ) |
||||
g++ -o $@ $^ $(CFLAGS) $(LIBS)
|
||||
|
||||
.PHONY: clean |
||||
|
||||
clean: |
||||
rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~
|
||||
@ -0,0 +1,12 @@
@@ -0,0 +1,12 @@
|
||||
#include <systemlib/mixer/mixer.h> |
||||
#include <systemlib/err.h> |
||||
#include "../../src/systemcmds/tests/tests.h" |
||||
|
||||
int main(int argc, char *argv[]) { |
||||
warnx("Host execution started"); |
||||
|
||||
char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix", |
||||
"../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; |
||||
|
||||
test_mixer(3, args); |
||||
} |
||||
@ -0,0 +1,100 @@
@@ -0,0 +1,100 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2012 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 mixer_load.c |
||||
* |
||||
* Programmable multi-channel mixer library. |
||||
*/ |
||||
|
||||
#include <nuttx/config.h> |
||||
#include <string.h> |
||||
#include <stdio.h> |
||||
#include <ctype.h> |
||||
|
||||
#include "mixer_load.h" |
||||
|
||||
int load_mixer_file(const char *fname, char *buf, unsigned maxlen) |
||||
{ |
||||
FILE *fp; |
||||
char line[120]; |
||||
|
||||
/* open the mixer definition file */ |
||||
fp = fopen(fname, "r"); |
||||
if (fp == NULL) { |
||||
return 1; |
||||
} |
||||
|
||||
/* read valid lines from the file into a buffer */ |
||||
buf[0] = '\0'; |
||||
for (;;) { |
||||
|
||||
/* get a line, bail on error/EOF */ |
||||
line[0] = '\0'; |
||||
if (fgets(line, sizeof(line), fp) == NULL) |
||||
break; |
||||
|
||||
/* if the line doesn't look like a mixer definition line, skip it */ |
||||
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) |
||||
continue; |
||||
|
||||
/* compact whitespace in the buffer */ |
||||
char *t, *f; |
||||
for (f = line; *f != '\0'; f++) { |
||||
/* scan for space characters */ |
||||
if (*f == ' ') { |
||||
/* look for additional spaces */ |
||||
t = f + 1; |
||||
while (*t == ' ') |
||||
t++; |
||||
if (*t == '\0') { |
||||
/* strip trailing whitespace */ |
||||
*f = '\0'; |
||||
} else if (t > (f + 1)) { |
||||
memmove(f + 1, t, strlen(t) + 1); |
||||
} |
||||
} |
||||
} |
||||
|
||||
/* if the line is too long to fit in the buffer, bail */ |
||||
if ((strlen(line) + strlen(buf) + 1) >= maxlen) { |
||||
return 1; |
||||
} |
||||
|
||||
/* add the line to the buffer */ |
||||
strcat(buf, line); |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
@ -0,0 +1,51 @@
@@ -0,0 +1,51 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2012 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 mixer_load.h |
||||
* |
||||
*/ |
||||
|
||||
|
||||
#ifndef _SYSTEMLIB_MIXER_LOAD_H |
||||
#define _SYSTEMLIB_MIXER_LOAD_H value |
||||
|
||||
#include <nuttx/config.h> |
||||
|
||||
__BEGIN_DECLS |
||||
|
||||
__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); |
||||
|
||||
__END_DECLS |
||||
|
||||
#endif |
||||
@ -0,0 +1,199 @@
@@ -0,0 +1,199 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 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_mixer.hpp |
||||
* |
||||
* Mixer load test |
||||
*/ |
||||
|
||||
#include <nuttx/config.h> |
||||
|
||||
#include <sys/types.h> |
||||
|
||||
#include <stdio.h> |
||||
#include <stdlib.h> |
||||
#include <unistd.h> |
||||
#include <fcntl.h> |
||||
#include <errno.h> |
||||
#include <string.h> |
||||
#include <time.h> |
||||
|
||||
#include <systemlib/err.h> |
||||
#include <systemlib/mixer/mixer.h> |
||||
|
||||
#include "tests.h" |
||||
|
||||
static int mixer_callback(uintptr_t handle, |
||||
uint8_t control_group, |
||||
uint8_t control_index, |
||||
float &control); |
||||
|
||||
int test_mixer(int argc, char *argv[]) |
||||
{ |
||||
warnx("testing mixer"); |
||||
|
||||
char *filename = "/etc/mixers/IO_pass.mix"; |
||||
|
||||
if (argc > 1) |
||||
filename = argv[1]; |
||||
|
||||
warnx("loading: %s", filename); |
||||
|
||||
char buf[2048]; |
||||
|
||||
load_mixer_file(filename, &buf[0], sizeof(buf)); |
||||
unsigned loaded = strlen(buf); |
||||
|
||||
warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); |
||||
|
||||
/* load the mixer in chunks, like
|
||||
* in the case of a remote load, |
||||
* e.g. on PX4IO. |
||||
*/ |
||||
|
||||
unsigned nused = 0; |
||||
|
||||
const unsigned chunk_size = 64; |
||||
|
||||
MixerGroup mixer_group(mixer_callback, 0); |
||||
|
||||
/* load at once test */ |
||||
unsigned xx = loaded; |
||||
mixer_group.load_from_buf(&buf[0], xx); |
||||
warnx("complete buffer load: loaded %u mixers", mixer_group.count()); |
||||
if (mixer_group.count() != 8) |
||||
return 1; |
||||
|
||||
unsigned empty_load = 2; |
||||
char empty_buf[2]; |
||||
empty_buf[0] = ' '; |
||||
empty_buf[1] = '\0'; |
||||
mixer_group.reset(); |
||||
mixer_group.load_from_buf(&empty_buf[0], empty_load); |
||||
warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); |
||||
if (empty_load != 0) |
||||
return 1; |
||||
|
||||
/* FIRST mark the mixer as invalid */ |
||||
bool mixer_ok = false; |
||||
/* THEN actually delete it */ |
||||
mixer_group.reset(); |
||||
char mixer_text[256]; /* large enough for one mixer */ |
||||
unsigned mixer_text_length = 0; |
||||
|
||||
unsigned transmitted = 0; |
||||
|
||||
warnx("transmitted: %d, loaded: %d", transmitted, loaded); |
||||
|
||||
while (transmitted < loaded) { |
||||
|
||||
unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted; |
||||
|
||||
/* check for overflow - this would be really fatal */ |
||||
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { |
||||
bool mixer_ok = false; |
||||
return 1; |
||||
} |
||||
|
||||
/* append mixer text and nul-terminate */ |
||||
memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length); |
||||
mixer_text_length += text_length; |
||||
mixer_text[mixer_text_length] = '\0'; |
||||
warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); |
||||
|
||||
/* process the text buffer, adding new mixers as their descriptions can be parsed */ |
||||
unsigned resid = mixer_text_length; |
||||
mixer_group.load_from_buf(&mixer_text[0], resid); |
||||
|
||||
/* if anything was parsed */ |
||||
if (resid != mixer_text_length) { |
||||
|
||||
/* only set mixer ok if no residual is left over */ |
||||
if (resid == 0) { |
||||
mixer_ok = true; |
||||
} else { |
||||
/* not yet reached the end of the mixer, set as not ok */ |
||||
mixer_ok = false; |
||||
} |
||||
|
||||
warnx("used %u", mixer_text_length - resid); |
||||
|
||||
/* copy any leftover text to the base of the buffer for re-use */ |
||||
if (resid > 0) |
||||
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); |
||||
|
||||
mixer_text_length = resid; |
||||
} |
||||
|
||||
transmitted += text_length; |
||||
} |
||||
|
||||
warnx("chunked load: loaded %u mixers", mixer_group.count()); |
||||
|
||||
if (mixer_group.count() != 8) |
||||
return 1; |
||||
|
||||
/* load multirotor at once test */ |
||||
mixer_group.reset(); |
||||
|
||||
if (argc > 2) |
||||
filename = argv[2]; |
||||
else |
||||
filename = "/etc/mixers/FMU_quad_w.mix"; |
||||
|
||||
load_mixer_file(filename, &buf[0], sizeof(buf)); |
||||
loaded = strlen(buf); |
||||
|
||||
warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); |
||||
|
||||
unsigned mc_loaded = loaded; |
||||
mixer_group.load_from_buf(&buf[0], mc_loaded); |
||||
warnx("complete buffer load: loaded %u mixers", mixer_group.count()); |
||||
if (mixer_group.count() != 8) |
||||
return 1; |
||||
} |
||||
|
||||
static int |
||||
mixer_callback(uintptr_t handle, |
||||
uint8_t control_group, |
||||
uint8_t control_index, |
||||
float &control) |
||||
{ |
||||
if (control_group != 0) |
||||
return -1; |
||||
|
||||
control = 0.0f; |
||||
|
||||
return 0; |
||||
} |
||||
Loading…
Reference in new issue