fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 12:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

land_detector: multicopter ground contact fixes, improvements, and cleanup

Browse Source 
- if "landed" and "maybe_landed" states are false then both the "hit_ground" and the "low_thrust" condition need to be true in order to detect landing
 - ground contact MC NAN setpoint workaround
 - ground contact additionally check acceleration setpoint
 - schedule with vehicle_local_position updates (most updates require valid local position)
 - don't allow LNDMC_Z_VEL_MAX to exceed MPC_LAND_SPEED
 - ground contact horizontal movement checks default to failed if estimates aren't available
This commit is contained in:
Daniel Agar
2020-08-02 13:37:23 -04:00
committed by GitHub
parent 63a23957b1
commit f5f2897486
13 changed files with 223 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/VtolLandDetector.cpp
+11 -10
View File
@@ -50,15 +50,13 @@ namespace land_detector
void VtolLandDetector::_update_topics()
{
MulticopterLandDetector::_update_topics();
_airspeed_validated_sub.update(&_airspeed_validated);
_vehicle_status_sub.update(&_vehicle_status);
}
bool VtolLandDetector::_get_maybe_landed_state()
{
// If in Fixed-wing mode, only trigger if disarmed
if ((_vehicle_status.timestamp != 0) && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return !_actuator_armed.armed;
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return !_armed;
}
return MulticopterLandDetector::_get_maybe_landed_state();
@@ -67,17 +65,20 @@ bool VtolLandDetector::_get_maybe_landed_state()
bool VtolLandDetector::_get_landed_state()
{
// If in Fixed-wing mode, only trigger if disarmed
if ((_vehicle_status.timestamp != 0) && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return !_actuator_armed.armed;
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return !_armed;
}
// this is returned from the mutlicopter land detector
bool landed = MulticopterLandDetector::_get_landed_state();
// for vtol we additionally consider airspeed
if (hrt_elapsed_time(&_airspeed_validated.timestamp) < 1_s && PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s)) {
airspeed_validated_s airspeed_validated{};
_airspeed_validated_sub.copy(&airspeed_validated);
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s;
if (hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s;
} else {
// if airspeed does not update, set it to zero and rely on multicopter land detector
@@ -97,8 +98,8 @@ bool VtolLandDetector::_get_landed_state()
bool VtolLandDetector::_get_freefall_state()
{
bool free_fall_detected =
MulticopterLandDetector::_get_freefall_state(); // true if falling or in a parabolic flight (low gravity)
// true if falling or in a parabolic flight (low gravity)
bool free_fall_detected = MulticopterLandDetector::_get_freefall_state();
// only return a positive free fall detected if not in fixed-wing mode
return _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && free_fall_detected;