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

land_detector: Implement ModuleParams inheritance in the FixedwingLandDetector class.

Browse Source
This commit is contained in:
Mark Sauder
2019-06-28 10:26:59 -04:00
committed by Daniel Agar
parent 0b2af5c519
commit c22ed9397e
3 changed files with 31 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/FixedwingLandDetector.cpp
+15 -19
View File
@@ -51,30 +51,25 @@ namespace land_detector
FixedwingLandDetector::FixedwingLandDetector()
{
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
_paramHandle.maxXYAccel = param_find("LNDFW_XYACC_MAX");
// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
_landed_hysteresis.set_hysteresis_time_from(false, LANDED_TRIGGER_TIME_US);
_landed_hysteresis.set_hysteresis_time_from(true, FLYING_TRIGGER_TIME_US);
}
void FixedwingLandDetector::_update_topics()
{
_airspeedSub.update(&_airspeed);
_airspeed_sub.update(&_airspeed);
_sensor_bias_sub.update(&_sensor_bias);
_vehicle_local_position_sub.update(&_vehicle_local_position);
}
void FixedwingLandDetector::_update_params()
{
param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
param_get(_paramHandle.maxXYAccel, &_params.maxXYAccel);
parameter_update_s param_update;
if (_param_update_sub.update(&param_update)) {
_update_params();
}
}
float FixedwingLandDetector::_get_max_altitude()
@@ -96,7 +91,7 @@ bool FixedwingLandDetector::_get_landed_state()
if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500 * 1000) {
// horizontal velocity
// Horizontal velocity complimentary filter.
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx +
_vehicle_local_position.vy * _vehicle_local_position.vy);
@@ -104,7 +99,7 @@ bool FixedwingLandDetector::_get_landed_state()
_velocity_xy_filtered = val;
}
// vertical velocity
// Vertical velocity complimentary filter.
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicle_local_position.vz);
if (PX4_ISFINITE(val)) {
@@ -113,17 +108,18 @@ bool FixedwingLandDetector::_get_landed_state()
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
// a leaking lowpass prevents biases from building up, but
// gives a mostly correct response for short impulses
// A leaking lowpass prevents biases from building up, but
// gives a mostly correct response for short impulses.
const float acc_hor = sqrtf(_sensor_bias.accel_x * _sensor_bias.accel_x +
_sensor_bias.accel_y * _sensor_bias.accel_y);
_accel_horz_lp = _accel_horz_lp * 0.8f + acc_hor * 0.18f;
// crude land detector for fixedwing
landDetected = _velocity_xy_filtered < _params.maxVelocity
&& _velocity_z_filtered < _params.maxClimbRate
&& _airspeed_filtered < _params.maxAirSpeed
&& _accel_horz_lp < _params.maxXYAccel;
landDetected = _accel_horz_lp < _param_lndfw_xyaccel_max.get()
&& _airspeed_filtered < _param_lndfw_airspd.get()
&& _velocity_xy_filtered < _param_lndfw_vel_xy_max.get()
&& _velocity_z_filtered < _param_lndfw_vel_z_max.get();
} else {
// Control state topic has timed out and we need to assume we're landed.