From c22ed9397e26e4112a863b13ce05d5c3f602112e Mon Sep 17 00:00:00 2001 From: Mark Sauder Date: Fri, 28 Jun 2019 08:26:59 -0600 Subject: [PATCH] land_detector: Implement ModuleParams inheritance in the FixedwingLandDetector class. --- .../land_detector/FixedwingLandDetector.cpp | 34 ++++++++----------- .../land_detector/FixedwingLandDetector.h | 32 ++++++++--------- src/modules/land_detector/LandDetector.h | 3 +- 3 files changed, 31 insertions(+), 38 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 71ab489d79..6d0a865870 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -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(¶m_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. diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index be17117b8c..cdfa44f28a 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -73,32 +73,28 @@ private: static constexpr hrt_abstime LANDED_TRIGGER_TIME_US = 2_s; static constexpr hrt_abstime FLYING_TRIGGER_TIME_US = 0_us; - struct { - param_t maxVelocity; - param_t maxClimbRate; - param_t maxAirSpeed; - param_t maxXYAccel; - } _paramHandle{}; - - struct { - float maxVelocity; - float maxClimbRate; - float maxAirSpeed; - float maxXYAccel; - } _params{}; - - uORB::Subscription _airspeedSub{ORB_ID(airspeed)}; + uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; + uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position}); + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; airspeed_s _airspeed{}; sensor_bias_s _sensor_bias{}; vehicle_local_position_s _vehicle_local_position{}; + float _accel_horz_lp{0.0f}; + float _airspeed_filtered{0.0f}; float _velocity_xy_filtered{0.0f}; float _velocity_z_filtered{0.0f}; - float _airspeed_filtered{0.0f}; - float _accel_horz_lp{0.0f}; + + DEFINE_PARAMETERS_CUSTOM_PARENT( + LandDetector, + (ParamFloat) _param_lndfw_xyaccel_max, + (ParamFloat) _param_lndfw_airspd, + (ParamFloat) _param_lndfw_vel_xy_max, + (ParamFloat) _param_lndfw_vel_z_max + ); + }; } // namespace land_detector diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index d8125fac01..36d90def5d 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -176,7 +176,8 @@ private: uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; - DEFINE_PARAMETERS( + DEFINE_PARAMETERS_CUSTOM_PARENT( + ModuleParams, (ParamInt) _param_total_flight_time_high, (ParamInt) _param_total_flight_time_low );