From 63136e354330bcf8efe2757187515eeaa8bc3bcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 00:11:16 +0200 Subject: [PATCH] Resurrected C++ change commit, now back up to same state as master --- src/modules/attitude_estimator_ekf/Makefile | 57 +++++++++++++++++ ...main.c => attitude_estimator_ekf_main.cpp} | 18 ++++-- src/modules/attitude_estimator_ekf/module.mk | 63 +++++++++++++++---- 3 files changed, 120 insertions(+), 18 deletions(-) create mode 100755 src/modules/attitude_estimator_ekf/Makefile rename src/modules/attitude_estimator_ekf/{attitude_estimator_ekf_main.c => attitude_estimator_ekf_main.cpp} (97%) diff --git a/src/modules/attitude_estimator_ekf/Makefile b/src/modules/attitude_estimator_ekf/Makefile new file mode 100755 index 0000000000..46a54c6607 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/Makefile @@ -0,0 +1,57 @@ +############################################################################ +# +# 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. +# +############################################################################ + +APPNAME = attitude_estimator_ekf +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +CXXSRCS = attitude_estimator_ekf_main.cpp + +CSRCS = attitude_estimator_ekf_params.c \ + codegen/eye.c \ + codegen/attitudeKalmanfilter.c \ + codegen/mrdivide.c \ + codegen/rdivide.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/norm.c \ + codegen/cross.c + + +# XXX this is *horribly* broken +INCLUDES += $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp similarity index 97% rename from src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c rename to src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 9fc4dfc838..1a50dde0f4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -65,11 +66,17 @@ #include #include +#ifdef __cplusplus +extern "C" { +#endif #include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter.h" #include "attitude_estimator_ekf_params.h" +#ifdef __cplusplus +} +#endif -__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); +extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -264,10 +271,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* Main loop*/ while (!thread_should_exit) { - struct pollfd fds[2] = { - { .fd = sub_raw, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN } - }; + struct pollfd fds[2]; + fds[0].fd = sub_raw; + fds[0].events = POLLIN; + fds[1].fd = sub_params; + fds[1].events = POLLIN; int ret = poll(fds, 2, 1000); if (ret < 0) { diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 4033617c4b..3034018aa2 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -1,16 +1,53 @@ +############################################################################ +# +# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Attitude estimator (Extended Kalman Filter) +# MODULE_NAME = attitude_estimator_ekf -SRCS = attitude_estimator_ekf_main.c \ - attitude_estimator_ekf_params.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/attitudeKalmanfilter.c \ - codegen/cross.c \ - codegen/eye.c \ - codegen/mrdivide.c \ - codegen/norm.c \ - codegen/rdivide.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c +CXXSRCS = attitude_estimator_ekf_main.cpp + +SRCS = attitude_estimator_ekf_params.c \ + codegen/eye.c \ + codegen/attitudeKalmanfilter.c \ + codegen/mrdivide.c \ + codegen/rdivide.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/norm.c \ + codegen/cross.c