diff --git a/src/modules/land_detector/RoverLandDetector.cpp b/src/modules/land_detector/RoverLandDetector.cpp index 8e59c0c5d2..90e7b4b7ef 100644 --- a/src/modules/land_detector/RoverLandDetector.cpp +++ b/src/modules/land_detector/RoverLandDetector.cpp @@ -51,41 +51,33 @@ bool RoverLandDetector::_get_ground_contact_state() bool RoverLandDetector::_get_landed_state() { - _update_topics(); - const float distance_to_home = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _home_position(0), _home_position(1)); - + // If Landing has been requested then say we have landed. 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 (_vehicle_global_position_sub.updated()) { + // 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) { 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()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = matrix::Vector2d(home_position.lat, home_position.lon); - } + return !_armed; // If we are armed we are not landed. } 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 diff --git a/src/modules/land_detector/RoverLandDetector.h b/src/modules/land_detector/RoverLandDetector.h index c90ac8d140..91e69e46e7 100644 --- a/src/modules/land_detector/RoverLandDetector.h +++ b/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 * @author Julian Oes @@ -44,7 +44,7 @@ #include "LandDetector.h" #include #include -#include +#include 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)}; - matrix::Vector2d _curr_pos{}; - matrix::Vector2d _home_position{}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; DEFINE_PARAMETERS( - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rtl_land_delay + (ParamFloat) _param_nav_acc_rad ) };