Land detector: Detect ground contact separately

This allows to detect ground contact before concluding the system is landed. This allows to disable some parts of the horizontal control system and only control the vertical position to avoid tipping over.
;

src/modules/land_detector/LandDetector.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2017 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
@@ -31,12 +31,13 @@
  *
  ****************************************************************************/
 
-/*
+/**
  * @file LandDetector.h
 Land detector interface for multicopter, fixedwing and VTOL implementations.
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
  * @author Julian Oes <julian@oes.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #pragma once
@@ -56,13 +57,14 @@ public:
 	enum class LandDetectionState {
 		FLYING = 0,
 		LANDED = 1,
-		FREEFALL = 2
+		FREEFALL = 2,
+		GROUND_CONTACT = 3
 	};
 
 	LandDetector();
 	virtual ~LandDetector();
 
-	/*
+	/**
 	 * @return true if this task is currently running.
 	 */
 	inline bool is_running() const
@@ -71,7 +73,7 @@ public:
 	}
 
 
-	/*
+	/**
 	 * @return current state.
 	 */
 	LandDetectionState get_state() const
@@ -79,57 +81,62 @@ public:
 		return _state;
 	}
 
-	/*
+	/**
 	 * Tells the task that it should exit.
 	 */
 	void stop();
 
-	/*
+	/**
 	 * Get the work queue going.
 	 */
 	int start();
 
 protected:
-	/*
+	/**
 	 * Called once to initialize uORB topics.
 	 */
 	virtual void _initialize_topics() = 0;
 
-	/*
+	/**
 	 * Update uORB topics.
 	 */
 	virtual void _update_topics() = 0;
 
 
-	/*
+	/**
 	 * Update parameters.
 	 */
 	virtual void _update_params() = 0;
 
-	/*
+	/**
 	 * @return true if UAV is in a landed state.
 	 */
 	virtual bool _get_landed_state() = 0;
 
-	/*
+	/**
+	 * @return true if UAV is touching ground but not landed
+	 */
+	bool _get_ground_contact_state() { return _get_landed_state(); }
+
+	/**
 	 * @return true if UAV is in free-fall state.
 	 */
 	virtual bool _get_freefall_state() = 0;
 
-	/*
+	/**
 	 * Convenience function for polling uORB subscriptions.
 	 *
 	 * @return true if there was new data and it was successfully copied
 	 */
 	static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer);
 
-	/* Run main land detector loop at this rate in Hz. */
+	/** Run main land detector loop at this rate in Hz. */
 	static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50;
 
-	/* Time in us that landing conditions have to hold before triggering a land. */
+	/** Time in us that landing conditions have to hold before triggering a land. */
 	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 2000000;
 
-	/* Time interval in us in which wider acceptance thresholds are used after arming. */
+	/** Time interval in us in which wider acceptance thresholds are used after arming. */
 	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2000000;
 
 	orb_advert_t _landDetectedPub;

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -136,7 +136,7 @@ bool MulticopterLandDetector::_get_freefall_state()
 	return (acc_norm < _params.freefall_acc_threshold);	//true if we are currently falling
 }
 
-bool MulticopterLandDetector::_get_landed_state()
+bool MulticopterLandDetector::_get_ground_contact_state()
 {
 	// Time base for this function
 	const uint64_t now = hrt_absolute_time();
@@ -217,6 +217,23 @@ bool MulticopterLandDetector::_get_landed_state()
 	// an accurate in-air indication.
 	bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor;
 
+	if (!minimalThrust || verticalMovement) {
+		return false;
+	}
+
+	return true;
+}
+
+bool MulticopterLandDetector::_get_landed_state()
+{
+	float armThresholdFactor = 1.0f;
+
+	// 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_US) {
+		armThresholdFactor = 2.5f;
+	}
+
 	// Check if we are moving horizontally.
 	bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
 					+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
@@ -229,7 +246,7 @@ bool MulticopterLandDetector::_get_landed_state()
 			(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
 
 
-	if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
+	if (!_get_ground_contact_state() || rotating || horizontalMovement) {
 		// Sensed movement or thottle high, so reset the land detector.
 		return false;
 	}

src/modules/land_detector/MulticopterLandDetector.h

@@ -71,6 +71,8 @@ protected:
 
 	virtual bool _get_landed_state() override;
 
+	bool _get_ground_contact_state();
+
 	virtual bool _get_freefall_state() override;
 private:
 

src/modules/land_detector/VtolLandDetector.cpp

@@ -71,6 +71,11 @@ void VtolLandDetector::_update_topics()
 	_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
 }
 
+bool VtolLandDetector::_get_ground_contact_state()
+{
+	return MulticopterLandDetector::_get_ground_contact_state();
+}
+
 bool VtolLandDetector::_get_landed_state()
 {
 	// this is returned from the mutlicopter land detector

src/modules/land_detector/VtolLandDetector.h

@@ -62,6 +62,8 @@ protected:
 
 	virtual bool _get_landed_state() override;
 
+	bool _get_ground_contact_state();
+
 	virtual bool _get_freefall_state() override;
 
 private: