From 9cb3ea44fbb787f05895bc790acf8f0a1fc696b1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 2 Mar 2026 23:22:35 +0100 Subject: [PATCH] sih: cleanup initializing variables in constructor (#26578) --- src/modules/simulation/simulator_sih/sih.cpp | 22 +++----------------- src/modules/simulation/simulator_sih/sih.hpp | 18 ++-------------- 2 files changed, 5 insertions(+), 35 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 68b9e380de..dc27fa0fd4 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -57,7 +57,9 @@ ModuleBase::Descriptor Sih::desc{task_spawn, custom_command, print_usage}; Sih::Sih() : ModuleParams(nullptr) -{} +{ + srand(1234); // initialize the random seed once before calling generate_wgn() +} Sih::~Sih() { @@ -70,7 +72,6 @@ void Sih::run() _px4_accel.set_temperature(T1_C); _px4_gyro.set_temperature(T1_C); - init_variables(); parameters_updated(); const hrt_abstime task_start = hrt_absolute_time(); @@ -100,7 +101,6 @@ static uint64_t micros() void Sih::lockstep_loop() { - int rate = math::min(_imu_gyro_ratemax.get(), _imu_integration_rate.get()); // default to 400Hz (2500 us interval) @@ -284,22 +284,6 @@ void Sih::parameters_updated() _T_TAU = _sih_thrust_tau.get(); } -void Sih::init_variables() -{ - srand(1234); // initialize the random seed once before calling generate_wgn() - - _lpos = Vector3f(0.0f, 0.0f, 0.0f); - _v_N = Vector3f(0.0f, 0.0f, 0.0f); - _v_N_dot = Vector3f(0.0f, 0.0f, 0.0f); - _p_E = Vector3d(Wgs84::equatorial_radius, 0.0, 0.0); - _v_E = Vector3f(0.0f, 0.0f, 0.0f); - _q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); - _q_E = Quatf(Eulerf(0.f, -M_PI_2_F, 0.f)); - _w_B = Vector3f(0.0f, 0.0f, 0.0f); - - _u[0] = _u[1] = _u[2] = _u[3] = 0.0f; -} - void Sih::read_motors(const float dt) { actuator_outputs_s actuators_out; diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 8504d26285..94cc75cafb 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -150,8 +150,6 @@ private: static constexpr float RP = 0.1f; // radius of the propeller [m] static constexpr float FLAP_MAX = M_PI_F / 12.0f; // 15 deg, maximum control surface deflection - void init_variables(); - // read the motor signals outputted from the mixer void read_motors(const float dt); @@ -218,8 +216,8 @@ private: // Quantities in Earth-centered-Earth-fixed coordinates matrix::Vector3f _Fa_E{}; // aerodynamic force in ECEF frame [N] matrix::Vector3f _specific_force_E{}; - matrix::Quatf _q_E{}; - matrix::Vector3d _p_E{}; + matrix::Quatf _q_E{matrix::Eulerf(0.f, -M_PI_2_F, 0.f)}; + matrix::Vector3d _p_E{Wgs84::equatorial_radius, 0.0, 0.0}; matrix::Vector3f _v_E{}; matrix::Vector3f _v_E_dot{}; matrix::Dcmf _R_N2E; // local navigation to ECEF frame rotation matrix @@ -261,18 +259,6 @@ private: AeroSeg(0.0225f, 0.110f, 0.0f, matrix::Vector3f(0.083f - TS_CM, 0.239f, 0.0f), 0.0f, TS_AR) }; - // AeroSeg _ts[NB_TS_SEG] = { - // AeroSeg(0.0225f, 0.110f, -90.0f, matrix::Vector3f(0.0f, -0.239f, TS_CM-0.083f), 0.0f, TS_AR), - // AeroSeg(0.0383f, 0.125f, -90.0f, matrix::Vector3f(0.0f, -0.208f, TS_CM-0.094f), 0.0f, TS_AR, 0.063f), - // AeroSeg(0.0884f, 0.148f, -90.0f, matrix::Vector3f(0.0f, -0.143f, TS_CM-0.111f), 0.0f, TS_AR, 0.063f, TS_RP), - // AeroSeg(0.0633f, 0.176f, -90.0f, matrix::Vector3f(0.0f, -0.068f, TS_CM-0.132f), 0.0f, TS_AR, 0.063f), - // AeroSeg(0.0750f, 0.231f, -90.0f, matrix::Vector3f(0.0f, 0.000f, TS_CM-0.173f), 0.0f, TS_AR), - // AeroSeg(0.0633f, 0.176f, -90.0f, matrix::Vector3f(0.0f, 0.068f, TS_CM-0.132f), 0.0f, TS_AR, 0.063f), - // AeroSeg(0.0884f, 0.148f, -90.0f, matrix::Vector3f(0.0f, 0.143f, TS_CM-0.111f), 0.0f, TS_AR, 0.063f, TS_RP), - // AeroSeg(0.0383f, 0.125f, -90.0f, matrix::Vector3f(0.0f, 0.208f, TS_CM-0.094f), 0.0f, TS_AR, 0.063f), - // AeroSeg(0.0225f, 0.110f, -90.0f, matrix::Vector3f(0.0f, 0.239f, TS_CM-0.083f), 0.0f, TS_AR) - // }; - // parameters MapProjection _lpos_ref{}; float _lpos_ref_alt;