From ab41319009197ebdfcae28cecc70987dcc29fdb1 Mon Sep 17 00:00:00 2001 From: Nicolas MARTIN Date: Tue, 13 Oct 2020 15:56:08 +0200 Subject: [PATCH] SIH: add distance sensor --- src/modules/sih/sih.cpp | 31 +++++++++++++++++++++++++++++++ src/modules/sih/sih.hpp | 12 +++++++++++- src/modules/sih/sih_params.c | 24 ++++++++++++++++++++++++ 3 files changed, 66 insertions(+), 1 deletion(-) diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 5686182dca..34cc587849 100644 --- a/src/modules/sih/sih.cpp +++ b/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 diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 4f877669a3..60f11fbd5c 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -56,6 +56,7 @@ #include // to publish groundtruth #include // to publish groundtruth #include // to publish groundtruth +#include using namespace time_literals; @@ -96,6 +97,10 @@ private: sensor_gps_s _sensor_gps{}; uORB::Publication _sensor_gps_pub{ORB_ID(sensor_gps)}; + // to publish the distance sensor + distance_sensor_s _distance_snsr{}; + uORB::Publication _distance_snsr_pub{ORB_ID(distance_sensor)}; + // angular velocity groundtruth vehicle_angular_velocity_s _vehicle_angular_velocity_gt{}; uORB::Publication _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) _sih_baro_offset, (ParamFloat) _sih_mag_offset_x, (ParamFloat) _sih_mag_offset_y, - (ParamFloat) _sih_mag_offset_z + (ParamFloat) _sih_mag_offset_z, + (ParamFloat) _sih_distance_snsr_min, + (ParamFloat) _sih_distance_snsr_max ) }; diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c index a0aefd5637..a79ecb12c4 100644 --- a/src/modules/sih/sih_params.c +++ b/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);