SIH - change how LAT and LON is used for Takeoff location (#23363)

change how lat long is used for SIH
PX4 · Jul 8, 2024 · aed0fd4 · aed0fd4
1 parent 4bc0286
commit aed0fd4
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 15 deletions.
diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@@ -15,6 +15,9 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
 	if [ -n "${PX4_HOME_LON}" ]; then
 		param set SIH_LOC_LON0 ${PX4_HOME_LON}
 	fi
+	if [ -n "${PX4_HOME_ALT}" ]; then
+		param set SIH_LOC_H0 ${PX4_HOME_ALT}
+	fi
 
 	if simulator_sih start; then
 

diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp
@@ -243,8 +243,8 @@ void Sih::parameters_updated()
 	_KDW = _sih_kdw.get();
 	_H0 = _sih_h0.get();
 
-	_LAT0 = (double)_sih_lat0.get() * 1.0e-7;
-	_LON0 = (double)_sih_lon0.get() * 1.0e-7;
+	_LAT0 = (double)_sih_lat0.get();
+	_LON0 = (double)_sih_lon0.get();
 	_COS_LAT0 = cosl((long double)radians(_LAT0));
 
 	_MASS = _sih_mass.get();

diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp
@@ -273,8 +273,8 @@ class Sih : public ModuleBase<Sih>, public ModuleParams
 		(ParamFloat<px4::params::SIH_L_PITCH>) _sih_l_pitch,
 		(ParamFloat<px4::params::SIH_KDV>) _sih_kdv,
 		(ParamFloat<px4::params::SIH_KDW>) _sih_kdw,
-		(ParamInt<px4::params::SIH_LOC_LAT0>) _sih_lat0,
-		(ParamInt<px4::params::SIH_LOC_LON0>) _sih_lon0,
+		(ParamFloat<px4::params::SIH_LOC_LAT0>) _sih_lat0,
+		(ParamFloat<px4::params::SIH_LOC_LON0>) _sih_lon0,
 		(ParamFloat<px4::params::SIH_LOC_H0>) _sih_h0,
 		(ParamFloat<px4::params::SIH_DISTSNSR_MIN>) _sih_distance_snsr_min,
 		(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,

diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c
@@ -235,33 +235,31 @@ PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f);
  * Initial geodetic latitude
  *
  * This value represents the North-South location on Earth where the simulation begins.
- * A value of 45 deg should be written 450000000.
  *
  * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
  * to represent a physical ground location on Earth.
  *
- * @unit deg*1e7
- * @min -850000000
- * @max  850000000
+ * @unit deg
+ * @min -90
+ * @max  90
  * @group Simulation In Hardware
  */
-PARAM_DEFINE_INT32(SIH_LOC_LAT0, 454671160);
+PARAM_DEFINE_FLOAT(SIH_LOC_LAT0, 47.397742f);
 
 /**
  * Initial geodetic longitude
  *
  * This value represents the East-West location on Earth where the simulation begins.
- * A value of 45 deg should be written 450000000.
  *
  * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
  * to represent a physical ground location on Earth.
  *
- * @unit deg*1e7
- * @min -1800000000
- * @max  1800000000
+ * @unit deg
+ * @min -180
+ * @max  180
  * @group Simulation In Hardware
  */
-PARAM_DEFINE_INT32(SIH_LOC_LON0, -737578370);
+PARAM_DEFINE_FLOAT(SIH_LOC_LON0, 8.545594f);
 
 /**
  * Initial AMSL ground altitude
@@ -282,7 +280,7 @@ PARAM_DEFINE_INT32(SIH_LOC_LON0, -737578370);
  * @increment 0.01
  * @group Simulation In Hardware
  */
-PARAM_DEFINE_FLOAT(SIH_LOC_H0, 32.34f);
+PARAM_DEFINE_FLOAT(SIH_LOC_H0, 489.4f);
 
 /**
  * distance sensor minimum range