fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 16:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

land_detector: multicopter ground contact fixes, improvements, and cleanup

Browse Source 
- if "landed" and "maybe_landed" states are false then both the "hit_ground" and the "low_thrust" condition need to be true in order to detect landing
 - ground contact MC NAN setpoint workaround
 - ground contact additionally check acceleration setpoint
 - schedule with vehicle_local_position updates (most updates require valid local position)
 - don't allow LNDMC_Z_VEL_MAX to exceed MPC_LAND_SPEED
 - ground contact horizontal movement checks default to failed if estimates aren't available
This commit is contained in:
Daniel Agar
2020-08-02 13:37:23 -04:00
committed by GitHub
parent 63a23957b1
commit f5f2897486
13 changed files with 223 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/FixedwingLandDetector.cpp
+7 -12
View File
@@ -51,16 +51,10 @@ FixedwingLandDetector::FixedwingLandDetector()
_landed_hysteresis.set_hysteresis_time_from(true, FLYING_TRIGGER_TIME_US);
}
void FixedwingLandDetector::_update_topics()
{
LandDetector::_update_topics();
_airspeed_validated_sub.update(&_airspeed_validated);
}
bool FixedwingLandDetector::_get_landed_state()
{
// Only trigger flight conditions if we are armed.
if (!_actuator_armed.armed) {
if (!_armed) {
return true;
}
@@ -83,19 +77,20 @@ bool FixedwingLandDetector::_get_landed_state()
_velocity_z_filtered = val;
}
airspeed_validated_s airspeed_validated{};
_airspeed_validated_sub.copy(&airspeed_validated);
// set _airspeed_filtered to 0 if airspeed data is invalid
if (!PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed_validated.timestamp) > 1_s) {
if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
_airspeed_filtered = 0.0f;
} else {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s;
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s;
}
// A leaking lowpass prevents biases from building up, but
// gives a mostly correct response for short impulses.
const matrix::Vector3f accel{_vehicle_acceleration.xyz};
const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1));
const float acc_hor = matrix::Vector2f(_acceleration).norm();
_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;
// Crude land detector for fixedwing.