MulticopterLandDetector: Detect land even if autopilot is not landing

3 files changed, 23 insertions, 25 deletions

diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
index 7df89257f..38f6c00a9 100644
--- a/src/modules/land_detector/FixedwingLandDetector.cpp
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -43,7 +43,7 @@
 #include <cmath>
 #include <drivers/drv_hrt.h>
 
-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 36d92fd79..81af029fb 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -44,7 +44,7 @@
 #include <cmath>
 #include <drivers/drv_hrt.h>
 
-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();
 
-	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 {
+	//Only trigger flight conditions if we are armed
+	if(!_arming.armed) {
+		return true;
+	}
 
-		//Check if we are moving vertically
-		bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX;
+	const uint64_t now = hrt_absolute_time();
 
-		//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;
+	//Check if we are moving vertically
+	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_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 we are moving horizontally
+	bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
+					+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX;
 
-		//Check if thrust output is minimal (about half of default)
-		bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_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;
 
-		if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
-			//Sensed movement, so reset the land detector
-			_landTimer = now;
-		}
+	//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;
+		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 7c0ffb82a..9e135b5f1 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");
 }
 
 /**