mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 12:33:34 +08:00
mc_hover_thrust_estimator: validity flag and other small improvements/fixes
- track and publish validity based on hover thrust variance, innovation test ratio, and hysteresis - only publish on actual updates or becoming inactive - fix dt (previous timestamp wasn't being saved) - use local position timestamp (corresponding) to accel data rather than current time to avoid unnecessary timing jitter - check local position validity before using - mc_hover_thrust_estimator: move from wq:lp_default -> wq:nav_and_controllers to ensure the hover thrust estimator runs after the position controller and uses the same vehicle_local_position data - land_detector: check hover thrust estimate validity and adjust low throttle thresholds if hover thrust is available - mc_pos_control: only use hover thrust estimate if valid
This commit is contained in:
@@ -106,7 +106,11 @@ void MulticopterLandDetector::_update_topics()
|
||||
hover_thrust_estimate_s hte;
|
||||
|
||||
if (_hover_thrust_estimate_sub.update(&hte)) {
|
||||
_params.hoverThrottle = hte.hover_thrust;
|
||||
if (hte.valid) {
|
||||
_params.hoverThrottle = hte.hover_thrust;
|
||||
}
|
||||
|
||||
_hover_thrust_estimate_valid = hte.valid;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -182,9 +186,9 @@ bool MulticopterLandDetector::_get_ground_contact_state()
|
||||
}
|
||||
|
||||
|
||||
// low thrust: 30% of throttle range between min and hover
|
||||
const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.3f;
|
||||
|
||||
// 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 sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover;
|
||||
bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle);
|
||||
|
||||
// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
|
||||
|
||||
@@ -114,6 +114,7 @@ private:
|
||||
|
||||
bool _flag_control_climb_rate_enabled{false};
|
||||
bool _hover_thrust_initialized{false};
|
||||
bool _hover_thrust_estimate_valid{false};
|
||||
|
||||
float _actuator_controls_throttle{0.f};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user