rover: update land detector

src/modules/land_detector/RoverLandDetector.cpp

@@ -51,41 +51,33 @@ bool RoverLandDetector::_get_ground_contact_state()
 
 bool RoverLandDetector::_get_landed_state()
 {
-	_update_topics();
+	// If Landing has been requested then say we have landed.
-	const float distance_to_home = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
-				       _home_position(0), _home_position(1));
-
 	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
 	    || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
-		return true; // If Landing has been requested then say we have landed.
+		return true;
 
-	} else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
-		   && distance_to_home < _param_nav_acc_rad.get() && _param_rtl_land_delay.get() > -FLT_EPSILON) {
-		return true; // If the rover reaches the home position during RTL we say we have landed.
-
-	} else {
-		return !_armed;  // If we are armed we are not landed.
 	}
-}
 
-void RoverLandDetector::_update_topics()
+	// If we are in RTL and have reached the last valid waypoint then we are landed.
-{
+	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
-	if (_vehicle_global_position_sub.updated()) {
 		vehicle_global_position_s vehicle_global_position{};
 		_vehicle_global_position_sub.copy(&vehicle_global_position);
-		_curr_pos = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
+		position_setpoint_triplet_s position_setpoint_triplet{};
+		_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
+
+		const float distance_to_curr_wp = get_distance_to_next_waypoint(vehicle_global_position.lat,
+						  vehicle_global_position.lon,
+						  position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
+		return distance_to_curr_wp < _param_nav_acc_rad.get() && !position_setpoint_triplet.next.valid;
+
 	}
 
-	if (_home_position_sub.updated()) {
+	return !_armed;  // If we are armed we are not landed.
-		home_position_s home_position{};
-		_home_position_sub.copy(&home_position);
-		_home_position = matrix::Vector2d(home_position.lat, home_position.lon);
-	}
 }
 
 void RoverLandDetector::_set_hysteresis_factor(const int factor)
 {
-	_landed_hysteresis.set_hysteresis_time_from(true, 0_ms);
+	_landed_hysteresis.set_hysteresis_time_from(true, 500_ms);
 }
 
 } // namespace land_detector

src/modules/land_detector/RoverLandDetector.h

@@ -33,7 +33,7 @@
 
 /**
  * @file RoverLandDetector.h
- * Land detection implementation for VTOL also called hybrids.
+ * Land detection implementation for rovers.
  *
  * @author Roman Bapst <bapstr@gmail.com>
  * @author Julian Oes <julian@oes.ch>
@@ -44,7 +44,7 @@
 #include "LandDetector.h"
 #include <lib/geo/geo.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/home_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 
 namespace land_detector
 {
@@ -59,17 +59,13 @@ protected:
 	bool _get_ground_contact_state() override;
 	bool _get_landed_state() override;
 	void _set_hysteresis_factor(const int factor) override;
-	void _update_topics() override;
 
 private:
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
-	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
+	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
-	matrix::Vector2d _curr_pos{};
-	matrix::Vector2d _home_position{};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
-		(ParamFloat<px4::params::RTL_LAND_DELAY>) _param_rtl_land_delay
 	)
 
 };