LandDetector: Externalized algorithm parameters

7 files changed, 221 insertions, 24 deletions

diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
index 38f6c00a9..0d6f37310 100644
--- a/src/modules/land_detector/FixedwingLandDetector.cpp
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -44,17 +44,22 @@
 #include <drivers/drv_hrt.h>
 
 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, &paramUpdate);
+	}
+
+	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 6737af68a..0e9c092ee 100644
--- a/src/modules/land_detector/FixedwingLandDetector.h
+++ b/src/modules/land_detector/FixedwingLandDetector.h
@@ -44,6 +44,8 @@
 #include "LandDetector.h"
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
 
 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 ba15ad498..09db6e474 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 8c25ae5fe..278b0cd51 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -45,11 +45,14 @@
 #include <drivers/drv_hrt.h>
 
 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, &paramUpdate);
+	}
+
+	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 5b4172a36..7bb7f1a90 100644
--- a/src/modules/land_detector/MulticopterLandDetector.h
+++ b/src/modules/land_detector/MulticopterLandDetector.h
@@ -48,6 +48,8 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
 
 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 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 <anton.babushkin@me.com>
+ *
+ * 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 <jnsn.johan@gmail.com>
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * 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 db0e18447..e08a4b7a8 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