fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 06:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC LandDetector: widen thresholds for vz and rotational movment always in maybe landed and 2s after

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-10-17 11:03:00 +02:00
parent 27309a45cc
commit 91adb4c9e0
2 changed files with 15 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+13 -14
View File
@@ -147,14 +147,15 @@ bool MulticopterLandDetector::_get_ground_contact_state()
const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s);
if (lpos_available && _vehicle_local_position.v_z_valid) {
// Check if we are moving vertically - this might see a spike after arming due to
// throttle-up vibration. If accelerating fast the throttle thresholds will still give
// an accurate in-air indication.
// Check if we are moving vertically.
// Use wider threshold if currently in "maybe landed" state, or time since then is less
// than LAND_DETECTOR_TAKEOFF_PHASE_TIME_US, as estimation for
// vertical speed is often deteriorated when on the ground or due to propeller
// up/down throttling.
float max_vertical_velocity = _param_lndmc_z_vel_max.get();
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.
if ((time_now_us - _last_time_maybe_landed) < LAND_DETECTOR_TAKEOFF_PHASE_TIME_US) {
max_vertical_velocity *= 2.5f;
}
@@ -266,8 +267,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
// Next look if vehicle is not rotating (do not consider yaw)
float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get());
// Widen max rotation thresholds for landed state right after landed
if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
// Widen max rotation thresholds if either in maybe landed state or less than
// LAND_DETECTOR_TAKEOFF_PHASE_TIME_US passed since then.
if ((time_now_us - _last_time_maybe_landed) < LAND_DETECTOR_TAKEOFF_PHASE_TIME_US) {
max_rotation_threshold *= 2.5f;
}
@@ -292,12 +294,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
bool MulticopterLandDetector::_get_landed_state()
{
// reset the landed_time
if (!_maybe_landed_hysteresis.get_state()) {
_landed_time = 0;
} else if (_landed_time == 0) {
_landed_time = hrt_absolute_time();
// update _last_time_maybe_landed as long as the vehicle is maybe landed
if (_maybe_landed_hysteresis.get_state()) {
_last_time_maybe_landed = hrt_absolute_time();
}
// When not armed, consider to be landed
src/modules/land_detector/MulticopterLandDetector.h
+2 -3
View File
@@ -84,8 +84,8 @@ private:
/** Time in us that freefall has to hold before triggering freefall */
static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms;
/** Time interval in us in which wider acceptance thresholds are used after landed. */
static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s;
/** Time interval in us in which wider acceptance thresholds are used after the "maybe landed" is cleared before takeoff. */
static constexpr hrt_abstime LAND_DETECTOR_TAKEOFF_PHASE_TIME_US = 2_s;
/** Distance above ground below which entering ground contact state is possible when distance to ground is available. */
static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f;
@@ -123,7 +123,6 @@ private:
uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED};
hrt_abstime _min_thrust_start{0}; ///< timestamp when minimum trust was applied first
hrt_abstime _landed_time{0};
bool _in_descend{false}; ///< vehicle is commanded to desend
bool _horizontal_movement{false}; ///< vehicle is moving horizontally