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: add rtl as a landed condition for rovers (#23646)

Browse Source 
The RTL sequence from the navigator requires the vehicle to land. This is now handled for rovers by setting its state to landed if it is within the acceptance radius of the home position when in return mode.
This commit is contained in:
chfriedrich98 2024-09-06 17:21:49 +02:00 committed by GitHub
parent 67ee4817ae
commit bb0210ecd7
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 45 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -42,6 +42,9 @@
#pragma once
#include "LandDetector.h"
#include <lib/geo/geo.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
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<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RTL_LAND_DELAY>) _param_rtl_land_delay
)
};
} // namespace land_detector