refactor(sih): some suggestions from trying to review

Honestly I don't understand why there needs to be another reference. I'll need to continue reviewing later since it's not trivial.

src/modules/simulation/simulator_sih/sih.cpp

@@ -72,6 +72,15 @@ void Sih::run()
 	_px4_accel.set_temperature(T1_C);
 	_px4_gyro.set_temperature(T1_C);
 
+	// Initialize position, velocity and attitude
+	_lla = LatLonAlt(_sih_lat0.get(), _sih_lon0.get(), _sih_h0.get());
+	_p_E = _p_E_ref = _lla.toEcef();
+	_lpos_ref.initReference(_lla.latitude_deg(), _lla.longitude_deg()); // Initialize MapProjection reference
+	_lpos_ref_alt = _lla.altitude();
+	_R_E2N_ref = computeRotEcefToNed(_lla); // Reference frame rotation (fixed, used for local position computation)
+	_R_N2E = _R_E2N_ref.transpose();
+	_q_E = Quatf(_R_N2E) * _q;
+
 	parameters_updated();
 
 	const hrt_abstime task_start = hrt_absolute_time();
@@ -246,27 +255,6 @@ void Sih::parameters_updated()
 	_KDV = _sih_kdv.get();
 	_KDW = _sih_kdw.get();
 
-	if (!_lpos_ref.isInitialized()
-	    || (fabsf(static_cast<float>(_lpos_ref.getProjectionReferenceLat()) - _sih_lat0.get()) > FLT_EPSILON)
-	    || (fabsf(static_cast<float>(_lpos_ref.getProjectionReferenceLon()) - _sih_lon0.get()) > FLT_EPSILON)
-	    || (fabsf(_lpos_ref_alt - _sih_h0.get()) > FLT_EPSILON)) {
-		_lpos_ref.initReference(static_cast<double>(_sih_lat0.get()), static_cast<double>(_sih_lon0.get()));
-		_lpos_ref_alt = _sih_h0.get();
-
-		// Reset earth position, velocity and attitude
-		_lla.setLatitudeDeg(static_cast<double>(_sih_lat0.get()));
-		_lla.setLongitudeDeg(static_cast<double>(_sih_lon0.get()));
-		_lla.setAltitude(_lpos_ref_alt);
-		_p_E = _lla.toEcef();
-
-		const Dcmf R_E2N = computeRotEcefToNed(_lla);
-		_R_N2E = R_E2N.transpose();
-		_v_E = _R_N2E * _v_N;
-
-		_q_E = Quatf(_R_N2E) * _q;
-		_q_E.normalize();
-	}
-
 	_MASS = _sih_mass.get();
 
 	_I = diag(Vector3f(_sih_ixx.get(), _sih_iyy.get(), _sih_izz.get()));
@@ -392,6 +380,7 @@ void Sih::generate_ts_aerodynamics()
 	// the aerodynamic is resolved in a frame like a standard aircraft (nose-right-belly)
 	Vector3f v_ts = _R_S2B.transpose() * v_B;
 	Vector3f w_ts = _R_S2B.transpose() * _w_B;
+	// NED frame: _lpos(2) above reference is negative
 	float altitude = _lpos_ref_alt - _lpos(2);
 
 	Vector3f Fa_ts{};
@@ -563,8 +552,10 @@ void Sih::equations_of_motion(const float dt)
 
 	ecefToNed();
 
-	_lpos_ref.project(_lla.latitude_deg(), _lla.longitude_deg(), _lpos(0), _lpos(1));
-	_lpos(2) = -(_lla.altitude() - _lpos_ref_alt);
+	// Compute local position directly from ECEF difference (avoids lat/lon precision issues)
+	// Use the fixed reference rotation to maintain a consistent local frame
+	const Vector3d dp_E = _p_E - _p_E_ref;
+	_lpos = _R_E2N_ref * Vector3f(dp_E);
 }
 
 void Sih::ecefToNed()
@@ -741,13 +732,18 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us)
 
 	{
 		// publish global position groundtruth
+		// Use MapProjection reproject to ensure consistency with local position
 		vehicle_global_position_s global_position{};
 		global_position.timestamp_sample = time_now_us;
-		global_position.lat = _lla.latitude_deg();
-		global_position.lon = _lla.longitude_deg();
-		global_position.alt = _lla.altitude();
+
+		double lat, lon;
+		_lpos_ref.reproject(_lpos(0), _lpos(1), lat, lon);
+		global_position.lat = lat;
+		global_position.lon = lon;
+		// NED convention: _lpos(2) is negative when above reference, so alt = ref - (-height) = ref + height
+		global_position.alt = static_cast<double>(_lpos_ref_alt) - static_cast<double>(_lpos(2));
 		global_position.alt_ellipsoid = global_position.alt;
-		global_position.terrain_alt = -_lpos(2);
+		global_position.terrain_alt = static_cast<double>(_lpos_ref_alt);
 		global_position.timestamp = hrt_absolute_time();
 		_global_position_ground_truth_pub.publish(global_position);
 	}

src/modules/simulation/simulator_sih/sih.hpp

@@ -228,6 +228,8 @@ private:
 
 	LatLonAlt _lla{};
 	matrix::Vector3f _lpos{};  // position in a local tangent-plane frame [m]
+	matrix::Vector3d _p_E_ref{}; // ECEF reference position for local frame origin [m]
+	matrix::Dcmf _R_E2N_ref{}; // Rotation from ECEF to reference NED frame
 
 	float _u[NUM_ACTUATORS_MAX] {}; // thruster signals
 

src/modules/simulation/simulator_sih/sih_params.c

@@ -242,6 +242,7 @@ PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f);
  * @unit deg
  * @min -90
  * @max  90
+ * @reboot_required true
  * @group Simulation In Hardware
  */
 PARAM_DEFINE_FLOAT(SIH_LOC_LAT0, 47.397742f);
@@ -257,6 +258,7 @@ PARAM_DEFINE_FLOAT(SIH_LOC_LAT0, 47.397742f);
  * @unit deg
  * @min -180
  * @max  180
+ * @reboot_required true
  * @group Simulation In Hardware
  */
 PARAM_DEFINE_FLOAT(SIH_LOC_LON0, 8.545594f);
@@ -278,6 +280,7 @@ PARAM_DEFINE_FLOAT(SIH_LOC_LON0, 8.545594f);
  * @max 8848.0
  * @decimal 2
  * @increment 0.01
+ * @reboot_required true
  * @group Simulation In Hardware
  */
 PARAM_DEFINE_FLOAT(SIH_LOC_H0, 489.4f);