From 98d51179ad5eed10010d9dfe5a1e6d4c96cce999 Mon Sep 17 00:00:00 2001
From: Emmanuel Roussel <rousselmanu@gmail.com>
Date: Thu, 28 Jan 2016 11:09:10 +0100
Subject: [PATCH] Added time to trigger free-fall as a parameter

---
 src/modules/land_detector/LandDetector.h       |  5 +----
 .../land_detector/MulticopterLandDetector.cpp  | 18 ++++++++++++------
 .../land_detector/MulticopterLandDetector.h    |  2 ++
 .../land_detector/land_detector_params.c       | 14 +++++++++++++-
 4 files changed, 28 insertions(+), 11 deletions(-)

diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h
index 0c3f28ff73..5e2bd1920c 100644
--- a/src/modules/land_detector/LandDetector.h
+++ b/src/modules/land_detector/LandDetector.h
@@ -108,10 +108,7 @@ protected:
 
 	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000;  /**< usec that landing conditions have to hold
                                                                           before triggering a land */
-	static constexpr uint64_t FREEFALL_DETECTOR_TRIGGER_TIME = 300000;  /**< usec that freefall conditions have to hold
-                                                                          before triggering a freefall */
-	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME =
-		2000000;	/**< time interval in which wider acceptance thresholds are used after arming */
+	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 2000000;	/**< time interval in which wider acceptance thresholds are used after arming */
 
 protected:
 	orb_advert_t				_landDetectedPub;		/**< publisher for position in local frame */
diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index d7594487d7..6b928067fb 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -70,6 +70,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
 	_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
 	_paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR");
+	_paramHandle.ff_trigger_time_ms = param_find("LNDMC_FFALL_TRIG");
 }
 
 void MulticopterLandDetector::initialize()
@@ -123,17 +124,21 @@ bool MulticopterLandDetector::get_freefall_state()
 	for (int i = 0; i < 3; i++) {
 		acc_norm += _sensors.accelerometer_m_s2[i] * _sensors.accelerometer_m_s2[i];
 	}
-	acc_norm = sqrtf(acc_norm);
+	acc_norm = sqrtf(acc_norm);	//norm of specific force
 
+	int32_t elapsed_time_ms = (now - _freefallTimer)/1000;
 	bool freefall = (acc_norm < _params.acc_threshold_m_s2);
-	if(!freefall){
+	if (!freefall || _freefallTimer == 0) {	//reset timer if uav not falling
 		_freefallTimer = now;
 		return false;
-	}else{
-		PX4_WARN("[freefalldetector] FREE FALL !! \tacc_norm:\t%8.4f", (double)acc_norm);
+	}/* else {	//DEBUG
+		PX4_WARN("[freefalldetector] FREE FALL !! \tacc_norm:\t%8.4f, \telapsed time:\t%8.4f ms", (double)acc_norm, (double)elapsed_time_ms);
 	}
-
-	return (now - _freefallTimer) > FREEFALL_DETECTOR_TRIGGER_TIME;
+	bool falling = elapsed_time_ms > _params.ff_trigger_time_ms;
+	if (falling) {
+		PX4_WARN("[freefalldetector] TRIGGERED");
+	}*/
+	return elapsed_time_ms > _params.ff_trigger_time_ms;
 }
 
 bool MulticopterLandDetector::get_landed_state()
@@ -219,6 +224,7 @@ void MulticopterLandDetector::updateParameterCache(const bool force)
 		_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
 		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
 		param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
+		param_get(_paramHandle.ff_trigger_time_ms, &_params.ff_trigger_time_ms);
 	}
 }
 
diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h
index 69970eb5a4..cc0fc3aff2 100644
--- a/src/modules/land_detector/MulticopterLandDetector.h
+++ b/src/modules/land_detector/MulticopterLandDetector.h
@@ -103,6 +103,7 @@ private:
 		param_t maxRotation;
 		param_t maxThrottle;
 		param_t acc_threshold_m_s2;
+		param_t ff_trigger_time_ms;
 	}		_paramHandle;
 
 	struct {
@@ -111,6 +112,7 @@ private:
 		float maxRotation_rad_s;
 		float maxThrottle;
 		float acc_threshold_m_s2;
+		uint32_t ff_trigger_time_ms;
 	} _params;
 
 private:
diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c
index 0be9116098..dfbeaddc69 100644
--- a/src/modules/land_detector/land_detector_params.c
+++ b/src/modules/land_detector/land_detector_params.c
@@ -93,7 +93,19 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f);
  *
  * @group Land Detector
  */
-PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 1.0f);
+PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 2.0f);
+
+/**
+ * Multicopter free-fall trigger time
+ *
+ * milliseconds that freefall conditions have to hold before triggering a freefall
+ *
+ * @min 20
+ * @max 5000
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_INT32(LNDMC_FFALL_TRIG, 200);	//minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h
 
 /**
  * Fixedwing max horizontal velocity