fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 03:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

landdetector: use control mode, add minimum speed to detect hit-ground

Browse Source
This commit is contained in:
Dennis Mannhart
2017-08-01 19:31:34 +02:00
committed by Lorenz Meier
parent 549d8da6ee
commit bc46f13d90
2 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+6 -6
View File
@@ -214,7 +214,8 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
// we then can assume that the vehicle hit ground // we then can assume that the vehicle hit ground
bool in_descend = _is_velocity_control_active() && (_vehicleLocalPositionSetpoint.vz >= 0.9f * _params.landSpeed); bool in_descend = _is_climb_rate_enabled()
&& (_vehicleLocalPositionSetpoint.vz >= 0.9f * math::max(_params.landSpeed, 0.1f));
bool hit_ground = in_descend && !verticalMovement; bool hit_ground = in_descend && !verticalMovement;
// 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
@@ -360,13 +361,12 @@ bool MulticopterLandDetector::_has_manual_control_present()
return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0; return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0;
} }
bool MulticopterLandDetector::_is_velocity_control_active() bool MulticopterLandDetector::_is_climb_rate_enabled()
{ {
bool is_finite = PX4_ISFINITE(_vehicleLocalPositionSetpoint.vx) && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vy) bool has_updated = (_vehicleLocalPositionSetpoint.timestamp != 0)
&& PX4_ISFINITE(_vehicleLocalPositionSetpoint.vz); && (hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500000);
return (_vehicleLocalPositionSetpoint.timestamp != 0) && return (_control_mode.flag_control_climb_rate_enabled && has_updated && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vz));
(hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500000) && is_finite;
} }
bool MulticopterLandDetector::_has_low_thrust() bool MulticopterLandDetector::_has_low_thrust()
src/modules/land_detector/MulticopterLandDetector.h
+1 -1
View File
@@ -147,7 +147,7 @@ private:
bool _has_manual_control_present(); bool _has_manual_control_present();
bool _has_minimal_thrust(); bool _has_minimal_thrust();
bool _has_low_thrust(); bool _has_low_thrust();
bool _is_velocity_control_active(); bool _is_climb_rate_enabled();
}; };