diff options
Diffstat (limited to 'src/modules/land_detector/FixedwingLandDetector.cpp')
| -rw-r--r-- | src/modules/land_detector/FixedwingLandDetector.cpp | 33 |
1 files changed, 28 insertions, 5 deletions
diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 38f6c00a9..0d6f37310 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -44,17 +44,22 @@ #include <drivers/drv_hrt.h> FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), + _paramHandle(), + _params(), _vehicleLocalPositionSub(-1), _vehicleLocalPosition({}), _airspeedSub(-1), _airspeed({}), + _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), _landDetectTrigger(0) { - //ctor + _paramHandle.maxVelocity = param_find("LAND_FW_VELOCITY_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_FW_MAX_CLIMB_RATE"); + _paramHandle.maxAirSpeed = param_find("LAND_FW_AIR_SPEED_MAX"); } void FixedwingLandDetector::initialize() @@ -85,9 +90,9 @@ bool FixedwingLandDetector::update() _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + if (_velocity_xy_filtered < _params.maxVelocity + && _velocity_z_filtered < _params.maxClimbRate + && _airspeed_filtered < _params.maxAirSpeed) { //These conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -96,8 +101,26 @@ bool FixedwingLandDetector::update() } else { //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } return landDetected; } + +void FixedwingLandDetector::updateParameterCache(const bool force) +{ + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + } +} |