diff --git a/src/modules/fw_pos_control_l1/launchdetection/CMakeLists.txt b/src/modules/fw_pos_control_l1/launchdetection/CMakeLists.txt index d7be8b54b2..1f7d85d290 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/launchdetection/CMakeLists.txt @@ -33,5 +33,4 @@ px4_add_library(launchdetection LaunchDetector.cpp - CatapultLaunchMethod.cpp -) \ No newline at end of file +) diff --git a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp deleted file mode 100644 index 4c058032c3..0000000000 --- a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp +++ /dev/null @@ -1,112 +0,0 @@ -/**************************************************************************** - * - * 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 - * - */ - -#include "CatapultLaunchMethod.h" - -#include - -namespace launchdetection -{ - -CatapultLaunchMethod::CatapultLaunchMethod(ModuleParams *parent) : - ModuleParams(parent) -{ -} - -void CatapultLaunchMethod::update(const float dt, float accel_x) -{ - switch (state) { - case LAUNCHDETECTION_RES_NONE: - - /* Detect a acceleration that is longer and stronger as the minimum given by the params */ - if (accel_x > _param_laun_cat_a.get()) { - _integrator += dt; - - if (_integrator > _param_laun_cat_t.get()) { - if (_param_laun_cat_mdel.get() > 0.0f) { - state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; - PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle", - double(_param_laun_cat_mdel.get())); - - } else { - /* No motor delay set: go directly to enablemotors state */ - state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - PX4_WARN("Launch detected: enablemotors (delay not activated)"); - } - } - - } else { - reset(); - } - - break; - - case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL: - /* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is - * over to allow full throttle */ - _motorDelayCounter += dt; - - if (_motorDelayCounter > _param_laun_cat_mdel.get()) { - PX4_INFO("Launch detected: state enablemotors"); - state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - } - - break; - - default: - break; - - } -} - -LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const -{ - return state; -} - -void CatapultLaunchMethod::reset() -{ - _integrator = 0.0f; - _motorDelayCounter = 0.0f; - state = LAUNCHDETECTION_RES_NONE; -} - -} // namespace launchdetection diff --git a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h b/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h deleted file mode 100644 index a4b2abe831..0000000000 --- a/src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h +++ /dev/null @@ -1,78 +0,0 @@ -/**************************************************************************** - * - * 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 - */ - -#ifndef CATAPULTLAUNCHMETHOD_H_ -#define CATAPULTLAUNCHMETHOD_H_ - -#include "LaunchMethod.h" - -#include -#include - -namespace launchdetection -{ - -class CatapultLaunchMethod : public LaunchMethod, public ModuleParams -{ -public: - CatapultLaunchMethod(ModuleParams *parent); - ~CatapultLaunchMethod() override = default; - - void update(const float dt, float accel_x) override; - LaunchDetectionResult getLaunchDetected() const override; - void reset() override; - -private: - float _integrator{0.0f}; - float _motorDelayCounter{0.0f}; - - LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_laun_cat_a, - (ParamFloat) _param_laun_cat_t, - (ParamFloat) _param_laun_cat_mdel - ) - -}; - -#endif /* CATAPULTLAUNCHMETHOD_H_ */ - -} // namespace launchdetection diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp index e016dd4d7f..30609c1b4f 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 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 @@ -32,12 +32,11 @@ ****************************************************************************/ /** * @file launchDetection.cpp - * Auto Detection for different launch methods (e.g. catapult) + * Auto launch detection for catapult/hand-launch vehicles * * @author Thomas Gubler */ -#include "CatapultLaunchMethod.h" #include "LaunchDetector.h" #include @@ -45,57 +44,63 @@ namespace launchdetection { -LaunchDetector::LaunchDetector(ModuleParams *parent) : - ModuleParams(parent) -{ - /* init all detectors */ - _launchMethods[0] = new CatapultLaunchMethod(this); -} - -LaunchDetector::~LaunchDetector() -{ - delete _launchMethods[0]; -} - -void LaunchDetector::reset() -{ - /* Reset all detectors */ - for (const auto launchMethod : _launchMethods) { - launchMethod->reset(); - } - - /* Reset active launchdetector */ - _activeLaunchDetectionMethodIndex = -1; -} - void LaunchDetector::update(const float dt, float accel_x) { - if (launchDetectionEnabled()) { - for (const auto launchMethod : _launchMethods) { - launchMethod->update(dt, accel_x); - } - } -} + switch (state) { + case LAUNCHDETECTION_RES_NONE: -LaunchDetectionResult LaunchDetector::getLaunchDetected() -{ - if (launchDetectionEnabled()) { - if (_activeLaunchDetectionMethodIndex < 0) { - /* None of the active _launchmethods has detected a launch, check all _launchmethods */ - for (unsigned i = 0; i < (sizeof(_launchMethods) / sizeof(_launchMethods[0])); i++) { - if (_launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { - PX4_INFO("selecting launchmethod %d", i); - _activeLaunchDetectionMethodIndex = i; // from now on only check this method - return _launchMethods[i]->getLaunchDetected(); + /* Detect a acceleration that is longer and stronger as the minimum given by the params */ + if (accel_x > _param_laun_cat_a.get()) { + _integrator += dt; + + if (_integrator > _param_laun_cat_t.get()) { + if (_param_laun_cat_mdel.get() > 0.0f) { + state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; + PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle", + double(_param_laun_cat_mdel.get())); + + } else { + /* No motor delay set: go directly to enablemotors state */ + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + PX4_WARN("Launch detected: enablemotors (delay not activated)"); } } } else { - return _launchMethods[_activeLaunchDetectionMethodIndex]->getLaunchDetected(); + reset(); } - } - return LAUNCHDETECTION_RES_NONE; + break; + + case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL: + /* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is + * over to allow full throttle */ + _motorDelayCounter += dt; + + if (_motorDelayCounter > _param_laun_cat_mdel.get()) { + PX4_INFO("Launch detected: state enablemotors"); + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + + break; + + default: + break; + + } } +LaunchDetectionResult LaunchDetector::getLaunchDetected() const +{ + return state; +} + +void LaunchDetector::reset() +{ + _integrator = 0.0f; + _motorDelayCounter = 0.0f; + state = LAUNCHDETECTION_RES_NONE; +} + + } // namespace launchdetection diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h index 145ac893f9..c873e81b44 100644 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h +++ b/src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 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 @@ -33,7 +33,7 @@ /** * @file LaunchDetector.h - * Auto Detection for different launch methods (e.g. catapult) + * Auto launch detection for catapult/hand-launch vehicles * * @author Thomas Gubler */ @@ -41,17 +41,26 @@ #ifndef LAUNCHDETECTOR_H #define LAUNCHDETECTOR_H -#include "LaunchMethod.h" #include namespace launchdetection { +enum LaunchDetectionResult { + LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */ + LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should + control the attitude. However any motors should not throttle + up. For instance this is used to have a delay for the motor + when launching a fixed wing aircraft from a bungee */ + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, the controller should control + attitude and also throttle up the motors. */ +}; + class __EXPORT LaunchDetector : public ModuleParams { public: - LaunchDetector(ModuleParams *parent); - ~LaunchDetector() override; + LaunchDetector(ModuleParams *parent) : ModuleParams(parent) {} + ~LaunchDetector() = default; LaunchDetector(const LaunchDetector &) = delete; LaunchDetector operator=(const LaunchDetector &) = delete; @@ -59,22 +68,20 @@ public: void reset(); void update(const float dt, float accel_x); - LaunchDetectionResult getLaunchDetected(); + LaunchDetectionResult getLaunchDetected() const; bool launchDetectionEnabled() { return _param_laun_all_on.get(); } private: - /* holds an index to the launchMethod in the array _launchMethods - * which detected a Launch. If no launchMethod has detected a launch yet the - * value is -1. Once one launchMethod has detected a launch only this - * method is checked for further advancing in the state machine - * (e.g. when to power up the motors) - */ - int _activeLaunchDetectionMethodIndex{-1}; + float _integrator{0.f}; + float _motorDelayCounter{0.f}; - LaunchMethod *_launchMethods[1]; + LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE}; DEFINE_PARAMETERS( - (ParamBool) _param_laun_all_on + (ParamBool) _param_laun_all_on, + (ParamFloat) _param_laun_cat_a, + (ParamFloat) _param_laun_cat_t, + (ParamFloat) _param_laun_cat_mdel ) }; diff --git a/src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h b/src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h deleted file mode 100644 index 43d9535486..0000000000 --- a/src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * 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 - */ - -#ifndef LAUNCHMETHOD_H_ -#define LAUNCHMETHOD_H_ - -namespace launchdetection -{ - -enum LaunchDetectionResult { - LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */ - LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should - control the attitude. However any motors should not throttle - up. For instance this is used to have a delay for the motor - when launching a fixed wing aircraft from a bungee */ - LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, the controller should control - attitude and also throttle up the motors. */ -}; - -class LaunchMethod -{ -public: - virtual ~LaunchMethod() = default; - - virtual void update(const float dt, float accel_x) = 0; - virtual LaunchDetectionResult getLaunchDetected() const = 0; - virtual void reset() = 0; - -}; - -} // namespace launchdetection - -#endif /* LAUNCHMETHOD_H_ */