From 92add9cf8003c4fd8b01143626c3a101062dd9dd Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 14:36:04 +0100 Subject: LandDetector: Externalized algorithm parameters --- src/modules/land_detector/land_detector_params.c | 105 +++++++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 src/modules/land_detector/land_detector_params.c (limited to 'src/modules/land_detector/land_detector_params.c') 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 000000000..4aecae537 --- /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); -- cgit v1.2.3 From 510a314386529f95978078d27da25368435d8d90 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 14:58:06 +0100 Subject: LandDetector: Shorter and less ambiguous param names --- src/modules/land_detector/FixedwingLandDetector.cpp | 6 +++--- src/modules/land_detector/MulticopterLandDetector.cpp | 8 ++++---- src/modules/land_detector/land_detector_params.c | 12 ++++++------ 3 files changed, 13 insertions(+), 13 deletions(-) (limited to '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 0d6f37310..52084e4c0 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 278b0cd51..537134618 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 4aecae537..e2acf42b3 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 -- cgit v1.2.3 From b1127315b453e129753e1f59aff0b0f6906cbaac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Jan 2015 23:26:43 +0100 Subject: Fixed land detector param names --- .../land_detector/FixedwingLandDetector.cpp | 6 ++-- src/modules/land_detector/LandDetector.cpp | 41 ++++++++++++++++++++++ .../land_detector/MulticopterLandDetector.cpp | 10 +++--- src/modules/land_detector/land_detector_main.cpp | 2 +- src/modules/land_detector/land_detector_params.c | 17 +++++---- 5 files changed, 58 insertions(+), 18 deletions(-) (limited to '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 52084e4c0..74a197bd2 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 1dd09b6a6..61e678b41 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 537134618..cc984f11f 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 9e135b5f1..1e43e7ad5 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 e2acf42b3..0302bc7c1 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); -- cgit v1.2.3