diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index fa636e496c..2f46c21f3f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -95,6 +95,13 @@ bool MulticopterLandDetector::update() // first poll for new data from our subscriptions updateSubscriptions(); + updateParameterCache(false); + + return get_landed_state(); +} + +bool MulticopterLandDetector::get_landed_state() +{ // only trigger flight conditions if we are armed if (!_arming.armed) { _arming_time = 0; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index d001be4e7f..3ee697a468 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -60,24 +60,29 @@ protected: /** * @brief polls all subscriptions and pulls any data that has changed **/ - void updateSubscriptions(); + virtual void updateSubscriptions(); /** * @brief Runs one iteration of the land detection algorithm **/ - bool update() override; + virtual bool update() override; /** * @brief Initializes the land detection algorithm **/ - void initialize() override; + virtual void initialize() override; - -private: /** * @brief download and update local parameter cache **/ - void updateParameterCache(const bool force); + virtual void updateParameterCache(const bool force); + + /** + * @brief get multicopter landed state + **/ + bool get_landed_state(); + +private: /** * @brief Handles for interesting parameters diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp new file mode 100644 index 0000000000..3643993b18 --- /dev/null +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 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 VtolLandDetector.cpp + * Land detection algorithm for vtol + * + * @author Roman Bapst + */ + +#include "VtolLandDetector.h" +#include + +VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(), + _paramHandle(), + _params(), + _airspeedSub(-1), + _parameterSub(-1), + _airspeed{}, + _was_in_air(false), + _airspeed_filtered(0) +{ + _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); +} + +void VtolLandDetector::initialize() +{ + MulticopterLandDetector::initialize(); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + + // download parameters + updateParameterCache(true); +} + +void VtolLandDetector::updateSubscriptions() +{ + MulticopterLandDetector::updateSubscriptions(); + + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); +} + +bool VtolLandDetector::update() +{ + updateSubscriptions(); + updateParameterCache(false); + + // this is returned from the mutlicopter land detector + bool landed = get_landed_state(); + + // for vtol we additionally consider airspeed + if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + } else { + // if airspeed does not update, set it to zero and rely on multicopter land detector + _airspeed_filtered = 0.0f; + } + + // only consider airspeed if we have been in air before to avoid false + // detections in the case of wind on the ground + if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) { + landed = false; + } + + _was_in_air = !landed; + + return landed; +} + +void VtolLandDetector::updateParameterCache(const bool force) +{ + MulticopterLandDetector::updateParameterCache(force); + + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + } +} diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h new file mode 100644 index 0000000000..d36140d64f --- /dev/null +++ b/src/modules/land_detector/VtolLandDetector.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 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 VtolLandDetector.h + * Land detection algorithm for vtol + * + * @author Roman Bapst + */ + +#ifndef __VTOL_LAND_DETECTOR_H__ +#define __VTOL_LAND_DETECTOR_H__ + +#include "MulticopterLandDetector.h" +#include + +class VtolLandDetector : public MulticopterLandDetector +{ +public: + VtolLandDetector(); + + +private: + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions() override; + + /** + * @brief Runs one iteration of the land detection algorithm + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force) override; + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxAirSpeed; + } _paramHandle; + + struct { + float maxAirSpeed; + } _params; + + int _airspeedSub; + int _parameterSub; + + struct airspeed_s _airspeed; + + bool _was_in_air; /**< indicates whether the vehicle was in the air in the previous iteration */ + float _airspeed_filtered; /**< low pass filtered airspeed */ +}; + +#endif diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 0aa6e1fad2..1646bf89da 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -53,6 +53,7 @@ #include "FixedwingLandDetector.h" #include "MulticopterLandDetector.h" +#include "VtolLandDetector.h" //Function prototypes static int land_detector_start(const char *mode); @@ -113,6 +114,9 @@ static int land_detector_start(const char *mode) } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); + } else if (!strcmp(mode, "vtol")) { + land_detector_task = new VtolLandDetector(); + } else { warnx("[mode] must be either 'fixedwing' or 'multicopter'"); return -1;