diff options
15 files changed, 561 insertions, 772 deletions
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 8ef1c333a..e13a88ca5 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -71,12 +71,7 @@ MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan - -# -# Vehicle land detection -# -MODULES += modules/mc_land_detector -MODULES += modules/fw_land_detector +MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.cpp b/src/modules/fw_land_detector/FixedwingLandDetector.cpp deleted file mode 100644 index 5fbeb2f45..000000000 --- a/src/modules/fw_land_detector/FixedwingLandDetector.cpp +++ /dev/null @@ -1,173 +0,0 @@ -/**************************************************************************** - * - * 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 FixedwingLandDetector.cpp - * Land detection algorithm for fixedwings - * - * @author Johan Jansen <jnsn.johan@gmail.com> - */ - -#include "FixedwingLandDetector.h" - -#include <stdio.h> -#include <math.h> -#include <drivers/drv_hrt.h> -#include <unistd.h> //usleep - -FixedwingLandDetector::FixedwingLandDetector() : - _landDetectedPub(-1), - _landDetected({0, false}), - - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), - - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0), - - _taskShouldExit(false), - _taskIsRunning(false) -{ - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); -} - -FixedwingLandDetector::~FixedwingLandDetector() -{ - _taskShouldExit = true; - close(_landDetectedPub); -} - -void FixedwingLandDetector::shutdown() -{ - _taskShouldExit = true; -} - -/** -* @brief Convinience function for polling uORB subscriptions -* @return true if there was new data and it was successfully copied -**/ -static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) -{ - bool newData = false; - - //Check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } - - if (!newData) { - return false; - } - - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } - - return true; -} - -void FixedwingLandDetector::updateSubscriptions() -{ - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); -} - -void FixedwingLandDetector::landDetectorLoop() -{ - //This should never happen! - if (_taskIsRunning) { return; } - - //Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - - _taskIsRunning = true; - _taskShouldExit = false; - - while (!_taskShouldExit) { - - //First poll for new data from our subscriptions - updateSubscriptions(); - - const uint64_t now = hrt_absolute_time(); - bool landDetected = false; - - //TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - - /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { - - //These conditions need to be stable for a period of time before we trust them - if (now > _landDetectTrigger) { - landDetected = true; - } - - } else { - //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; - } - - //Publish if land detection state has changed - if (_landDetected.landed != landDetected) { - _landDetected.timestamp = now; - _landDetected.landed = landDetected; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - - //Limit loop rate - usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); - } - - _taskIsRunning = false; - _exit(0); -} - -bool FixedwingLandDetector::isRunning() const -{ - return _taskIsRunning; -} diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h deleted file mode 100644 index be9b17b74..000000000 --- a/src/modules/fw_land_detector/FixedwingLandDetector.h +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************** - * - * 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 FixedwingLandDetector.h - * Land detection algorithm for fixedwing - * - * @author Johan Jansen <jnsn.johan@gmail.com> - */ - -#ifndef __FIXED_WING_LAND_DETECTOR_H__ -#define __FIXED_WING_LAND_DETECTOR_H__ - -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_land_detected.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/airspeed.h> - -class FixedwingLandDetector -{ -public: - FixedwingLandDetector(); - ~FixedwingLandDetector(); - - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); - - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; - - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); - - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); - -protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); - - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = - 2000000; /**< usec that landing conditions have to hold before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ - -private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; - - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - uint64_t _landDetectTrigger; - - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ -}; - -#endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/fw_land_detector/fw_land_detector_main.cpp b/src/modules/fw_land_detector/fw_land_detector_main.cpp deleted file mode 100644 index a45a51863..000000000 --- a/src/modules/fw_land_detector/fw_land_detector_main.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/**************************************************************************** - * - * 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 fw_land_detector_main.cpp - * Land detection algorithm for fixed wings - * - * @author Johan Jansen <jnsn.johan@gmail.com> - */ - -#include <unistd.h> //usleep -#include <stdio.h> -#include <string.h> -#include <stdlib.h> -#include <errno.h> -#include <drivers/drv_hrt.h> -#include <systemlib/systemlib.h> //Scheduler -#include <systemlib/err.h> //print to console - -#include "FixedwingLandDetector.h" - -//Function prototypes -static int fw_land_detector_start(); -static void fw_land_detector_stop(); - -/** - * land detector app start / stop handling function - * This makes the land detector module accessible from the nuttx shell - * @ingroup apps - */ -extern "C" __EXPORT int fw_land_detector_main(int argc, char *argv[]); - -//Private variables -static FixedwingLandDetector *fw_land_detector_task = nullptr; -static int _landDetectorTaskID = -1; - -/** -* Deamon thread function -**/ -static void fw_land_detector_deamon_thread(int argc, char *argv[]) -{ - fw_land_detector_task->landDetectorLoop(); -} - -/** -* Stop the task, force killing it if it doesn't stop by itself -**/ -static void fw_land_detector_stop() -{ - if (fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { - errx(1, "not running"); - return; - } - - fw_land_detector_task->shutdown(); - - //Wait for task to die - int i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_landDetectorTaskID); - break; - } - } while (fw_land_detector_task->isRunning()); - - - delete fw_land_detector_task; - fw_land_detector_task = nullptr; - _landDetectorTaskID = -1; - warn("fw_land_detector has been stopped"); -} - -/** -* Start new task, fails if it is already running. Returns OK if successful -**/ -static int fw_land_detector_start() -{ - if (fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { - errx(1, "already running"); - return -1; - } - - //Allocate memory - fw_land_detector_task = new FixedwingLandDetector(); - - if (fw_land_detector_task == nullptr) { - errx(1, "alloc failed"); - return -1; - } - - //Start new thread task - _landDetectorTaskID = task_spawn_cmd("fw_land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)&fw_land_detector_deamon_thread, - nullptr); - - if (_landDetectorTaskID < 0) { - errx(1, "task start failed: %d", -errno); - return -1; - } - - /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - - while (!fw_land_detector_task->isRunning()) { - usleep(50000); - printf("."); - fflush(stdout); - - if (hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); - fw_land_detector_stop(); - exit(1); - } - } - - printf("\n"); - - exit(0); - return 0; -} - -/** -* Main entry point for this module -**/ -int fw_land_detector_main(int argc, char *argv[]) -{ - - if (argc < 1) { - warnx("usage: fw_land_detector {start|stop|status}"); - exit(0); - } - - if (!strcmp(argv[1], "start")) { - fw_land_detector_start(); - } - - if (!strcmp(argv[1], "stop")) { - fw_land_detector_stop(); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (fw_land_detector_task) { - - if (fw_land_detector_task->isRunning()) { - warnx("running"); - - } else { - errx(1, "exists, but not running"); - } - - exit(0); - - } else { - errx(1, "not running"); - } - } - - warn("usage: fw_land_detector {start|stop|status}"); - return 1; -} diff --git a/src/modules/fw_land_detector/module.mk b/src/modules/fw_land_detector/module.mk deleted file mode 100644 index 894b29a7b..000000000 --- a/src/modules/fw_land_detector/module.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Fixedwing land detector -# - -MODULE_COMMAND = fw_land_detector - -SRCS = fw_land_detector_main.cpp \ - FixedwingLandDetector.cpp - -EXTRACXXFLAGS = -Weffc++
\ No newline at end of file diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp new file mode 100644 index 000000000..42735f38c --- /dev/null +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 FixedwingLandDetector.cpp + * Land detection algorithm for fixedwings + * + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#include "FixedwingLandDetector.h" + +#include <cmath> +#include <drivers/drv_hrt.h> + +FixedwingLandDetector::FixedwingLandDetector() : + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), + + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) +{ + //ctor +} + +void FixedwingLandDetector::initialize() +{ + //Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); +} + +void FixedwingLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); +} + +bool FixedwingLandDetector::update() +{ + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; + + //TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + /* crude land detector for fixedwing */ + if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX + && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX + && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + + //These conditions need to be stable for a period of time before we trust them + if (now > _landDetectTrigger) { + landDetected = true; + } + + } else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } + + return landDetected; +} diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h new file mode 100644 index 000000000..faeece6f3 --- /dev/null +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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 FixedwingLandDetector.h + * Land detection algorithm for fixedwing + * + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#ifndef __FIXED_WING_LAND_DETECTOR_H__ +#define __FIXED_WING_LAND_DETECTOR_H__ + +#include "LandDetector.h" +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/airspeed.h> + +class FixedwingLandDetector : public LandDetector +{ +public: + FixedwingLandDetector(); + +protected: + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); + + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold + before triggering a land */ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + +private: + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; + + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; +}; + +#endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp new file mode 100644 index 000000000..a39e53818 --- /dev/null +++ b/src/modules/land_detector/LandDetector.cpp @@ -0,0 +1,77 @@ +#include "LandDetector.h" +#include <unistd.h> //usleep +#include <drivers/drv_hrt.h> + +LandDetector::LandDetector() : + _landDetectedPub(-1), + _landDetected({0, false}), + _taskShouldExit(false), + _taskIsRunning(false) +{ + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); +} + +LandDetector::~LandDetector() +{ + _taskShouldExit = true; + close(_landDetectedPub); +} + +void LandDetector::shutdown() +{ + _taskShouldExit = true; +} + +void LandDetector::start() +{ + //Make sure this method has not already been called by another thread + if(isRunning()) { + return; + } + + //Task is now running, keep doing so until shutdown() has been called + _taskIsRunning = true; + _taskShouldExit = false; + while(isRunning()) { + + bool landDetected = update(); + + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = landDetected; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + + //Limit loop rate + usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); +} + +bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } + + if (!newData) { + return false; + } + + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h new file mode 100644 index 000000000..6d7465691 --- /dev/null +++ b/src/modules/land_detector/LandDetector.h @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 LandDetector.h + * Abstract interface for land detector algorithms + * + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#ifndef __LAND_DETECTOR_H__ +#define __LAND_DETECTOR_H__ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_land_detected.h> + +class LandDetector +{ +public: + + LandDetector(); + virtual ~LandDetector(); + + /** + * @return true if this task is currently running + **/ + inline bool isRunning() const {return _taskIsRunning;} + + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); + + /** + * @brief Blocking function that should be called from it's own task thread. This method will + * run the underlying algorithm at the desired update rate and publish if the landing state changes. + **/ + void start(); + +protected: + + /** + * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm + * @return true if a landing was detected and this should be broadcast to the rest of the system + **/ + virtual bool update() = 0; + + /** + * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, + * uORB topic subscription, creating file descriptors, etc.) + **/ + virtual void initialize() = 0; + + /** + * @brief Convinience function for polling uORB subscriptions + * @return true if there was new data and it was successfully copied + **/ + bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); + + static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + +protected: + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + +private: + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ +}; + +#endif //__LAND_DETECTOR_H__
\ No newline at end of file diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp new file mode 100644 index 000000000..36d92fd79 --- /dev/null +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * 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 MulticopterLandDetector.cpp + * Land detection algorithm + * + * @author Johan Jansen <jnsn.johan@gmail.com> + * @author Morten Lysgaard <morten@lysgaard.no> + */ + +#include "MulticopterLandDetector.h" + +#include <cmath> +#include <drivers/drv_hrt.h> + +MulticopterLandDetector::MulticopterLandDetector() : + _vehicleGlobalPositionSub(-1), + _sensorsCombinedSub(-1), + _waypointSub(-1), + _actuatorsSub(-1), + _armingSub(-1), + + _vehicleGlobalPosition({}), + _sensors({}), + _waypoint({}), + _actuators({}), + _arming({}), + + _landTimer(0) +{ + //ctor +} + +void MulticopterLandDetector::initialize() +{ + //Subscribe to position, attitude, arming and velocity changes + _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); + _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); +} + +void MulticopterLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); + orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); + orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); + orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); +} + +bool MulticopterLandDetector::update() +{ + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + + //only detect landing if the autopilot is actively trying to land + if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + _landTimer = now; + + } else { + + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + + //Check if we are moving horizontally + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector + _landTimer = now; + } + + } + + return now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME; +} diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index fe18374c3..08a8132ba 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -32,69 +32,52 @@ ****************************************************************************/ /** - * @file MultiCopterLandDetector.h + * @file MulticopterLandDetector.h * Land detection algorithm for multicopters * * @author Johan Jansen <jnsn.johan@gmail.com> - * @author Morten Lysgaard <Morten@mycptr.com> + * @author Morten Lysgaard <morten@lysgaard.no> */ #ifndef __MULTICOPTER_LAND_DETECTOR_H__ #define __MULTICOPTER_LAND_DETECTOR_H__ -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_land_detected.h> +#include "LandDetector.h" #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_armed.h> -class MulticopterLandDetector +class MulticopterLandDetector : public LandDetector { public: MulticopterLandDetector(); - ~MulticopterLandDetector(); - - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); - - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; +protected: /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + * @brief polls all subscriptions and pulls any data that has changed **/ - void landDetectorLoop(); + void updateSubscriptions(); /** - * @brief Tells the Land Detector task that it should exit + * @brief Runs one iteration of the land detection algorithm **/ - void shutdown(); + bool update() override; -protected: /** - * @brief polls all subscriptions and pulls any data that has changed + * @brief Initializes the land detection algorithm **/ - void updateSubscriptions(); + void initialize() override; //Algorithm parameters (TODO: should probably be externalized) static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - int _vehicleGlobalPositionSub; /**< notification of global position */ int _sensorsCombinedSub; int _waypointSub; @@ -107,9 +90,6 @@ private: struct actuator_controls_s _actuators; struct actuator_armed_s _arming; - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ - uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; diff --git a/src/modules/mc_land_detector/mc_land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 6ad3f82a4..2bc89e752 100644 --- a/src/modules/mc_land_detector/mc_land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file mc_land_detector_main.cpp - * Land detection algorithm for multicopters + * @file land_detector_main.cpp + * Land detection algorithm * * @author Johan Jansen <jnsn.johan@gmail.com> */ @@ -47,42 +47,44 @@ #include <systemlib/systemlib.h> //Scheduler #include <systemlib/err.h> //print to console +#include "FixedwingLandDetector.h" #include "MulticopterLandDetector.h" //Function prototypes -static int mc_land_detector_start(); -static void mc_land_detector_stop(); +static int land_detector_start(const char* mode); +static void land_detector_stop(); /** * land detector app start / stop handling function * This makes the land detector module accessible from the nuttx shell * @ingroup apps */ -extern "C" __EXPORT int mc_land_detector_main(int argc, char *argv[]); +extern "C" __EXPORT int land_detector_main(int argc, char *argv[]); //Private variables -static MulticopterLandDetector *mc_land_detector_task = nullptr; +static LandDetector *land_detector_task = nullptr; static int _landDetectorTaskID = -1; +static char _currentMode[12]; /** * Deamon thread function **/ -static void mc_land_detector_deamon_thread(int argc, char *argv[]) +static void land_detector_deamon_thread(int argc, char *argv[]) { - mc_land_detector_task->landDetectorLoop(); + land_detector_task->start(); } /** * Stop the task, force killing it if it doesn't stop by itself **/ -static void mc_land_detector_stop() +static void land_detector_stop() { - if (mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { + if (land_detector_task == nullptr || _landDetectorTaskID == -1) { errx(1, "not running"); return; } - mc_land_detector_task->shutdown(); + land_detector_task->shutdown(); //Wait for task to die int i = 0; @@ -96,39 +98,49 @@ static void mc_land_detector_stop() task_delete(_landDetectorTaskID); break; } - } while (mc_land_detector_task->isRunning()); + } while (land_detector_task->isRunning()); - delete mc_land_detector_task; - mc_land_detector_task = nullptr; + delete land_detector_task; + land_detector_task = nullptr; _landDetectorTaskID = -1; - warn("mc_land_detector has been stopped"); + warn("land_detector has been stopped"); } /** * Start new task, fails if it is already running. Returns OK if successful **/ -static int mc_land_detector_start() +static int land_detector_start(const char* mode) { - if (mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { + if (land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); return -1; } //Allocate memory - mc_land_detector_task = new MulticopterLandDetector(); + if(!strcmp(mode, "fixedwing")) { + land_detector_task = new FixedwingLandDetector(); + } + else if(!strcmp(mode, "multicopter")) { + land_detector_task = new MulticopterLandDetector(); + } + else { + errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); + return -1; + } - if (mc_land_detector_task == nullptr) { + //Check if alloc worked + if (land_detector_task == nullptr) { errx(1, "alloc failed"); return -1; } //Start new thread task - _landDetectorTaskID = task_spawn_cmd("mc_land_detector", + _landDetectorTaskID = task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, - (main_t)&mc_land_detector_deamon_thread, + (main_t)&land_detector_deamon_thread, nullptr); if (_landDetectorTaskID < 0) { @@ -139,20 +151,23 @@ static int mc_land_detector_start() /* avoid memory fragmentation by not exiting start handler until the task has fully started */ const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - while (!mc_land_detector_task->isRunning()) { + while (!land_detector_task->isRunning()) { usleep(50000); printf("."); fflush(stdout); if (hrt_absolute_time() > timeout) { err(1, "start failed - timeout"); - mc_land_detector_stop(); + land_detector_stop(); exit(1); } } printf("\n"); + //Remember current active mode + strncpy(_currentMode, mode, 12); + exit(0); return 0; } @@ -160,31 +175,31 @@ static int mc_land_detector_start() /** * Main entry point for this module **/ -int mc_land_detector_main(int argc, char *argv[]) +int land_detector_main(int argc, char *argv[]) { if (argc < 1) { - warnx("usage: mc_land_detector {start|stop|status}"); + warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); exit(0); } - if (!strcmp(argv[1], "start")) { - mc_land_detector_start(); + if (argc >= 2 && !strcmp(argv[1], "start")) { + land_detector_start(argv[2]); } if (!strcmp(argv[1], "stop")) { - mc_land_detector_stop(); + land_detector_stop(); exit(0); } if (!strcmp(argv[1], "status")) { - if (mc_land_detector_task) { + if (land_detector_task) { - if (mc_land_detector_task->isRunning()) { - warnx("running"); + if (land_detector_task->isRunning()) { + warnx("running (%s)", _currentMode); } else { - errx(1, "exists, but not running"); + errx(1, "exists, but not running (%s)", _currentMode); } exit(0); @@ -194,6 +209,6 @@ int mc_land_detector_main(int argc, char *argv[]) } } - warn("usage: mc_land_detector {start|stop|status}"); + warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); return 1; } diff --git a/src/modules/land_detector/module.mk b/src/modules/land_detector/module.mk new file mode 100644 index 000000000..db0e18447 --- /dev/null +++ b/src/modules/land_detector/module.mk @@ -0,0 +1,12 @@ +# +# Land detector +# + +MODULE_COMMAND = land_detector + +SRCS = land_detector_main.cpp \ + LandDetector.cpp \ + MulticopterLandDetector.cpp \ + FixedwingLandDetector.cpp + +EXTRACXXFLAGS = -Weffc++ -Os diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp deleted file mode 100644 index d23ff9017..000000000 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ /dev/null @@ -1,202 +0,0 @@ -/**************************************************************************** - * - * 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 MultiCopterLandDetector.cpp - * Land detection algorithm for multicopters - * - * @author Johan Jansen <jnsn.johan@gmail.com> - * @author Morten Lysgaard <morten@lysgaard.no> - */ - -#include "MulticopterLandDetector.h" - -#include <stdio.h> -#include <cmath> -#include <drivers/drv_hrt.h> -#include <unistd.h> //usleep - -MulticopterLandDetector::MulticopterLandDetector() : - _landDetectedPub(-1), - _landDetected({0, false}), - - _vehicleGlobalPositionSub(-1), - _sensorsCombinedSub(-1), - _waypointSub(-1), - _actuatorsSub(-1), - _armingSub(-1), - - _vehicleGlobalPosition({}), - _sensors({}), - _waypoint({}), - _actuators({}), - _arming({}), - - _taskShouldExit(false), - _taskIsRunning(false), - _landTimer(0) -{ - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); -} - -MulticopterLandDetector::~MulticopterLandDetector() -{ - _taskShouldExit = true; - close(_landDetectedPub); -} - -/** -* @brief Convinience function for polling uORB subscriptions -* @return true if there was new data and it was successfully copied -**/ -static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) -{ - bool newData = false; - - //Check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } - - if (!newData) { - return false; - } - - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } - - return true; -} - -void MulticopterLandDetector::shutdown() -{ - _taskShouldExit = true; -} - -void MulticopterLandDetector::updateSubscriptions() -{ - orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); - orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); - orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); - orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); - orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); -} - -void MulticopterLandDetector::landDetectorLoop() -{ - //This should never happen! - if (_taskIsRunning) { return; } - - //Subscribe to position, attitude, arming and velocity changes - _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); - _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); - _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - _armingSub = orb_subscribe(ORB_ID(actuator_armed)); - - //Begin task - _taskIsRunning = true; - _taskShouldExit = false; - - while (!_taskShouldExit) { - - //First poll for new data from our subscriptions - updateSubscriptions(); - - const uint64_t now = hrt_absolute_time(); - - //only detect landing if the autopilot is actively trying to land - if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { - _landTimer = now; - - } else { - - //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; - - //Check if we are moving horizontally - bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; - - //Next look if all rotation angles are not moving - bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + - _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + - _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; - - //Check if thrust output is minimal (about half of default) - bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; - - if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector - _landTimer = now; - } - - } - - // if we have detected a landing for 2 continuous seconds - if (now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { - if (!_landDetected.landed) { - _landDetected.timestamp = now; - _landDetected.landed = true; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - - } else { - // if we currently think we have landed, but the latest data says we are flying - if (_landDetected.landed) { - _landDetected.timestamp = now; - _landDetected.landed = false; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - } - - //Limit loop rate - usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); - } - - _taskIsRunning = false; - _exit(0); -} - -bool MulticopterLandDetector::isRunning() const -{ - return _taskIsRunning; -} diff --git a/src/modules/mc_land_detector/module.mk b/src/modules/mc_land_detector/module.mk deleted file mode 100644 index 023d48979..000000000 --- a/src/modules/mc_land_detector/module.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Multirotor land detector -# - -MODULE_COMMAND = mc_land_detector - -SRCS = mc_land_detector_main.cpp \ - MulticopterLandDetector.cpp - -EXTRACXXFLAGS = -Weffc++ |