From c5b890e87d545328734a815d90ce16d396630048 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:17:59 +0200 Subject: [PATCH 01/37] Moved mixer file load / compression into mixer library. --- src/modules/systemlib/mixer/mixer.cpp | 2 + src/modules/systemlib/mixer/mixer.h | 2 + src/modules/systemlib/mixer/mixer_load.c | 99 +++++++++++++++++++ src/modules/systemlib/mixer/mixer_load.h | 51 ++++++++++ .../systemlib/mixer/mixer_multirotor.cpp | 4 +- src/modules/systemlib/mixer/module.mk | 3 +- src/systemcmds/mixer/{mixer.c => mixer.cpp} | 55 ++--------- src/systemcmds/mixer/module.mk | 2 +- 8 files changed, 167 insertions(+), 51 deletions(-) create mode 100644 src/modules/systemlib/mixer/mixer_load.c create mode 100644 src/modules/systemlib/mixer/mixer_load.h rename src/systemcmds/mixer/{mixer.c => mixer.cpp} (73%) diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index b1bb1a66d7..b4b82c539f 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -50,6 +50,8 @@ #include #include #include +#include +#include #include "mixer.h" diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index bbfa130a9d..6c322ff92e 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -128,7 +128,9 @@ #ifndef _SYSTEMLIB_MIXER_MIXER_H #define _SYSTEMLIB_MIXER_MIXER_H value +#include #include "drivers/drv_mixer.h" +#include "mixer_load.h" /** * Abstract class defining a mixer mixing zero or more inputs to diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c new file mode 100644 index 0000000000..18c4e474a6 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +#include "mixer_load.h" + +int load_mixer_file(const char *fname, char *buf) +{ + FILE *fp; + char line[120]; + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + if (fp == NULL) + err(1, "can't open %s", fname); + + /* 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 = buf; *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) >= sizeof(buf)) + break; + + /* add the line to the buffer */ + strcat(buf, line); + } + + return 0; +} + diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h new file mode 100644 index 0000000000..b2327a4f72 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.h @@ -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 + +__BEGIN_DECLS + +__EXPORT int load_mixer_file(const char *fname, char *buf); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 2446cc3fbe..89afc272c0 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -130,7 +130,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { { 1.000000, 0.000000, -1.00 }, { -1.000000, 0.000000, -1.00 }, }; -const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { +const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], &_config_quad_v[0], @@ -140,7 +140,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME &_config_octa_x[0], &_config_octa_plus[0], }; -const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { +const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ 4, /* quad_plus */ 4, /* quad_v */ diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index 4d45e1c506..fc7485e202 100644 --- a/src/modules/systemlib/mixer/module.mk +++ b/src/modules/systemlib/mixer/module.mk @@ -39,4 +39,5 @@ LIBNAME = mixerlib SRCS = mixer.cpp \ mixer_group.cpp \ mixer_multirotor.cpp \ - mixer_simple.cpp + mixer_simple.cpp \ + mixer_load.c diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.cpp similarity index 73% rename from src/systemcmds/mixer/mixer.c rename to src/systemcmds/mixer/mixer.cpp index e642ed0676..b1ebebbb44 100644 --- a/src/systemcmds/mixer/mixer.c +++ b/src/systemcmds/mixer/mixer.cpp @@ -47,10 +47,15 @@ #include #include -#include +#include #include -__EXPORT int mixer_main(int argc, char *argv[]); +/** + * Mixer utility for loading mixer files to devices + * + * @ingroup apps + */ +extern "C" __EXPORT int mixer_main(int argc, char *argv[]); static void usage(const char *reason) noreturn_function; static void load(const char *devname, const char *fname) noreturn_function; @@ -87,8 +92,6 @@ static void load(const char *devname, const char *fname) { int dev; - FILE *fp; - char line[120]; char buf[2048]; /* open the device */ @@ -99,49 +102,7 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - /* open the mixer definition file */ - fp = fopen(fname, "r"); - if (fp == NULL) - err(1, "can't open %s", fname); - - /* 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 = buf; *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) >= sizeof(buf)) - break; - - /* add the line to the buffer */ - strcat(buf, line); - } + load_mixer_file(fname, &buf[0]); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index d5a3f9f77b..cdbff75f04 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -36,7 +36,7 @@ # MODULE_COMMAND = mixer -SRCS = mixer.c +SRCS = mixer.cpp MODULE_STACKSIZE = 4096 From 7d6a4ece6b1875bab62c4dbcb95fcc9fa5661674 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:19:09 +0200 Subject: [PATCH 02/37] Added mixer test --- src/systemcmds/tests/module.mk | 1 + src/systemcmds/tests/test_mixer.cpp | 74 +++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 5 ++ src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 81 insertions(+) create mode 100644 src/systemcmds/tests/test_mixer.cpp diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 754d3a0da9..6729ce4ae3 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -23,6 +23,7 @@ SRCS = test_adc.c \ test_uart_console.c \ test_uart_loopback.c \ test_uart_send.c \ + test_mixer.cpp \ tests_file.c \ tests_main.c \ tests_param.c diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp new file mode 100644 index 0000000000..acb7bd88f6 --- /dev/null +++ b/src/systemcmds/tests/test_mixer.cpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "tests.h" + +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]); + + warnx("loaded: %s", &buf[0]); + + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c02ea6808f..c483108cf0 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -76,6 +76,8 @@ * Public Function Prototypes ****************************************************************************/ +__BEGIN_DECLS + extern int test_sensors(int argc, char *argv[]); extern int test_gpio(int argc, char *argv[]); extern int test_hrt(int argc, char *argv[]); @@ -98,5 +100,8 @@ extern int test_jig_voltages(int argc, char *argv[]); extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); +extern int test_mixer(int argc, char *argv[]); + +__END_DECLS #endif /* __APPS_PX4_TESTS_H */ diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9f8c5c9eae..9eb9c632ec 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -110,6 +110,7 @@ const struct { {"param", test_param, 0}, {"bson", test_bson, 0}, {"file", test_file, 0}, + {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 42b75ae8963b2f711a72ac1cb6cfd1b44bd826b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Oct 2013 20:19:34 +0200 Subject: [PATCH 03/37] Added host-building mixer test --- Tools/tests-host/.gitignore | 2 ++ Tools/tests-host/Makefile | 36 +++++++++++++++++++++++++++++ Tools/tests-host/arch/board/board.h | 0 Tools/tests-host/mixer_test.cpp | 12 ++++++++++ Tools/tests-host/nuttx/config.h | 0 5 files changed, 50 insertions(+) create mode 100644 Tools/tests-host/.gitignore create mode 100644 Tools/tests-host/Makefile create mode 100644 Tools/tests-host/arch/board/board.h create mode 100644 Tools/tests-host/mixer_test.cpp create mode 100644 Tools/tests-host/nuttx/config.h diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore new file mode 100644 index 0000000000..61e0915515 --- /dev/null +++ b/Tools/tests-host/.gitignore @@ -0,0 +1,2 @@ +./obj/* +mixer_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile new file mode 100644 index 0000000000..c603b2aa2a --- /dev/null +++ b/Tools/tests-host/Makefile @@ -0,0 +1,36 @@ + +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 +OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) + +#$(DEPS) +$(ODIR)/%.o: %.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/systemcmds/tests/%.c + $(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) + +# +mixer_test: $(OBJ) + g++ -o $@ $^ $(CFLAGS) $(LIBS) + +.PHONY: clean + +clean: + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ \ No newline at end of file diff --git a/Tools/tests-host/arch/board/board.h b/Tools/tests-host/arch/board/board.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp new file mode 100644 index 0000000000..5d92040f11 --- /dev/null +++ b/Tools/tests-host/mixer_test.cpp @@ -0,0 +1,12 @@ +#include +#include + +extern int test_mixer(int argc, char *argv[]); + +int main(int argc, char *argv[]) { + warnx("Host execution started"); + + char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix"}; + + test_mixer(2, args); +} \ No newline at end of file diff --git a/Tools/tests-host/nuttx/config.h b/Tools/tests-host/nuttx/config.h new file mode 100644 index 0000000000..e69de29bb2 From 1dc9569e31717aefab8e05b858122f433dab1698 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Oct 2013 11:44:26 +0200 Subject: [PATCH 04/37] Fixed mixer chunk load and line ending detection for good. --- src/modules/systemlib/mixer/mixer.cpp | 27 ++++ src/modules/systemlib/mixer/mixer.h | 23 ++++ src/modules/systemlib/mixer/mixer_group.cpp | 15 ++ src/modules/systemlib/mixer/mixer_load.c | 15 +- src/modules/systemlib/mixer/mixer_load.h | 2 +- .../systemlib/mixer/mixer_multirotor.cpp | 10 +- src/modules/systemlib/mixer/mixer_simple.cpp | 62 +++------ src/systemcmds/mixer/mixer.cpp | 2 +- src/systemcmds/tests/test_mixer.cpp | 129 +++++++++++++++++- 9 files changed, 229 insertions(+), 56 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index b4b82c539f..cce46bf5fc 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -116,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler) return 0; } +const char * +Mixer::findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; + } + return nullptr; +} + +const char * +Mixer::skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) { + /* skip up to it AND one beyond - could be on the NUL symbol now */ + buflen -= (p - buf) + 1; + return p + 1; + } + + return nullptr; +} + /****************************************************************************/ NullMixer::NullMixer() : diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 6c322ff92e..723bf9f3b7 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -212,6 +212,24 @@ protected: */ static int scale_check(struct mixer_scaler_s &scaler); + /** + * Find a tag + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @param tag character to search for. + */ + static const char * findtag(const char *buf, unsigned &buflen, char tag); + + /** + * Skip a line + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @return 0 / OK if a line could be skipped, 1 else + */ + static const char * skipline(const char *buf, unsigned &buflen); + private: }; @@ -240,6 +258,11 @@ public: */ void reset(); + /** + * Count the mixers in the group. + */ + unsigned count(); + /** * Adds mixers to the group based on a text description in a buffer. * diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 1dbc512cdb..3ed99fba09 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space) return index; } +unsigned +MixerGroup::count() +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr)) { + mixer = mixer->_next; + index++; + } + + return index; +} + void MixerGroup::groups_required(uint32_t &groups) { @@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) /* only adjust buflen if parsing was successful */ buflen = resid; + debug("SUCCESS - buflen: %d", buflen); } else { /* diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index 18c4e474a6..a55ddf8a35 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -38,22 +38,22 @@ */ #include -#include #include #include #include #include "mixer_load.h" -int load_mixer_file(const char *fname, char *buf) +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) - err(1, "can't open %s", fname); + if (fp == NULL) { + return 1; + } /* read valid lines from the file into a buffer */ buf[0] = '\0'; @@ -70,7 +70,7 @@ int load_mixer_file(const char *fname, char *buf) /* compact whitespace in the buffer */ char *t, *f; - for (f = buf; *f != '\0'; f++) { + for (f = line; *f != '\0'; f++) { /* scan for space characters */ if (*f == ' ') { /* look for additional spaces */ @@ -87,8 +87,9 @@ int load_mixer_file(const char *fname, char *buf) } /* if the line is too long to fit in the buffer, bail */ - if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) - break; + if ((strlen(line) + strlen(buf) + 1) >= maxlen) { + return 1; + } /* add the line to the buffer */ strcat(buf, line); diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h index b2327a4f72..4b7091d5b2 100644 --- a/src/modules/systemlib/mixer/mixer_load.h +++ b/src/modules/systemlib/mixer/mixer_load.h @@ -44,7 +44,7 @@ __BEGIN_DECLS -__EXPORT int load_mixer_file(const char *fname, char *buf); +__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); __END_DECLS diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 89afc272c0..b89f341b60 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -205,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } if (used > (int)buflen) { - debug("multirotor spec used %d of %u", used, buflen); + debug("OVERFLOW: multirotor spec used %d of %u", used, buflen); return nullptr; } - buflen -= used; + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return nullptr; + } + + debug("remaining in buf: %d, first char: %c", buflen, buf[0]); if (!strcmp(geomname, "4+")) { geometry = MultirotorMixer::QUAD_PLUS; diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index c8434f991b..c3985b5de6 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -55,7 +55,7 @@ #include "mixer.h" #define debug(fmt, args...) do { } while(0) -// #define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, @@ -71,28 +71,6 @@ SimpleMixer::~SimpleMixer() free(_info); } -static const char * -findtag(const char *buf, unsigned &buflen, char tag) -{ - while (buflen >= 2) { - if ((buf[0] == tag) && (buf[1] == ':')) - return buf; - buf++; - buflen--; - } - return nullptr; -} - -static void -skipline(const char *buf, unsigned &buflen) -{ - const char *p; - - /* if we can find a CR or NL in the buffer, skip up to it */ - if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) - buflen -= (p - buf); -} - int SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) { @@ -111,7 +89,12 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); return -1; } - skipline(buf, buflen); + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } scaler.negative_scale = s[0] / 10000.0f; scaler.positive_scale = s[1] / 10000.0f; @@ -130,7 +113,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale buf = findtag(buf, buflen, 'S'); if ((buf == nullptr) || (buflen < 16)) { - debug("contorl parser failed finding tag, ret: '%s'", buf); + debug("control parser failed finding tag, ret: '%s'", buf); return -1; } @@ -139,7 +122,12 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale debug("control parse failed on '%s'", buf); return -1; } - skipline(buf, buflen); + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } control_group = u[0]; control_index = u[1]; @@ -161,29 +149,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c int used; const char *end = buf + buflen; - /* enforce that the mixer ends with space or a new line */ - for (int i = buflen - 1; i >= 0; i--) { - if (buf[i] == '\0') - continue; - - /* require a space or newline at the end of the buffer, fail on printable chars */ - if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { - /* found a line ending or space, so no split symbols / numbers. good. */ - break; - } else { - debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); - goto out; - } - - } - /* get the base info for the mixer */ if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { debug("simple parse failed on '%s'", buf); goto out; } - buflen -= used; + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + goto out; + } mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index b1ebebbb44..6da39d371b 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -102,7 +102,7 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - load_mixer_file(fname, &buf[0]); + load_mixer_file(fname, &buf[0], sizeof(buf)); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index acb7bd88f6..4da86042d2 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -53,6 +54,11 @@ #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"); @@ -66,9 +72,128 @@ int test_mixer(int argc, char *argv[]) char buf[2048]; - load_mixer_file(filename, &buf[0]); + load_mixer_file(filename, &buf[0], sizeof(buf)); + unsigned loaded = strlen(buf); - warnx("loaded: %s", &buf[0]); + 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; } From 5671df0af4e258ef0a83377cdbd50e59734aa00b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Oct 2013 11:44:42 +0200 Subject: [PATCH 05/37] Improved mixer tests --- Tools/tests-host/Makefile | 7 +++++-- Tools/tests-host/mixer_test.cpp | 8 ++++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index c603b2aa2a..97410ff47c 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -10,14 +10,14 @@ 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 +_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/%.c +$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp $(CC) -c -o $@ $< $(CFLAGS) $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp @@ -26,6 +26,9 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp $(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) diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp index 5d92040f11..042322aadc 100644 --- a/Tools/tests-host/mixer_test.cpp +++ b/Tools/tests-host/mixer_test.cpp @@ -1,12 +1,12 @@ #include #include - -extern int test_mixer(int argc, char *argv[]); +#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"}; + char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix", + "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; - test_mixer(2, args); + test_mixer(3, args); } \ No newline at end of file From af7288ed936d642f3141a3e8792799909d351885 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 09:56:57 +0200 Subject: [PATCH 06/37] Enable position control for Easystar type planes --- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 3 +-- ROMFS/px4fmu_common/init.d/101_hk_bixler | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 828540ad90..1aa1ef494e 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -98,8 +98,7 @@ else mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix fi fw_att_control start -# Not ready yet for prime-time -#fw_pos_control_l1 start +fw_pos_control_l1 start if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index 5a9cb2171a..4d8a24c4ae 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -91,8 +91,7 @@ att_pos_estimator_ekf start # mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix fw_att_control start -# Not ready yet for prime-time -#fw_pos_control_l1 start +fw_pos_control_l1 start if [ $EXIT_ON_END == yes ] then From 57b8dee7092eb84e8f4f31dcee85736eedb2ca8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 13:41:37 +0200 Subject: [PATCH 07/37] Bring back proper log conversion copy operation --- src/modules/sdlog2/sdlog2.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec8033202d..080aa550cb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -583,6 +583,15 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } + const char *converter_in = "/etc/logging/conv.zip"; + char* converter_out = malloc(120); + sprintf(converter_out, "%s/conv.zip", folder_path); + + if (file_copy(converter_in, converter_out)) { + errx(1, "unable to copy conversion scripts, exiting."); + } + free(converter_out); + /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); @@ -1251,7 +1260,7 @@ int file_copy(const char *file_old, const char *file_new) fclose(source); fclose(target); - return ret; + return OK; } void handle_command(struct vehicle_command_s *cmd) From c6b58491bbd7390650723c65b4d4e9ec0922c8de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 22:18:44 +0200 Subject: [PATCH 08/37] Work queue in RGB driver without work_cancel() --- src/drivers/rgbled/rgbled.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index fedff769b8..aeb7e2f787 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -248,6 +248,11 @@ RGBLED::led_trampoline(void *arg) void RGBLED::led() { + if (!_should_run) { + _running = false; + return; + } + switch (_mode) { case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_NORMAL: @@ -471,11 +476,6 @@ RGBLED::set_mode(rgbled_mode_t mode) work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } - /* if it should stop, then cancel the workq */ - if (!should_run && _running) { - _running = false; - work_cancel(LPWORK, &_work); - } } } From fbe595a591734bffa95d28125b8e0bda117d7314 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 23:10:12 +0200 Subject: [PATCH 09/37] Fixed some stupid compile errors --- src/drivers/rgbled/rgbled.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index aeb7e2f787..ea87b37d94 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -103,6 +103,7 @@ private: bool _running; int _led_interval; + bool _should_run; int _counter; void set_color(rgbled_color_t ledcolor); @@ -136,6 +137,7 @@ RGBLED::RGBLED(int bus, int rgbled) : _brightness(1.0f), _running(false), _led_interval(0), + _should_run(false), _counter(0) { memset(&_work, 0, sizeof(_work)); @@ -414,10 +416,10 @@ RGBLED::set_mode(rgbled_mode_t mode) { if (mode != _mode) { _mode = mode; - bool should_run = false; switch (mode) { case RGBLED_MODE_OFF: + _should_run = false; send_led_enable(false); break; @@ -428,7 +430,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_SLOW: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 2000; _brightness = 1.0f; @@ -436,7 +438,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_NORMAL: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 500; _brightness = 1.0f; @@ -444,7 +446,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_FAST: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 100; _brightness = 1.0f; @@ -452,14 +454,14 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BREATHE: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 25; send_led_enable(true); break; case RGBLED_MODE_PATTERN: - should_run = true; + _should_run = true; _counter = 0; _brightness = 1.0f; send_led_enable(true); @@ -471,7 +473,7 @@ RGBLED::set_mode(rgbled_mode_t mode) } /* if it should run now, start the workq */ - if (should_run && !_running) { + if (_should_run && !_running) { _running = true; work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } From 39336fd58533c85ef6bad93b80dd51e62b6eba3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 10:46:28 +0200 Subject: [PATCH 10/37] Better defaults for RC SCALE --- ROMFS/px4fmu_common/init.d/1000_rc_fw.hil | 2 ++ ROMFS/px4fmu_common/init.d/100_mpx_easystar | 2 ++ ROMFS/px4fmu_common/init.d/101_hk_bixler | 2 ++ ROMFS/px4fmu_common/init.d/31_io_phantom | 2 ++ 4 files changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil index 6e29bd6f80..5e4028bbb8 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil @@ -33,6 +33,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 1aa1ef494e..4f843e9aa1 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index 4d8a24c4ae..cef86c34d0 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 8fe94452f8..6528337454 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 17 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save From 8ed0796448e49ae34dbfdf31c056e402834c0b8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 22:17:53 +0200 Subject: [PATCH 11/37] L1 control diagram illustrating corner case --- Documentation/l1_control.odg | Bin 0 -> 11753 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/l1_control.odg diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg new file mode 100644 index 0000000000000000000000000000000000000000..69910c677459aae4664a08b1bbeb6aaed48721ad GIT binary patch literal 11753 zcmeIYcT`hb6E+?}5RhIK&`_n9&^yvadPjN)0YXV23B3u@K|llnkuJSS?;uK15Ru+N zdM_eH`fuUAzIyNLeSi1+{{CjIvsQA>%rmoRpG@|%_h_nMUL^+rZ~%Z%Jyp3hOzt)g z003}#;Q#;tN0=iN?coFkJ2^SPAYe2M0q1drTX7@6C>V+x;RJ90}SAg!z% z{uB2vlfuW#%g6pMS{@?q?9Vm5((A#{@df>=N4j814A@)49(p|gy?>GWbTAS*g+v^9w?tE zm=7et#b;>=wG!sz;uYcpSwV%kz@ox}d?FAo5o=x{A*(-K|Mc>Q*Z=&sQI{`13JrtM zeNPp_@)Da1f^dPOB^mgI7}B}SEh%k1=k=$Nm3>RAY)dy{HWS$Nkov*;lLe8}!}v&L z(N|#_9VzNxV}R9rK7~VehXb+Q8l_jnJ+9ER?M4lV6pORGmfT}AN59BjO?Tb$UORUR z`zpGhzL`aZ^;%CYK-)7^qHmziq14>%#l0ONQe|lJQ-GDRhagVMCR^%mhQ=-mfe-2Bx`U}C#un++Os67@%!yw)4j1ta>ms?RJtUCm>@ zeU4O|5`?^XibmDQ7ObL$zf|BRZ$Z~S>_=1U5ho^!DFc%QU1-%teKUw#6q26`Zcm7s z=Lrz50|mQ$+_C%;uFq}s++SMxMqbE3DJQY#_95>6$~~w0JKWT+ zso(T8{aAJUHeUA<-z45bF9#~KK9VMW=jP*$znAgCiB!Gs)j}%=NxVGpDb^%OS-Oe7 zl4Wx=YlulWW9=&VfXdyDZtrc^gN{5REy0MJ75An|xAE&DzTvTp+pLS!PC4 z7g~A2ZxFDu;B~yy51Uau_Fuc%ywRpxI>Bv|Rv1o^O~c2)VS42HB`h%7#$X($3|SAK zE&Upm!UaiTq&O-y9gx`{td_6(B<%jwf2;4`FCV8b-bTvTttKIbt^qiVOQJC?Gt!QZ-3qnna!1$MI>%j{%!V zLhcrMndtnis}0?@%ece4+jKG%2##W)JOkV0%z*h?5WTUaGVzG~llkir90hx_xpyd2 zZ!3v5MFai9E3ZiBUM*3z|CZmDbgOU}6kuD%a(pAvEPr)UO>j}m#XPg5*kEbwi5Xd4 z<Nm3sws@^)Zo5n|nHJcxBNq>?%!s)fa3NfblmUU=%H&t|m`9jmeIszP>_#@B z^>VnJp&2DFmLN{1WJ*?5Gk3=2$==rH1aX(Y*iWHJX^X4t_y|@c(?w+_J9n@T+QICPGgCpyjuD@t=G00;t(zFhLTJ34HdP9P`Vk@)l)sDdl<_s45L z;A3O5KGSWes*1lRzS!kisO{k_Q>!_Ob>%8R0p`aLebnNu?1@c#3k3|_#p@m(u^ zCZka`f#NA}ga@~)>9gIt+&oX!a{&ibA8O`Q+>%geVA-+?)`*4qrP~IjeO0Y<{zB?VAd(47v;ASpd z)Qiyw?%MG zsa;PEZ^tu9oW<@Fx8%mI1f3Jodda*-iLN^{*4a~48UvLsdN*_*nbxb|za1>>y|uZY zcPtj&MYo;jm9>^issP~E;R?B>!PlvuH$3pe~6_s<&vdpt3C{4PPC&@#z zdl7vub&-cUi7`S25tD~*1p(r^*TgXSv=EsVXOH^})#t-j9hh@o9?RX)&NsfUNboA- zddf3(Iz8ba4)e+?K~;k=^Y#o3-rAl32~q~(XVx}Nl%5pKcyqzlu>s};mrpG56RwJav|C;q(H!CZ#F6|1ph98jyekYr54+8&4N#>1k z=zaVnvzN{bZY3MT6g&P@C7vlueZc(C$7>yZ7<8wOdU0CrHO20XjXOei-8jJpn)bLB zLkg)SL+f-#CUOh1!rV-%p4W0gpWY2Tyfam(DQ*xefScnF^_<&|z>1La2rgzcmr%L! zytOeK$$R(=e(n_`7qTNzQO0aS?I!qe#;v>XA(~-NH7|=7tJWJ7t<^0+1_!U|Eg$O{ zJ)5Hkhp-a9DMxYKQa0P~58Wa${W=!>M&SM!wNSAg>jf=g!fRUud6%~!zE%o-JU*|f zqO!fkph$$h+MOvPb5}p_fx;P#QWK>`mwI+A93mRU!`mFoT|}Y)KEB|Um!L|7ZDZp? zO{$1Ulh)^k_NxA-m*E`CUAFQOUll0+jzeD#m-po*$PP7oK`pxh&@1tlf)s{kDb@lPe8KAm2SH=eZ38T>-HI8 z>Q2a6*TIQtF!9Z*vTw%3Ggld~9d&5(>v2dEZ9QWU>v^o6)DLt$V@U3Pe)1w*!_9N& zrR3Bq!!f4OYbFk_=Fsi}V|jSF2CKg9+~;aajYF56$!|x1a|b$aTC1Ac#e`%F;I%Bv z!I`b0%FqJAZA&c6whctQO+>CFWpBzGHHl5Hb`&o_T16sFB5l!EhnB)+aRanYkvuh5C)W_tpLt|gza4s z$-tF9;y2Wy%7Gsk8LZNnWH)SaNO~lc1Bo&EL?}n<+CD$f$NH8#_-x8ChE0h(C*n=_>pdv8 zsRo1UrW_Uouj|&D(sH1L*$M{kdKlG1A2#cRDJBKo8bIT35Obd?>(HEY_t+|%HH9(2zA)gy+WHq{PLPpSYlUjVoZ>_awD|^~l@^e9(J&(I%GS@oErJLS`D+pM`ikL3I z8%AFoYu&)A`=Ikk=kaLH`#S%dQ&h*@uV}ni2lVkmcZ63?y_ks(&`Eb3grW5Kg`<)N z=MHoVz>3!sl;w>3>9@xE1`bC*g^9wIYBss$0?Z&EBb~)jHE9eC^T7_I8UAQKxRoQ# z`yzL)UC3n2_L^ET(r)n6v{2-mZEm@!>AR$wdmZkA@U0EBq2vbd?U$;#kbvoXiA3nA zL#DpK>1!}1u9k%9b&LkggGXpPPwW|Eog3D^CIjGoFB>TNZnY|ZHVam4z_mMIHFCSY|~|eS91GRq*3mvGP(E>S{@i?d6c-n{#NKazVI=xt)SE z?SV&>kikjac<<1;z>;mqGc(7zd-sKJ9q(%&OZX?V`Q+>+_1CaA)ek%5t4$eYMbCeu ze_NCA&?%)SSNt9WXN^X*?D+l*mYuHm1GnZ|2U3aM-C5k1%y!;g-JyLGyfaokg!PIj zjcaxS?GZmMWIMdAuykktqf2K>v|-)1jA=qj>Yibz;mN%mK_65n4_`K~e}AXgE6vv= zy)c*3$^c!YC*5=EVi|8k-6RNO)ZBKu%b!QTIFPf2QgOl?U}E>90--sYSd!+-;1@Ao_VrltH%<(H(Z>%`)SR~)@=%P zJUuiWU3bVwv=&)%0nQn-EXUdRPcomy_~Na({0R?KO*fyYN}}E}$~@rrPGdv{<&rL`LQ9H>;Kx zL?hZb+^2u9k1Y}jxW;t$q#`9twkRx2U?Fh6s_H$cFmK?54)+_ zeIvK298)li@2S>XSN!mHTCB0>P*K`&lJ=|bMoFt|4eeArpvAA)ll)3J3FPw^wHylW zeBcn(169O8EGfoi$k%Y4iCI*uaNgWN-B_d5!nRAK=N@Fk!C%UwwR_S-k`>zeumf9h zvYZ%v_sdRH(qjOQJV7w0HC>d4s%Uu%_^v#;S$`Pr2f^e+jo6)|cWq1Wt_po2e*JMQ z8BccjQ4B0vG<-mMSujU6E@Fgd+ZKzM3dqPkO`;)Y^o6_)>p?HJKa{#Xt1XEB6Sf=Y zea@{!_2_Eao{T%W0g9X-+YKTB*|vcjOYCrd42J+Olb|pqe|v{?_RgzF&=Ux;yc(HS z1Y@fjzLllRhb7ALS60IEd6+G7_IDL|b=;I?>OlLZ9i<`9wk#H>7$2QnSg^<4{-y0<5j`Qw)4^z`%_Sod+m zRC|H(ZnSxX6t-YU-D&-P@i1SHaeem)_sC-@?ENUHpyUGd&2BVPj~Jki=BzSSAfMh4 zgf8(CF>AAvv0WG&Xusej_i@Nhl@6x-cC=sdLbjl^A)XwP>wl0D?c04QWh6Ht9uyZw z?x!!H;@g+A*9XZm9muFswI8-WAyJ25odPdf>1w|J3T9V5n;p@3tN zC(}=Er&#A`K=@Z6QIf(mN;THO%~f5DgPq;OVtztirw5IBIzUGcGOO7cpR|bE`VyE$ z(GZ9FzE^XS>sg&DUZ8=hEmVYvev5|B9?gBL0~L2}Ptg>qdwU>TTfC$)l(?ttMdjX; zqH=mfK+EO0=N?EUhxjr$1pHqlpG zDDA$E-}H%!sr@eN9%w=`Cq(Obw0H4Nv@CFdi*`!KR8TZU>Ht;uKI>Dbd^bxpM~Bu5 zmFm`q`+;Uc}ufCFoP}2cLbK@>I9s^B$^`VAGT4d4lX{m*J)v-;w=*kBMQ*?e zu3Nl*oTn8+%c)BXWv8j;o&fRFO#ad8=tdAo4ppq{Q}%V?OU%%H$3-c9qRhghB^Iw{ zw++-Vn_A(%mh`ew1C9OEMV^}92>(3mv%|irI>Y18I~N_BTP=qdbN@sFT|4IlC^@rzT;1|I#%qIOg(S$xi~t0#y(a=Q0G zQ;qU(LR6&22u=)TX}JzJ=7-o`dZ;JncpYecFzrs1FoI7gfsI`V+_oT@o{O0;OOg-! zCJWw`Qjsf_CLe;IqB4>XL0zdTu3x3@L^Z{C$0Q$WwaGgqMb-NOgI2-*N_!xA;jG&u zorDG!lkew`^G0owR>FJ|d2_rhf!0y0w?{(W&of7_KT7zA%JrLK_Wy4EDC_@MnK&i7 zc~#xHXf(?8@<>5hL0!H?)*|4q$DO|%HUNMC06-1_w}#pLd18a&J2V2}-~@(49sUpPF3)3JP?lii|FEO|U7SdS4HAm_ z|FOLM%l~_NE+hTl{(fh5nUbGYzdQJ0`He3y1Oj#V=4(lsm)Y>Sgzgbm`fjAc#0Q#2 zH!RDpxMH_{+^CaLiEF=gpPaQ|D%Ic}eF*I?rN^U2)tE-5aOqe!R7GbEz@%}D?7Z0!(-eQTvY=}91o;Dg1GflmlDis6glMO?|cw$?HSgQDCcW+}rV z?HD{=I+OT>d9#Ia^zOQ^q*%k&2qtgmP0FmVsvK@Q&}Wev@ek(4mkg~j*S$7durkhP zCemCzn0L!iY*U1~W$BA6ypOBxdqokZdf3(#_HJ2pi6={AebcmhGdS#FPJ{DVYanaq z2$9~CyO>R%t`W#@>2=jOFULKOqDEhfy-I}^*t>s7rx%3b$NPrEJSO(~7)ERqi)kXW z9UJ3QQJNflW=Gu-24zpZ)|4coy(`I7Q#$2GUF$EfJITOh2{_M%fwmR>Lt6h&8Wg2T*IXEU=2&ZADAR$>^l8vHn8CNdnqTW(+!TB+RFsI1(Lfj zAVRzFWd0%A>>5N|&8v}iC5Ai4+g&L%+s5=F@>kj_b$KtINKd-AG9}8lT2UrCTRf8p z1d$h%apFmNH2at{V_n?S3@H5IvpVJA>PCg-HuF{_e{jb7_=(Dy6P4-A6vUZ|YfA$1 z>{Ef2dqm`sgC#!84i#_GDPf!+LU0qzeUFpISQEGPosWDZxxIq@?A-oH6diKUmf1OD zivmpS?Zci4w3QhTjRncDUMExH<9@hf1rU!O`ivXx2iE>vIqzDURy_SO3!RhPDo~3z z-*~;MkeVRBT1If5I6<_8EOzesSoi0)$m|=V&R+AySls;7UXM$B6tf?uurmcMXyx0N zg^A8PnSWao)AZZ;zy>XfZ)5XFG4psD$0NMM-ll@$XBE-z70tL6Ok|s=P1aM3m4K<`bHq-XDW3*u=fXGsa{LpEf2^$oqc8GLNOv!>7qbv zB1+aQsN7Qxx(XiJmS%Pd28E1JZ@0Vt2 zY2TD1Z4J&0E=|s7KgPWqwv;^x!KYfME(NoK?t_TS$Uk?xNgt@;D$xe-pFiYJ+zr)u zY*|&z^q$DnenI@&ErLZzmW4!>3OvQpDy>}6vI%~5rfD6eW=eU(^*T#--!m~w!$$`SMlr>izSt+pNk zAn6l)*;neq467eH$!rTN=pfngM>0p2#PLU2v}fxkOiA5j%pbh6as8^PO;j7_-C!~w z`dnO1T^r(7Xf^|>B)V?}ucqlulU}_~R)iCFC!ScWTnY7~1~L9FTRncog*Kd_TB-KE zEE0M>uPOJ7;}i3VK$O|s!i(znF#j87{x40}4Yo*XG>T6ek8bPloC%pbFQv#e1Ych^ zIF9`cjM&4c&FpAk+Vx*kC<;FlrMR7)qz^ECJEXG>z!Ge43l#%hXWXWnsyc4uCj0JCvZWKo(%>AGC_9%=Ag;@^*5}_UdWJ{!)*1mfGM4Zvqf}} zI{MMKH3QOag80`BOH`55Ihq5g_RO;)z^zIQsTza;K`FM8lK**WuHfML4)SIG-m$yR z`;C&;)cy~elXbS#itef+T#J(f;cIceaYCOih((F(>~bX4S8|bW1rGMqZ*JC)t7ya= zTA%nnOW;U#NE;ZKcVqD~YsSEk6lWh&R%5&Aa9`RKE$7DhwNSTa+sid^X?kxF+oMCF z+>6KoKEgPg-Y**1@<~3mxA(pkh4wdE5W#skNo`Aw#F#V4mnsR{#z@3~Q@W)CmgKsZS>LGX-q?c>WksjVt+&0soGj@z zwjZlPWad(5wAxe?V<)m-k-+Ouc#x0$FgSQ2sg}l!jp}~s)g(9Qwndr9< zEg$LKaMNG)`r0=T#o>BX*KM|}qq@LZ(t_zhq|$N$?4LZRx?Ck)>C)b)z3d$1|FKH? z-Fk-t005{d=*n;@t1I$2g5fZ0DC+y!aj{M-{4E`R(;)}LmVa?BF7qb1zd8K`p!k9u z7av1M&`+8)-!Ug&Cu`pta?)fYIk3_DQJ*FBy}QF0haZ?C{UD_^1DX~Ref)USExChD zK$0moA)OtSSOV)t%CGA2DQ6;Mm|&}z7RxTK51IT9x|3F)b6s^9gX>Iv^Mqt@e|2tj zIUE}oV->4boy4q@6kmxffZ>1QqX-Z*(t?q(Y<%NPwQn-+VE}I`2JXJ2*lMqFH$Xlo1REyk{+Z^K$mmO{>&}9=_c}pqpER6 zCH8gZeXSMR`I+Ri==Iiy`!`u|C1^?#a5&C#``%=23D1oHr+hFklUe{+>?FMc08C?E zCiMyiIpCk$Gyf_-nl=A!_4gjkpQ3fy;`wJs<`1hMQ}}B;z-8mqPwBt<*EXuZTmHJ` zciArRQz-HN(me3P@Sk=Be~JU;KkpCz9p$Ih9~|tb+`aYRl;gh`*H* z-+=wf(Ecq>)o*ZqWo-WzXY)5WzcRRg#<}EhKZW`Be<$g$jP9S2E(y_3G5-zHPloqz zQR07t@{{rXf%4;>z4ZNqwq0_*pVIvslz(7=|LcAF>i`*m|DCFz{O_-!{aPz88Q@Pb s1pdkge|Y~fq+iRyWhMP7_gMd-u4<}aVPC2d1Bd}aR{?+&wo9Y`4}A~OqW}N^ literal 0 HcmV?d00001 From 4532eca4ef6f6170765062ccd2ca7f29814e0b3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 22:55:16 +0200 Subject: [PATCH 12/37] Covered corner case in L1 controller not adressed so far in existing concepts --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 33 +++++++++++++++++------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index c5c0c7a3c1..daf136d495 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -130,8 +130,12 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); - float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + + /* calculate angle of airplane position vector relative to line) */ + + // XXX this could probably also be based solely on the dot product + float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); /* extension from [2], fly directly to A */ if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { @@ -148,21 +152,30 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); -// XXX this can be useful as last-resort guard, but is currently not needed -#if 0 - } else if (absf(bearing_wp_b) > math::radians(80.0f)) { - /* extension, fly back to waypoint */ + /* + * If the AB vector and the vector from B to airplane point in the same + * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. + */ + } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { + /* + * Extension, fly back to waypoint. + * + * This corner case is possible if the system was following + * the AB line from waypoint A to waypoint B, then is + * switched to manual mode (or otherwise misses the waypoint) + * and behind the waypoint continues to follow the AB line. + */ /* calculate eta to fly to waypoint B */ /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit); + xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit); + ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = bearing_wp_b; -#endif + _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + } else { /* calculate eta to fly along the line between A and B */ From 3ca94e7943fc76053114ab97d4b63dc6902fd5bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 13:38:21 +0200 Subject: [PATCH 13/37] Prevent warnings by data conversion --- mavlink/include/mavlink/v1.0/mavlink_conversions.h | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/mavlink/include/mavlink/v1.0/mavlink_conversions.h b/mavlink/include/mavlink/v1.0/mavlink_conversions.h index d893635770..51afac87c3 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_conversions.h +++ b/mavlink/include/mavlink/v1.0/mavlink_conversions.h @@ -9,6 +9,10 @@ #endif #include +#ifndef M_PI_2 + #define M_PI_2 ((float)asin(1)) +#endif + /** * @file mavlink_conversions.h * @@ -49,12 +53,12 @@ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, flo float phi, theta, psi; theta = asin(-dcm[2][0]); - if (fabs(theta - M_PI_2) < 1.0e-3f) { + if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; - psi = (atan2(dcm[1][2] - dcm[0][1], + psi = (atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1]) + phi); - } else if (fabs(theta + M_PI_2) < 1.0e-3f) { + } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; psi = atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1] - phi); @@ -128,4 +132,4 @@ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, flo dcm[2][2] = cosPhi * cosThe; } -#endif \ No newline at end of file +#endif From 0f67c5cbb0f69e5b9dda4e4e75120e63e466e1f8 Mon Sep 17 00:00:00 2001 From: Alexander Lourier Date: Fri, 18 Oct 2013 03:47:15 +0400 Subject: [PATCH 14/37] Parameters list generator --- Tools/px4params/README.md | 9 ++ Tools/px4params/dokuwikiout.py | 27 ++++ Tools/px4params/output.py | 5 + Tools/px4params/parser.py | 220 +++++++++++++++++++++++++++ Tools/px4params/px_process_params.py | 61 ++++++++ Tools/px4params/scanner.py | 34 +++++ Tools/px4params/xmlout.py | 22 +++ src/modules/sensors/sensor_params.c | 26 ++++ 8 files changed, 404 insertions(+) create mode 100644 Tools/px4params/README.md create mode 100644 Tools/px4params/dokuwikiout.py create mode 100644 Tools/px4params/output.py create mode 100644 Tools/px4params/parser.py create mode 100755 Tools/px4params/px_process_params.py create mode 100644 Tools/px4params/scanner.py create mode 100644 Tools/px4params/xmlout.py diff --git a/Tools/px4params/README.md b/Tools/px4params/README.md new file mode 100644 index 0000000000..a23b447995 --- /dev/null +++ b/Tools/px4params/README.md @@ -0,0 +1,9 @@ +h1. PX4 Parameters Processor + +It's designed to scan PX4 source codes, find declarations of tunable parameters, +and generate the list in various formats. + +Currently supported formats are: + +* XML for the parametric UI generator +* Human-readable description in DokuWiki format diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py new file mode 100644 index 0000000000..33f76b415e --- /dev/null +++ b/Tools/px4params/dokuwikiout.py @@ -0,0 +1,27 @@ +import output + +class DokuWikiOutput(output.Output): + def Generate(self, groups): + result = "" + for group in groups: + result += "==== %s ====\n\n" % group.GetName() + for param in group.GetParams(): + code = param.GetFieldValue("code") + name = param.GetFieldValue("short_desc") + if code != name: + name = "%s (%s)" % (name, code) + result += "=== %s ===\n\n" % name + long_desc = param.GetFieldValue("long_desc") + if long_desc is not None: + result += "%s\n\n" % long_desc + min_val = param.GetFieldValue("min") + if min_val is not None: + result += "* Minimal value: %s\n" % min_val + max_val = param.GetFieldValue("max") + if max_val is not None: + result += "* Maximal value: %s\n" % max_val + def_val = param.GetFieldValue("default") + if def_val is not None: + result += "* Default value: %s\n" % def_val + result += "\n" + return result diff --git a/Tools/px4params/output.py b/Tools/px4params/output.py new file mode 100644 index 0000000000..c092468713 --- /dev/null +++ b/Tools/px4params/output.py @@ -0,0 +1,5 @@ +class Output(object): + def Save(self, groups, fn): + data = self.Generate(groups) + with open(fn, 'w') as f: + f.write(data) diff --git a/Tools/px4params/parser.py b/Tools/px4params/parser.py new file mode 100644 index 0000000000..251be672f2 --- /dev/null +++ b/Tools/px4params/parser.py @@ -0,0 +1,220 @@ +import sys +import re + +class ParameterGroup(object): + """ + Single parameter group + """ + def __init__(self, name): + self.name = name + self.params = [] + + def AddParameter(self, param): + """ + Add parameter to the group + """ + self.params.append(param) + + def GetName(self): + """ + Get parameter group name + """ + return self.name + + def GetParams(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.params, + cmp=lambda x, y: cmp(x.GetFieldValue("code"), + y.GetFieldValue("code"))) + +class Parameter(object): + """ + Single parameter + """ + + # Define sorting order of the fields + priority = { + "code": 10, + "type": 9, + "short_desc": 8, + "long_desc": 7, + "default": 6, + "min": 5, + "max": 4, + # all others == 0 (sorted alphabetically) + } + + def __init__(self): + self.fields = {} + + def SetField(self, code, value): + """ + Set named field value + """ + self.fields[code] = value + + def GetFieldCodes(self): + """ + Return list of existing field codes in convenient order + """ + return sorted(self.fields.keys(), + cmp=lambda x, y: cmp(self.priority.get(y, 0), + self.priority.get(x, 0)) or cmp(x, y)) + + def GetFieldValue(self, code): + """ + Return value of the given field code or None if not found. + """ + return self.fields.get(code) + +class Parser(object): + """ + Parses provided data and stores all found parameters internally. + """ + + re_split_lines = re.compile(r'[\r\n]+') + re_comment_start = re.compile(r'^\/\*\*') + re_comment_content = re.compile(r'^\*\s*(.*)') + re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)') + re_comment_end = re.compile(r'(.*?)\s*\*\/') + re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') + re_cut_type_specifier = re.compile(r'[a-z]+$') + re_is_a_number = re.compile(r'^-?[0-9\.]') + re_remove_dots = re.compile(r'\.+$') + + valid_tags = set(["min", "max", "group"]) + + # Order of parameter groups + priority = { + # All other groups = 0 (sort alphabetically) + "Miscellaneous": -10 + } + + def __init__(self): + self.param_groups = {} + + def GetSupportedExtensions(self): + """ + Returns list of supported file extensions that can be parsed by this + parser. + """ + return ["cpp", "c"] + + def Parse(self, contents): + """ + Incrementally parse program contents and append all found parameters + to the list. + """ + # This code is essentially a comment-parsing grammar. "state" + # represents parser state. It contains human-readable state + # names. + state = None + for line in self.re_split_lines.split(contents): + line = line.strip() + # Ignore empty lines + if line == "": + continue + if self.re_comment_start.match(line): + state = "wait-short" + short_desc = None + long_desc = None + tags = {} + elif state is not None and state != "comment-processed": + m = self.re_comment_end.search(line) + if m: + line = m.group(1) + last_comment_line = True + else: + last_comment_line = False + m = self.re_comment_content.match(line) + if m: + comment_content = m.group(1) + if comment_content == "": + # When short comment ends with empty comment line, + # start waiting for the next part - long comment. + if state == "wait-short-end": + state = "wait-long" + else: + m = self.re_comment_tag.match(comment_content) + if m: + tag, desc = m.group(1, 2) + tags[tag] = desc + current_tag = tag + state = "wait-tag-end" + elif state == "wait-short": + # Store first line of the short description + short_desc = comment_content + state = "wait-short-end" + elif state == "wait-short-end": + # Append comment line to the short description + short_desc += "\n" + comment_content + elif state == "wait-long": + # Store first line of the long description + long_desc = comment_content + state = "wait-long-end" + elif state == "wait-long-end": + # Append comment line to the long description + long_desc += "\n" + comment_content + elif state == "wait-tag-end": + # Append comment line to the tag text + tags[current_tag] += "\n" + comment_content + else: + raise AssertionError( + "Invalid parser state: %s" % state) + elif not last_comment_line: + # Invalid comment line (inside comment, but not starting with + # "*" or "*/". Reset parsed content. + state = None + if last_comment_line: + state = "comment-processed" + else: + # Non-empty line outside the comment + m = self.re_parameter_definition.match(line) + if m: + tp, code, defval = m.group(1, 2, 3) + # Remove trailing type specifier from numbers: 0.1f => 0.1 + if self.re_is_a_number.match(defval): + defval = self.re_cut_type_specifier.sub('', defval) + param = Parameter() + param.SetField("code", code) + param.SetField("short_desc", code) + param.SetField("type", tp) + param.SetField("default", defval) + # If comment was found before the parameter declaration, + # inject its data into the newly created parameter. + group = "Miscellaneous" + if state == "comment-processed": + if short_desc is not None: + param.SetField("short_desc", + self.re_remove_dots.sub('', short_desc)) + if long_desc is not None: + param.SetField("long_desc", long_desc) + for tag in tags: + if tag == "group": + group = tags[tag] + elif tag not in self.valid_tags: + sys.stderr.write("Skipping invalid" + "documentation tag: '%s'\n" % tag) + else: + param.SetField(tag, tags[tag]) + # Store the parameter + if group not in self.param_groups: + self.param_groups[group] = ParameterGroup(group) + self.param_groups[group].AddParameter(param) + # Reset parsed comment. + state = None + + def GetParamGroups(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.param_groups.values(), + cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0), + self.priority.get(x.GetName(), 0)) or cmp(x.GetName(), + y.GetName())) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py new file mode 100755 index 0000000000..cdf5aba7c0 --- /dev/null +++ b/Tools/px4params/px_process_params.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# PX4 paramers processor (main executable file) +# +# It scans src/ subdirectory of the project, collects all parameters +# definitions, and outputs list of parameters in XML and DokuWiki formats. +# + +import scanner +import parser +import xmlout +import dokuwikiout + +# Initialize parser +prs = parser.Parser() + +# Scan directories, and parse the files +sc = scanner.Scanner() +sc.ScanDir("../../src", prs) +output = prs.GetParamGroups() + +# Output into XML +out = xmlout.XMLOutput() +out.Save(output, "parameters.xml") + +# Output into DokuWiki +out = dokuwikiout.DokuWikiOutput() +out.Save(output, "parameters.wiki") diff --git a/Tools/px4params/scanner.py b/Tools/px4params/scanner.py new file mode 100644 index 0000000000..b5a1af47c3 --- /dev/null +++ b/Tools/px4params/scanner.py @@ -0,0 +1,34 @@ +import os +import re + +class Scanner(object): + """ + Traverses directory tree, reads all source files, and passes their contents + to the Parser. + """ + + re_file_extension = re.compile(r'\.([^\.]+)$') + + def ScanDir(self, srcdir, parser): + """ + Scans provided path and passes all found contents to the parser using + parser.Parse method. + """ + extensions = set(parser.GetSupportedExtensions()) + for dirname, dirnames, filenames in os.walk(srcdir): + for filename in filenames: + m = self.re_file_extension.search(filename) + if m: + ext = m.group(1) + if ext in extensions: + path = os.path.join(dirname, filename) + self.ScanFile(path, parser) + + def ScanFile(self, path, parser): + """ + Scans provided file and passes its contents to the parser using + parser.Parse method. + """ + with open(path, 'r') as f: + contents = f.read() + parser.Parse(contents) diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py new file mode 100644 index 0000000000..d56802b158 --- /dev/null +++ b/Tools/px4params/xmlout.py @@ -0,0 +1,22 @@ +import output +from xml.dom.minidom import getDOMImplementation + +class XMLOutput(output.Output): + def Generate(self, groups): + impl = getDOMImplementation() + xml_document = impl.createDocument(None, "parameters", None) + xml_parameters = xml_document.documentElement + for group in groups: + xml_group = xml_document.createElement("group") + xml_group.setAttribute("name", group.GetName()) + xml_parameters.appendChild(xml_group) + for param in group.GetParams(): + xml_param = xml_document.createElement("parameter") + xml_group.appendChild(xml_param) + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + xml_field = xml_document.createElement(code) + xml_param.appendChild(xml_field) + xml_value = xml_document.createTextNode(value) + xml_field.appendChild(xml_value) + return xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 4d719e0e15..8875f65fc6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -44,8 +44,34 @@ #include +/** + * Gyro X offset FIXME + * + * This is an X-axis offset for the gyro. + * Adjust it according to the calibration data. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); + +/** + * Gyro Y offset FIXME with dot. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); + +/** + * Gyro Z offset FIXME + * + * @min -5.0 + * @max 5.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); From 6d406968eaba0924fe48945fd8a71b6f8a0adc1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Oct 2013 11:11:15 +0200 Subject: [PATCH 15/37] Added hexrotor support --- ROMFS/px4fmu_common/init.d/12-13_hex | 95 ++++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 14 ++++ 2 files changed, 109 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/12-13_hex diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex new file mode 100644 index 0000000000..0f0bb05ced --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -0,0 +1,95 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on Hex" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ +# 13 = hexarotor +# +param set MAV_TYPE 13 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 900 900 + px4io min 1200 1200 1200 1200 1200 1200 + px4io max 1900 1900 1900 1900 1900 1900 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output $MIXER + +# +# Set PWM output frequency to 400 Hz +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 93e76184d4..ced8750015 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -177,6 +177,20 @@ then sh /etc/init.d/11_dji_f450 set MODE custom fi + + if param compare SYS_AUTOSTART 12 + then + set MIXER /etc/mixers/FMU_hex_x.mix + sh /etc/init.d/12-13_hex + set MODE custom + fi + + if param compare SYS_AUTOSTART 13 + then + set MIXER /etc/mixers/FMU_hex_+.mix + sh /etc/init.d/12-13_hex + set MODE custom + fi if param compare SYS_AUTOSTART 15 then From 233a068a7b6eb4851aa47b80a1852851bc851d73 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 17:54:57 +0200 Subject: [PATCH 16/37] quad hil + rotor configuration startup script --- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 110 ++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 40 ++++--- 2 files changed, 135 insertions(+), 15 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil new file mode 100644 index 0000000000..1c85e04f2a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -0,0 +1,110 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILS quadrotor + starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.0 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.05 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 3.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.05 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.5 + param set MPC_THR_MIN 0.1 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ced8750015..fd588017f9 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -110,23 +110,33 @@ then then sh /etc/init.d/1000_rc_fw.hil set MODE custom - else - if param compare SYS_AUTOSTART 1001 - then - sh /etc/init.d/1001_rc_quad.hil - set MODE custom - else - if param compare SYS_AUTOSTART 1002 - then - sh /etc/init.d/1002_rc_fw_state.hil - set MODE custom - else - # Try to get an USB console - nshterm /dev/ttyACM0 & - fi - fi fi + if param compare SYS_AUTOSTART 1001 + then + sh /etc/init.d/1001_rc_quad.hil + set MODE custom + fi + + if param compare SYS_AUTOSTART 1002 + then + sh /etc/init.d/1002_rc_fw_state.hil + set MODE custom + fi + + if param compare SYS_AUTOSTART 1003 + then + sh /etc/init.d/1003_rc_quad_+.hil + set MODE custom + fi + + if [ $MODE != custom ] + then + # Try to get an USB console + nshterm /dev/ttyACM0 & + fi + + # # Upgrade PX4IO firmware # From 5cd675d8cce6a3c35eab1970cfb8668b0a6b24e5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 19 Oct 2013 11:42:06 +0200 Subject: [PATCH 17/37] The mavlink baudrate was too high in the custom_io_esc startup script --- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc index e645d9d549..2bfaed76cb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -58,7 +58,7 @@ usleep 10000 if px4io detect then # Start MAVLink (depends on orb) - mavlink start -d /dev/ttyS1 -b 115200 + mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 sh /etc/init.d/rc.io @@ -78,7 +78,7 @@ then else fmu mode_pwm # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS1 -b 115200 + mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes From dbb49c035bb85876b234fc057d9f244b43453a44 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Oct 2013 15:19:42 +1100 Subject: [PATCH 18/37] rgbled: fixed detection of device on PX4v1 There is a serial EEPROM on the PX4IOv1 board that answers on I2C address 0x55. We need some extra I2C transfers to ensure we are talking to a real RGBLED device. --- src/drivers/rgbled/rgbled.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index ea87b37d94..34a45c7393 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -172,7 +172,20 @@ RGBLED::probe() bool on, powersave; uint8_t r, g, b; - ret = get(on, powersave, r, g, b); + /** + this may look strange, but is needed. There is a serial + EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to + a bunch of I2C addresses, including the 0x55 used by this + LED device. So we need to do enough operations to be sure + we are talking to the right device. These 3 operations seem + to be enough, as the 3rd one consistently fails if no + RGBLED is on the bus. + */ + if ((ret=get(on, powersave, r, g, b)) != OK || + (ret=send_led_enable(false) != OK) || + (ret=send_led_enable(false) != OK)) { + return ret; + } return ret; } From e3fe4437204b2aecdaa1131fcb4bb0c7751a43df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Oct 2013 15:21:02 +1100 Subject: [PATCH 19/37] rgbled: fixed getopt() handling this allows the -a option to be used, for example rgbled -a 0x55 start --- src/drivers/rgbled/rgbled.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 34a45c7393..d49211b7b8 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -574,7 +574,7 @@ rgbled_main(int argc, char *argv[]) int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) { + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { switch (ch) { case 'a': rgbledadr = strtol(optarg, NULL, 0); @@ -590,7 +590,12 @@ rgbled_main(int argc, char *argv[]) } } - const char *verb = argv[1]; + if (optind >= argc) { + rgbled_usage(); + exit(1); + } + + const char *verb = argv[optind]; int fd; int ret; From 14e2464fabbae27f9655efdb3e5ee55479c469f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Oct 2013 15:21:46 +1100 Subject: [PATCH 20/37] rgbled: don't try the same bus twice on PX4v1 the external I2C bus is the same as the LED bus --- src/drivers/rgbled/rgbled.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index d49211b7b8..727c86e02a 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -616,6 +616,9 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } i2cdevice = PX4_I2C_BUS_LED; } } From 6a624ff753ad9ba6e7fb74783b6b8294a1c17957 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 19 Oct 2013 23:04:36 +0200 Subject: [PATCH 21/37] Fix gyro calibration for rotated sensors. The calibration routine now uses the raw sensor values instead of the already rotated values. --- src/modules/commander/gyro_calibration.cpp | 194 +++++++++------------ 1 file changed, 86 insertions(+), 108 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 369e6da624..b6de5141f6 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -60,17 +60,7 @@ int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); - const unsigned calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - unsigned calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { + struct gyro_scale gyro_scale = { 0.0f, 1.0f, 0.0f, @@ -79,27 +69,40 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, }; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + struct gyro_report gyro_report; + + /* reset all offsets to zero and all scales to one */ + int fd = open(GYRO_DEVICE_PATH, 0); + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to reset scale / offsets for gyro"); close(fd); + + /*** --- OFFSETS --- ***/ + + /* determine gyro mean values */ + const unsigned calibration_count = 5000; + unsigned calibration_counter = 0; unsigned poll_errcount = 0; while (calibration_counter < calibration_count) { /* wait blocking for new data */ struct pollfd fds[1]; - fds[0].fd = sub_sensor_combined; + fds[0].fd = sub_sensor_gyro; fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + gyro_scale.x_offset += gyro_report.x; + gyro_scale.y_offset += gyro_report.y; + gyro_scale.z_offset += gyro_report.z; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); @@ -110,67 +113,49 @@ int do_gyro_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); - close(sub_sensor_combined); + close(sub_sensor_gyro); return ERROR; } } - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; + gyro_scale.x_offset /= calibration_count; + gyro_scale.y_offset /= calibration_count; + gyro_scale.z_offset /= calibration_count; - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warnx("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, "gyro store failed"); - close(sub_sensor_combined); - return ERROR; - } - - tune_neutral(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); - close(sub_sensor_combined); + if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { + mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)"); + close(sub_sensor_gyro); return ERROR; } + /* beep on calibration end */ mavlink_log_info(mavlink_fd, "offset calibration done."); + tune_neutral(); + + /* set offset parameters to new values */ + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!"); + } + -#if 0 /*** --- SCALING --- ***/ +#if 0 + /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); + /* apply new offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to apply new offsets for gyro"); + + close(fd); + + unsigned rotations_count = 30; float gyro_integral = 0.0f; float baseline_integral = 0.0f; @@ -246,52 +231,45 @@ int do_gyro_calibration(int mavlink_fd) warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); -#else - float gyro_scales[] = { 1.0f, 1.0f, 1.0f }; -#endif - - - if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { - - if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0])) - || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1])) - || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - gyro_scales[0], - gyro_offset[1], - gyro_scales[1], - gyro_offset[2], - gyro_scales[2], - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - /* third beep by cal end routine */ - close(sub_sensor_combined); - return OK; - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - close(sub_sensor_combined); + if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { + mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); + close(sub_sensor_gyro); return ERROR; } + + /* beep on calibration end */ + mavlink_log_info(mavlink_fd, "scale calibration done."); + tune_neutral(); + +#endif + + /* set scale parameters to new values */ + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!"); + } + + /* apply new scaling and offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to apply new scale for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warnx("WARNING: auto-save of params to storage failed"); + mavlink_log_critical(mavlink_fd, "gyro store failed"); + close(sub_sensor_gyro); + return ERROR; + } + + close(sub_sensor_gyro); + return OK; } From ef6f1f6f808e49ff3aca68aa08001e37093b89ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Oct 2013 19:36:42 +0200 Subject: [PATCH 22/37] get_rot_matrix() moved to separate library, accel calibration of rotated board fixed --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/lib/conversion/module.mk | 38 ++++++++ src/lib/conversion/rotation.cpp | 30 ++++++ src/lib/conversion/rotation.h | 89 ++++++++++++++++++ .../commander/accelerometer_calibration.cpp | 45 ++++++--- src/modules/commander/module.mk | 1 - src/modules/sensors/sensors.cpp | 93 +------------------ 8 files changed, 192 insertions(+), 106 deletions(-) create mode 100644 src/lib/conversion/module.mk create mode 100644 src/lib/conversion/rotation.cpp create mode 100644 src/lib/conversion/rotation.h diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 46640f3c51..862abde92b 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -115,6 +115,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/conversion # # Demo apps diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bb589cb9f3..71986c3a34 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -111,6 +111,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/conversion # # Demo apps diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk new file mode 100644 index 0000000000..102711aaf7 --- /dev/null +++ b/src/lib/conversion/module.mk @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Rotation library +# + +SRCS = rotation.cpp diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp new file mode 100644 index 0000000000..b65226cf1c --- /dev/null +++ b/src/lib/conversion/rotation.cpp @@ -0,0 +1,30 @@ +/* + * rotation.cpp + * + * Created on: 20.10.2013 + * Author: ton + */ + +#include "math.h" +#include "rotation.h" + +__EXPORT void +get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +{ + /* first set to zero */ + rot_matrix->Matrix::zero(3, 3); + + float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; + float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; + float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; + + math::EulerAngles euler(roll, pitch, yaw); + + math::Dcm R(euler); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + (*rot_matrix)(i, j) = R(i, j); + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h new file mode 100644 index 0000000000..97c6a09076 --- /dev/null +++ b/src/lib/conversion/rotation.h @@ -0,0 +1,89 @@ +/* + * rotation.h + * + * Created on: 20.10.2013 + * Author: ton + */ + +#ifndef ROTATION_H_ +#define ROTATION_H_ + +#include +#include + +/** + * Enum for board and external compass rotations. + * This enum maps from board attitude to airframe attitude. + */ +enum Rotation { + ROTATION_NONE = 0, + ROTATION_YAW_45 = 1, + ROTATION_YAW_90 = 2, + ROTATION_YAW_135 = 3, + ROTATION_YAW_180 = 4, + ROTATION_YAW_225 = 5, + ROTATION_YAW_270 = 6, + ROTATION_YAW_315 = 7, + ROTATION_ROLL_180 = 8, + ROTATION_ROLL_180_YAW_45 = 9, + ROTATION_ROLL_180_YAW_90 = 10, + ROTATION_ROLL_180_YAW_135 = 11, + ROTATION_PITCH_180 = 12, + ROTATION_ROLL_180_YAW_225 = 13, + ROTATION_ROLL_180_YAW_270 = 14, + ROTATION_ROLL_180_YAW_315 = 15, + ROTATION_ROLL_90 = 16, + ROTATION_ROLL_90_YAW_45 = 17, + ROTATION_ROLL_90_YAW_90 = 18, + ROTATION_ROLL_90_YAW_135 = 19, + ROTATION_ROLL_270 = 20, + ROTATION_ROLL_270_YAW_45 = 21, + ROTATION_ROLL_270_YAW_90 = 22, + ROTATION_ROLL_270_YAW_135 = 23, + ROTATION_PITCH_90 = 24, + ROTATION_PITCH_270 = 25, + ROTATION_MAX +}; + +typedef struct { + uint16_t roll; + uint16_t pitch; + uint16_t yaw; +} rot_lookup_t; + +const rot_lookup_t rot_lookup[] = { + { 0, 0, 0 }, + { 0, 0, 45 }, + { 0, 0, 90 }, + { 0, 0, 135 }, + { 0, 0, 180 }, + { 0, 0, 225 }, + { 0, 0, 270 }, + { 0, 0, 315 }, + {180, 0, 0 }, + {180, 0, 45 }, + {180, 0, 90 }, + {180, 0, 135 }, + { 0, 180, 0 }, + {180, 0, 225 }, + {180, 0, 270 }, + {180, 0, 315 }, + { 90, 0, 0 }, + { 90, 0, 45 }, + { 90, 0, 90 }, + { 90, 0, 135 }, + {270, 0, 0 }, + {270, 0, 45 }, + {270, 0, 90 }, + {270, 0, 135 }, + { 0, 90, 0 }, + { 0, 270, 0 } +}; + +/** + * Get the rotation matrix + */ +__EXPORT void +get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + +#endif /* ROTATION_H_ */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index cfa7d9e8a1..c9bfedbda0 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -112,11 +112,13 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -144,24 +146,41 @@ int do_accel_calibration(int mavlink_fd) { int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); if (res == OK) { - /* measurements complete successfully, set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { + /* measurements complete successfully, rotate calibration values */ + param_t board_rotation_h = param_find("SENS_BOARD_ROT"); + enum Rotation board_rotation_id; + param_get(board_rotation_h, &(board_rotation_id)); + math::Matrix board_rotation(3, 3); + get_rot_matrix(board_rotation_id, &board_rotation); + board_rotation = board_rotation.transpose(); + math::Vector3 vect(3); + vect(0) = accel_offs[0]; + vect(1) = accel_offs[1]; + vect(2) = accel_offs[2]; + math::Vector3 accel_offs_rotated = board_rotation * vect; + vect(0) = accel_scale[0]; + vect(1) = accel_scale[1]; + vect(2) = accel_scale[2]; + math::Vector3 accel_scale_rotated = board_rotation * vect; + + /* set parameters */ + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0))) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1))) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { - accel_offs[0], - accel_scale[0], - accel_offs[1], - accel_scale[1], - accel_offs[2], - accel_scale[2], + accel_offs_rotated(0), + accel_scale_rotated(0), + accel_offs_rotated(1), + accel_scale_rotated(1), + accel_offs_rotated(2), + accel_scale_rotated(2), }; if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 91d75121eb..554dfcb080 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -47,4 +47,3 @@ SRCS = commander.cpp \ baro_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp - diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fcacaf8f9..1b79de8fd2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include @@ -135,75 +136,6 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) -/** - * Enum for board and external compass rotations. - * This enum maps from board attitude to airframe attitude. - */ -enum Rotation { - ROTATION_NONE = 0, - ROTATION_YAW_45 = 1, - ROTATION_YAW_90 = 2, - ROTATION_YAW_135 = 3, - ROTATION_YAW_180 = 4, - ROTATION_YAW_225 = 5, - ROTATION_YAW_270 = 6, - ROTATION_YAW_315 = 7, - ROTATION_ROLL_180 = 8, - ROTATION_ROLL_180_YAW_45 = 9, - ROTATION_ROLL_180_YAW_90 = 10, - ROTATION_ROLL_180_YAW_135 = 11, - ROTATION_PITCH_180 = 12, - ROTATION_ROLL_180_YAW_225 = 13, - ROTATION_ROLL_180_YAW_270 = 14, - ROTATION_ROLL_180_YAW_315 = 15, - ROTATION_ROLL_90 = 16, - ROTATION_ROLL_90_YAW_45 = 17, - ROTATION_ROLL_90_YAW_90 = 18, - ROTATION_ROLL_90_YAW_135 = 19, - ROTATION_ROLL_270 = 20, - ROTATION_ROLL_270_YAW_45 = 21, - ROTATION_ROLL_270_YAW_90 = 22, - ROTATION_ROLL_270_YAW_135 = 23, - ROTATION_PITCH_90 = 24, - ROTATION_PITCH_270 = 25, - ROTATION_MAX -}; - -typedef struct { - uint16_t roll; - uint16_t pitch; - uint16_t yaw; -} rot_lookup_t; - -const rot_lookup_t rot_lookup[] = { - { 0, 0, 0 }, - { 0, 0, 45 }, - { 0, 0, 90 }, - { 0, 0, 135 }, - { 0, 0, 180 }, - { 0, 0, 225 }, - { 0, 0, 270 }, - { 0, 0, 315 }, - {180, 0, 0 }, - {180, 0, 45 }, - {180, 0, 90 }, - {180, 0, 135 }, - { 0, 180, 0 }, - {180, 0, 225 }, - {180, 0, 270 }, - {180, 0, 315 }, - { 90, 0, 0 }, - { 90, 0, 45 }, - { 90, 0, 90 }, - { 90, 0, 135 }, - {270, 0, 0 }, - {270, 0, 45 }, - {270, 0, 90 }, - {270, 0, 135 }, - { 0, 90, 0 }, - { 0, 270, 0 } -}; - /** * Sensor app start / stop handling function * @@ -384,11 +316,6 @@ private: */ int parameters_update(); - /** - * Get the rotation matrices - */ - void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); - /** * Do accel-related initialisation. */ @@ -803,24 +730,6 @@ Sensors::parameters_update() return OK; } -void -Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) -{ - /* first set to zero */ - rot_matrix->Matrix::zero(3, 3); - - float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; - float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; - float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; - - math::EulerAngles euler(roll, pitch, yaw); - - math::Dcm R(euler); - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - (*rot_matrix)(i, j) = R(i, j); -} - void Sensors::accel_init() { From b75c8e672fb401d9b1673d53a1972b9dddfa6b15 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Oct 2013 23:16:23 +0200 Subject: [PATCH 23/37] accelerometer calibration fix --- .../commander/accelerometer_calibration.cpp | 45 +++++++------------ 1 file changed, 17 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index c9bfedbda0..b6aa64aa4d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -129,7 +129,7 @@ #endif static const int ERROR = -1; -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); int mat_invert3(float src[3][3], float dst[3][3]); @@ -142,8 +142,8 @@ int do_accel_calibration(int mavlink_fd) { /* measure and calculate offsets & scales */ float accel_offs[3]; - float accel_scale[3]; - int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); + float accel_T[3][3]; + int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); if (res == OK) { /* measurements complete successfully, rotate calibration values */ @@ -153,34 +153,31 @@ int do_accel_calibration(int mavlink_fd) { math::Matrix board_rotation(3, 3); get_rot_matrix(board_rotation_id, &board_rotation); board_rotation = board_rotation.transpose(); - math::Vector3 vect(3); - vect(0) = accel_offs[0]; - vect(1) = accel_offs[1]; - vect(2) = accel_offs[2]; - math::Vector3 accel_offs_rotated = board_rotation * vect; - vect(0) = accel_scale[0]; - vect(1) = accel_scale[1]; - vect(2) = accel_scale[2]; - math::Vector3 accel_scale_rotated = board_rotation * vect; + math::Vector3 accel_offs_vec; + accel_offs_vec.set(&accel_offs[0]); + math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec; + math::Matrix accel_T_mat(3, 3); + accel_T_mat.set(&accel_T[0][0]); + math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation; /* set parameters */ if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0))) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1))) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) { + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { accel_offs_rotated(0), - accel_scale_rotated(0), + accel_T_rotated(0, 0), accel_offs_rotated(1), - accel_scale_rotated(1), + accel_T_rotated(1, 1), accel_offs_rotated(2), - accel_scale_rotated(2), + accel_T_rotated(2, 2), }; if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) @@ -206,7 +203,7 @@ int do_accel_calibration(int mavlink_fd) { /* exit accel calibration mode */ } -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) { const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; @@ -282,21 +279,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } close(sensor_combined_sub); - /* calculate offsets and rotation+scale matrix */ - float accel_T[3][3]; + /* calculate offsets and transform matrix */ int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); return ERROR; } - /* convert accel transform matrix to scales, - * rotation part of transform matrix is not used by now - */ - for (int i = 0; i < 3; i++) { - accel_scale[i] = accel_T[i][i]; - } - return OK; } From 0dc9c9ac262c10866cbaf23ca98c8ce4582b5993 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Oct 2013 23:28:09 +0200 Subject: [PATCH 24/37] accelerometer_calibration: code style fixed, lib/conversion copyright fix --- src/lib/conversion/module.mk | 2 +- src/lib/conversion/rotation.cpp | 40 +++++- src/lib/conversion/rotation.h | 40 +++++- .../commander/accelerometer_calibration.cpp | 122 +++++++++++------- 4 files changed, 149 insertions(+), 55 deletions(-) diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk index 102711aaf7..f5f59a2dca 100644 --- a/src/lib/conversion/module.mk +++ b/src/lib/conversion/module.mk @@ -32,7 +32,7 @@ ############################################################################ # -# Rotation library +# Conversion library # SRCS = rotation.cpp diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index b65226cf1c..b078562c2d 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -1,8 +1,40 @@ -/* - * rotation.cpp +/**************************************************************************** * - * Created on: 20.10.2013 - * Author: ton + * 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 rotation.cpp + * + * Vector rotation library */ #include "math.h" diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 97c6a09076..85c63c0fcb 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -1,8 +1,40 @@ -/* - * rotation.h +/**************************************************************************** * - * Created on: 20.10.2013 - * Author: ton + * 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 rotation.h + * + * Vector rotation library */ #ifndef ROTATION_H_ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index b6aa64aa4d..4880af9071 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -135,7 +135,8 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -int do_accel_calibration(int mavlink_fd) { +int do_accel_calibration(int mavlink_fd) +{ /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); @@ -162,11 +163,11 @@ int do_accel_calibration(int mavlink_fd) { /* set parameters */ if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } @@ -194,6 +195,7 @@ int do_accel_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "accel calibration done"); return OK; + } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); @@ -203,7 +205,8 @@ int do_accel_calibration(int mavlink_fd) { /* exit accel calibration mode */ } -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) +{ const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; @@ -243,12 +246,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", - (!data_collected[0]) ? "x+ " : "", - (!data_collected[1]) ? "x- " : "", - (!data_collected[2]) ? "y+ " : "", - (!data_collected[3]) ? "y- " : "", - (!data_collected[4]) ? "z+ " : "", - (!data_collected[5]) ? "z- " : ""); + (!data_collected[0]) ? "x+ " : "", + (!data_collected[1]) ? "x- " : "", + (!data_collected[2]) ? "y+ " : "", + (!data_collected[3]) ? "y- " : "", + (!data_collected[4]) ? "z+ " : "", + (!data_collected[5]) ? "z- " : ""); if (old_done_count != done_count) mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); @@ -257,6 +260,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) { close(sensor_combined_sub); return ERROR; @@ -270,17 +274,19 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], - (double)accel_ref[orient][0], - (double)accel_ref[orient][1], - (double)accel_ref[orient][2]); + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); data_collected[orient] = true; tune_neutral(); } + close(sensor_combined_sub); /* calculate offsets and transform matrix */ int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res != 0) { mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); return ERROR; @@ -295,7 +301,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float * @return 0..5 according to orientation when vehicle is still and ready for measurements, * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 */ -int detect_orientation(int mavlink_fd, int sub_sensor_combined) { +int detect_orientation(int mavlink_fd, int sub_sensor_combined) +{ struct sensor_combined_s sensor; /* exponential moving average of accel */ float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; @@ -326,30 +333,35 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { while (true) { /* wait blocking for new data */ int poll_ret = poll(fds, 1, 1000); + if (poll_ret) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); t = hrt_absolute_time(); float dt = (t - t_prev) / 1000000.0f; t_prev = t; float w = dt / ema_len; + for (int i = 0; i < 3; i++) { accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); + if (d > accel_disp[i]) accel_disp[i] = d; } + /* still detector with hysteresis */ - if ( accel_disp[0] < still_thr2 && - accel_disp[1] < still_thr2 && - accel_disp[2] < still_thr2 ) { + if (accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2) { /* is still now */ if (t_still == 0) { /* first time */ mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); t_still = t; t_timeout = t + timeout; + } else { /* still since t_still */ if (t > t_still + still_time) { @@ -357,18 +369,21 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { break; } } - } else if ( accel_disp[0] > still_thr2 * 2.0f || - accel_disp[1] > still_thr2 * 2.0f || - accel_disp[2] > still_thr2 * 2.0f) { + + } else if (accel_disp[0] > still_thr2 * 2.0f || + accel_disp[1] > still_thr2 * 2.0f || + accel_disp[2] > still_thr2 * 2.0f) { /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); t_still = 0; } } + } else if (poll_ret == 0) { poll_errcount++; } + if (t > t_timeout) { poll_errcount++; } @@ -379,29 +394,34 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } - if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 0; // [ g, 0, 0 ] - if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 1; // [ -g, 0, 0 ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 2; // [ 0, g, 0 ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 3; // [ 0, -g, 0 ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) return 4; // [ 0, 0, g ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); @@ -412,7 +432,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) +{ struct pollfd fds[1]; fds[0].fd = sensor_combined_sub; fds[0].events = POLLIN; @@ -423,12 +444,16 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp while (count < samples_num) { int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + for (int i = 0; i < 3; i++) accel_sum[i] += sensor.accelerometer_m_s2[i]; + count++; + } else { errcount++; continue; @@ -445,10 +470,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp return OK; } -int mat_invert3(float src[3][3], float dst[3][3]) { +int mat_invert3(float src[3][3], float dst[3][3]) +{ float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + - src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + if (det == 0.0f) return ERROR; // Singular matrix @@ -465,7 +492,8 @@ int mat_invert3(float src[3][3], float dst[3][3]) { return OK; } -int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) +{ /* calculate offsets */ for (int i = 0; i < 3; i++) { accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; @@ -474,6 +502,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* fill matrix A for linear equations system*/ float mat_A[3][3]; memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { float a = accel_ref[i * 2][j] - accel_offs[j]; @@ -483,6 +512,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* calculate inverse matrix for A */ float mat_A_inv[3][3]; + if (mat_invert3(mat_A, mat_A_inv) != OK) return ERROR; From ed79b686c57f41d9d6bd3726fe0071e11740b3d7 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 20 Oct 2013 00:08:36 +0200 Subject: [PATCH 25/37] Adjusted mavlink info messages during gyro calibration to not break QGroundControl. --- src/modules/commander/gyro_calibration.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index b6de5141f6..e1d6e8340d 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -129,7 +129,7 @@ int do_gyro_calibration(int mavlink_fd) } /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "offset calibration done."); + mavlink_log_info(mavlink_fd, "gyro offset calibration done."); tune_neutral(); /* set offset parameters to new values */ @@ -240,7 +240,7 @@ int do_gyro_calibration(int mavlink_fd) } /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "scale calibration done."); + mavlink_log_info(mavlink_fd, "gyro scale calibration done."); tune_neutral(); #endif @@ -270,6 +270,8 @@ int do_gyro_calibration(int mavlink_fd) return ERROR; } + mavlink_log_info(mavlink_fd, "gyro calibration done."); + close(sub_sensor_gyro); return OK; } From ea89f23c917733f8a682c82e24e1e4f223f79843 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Oct 2013 20:07:47 +0200 Subject: [PATCH 26/37] calibration: bugs fixed, mavlink messages cleanup --- src/drivers/drv_accel.h | 2 +- .../commander/accelerometer_calibration.cpp | 201 +++++++++------- src/modules/commander/commander.cpp | 54 +++-- src/modules/commander/gyro_calibration.cpp | 219 ++++++++++-------- 4 files changed, 266 insertions(+), 210 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index eff5e73495..8a4f684284 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -66,7 +66,7 @@ struct accel_report { int16_t temperature_raw; }; -/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ +/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ struct accel_scale { float x_offset; float x_scale; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 4880af9071..d11d7eb5d3 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -100,6 +100,24 @@ * accel_T = A^-1 * g * g = 9.80665 * + * ===== Rotation ===== + * + * Calibrating using model: + * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r) + * + * Actual correction: + * accel_corr = rot * accel_T * (accel_raw - accel_offs) + * + * Known: accel_T_r, accel_offs_r, rot + * Unknown: accel_T, accel_offs + * + * Solution: + * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs) + * => accel_T = rot^-1 * accel_T_r * rot + * => accel_offs = rot^-1 * accel_offs_r + * * @author Anton Babushkin */ @@ -137,72 +155,97 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { - /* announce change */ - mavlink_log_info(mavlink_fd, "accel calibration started"); - mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); + mavlink_log_info(mavlink_fd, "accel calibration: started"); + mavlink_log_info(mavlink_fd, "accel calibration: progress <0>"); + + struct accel_scale accel_scale = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + int res = OK; + + /* reset all offsets to zero and all scales to one */ + int fd = open(ACCEL_DEVICE_PATH, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + } /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_T[3][3]; - int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); if (res == OK) { - /* measurements complete successfully, rotate calibration values */ + /* measurements completed successfully, rotate calibration values */ param_t board_rotation_h = param_find("SENS_BOARD_ROT"); - enum Rotation board_rotation_id; - param_get(board_rotation_h, &(board_rotation_id)); + int32_t board_rotation_int; + param_get(board_rotation_h, &(board_rotation_int)); + enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; math::Matrix board_rotation(3, 3); get_rot_matrix(board_rotation_id, &board_rotation); - board_rotation = board_rotation.transpose(); + math::Matrix board_rotation_t = board_rotation.transpose(); math::Vector3 accel_offs_vec; accel_offs_vec.set(&accel_offs[0]); - math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec; + math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; math::Matrix accel_T_mat(3, 3); accel_T_mat.set(&accel_T[0][0]); - math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation; + math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + + accel_scale.x_offset = accel_offs_rotated(0); + accel_scale.x_scale = accel_T_rotated(0, 0); + accel_scale.y_offset = accel_offs_rotated(1); + accel_scale.y_scale = accel_T_rotated(1, 1); + accel_scale.z_offset = accel_offs_rotated(2); + accel_scale.z_scale = accel_T_rotated(2, 2); /* set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset)) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset)) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset)) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting accel params failed"); + res = ERROR; } - - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offs_rotated(0), - accel_T_rotated(0, 0), - accel_offs_rotated(1), - accel_T_rotated(1, 1), - accel_offs_rotated(2), - accel_T_rotated(2, 2), - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - mavlink_log_info(mavlink_fd, "accel calibration done"); - return OK; - - } else { - /* measurements error */ - mavlink_log_info(mavlink_fd, "accel calibration aborted"); - return ERROR; } - /* exit accel calibration mode */ + if (res == OK) { + /* apply new scaling and offsets */ + int fd = open(ACCEL_DEVICE_PATH, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for accel"); + } + } + + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + } + } + + if (res == OK) { + mavlink_log_info(mavlink_fd, "accel calibration: done"); + + } else { + mavlink_log_info(mavlink_fd, "accel calibration: failed"); + } + + return res; } int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) @@ -212,27 +255,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; - /* reset existing calibration */ - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); - close(fd); - - if (OK != ioctl_res) { - warn("ERROR: failed to set scale / offsets for accel"); - return ERROR; - } - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); unsigned done_count = 0; + int res = OK; while (true) { bool done = true; @@ -245,6 +271,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } } + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel calibration: progress <%u>", 17 * done_count); + + if (done) + break; + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", (!data_collected[1]) ? "x- " : "", @@ -253,17 +285,11 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float (!data_collected[4]) ? "z+ " : "", (!data_collected[5]) ? "z- " : ""); - if (old_done_count != done_count) - mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); - - if (done) - break; - int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { - close(sensor_combined_sub); - return ERROR; + res = ERROR; + break; } if (data_collected[orient]) { @@ -284,15 +310,16 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float close(sensor_combined_sub); - /* calculate offsets and transform matrix */ - int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res == OK) { + /* calculate offsets and transform matrix */ + res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); - return ERROR; + if (res != OK) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); + } } - return OK; + return res; } /* @@ -309,7 +336,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* max-hold dispersion of accel */ float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; /* EMA time constant in seconds*/ - float ema_len = 0.2f; + float ema_len = 0.5f; /* set "still" threshold to 0.25 m/s^2 */ float still_thr2 = pow(0.25f, 2); /* set accel error threshold to 5m/s^2 */ @@ -342,8 +369,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) float w = dt / ema_len; for (int i = 0; i < 3; i++) { - accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; - float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; + float d = sensor.accelerometer_m_s2[i] - accel_ema[i]; + accel_ema[i] += d * w; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); @@ -389,8 +416,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); - return -1; + mavlink_log_critical(mavlink_fd, "ERROR: failed reading sensor"); + return ERROR; } } @@ -424,9 +451,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) return 5; // [ 0, 0, -g ] - mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); - return -2; // Can't detect orientation + return ERROR; // Can't detect orientation } /* diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ef509980c..9545ef171d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,8 +369,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (hil_ret == OK && control_mode->flag_system_hil_enabled) { /* reset the arming mode to disarmed */ arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + } else { mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } @@ -481,27 +483,28 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: - { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; - if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off) { - print_reject_arm("NOT ARMING: Press safety switch first."); - arming_res = TRANSITION_DENIED; + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + transition_result_t arming_res = TRANSITION_NOT_CHANGED; - } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); - } + if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + } + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } } - } break; default: @@ -940,7 +943,7 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - struct stat statbuf; + //struct stat statbuf; //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } @@ -970,6 +973,7 @@ int commander_thread_main(int argc, char *argv[]) if (armed.armed) { arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + } else { arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } @@ -1244,12 +1248,14 @@ int commander_thread_main(int argc, char *argv[]) counter++; int blink_state = blink_msg_state(); + if (blink_state > 0) { /* blinking LED message, don't touch LEDs */ if (blink_state == 2) { /* blinking LED message completed, restore normal state */ control_status_leds(&status, &armed, true); } + } else { /* normal state */ control_status_leds(&status, &armed, status_changed); @@ -1264,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[]) ret = pthread_join(commander_low_prio_thread, NULL); if (ret) { - warn("join failed", ret); + warn("join failed: %d", ret); } rgbled_set_mode(RGBLED_MODE_OFF); @@ -1308,6 +1314,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan /* driving rgbled */ if (changed) { bool set_normal_color = false; + /* set mode */ if (status->arming_state == ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); @@ -1332,6 +1339,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { rgbled_set_color(RGBLED_COLOR_AMBER); } + /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { @@ -1694,11 +1702,10 @@ void *commander_low_prio_loop(void *arg) fds[0].events = POLLIN; while (!thread_should_exit) { - - /* wait for up to 100ms for data */ + /* wait for up to 200ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); - /* timed out - periodic check for _task_should_exit, etc. */ + /* timed out - periodic check for thread_should_exit, etc. */ if (pret == 0) continue; @@ -1773,7 +1780,7 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_rc_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { @@ -1854,7 +1861,6 @@ void *commander_low_prio_loop(void *arg) /* send acknowledge command */ // XXX TODO } - } close(cmd_sub); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index e1d6e8340d..219ae6cb2f 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -58,7 +58,7 @@ static const int ERROR = -1; int do_gyro_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); + mavlink_log_info(mavlink_fd, "gyro calibration: started"); struct gyro_scale gyro_scale = { 0.0f, @@ -69,79 +69,85 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, }; + int res = OK; + + /* reset all offsets to zero and all scales to one */ + int fd = open(GYRO_DEVICE_PATH, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + } + /* subscribe to gyro sensor topic */ int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); struct gyro_report gyro_report; - /* reset all offsets to zero and all scales to one */ - int fd = open(GYRO_DEVICE_PATH, 0); + if (res == OK) { + /* determine gyro mean values */ + const unsigned calibration_count = 5000; + unsigned calibration_counter = 0; + unsigned poll_errcount = 0; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) - warn("WARNING: failed to reset scale / offsets for gyro"); + while (calibration_counter < calibration_count) { + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_gyro; + fds[0].events = POLLIN; - close(fd); + int poll_ret = poll(fds, 1, 1000); + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + gyro_scale.x_offset += gyro_report.x; + gyro_scale.y_offset += gyro_report.y; + gyro_scale.z_offset += gyro_report.z; + calibration_counter++; - /*** --- OFFSETS --- ***/ + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, "gyro calibration: progress <%u>", (calibration_counter * 100) / calibration_count); - /* determine gyro mean values */ - const unsigned calibration_count = 5000; - unsigned calibration_counter = 0; - unsigned poll_errcount = 0; + } else { + poll_errcount++; + } - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_sensor_gyro; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); - gyro_scale.x_offset += gyro_report.x; - gyro_scale.y_offset += gyro_report.y; - gyro_scale.z_offset += gyro_report.z; - calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) - mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); - - } else { - poll_errcount++; + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, "ERROR: failed reading gyro sensor"); + res = ERROR; + break; + } } - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); - close(sub_sensor_gyro); - return ERROR; + gyro_scale.x_offset /= calibration_count; + gyro_scale.y_offset /= calibration_count; + gyro_scale.z_offset /= calibration_count; + } + + if (res == OK) { + /* check offsets */ + if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { + mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + res = ERROR; } } - gyro_scale.x_offset /= calibration_count; - gyro_scale.y_offset /= calibration_count; - gyro_scale.z_offset /= calibration_count; - - if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { - mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)"); - close(sub_sensor_gyro); - return ERROR; + if (res == OK) { + /* set offset parameters to new values */ + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting gyro offs params failed"); + res = ERROR; + } } - /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro offset calibration done."); +#if 0 + /* beep on offset calibration end */ + mavlink_log_info(mavlink_fd, "gyro offset calibration done"); tune_neutral(); - /* set offset parameters to new values */ - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!"); - } - - - /*** --- SCALING --- ***/ -#if 0 + /* scale calibration */ /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); @@ -163,9 +169,11 @@ int do_gyro_calibration(int mavlink_fd) // XXX change to mag topic orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; - if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; + float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); + + if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F; + + if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F; uint64_t last_time = hrt_absolute_time(); @@ -175,7 +183,7 @@ int do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) { + && (hrt_absolute_time() - start_time > 5 * 1e6)) { mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); close(sub_sensor_combined); return OK; @@ -203,14 +211,17 @@ int do_gyro_calibration(int mavlink_fd) // calculate error between estimate and measurement // apply declination correction for true heading as well. //float mag = -atan2f(magNav(1),magNav(0)); - float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag > M_PI_F) mag -= 2*M_PI_F; - if (mag < -M_PI_F) mag += 2*M_PI_F; + float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); + + if (mag > M_PI_F) mag -= 2 * M_PI_F; + + if (mag < -M_PI_F) mag += 2 * M_PI_F; float diff = mag - mag_last; - if (diff > M_PI_F) diff -= 2*M_PI_F; - if (diff < -M_PI_F) diff += 2*M_PI_F; + if (diff > M_PI_F) diff -= 2 * M_PI_F; + + if (diff < -M_PI_F) diff += 2 * M_PI_F; baseline_integral += diff; mag_last = mag; @@ -220,15 +231,15 @@ int do_gyro_calibration(int mavlink_fd) // warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); - // } else if (poll_ret == 0) { - // /* any poll failure for 1s is a reason to abort */ - // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - // return; + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; } } float gyro_scale = baseline_integral / gyro_integral; - + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); @@ -236,42 +247,54 @@ int do_gyro_calibration(int mavlink_fd) if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); close(sub_sensor_gyro); + mavlink_log_critical(mavlink_fd, "gyro calibration failed"); return ERROR; } /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro scale calibration done."); + mavlink_log_info(mavlink_fd, "gyro scale calibration done"); tune_neutral(); #endif - /* set scale parameters to new values */ - if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) - || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) - || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!"); - } - - /* apply new scaling and offsets */ - fd = open(GYRO_DEVICE_PATH, 0); - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) - warn("WARNING: failed to apply new scale for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warnx("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, "gyro store failed"); - close(sub_sensor_gyro); - return ERROR; - } - - mavlink_log_info(mavlink_fd, "gyro calibration done."); - close(sub_sensor_gyro); - return OK; + + if (res == OK) { + /* set scale parameters to new values */ + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting gyro scale params failed"); + res = ERROR; + } + } + + if (res == OK) { + /* apply new scaling and offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro"); + } + } + + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + } + } + + if (res == OK) { + mavlink_log_info(mavlink_fd, "gyro calibration: done"); + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration: failed"); + } + + return res; } From ef42ef15c6991800111b25374b0f6e3935c2a9a9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Oct 2013 22:24:59 +0200 Subject: [PATCH 27/37] accel/gyro/mag calibration: big cleanup, use common messages --- .../commander/accelerometer_calibration.cpp | 40 ++- src/modules/commander/calibration_messages.h | 57 ++++ src/modules/commander/gyro_calibration.cpp | 37 ++- src/modules/commander/mag_calibration.cpp | 302 +++++++++--------- 4 files changed, 252 insertions(+), 184 deletions(-) create mode 100644 src/modules/commander/calibration_messages.h diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d11d7eb5d3..49a8d6b339 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -122,6 +122,7 @@ */ #include "accelerometer_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include @@ -147,6 +148,8 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "accel"; + int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); @@ -155,8 +158,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "accel calibration: started"); - mavlink_log_info(mavlink_fd, "accel calibration: progress <0>"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); struct accel_scale accel_scale = { 0.0f, @@ -175,7 +177,7 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } /* measure and calculate offsets & scales */ @@ -213,7 +215,7 @@ int do_accel_calibration(int mavlink_fd) || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting accel params failed"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); res = ERROR; } } @@ -225,7 +227,7 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for accel"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } @@ -234,15 +236,15 @@ int do_accel_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } } if (res == OK) { - mavlink_log_info(mavlink_fd, "accel calibration: done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, "accel calibration: failed"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } return res; @@ -266,13 +268,16 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float done_count = 0; for (int i = 0; i < 6; i++) { - if (!data_collected[i]) { + if (data_collected[i]) { + done_count++; + + } else { done = false; } } if (old_done_count != done_count) - mavlink_log_info(mavlink_fd, "accel calibration: progress <%u>", 17 * done_count); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); if (done) break; @@ -293,7 +298,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); continue; } @@ -374,6 +379,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); + if (d > still_thr2 * 8.0f) + d = still_thr2 * 8.0f; + if (d > accel_disp[i]) accel_disp[i] = d; } @@ -397,12 +405,12 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) } } - } else if (accel_disp[0] > still_thr2 * 2.0f || - accel_disp[1] > still_thr2 * 2.0f || - accel_disp[2] > still_thr2 * 2.0f) { + } else if (accel_disp[0] > still_thr2 * 4.0f || + accel_disp[1] > still_thr2 * 4.0f || + accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); + mavlink_log_info(mavlink_fd, "detected motion, hold still..."); t_still = 0; } } @@ -416,7 +424,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) } if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: failed reading sensor"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); return ERROR; } } diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h new file mode 100644 index 0000000000..fd8b8564df --- /dev/null +++ b/src/modules/commander/calibration_messages.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 calibration_messages.h + * + * Common calibration messages. + * + * @author Anton Babushkin + */ + +#ifndef CALIBRATION_MESSAGES_H_ +#define CALIBRATION_MESSAGES_H_ + +#define CAL_STARTED_MSG "%s calibration: started" +#define CAL_DONE_MSG "%s calibration: done" +#define CAL_FAILED_MSG "%s calibration: failed" +#define CAL_PROGRESS_MSG "%s calibration: progress <%u>" + +#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" +#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration" +#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration" +#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters" +#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters" + +#endif /* CALIBRATION_MESSAGES_H_ */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 219ae6cb2f..30cd0d48d4 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -33,10 +33,12 @@ /** * @file gyro_calibration.cpp + * * Gyroscope calibration routine */ #include "gyro_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include @@ -56,9 +58,12 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "gyro"; + int do_gyro_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "gyro calibration: started"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); struct gyro_scale gyro_scale = { 0.0f, @@ -77,19 +82,19 @@ int do_gyro_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } - /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); - struct gyro_report gyro_report; - if (res == OK) { /* determine gyro mean values */ const unsigned calibration_count = 5000; unsigned calibration_counter = 0; unsigned poll_errcount = 0; + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + struct gyro_report gyro_report; + while (calibration_counter < calibration_count) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -106,19 +111,21 @@ int do_gyro_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) - mavlink_log_info(mavlink_fd, "gyro calibration: progress <%u>", (calibration_counter * 100) / calibration_count); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); } else { poll_errcount++; } if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: failed reading gyro sensor"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); res = ERROR; break; } } + close(sub_sensor_gyro); + gyro_scale.x_offset /= calibration_count; gyro_scale.y_offset /= calibration_count; gyro_scale.z_offset /= calibration_count; @@ -137,7 +144,7 @@ int do_gyro_calibration(int mavlink_fd) if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting gyro offs params failed"); + mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; } } @@ -257,14 +264,12 @@ int do_gyro_calibration(int mavlink_fd) #endif - close(sub_sensor_gyro); - if (res == OK) { /* set scale parameters to new values */ if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting gyro scale params failed"); + mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } } @@ -276,7 +281,7 @@ int do_gyro_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } @@ -285,15 +290,15 @@ int do_gyro_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } } if (res == OK) { - mavlink_log_info(mavlink_fd, "gyro calibration: done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, "gyro calibration: failed"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } return res; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index b0d4266be5..09f4104f89 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -33,12 +33,14 @@ /** * @file mag_calibration.cpp + * * Magnetometer calibration routine */ #include "mag_calibration.h" #include "commander_helper.h" #include "calibration_routines.h" +#include "calibration_messages.h" #include #include @@ -59,26 +61,20 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "mag"; + int do_mag_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait."); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); /* 45 seconds */ uint64_t calibration_interval = 45 * 1000 * 1000; - /* maximum 2000 values */ + /* maximum 500 values */ const unsigned int calibration_maxcount = 500; unsigned int calibration_counter = 0; - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ struct mag_scale mscale_null = { 0.0f, 1.0f, @@ -88,97 +84,92 @@ int do_mag_calibration(int mavlink_fd) 1.0f, }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + int res = OK; + + /* erase old calibration */ + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); + if (res == OK) { + /* calibrate range */ + res = ioctl(fd, MAGIOCCALIBRATE, fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale"); + } } close(fd); - mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + float *x; + float *y; + float *z; - /* calibrate offsets */ + if (res == OK) { + /* allocate memory */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - // uint64_t calibration_start = hrt_absolute_time(); + x = (float *)malloc(sizeof(float) * calibration_maxcount); + y = (float *)malloc(sizeof(float) * calibration_maxcount); + z = (float *)malloc(sizeof(float) * calibration_maxcount); - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return ERROR; + if (x == NULL || y == NULL || z == NULL) { + mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); + res = ERROR; + } } - mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting.."); + if (res == OK) { + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; - unsigned poll_errcount = 0; + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + /* calibrate offsets */ + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + unsigned poll_errcount = 0; - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; + mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { - axis_index++; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; - mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); - tune_neutral(); + int poll_ret = poll(fds, 1, 1000); - axis_deadline += calibration_interval / 3; + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + calibration_counter++; + + if (calibration_counter % (calibration_maxcount / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } } - if (!(axis_index < 3)) { - break; - } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - calibration_counter++; - if (calibration_counter % (calibration_maxcount / 20) == 0) - mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount); - - - } else { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); - close(sub_mag); - free(x); - free(y); - free(z); - return ERROR; - } - - + close(sub_mag); } float sphere_x; @@ -186,93 +177,100 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; - mavlink_log_info(mavlink_fd, "mag cal progress <70> percent"); - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - mavlink_log_info(mavlink_fd, "mag cal progress <80> percent"); + if (res == OK) { + /* sphere fit */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); - free(x); - free(y); - free(z); + if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) { + mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); + res = ERROR; + } + } - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + if (x != NULL) + free(x); - fd = open(MAG_DEVICE_PATH, 0); + if (y != NULL) + free(y); + if (z != NULL) + free(z); + + if (res == OK) { + /* apply calibration and set parameters */ struct mag_scale mscale; - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); + fd = open(MAG_DEVICE_PATH, 0); + res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale); - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); + } - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); + if (res == OK) { + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } + } close(fd); - /* announce and set new offset */ + if (res == OK) { + /* set parameters */ + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) + res = ERROR; - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) + res = ERROR; + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + } + + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90); } - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + } } - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); + mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + + } else { + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - close(sub_mag); - return ERROR; - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); - mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); - - close(sub_mag); - return OK; - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - close(sub_mag); - return ERROR; + return res; } } From 495073935e428d99833135bb227f3085d2def1cf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Oct 2013 23:33:01 +0200 Subject: [PATCH 28/37] accelerometer_calibration: stability fix --- src/modules/commander/accelerometer_calibration.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 49a8d6b339..5eeca5a1a3 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -180,10 +180,13 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } - /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_T[3][3]; - res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + + if (res == OK) { + /* measure and calculate offsets & scales */ + res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + } if (res == OK) { /* measurements completed successfully, rotate calibration values */ From 7f0ced968e5d518776930296ed870a47cc8c1756 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 21 Oct 2013 21:28:26 -0400 Subject: [PATCH 29/37] Working on roboclaw driver. --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/roboclaw/RoboClaw.cpp | 250 +++++++++++++++++++++++++ src/drivers/roboclaw/RoboClaw.hpp | 187 ++++++++++++++++++ src/drivers/roboclaw/module.mk | 41 ++++ src/drivers/roboclaw/roboclaw_main.cpp | 181 ++++++++++++++++++ 6 files changed, 661 insertions(+), 1 deletion(-) create mode 100644 src/drivers/roboclaw/RoboClaw.cpp create mode 100644 src/drivers/roboclaw/RoboClaw.hpp create mode 100644 src/drivers/roboclaw/module.mk create mode 100644 src/drivers/roboclaw/roboclaw_main.cpp diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 46640f3c51..3cccc8447a 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -33,7 +33,7 @@ MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl -MODULES += drivers/md25 +MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bb589cb9f3..a2027ded9b 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -31,6 +31,7 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm +MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp new file mode 100644 index 0000000000..88b22e72aa --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -0,0 +1,250 @@ +/**************************************************************************** + * + * 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 RoboClaw.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include "RoboClaw.hpp" +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +uint8_t RoboClaw::checksum_mask = 0x7f; + +RoboClaw::RoboClaw(const char *deviceName, uint16_t address): + _address(address), + _uart(0), + _controlPoll(), + _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _motor1Position(0), + _motor1Speed(0), + _motor2Position(0), + _motor2Speed(0) +{ + // setup control polling + _controlPoll.fd = _actuators.getHandle(); + _controlPoll.events = POLLIN; + + // start serial port + _uart = open(deviceName, O_RDWR | O_NOCTTY); + struct termios uart_config; + tcgetattr(_uart, &uart_config); + uart_config.c_oflag &= ~ONLCR; // no CR for every LF + cfsetispeed(&uart_config, B38400); + cfsetospeed(&uart_config, B38400); + tcsetattr(_uart, TCSANOW, &uart_config); + + // setup default settings, reset encoders + resetEncoders(); +} + +RoboClaw::~RoboClaw() +{ + setMotorDutyCycle(MOTOR_1, 0.0); + setMotorDutyCycle(MOTOR_2, 0.0); + close(_uart); +} + +int RoboClaw::readEncoder(e_motor motor) +{ + uint16_t sum = 0; + if (motor == MOTOR_1) { + _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); + } + uint8_t rbuf[6]; + int ret = read(_uart, rbuf, 6); + uint32_t count = 0; + uint8_t * countBytes = (uint8_t *)(&count); + countBytes[3] = rbuf[0]; + countBytes[2] = rbuf[1]; + countBytes[1] = rbuf[2]; + countBytes[0] = rbuf[3]; + uint8_t status = rbuf[4]; + if ((sum + _sumBytes(rbuf,5)) & + checksum_mask == rbuf[5]) return -1; + int overFlow = 0; + if (status & STATUS_UNDERFLOW) { + overFlow = -1; + } else if (status & STATUS_OVERFLOW) { + overFlow = +1; + } + if (motor == MOTOR_1) { + _motor1Overflow += overFlow; + } else if (motor == MOTOR_2) { + _motor2Overflow += overFlow; + } + return ret; +} + +void RoboClaw::status(char *string, size_t n) +{ + snprintf(string, n, + "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n" + "motor 2 position:\t%10.2f\tspeed:\t%10.2f\n", + double(getMotorPosition(MOTOR_1)), + double(getMotorSpeed(MOTOR_1)), + double(getMotorPosition(MOTOR_2)), + double(getMotorSpeed(MOTOR_2))); +} + +float RoboClaw::getMotorPosition(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Position; + } else if (motor == MOTOR_2) { + return _motor2Position; + } +} + +float RoboClaw::getMotorSpeed(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Speed; + } else if (motor == MOTOR_2) { + return _motor2Speed; + } +} + +int RoboClaw::setMotorSpeed(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + uint8_t speed = fabs(value)*127; + // send command + if (motor == MOTOR_1) { + if (value > 0) { + _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + } else { + _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + } + } else if (motor == MOTOR_2) { + if (value > 0) { + _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + } else { + _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + } + } +} + +int RoboClaw::setMotorDutyCycle(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + int16_t duty = 1500*value; + // send command + if (motor == MOTOR_1) { + _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + (uint8_t *)(&duty), 2, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + (uint8_t *)(&duty), 2, sum); + } +} + +void RoboClaw::update() +{ + // wait for an actuator publication, + // check for exit condition every second + // note "::poll" is required to distinguish global + // poll from member function for driver + if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error + + // if new data, send to motors + if (_actuators.updated()) { + _actuators.update(); + setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + } +} + +uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) +{ + uint16_t sum = 0; + for (size_t i=0;i +#include +#include +#include +#include + +/** + * This is a driver for the RoboClaw motor controller + */ +class RoboClaw +{ +public: + + /** control channels */ + enum e_channel { + CH_VOLTAGE_LEFT = 0, + CH_VOLTAGE_RIGHT + }; + + /** motors */ + enum e_motor { + MOTOR_1 = 0, + MOTOR_2 + }; + + /** + * constructor + * @param deviceName the name of the + * serial port e.g. "/dev/ttyS2" + * @param address the adddress of the motor + * (selectable on roboclaw) + */ + RoboClaw(const char *deviceName, uint16_t address); + + /** + * deconstructor + */ + virtual ~RoboClaw(); + + /** + * @return position of a motor, rev + */ + float getMotorPosition(e_motor motor); + + /** + * @return speed of a motor, rev/sec + */ + float getMotorSpeed(e_motor motor); + + /** + * set the speed of a motor, rev/sec + */ + int setMotorSpeed(e_motor motor, float value); + + /** + * set the duty cycle of a motor, rev/sec + */ + int setMotorDutyCycle(e_motor motor, float value); + + /** + * reset the encoders + * @return status + */ + int resetEncoders(); + + /** + * main update loop that updates RoboClaw motor + * dutycycle based on actuator publication + */ + void update(); + + /** + * read data from serial + */ + int readEncoder(e_motor motor); + + /** + * print status + */ + void status(char *string, size_t n); + +private: + + // Quadrature status flags + enum e_quadrature_status_flags { + STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/ + STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/ + STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/ + }; + + // commands + // We just list the commands we want from the manual here. + enum e_command { + + // simple + CMD_DRIVE_FWD_1 = 0, + CMD_DRIVE_REV_1 = 1, + CMD_DRIVE_FWD_2 = 4, + CMD_DRIVE_REV_2 = 5, + + // encoder commands + CMD_READ_ENCODER_1 = 16, + CMD_READ_ENCODER_2 = 17, + CMD_RESET_ENCODERS = 20, + + // advanced motor control + CMD_READ_SPEED_HIRES_1 = 30, + CMD_READ_SPEED_HIRES_2 = 31, + CMD_SIGNED_DUTYCYCLE_1 = 32, + CMD_SIGNED_DUTYCYCLE_2 = 33, + }; + + static uint8_t checksum_mask; + + uint16_t _address; + + int _uart; + + /** poll structure for control packets */ + struct pollfd _controlPoll; + + /** actuator controls subscription */ + control::UOrbSubscription _actuators; + + // private data + float _motor1Position; + float _motor1Speed; + int16_t _motor1Overflow; + + float _motor2Position; + float _motor2Speed; + int16_t _motor2Overflow; + + // private methods + uint16_t _sumBytes(uint8_t * buf, size_t n); + int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum); +}; + +// unit testing +int roboclawTest(const char *deviceName, uint8_t address); + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk new file mode 100644 index 0000000000..1abecf198b --- /dev/null +++ b/src/drivers/roboclaw/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# RoboClaw Motor Controller +# + +MODULE_COMMAND = roboclaw + +SRCS = roboclaw_main.cpp \ + RoboClaw.cpp diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp new file mode 100644 index 0000000000..a61c646fcf --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * 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 roboclaw_main.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include "RoboClaw.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int roboclaw_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(); + +static void usage() +{ + fprintf(stderr, "usage: roboclaw " + "{start|stop|status|test}\n\n"); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int roboclaw_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage(); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("roboclaw already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn_cmd("roboclaw", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + roboclaw_thread_main, + (const char **)argv); + exit(0); + + } else if (!strcmp(argv[1], "test")) { + + if (argc < 4) { + printf("usage: roboclaw test device address\n"); + exit(-1); + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + + roboclawTest(deviceName, address); + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "stop")) { + + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "status")) { + + if (thread_running) { + printf("\troboclaw app is running\n"); + + } else { + printf("\troboclaw app not started\n"); + } + exit(0); + } + + usage(); + exit(1); +} + +int roboclaw_thread_main(int argc, char *argv[]) +{ + printf("[roboclaw] starting\n"); + + // skip parent process args + argc -=2; + argv +=2; + + if (argc < 3) { + printf("usage: roboclaw start device address\n"); + return -1; + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + + // start + RoboClaw roboclaw(deviceName, address); + + thread_running = true; + + // loop + while (!thread_should_exit) { + roboclaw.update(); + } + + // exit + printf("[roboclaw] exiting.\n"); + thread_running = false; + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 From ce68f93867abfd3ea528809144f7c045d9bce544 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 21 Oct 2013 23:40:36 -0400 Subject: [PATCH 30/37] Debugging roboclaw comm. --- src/drivers/roboclaw/RoboClaw.cpp | 52 ++++++++++++++++++++++---- src/drivers/roboclaw/RoboClaw.hpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 8 +++- 3 files changed, 52 insertions(+), 10 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 88b22e72aa..aecc511aed 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -75,12 +75,18 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address): // start serial port _uart = open(deviceName, O_RDWR | O_NOCTTY); + if (_uart < 0) err(1, "could not open %s", deviceName); + int ret = 0; struct termios uart_config; - tcgetattr(_uart, &uart_config); + ret = tcgetattr(_uart, &uart_config); + if (ret < 0) err (1, "failed to get attr"); uart_config.c_oflag &= ~ONLCR; // no CR for every LF - cfsetispeed(&uart_config, B38400); - cfsetospeed(&uart_config, B38400); - tcsetattr(_uart, TCSANOW, &uart_config); + ret = cfsetispeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set input speed"); + ret = cfsetospeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set output speed"); + ret = tcsetattr(_uart, TCSANOW, &uart_config); + if (ret < 0) err (1, "failed to set attr"); // setup default settings, reset encoders resetEncoders(); @@ -110,23 +116,30 @@ int RoboClaw::readEncoder(e_motor motor) countBytes[1] = rbuf[2]; countBytes[0] = rbuf[3]; uint8_t status = rbuf[4]; - if ((sum + _sumBytes(rbuf,5)) & - checksum_mask == rbuf[5]) return -1; + if (((sum + _sumBytes(rbuf,5)) & checksum_mask) + == rbuf[5]) return -1; int overFlow = 0; if (status & STATUS_UNDERFLOW) { + printf("roboclaw: underflow\n"); overFlow = -1; } else if (status & STATUS_OVERFLOW) { + printf("roboclaw: overflow\n"); overFlow = +1; } + static int64_t overflowAmount = 0x100000000LL; if (motor == MOTOR_1) { _motor1Overflow += overFlow; + _motor1Position = count + + _motor1Overflow*_motor1Position; } else if (motor == MOTOR_2) { _motor2Overflow += overFlow; + _motor2Position = count + + _motor2Overflow*_motor2Position; } return ret; } -void RoboClaw::status(char *string, size_t n) +void RoboClaw::printStatus(char *string, size_t n) { snprintf(string, n, "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n" @@ -195,6 +208,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) } } +int RoboClaw::resetEncoders() +{ + uint16_t sum = 0; + return _sendCommand(CMD_RESET_ENCODERS, + nullptr, 0, sum); +} + void RoboClaw::update() { // wait for an actuator publication, @@ -214,9 +234,13 @@ void RoboClaw::update() uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) { uint16_t sum = 0; + printf("sum\n"); for (size_t i=0;i Date: Tue, 22 Oct 2013 05:04:13 -0400 Subject: [PATCH 31/37] Roboclaw encoders/ dutycycledrive complete. --- ROMFS/px4fmu_common/init.d/40_io_segway | 16 +-- ROMFS/px4fmu_common/init.d/rcS | 6 ++ src/drivers/roboclaw/RoboClaw.cpp | 131 ++++++++++++++++-------- src/drivers/roboclaw/RoboClaw.hpp | 11 +- src/drivers/roboclaw/roboclaw_main.cpp | 28 +++-- 5 files changed, 130 insertions(+), 62 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 4b7ed52869..ffe7f695b1 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -21,8 +21,8 @@ param set MAV_TYPE 10 # # Start MAVLink (depends on orb) # -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 +#mavlink start -d /dev/ttyS1 -b 57600 +#usleep 5000 # # Start and configure PX4IO interface @@ -32,25 +32,25 @@ sh /etc/init.d/rc.io # # Start the commander (depends on orb, mavlink) # -commander start +#commander start # # Start the sensors (depends on orb, px4io) # -sh /etc/init.d/rc.sensors +#sh /etc/init.d/rc.sensors # # Start GPS interface (depends on orb) # -gps start +#gps start # # Start the attitude estimator (depends on orb) # -attitude_estimator_ekf start +#attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -md25 start 3 0x58 -segway start +roboclaw test /dev/ttyS2 128 +#segway start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index fd588017f9..3a458286ed 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -272,6 +272,12 @@ then sh /etc/init.d/30_io_camflyer set MODE custom fi + + if param compare SYS_AUTOSTART 40 + then + sh /etc/init.d/40_io_segway + set MODE custom + fi if param compare SYS_AUTOSTART 31 then diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index aecc511aed..3bbd7f48f5 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -59,15 +59,19 @@ uint8_t RoboClaw::checksum_mask = 0x7f; -RoboClaw::RoboClaw(const char *deviceName, uint16_t address): +RoboClaw::RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev): _address(address), + _pulsesPerRev(pulsesPerRev), _uart(0), _controlPoll(), _actuators(NULL, ORB_ID(actuator_controls_0), 20), _motor1Position(0), _motor1Speed(0), + _motor1Overflow(0), _motor2Position(0), - _motor2Speed(0) + _motor2Speed(0), + _motor2Overflow(0) { // setup control polling _controlPoll.fd = _actuators.getHandle(); @@ -90,6 +94,13 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address): // setup default settings, reset encoders resetEncoders(); + + // XXX roboclaw gives 128 as first several csums + // have to read a couple of messages first + for (int i=0;i<5;i++) { + if (readEncoder(MOTOR_1) > 0) break; + usleep(3000); + } } RoboClaw::~RoboClaw() @@ -107,8 +118,18 @@ int RoboClaw::readEncoder(e_motor motor) } else if (motor == MOTOR_2) { _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); } - uint8_t rbuf[6]; - int ret = read(_uart, rbuf, 6); + uint8_t rbuf[50]; + usleep(5000); + int nread = read(_uart, rbuf, 50); + if (nread < 6) { + printf("failed to read\n"); + return -1; + } + //printf("received: \n"); + //for (int i=0;i 0) { - _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); } else { - _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); } } else if (motor == MOTOR_2) { if (value > 0) { - _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); } else { - _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); } } + return -1; } int RoboClaw::setMotorDutyCycle(e_motor motor, float value) @@ -200,12 +234,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) int16_t duty = 1500*value; // send command if (motor == MOTOR_1) { - _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, (uint8_t *)(&duty), 2, sum); } else if (motor == MOTOR_2) { - _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, (uint8_t *)(&duty), 2, sum); } + return -1; } int RoboClaw::resetEncoders() @@ -215,13 +250,13 @@ int RoboClaw::resetEncoders() nullptr, 0, sum); } -void RoboClaw::update() +int RoboClaw::update() { // wait for an actuator publication, // check for exit condition every second // note "::poll" is required to distinguish global // poll from member function for driver - if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error + if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error // if new data, send to motors if (_actuators.updated()) { @@ -229,56 +264,68 @@ void RoboClaw::update() setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); } + return 0; } uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) { uint16_t sum = 0; - printf("sum\n"); + //printf("sum\n"); for (size_t i=0;i Date: Tue, 22 Oct 2013 05:08:20 -0400 Subject: [PATCH 32/37] Updated script. --- ROMFS/px4fmu_common/init.d/40_io_segway | 16 ++++++++-------- ROMFS/px4fmu_common/init.d/rcS | 12 ++++++------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index ffe7f695b1..fb9dec0437 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -21,8 +21,8 @@ param set MAV_TYPE 10 # # Start MAVLink (depends on orb) # -#mavlink start -d /dev/ttyS1 -b 57600 -#usleep 5000 +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 # # Start and configure PX4IO interface @@ -32,25 +32,25 @@ sh /etc/init.d/rc.io # # Start the commander (depends on orb, mavlink) # -#commander start +commander start # # Start the sensors (depends on orb, px4io) # -#sh /etc/init.d/rc.sensors +sh /etc/init.d/rc.sensors # # Start GPS interface (depends on orb) # -#gps start +gps start # # Start the attitude estimator (depends on orb) # -#attitude_estimator_ekf start +attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -roboclaw test /dev/ttyS2 128 -#segway start +roboclaw start /dev/ttyS2 128 1200 +segway start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3a458286ed..424787d54a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -273,18 +273,18 @@ then set MODE custom fi - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi - if param compare SYS_AUTOSTART 31 then sh /etc/init.d/31_io_phantom set MODE custom fi + if param compare SYS_AUTOSTART 40 + then + sh /etc/init.d/40_io_segway + set MODE custom + fi + if param compare SYS_AUTOSTART 100 then sh /etc/init.d/100_mpx_easystar From 108d723a4902086f883136c7de6a75d65256500b Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:10:26 -0400 Subject: [PATCH 33/37] Removed old timing hack. --- src/drivers/roboclaw/RoboClaw.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 3bbd7f48f5..73bd5dadac 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -94,13 +94,6 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, // setup default settings, reset encoders resetEncoders(); - - // XXX roboclaw gives 128 as first several csums - // have to read a couple of messages first - for (int i=0;i<5;i++) { - if (readEncoder(MOTOR_1) > 0) break; - usleep(3000); - } } RoboClaw::~RoboClaw() From d143e827dcd774548bca6d6b4cb6fb69ef35a826 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:43:10 -0400 Subject: [PATCH 34/37] Updated segway controller for new state machine. --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- src/modules/segway/BlockSegwayController.cpp | 13 +++++++------ 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 3cccc8447a..85ac3f546f 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX needs state machine update +MODULES += modules/segway MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a2027ded9b..c0972be9eb 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -78,7 +78,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX needs state machine update +MODULES += modules/segway MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index b1dc39445f..96a443c6ea 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -26,7 +26,7 @@ void BlockSegwayController::update() { _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.main_state == MAIN_STATE_AUTO) { // update guidance } @@ -34,17 +34,18 @@ void BlockSegwayController::update() { float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed); // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.main_state == MAIN_STATE_AUTO || + _status.main_state == MAIN_STATE_SEATBELT || + _status.main_state == MAIN_STATE_EASY) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else if (_status.main_state == MAIN_STATE_MANUAL) { + if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { _actuators.control[CH_LEFT] = _manual.throttle; _actuators.control[CH_RIGHT] = _manual.pitch; - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; } From c4a1a338ff4378c908ec34c5a5f408c1b96d0735 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:43:27 -0400 Subject: [PATCH 35/37] Changed driver to control motor duty cycle. --- src/drivers/roboclaw/RoboClaw.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 73bd5dadac..d65a9be361 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -254,8 +254,8 @@ int RoboClaw::update() // if new data, send to motors if (_actuators.updated()) { _actuators.update(); - setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); - setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); } return 0; } From 28b4e978534e164d08125a9b0cf1fe428d9ad122 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 22 Oct 2013 21:01:30 +0200 Subject: [PATCH 36/37] Fixed bug with fd leak in rc_calibration_check --- src/modules/commander/commander.cpp | 4 ++-- src/modules/systemlib/rc_check.c | 4 +--- src/modules/systemlib/rc_check.h | 2 +- src/systemcmds/preflight_check/preflight_check.c | 4 ++-- 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ef509980c..db758c3865 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -687,7 +687,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check()); + bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -802,7 +802,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check()); + rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 60d6473b8c..b4350cc243 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -47,14 +47,12 @@ #include #include -int rc_calibration_check(void) { +int rc_calibration_check(int mavlink_fd) { char nbuf[20]; param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, _parameter_handles_rev, _parameter_handles_dz; - int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - float param_min, param_max, param_trim, param_rev, param_dz; /* first check channel mappings */ diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index e2238d1516..e70b83ccee 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -47,6 +47,6 @@ * @return 0 / OK if RC calibration is ok, index + 1 of the first * channel that failed else (so 1 == first channel failed) */ -__EXPORT int rc_calibration_check(void); +__EXPORT int rc_calibration_check(int mavlink_fd); __END_DECLS diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e9c5f1a2cf..1c58a2db69 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -140,7 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - bool rc_ok = (OK == rc_calibration_check()); + bool rc_ok = (OK == rc_calibration_check(mavlink_fd)); /* warn */ if (!rc_ok) @@ -227,4 +227,4 @@ static int led_off(int leds, int led) static int led_on(int leds, int led) { return ioctl(leds, LED_ON, led); -} \ No newline at end of file +} From 2f66a8894f1f8035bfc076306aa0d83197be108a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 22 Oct 2013 21:02:29 +0200 Subject: [PATCH 37/37] param_save_default() rewritten: don't try 10 times to do every operation but do it safe using temp file --- src/modules/systemlib/param/param.c | 95 ++++++++++++++--------------- 1 file changed, 47 insertions(+), 48 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index ccdb2ea38b..398657dd79 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -508,64 +508,63 @@ param_get_default_file(void) int param_save_default(void) { - int result; - unsigned retries = 0; - - /* delete the file in case it exists */ - struct stat buffer; - if (stat(param_get_default_file(), &buffer) == 0) { - - do { - result = unlink(param_get_default_file()); - if (result != 0) { - retries++; - usleep(1000 * retries); - } - } while (result != OK && retries < 10); - - if (result != OK) - warnx("unlinking file %s failed.", param_get_default_file()); - } - - /* create the file */ + int res; int fd; - do { - /* do another attempt in case the unlink call is not synced yet */ - fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + const char *filename = param_get_default_file(); + const char *filename_tmp = malloc(strlen(filename) + 5); + sprintf(filename_tmp, "%s.tmp", filename); + + /* delete temp file if exist */ + res = unlink(filename_tmp); + + if (res != OK && errno == ENOENT) + res = OK; + + if (res != OK) + warn("failed to delete temp file: %s", filename_tmp); + + if (res == OK) { + /* write parameters to temp file */ + fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL); + if (fd < 0) { - retries++; - usleep(1000 * retries); + warn("failed to open temp file: %s", filename_tmp); + res = ERROR; } - } while (fd < 0 && retries < 10); + if (res == OK) { + res = param_export(fd, false); - if (fd < 0) { - - warn("opening '%s' for writing failed", param_get_default_file()); - return fd; - } - - do { - result = param_export(fd, false); - - if (result != OK) { - retries++; - usleep(1000 * retries); + if (res != OK) + warnx("failed to write parameters to file: %s", filename_tmp); } - } while (result != 0 && retries < 10); - - - close(fd); - - if (result != OK) { - warn("error exporting parameters to '%s'", param_get_default_file()); - (void)unlink(param_get_default_file()); - return result; + close(fd); } - return 0; + if (res == OK) { + /* delete parameters file */ + res = unlink(filename); + + if (res != OK && errno == ENOENT) + res = OK; + + if (res != OK) + warn("failed to delete parameters file: %s", filename); + } + + if (res == OK) { + /* rename temp file to parameters */ + res = rename(filename_tmp, filename); + + if (res != OK) + warn("failed to rename %s to %s", filename_tmp, filename); + } + + free(filename_tmp); + + return res; } /**