diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 34cc587849..9e7e8a5943 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -150,7 +150,8 @@ void Sih::Run() } // distance sensor published at 50 Hz - if (_now - _dist_snsr_time >= 20_ms) { + if (_now - _dist_snsr_time >= 20_ms + && fabs(_distance_snsr_override) < 10000) { _dist_snsr_time = _now; send_dist_snsr(); } @@ -201,6 +202,7 @@ void Sih::parameters_updated() _distance_snsr_min = _sih_distance_snsr_min.get(); _distance_snsr_max = _sih_distance_snsr_max.get(); + _distance_snsr_override = _sih_distance_snsr_override.get(); } // initialization of the variables for the simulator @@ -373,12 +375,17 @@ void Sih::send_dist_snsr() _distance_snsr.signal_quality = -1; _distance_snsr.device_id = 0; - _distance_snsr.current_distance = -_p_I(2) / _C_IB(2, 2); + if (_distance_snsr_override >= 0.f) { + _distance_snsr.current_distance = _distance_snsr_override; - if (_distance_snsr.current_distance > _distance_snsr_max) { - // this is based on lightware lw20 behavior - _distance_snsr.current_distance = UINT16_MAX / 100.f; + } else { + _distance_snsr.current_distance = -_p_I(2) / _C_IB(2, 2); + + if (_distance_snsr.current_distance > _distance_snsr_max) { + // this is based on lightware lw20 behavior + _distance_snsr.current_distance = UINT16_MAX / 100.f; + } } _distance_snsr_pub.publish(_distance_snsr); diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 60f11fbd5c..d5be2ba5e4 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -184,7 +184,7 @@ private: int _gps_used; float _baro_offset_m, _mag_offset_x, _mag_offset_y, _mag_offset_z; - float _distance_snsr_min, _distance_snsr_max; + float _distance_snsr_min, _distance_snsr_max, _distance_snsr_override; // parameters defined in sih_params.c DEFINE_PARAMETERS( @@ -215,6 +215,7 @@ private: (ParamFloat) _sih_mag_offset_y, (ParamFloat) _sih_mag_offset_z, (ParamFloat) _sih_distance_snsr_min, - (ParamFloat) _sih_distance_snsr_max + (ParamFloat) _sih_distance_snsr_max, + (ParamFloat) _sih_distance_snsr_override ) }; diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c index a79ecb12c4..83177478c6 100644 --- a/src/modules/sih/sih_params.c +++ b/src/modules/sih/sih_params.c @@ -415,3 +415,12 @@ PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MIN, 0.0f); * @group Simulation In Hardware */ PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MAX, 100.0f); + +/** + * if >= 0 the distance sensor measures will be overrided by this value + * Absolute value superior to 10000 will disable distance sensor + * + * @unit m + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_DISTSNSR_OVR, -1.0f);