dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Dennis Mannhart 2017-07-04 15:36:16 +02:00 committed by Lorenz Meier
parent 549d8da6ee
commit bc46f13d90
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/land_detector/MulticopterLandDetector.cpp
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,
// 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;
// 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;
}
bool MulticopterLandDetector::_is_velocity_control_active()
bool MulticopterLandDetector::_is_climb_rate_enabled()
{
bool is_finite = PX4_ISFINITE(_vehicleLocalPositionSetpoint.vx) && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vy)
&& PX4_ISFINITE(_vehicleLocalPositionSetpoint.vz);
bool has_updated = (_vehicleLocalPositionSetpoint.timestamp != 0)
&& (hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500000);
return (_vehicleLocalPositionSetpoint.timestamp != 0) &&
(hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500000) && is_finite;
return (_control_mode.flag_control_climb_rate_enabled && has_updated && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vz));
}
bool MulticopterLandDetector::_has_low_thrust()

2
src/modules/land_detector/MulticopterLandDetector.h
View File

@ -147,7 +147,7 @@ private:
bool _has_manual_control_present();
bool _has_minimal_thrust();
bool _has_low_thrust();
bool _is_velocity_control_active();
bool _is_climb_rate_enabled();
};