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.

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;
 

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<px4::params::SIH_NOISE_SCALE>) _sih_noise_scale,
 		(ParamInt<px4::params::SIM_ARSPD_FAIL>) _sim_failure
 	)
 };

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;
 

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<px4::params::SIH_NOISE_SCALE>) _sih_noise_scale,
 		(ParamFloat<px4::params::SIM_BARO_OFF_P>) _sim_baro_off_p,
 		(ParamFloat<px4::params::SIM_BARO_OFF_T>) _sim_baro_off_t
 	)

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;

src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp

@@ -91,6 +91,7 @@ private:
 	bool _gps_blocked{false};
 
 	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SIH_NOISE_SCALE>) _sih_noise_scale,
 		(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used
 	)
 };

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;
 

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;

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<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
 		(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,
 		(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau,
+		(ParamFloat<px4::params::SIH_NOISE_SCALE>) _sih_noise_scale,
 		(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype
 	)
 };

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