fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: update land detector

Browse Source
This commit is contained in:
chfriedrich98 2025-04-01 10:26:41 +02:00 committed by Silvan Fuhrer
parent 771d09b968
commit 4e436cc64e
2 changed files with 18 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/modules/land_detector/RoverLandDetector.cpp
View File

@ -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

12
src/modules/land_detector/RoverLandDetector.h
View File

@ -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)};
matrix::Vector2d _curr_pos{};
matrix::Vector2d _home_position{};
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RTL_LAND_DELAY>) _param_rtl_land_delay
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
)
};