From 892f199cb54b34a37a394d16cfe5ea1aea7247fa Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 13 Mar 2026 14:54:06 +0100 Subject: [PATCH] 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 | 43 +++++--------------- src/modules/simulation/simulator_sih/sih.hpp | 5 +-- 2 files changed, 12 insertions(+), 36 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 6c0db4ee88..fee2fa04fb 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -72,7 +72,15 @@ void Sih::run() _px4_accel.set_temperature(T1_C); _px4_gyro.set_temperature(T1_C); - init_position_reference(); + // 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(); @@ -266,37 +274,6 @@ void Sih::parameters_updated() _v_wind_N = Vector3f(_sih_wind_n.get(), _sih_wind_e.get(), 0.f); } -void Sih::init_position_reference() -{ - // Reset earth position, velocity and attitude - _lla.setLatitudeDeg(static_cast(_sih_lat0.get())); - _lla.setLongitudeDeg(static_cast(_sih_lon0.get())); - _lla.setAltitude(_sih_h0.get()); - _p_E = _lla.toEcef(); - - // Store ECEF reference position - this is the origin of the local frame - _p_E_ref = _p_E; - - // Get canonical LLA from ECEF for reference initialization - _lla = LatLonAlt::fromEcef(_p_E); - - // Initialize MapProjection reference (used for reprojection and publishing ref_lat/ref_lon) - _lpos_ref.initReference(_lla.latitude_deg(), _lla.longitude_deg()); - _lpos_ref_alt = _lla.altitude(); - - // Store reference frame rotation (fixed, used for local position computation) - _R_E2N_ref = computeRotEcefToNed(_lla); - - // Local position is exactly at origin since p_E == p_E_ref - _lpos.setZero(); - - _R_N2E = _R_E2N_ref.transpose(); - _v_E = _R_N2E * _v_N; - - _q_E = Quatf(_R_N2E) * _q; - _q_E.normalize(); -} - void Sih::read_motors(const float dt) { actuator_outputs_s actuators_out; @@ -403,7 +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 convention: _lpos(2) is negative when above reference, so altitude = ref - (-height) = ref + height + // NED frame: _lpos(2) above reference is negative float altitude = _lpos_ref_alt - _lpos(2); Vector3f Fa_ts{}; diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index aebc15869d..54febfafec 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -122,7 +122,6 @@ public: private: void parameters_updated(); - void init_position_reference(); // simulated sensors PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION @@ -229,8 +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 + 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