fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 07:51:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

land_detector: relax data timeouts uniformly

Browse Source 
* if we're hitting these timeouts there are much larger problems in the system
This commit is contained in:
Daniel Agar
2020-01-20 17:14:00 -05:00
committed by GitHub
parent d1260aa28c
commit 635ceccfdb
3 changed files with 5 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/FixedwingLandDetector.cpp
+1 -1
View File
@@ -66,7 +66,7 @@ bool FixedwingLandDetector::_get_landed_state()
bool landDetected = false; bool landDetected = false;
if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500_ms) { if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) {
// Horizontal velocity complimentary filter. // Horizontal velocity complimentary filter.
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx +
src/modules/land_detector/MulticopterLandDetector.cpp
+3 -14
View File
@@ -147,7 +147,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
vertical_movement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f; vertical_movement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f;
} else { } else {
// Adjust max_climb_rate if land_speed is lower than 2x max_climb_rate // Adjust max_climb_rate if land_speed is lower than 2x max_climb_rate
float max_climb_rate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) : float max_climb_rate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) :
_param_lndmc_z_vel_max.get(); _param_lndmc_z_vel_max.get();
@@ -171,9 +170,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
bool MulticopterLandDetector::_get_maybe_landed_state() bool MulticopterLandDetector::_get_maybe_landed_state()
{ {
// Time base for this function
const hrt_abstime now = hrt_absolute_time();
// When not armed, consider to be maybe-landed // When not armed, consider to be maybe-landed
if (!_actuator_armed.armed) { if (!_actuator_armed.armed) {
return true; return true;
@@ -181,7 +177,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
if (_has_minimal_thrust()) { if (_has_minimal_thrust()) {
if (_min_trust_start == 0) { if (_min_trust_start == 0) {
_min_trust_start = now; _min_trust_start = hrt_absolute_time();
} }
} else { } else {
@@ -224,19 +220,15 @@ bool MulticopterLandDetector::_get_landed_state()
// reset the landed_time // reset the landed_time
if (!_maybe_landed_hysteresis.get_state()) { if (!_maybe_landed_hysteresis.get_state()) {
_landed_time = 0; _landed_time = 0;
} else if (_landed_time == 0) { } else if (_landed_time == 0) {
_landed_time = hrt_absolute_time(); _landed_time = hrt_absolute_time();
} }
// if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0) // if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0)
// therefore check if all other condition of the landed state remain true // therefore check if all other condition of the landed state remain true
return _maybe_landed_hysteresis.get_state(); return _maybe_landed_hysteresis.get_state();
} }
float MulticopterLandDetector::_get_max_altitude() float MulticopterLandDetector::_get_max_altitude()
@@ -265,9 +257,7 @@ float MulticopterLandDetector::_get_max_altitude()
bool MulticopterLandDetector::_has_altitude_lock() bool MulticopterLandDetector::_has_altitude_lock()
{ {
return _vehicle_local_position.timestamp != 0 && return (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.z_valid;
hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500_ms &&
_vehicle_local_position.z_valid;
} }
bool MulticopterLandDetector::_has_position_lock() bool MulticopterLandDetector::_has_position_lock()
@@ -277,8 +267,7 @@ bool MulticopterLandDetector::_has_position_lock()
bool MulticopterLandDetector::_is_climb_rate_enabled() bool MulticopterLandDetector::_is_climb_rate_enabled()
{ {
bool has_updated = (_vehicle_local_position_setpoint.timestamp != 0) bool has_updated = (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 1_s);
&& (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 500_ms);
return (_vehicle_control_mode.flag_control_climb_rate_enabled && has_updated return (_vehicle_control_mode.flag_control_climb_rate_enabled && has_updated
&& PX4_ISFINITE(_vehicle_local_position_setpoint.vz)); && PX4_ISFINITE(_vehicle_local_position_setpoint.vz));
src/modules/land_detector/VtolLandDetector.cpp
+1 -1
View File
@@ -74,7 +74,7 @@ bool VtolLandDetector::_get_landed_state()
bool landed = MulticopterLandDetector::_get_landed_state(); bool landed = MulticopterLandDetector::_get_landed_state();
// for vtol we additionally consider airspeed // for vtol we additionally consider airspeed
if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000 && _airspeed.confidence > 0.99f if (hrt_elapsed_time(&_airspeed.timestamp) < 1_s && _airspeed.confidence > 0.99f
&& PX4_ISFINITE(_airspeed.indicated_airspeed_m_s)) { && PX4_ISFINITE(_airspeed.indicated_airspeed_m_s)) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.indicated_airspeed_m_s; _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.indicated_airspeed_m_s;