Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

land_detector: continue to use hover thrust if it was valid recently #15723

Merged
merged 1 commit into from
Sep 15, 2020
Merged

land_detector: continue to use hover thrust if it was valid recently #15723

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 12 additions & 9 deletions src/modules/land_detector/MulticopterLandDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,9 +108,8 @@ void MulticopterLandDetector::_update_topics()
if (_hover_thrust_estimate_sub.update(&hte)) {
if (hte.valid) {
_params.hoverThrottle = hte.hover_thrust;
_hover_thrust_estimate_last_valid = hte.timestamp;
}

_hover_thrust_estimate_valid = hte.valid;
}
}
}
Expand Down Expand Up @@ -153,7 +152,10 @@ bool MulticopterLandDetector::_get_freefall_state()

bool MulticopterLandDetector::_get_ground_contact_state()
{
const bool lpos_available = (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s);
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);
Expand All @@ -166,7 +168,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// an accurate in-air indication.
float max_climb_rate = math::min(land_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get());

if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives.
max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f;
Expand All @@ -187,7 +189,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()


// 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);

Expand Down Expand Up @@ -234,6 +236,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
return true;
}

const hrt_abstime time_now_us = hrt_absolute_time();

// minimal throttle: initially 10% of throttle range between min and hover
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
Expand All @@ -246,7 +249,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
// Check if thrust output is less than the minimum throttle.
if (_actuator_controls_throttle <= sys_min_throttle) {
if (_min_thrust_start == 0) {
_min_thrust_start = hrt_absolute_time();
_min_thrust_start = time_now_us;
}

} else {
Expand All @@ -263,7 +266,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
float landThresholdFactor = 1.f;

// Widen acceptance thresholds for landed state right after landed
if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
landThresholdFactor = 2.5f;
}

Expand All @@ -278,12 +281,12 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
}

// If vertical velocity is available: ground contact, no thrust, no movement -> landed
if ((hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) {
if (((time_now_us - _vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) {
return _ground_contact_hysteresis.get_state();
}

// Otherwise, landed if the system has minimum thrust (manual or in failsafe) and no rotation for at least 8 seconds
return (_min_thrust_start > 0) && (hrt_elapsed_time(&_min_thrust_start) > 8_s);
return (_min_thrust_start > 0) && ((time_now_us - _min_thrust_start) > 8_s);
}

bool MulticopterLandDetector::_get_landed_state()
Expand Down
3 changes: 2 additions & 1 deletion src/modules/land_detector/MulticopterLandDetector.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,9 +112,10 @@ class MulticopterLandDetector : public LandDetector
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};

hrt_abstime _hover_thrust_estimate_last_valid{0};

bool _flag_control_climb_rate_enabled{false};
bool _hover_thrust_initialized{false};
bool _hover_thrust_estimate_valid{false};

float _actuator_controls_throttle{0.f};

Expand Down