SIH: add distance sensor fault simulation

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);

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<px4::params::SIH_MAG_OFFSET_Y>) _sih_mag_offset_y,
 		(ParamFloat<px4::params::SIH_MAG_OFFSET_Z>) _sih_mag_offset_z,
 		(ParamFloat<px4::params::SIH_DISTSNSR_MIN>) _sih_distance_snsr_min,
-		(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max
+		(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
+		(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override
 	)
 };

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);