From ec57832a8fc2e848264ecb56c402916eff0ee655 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 21 Dec 2017 12:17:32 -0500 Subject: [PATCH] FW land detector increase trigger time and cleanup (#8486) --- .../land_detector/FixedwingLandDetector.cpp | 26 +-- .../land_detector/FixedwingLandDetector.h | 7 +- src/modules/land_detector/LandDetector.h | 8 +- .../land_detector/land_detector_params.c | 162 ------------------ .../land_detector/land_detector_params_fw.c | 88 ++++++++++ .../land_detector/land_detector_params_mc.c | 131 ++++++++++++++ 6 files changed, 231 insertions(+), 191 deletions(-) create mode 100644 src/modules/land_detector/land_detector_params_fw.c create mode 100644 src/modules/land_detector/land_detector_params_mc.c diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 17bd30651a..0bd7a0d48e 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -57,7 +57,9 @@ FixedwingLandDetector::FixedwingLandDetector() _paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX"); // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). - _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US); + _landed_hysteresis.set_hysteresis_time_from(false, LANDED_TRIGGER_TIME_US); + + _landed_hysteresis.set_hysteresis_time_from(true, FLYING_TRIGGER_TIME_US); } void FixedwingLandDetector::_initialize_topics() @@ -86,26 +88,8 @@ float FixedwingLandDetector::_get_max_altitude() { // TODO // This means no altitude limit as the limit - // is always current position plus 1000 meters - return roundf(-_local_pos.z + 1000); -} - -bool FixedwingLandDetector::_get_freefall_state() -{ - // TODO - return false; -} - -bool FixedwingLandDetector::_get_ground_contact_state() -{ - // TODO - return false; -} - -bool FixedwingLandDetector::_get_maybe_landed_state() -{ - // TODO - return false; + // is always current position plus 10000 meters + return roundf(-_local_pos.z + 10000); } bool FixedwingLandDetector::_get_landed_state() diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 1a7e420e4a..babc2f239b 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -60,16 +60,15 @@ protected: void _initialize_topics() override; void _update_params() override; void _update_topics() override; + bool _get_landed_state() override; - bool _get_maybe_landed_state() override; - bool _get_ground_contact_state() override; - bool _get_freefall_state() override; float _get_max_altitude() override; private: /** Time in us that landing conditions have to hold before triggering a land. */ - static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 1500000; + static constexpr uint64_t LANDED_TRIGGER_TIME_US = 2000000; + static constexpr uint64_t FLYING_TRIGGER_TIME_US = 0; struct { param_t maxVelocity; diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index aadb4e32fa..08d5c61322 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -120,17 +120,17 @@ protected: /** * @return true if UAV is in almost landed state */ - virtual bool _get_maybe_landed_state() = 0; + virtual bool _get_maybe_landed_state() { return false; } /** * @return true if UAV is touching ground but not landed */ - virtual bool _get_ground_contact_state() = 0; + virtual bool _get_ground_contact_state() { return false; } /** * @return true if UAV is in free-fall state. */ - virtual bool _get_freefall_state() = 0; + virtual bool _get_freefall_state() { return false; } /** * @return maximum altitude that can be reached @@ -167,7 +167,7 @@ private: void _cycle(); - void _check_params(const bool force); + void _check_params(bool force = false); void _update_state(); diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 60e472bafa..2376a2b4c9 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -31,150 +31,6 @@ * ****************************************************************************/ -/** - * @file land_detector.c - * Land detector algorithm parameters. - * - * @author Johan Jansen - */ - -/** - * Multicopter max climb rate - * - * Maximum vertical velocity allowed in the landed state (m/s up and down) - * - * @unit m/s - * @decimal 1 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.50f); - -/** - * Multicopter max horizontal velocity - * - * Maximum horizontal velocity allowed in the landed state (m/s) - * - * @unit m/s - * @decimal 1 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f); - -/** - * Multicopter max rotation - * - * Maximum allowed angular velocity around each axis allowed in the landed state. - * - * @unit deg/s - * @decimal 1 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); - -/** - * Multicopter specific force threshold - * - * Multicopter threshold on the specific force measured by accelerometers in m/s^2 for free-fall detection - * - * @unit m/s^2 - * @min 0.1 - * @max 10 - * @decimal 2 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 2.0f); - -/** - * Multicopter sub-hover throttle scaling - * - * The range between throttle_min and throttle_hover is scaled - * by this parameter to define how close to minimum throttle - * the current throttle value needs to be in order to get - * accepted as landed. - * - * @min 0.05 - * @max 0.5 - * @decimal 2 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_THR_RANGE, 0.1f); - -/** - * Multicopter free-fall trigger time - * - * Seconds (decimal) that freefall conditions have to met before triggering a freefall. - * Minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h - * - * @unit s - * @min 0.02 - * @max 5 - * @decimal 2 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3); - -/** - * Fixedwing max horizontal velocity - * - * Maximum horizontal velocity allowed in the landed state (m/s) - * - * @unit m/s - * @min 0.5 - * @max 10 - * @decimal 1 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); - -/** - * Fixedwing max climb rate - * - * Maximum vertical velocity allowed in the landed state (m/s up and down) - * - * @unit m/s - * @min 5 - * @max 20 - * @decimal 1 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); - -/** - * Fixedwing max short-term velocity - * - * Maximum velocity integral in flight direction allowed in the landed state (m/s) - * - * @unit m/s - * @min 2 - * @max 10 - * @decimal 1 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 4.0f); - -/** - * Airspeed max - * - * Maximum airspeed allowed in the landed state (m/s) - * - * @unit m/s - * @min 4 - * @max 20 - * @decimal 1 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 8.00f); - /** * Total flight time in microseconds * @@ -198,21 +54,3 @@ PARAM_DEFINE_INT32(LND_FLIGHT_T_HI, 0); * */ PARAM_DEFINE_INT32(LND_FLIGHT_T_LO, 0); - -/** - * Maximum altitude for multicopters - * - * The system will obey this limit as a - * hard altitude limit. This setting will - * be consolidated with the GF_MAX_VER_DIST - * parameter. - * A negative value indicates no altitude limitation. - * - * @unit m - * @min -1 - * @max 10000 - * @decimal 2 - * @group Land Detector - * - */ -PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f); diff --git a/src/modules/land_detector/land_detector_params_fw.c b/src/modules/land_detector/land_detector_params_fw.c new file mode 100644 index 0000000000..bf5b9b1315 --- /dev/null +++ b/src/modules/land_detector/land_detector_params_fw.c @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2014-2017 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. + * + ****************************************************************************/ + +/** + * Fixedwing max horizontal velocity + * + * Maximum horizontal velocity allowed in the landed state (m/s) + * + * @unit m/s + * @min 0.5 + * @max 10 + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); + +/** + * Fixedwing max climb rate + * + * Maximum vertical velocity allowed in the landed state (m/s up and down) + * + * @unit m/s + * @min 5 + * @max 20 + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); + +/** + * Fixedwing max short-term velocity + * + * Maximum velocity integral in flight direction allowed in the landed state (m/s) + * + * @unit m/s + * @min 2 + * @max 10 + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 8.0f); + +/** + * Airspeed max + * + * Maximum airspeed allowed in the landed state (m/s) + * + * @unit m/s + * @min 4 + * @max 20 + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 8.00f); diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c new file mode 100644 index 0000000000..726a8f57d7 --- /dev/null +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2014-2016 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. + * + ****************************************************************************/ + +/** + * Multicopter max climb rate + * + * Maximum vertical velocity allowed in the landed state (m/s up and down) + * + * @unit m/s + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.50f); + +/** + * Multicopter max horizontal velocity + * + * Maximum horizontal velocity allowed in the landed state (m/s) + * + * @unit m/s + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f); + +/** + * Multicopter max rotation + * + * Maximum allowed angular velocity around each axis allowed in the landed state. + * + * @unit deg/s + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); + +/** + * Multicopter specific force threshold + * + * Multicopter threshold on the specific force measured by accelerometers in m/s^2 for free-fall detection + * + * @unit m/s^2 + * @min 0.1 + * @max 10 + * @decimal 2 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 2.0f); + +/** + * Multicopter sub-hover throttle scaling + * + * The range between throttle_min and throttle_hover is scaled + * by this parameter to define how close to minimum throttle + * the current throttle value needs to be in order to get + * accepted as landed. + * + * @min 0.05 + * @max 0.5 + * @decimal 2 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_THR_RANGE, 0.1f); + +/** + * Multicopter free-fall trigger time + * + * Seconds (decimal) that freefall conditions have to met before triggering a freefall. + * Minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h + * + * @unit s + * @min 0.02 + * @max 5 + * @decimal 2 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3); + +/** + * Maximum altitude for multicopters + * + * The system will obey this limit as a + * hard altitude limit. This setting will + * be consolidated with the GF_MAX_VER_DIST + * parameter. + * A negative value indicates no altitude limitation. + * + * @unit m + * @min -1 + * @max 10000 + * @decimal 2 + * @group Land Detector + * + */ +PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f);