dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MulticopterLandDetector: refactor maybe_landed condition 

removing all early returns
This commit is contained in:
Matthias Grob 2022-11-08 16:05:20 +01:00 committed by Daniel Agar
parent b7d2868de9
commit c85d4fdb1c
2 changed files with 23 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
src/modules/land_detector/MulticopterLandDetector.cpp
View File

@ -79,6 +79,7 @@ MulticopterLandDetector::MulticopterLandDetector()
_paramHandle.minThrottle = param_find("MPC_THR_MIN");
_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
_minimum_thrust_8s_hysteresis.set_hysteresis_time_from(false, 8_s);
}
void MulticopterLandDetector::_update_topics()
@ -220,48 +221,28 @@ bool MulticopterLandDetector::_get_ground_contact_state()
const bool skip_close_to_ground_check = !_dist_bottom_is_observable || !_vehicle_local_position.dist_bottom_valid;
_close_to_ground_or_skipped_check = _is_close_to_ground() || skip_close_to_ground_check;
// When not armed, consider to have ground-contact
if (!_armed) {
return true;
}
// TODO: we need an accelerometer based check for vertical movement for flying without GPS
return _close_to_ground_or_skipped_check && ground_contact && !_horizontal_movement
&& !_vertical_movement;
return !_armed ||
(_close_to_ground_or_skipped_check && ground_contact
&& !_horizontal_movement && !_vertical_movement);
}
bool MulticopterLandDetector::_get_maybe_landed_state()
{
// When not armed, consider to be maybe-landed
if (!_armed) {
return true;
}
hrt_abstime now = hrt_absolute_time();
const hrt_abstime time_now_us = hrt_absolute_time();
float minimum_thrust_threshold{0.f};
// minimal throttle: initially 10% of throttle range between min and hover
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
// Determine the system min throttle based on flight mode
if (!_flag_control_climb_rate_enabled) {
sys_min_throttle = (_params.minManThrottle + 0.01f);
}
// 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 = time_now_us;
}
if (_flag_control_climb_rate_enabled) {
// 10% of throttle range between min and hover
minimum_thrust_threshold = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
} else {
_min_thrust_start = 0;
return false;
minimum_thrust_threshold = (_params.minManThrottle + 0.01f);
}
if (_freefall_hysteresis.get_state()) {
return false;
}
const bool minimum_thrust_now = _actuator_controls_throttle <= minimum_thrust_threshold;
_minimum_thrust_8s_hysteresis.set_state_and_update(minimum_thrust_now, now);
// Next look if vehicle is not rotating (do not consider yaw)
float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get());
@ -272,33 +253,23 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
max_rotation_threshold *= 2.5f;
}
if (_angular_velocity.xy().norm() > max_rotation_threshold) {
_rotational_movement = true;
return false;
} else {
_rotational_movement = false;
}
_rotational_movement = _angular_velocity.xy().norm() > max_rotation_threshold;
// If vertical velocity is available: ground contact, no thrust, no movement -> landed
if (((time_now_us - _vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) {
return _ground_contact_hysteresis.get_state();
}
const bool local_position_updated = (now - _vehicle_local_position.timestamp) < 1_s;
const bool vertical_velocity_valid = _vehicle_local_position.v_z_valid;
const bool vertical_estimate = local_position_updated && vertical_velocity_valid;
// 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) && ((time_now_us - _min_thrust_start) > 8_s);
return !_armed ||
(minimum_thrust_now && !_freefall_hysteresis.get_state() && !_rotational_movement
&& ((vertical_estimate && _ground_contact_hysteresis.get_state())
|| (!vertical_estimate && _minimum_thrust_8s_hysteresis.get_state())));
}
bool MulticopterLandDetector::_get_landed_state()
{
// When not armed, consider to be landed
if (!_armed) {
return true;
}
// 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
return _maybe_landed_hysteresis.get_state();
// all maybe_landed conditions need to hold longer
return !_armed || _maybe_landed_hysteresis.get_state();
}
bool MulticopterLandDetector::_get_ground_effect_state()

2
src/modules/land_detector/MulticopterLandDetector.h
View File

@ -119,7 +119,7 @@ private:
uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED};
hrt_abstime _min_thrust_start{0}; ///< timestamp when minimum trust was applied first
systemlib::Hysteresis _minimum_thrust_8s_hysteresis{false};
bool _in_descend{false}; ///< vehicle is commanded to desend
bool _horizontal_movement{false}; ///< vehicle is moving horizontally