LandDetector:

- constructor initalization fix
- set trigger time for ground contact hysteresis
- updated ground_contact_state logic
MulticopterLandDetector:
- added hysteresis for ground_contact
VtolLandDetector:
- get_ground_contact_state function that return the one form MultcopterLandDetector
FixedWingLandDetector:
- get_ground_contact_state with a return false: requires implementation

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -97,6 +97,11 @@ bool FixedwingLandDetector::_get_freefall_state()
 	// TODO
 	return false;
 }
+bool FixedwingLandDetector::_get_ground_contact_state(){
+
+	// TODO
+	return false;
+}
 
 bool FixedwingLandDetector::_get_landed_state()
 {

src/modules/land_detector/FixedwingLandDetector.h

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

src/modules/land_detector/LandDetector.cpp

@@ -52,14 +52,18 @@ namespace land_detector
 LandDetector::LandDetector() :
 	_landDetectedPub(nullptr),
 	_landDetected{0, false, false},
+	_parameterSub(0),
+	_state{},
 	_freefall_hysteresis(false),
 	_landed_hysteresis(true),
+	_ground_contact_hysteresis(false),
 	_taskShouldExit(false),
 	_taskIsRunning(false),
 	_work{}
 {
-	// Use Trigger time when transitioning from in-air (false) to landed (true).
+	// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
 	_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
+	_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US);
 }
 
 LandDetector::~LandDetector()
@@ -98,6 +102,7 @@ void LandDetector::_cycle()
 		_landDetected.timestamp = hrt_absolute_time();
 		_landDetected.landed = false;
 		_landDetected.freefall = false;
+		_landDetected.ground_contact = false;
 
 		// Initialize uORB topics.
 		_initialize_topics();
@@ -116,15 +121,18 @@ void LandDetector::_cycle()
 
 	bool landDetected = (_state == LandDetectionState::LANDED);
 	bool freefallDetected = (_state == LandDetectionState::FREEFALL);
+	bool groundContactDetected = ( _state == LandDetectionState::GROUND_CONTACT);
 
 	// Only publish very first time or when the result has changed.
 	if ((_landDetectedPub == nullptr) ||
 	    (_landDetected.landed != landDetected) ||
-	    (_landDetected.freefall != freefallDetected)) {
+	    (_landDetected.freefall != freefallDetected) ||
+		(_landDetected.ground_contact != groundContactDetected)) {
 
 		_landDetected.timestamp = hrt_absolute_time();
 		_landDetected.landed = (_state == LandDetectionState::LANDED);
 		_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
+		_landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT);
 
 		int instance;
 		orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected,
@@ -160,15 +168,26 @@ void LandDetector::_check_params(const bool force)
 
 void LandDetector::_update_state()
 {
+	/* ground contact and landed can be true simultaneously but only one state can be true at a particular time
+	 * with higher priority for landed */
+	bool groundContact = _get_ground_contact_state();
 	bool landed = _get_landed_state();
+	bool freefall = _get_freefall_state();
+
+	_ground_contact_hysteresis.set_state_and_update(groundContact);
 	_landed_hysteresis.set_state_and_update(landed);
-	_freefall_hysteresis.set_state_and_update(_get_freefall_state());
+	_freefall_hysteresis.set_state_and_update(freefall);
 
 	if (_freefall_hysteresis.get_state()) {
 		_state = LandDetectionState::FREEFALL;
 
 	} else if (_landed_hysteresis.get_state()) {
 		_state = LandDetectionState::LANDED;
+		/* need to set ground_contact_state to false */
+		_ground_contact_hysteresis.set_state_and_update(false);
+
+	} else if (_ground_contact_hysteresis.get_state()){
+		_state = LandDetectionState::GROUND_CONTACT;
 
 	} else {
 		_state = LandDetectionState::FLYING;

src/modules/land_detector/LandDetector.h

@@ -116,7 +116,7 @@ protected:
 	/**
 	 * @return true if UAV is touching ground but not landed
 	 */
-	bool _get_ground_contact_state() { return _get_landed_state(); }
+	virtual bool _get_ground_contact_state()  = 0;
 
 	/**
 	 * @return true if UAV is in free-fall state.
@@ -134,7 +134,10 @@ protected:
 	static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50;
 
 	/** Time in us that landing conditions have to hold before triggering a land. */
-	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 2000000;
+	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 1500000;
+
+	/** Time in us that ground contact condition have to hold before triggering contact ground */
+	static constexpr uint64_t GROUND_CONTACT_TRIGGER_TIME_US = 1000000;
 
 	/** Time interval in us in which wider acceptance thresholds are used after arming. */
 	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2000000;
@@ -148,6 +151,8 @@ protected:
 
 	systemlib::Hysteresis _freefall_hysteresis;
 	systemlib::Hysteresis _landed_hysteresis;
+	systemlib::Hysteresis _ground_contact_hysteresis;
+
 
 private:
 	static void _cycle_trampoline(void *arg);

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -112,7 +112,7 @@ void MulticopterLandDetector::_update_params()
 	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
 	param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold);
 	param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time);
-	_freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.freefall_trigger_time);
+	_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time));
 }
 
 

src/modules/land_detector/MulticopterLandDetector.h

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

src/modules/land_detector/VtolLandDetector.h

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