fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 03:43:34 +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-08-01 19:31:34 +02:00
committed by Lorenz Meier
parent bc46f13d90
commit 9baf41bef1
1 changed files with 6 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+6 -7
View File
@@ -218,9 +218,13 @@ bool MulticopterLandDetector::_get_ground_contact_state()
&& (_vehicleLocalPositionSetpoint.vz >= 0.9f * math::max(_params.landSpeed, 0.1f)); && (_vehicleLocalPositionSetpoint.vz >= 0.9f * math::max(_params.landSpeed, 0.1f));
bool hit_ground = in_descend && !verticalMovement; 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 // 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 // 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())) { (!verticalMovement || !_has_altitude_lock())) {
return true; return true;
} }
@@ -276,10 +280,6 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
armThresholdFactor = 2.5f; 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. // Next look if all rotation angles are not moving.
float maxRotationScaled = _params.maxRotation_rad_s * armThresholdFactor; float maxRotationScaled = _params.maxRotation_rad_s * armThresholdFactor;
@@ -287,8 +287,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating && if (_ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating) {
(!horizontalMovement || !_has_position_lock())) {
// Ground contact, no thrust and no movement -> landed // Ground contact, no thrust and no movement -> landed
return true; return true;
} }