diff options
Diffstat (limited to 'src/lib/launchdetection')
| -rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.cpp | 98 | ||||
| -rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.h | 75 | ||||
| -rw-r--r-- | src/lib/launchdetection/LaunchDetector.cpp | 93 | ||||
| -rw-r--r-- | src/lib/launchdetection/LaunchDetector.h | 79 | ||||
| -rw-r--r-- | src/lib/launchdetection/LaunchMethod.h | 60 | ||||
| -rw-r--r-- | src/lib/launchdetection/launchdetection_params.c | 89 | ||||
| -rw-r--r-- | src/lib/launchdetection/module.mk | 42 |
7 files changed, 536 insertions, 0 deletions
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp new file mode 100644 index 000000000..c555a0a69 --- /dev/null +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 CatapultLaunchMethod.cpp + * Catapult Launch detection + * + * @author Thomas Gubler <thomasgubler@gmail.com> + * + */ + +#include "CatapultLaunchMethod.h" +#include <systemlib/err.h> + +namespace launchdetection +{ + +CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : + SuperBlock(parent, "CAT"), + last_timestamp(hrt_absolute_time()), + integrator(0.0f), + launchDetected(false), + threshold_accel(this, "A"), + threshold_time(this, "T") +{ + +} + +CatapultLaunchMethod::~CatapultLaunchMethod() { + +} + +void CatapultLaunchMethod::update(float accel_x) +{ + float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f; + last_timestamp = hrt_absolute_time(); + + if (accel_x > threshold_accel.get()) { + integrator += accel_x * dt; +// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", +// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); + if (integrator > threshold_accel.get() * threshold_time.get()) { + launchDetected = true; + } + + } else { +// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", +// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); + /* reset integrator */ + integrator = 0.0f; + launchDetected = false; + } + +} + +bool CatapultLaunchMethod::getLaunchDetected() +{ + return launchDetected; +} + + +void CatapultLaunchMethod::reset() +{ + integrator = 0.0f; + launchDetected = false; +} + +} diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h new file mode 100644 index 000000000..23757f6f3 --- /dev/null +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 CatapultLaunchMethod.h + * Catpult Launch detection + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#ifndef CATAPULTLAUNCHMETHOD_H_ +#define CATAPULTLAUNCHMETHOD_H_ + +#include "LaunchMethod.h" + +#include <drivers/drv_hrt.h> +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +namespace launchdetection +{ + +class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock +{ +public: + CatapultLaunchMethod(SuperBlock *parent); + ~CatapultLaunchMethod(); + + void update(float accel_x); + bool getLaunchDetected(); + void reset(); + +private: + hrt_abstime last_timestamp; + float integrator; + bool launchDetected; + + control::BlockParamFloat threshold_accel; + control::BlockParamFloat threshold_time; + +}; + +#endif /* CATAPULTLAUNCHMETHOD_H_ */ + +} diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp new file mode 100644 index 000000000..3bf47bbb0 --- /dev/null +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 launchDetection.cpp + * Auto Detection for different launch methods (e.g. catapult) + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include "LaunchDetector.h" +#include "CatapultLaunchMethod.h" +#include <systemlib/err.h> + +namespace launchdetection +{ + +LaunchDetector::LaunchDetector() : + SuperBlock(NULL, "LAUN"), + launchdetection_on(this, "ALL_ON"), + throttlePreTakeoff(this, "THR_PRE") +{ + /* init all detectors */ + launchMethods[0] = new CatapultLaunchMethod(this); + + + /* update all parameters of all detectors */ + updateParams(); +} + +LaunchDetector::~LaunchDetector() +{ + +} + +void LaunchDetector::reset() +{ + /* Reset all detectors */ + launchMethods[0]->reset(); +} + +void LaunchDetector::update(float accel_x) +{ + if (launchdetection_on.get() == 1) { + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + launchMethods[i]->update(accel_x); + } + } +} + +bool LaunchDetector::getLaunchDetected() +{ + if (launchdetection_on.get() == 1) { + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + if(launchMethods[i]->getLaunchDetected()) { + return true; + } + } + } + + return false; +} + +} diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h new file mode 100644 index 000000000..1a214b66e --- /dev/null +++ b/src/lib/launchdetection/LaunchDetector.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 LaunchDetector.h + * Auto Detection for different launch methods (e.g. catapult) + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#ifndef LAUNCHDETECTOR_H +#define LAUNCHDETECTOR_H + +#include <stdbool.h> +#include <stdint.h> + +#include "LaunchMethod.h" +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +namespace launchdetection +{ + +class __EXPORT LaunchDetector : public control::SuperBlock +{ +public: + LaunchDetector(); + ~LaunchDetector(); + void reset(); + + void update(float accel_x); + bool getLaunchDetected(); + bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; + + float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } + +// virtual bool getLaunchDetected(); +protected: +private: + LaunchMethod* launchMethods[1]; + control::BlockParamInt launchdetection_on; + control::BlockParamFloat throttlePreTakeoff; + + +}; + +} + +#endif // LAUNCHDETECTOR_H diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h new file mode 100644 index 000000000..e04467f6a --- /dev/null +++ b/src/lib/launchdetection/LaunchMethod.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 LaunchMethod.h + * Base class for different launch methods + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#ifndef LAUNCHMETHOD_H_ +#define LAUNCHMETHOD_H_ + +namespace launchdetection +{ + +class LaunchMethod +{ +public: + virtual void update(float accel_x) = 0; + virtual bool getLaunchDetected() = 0; + virtual void reset() = 0; + +protected: +private: +}; + +} + +#endif /* LAUNCHMETHOD_H_ */ diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c new file mode 100644 index 000000000..8df8c696c --- /dev/null +++ b/src/lib/launchdetection/launchdetection_params.c @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 launchdetection_params.c + * + * Parameters for launchdetection + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * Catapult launch detection parameters, accessible via MAVLink + * + */ + +/** + * Enable launch detection. + * + * @min 0 + * @max 1 + * @group Launch detection + */ +PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); + +/** + * Catapult accelerometer theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); + +/** + * Catapult time theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); + +/** + * Throttle setting while detecting launch. + * + * The throttle is set to this value while the system is waiting for the take-off. + * + * @min 0 + * @max 1 + * @group Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f); diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk new file mode 100644 index 000000000..d92f0bb45 --- /dev/null +++ b/src/lib/launchdetection/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013, 2014 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. +# +############################################################################ + +# +# Launchdetection Library +# + +SRCS = LaunchDetector.cpp \ + CatapultLaunchMethod.cpp \ + launchdetection_params.c + +MAXOPTIMIZATION = -Os |