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

landdetector: check horizontal movement as criteria to enter ground contact

Browse Source
This commit is contained in:
Dennis Mannhart 2017-05-31 17:45:40 +02:00 committed by Lorenz Meier
parent bc46f13d90
commit 9baf41bef1
1 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/land_detector/MulticopterLandDetector.cpp
View File

@ -218,9 +218,13 @@ bool MulticopterLandDetector::_get_ground_contact_state()
&& (_vehicleLocalPositionSetpoint.vz >= 0.9f * math::max(_params.landSpeed, 0.1f));
bool hit_ground = in_descend && !verticalMovement;
// Check if we are moving horizontally.
bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
// If pilots commands down or in auto mode and we are already below minimal thrust and we do not move down we assume ground contact
// TODO: we need an accelerometer based check for vertical movement for flying without GPS
if (manual_control_idle_or_auto && (_has_low_thrust() || hit_ground) &&
if (manual_control_idle_or_auto && _has_low_thrust() && (!horizontalMovement || !_has_position_lock()) &&
(!verticalMovement || !_has_altitude_lock())) {
return true;
}
@ -276,10 +280,6 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
armThresholdFactor = 2.5f;
}
// Check if we are moving horizontally.
bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
// Next look if all rotation angles are not moving.
float maxRotationScaled = _params.maxRotation_rad_s * armThresholdFactor;
@ -287,8 +287,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating &&
(!horizontalMovement || !_has_position_lock())) {
if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating) {
// Ground contact, no thrust and no movement -> landed
return true;
}