forked from Archive/PX4-Autopilot
land_detector: continue respecting hover thrust throughout descent
This commit is contained in:
parent
1fa08c3997
commit
68c171fd4f
|
@ -161,7 +161,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
|
|||
const hrt_abstime time_now_us = hrt_absolute_time();
|
||||
|
||||
const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s);
|
||||
const bool hover_thrust_estimate_valid = ((time_now_us - _hover_thrust_estimate_last_valid) < 1_s);
|
||||
|
||||
// land speed threshold, 90% of MPC_LAND_SPEED
|
||||
const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
|
||||
|
@ -200,8 +199,15 @@ bool MulticopterLandDetector::_get_ground_contact_state()
|
|||
_below_gnd_effect_hgt = false;
|
||||
}
|
||||
|
||||
const bool hover_thrust_estimate_valid = ((time_now_us - _hover_thrust_estimate_last_valid) < 1_s);
|
||||
|
||||
if (!_in_descend || hover_thrust_estimate_valid) {
|
||||
// continue using valid hover thrust if it became invalid during descent
|
||||
_hover_thrust_estimate_valid = hover_thrust_estimate_valid;
|
||||
}
|
||||
|
||||
// low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available
|
||||
const float thr_pct_hover = hover_thrust_estimate_valid ? 0.6f : 0.3f;
|
||||
const float thr_pct_hover = _hover_thrust_estimate_valid ? 0.6f : 0.3f;
|
||||
const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover;
|
||||
bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle);
|
||||
|
||||
|
|
|
@ -117,6 +117,7 @@ private:
|
|||
uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
|
||||
|
||||
hrt_abstime _hover_thrust_estimate_last_valid{0};
|
||||
bool _hover_thrust_estimate_valid{false};
|
||||
|
||||
bool _flag_control_climb_rate_enabled{false};
|
||||
bool _hover_thrust_initialized{false};
|
||||
|
|
Loading…
Reference in New Issue