Make land detector more robust during initial spool-up

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -49,7 +49,11 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
 	_vehicleLocalPositionSub(-1),
 	_vehicleLocalPosition({}),
 	_airspeedSub(-1),
-	_airspeed({}),
+	_vehicleStatusSub(-1),
+	_armingSub(-1),
+	_airspeed{},
+	_vehicleStatus{},
+	_arming{},
 	_parameterSub(-1),
 	_velocity_xy_filtered(0.0f),
 	_velocity_z_filtered(0.0f),
@@ -66,6 +70,8 @@ void FixedwingLandDetector::initialize()
 	// Subscribe to local position and airspeed data
 	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
+	_vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status));
+	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 
 	updateParameterCache(true);
 }
@@ -74,6 +80,8 @@ void FixedwingLandDetector::updateSubscriptions()
 {
 	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
 	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+	orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus);
+	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
 }
 
 bool FixedwingLandDetector::update()
@@ -81,6 +89,11 @@ bool FixedwingLandDetector::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();
 	bool landDetected = false;
 

src/modules/land_detector/FixedwingLandDetector.h

@@ -44,7 +44,9 @@
 #include "LandDetector.h"
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
 #include <systemlib/param/param.h>
 
 class FixedwingLandDetector : public LandDetector
@@ -90,11 +92,15 @@ private:
 	} _params;
 
 private:
-	int                                     _vehicleLocalPositionSub;   /**< notification of local position */
+	int					_vehicleLocalPositionSub;	/**< notification of local position */
-	struct vehicle_local_position_s         _vehicleLocalPosition;      /**< the result from local position subscription */
+	struct vehicle_local_position_s		_vehicleLocalPosition;		/**< the result from local position subscription */
-	int                                     _airspeedSub;
+	int					_airspeedSub;
-	struct airspeed_s                       _airspeed;
+	int					_vehicleStatusSub;
-	int 									_parameterSub;
+	int					_armingSub;
+	struct airspeed_s			_airspeed;
+	struct vehicle_status_s 		_vehicleStatus;
+	struct actuator_armed_s			_arming;
+	int 					_parameterSub;
 
 	float _velocity_xy_filtered;
 	float _velocity_z_filtered;

src/modules/land_detector/LandDetector.cpp

@@ -46,8 +46,9 @@
 LandDetector::LandDetector() :
 	_landDetectedPub(-1),
 	_landDetected({0, false}),
-	      _taskShouldExit(false),
+	_arming_time(0),
-	      _taskIsRunning(false)
+	_taskShouldExit(false),
+	_taskIsRunning(false)
 {
 	// ctor
 }

src/modules/land_detector/LandDetector.h

@@ -96,10 +96,11 @@ 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 LAND_DETECTOR_ARM_PHASE_TIME = 1000000;	/**< time interval in which wider acceptance thresholds are used after arming */
 
-protected:
+	orb_advert_t				_landDetectedPub;		/**< publisher for position in local frame */
-	orb_advert_t                            _landDetectedPub;           /**< publisher for position in local frame */
+	struct vehicle_land_detected_s		_landDetected;			/**< local vehicle position */
-	struct vehicle_land_detected_s          _landDetected;              /**< local vehicle position */
+	uint64_t				_arming_time;			/**< timestamp of arming time */
 
 private:
 	bool _taskShouldExit;                                               /**< true if it is requested that this task should exit */

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -53,12 +53,12 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_actuatorsSub(-1),
 	_armingSub(-1),
 	_parameterSub(-1),
-    _attitudeSub(-1),
+	_attitudeSub(-1),
-	_vehicleGlobalPosition({}),
+	_vehicleGlobalPosition{},
-	_vehicleStatus({}),
+	_vehicleStatus{},
-	_actuators({}),
+	_actuators{},
-	_arming({}),
+	_arming{},
-    _vehicleAttitude({}),
+	_vehicleAttitude{},
 	_landTimer(0)
 {
 	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
@@ -97,7 +97,10 @@ bool MulticopterLandDetector::update()
 
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
+		_arming_time = 0;
 		return true;
+	} else if (_arming_time == 0) {
+		_arming_time = hrt_absolute_time();
 	}
 
 	// return status based on armed state if no position lock is available
@@ -110,8 +113,18 @@ bool MulticopterLandDetector::update()
 
 	const uint64_t now = hrt_absolute_time();
 
-	// check if we are moving vertically
+	float armThresholdFactor = 1.0f;
-	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
+
+	// Widen acceptance thresholds for landed state right after arming
+	// so that motor spool-up and other effects do not trigger false negatives
+	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
+		armThresholdFactor = 2.5f;
+	}
+
+	// check if we are moving vertically - this might see a spike after arming due to
+	// throttle-up vibration. If accelerating fast the throttle thresholds will still give
+	// an accurate in-air indication
+	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor;
 
 	// check if we are moving horizontally
 	bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
@@ -119,9 +132,11 @@ bool MulticopterLandDetector::update()
 					&& _vehicleStatus.condition_global_position_valid;
 
 	// next look if all rotation angles are not moving
-	bool rotating = fabsf(_vehicleAttitude.rollspeed)  > _params.maxRotation ||
+	float maxRotationScaled = _params.maxRotation * armThresholdFactor;
-				    fabsf(_vehicleAttitude.pitchspeed) > _params.maxRotation ||
+
-        			fabsf(_vehicleAttitude.yawspeed)   > _params.maxRotation;
+	bool rotating = (fabsf(_vehicleAttitude.rollspeed)  > maxRotationScaled) ||
+				    (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
+				(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
 
 	// check if thrust output is minimal (about half of default)
 	bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;

src/modules/land_detector/MulticopterLandDetector.h

@@ -97,20 +97,20 @@ private:
 	} _params;
 
 private:
-	int _vehicleGlobalPositionSub;                                      /**< notification of global position */
+	int _vehicleGlobalPositionSub;						/**< notification of global position */
 	int _vehicleStatusSub;
 	int _actuatorsSub;
 	int _armingSub;
 	int _parameterSub;
 	int _attitudeSub;
 
-	struct vehicle_global_position_s          _vehicleGlobalPosition;   /**< the result from global position subscription */
+	struct vehicle_global_position_s	_vehicleGlobalPosition;		/**< the result from global position subscription */
-	struct vehicle_status_s 			      _vehicleStatus;
+	struct vehicle_status_s 		_vehicleStatus;
-	struct actuator_controls_s                _actuators;
+	struct actuator_controls_s		_actuators;
-	struct actuator_armed_s                   _arming;
+	struct actuator_armed_s			_arming;
-	struct vehicle_attitude_s                 _vehicleAttitude;
+	struct vehicle_attitude_s		_vehicleAttitude;
 
-	uint64_t _landTimer;                                                /**< timestamp in microseconds since a possible land was detected*/
+	uint64_t _landTimer;							/**< timestamp in microseconds since a possible land was detected*/
 };
 
 #endif //__MULTICOPTER_LAND_DETECTOR_H__