From d0af62783d1b18da6a15cb2da63e7ea88f5c398a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:39:22 +0100 Subject: [PATCH 01/25] uORB: Added vehicle_landed uORB topic --- src/modules/uORB/objects_common.cpp | 3 +++ .../uORB/topics/vehicle_land_detected.h | 23 +++++++++++++++++++ 2 files changed, 26 insertions(+) create mode 100644 src/modules/uORB/topics/vehicle_land_detected.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 293412455e..78fdf4de75 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -83,6 +83,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/vehicle_land_detected.h" +ORB_DEFINE(vehicle_land_detected, struct vehicle_land_detected_s); + #include "topics/satellite_info.h" ORB_DEFINE(satellite_info, struct satellite_info_s); diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h new file mode 100644 index 0000000000..0de29498d4 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_land_detected.h @@ -0,0 +1,23 @@ +#ifndef __TOPIC_VEHICLE_LANDED_H__ +#define __TOPIC_VEHICLE_LANDED_H__ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_land_detected_s { + uint64_t timestamp; /**< timestamp of the setpoint */ + bool landed; /**< true if vehicle is currently landed on the ground*/ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_land_detected); + +#endif //__TOPIC_VEHICLE_LANDED_H__ From 642063c3b8fdcd3f5e748666d1bb0412ea434b5f Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:40:11 +0100 Subject: [PATCH 02/25] LandDetector: Added crude land detectors for multicopter and fixedwing --- makefiles/config_px4fmu-v2_default.mk | 6 + .../FixedwingLandDetector.cpp | 171 ++++++++++++++ .../fw_land_detector/FixedwingLandDetector.h | 107 +++++++++ .../fw_land_detector_main.cpp | 195 ++++++++++++++++ src/modules/fw_land_detector/module.mk | 10 + .../MulticopterLandDetector.cpp | 212 ++++++++++++++++++ .../MulticopterLandDetector.h | 116 ++++++++++ .../mc_land_detector_main.cpp | 195 ++++++++++++++++ src/modules/mc_land_detector/module.mk | 10 + 9 files changed, 1022 insertions(+) create mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.cpp create mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.h create mode 100644 src/modules/fw_land_detector/fw_land_detector_main.cpp create mode 100644 src/modules/fw_land_detector/module.mk create mode 100644 src/modules/mc_land_detector/MulticopterLandDetector.cpp create mode 100644 src/modules/mc_land_detector/MulticopterLandDetector.h create mode 100644 src/modules/mc_land_detector/mc_land_detector_main.cpp create mode 100644 src/modules/mc_land_detector/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 577140f054..8ef1c333ae 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -72,6 +72,12 @@ MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan +# +# Vehicle land detection +# +MODULES += modules/mc_land_detector +MODULES += modules/fw_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 new file mode 100644 index 0000000000..6f5eca3ce0 --- /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 + */ + +#include "FixedwingLandDetector.h" + +#include +#include +#include +#include //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 0000000000..6a0f951dd1 --- /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 + * @author Morten Lysgaard + */ + +#ifndef __FIXED_WING_LAND_DETECTOR_H__ +#define __FIXED_WING_LAND_DETECTOR_H__ + +#include +#include +#include +#include + +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 0000000000..a9eefc4064 --- /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 + */ + +#include //usleep +#include +#include +#include +#include +#include +#include //Scheduler +#include //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 0000000000..894b29a7b5 --- /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 0000000000..39064de6b5 --- /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 + * @author Morten Lysgaard + */ + +#include "MulticopterLandDetector.h" + +#include +#include +#include +#include //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 0000000000..7421898c12 --- /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 + * @author Morten Lysgaard + */ + +#ifndef __MULTICOPTER_LAND_DETECTOR_H__ +#define __MULTICOPTER_LAND_DETECTOR_H__ + +#include +#include +#include +#include +#include +#include +#include + +//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 0000000000..2992ead99c --- /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 + */ + +#include //usleep +#include +#include +#include +#include +#include +#include //Scheduler +#include //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 0000000000..023d48979a --- /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++ From cffba8440e056b71a01003a0d90a7e6f4cffd648 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:46:59 +0100 Subject: [PATCH 03/25] EKF: Removed the fixed wing land detector in the EKF module --- .../ekf_att_pos_estimator_main.cpp | 25 +------------------ 1 file changed, 1 insertion(+), 24 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c41777968e..866e5dc841 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -290,10 +290,6 @@ private: AttPosEKF *_ekf; - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - /** * Update our local parameter cache. */ @@ -422,10 +418,7 @@ FixedwingEstimator::FixedwingEstimator() : _mavlink_fd(-1), _parameters{}, _parameter_handles{}, - _ekf(nullptr), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f) + _ekf(nullptr) { _last_run = hrt_absolute_time(); @@ -1434,22 +1427,6 @@ FixedwingEstimator::task_main() _local_pos.v_z_valid = true; _local_pos.xy_global = true; - _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); - _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; - - - /* crude land detector for fixedwing only, - * TODO: adapt so that it works for both, maybe move to another location - */ - if (_velocity_xy_filtered < 5 - && _velocity_z_filtered < 10 - && _airspeed_filtered < 10) { - _local_pos.landed = true; - } else { - _local_pos.landed = false; - } - _local_pos.z_global = false; _local_pos.yaw = _att.yaw; From 051a6972281dc9f8c15b5d8c73ac808416944932 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:53:00 +0100 Subject: [PATCH 04/25] uORB: Added missing license header --- .../uORB/topics/vehicle_land_detected.h | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h index 0de29498d4..51b3568e82 100644 --- a/src/modules/uORB/topics/vehicle_land_detected.h +++ b/src/modules/uORB/topics/vehicle_land_detected.h @@ -1,3 +1,43 @@ +/**************************************************************************** + * + * 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 vehicle_land_detected.h + * Land detected uORB topic + * + * @author Johan Jansen + */ + #ifndef __TOPIC_VEHICLE_LANDED_H__ #define __TOPIC_VEHICLE_LANDED_H__ From 3a4b3d094a07d41c84efabdc134763bec9372597 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:53:47 +0100 Subject: [PATCH 05/25] LandDetector: Removed commented debug info --- .../mc_land_detector/MulticopterLandDetector.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp index 39064de6b5..63dc54d5ef 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -143,16 +143,6 @@ void MulticopterLandDetector::landDetectorLoop() _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; From b5c7c6a15b4badf56c335746b32ef138afaca539 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:59:15 +0100 Subject: [PATCH 06/25] ROMFS: Added the respective land detector to the startup scripts --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 9 +++++++++ ROMFS/px4fmu_common/init.d/rc.mc_apps | 5 +++++ 2 files changed, 14 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 9aca3fc5f4..c6947009bb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,3 +13,12 @@ ekf_att_pos_estimator start # fw_att_control start fw_pos_control_l1 start + +# +# Start Land Detector +# +fw_land_detector start + +# +# Misc apps +# \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 268eb9bba0..9c3d6352b3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -10,3 +10,8 @@ position_estimator_inav start mc_att_control start mc_pos_control start + +# +# Start Land Detector +# +mc_land_detector start From eefbf366fbc2bef58f0bc283f7d02fb49023faa6 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 12:16:07 +0100 Subject: [PATCH 07/25] LandDetector: Fixed some typos and magic constant --- src/modules/fw_land_detector/FixedwingLandDetector.h | 3 +-- src/modules/mc_land_detector/MulticopterLandDetector.cpp | 5 +++-- src/modules/mc_land_detector/MulticopterLandDetector.h | 1 - 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h index 6a0f951dd1..9004457400 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.h +++ b/src/modules/fw_land_detector/FixedwingLandDetector.h @@ -33,10 +33,9 @@ /** * @file FixedwingLandDetector.h - * Land detection algorithm for multicopters + * Land detection algorithm for fixedwing * * @author Johan Jansen - * @author Morten Lysgaard */ #ifndef __FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp index 63dc54d5ef..90b36a795c 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -147,6 +147,7 @@ void MulticopterLandDetector::landDetectorLoop() //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; @@ -165,8 +166,8 @@ void MulticopterLandDetector::landDetectorLoop() } - // if we have detected a landing for 2 continious seconds - if(now-_landTimer > 2000000) { + // if we have detected a landing for 2 continuous seconds + if(now-_landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { if(!_landDetected.landed) { _landDetected.timestamp = now; diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/mc_land_detector/MulticopterLandDetector.h index 7421898c12..1aeaa0d62a 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/mc_land_detector/MulticopterLandDetector.h @@ -50,7 +50,6 @@ #include #include -//TODO: add crash detection to this module? class MulticopterLandDetector { public: MulticopterLandDetector(); From 28adc8850075da70206864c9f285456bb32c086c Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 12:25:18 +0100 Subject: [PATCH 08/25] Commander: Subscribe and use land detector --- src/modules/commander/commander.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f511f38762..142ff978c4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include #include + #include #include #include @@ -964,6 +965,11 @@ int commander_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); + /* Subscribe to land detector */ + int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + struct vehicle_land_detected_s land_detector; + memset(&land_detector, 0, sizeof(land_detector)); + /* * The home position is set based on GPS only, to prevent a dependency between * position estimator and commander. RAW GPS is more than good enough for a @@ -1379,9 +1385,15 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + /* Update land detector */ + orb_check(land_detector_sub, &updated); + if(updated) { + orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); + } + if (status.condition_local_altitude_valid) { - if (status.condition_landed != local_position.landed) { - status.condition_landed = local_position.landed; + if (status.condition_landed != land_detector.landed) { + status.condition_landed = land_detector.landed; status_changed = true; if (status.condition_landed) { From 98ab83142c1bf7e5170e7527dde1a9e5132b5422 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 12:31:05 +0100 Subject: [PATCH 09/25] InertialNav: Removed land detector from position estimator --- .../position_estimator_inav_main.c | 15 +++++++++------ src/modules/uORB/topics/vehicle_local_position.h | 2 +- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 2f972fc9f2..b418e3a76b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -246,9 +246,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate - float alt_avg = 0.0f; - bool landed = true; - hrt_abstime landed_time = 0; + //float alt_avg = 0.0f; + //bool landed = true; + //hrt_abstime landed_time = 0; hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; @@ -1068,12 +1068,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - /* detect land */ + +/* + // detect land alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; float alt_disp2 = - z_est[0] - alt_avg; alt_disp2 = alt_disp2 * alt_disp2; float land_disp2 = params.land_disp * params.land_disp; - /* get actual thrust output */ + // get actual thrust output float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { @@ -1097,6 +1099,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed_time = 0; } } +*/ if (verbose_mode) { /* print updates rate */ @@ -1148,7 +1151,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = landed; + local_pos.landed = false; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 5d39c897d4..0e0bc9781a 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,7 +77,7 @@ struct vehicle_local_position_s { double ref_lat; /**< Reference point latitude in degrees */ double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - bool landed; /**< true if vehicle is landed */ + bool landed; /**< true if vehicle is landed TODO: deprecated, use vehicle_land_detected instead (remove this line?)*/ /* Distance to surface */ float dist_bottom; /**< Distance to bottom surface (ground) */ float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ From 546b5727b442ac7520d7ce72e15732378a1a0799 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:33:35 +0100 Subject: [PATCH 10/25] Formatting: Run AStyle to fix indenting --- .../FixedwingLandDetector.cpp | 156 +++++++------- .../fw_land_detector/FixedwingLandDetector.h | 88 ++++---- .../fw_land_detector_main.cpp | 172 +++++++-------- .../MulticopterLandDetector.cpp | 203 +++++++++--------- .../MulticopterLandDetector.h | 95 ++++---- .../mc_land_detector_main.cpp | 30 +-- 6 files changed, 378 insertions(+), 366 deletions(-) diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.cpp b/src/modules/fw_land_detector/FixedwingLandDetector.cpp index 6f5eca3ce0..5fbeb2f45b 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.cpp +++ b/src/modules/fw_land_detector/FixedwingLandDetector.cpp @@ -46,37 +46,37 @@ #include //usleep FixedwingLandDetector::FixedwingLandDetector() : - _landDetectedPub(-1), - _landDetected({0,false}), - - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), + _landDetectedPub(-1), + _landDetected({0, false}), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0), + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), - _taskShouldExit(false), - _taskIsRunning(false) + _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); + //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); - } + _taskShouldExit = true; + close(_landDetectedPub); +} void FixedwingLandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } /** @@ -85,87 +85,89 @@ void FixedwingLandDetector::shutdown() **/ static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if(orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if(!newData) { - return false; - } + if (!newData) { + return false; + } - if(orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } void FixedwingLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + 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; + //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)); + //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) { + _taskIsRunning = true; + _taskShouldExit = false; - //First poll for new data from our subscriptions - updateSubscriptions(); + while (!_taskShouldExit) { - const uint64_t now = hrt_absolute_time(); - bool landDetected = false; + //First poll for new data from our subscriptions + updateSubscriptions(); - //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; + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; - /* 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) { + //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; - //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; - } + /* 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) { - //Publish if land detection state has changed - if(_landDetected.landed != landDetected) { - _landDetected.timestamp = now; - _landDetected.landed = landDetected; + //These conditions need to be stable for a period of time before we trust them + if (now > _landDetectTrigger) { + landDetected = true; + } - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } + } else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } - //Limit loop rate - usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); - } + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = now; + _landDetected.landed = landDetected; - _taskIsRunning = false; - _exit(0); + /* 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; + return _taskIsRunning; } diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h index 9004457400..be9b17b74b 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.h +++ b/src/modules/fw_land_detector/FixedwingLandDetector.h @@ -46,61 +46,63 @@ #include #include -class FixedwingLandDetector { +class FixedwingLandDetector +{ public: - FixedwingLandDetector(); - ~FixedwingLandDetector(); + 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(); + /** + * @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; + /** + * @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 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(); + /** + * @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(); + /** + * @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*/ + //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 */ + 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; + 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 */ + 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 index a9eefc4064..a45a518639 100644 --- a/src/modules/fw_land_detector/fw_land_detector_main.cpp +++ b/src/modules/fw_land_detector/fw_land_detector_main.cpp @@ -61,7 +61,7 @@ static void fw_land_detector_stop(); extern "C" __EXPORT int fw_land_detector_main(int argc, char *argv[]); //Private variables -static FixedwingLandDetector* fw_land_detector_task = nullptr; +static FixedwingLandDetector *fw_land_detector_task = nullptr; static int _landDetectorTaskID = -1; /** @@ -69,7 +69,7 @@ static int _landDetectorTaskID = -1; **/ static void fw_land_detector_deamon_thread(int argc, char *argv[]) { - fw_land_detector_task->landDetectorLoop(); + fw_land_detector_task->landDetectorLoop(); } /** @@ -77,31 +77,32 @@ static void fw_land_detector_deamon_thread(int argc, char *argv[]) **/ static void fw_land_detector_stop() { - if(fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { - errx(1, "not running"); - return; - } + if (fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { + errx(1, "not running"); + return; + } - fw_land_detector_task->shutdown(); + fw_land_detector_task->shutdown(); - //Wait for task to die - int i = 0; - do { - /* wait 20ms */ - usleep(20000); + //Wait for task to die + int i = 0; - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_landDetectorTaskID); - break; - } - } while (fw_land_detector_task->isRunning()); + 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"); + delete fw_land_detector_task; + fw_land_detector_task = nullptr; + _landDetectorTaskID = -1; + warn("fw_land_detector has been stopped"); } /** @@ -109,48 +110,51 @@ static void fw_land_detector_stop() **/ static int fw_land_detector_start() { - if(fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { - errx(1, "already running"); - return -1; - } + 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; - } + //Allocate memory + fw_land_detector_task = new FixedwingLandDetector(); - //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 (fw_land_detector_task == nullptr) { + errx(1, "alloc failed"); + return -1; + } - if (_landDetectorTaskID < 0) { - errx(1, "task start failed: %d", -errno); - 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); - /* 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 (_landDetectorTaskID < 0) { + errx(1, "task start failed: %d", -errno); + return -1; + } - if(hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); - fw_land_detector_stop(); - exit(1); - } - } - printf("\n"); + /* 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 - exit(0); - return 0; + 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; } /** @@ -159,37 +163,37 @@ static int fw_land_detector_start() int fw_land_detector_main(int argc, char *argv[]) { - if (argc < 1) { - warnx("usage: fw_land_detector {start|stop|status}"); - exit(0); - } + 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], "start")) { + fw_land_detector_start(); + } - if (!strcmp(argv[1], "stop")) { - fw_land_detector_stop(); - exit(0); - } + if (!strcmp(argv[1], "stop")) { + fw_land_detector_stop(); + exit(0); + } - if (!strcmp(argv[1], "status")) { - if (fw_land_detector_task) { + 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"); - } + if (fw_land_detector_task->isRunning()) { + warnx("running"); - exit(0); + } else { + errx(1, "exists, but not running"); + } - } else { - errx(1, "not running"); - } - } + exit(0); - warn("usage: fw_land_detector {start|stop|status}"); - return 1; + } else { + errx(1, "not running"); + } + } + + warn("usage: fw_land_detector {start|stop|status}"); + return 1; } diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp index 90b36a795c..d23ff9017a 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -47,35 +47,35 @@ #include //usleep MulticopterLandDetector::MulticopterLandDetector() : - _landDetectedPub(-1), - _landDetected({0,false}), - - _vehicleGlobalPositionSub(-1), - _sensorsCombinedSub(-1), - _waypointSub(-1), - _actuatorsSub(-1), - _armingSub(-1), + _landDetectedPub(-1), + _landDetected({0, false}), - _vehicleGlobalPosition({}), - _sensors({}), - _waypoint({}), - _actuators({}), - _arming({}), + _vehicleGlobalPositionSub(-1), + _sensorsCombinedSub(-1), + _waypointSub(-1), + _actuatorsSub(-1), + _armingSub(-1), - _taskShouldExit(false), - _taskIsRunning(false), - _landTimer(0) + _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); + //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); + _taskShouldExit = true; + close(_landDetectedPub); } /** @@ -84,120 +84,119 @@ MulticopterLandDetector::~MulticopterLandDetector() **/ static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if(orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if(!newData) { - return false; - } + if (!newData) { + return false; + } - if(orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } void MulticopterLandDetector::shutdown() { - _taskShouldExit = true; + _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); + 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; + //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)); + //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) { + //Begin task + _taskIsRunning = true; + _taskShouldExit = false; - //First poll for new data from our subscriptions - updateSubscriptions(); + while (!_taskShouldExit) { - const uint64_t now = hrt_absolute_time(); + //First poll for new data from our subscriptions + updateSubscriptions(); - //only detect landing if the autopilot is actively trying to land - if(!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { - _landTimer = now; - } - else { + const uint64_t now = hrt_absolute_time(); - //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + //only detect landing if the autopilot is actively trying to land + if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + _landTimer = now; - //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; + } else { - //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 we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; - //Check if thrust output is minimal (about half of default) - bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_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; - if(verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector - _landTimer = now; - } + //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 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; + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector + _landTimer = now; + } - /* 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); - } - } + // 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; - //Limit loop rate - usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); - } + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } - _taskIsRunning = false; - _exit(0); + } 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; + return _taskIsRunning; } diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/mc_land_detector/MulticopterLandDetector.h index 1aeaa0d62a..fe18374c3b 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/mc_land_detector/MulticopterLandDetector.h @@ -50,66 +50,67 @@ #include #include -class MulticopterLandDetector { +class MulticopterLandDetector +{ public: - MulticopterLandDetector(); - ~MulticopterLandDetector(); + 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(); + /** + * @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; + /** + * @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 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(); + /** + * @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(); + /** + * @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 */ + //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 */ + 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; + 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 */ + 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; - uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ + 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 index 2992ead99c..6ad3f82a45 100644 --- a/src/modules/mc_land_detector/mc_land_detector_main.cpp +++ b/src/modules/mc_land_detector/mc_land_detector_main.cpp @@ -61,7 +61,7 @@ static void mc_land_detector_stop(); extern "C" __EXPORT int mc_land_detector_main(int argc, char *argv[]); //Private variables -static MulticopterLandDetector* mc_land_detector_task = nullptr; +static MulticopterLandDetector *mc_land_detector_task = nullptr; static int _landDetectorTaskID = -1; /** @@ -77,7 +77,7 @@ static void mc_land_detector_deamon_thread(int argc, char *argv[]) **/ static void mc_land_detector_stop() { - if(mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { + if (mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { errx(1, "not running"); return; } @@ -86,6 +86,7 @@ static void mc_land_detector_stop() //Wait for task to die int i = 0; + do { /* wait 20ms */ usleep(20000); @@ -109,13 +110,14 @@ static void mc_land_detector_stop() **/ static int mc_land_detector_start() { - if(mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { + 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; @@ -123,11 +125,11 @@ static int mc_land_detector_start() //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); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&mc_land_detector_deamon_thread, + nullptr); if (_landDetectorTaskID < 0) { errx(1, "task start failed: %d", -errno); @@ -136,17 +138,19 @@ 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()) { usleep(50000); printf("."); fflush(stdout); - if(hrt_absolute_time() > timeout) { + if (hrt_absolute_time() > timeout) { err(1, "start failed - timeout"); mc_land_detector_stop(); exit(1); } } + printf("\n"); exit(0); @@ -170,16 +174,16 @@ int mc_land_detector_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { mc_land_detector_stop(); - exit(0); + exit(0); } if (!strcmp(argv[1], "status")) { if (mc_land_detector_task) { - if(mc_land_detector_task->isRunning()) { + if (mc_land_detector_task->isRunning()) { warnx("running"); - } - else { + + } else { errx(1, "exists, but not running"); } From 57ed27304a394fb9fec8e2ae4bfca9b2a77d6c7e Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:56:06 +0100 Subject: [PATCH 11/25] HIL: Added land detector to HIL simulation --- src/modules/mavlink/mavlink_receiver.cpp | 21 ++++++++++++++++++--- src/modules/mavlink/mavlink_receiver.h | 3 +++ 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 97108270c5..dfbf00b664 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -92,6 +92,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), status{}, hil_local_pos{}, + hil_land_detector{}, _control_mode{}, _global_pos_pub(-1), _local_pos_pub(-1), @@ -118,6 +119,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _land_detector_pub(-1), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -1353,9 +1355,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_local_pos.xy_global = true; hil_local_pos.z_global = true; - bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - hil_local_pos.landed = landed; - if (_local_pos_pub < 0) { _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); @@ -1364,6 +1363,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) } } + /* land detector */ + { + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? + if(hil_land_detector.landed != landed) { + hil_land_detector.landed = landed; + hil_land_detector.timestamp = hrt_absolute_time(); + + if (_land_detector_pub < 0) { + _land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector); + + } else { + orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector); + } + } + } + /* accelerometer */ { struct accel_report accel; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4afc9b3721..699996860a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -145,6 +146,7 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; + struct vehicle_land_detected_s hil_land_detector; struct vehicle_control_mode_s _control_mode; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; @@ -171,6 +173,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + orb_advert_t _land_detector_pub; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; From 6978ed6a61212859d1c58c769ce75f343bc2e4ca Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:56:36 +0100 Subject: [PATCH 12/25] INAV: Removed all references to land detector logic --- .../position_estimator_inav_main.c | 37 ------------------- 1 file changed, 37 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index b418e3a76b..39294e3c00 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -246,9 +246,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate - //float alt_avg = 0.0f; - //bool landed = true; - //hrt_abstime landed_time = 0; hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; @@ -1068,39 +1065,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - -/* - // detect land - alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; - float alt_disp2 = - z_est[0] - alt_avg; - alt_disp2 = alt_disp2 * alt_disp2; - float land_disp2 = params.land_disp * params.land_disp; - // get actual thrust output - float thrust = armed.armed ? actuator.control[3] : 0.0f; - - if (landed) { - if (alt_disp2 > land_disp2 || thrust > params.land_thr) { - landed = false; - landed_time = 0; - } - } else { - if (alt_disp2 < land_disp2 && thrust < params.land_thr) { - if (landed_time == 0) { - landed_time = t; // land detected first time - - } else { - if (t > landed_time + params.land_t * 1000000.0f) { - landed = true; - landed_time = 0; - } - } - - } else { - landed_time = 0; - } - } -*/ - if (verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { @@ -1151,7 +1115,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = false; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; From 6edb54ff7755fbb30c695984cf53b246ff497141 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:57:27 +0100 Subject: [PATCH 13/25] sdlog2: Added land detector log message (removed from local pos) --- src/modules/sdlog2/sdlog2.c | 12 +++++++++++- src/modules/sdlog2/sdlog2_messages.h | 7 +++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d35b702391..b470eefacc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -981,6 +982,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; + struct vehicle_land_detected_s land_detector; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; struct vision_position_estimate vision_pos; @@ -1016,6 +1018,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; + struct log_LAND_s log_LAND; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; @@ -1082,6 +1085,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int encoders_sub; + int land_detector_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1112,6 +1116,8 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + subs.land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); @@ -1514,13 +1520,17 @@ int sdlog2_thread_main(int argc, char *argv[]) (buf.local_pos.v_z_valid ? 8 : 0) | (buf.local_pos.xy_global ? 16 : 0) | (buf.local_pos.z_global ? 32 : 0); - log_msg.body.log_LPOS.landed = buf.local_pos.landed; log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); log_msg.body.log_LPOS.eph = buf.local_pos.eph; log_msg.body.log_LPOS.epv = buf.local_pos.epv; LOGBUFFER_WRITE_AND_COUNT(LPOS); } + /* --- LAND DETECTED --- */ + if (copy_if_updated(ORB_ID(vehicle_land_detected), subs.land_detector_sub, &buf.land_detector)) { + log_msg.body.log_LAND.landed = buf.land_detector.landed; + } + /* --- LOCAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) { log_msg.msg_type = LOG_LPSP_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5941bfac0e..246f5129f0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -427,6 +427,12 @@ struct log_ENCD_s { /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ #define LOG_AIR1_MSG 40 +/* --- LAND - LAND DETECTOR --- */ +#define LOG_LAND_MSG 41 +struct log_LAND_s { + uint8_t landed; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -495,6 +501,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), + LOG_FORMAT(LAND, "B", "landed"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 2da6439f742f7743adca22ad3a887e936e6c2277 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:57:46 +0100 Subject: [PATCH 14/25] uORB: Removed landed boolean flag from vehicle_local_position topic --- src/modules/uORB/topics/vehicle_local_position.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 0e0bc9781a..8b46c5a3f3 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,7 +77,6 @@ struct vehicle_local_position_s { double ref_lat; /**< Reference point latitude in degrees */ double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - bool landed; /**< true if vehicle is landed TODO: deprecated, use vehicle_land_detected instead (remove this line?)*/ /* Distance to surface */ float dist_bottom; /**< Distance to bottom surface (ground) */ float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ From 784fa9f4699c434671edfd1e4fb48897bea54d8f Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 7 Jan 2015 22:12:48 +0100 Subject: [PATCH 15/25] sdlog2: Removed vehicle_land_detected topic from logging --- src/modules/sdlog2/sdlog2.c | 10 ---------- src/modules/sdlog2/sdlog2_messages.h | 9 +-------- 2 files changed, 1 insertion(+), 18 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b470eefacc..7b7949239e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,7 +72,6 @@ #include #include #include -#include #include #include #include @@ -982,7 +981,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct vehicle_land_detected_s land_detector; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; struct vision_position_estimate vision_pos; @@ -1018,7 +1016,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; - struct log_LAND_s log_LAND; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; @@ -1085,7 +1082,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int encoders_sub; - int land_detector_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1116,7 +1112,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); - subs.land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); @@ -1526,11 +1521,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(LPOS); } - /* --- LAND DETECTED --- */ - if (copy_if_updated(ORB_ID(vehicle_land_detected), subs.land_detector_sub, &buf.land_detector)) { - log_msg.body.log_LAND.landed = buf.land_detector.landed; - } - /* --- LOCAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) { log_msg.msg_type = LOG_LPSP_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 246f5129f0..99f70a948e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -113,7 +113,6 @@ struct log_LPOS_s { int32_t ref_lon; float ref_alt; uint8_t pos_flags; - uint8_t landed; uint8_t ground_dist_flags; float eph; float epv; @@ -427,11 +426,6 @@ struct log_ENCD_s { /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ #define LOG_AIR1_MSG 40 -/* --- LAND - LAND DETECTOR --- */ -#define LOG_LAND_MSG 41 -struct log_LAND_s { - uint8_t landed; -}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -468,7 +462,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), + LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), @@ -501,7 +495,6 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), - LOG_FORMAT(LAND, "B", "landed"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 10a2dd8a346a6a08a0d9b52739f20b842d460646 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 7 Jan 2015 23:19:35 +0100 Subject: [PATCH 16/25] LandDetector: Merged fixedwing and multicopter into one module handling both algorithms --- makefiles/config_px4fmu-v2_default.mk | 7 +- .../FixedwingLandDetector.cpp | 173 --------------- .../fw_land_detector/FixedwingLandDetector.h | 108 ---------- .../fw_land_detector_main.cpp | 199 ----------------- src/modules/fw_land_detector/module.mk | 10 - .../land_detector/FixedwingLandDetector.cpp | 103 +++++++++ .../land_detector/FixedwingLandDetector.h | 88 ++++++++ src/modules/land_detector/LandDetector.cpp | 77 +++++++ src/modules/land_detector/LandDetector.h | 101 +++++++++ .../land_detector/MulticopterLandDetector.cpp | 120 +++++++++++ .../MulticopterLandDetector.h | 48 ++--- .../land_detector_main.cpp} | 81 ++++--- src/modules/land_detector/module.mk | 12 ++ .../MulticopterLandDetector.cpp | 202 ------------------ src/modules/mc_land_detector/module.mk | 10 - 15 files changed, 564 insertions(+), 775 deletions(-) delete mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.cpp delete mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.h delete mode 100644 src/modules/fw_land_detector/fw_land_detector_main.cpp delete mode 100644 src/modules/fw_land_detector/module.mk create mode 100644 src/modules/land_detector/FixedwingLandDetector.cpp create mode 100644 src/modules/land_detector/FixedwingLandDetector.h create mode 100644 src/modules/land_detector/LandDetector.cpp create mode 100644 src/modules/land_detector/LandDetector.h create mode 100644 src/modules/land_detector/MulticopterLandDetector.cpp rename src/modules/{mc_land_detector => land_detector}/MulticopterLandDetector.h (75%) rename src/modules/{mc_land_detector/mc_land_detector_main.cpp => land_detector/land_detector_main.cpp} (65%) create mode 100644 src/modules/land_detector/module.mk delete mode 100644 src/modules/mc_land_detector/MulticopterLandDetector.cpp delete mode 100644 src/modules/mc_land_detector/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 8ef1c333ae..e13a88ca57 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 5fbeb2f45b..0000000000 --- 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 - */ - -#include "FixedwingLandDetector.h" - -#include -#include -#include -#include //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 be9b17b74b..0000000000 --- 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 - */ - -#ifndef __FIXED_WING_LAND_DETECTOR_H__ -#define __FIXED_WING_LAND_DETECTOR_H__ - -#include -#include -#include -#include - -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 a45a518639..0000000000 --- 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 - */ - -#include //usleep -#include -#include -#include -#include -#include -#include //Scheduler -#include //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 894b29a7b5..0000000000 --- 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 0000000000..42735f38c8 --- /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 + */ + +#include "FixedwingLandDetector.h" + +#include +#include + +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 0000000000..faeece6f3c --- /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 + */ + +#ifndef __FIXED_WING_LAND_DETECTOR_H__ +#define __FIXED_WING_LAND_DETECTOR_H__ + +#include "LandDetector.h" +#include +#include + +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 0000000000..a39e538185 --- /dev/null +++ b/src/modules/land_detector/LandDetector.cpp @@ -0,0 +1,77 @@ +#include "LandDetector.h" +#include //usleep +#include + +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 0000000000..6d74656917 --- /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 + */ + +#ifndef __LAND_DETECTOR_H__ +#define __LAND_DETECTOR_H__ + +#include +#include + +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 0000000000..36d92fd798 --- /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 + * @author Morten Lysgaard + */ + +#include "MulticopterLandDetector.h" + +#include +#include + +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 similarity index 75% rename from src/modules/mc_land_detector/MulticopterLandDetector.h rename to src/modules/land_detector/MulticopterLandDetector.h index fe18374c3b..08a8132ba5 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -32,50 +32,27 @@ ****************************************************************************/ /** - * @file MultiCopterLandDetector.h + * @file MulticopterLandDetector.h * Land detection algorithm for multicopters * * @author Johan Jansen - * @author Morten Lysgaard + * @author Morten Lysgaard */ #ifndef __MULTICOPTER_LAND_DETECTOR_H__ #define __MULTICOPTER_LAND_DETECTOR_H__ -#include -#include +#include "LandDetector.h" #include #include #include #include #include -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; - - /** - * @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: /** @@ -83,18 +60,24 @@ protected: **/ void updateSubscriptions(); + /** + * @brief Runs one iteration of the land detection algorithm + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + 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 similarity index 65% rename from src/modules/mc_land_detector/mc_land_detector_main.cpp rename to src/modules/land_detector/land_detector_main.cpp index 6ad3f82a45..2bc89e7526 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 */ @@ -47,42 +47,44 @@ #include //Scheduler #include //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 0000000000..db0e184472 --- /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 d23ff9017a..0000000000 --- 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 - * @author Morten Lysgaard - */ - -#include "MulticopterLandDetector.h" - -#include -#include -#include -#include //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 023d48979a..0000000000 --- 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++ From 9ea086bf2d9b3d9d3d480f6ae83447b9669f3603 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 7 Jan 2015 23:28:20 +0100 Subject: [PATCH 17/25] Astyle: Run astyle to fix code formatting --- makefiles/config_px4fmu-v2_default.mk | 2 +- .../land_detector/FixedwingLandDetector.cpp | 74 +++++++-------- .../land_detector/FixedwingLandDetector.h | 52 +++++------ src/modules/land_detector/LandDetector.cpp | 89 ++++++++++--------- src/modules/land_detector/LandDetector.h | 70 +++++++-------- .../land_detector/MulticopterLandDetector.h | 3 +- .../land_detector/land_detector_main.cpp | 16 ++-- 7 files changed, 154 insertions(+), 152 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13a88ca57..3abebd82fa 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -71,7 +71,7 @@ MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan -MODULES += modules/land_detector +MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 42735f38c8..7df89257f7 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -39,65 +39,65 @@ */ #include "FixedwingLandDetector.h" - + #include #include FixedwingLandDetector::FixedwingLandDetector() : - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0) + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) { - //ctor + //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)); + //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); + 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(); + //First poll for new data from our subscriptions + updateSubscriptions(); - const uint64_t now = hrt_absolute_time(); - bool landDetected = false; + 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; + //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) { + /* 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; - } + //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; - } + } else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } - return landDetected; + return landDetected; } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index faeece6f3c..6737af68a1 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -48,41 +48,41 @@ class FixedwingLandDetector : public LandDetector { public: - FixedwingLandDetector(); + FixedwingLandDetector(); protected: - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - bool update() override; + /** + * @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 Initializes the land detection algorithm + **/ + void initialize() override; - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + /** + * @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 + //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*/ + 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; + 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; + 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 index a39e538185..5029ce185b 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -3,75 +3,76 @@ #include LandDetector::LandDetector() : - _landDetectedPub(-1), - _landDetected({0, false}), - _taskShouldExit(false), - _taskIsRunning(false) + _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); + //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); + _taskShouldExit = true; + close(_landDetectedPub); } void LandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } void LandDetector::start() { - //Make sure this method has not already been called by another thread - if(isRunning()) { - return; - } + //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()) { + //Task is now running, keep doing so until shutdown() has been called + _taskIsRunning = true; + _taskShouldExit = false; - bool landDetected = update(); + while (isRunning()) { - //Publish if land detection state has changed - if (_landDetected.landed != landDetected) { - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = landDetected; + bool landDetected = update(); - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = landDetected; - //Limit loop rate - usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); - } + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } - _taskIsRunning = false; - _exit(0); + //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; + bool newData = false; - //Check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if (!newData) { - return false; - } + if (!newData) { + return false; + } - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 6d74656917..f7120434c4 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -48,54 +48,54 @@ class LandDetector { public: - LandDetector(); - virtual ~LandDetector(); + LandDetector(); + virtual ~LandDetector(); - /** - * @return true if this task is currently running - **/ - inline bool isRunning() const {return _taskIsRunning;} + /** + * @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 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(); + /** + * @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 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 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); + /** + * @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 */ + 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 */ + 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 */ + 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.h b/src/modules/land_detector/MulticopterLandDetector.h index 08a8132ba5..5b4172a36d 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -75,7 +75,8 @@ protected: 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_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = + 2000000; /**< usec that landing conditions have to hold before triggering a land */ private: int _vehicleGlobalPositionSub; /**< notification of global position */ diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 2bc89e7526..7c0ffb82ad 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -51,7 +51,7 @@ #include "MulticopterLandDetector.h" //Function prototypes -static int land_detector_start(const char* mode); +static int land_detector_start(const char *mode); static void land_detector_stop(); /** @@ -110,7 +110,7 @@ static void land_detector_stop() /** * Start new task, fails if it is already running. Returns OK if successful **/ -static int land_detector_start(const char* mode) +static int land_detector_start(const char *mode) { if (land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); @@ -118,15 +118,15 @@ static int land_detector_start(const char* mode) } //Allocate memory - if(!strcmp(mode, "fixedwing")) { + if (!strcmp(mode, "fixedwing")) { land_detector_task = new FixedwingLandDetector(); - } - else if(!strcmp(mode, "multicopter")) { + + } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); - } - else { + + } else { errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); - return -1; + return -1; } //Check if alloc worked From 73c7b44f6a684df80b550a4ce52c883348684045 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 8 Jan 2015 14:22:39 +0100 Subject: [PATCH 18/25] ROMFS: Corrected land detector startup scripts --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index c6947009bb..173c1846b2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -17,7 +17,7 @@ fw_pos_control_l1 start # # Start Land Detector # -fw_land_detector start +land_detector start fixedwing # # Misc apps diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 9c3d6352b3..033b3b6406 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -14,4 +14,4 @@ mc_pos_control start # # Start Land Detector # -mc_land_detector start +land_detector start multicopter From 1356c44f0e27e9ab4d1c2df4cccbe4ac6fb2f1c4 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 14 Jan 2015 17:41:26 +0100 Subject: [PATCH 19/25] LandDetector: Fix land detection algorithm not being initialized --- src/modules/land_detector/LandDetector.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 5029ce185b..2f1310cab0 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -8,10 +8,7 @@ LandDetector::LandDetector() : _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); + //ctor } LandDetector::~LandDetector() @@ -32,6 +29,14 @@ void LandDetector::start() return; } + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + + //Initialize land detection algorithm + initialize(); + //Task is now running, keep doing so until shutdown() has been called _taskIsRunning = true; _taskShouldExit = false; From f1587da4c46e92474687f37ad49fbd003b7c91db Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 14 Jan 2015 17:47:17 +0100 Subject: [PATCH 20/25] MulticopterLandDetector: Detect land even if autopilot is not landing --- .../land_detector/FixedwingLandDetector.cpp | 2 +- .../land_detector/MulticopterLandDetector.cpp | 50 +++++++++---------- .../land_detector/land_detector_main.cpp | 2 +- 3 files changed, 26 insertions(+), 28 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 7df89257f7..38f6c00a95 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -43,7 +43,7 @@ #include #include -FixedwingLandDetector::FixedwingLandDetector() : +FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _vehicleLocalPositionSub(-1), _vehicleLocalPosition({}), _airspeedSub(-1), diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 36d92fd798..81af029fb1 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -44,7 +44,7 @@ #include #include -MulticopterLandDetector::MulticopterLandDetector() : +MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _vehicleGlobalPositionSub(-1), _sensorsCombinedSub(-1), _waypointSub(-1), @@ -86,34 +86,32 @@ bool MulticopterLandDetector::update() //First poll for new data from our subscriptions updateSubscriptions(); + //Only trigger flight conditions if we are armed + if(!_arming.armed) { + return true; + } + 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) { + //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 = _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector _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 false; } return now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME; diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 7c0ffb82ad..9e135b5f17 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -104,7 +104,7 @@ static void land_detector_stop() delete land_detector_task; land_detector_task = nullptr; _landDetectorTaskID = -1; - warn("land_detector has been stopped"); + errx(0, "land_detector has been stopped"); } /** From e40d207311126b05a7fd87739fb72d2ae8d7d500 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 10:06:56 +0100 Subject: [PATCH 21/25] AStyle: Fixed file formatting --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- src/modules/land_detector/LandDetector.cpp | 2 +- src/modules/land_detector/LandDetector.h | 2 +- src/modules/land_detector/MulticopterLandDetector.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 173c1846b2..9e2d1f6ffe 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -21,4 +21,4 @@ land_detector start fixedwing # # Misc apps -# \ No newline at end of file +# diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 2f1310cab0..1dd09b6a67 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -32,7 +32,7 @@ void LandDetector::start() //Advertise the first land detected uORB _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); //Initialize land detection algorithm initialize(); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index f7120434c4..ba15ad498b 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -98,4 +98,4 @@ private: bool _taskIsRunning; /**< task has reached main loop and is currently running */ }; -#endif //__LAND_DETECTOR_H__ \ No newline at end of file +#endif //__LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 81af029fb1..8c25ae5fe7 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -87,7 +87,7 @@ bool MulticopterLandDetector::update() updateSubscriptions(); //Only trigger flight conditions if we are armed - if(!_arming.armed) { + if (!_arming.armed) { return true; } From 92add9cf8003c4fd8b01143626c3a101062dd9dd Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 14:36:04 +0100 Subject: [PATCH 22/25] LandDetector: Externalized algorithm parameters --- .../land_detector/FixedwingLandDetector.cpp | 33 +++++- .../land_detector/FixedwingLandDetector.h | 29 ++++- src/modules/land_detector/LandDetector.h | 3 + .../land_detector/MulticopterLandDetector.cpp | 41 ++++++- .../land_detector/MulticopterLandDetector.h | 33 ++++-- .../land_detector/land_detector_params.c | 105 ++++++++++++++++++ src/modules/land_detector/module.mk | 1 + 7 files changed, 221 insertions(+), 24 deletions(-) create mode 100644 src/modules/land_detector/land_detector_params.c diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 38f6c00a95..0d6f373100 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -44,17 +44,22 @@ #include FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), + _paramHandle(), + _params(), _vehicleLocalPositionSub(-1), _vehicleLocalPosition({}), _airspeedSub(-1), _airspeed({}), + _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), _landDetectTrigger(0) { - //ctor + _paramHandle.maxVelocity = param_find("LAND_FW_VELOCITY_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_FW_MAX_CLIMB_RATE"); + _paramHandle.maxAirSpeed = param_find("LAND_FW_AIR_SPEED_MAX"); } void FixedwingLandDetector::initialize() @@ -85,9 +90,9 @@ bool FixedwingLandDetector::update() _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) { + if (_velocity_xy_filtered < _params.maxVelocity + && _velocity_z_filtered < _params.maxClimbRate + && _airspeed_filtered < _params.maxAirSpeed) { //These conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -96,8 +101,26 @@ bool FixedwingLandDetector::update() } else { //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } return landDetected; } + +void FixedwingLandDetector::updateParameterCache(const bool 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.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + } +} diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 6737af68a1..0e9c092ee0 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -44,6 +44,8 @@ #include "LandDetector.h" #include #include +#include +#include class FixedwingLandDetector : public LandDetector { @@ -66,18 +68,33 @@ protected: **/ 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: + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force); + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxVelocity; + param_t maxClimbRate; + param_t maxAirSpeed; + } _paramHandle; + + struct { + float maxVelocity; + float maxClimbRate; + float maxAirSpeed; + } _params; 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; + int _parameterSub; float _velocity_xy_filtered; float _velocity_z_filtered; diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index ba15ad498b..09db6e4746 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -89,6 +89,9 @@ protected: static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold + before triggering a land */ + protected: orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 8c25ae5fe7..278b0cd51e 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -45,11 +45,14 @@ #include MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), + _paramHandle(), + _params(), _vehicleGlobalPositionSub(-1), _sensorsCombinedSub(-1), _waypointSub(-1), _actuatorsSub(-1), _armingSub(-1), + _parameterSub(-1), _vehicleGlobalPosition({}), _sensors({}), @@ -59,7 +62,10 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _landTimer(0) { - //ctor + _paramHandle.maxRotation = param_find("LAND_MC_MAX_CLIMB_RATE"); + _paramHandle.maxVelocity = param_find("LAND_MC_VELOCITY_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_MC_ROTATION_MAX"); + _paramHandle.maxThrottle = param_find("LAND_MC_THRUST_MAX"); } void MulticopterLandDetector::initialize() @@ -70,6 +76,10 @@ void MulticopterLandDetector::initialize() _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + _parameterSub = orb_subscribe(ORB_ID(parameter_update)); + + //Download parameters + updateParameterCache(true); } void MulticopterLandDetector::updateSubscriptions() @@ -94,19 +104,19 @@ bool MulticopterLandDetector::update() const uint64_t now = hrt_absolute_time(); //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; //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; + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity; //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; + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation; //Check if thrust output is minimal (about half of default) - bool minimalThrust = _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { //Sensed movement, so reset the land detector @@ -114,5 +124,24 @@ bool MulticopterLandDetector::update() return false; } - return now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME; + return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME; +} + +void MulticopterLandDetector::updateParameterCache(const bool 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.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxRotation, &_params.maxRotation); + param_get(_paramHandle.maxThrottle, &_params.maxThrottle); + } } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 5b4172a36d..7bb7f1a907 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -48,6 +48,8 @@ #include #include #include +#include +#include class MulticopterLandDetector : public LandDetector { @@ -70,13 +72,29 @@ protected: **/ 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_TRIGGER_TIME = - 2000000; /**< usec that landing conditions have to hold before triggering a land */ + +private: + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force); + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxClimbRate; + param_t maxVelocity; + param_t maxRotation; + param_t maxThrottle; + } _paramHandle; + + struct { + float maxClimbRate; + float maxVelocity; + float maxRotation; + float maxThrottle; + } _params; private: int _vehicleGlobalPositionSub; /**< notification of global position */ @@ -84,6 +102,7 @@ private: int _waypointSub; int _actuatorsSub; int _armingSub; + int _parameterSub; struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c new file mode 100644 index 0000000000..4aecae5372 --- /dev/null +++ b/src/modules/land_detector/land_detector_params.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * 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 land_detector.c + * Land detector algorithm parameters. + * + * @author Johan Jansen + */ + +#include + +/** + * Multicopter max climb rate + * + * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_MAX_CLIMB_RATE, 0.30f); + +/** + * Multicopter max horizontal velocity + * + * Maximum horizontal velocity allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_VELOCITY_MAX, 1.00f); + +/** + * Multicopter max rotation + * + * Maximum allowed around each axis to trigger a land (radians per second) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_MAX_ROTATION, 0.20f); + +/** + * Multicopter max throttle + * + * Maximum actuator output on throttle before triggering a land + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_MAX_THROTTLE, 0.20f); + +/** + * Fixedwing max horizontal velocity + * + * Maximum horizontal velocity allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_FW_VELOCITY_MAX, 0.20f); + +/** + * Fixedwing max climb rate + * + * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_FW_MAX_CLIMB_RATE, 10.00f); + +/** + * Airspeed max + * + * Maximum airspeed allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_FW_AIRSPEED_MAX, 10.00f); diff --git a/src/modules/land_detector/module.mk b/src/modules/land_detector/module.mk index db0e184472..e08a4b7a81 100644 --- a/src/modules/land_detector/module.mk +++ b/src/modules/land_detector/module.mk @@ -5,6 +5,7 @@ MODULE_COMMAND = land_detector SRCS = land_detector_main.cpp \ + land_detector_params.c \ LandDetector.cpp \ MulticopterLandDetector.cpp \ FixedwingLandDetector.cpp From 510a314386529f95978078d27da25368435d8d90 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 14:58:06 +0100 Subject: [PATCH 23/25] LandDetector: Shorter and less ambiguous param names --- src/modules/land_detector/FixedwingLandDetector.cpp | 6 +++--- .../land_detector/MulticopterLandDetector.cpp | 8 ++++---- src/modules/land_detector/land_detector_params.c | 12 ++++++------ 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 0d6f373100..52084e4c05 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -57,9 +57,9 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _airspeed_filtered(0.0f), _landDetectTrigger(0) { - _paramHandle.maxVelocity = param_find("LAND_FW_VELOCITY_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_FW_MAX_CLIMB_RATE"); - _paramHandle.maxAirSpeed = param_find("LAND_FW_AIR_SPEED_MAX"); + _paramHandle.maxVelocity = param_find("LAND_FW_VEL_XY_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_FW_VEL_Z_MAX"); + _paramHandle.maxAirSpeed = param_find("LAND_FW_AIRSPEED_MAX"); } void FixedwingLandDetector::initialize() diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 278b0cd51e..5371346181 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -62,10 +62,10 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _landTimer(0) { - _paramHandle.maxRotation = param_find("LAND_MC_MAX_CLIMB_RATE"); - _paramHandle.maxVelocity = param_find("LAND_MC_VELOCITY_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_MC_ROTATION_MAX"); - _paramHandle.maxThrottle = param_find("LAND_MC_THRUST_MAX"); + _paramHandle.maxRotation = param_find("LAND_MC_Z_VEL_MAX"); + _paramHandle.maxVelocity = param_find("LAND_MC_XY_VEL_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_MC_ROT_MAX"); + _paramHandle.maxThrottle = param_find("LAND_MC_THR_MAX"); } void MulticopterLandDetector::initialize() diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 4aecae5372..e2acf42b3d 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -48,7 +48,7 @@ * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_MAX_CLIMB_RATE, 0.30f); +PARAM_DEFINE_FLOAT(LAND_MC_Z_VEL_MAX, 0.30f); /** * Multicopter max horizontal velocity @@ -57,7 +57,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_MAX_CLIMB_RATE, 0.30f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_VELOCITY_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LAND_MC_XY_VEL_MAX, 1.00f); /** * Multicopter max rotation @@ -66,7 +66,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_VELOCITY_MAX, 1.00f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_MAX_ROTATION, 0.20f); +PARAM_DEFINE_FLOAT(LAND_MC_ROT_MAX, 0.20f); /** * Multicopter max throttle @@ -75,7 +75,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_MAX_ROTATION, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_MAX_THROTTLE, 0.20f); +PARAM_DEFINE_FLOAT(LAND_MC_THR_MAX, 0.20f); /** * Fixedwing max horizontal velocity @@ -84,7 +84,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_MAX_THROTTLE, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_VELOCITY_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LAND_FW_VEL_XY_MAX, 0.20f); /** * Fixedwing max climb rate @@ -93,7 +93,7 @@ PARAM_DEFINE_FLOAT(LAND_FW_VELOCITY_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_MAX_CLIMB_RATE, 10.00f); +PARAM_DEFINE_FLOAT(LAND_FW_VEL_Z_MAX, 10.00f); /** * Airspeed max From b1127315b453e129753e1f59aff0b0f6906cbaac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Jan 2015 23:26:43 +0100 Subject: [PATCH 24/25] Fixed land detector param names --- .../land_detector/FixedwingLandDetector.cpp | 6 +-- src/modules/land_detector/LandDetector.cpp | 41 +++++++++++++++++++ .../land_detector/MulticopterLandDetector.cpp | 10 ++--- .../land_detector/land_detector_main.cpp | 2 +- .../land_detector/land_detector_params.c | 17 ++++---- 5 files changed, 58 insertions(+), 18 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 52084e4c05..74a197bd23 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -57,9 +57,9 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _airspeed_filtered(0.0f), _landDetectTrigger(0) { - _paramHandle.maxVelocity = param_find("LAND_FW_VEL_XY_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_FW_VEL_Z_MAX"); - _paramHandle.maxAirSpeed = param_find("LAND_FW_AIRSPEED_MAX"); + _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); + _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); + _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); } void FixedwingLandDetector::initialize() diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 1dd09b6a67..61e678b41c 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -1,3 +1,44 @@ +/**************************************************************************** + * + * 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.cpp + * Land detection algorithm + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + #include "LandDetector.h" #include //usleep #include diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 5371346181..cc984f11fa 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -33,7 +33,7 @@ /** * @file MulticopterLandDetector.cpp - * Land detection algorithm + * Land detection algorithm for multicopters * * @author Johan Jansen * @author Morten Lysgaard @@ -62,10 +62,10 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _landTimer(0) { - _paramHandle.maxRotation = param_find("LAND_MC_Z_VEL_MAX"); - _paramHandle.maxVelocity = param_find("LAND_MC_XY_VEL_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_MC_ROT_MAX"); - _paramHandle.maxThrottle = param_find("LAND_MC_THR_MAX"); + _paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX"); + _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); + _paramHandle.maxClimbRate = param_find("LNDMC_ROT_MAX"); + _paramHandle.maxThrottle = param_find("LNDMC_THR_MAX"); } void MulticopterLandDetector::initialize() diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 9e135b5f17..1e43e7ad5d 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -139,7 +139,7 @@ static int land_detector_start(const char *mode) _landDetectorTaskID = task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1024, + 1200, (main_t)&land_detector_deamon_thread, nullptr); diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index e2acf42b3d..0302bc7c14 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin + * Copyright (c) 2014, 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 @@ -48,7 +47,7 @@ * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_Z_VEL_MAX, 0.30f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); /** * Multicopter max horizontal velocity @@ -57,7 +56,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_Z_VEL_MAX, 0.30f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_XY_VEL_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); /** * Multicopter max rotation @@ -66,7 +65,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_XY_VEL_MAX, 1.00f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_ROT_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 0.20f); /** * Multicopter max throttle @@ -75,7 +74,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_ROT_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_THR_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); /** * Fixedwing max horizontal velocity @@ -84,7 +83,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_THR_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_VEL_XY_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.20f); /** * Fixedwing max climb rate @@ -93,7 +92,7 @@ PARAM_DEFINE_FLOAT(LAND_FW_VEL_XY_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_VEL_Z_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f); /** * Airspeed max @@ -102,4 +101,4 @@ PARAM_DEFINE_FLOAT(LAND_FW_VEL_Z_MAX, 10.00f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_AIRSPEED_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f); From 378dcc53d63a18811f17ab6f60c66b8315d8db25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Jan 2015 23:32:00 +0100 Subject: [PATCH 25/25] Code style: minor comment styling --- .../land_detector/FixedwingLandDetector.cpp | 13 ++++++------ src/modules/land_detector/LandDetector.cpp | 18 ++++++++--------- .../land_detector/MulticopterLandDetector.cpp | 20 +++++++++---------- 3 files changed, 24 insertions(+), 27 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 74a197bd23..8e5bcf7bae 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -51,7 +51,6 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _airspeedSub(-1), _airspeed({}), _parameterSub(-1), - _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), @@ -64,7 +63,7 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), void FixedwingLandDetector::initialize() { - //Subscribe to local position and airspeed data + // Subscribe to local position and airspeed data _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); } @@ -77,30 +76,30 @@ void FixedwingLandDetector::updateSubscriptions() bool FixedwingLandDetector::update() { - //First poll for new data from our subscriptions + // 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? + // 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 */ + // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity && _velocity_z_filtered < _params.maxClimbRate && _airspeed_filtered < _params.maxAirSpeed) { - //These conditions need to be stable for a period of time before we trust them + // 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 + // reset land detect trigger _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 61e678b41c..a4fbb9861f 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -49,7 +49,7 @@ LandDetector::LandDetector() : _taskShouldExit(false), _taskIsRunning(false) { - //ctor + // ctor } LandDetector::~LandDetector() @@ -65,20 +65,20 @@ void LandDetector::shutdown() void LandDetector::start() { - //Make sure this method has not already been called by another thread + // make sure this method has not already been called by another thread if (isRunning()) { return; } - //Advertise the first land detected uORB + // advertise the first land detected uORB _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = false; _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); - //Initialize land detection algorithm + // initialize land detection algorithm initialize(); - //Task is now running, keep doing so until shutdown() has been called + // task is now running, keep doing so until shutdown() has been called _taskIsRunning = true; _taskShouldExit = false; @@ -86,16 +86,16 @@ void LandDetector::start() bool landDetected = update(); - //Publish if land detection state has changed + // publish if land detection state has changed if (_landDetected.landed != landDetected) { _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = landDetected; - /* publish the land detected broadcast */ + // publish the land detected broadcast orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); } - //Limit loop rate + // limit loop rate usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); } @@ -107,7 +107,7 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void { bool newData = false; - //Check if there is new data to grab + // check if there is new data to grab if (orb_check(handle, &newData) != OK) { return false; } diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index cc984f11fa..277cb9363f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -53,13 +53,11 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _actuatorsSub(-1), _armingSub(-1), _parameterSub(-1), - _vehicleGlobalPosition({}), _sensors({}), _waypoint({}), _actuators({}), _arming({}), - _landTimer(0) { _paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX"); @@ -70,7 +68,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), void MulticopterLandDetector::initialize() { - //Subscribe to position, attitude, arming and velocity changes + // 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)); @@ -78,7 +76,7 @@ void MulticopterLandDetector::initialize() _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); - //Download parameters + // download parameters updateParameterCache(true); } @@ -93,33 +91,33 @@ void MulticopterLandDetector::updateSubscriptions() bool MulticopterLandDetector::update() { - //First poll for new data from our subscriptions + // first poll for new data from our subscriptions updateSubscriptions(); - //Only trigger flight conditions if we are armed + // only trigger flight conditions if we are armed if (!_arming.armed) { return true; } const uint64_t now = hrt_absolute_time(); - //Check if we are moving vertically + // check if we are moving vertically bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; - //Check if we are moving horizontally + // check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity; - //Next look if all rotation angles are not moving + // 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]) > _params.maxRotation; - //Check if thrust output is minimal (about half of default) + // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector + // sensed movement, so reset the land detector _landTimer = now; return false; }