diff --git a/src/modules/land_detector/RoverLandDetector.cpp b/src/modules/land_detector/RoverLandDetector.cpp index e9942c4eae..2011419db8 100644 --- a/src/modules/land_detector/RoverLandDetector.cpp +++ b/src/modules/land_detector/RoverLandDetector.cpp @@ -51,12 +51,40 @@ 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 (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { return true; // If Landing has been requested then say we have landed. + } 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()) { + 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); + } + + 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); + } +} + +void RoverLandDetector::_set_hysteresis_factor(const int factor) +{ + _landed_hysteresis.set_hysteresis_time_from(true, 0_ms); +} + } // namespace land_detector diff --git a/src/modules/land_detector/RoverLandDetector.h b/src/modules/land_detector/RoverLandDetector.h index 5ebc5b6677..c90ac8d140 100644 --- a/src/modules/land_detector/RoverLandDetector.h +++ b/src/modules/land_detector/RoverLandDetector.h @@ -42,6 +42,9 @@ #pragma once #include "LandDetector.h" +#include +#include +#include namespace land_detector { @@ -55,7 +58,20 @@ public: protected: bool _get_ground_contact_state() override; bool _get_landed_state() override; - void _set_hysteresis_factor(const int factor) 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{}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_rtl_land_delay + ) + }; } // namespace land_detector