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:
Daniel Agar
2020-08-03 10:30:52 -04:00
committed by GitHub
parent 958d5a36ec
commit ba640acfcc
7 changed files with 103 additions and 36 deletions
@@ -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};