From 1df63cb6b1c510bb303f98a685533b1ea0c28359 Mon Sep 17 00:00:00 2001
From: Nicolas MARTIN <nicolas1.martin1@gmail.com>
Date: Tue, 2 Feb 2021 14:19:55 +0100
Subject: [PATCH] SIH: add gps fix loss simulation from parameters

A new parameter allows to change the SIH number of gps satellites used
If it is below 4, fix is lost
---
 src/modules/sih/sih.cpp      | 30 +++++++++++++++++++++++++++---
 src/modules/sih/sih.hpp      |  8 ++++++--
 src/modules/sih/sih_params.c |  9 +++++++++
 3 files changed, 42 insertions(+), 5 deletions(-)

diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp
index 6cb2c7121f..ed34d573d4 100644
--- a/src/modules/sih/sih.cpp
+++ b/src/modules/sih/sih.cpp
@@ -61,7 +61,7 @@ Sih::Sih() :
 
 	parameters_updated();
 	init_variables();
-	init_sensors();
+	gps_no_fix();
 
 	const hrt_abstime task_start = hrt_absolute_time();
 	_last_run = task_start;
@@ -182,6 +182,8 @@ void Sih::parameters_updated()
 	_Im1 = inv(_I);
 
 	_mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
+
+	_gps_used = _sih_gps_used.get();
 }
 
 // initialization of the variables for the simulator
@@ -197,10 +199,10 @@ void Sih::init_variables()
 	_u[0] = _u[1] = _u[2] = _u[3] = 0.0f;
 }
 
-void Sih::init_sensors()
+void Sih::gps_fix()
 {
 	_sensor_gps.fix_type = 3;  // 3D fix
-	_sensor_gps.satellites_used = 8;
+	_sensor_gps.satellites_used = _gps_used;
 	_sensor_gps.heading = NAN;
 	_sensor_gps.heading_offset = NAN;
 	_sensor_gps.s_variance_m_s = 0.5f;
@@ -211,6 +213,21 @@ void Sih::init_sensors()
 	_sensor_gps.vdop = 1.1f;
 }
 
+void Sih::gps_no_fix()
+{
+	_sensor_gps.fix_type = 0;  // 3D fix
+	_sensor_gps.satellites_used = _gps_used;
+	_sensor_gps.heading = NAN;
+	_sensor_gps.heading_offset = NAN;
+	_sensor_gps.s_variance_m_s = 100.f;
+	_sensor_gps.c_variance_rad = 100.f;
+	_sensor_gps.eph = 100.f;
+	_sensor_gps.epv = 100.f;
+	_sensor_gps.hdop = 100.f;
+	_sensor_gps.vdop = 100.f;
+}
+
+
 // read the motor signals outputted from the mixer
 void Sih::read_motors()
 {
@@ -316,6 +333,13 @@ void Sih::send_gps()
 	_sensor_gps.cog_rad = atan2(_gps_vel(1),
 				    _gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
 
+	if (_gps_used >= 4) {
+		gps_fix();
+
+	} else {
+		gps_no_fix();
+	}
+
 	_sensor_gps_pub.publish(_sensor_gps);
 }
 
diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp
index 960eda4743..f45907285a 100644
--- a/src/modules/sih/sih.hpp
+++ b/src/modules/sih/sih.hpp
@@ -118,7 +118,8 @@ private:
 	static constexpr float TEMP_GRADIENT  = -6.5f / 1000.0f;    // temperature gradient in degrees per metre
 
 	void init_variables();
-	void init_sensors();
+	void gps_fix();
+	void gps_no_fix();
 	void read_motors();
 	void generate_force_and_torques();
 	void equations_of_motion();
@@ -174,6 +175,8 @@ private:
 	matrix::Matrix3f _Im1;  // inverse of the intertia matrix
 	matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G]
 
+	int _gps_used;
+
 	// parameters defined in sih_params.c
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _imu_gyro_ratemax,
@@ -196,6 +199,7 @@ private:
 		(ParamFloat<px4::params::SIH_LOC_H0>) _sih_h0,
 		(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
+		(ParamFloat<px4::params::SIH_LOC_MU_Z>) _sih_mu_z,
+		(ParamInt<px4::params::SIH_GPS_USED>) _sih_gps_used
 	)
 };
diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c
index cefc23365e..bf0ebc90a3 100644
--- a/src/modules/sih/sih_params.c
+++ b/src/modules/sih/sih_params.c
@@ -343,3 +343,12 @@ PARAM_DEFINE_FLOAT(SIH_LOC_MU_Y, -0.045f);
  * @group Simulation In Hardware
  */
 PARAM_DEFINE_FLOAT(SIH_LOC_MU_Z,  0.504f);
+
+/**
+ * Number of GPS satellites used
+ *
+ * @min 0
+ * @max  50
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_INT32(SIH_GPS_USED, 10);