SIH: add distance sensor

src/modules/sih/sih.cpp

@@ -67,6 +67,7 @@ Sih::Sih() :
 	_last_run = task_start;
 	_gps_time = task_start;
 	_serial_time = task_start;
+	_dist_snsr_time = task_start;
 }
 
 Sih::~Sih()
@@ -148,6 +149,12 @@ void Sih::Run()
 		send_gps();
 	}
 
+	// distance sensor published at 50 Hz
+	if (_now - _dist_snsr_time >= 20_ms) {
+		_dist_snsr_time = _now;
+		send_dist_snsr();
+	}
+
 	// send uart message every 40 ms
 	if (_now - _serial_time >= 40_ms) {
 		_serial_time = _now;
@@ -191,6 +198,9 @@ void Sih::parameters_updated()
 	_mag_offset_x = _sih_mag_offset_x.get();
 	_mag_offset_y = _sih_mag_offset_y.get();
 	_mag_offset_z = _sih_mag_offset_z.get();
+
+	_distance_snsr_min = _sih_distance_snsr_min.get();
+	_distance_snsr_max = _sih_distance_snsr_max.get();
 }
 
 // initialization of the variables for the simulator
@@ -353,6 +363,27 @@ void Sih::send_gps()
 	_sensor_gps_pub.publish(_sensor_gps);
 }
 
+void Sih::send_dist_snsr()
+{
+	_distance_snsr.timestamp = _now;
+	_distance_snsr.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	_distance_snsr.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+	_distance_snsr.min_distance = _distance_snsr_min;
+	_distance_snsr.max_distance = _distance_snsr_max;
+	_distance_snsr.signal_quality = -1;
+	_distance_snsr.device_id = 0;
+
+	_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);
+}
+
 void Sih::publish_sih()
 {
 	// publish angular velocity groundtruth

src/modules/sih/sih.hpp

@@ -56,6 +56,7 @@
 #include <uORB/topics/vehicle_angular_velocity.h>   // to publish groundtruth
 #include <uORB/topics/vehicle_attitude.h>           // to publish groundtruth
 #include <uORB/topics/vehicle_global_position.h>    // to publish groundtruth
+#include <uORB/topics/distance_sensor.h>
 
 using namespace time_literals;
 
@@ -96,6 +97,10 @@ private:
 	sensor_gps_s			_sensor_gps{};
 	uORB::Publication<sensor_gps_s>	_sensor_gps_pub{ORB_ID(sensor_gps)};
 
+	// to publish the distance sensor
+	distance_sensor_s                    _distance_snsr{};
+	uORB::Publication<distance_sensor_s> _distance_snsr_pub{ORB_ID(distance_sensor)};
+
 	// angular velocity groundtruth
 	vehicle_angular_velocity_s			_vehicle_angular_velocity_gt{};
 	uORB::Publication<vehicle_angular_velocity_s>	_vehicle_angular_velocity_gt_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
@@ -125,6 +130,7 @@ private:
 	void equations_of_motion();
 	void reconstruct_sensors_signals();
 	void send_gps();
+	void send_dist_snsr();
 	void publish_sih();
 
 	perf_counter_t  _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
@@ -135,6 +141,7 @@ private:
 	hrt_abstime _gps_time{0};
 	hrt_abstime _mag_time{0};
 	hrt_abstime _serial_time{0};
+	hrt_abstime _dist_snsr_time{0};
 	hrt_abstime _now{0};
 	float       _dt{0};         // sampling time [s]
 	bool        _grounded{true};// whether the vehicle is on the ground
@@ -177,6 +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;
 
 	// parameters defined in sih_params.c
 	DEFINE_PARAMETERS(
@@ -205,6 +213,8 @@ private:
 		(ParamFloat<px4::params::SIH_BARO_OFFSET>) _sih_baro_offset,
 		(ParamFloat<px4::params::SIH_MAG_OFFSET_X>) _sih_mag_offset_x,
 		(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_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
 	)
 };

src/modules/sih/sih_params.c

@@ -391,3 +391,27 @@ PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Y,  0.0f);
  * @group Simulation In Hardware
  */
 PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Z,  0.0f);
+
+/**
+ * distance sensor minimun range
+ *
+ * @unit m
+ * @min 0.0
+ * @max 10.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MIN, 0.0f);
+
+/**
+ * distance sensor maximun range
+ *
+ * @unit m
+ * @min 0.0
+ * @max 1000.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MAX, 100.0f);