diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-06 11:40:11 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-15 14:37:50 +0100 |
commit | 642063c3b8fdcd3f5e748666d1bb0412ea434b5f (patch) | |
tree | 65df726800a4f893c7de393931225a2afba1c323 /src/modules | |
parent | d0af62783d1b18da6a15cb2da63e7ea88f5c398a (diff) | |
download | px4-firmware-642063c3b8fdcd3f5e748666d1bb0412ea434b5f.tar.gz px4-firmware-642063c3b8fdcd3f5e748666d1bb0412ea434b5f.tar.bz2 px4-firmware-642063c3b8fdcd3f5e748666d1bb0412ea434b5f.zip |
LandDetector: Added crude land detectors for multicopter and fixedwing
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/fw_land_detector/FixedwingLandDetector.cpp | 171 | ||||
-rw-r--r-- | src/modules/fw_land_detector/FixedwingLandDetector.h | 107 | ||||
-rw-r--r-- | src/modules/fw_land_detector/fw_land_detector_main.cpp | 195 | ||||
-rw-r--r-- | src/modules/fw_land_detector/module.mk | 10 | ||||
-rw-r--r-- | src/modules/mc_land_detector/MulticopterLandDetector.cpp | 212 | ||||
-rw-r--r-- | src/modules/mc_land_detector/MulticopterLandDetector.h | 116 | ||||
-rw-r--r-- | src/modules/mc_land_detector/mc_land_detector_main.cpp | 195 | ||||
-rw-r--r-- | src/modules/mc_land_detector/module.mk | 10 |
8 files changed, 1016 insertions, 0 deletions
diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.cpp b/src/modules/fw_land_detector/FixedwingLandDetector.cpp new file mode 100644 index 000000000..6f5eca3ce --- /dev/null +++ b/src/modules/fw_land_detector/FixedwingLandDetector.cpp @@ -0,0 +1,171 @@ +/**************************************************************************** + * + * 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 new file mode 100644 index 000000000..6a0f951dd --- /dev/null +++ b/src/modules/fw_land_detector/FixedwingLandDetector.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 multicopters + * + * @author Johan Jansen <jnsn.johan@gmail.com> + * @author Morten Lysgaard <Morten@mycptr.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 new file mode 100644 index 000000000..a9eefc406 --- /dev/null +++ b/src/modules/fw_land_detector/fw_land_detector_main.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * 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 new file mode 100644 index 000000000..894b29a7b --- /dev/null +++ b/src/modules/fw_land_detector/module.mk @@ -0,0 +1,10 @@ +# +# 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/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp new file mode 100644 index 000000000..39064de6b --- /dev/null +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * 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 { + /* + static int debugcnt = 0; + if(debugcnt++ > 12) { + debugcnt = 0; + mavlink_log_critical(_mavlinkFd, "T: %.4f R: %.4f", (double)_actuators.control[3], + sqrt( _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])); + } + */ + + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + + 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 continious seconds + if(now-_landTimer > 2000000) { + 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/MulticopterLandDetector.h b/src/modules/mc_land_detector/MulticopterLandDetector.h new file mode 100644 index 000000000..7421898c1 --- /dev/null +++ b/src/modules/mc_land_detector/MulticopterLandDetector.h @@ -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 MultiCopterLandDetector.h + * Land detection algorithm for multicopters + * + * @author Johan Jansen <jnsn.johan@gmail.com> + * @author Morten Lysgaard <Morten@mycptr.com> + */ + +#ifndef __MULTICOPTER_LAND_DETECTOR_H__ +#define __MULTICOPTER_LAND_DETECTOR_H__ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_land_detected.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> + +//TODO: add crash detection to this module? +class MulticopterLandDetector { +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; + + /** + * @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 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; + int _actuatorsSub; + int _armingSub; + + struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ + struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ + struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ + 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*/ +}; + +#endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/mc_land_detector/mc_land_detector_main.cpp b/src/modules/mc_land_detector/mc_land_detector_main.cpp new file mode 100644 index 000000000..2992ead99 --- /dev/null +++ b/src/modules/mc_land_detector/mc_land_detector_main.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * 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 mc_land_detector_main.cpp + * Land detection algorithm for multicopters + * + * @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 "MulticopterLandDetector.h" + +//Function prototypes +static int mc_land_detector_start(); +static void mc_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[]); + +//Private variables +static MulticopterLandDetector* mc_land_detector_task = nullptr; +static int _landDetectorTaskID = -1; + +/** +* Deamon thread function +**/ +static void mc_land_detector_deamon_thread(int argc, char *argv[]) +{ + mc_land_detector_task->landDetectorLoop(); +} + +/** +* Stop the task, force killing it if it doesn't stop by itself +**/ +static void mc_land_detector_stop() +{ + if(mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { + errx(1, "not running"); + return; + } + + mc_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 (mc_land_detector_task->isRunning()); + + + delete mc_land_detector_task; + mc_land_detector_task = nullptr; + _landDetectorTaskID = -1; + warn("mc_land_detector has been stopped"); +} + +/** +* Start new task, fails if it is already running. Returns OK if successful +**/ +static int mc_land_detector_start() +{ + if(mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { + errx(1, "already running"); + return -1; + } + + //Allocate memory + mc_land_detector_task = new MulticopterLandDetector(); + if (mc_land_detector_task == nullptr) { + errx(1, "alloc failed"); + return -1; + } + + //Start new thread task + _landDetectorTaskID = task_spawn_cmd("mc_land_detector", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&mc_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 (!mc_land_detector_task->isRunning()) { + usleep(50000); + printf("."); + fflush(stdout); + + if(hrt_absolute_time() > timeout) { + err(1, "start failed - timeout"); + mc_land_detector_stop(); + exit(1); + } + } + printf("\n"); + + exit(0); + return 0; +} + +/** +* Main entry point for this module +**/ +int mc_land_detector_main(int argc, char *argv[]) +{ + + if (argc < 1) { + warnx("usage: mc_land_detector {start|stop|status}"); + exit(0); + } + + if (!strcmp(argv[1], "start")) { + mc_land_detector_start(); + } + + if (!strcmp(argv[1], "stop")) { + mc_land_detector_stop(); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (mc_land_detector_task) { + + if(mc_land_detector_task->isRunning()) { + warnx("running"); + } + else { + errx(1, "exists, but not running"); + } + + exit(0); + + } else { + errx(1, "not running"); + } + } + + warn("usage: mc_land_detector {start|stop|status}"); + return 1; +} diff --git a/src/modules/mc_land_detector/module.mk b/src/modules/mc_land_detector/module.mk new file mode 100644 index 000000000..023d48979 --- /dev/null +++ b/src/modules/mc_land_detector/module.mk @@ -0,0 +1,10 @@ +# +# Multirotor land detector +# + +MODULE_COMMAND = mc_land_detector + +SRCS = mc_land_detector_main.cpp \ + MulticopterLandDetector.cpp + +EXTRACXXFLAGS = -Weffc++ |