Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Launch Detection: consolidate in single class (use the only existing method, catapult)

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-11-03 18:51:34 +01:00
parent b05878690d
commit 35da7f9bc4
6 changed files with 73 additions and 322 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/launchdetection/CMakeLists.txt
+1 -2
View File
@@ -33,5 +33,4 @@
px4_add_library(launchdetection px4_add_library(launchdetection
LaunchDetector.cpp LaunchDetector.cpp
CatapultLaunchMethod.cpp )
)
src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp
-112
View File
@@ -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 <thomasgubler@gmail.com>
*
*/
#include "CatapultLaunchMethod.h"
#include <px4_platform_common/log.h>
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
src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h
-78
View File
@@ -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 <thomasgubler@gmail.com>
*/
#ifndef CATAPULTLAUNCHMETHOD_H_
#define CATAPULTLAUNCHMETHOD_H_
#include "LaunchMethod.h"
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
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<px4::params::LAUN_CAT_A>) _param_laun_cat_a,
(ParamFloat<px4::params::LAUN_CAT_T>) _param_laun_cat_t,
(ParamFloat<px4::params::LAUN_CAT_MDEL>) _param_laun_cat_mdel
)
};
#endif /* CATAPULTLAUNCHMETHOD_H_ */
} // namespace launchdetection
src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp
+50 -45
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -32,12 +32,11 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file launchDetection.cpp * @file launchDetection.cpp
* Auto Detection for different launch methods (e.g. catapult) * Auto launch detection for catapult/hand-launch vehicles
* *
* @author Thomas Gubler <thomasgubler@gmail.com> * @author Thomas Gubler <thomasgubler@gmail.com>
*/ */
#include "CatapultLaunchMethod.h"
#include "LaunchDetector.h" #include "LaunchDetector.h"
#include <px4_platform_common/log.h> #include <px4_platform_common/log.h>
@@ -45,57 +44,63 @@
namespace launchdetection 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) void LaunchDetector::update(const float dt, float accel_x)
{ {
if (launchDetectionEnabled()) { switch (state) {
for (const auto launchMethod : _launchMethods) { case LAUNCHDETECTION_RES_NONE:
launchMethod->update(dt, accel_x);
}
}
}
LaunchDetectionResult LaunchDetector::getLaunchDetected() /* Detect a acceleration that is longer and stronger as the minimum given by the params */
{ if (accel_x > _param_laun_cat_a.get()) {
if (launchDetectionEnabled()) { _integrator += dt;
if (_activeLaunchDetectionMethodIndex < 0) {
/* None of the active _launchmethods has detected a launch, check all _launchmethods */ if (_integrator > _param_laun_cat_t.get()) {
for (unsigned i = 0; i < (sizeof(_launchMethods) / sizeof(_launchMethods[0])); i++) { if (_param_laun_cat_mdel.get() > 0.0f) {
if (_launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
PX4_INFO("selecting launchmethod %d", i); PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle",
_activeLaunchDetectionMethodIndex = i; // from now on only check this method double(_param_laun_cat_mdel.get()));
return _launchMethods[i]->getLaunchDetected();
} else {
/* No motor delay set: go directly to enablemotors state */
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
PX4_WARN("Launch detected: enablemotors (delay not activated)");
} }
} }
} else { } 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 } // namespace launchdetection
src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h
+22 -15
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -33,7 +33,7 @@
/** /**
* @file LaunchDetector.h * @file LaunchDetector.h
* Auto Detection for different launch methods (e.g. catapult) * Auto launch detection for catapult/hand-launch vehicles
* *
* @author Thomas Gubler <thomasgubler@gmail.com> * @author Thomas Gubler <thomasgubler@gmail.com>
*/ */
@@ -41,17 +41,26 @@
#ifndef LAUNCHDETECTOR_H #ifndef LAUNCHDETECTOR_H
#define LAUNCHDETECTOR_H #define LAUNCHDETECTOR_H
#include "LaunchMethod.h"
#include <px4_platform_common/module_params.h> #include <px4_platform_common/module_params.h>
namespace launchdetection 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 class __EXPORT LaunchDetector : public ModuleParams
{ {
public: public:
LaunchDetector(ModuleParams *parent); LaunchDetector(ModuleParams *parent) : ModuleParams(parent) {}
~LaunchDetector() override; ~LaunchDetector() = default;
LaunchDetector(const LaunchDetector &) = delete; LaunchDetector(const LaunchDetector &) = delete;
LaunchDetector operator=(const LaunchDetector &) = delete; LaunchDetector operator=(const LaunchDetector &) = delete;
@@ -59,22 +68,20 @@ public:
void reset(); void reset();
void update(const float dt, float accel_x); void update(const float dt, float accel_x);
LaunchDetectionResult getLaunchDetected(); LaunchDetectionResult getLaunchDetected() const;
bool launchDetectionEnabled() { return _param_laun_all_on.get(); } bool launchDetectionEnabled() { return _param_laun_all_on.get(); }
private: private:
/* holds an index to the launchMethod in the array _launchMethods float _integrator{0.f};
* which detected a Launch. If no launchMethod has detected a launch yet the float _motorDelayCounter{0.f};
* 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};
LaunchMethod *_launchMethods[1]; LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE};
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamBool<px4::params::LAUN_ALL_ON>) _param_laun_all_on (ParamBool<px4::params::LAUN_ALL_ON>) _param_laun_all_on,
(ParamFloat<px4::params::LAUN_CAT_A>) _param_laun_cat_a,
(ParamFloat<px4::params::LAUN_CAT_T>) _param_laun_cat_t,
(ParamFloat<px4::params::LAUN_CAT_MDEL>) _param_laun_cat_mdel
) )
}; };
src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h
-70
View File
@@ -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 <thomasgubler@gmail.com>
*/
#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_ */