From cbbd621acd6e53ab43276bf67d54b2e33e5ee505 Mon Sep 17 00:00:00 2001 From: Balduin Date: Fri, 24 Jan 2025 14:37:15 +0100 Subject: [PATCH] add noise scale parameter this makes SIH artificial sensor noise smaller or larger. useful for deterministic-ish sim. setting it to 0 does not work right now for mysterious reasons. --- .../sensor_airspeed_sim/SensorAirspeedSim.cpp | 3 ++- .../sensor_airspeed_sim/SensorAirspeedSim.hpp | 1 + .../simulation/sensor_baro_sim/SensorBaroSim.cpp | 6 +++--- .../simulation/sensor_baro_sim/SensorBaroSim.hpp | 1 + .../simulation/sensor_gps_sim/SensorGpsSim.cpp | 9 +++++---- .../simulation/sensor_gps_sim/SensorGpsSim.hpp | 1 + .../simulation/simulator_mavlink/SimulatorMavlink.cpp | 2 +- src/modules/simulation/simulator_sih/sih.cpp | 11 +++++++---- src/modules/simulation/simulator_sih/sih.hpp | 2 ++ src/modules/simulation/simulator_sih/sih_params.c | 9 +++++++++ 10 files changed, 32 insertions(+), 13 deletions(-) diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index 634b61a740..e8e4ae3a66 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -135,7 +135,8 @@ void SensorAirspeedSim::Run() const float air_density = AIR_DENSITY_MSL / density_ratio; // calculate differential pressure + noise in hPa - const float diff_pressure_noise = (float)generate_wgn() * 0.01f; + float _noise_scale = _sih_noise_scale.get(); + const float diff_pressure_noise = _noise_scale * (float)generate_wgn() * 0.01f; float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density * body_velocity(0) * body_velocity( 0) + diff_pressure_noise; diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp index a7b5bf9bf1..aa177abf8d 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp @@ -100,6 +100,7 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; DEFINE_PARAMETERS( + (ParamFloat) _sih_noise_scale, (ParamInt) _sim_failure ) }; diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp index 7e8b050c03..b00c6446e5 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp @@ -149,12 +149,12 @@ void SensorBaroSim::Run() } else { // no need to repeat the calculation - use the second value from last update y1 = _baro_rnd_y2; - _baro_rnd_use_last = false; - } + _baro_rnd_use_last = false; } } // Apply noise and drift - const float abs_pressure_noise = 1.f * (float)y1; // 1 Pa RMS noise + float _noise_scale = _sih_noise_scale.get(); + const float abs_pressure_noise = _noise_scale * (float) y1; // 1 Pa RMS noise _baro_drift_pa += _baro_drift_pa_per_sec * dt; const float absolute_pressure_noisy = absolute_pressure + abs_pressure_noise + _baro_drift_pa; diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp index bd68a0d429..4f78fb0838 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp @@ -98,6 +98,7 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; DEFINE_PARAMETERS( + (ParamFloat) _sih_noise_scale, (ParamFloat) _sim_baro_off_p, (ParamFloat) _sim_baro_off_t ) diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index f29ddc4c37..48f418e08d 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -120,11 +120,12 @@ void SensorGpsSim::Run() vehicle_global_position_s gpos{}; _vehicle_global_position_sub.copy(&gpos); - double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); - double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); - double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f)); + float _noise_scale = _sih_noise_scale.get(); + double latitude = gpos.lat + (double) _noise_scale * math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); + double longitude = gpos.lon + (double) _noise_scale * math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); + double altitude = (double)(gpos.alt + (_noise_scale * generate_wgn() * 0.5f)); - Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f); + Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + 0.1f * noiseGauss3f(0.06f, 0.077f, 0.158f); // device id device::Device::DeviceId device_id; diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp index d18a677c84..bdab490d6d 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp @@ -91,6 +91,7 @@ private: bool _gps_blocked{false}; DEFINE_PARAMETERS( + (ParamFloat) _sih_noise_scale, (ParamInt) _sim_gps_used ) }; diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 3e80ed16a8..d0afd236d9 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -1246,7 +1246,7 @@ void SimulatorMavlink::run() void SimulatorMavlink::check_failure_injections() { - PX4_INFO("Entering SimulatorMavlink::check_failure_injections"); + // PX4_INFO("Entering SimulatorMavlink::check_failure_injections"); vehicle_command_s vehicle_command; diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 1f77528af4..23dbd06551 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -282,6 +282,8 @@ void Sih::parameters_updated() _distance_snsr_override = _sih_distance_snsr_override.get(); _T_TAU = _sih_thrust_tau.get(); + + _noise_scale = _sih_noise_scale.get(); } void Sih::init_variables() @@ -360,6 +362,7 @@ void Sih::generate_force_and_torques(const float dt) } else if (_vehicle == VehicleType::StandardVTOL) { + _T_B = Vector3f(_T_MAX * 2 * _u[7], 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]), _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]), @@ -620,13 +623,13 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) Vector3f gyro_noise; if (_T_B.longerThan(FLT_EPSILON)) { - accel_noise = noiseGauss3f(0.5f, 1.7f, 1.4f); - gyro_noise = noiseGauss3f(0.14f, 0.07f, 0.03f); + accel_noise = _noise_scale * noiseGauss3f(0.5f, 1.7f, 1.4f); + gyro_noise = _noise_scale * noiseGauss3f(0.14f, 0.07f, 0.03f); } else { // Lower noise when not armed - accel_noise = noiseGauss3f(0.1f, 0.1f, 0.1f); - gyro_noise = noiseGauss3f(0.01f, 0.01f, 0.01f); + accel_noise = _noise_scale * noiseGauss3f(0.1f, 0.1f, 0.1f); + gyro_noise = _noise_scale * noiseGauss3f(0.01f, 0.01f, 0.01f); } Vector3f specific_force_B = R_E2B * _specific_force_E; diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index bda4484884..4328debc3c 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -272,6 +272,7 @@ private: MapProjection _lpos_ref{}; float _lpos_ref_alt; float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _T_TAU; + float _noise_scale; matrix::Matrix3f _I; // vehicle inertia matrix matrix::Matrix3f _Im1; // inverse of the inertia matrix @@ -301,6 +302,7 @@ private: (ParamFloat) _sih_distance_snsr_max, (ParamFloat) _sih_distance_snsr_override, (ParamFloat) _sih_thrust_tau, + (ParamFloat) _sih_noise_scale, (ParamInt) _sih_vtype ) }; diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index 25c9498e63..fabc091338 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/src/modules/simulation/simulator_sih/sih_params.c @@ -339,3 +339,12 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f); * @group Simulation In Hardware */ PARAM_DEFINE_INT32(SIH_VEHICLE_TYPE, 0); + +/** + * SIH noise scale + * + * Increase or decrease artificial sensor noise added artificially in SIH. + * + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_NOISE_SCALE, 0.01f);