fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 22:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

SIH: add distance sensor fault simulation

Browse Source
This commit is contained in:
Nicolas MARTIN
2021-02-23 12:57:40 -05:00
committed by Daniel Agar
parent ab41319009
commit 80a73ab5b5
3 changed files with 24 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sih/sih.cpp
+12 -5
View File
@@ -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
+3 -2
View File
@@ -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
+9
View File
@@ -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);