From 8f54dc402d2b9bd149878189b9238c42b7576c96 Mon Sep 17 00:00:00 2001
From: Nicolas MARTIN <nicolas1.martin1@gmail.com>
Date: Mon, 22 Feb 2021 11:07:28 +0100
Subject: [PATCH] SIH: add baro offset (or pressure change) from parameters

An absolute value superior to 10000 block barometer publication
---
 src/modules/sih/sih.cpp      |  6 ++++--
 src/modules/sih/sih.hpp      |  4 +++-
 src/modules/sih/sih_params.c | 10 ++++++++++
 3 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp
index ed34d573d4..ff4f9292f0 100644
--- a/src/modules/sih/sih.cpp
+++ b/src/modules/sih/sih.cpp
@@ -132,7 +132,8 @@ void Sih::Run()
 	}
 
 	// baro published at 20 Hz
-	if (_now - _baro_time >= 50_ms) {
+	if (_now - _baro_time >= 50_ms
+	    && fabs(_baro_offset_m) < 10000) {
 		_baro_time = _now;
 		_px4_baro.set_temperature(_baro_temp_c);
 		_px4_baro.update(_now, _baro_p_mBar);
@@ -184,6 +185,7 @@ void Sih::parameters_updated()
 	_mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
 
 	_gps_used = _sih_gps_used.get();
+	_baro_offset_m = _sih_baro_offset.get();
 }
 
 // initialization of the variables for the simulator
@@ -301,7 +303,7 @@ void Sih::reconstruct_sensors_signals()
 	_mag = _C_IB.transpose() * _mu_I + noiseGauss3f(0.02f, 0.02f, 0.03f);
 
 	// barometer
-	float altitude = (_H0 - _p_I(2)) + generate_wgn() * 0.14f; // altitude with noise
+	float altitude = (_H0 - _p_I(2)) + _baro_offset_m + generate_wgn() * 0.14f; // altitude with noise
 	_baro_p_mBar = CONSTANTS_STD_PRESSURE_MBAR *        // reconstructed pressure in mBar
 		       powf((1.0f + altitude * TEMP_GRADIENT / T1_K), -CONSTANTS_ONE_G / (TEMP_GRADIENT * CONSTANTS_AIR_GAS_CONST));
 	_baro_temp_c = T1_K + CONSTANTS_ABSOLUTE_NULL_CELSIUS + TEMP_GRADIENT * altitude; // reconstructed temperture in celcius
diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp
index f45907285a..48484f38df 100644
--- a/src/modules/sih/sih.hpp
+++ b/src/modules/sih/sih.hpp
@@ -176,6 +176,7 @@ private:
 	matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G]
 
 	int _gps_used;
+	float _baro_offset_m;
 
 	// parameters defined in sih_params.c
 	DEFINE_PARAMETERS(
@@ -200,6 +201,7 @@ private:
 		(ParamFloat<px4::params::SIH_LOC_MU_X>) _sih_mu_x,
 		(ParamFloat<px4::params::SIH_LOC_MU_Y>) _sih_mu_y,
 		(ParamFloat<px4::params::SIH_LOC_MU_Z>) _sih_mu_z,
-		(ParamInt<px4::params::SIH_GPS_USED>) _sih_gps_used
+		(ParamInt<px4::params::SIH_GPS_USED>) _sih_gps_used,
+		(ParamFloat<px4::params::SIH_BARO_OFFSET>) _sih_baro_offset
 	)
 };
diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c
index bf0ebc90a3..517e52cf46 100644
--- a/src/modules/sih/sih_params.c
+++ b/src/modules/sih/sih_params.c
@@ -352,3 +352,13 @@ PARAM_DEFINE_FLOAT(SIH_LOC_MU_Z,  0.504f);
  * @group Simulation In Hardware
  */
 PARAM_DEFINE_INT32(SIH_GPS_USED, 10);
+
+/**
+ * Barometer offset in meters
+ *
+ * Absolute value superior to 10000 will disable barometer
+ *
+ * @unit m
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_BARO_OFFSET,  0.0f);