From b8bf76e3448e0adf97b710d98c664d43851875c9 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:03:53 -0700 Subject: [PATCH 01/49] Rename namespaec math to s2e::math --- src/components/base/sensor.hpp | 18 +- .../base/sensor_template_functions.hpp | 34 ++-- .../examples/example_change_structure.cpp | 2 +- .../ideal/angular_velocity_observer.hpp | 4 +- src/components/ideal/attitude_observer.cpp | 6 +- src/components/ideal/attitude_observer.hpp | 4 +- src/components/ideal/force_generator.cpp | 32 ++-- src/components/ideal/force_generator.hpp | 16 +- src/components/ideal/orbit_observer.cpp | 14 +- src/components/ideal/orbit_observer.hpp | 10 +- src/components/ideal/torque_generator.cpp | 16 +- src/components/ideal/torque_generator.hpp | 8 +- src/components/real/aocs/gnss_receiver.cpp | 60 +++---- src/components/real/aocs/gnss_receiver.hpp | 32 ++-- src/components/real/aocs/gyro_sensor.cpp | 8 +- src/components/real/aocs/gyro_sensor.hpp | 10 +- src/components/real/aocs/magnetometer.cpp | 8 +- src/components/real/aocs/magnetometer.hpp | 16 +- src/components/real/aocs/magnetorquer.cpp | 64 +++---- src/components/real/aocs/magnetorquer.hpp | 50 +++--- .../aocs/mtq_magnetometer_interference.cpp | 10 +- .../aocs/mtq_magnetometer_interference.hpp | 4 +- src/components/real/aocs/reaction_wheel.cpp | 38 ++--- src/components/real/aocs/reaction_wheel.hpp | 28 +-- .../real/aocs/reaction_wheel_jitter.cpp | 6 +- .../real/aocs/reaction_wheel_jitter.hpp | 40 ++--- .../real/aocs/reaction_wheel_ode.cpp | 4 +- .../real/aocs/reaction_wheel_ode.hpp | 4 +- src/components/real/aocs/star_sensor.cpp | 66 ++++---- src/components/real/aocs/star_sensor.hpp | 30 ++-- src/components/real/aocs/sun_sensor.cpp | 36 ++-- src/components/real/aocs/sun_sensor.hpp | 14 +- src/components/real/communication/antenna.cpp | 4 +- src/components/real/communication/antenna.hpp | 8 +- .../antenna_radiation_pattern.hpp | 6 +- .../ground_station_calculator.cpp | 2 +- src/components/real/mission/telescope.cpp | 48 +++--- src/components/real/mission/telescope.hpp | 26 +-- .../real/power/csv_scenario_interface.cpp | 4 +- .../real/power/csv_scenario_interface.hpp | 2 +- .../real/power/solar_array_panel.cpp | 18 +- .../real/power/solar_array_panel.hpp | 8 +- .../real/propulsion/simple_thruster.cpp | 30 ++-- src/disturbances/air_drag.cpp | 20 +-- src/disturbances/air_drag.hpp | 6 +- src/disturbances/disturbance.hpp | 24 +-- src/disturbances/disturbances.hpp | 6 +- src/disturbances/geopotential.cpp | 8 +- src/disturbances/geopotential.hpp | 2 +- src/disturbances/gravity_gradient.cpp | 4 +- src/disturbances/gravity_gradient.hpp | 2 +- src/disturbances/lunar_gravity_field.cpp | 12 +- src/disturbances/lunar_gravity_field.hpp | 2 +- src/disturbances/magnetic_disturbance.cpp | 4 +- src/disturbances/magnetic_disturbance.hpp | 4 +- .../solar_radiation_pressure_disturbance.cpp | 6 +- .../solar_radiation_pressure_disturbance.hpp | 4 +- src/disturbances/surface_force.cpp | 24 +-- src/disturbances/surface_force.hpp | 10 +- src/disturbances/third_body_gravity.cpp | 14 +- src/disturbances/third_body_gravity.hpp | 4 +- src/dynamics/attitude/attitude.cpp | 24 +-- src/dynamics/attitude/attitude.hpp | 36 ++-- src/dynamics/attitude/attitude_rk4.cpp | 34 ++-- src/dynamics/attitude/attitude_rk4.hpp | 12 +- .../attitude_with_cantilever_vibration.cpp | 20 +-- .../attitude_with_cantilever_vibration.hpp | 10 +- src/dynamics/attitude/controlled_attitude.cpp | 58 +++---- src/dynamics/attitude/controlled_attitude.hpp | 28 +-- src/dynamics/attitude/initialize_attitude.cpp | 20 +-- src/dynamics/attitude/initialize_attitude.hpp | 2 +- ...ode_attitude_with_cantilever_vibration.hpp | 76 ++++----- src/dynamics/dynamics.cpp | 2 +- src/dynamics/dynamics.hpp | 6 +- .../orbit/encke_orbit_propagation.cpp | 18 +- .../orbit/encke_orbit_propagation.hpp | 18 +- src/dynamics/orbit/initialize_orbit.cpp | 34 ++-- src/dynamics/orbit/initialize_orbit.hpp | 2 +- src/dynamics/orbit/orbit.cpp | 26 +-- src/dynamics/orbit/orbit.hpp | 40 ++--- src/dynamics/orbit/relative_orbit.cpp | 28 +-- src/dynamics/orbit/relative_orbit.hpp | 16 +- src/dynamics/orbit/rk4_orbit_propagation.cpp | 8 +- src/dynamics/orbit/rk4_orbit_propagation.hpp | 8 +- src/dynamics/thermal/heatload.cpp | 2 +- src/dynamics/thermal/node.cpp | 8 +- src/dynamics/thermal/node.hpp | 6 +- src/dynamics/thermal/temperature.cpp | 8 +- src/dynamics/thermal/temperature.hpp | 6 +- .../global/celestial_information.hpp | 30 ++-- src/environment/global/earth_rotation.cpp | 160 +++++++++--------- src/environment/global/earth_rotation.hpp | 16 +- src/environment/global/gnss_satellites.cpp | 12 +- src/environment/global/gnss_satellites.hpp | 6 +- .../global/hipparcos_catalogue.cpp | 14 +- .../global/hipparcos_catalogue.hpp | 4 +- src/environment/global/moon_rotation.cpp | 8 +- src/environment/global/moon_rotation.hpp | 4 +- src/environment/local/atmosphere.cpp | 2 +- src/environment/local/geomagnetic_field.cpp | 6 +- src/environment/local/geomagnetic_field.hpp | 10 +- .../local/local_celestial_information.cpp | 48 +++--- .../local/local_celestial_information.hpp | 22 +-- .../solar_radiation_pressure_environment.cpp | 10 +- src/logger/log_utility.hpp | 12 +- .../atmosphere/harris_priester_model.cpp | 10 +- .../atmosphere/harris_priester_model.hpp | 2 +- .../atmosphere/wrapper_nrlmsise00.cpp | 6 +- .../geodesy/geodetic_position.cpp | 10 +- .../geodesy/geodetic_position.hpp | 8 +- src/math_physics/gnss/antex_file_reader.cpp | 2 +- src/math_physics/gnss/antex_file_reader.hpp | 6 +- src/math_physics/gnss/sp3_file_reader.cpp | 10 +- src/math_physics/gnss/sp3_file_reader.hpp | 10 +- .../gravity/gravity_potential.cpp | 8 +- .../gravity/gravity_potential.hpp | 4 +- .../gravity/test_gravity_potential.cpp | 40 ++--- src/math_physics/math/constants.hpp | 4 +- src/math_physics/math/interpolation.cpp | 4 +- src/math_physics/math/interpolation.hpp | 4 +- src/math_physics/math/matrix.hpp | 4 +- .../math/matrix_template_functions.hpp | 4 +- src/math_physics/math/matrix_vector.hpp | 4 +- .../math/matrix_vector_template_functions.hpp | 4 +- .../math/ordinary_differential_equation.hpp | 6 +- ...fferential_equation_template_functions.hpp | 4 +- src/math_physics/math/quaternion.cpp | 4 +- src/math_physics/math/quaternion.hpp | 4 +- src/math_physics/math/s2e_math.cpp | 12 +- src/math_physics/math/s2e_math.hpp | 4 +- src/math_physics/math/test_interpolation.cpp | 30 ++-- src/math_physics/math/test_matrix.cpp | 60 +++---- src/math_physics/math/test_matrix_vector.cpp | 10 +- src/math_physics/math/test_quaternion.cpp | 134 +++++++-------- src/math_physics/math/test_s2e_math.cpp | 14 +- src/math_physics/math/test_vector.cpp | 94 +++++----- src/math_physics/math/vector.cpp | 4 +- src/math_physics/math/vector.hpp | 4 +- .../math/vector_template_functions.hpp | 4 +- .../dormand_prince_5.hpp | 4 +- .../dormand_prince_5_implementation.hpp | 6 +- .../embedded_runge_kutta_implementation.hpp | 6 +- .../numerical_integration/interface_ode.hpp | 2 +- .../numerical_integrator.hpp | 10 +- .../numerical_integration/ode_examples.hpp | 16 +- .../numerical_integration/runge_kutta.hpp | 2 +- .../numerical_integration/runge_kutta_4.hpp | 2 +- .../runge_kutta_fehlberg.hpp | 2 +- .../runge_kutta_fehlberg_implementation.hpp | 8 +- .../runge_kutta_template.hpp | 4 +- .../test_runge_kutta.cpp | 86 +++++----- .../optics/gaussian_beam_base.cpp | 8 +- .../optics/gaussian_beam_base.hpp | 12 +- .../orbit/interpolation_orbit.cpp | 8 +- .../orbit/interpolation_orbit.hpp | 6 +- src/math_physics/orbit/kepler_orbit.cpp | 16 +- src/math_physics/orbit/kepler_orbit.hpp | 10 +- src/math_physics/orbit/orbital_elements.cpp | 16 +- src/math_physics/orbit/orbital_elements.hpp | 8 +- .../orbit/relative_orbit_models.cpp | 8 +- .../orbit/relative_orbit_models.hpp | 4 +- .../orbit/test_interpolation_orbit.cpp | 4 +- .../moon_rotation_utilities.cpp | 30 ++-- .../moon_rotation_utilities.hpp | 6 +- .../randomization/random_walk.hpp | 8 +- .../random_walk_template_functions.hpp | 6 +- .../initialize_file_access.cpp | 4 +- .../initialize_file_access.hpp | 6 +- .../ground_station/ground_station.cpp | 14 +- .../initialize_monte_carlo_parameters.cpp | 70 ++++---- .../initialize_monte_carlo_parameters.hpp | 26 +-- .../initialize_monte_carlo_simulation.cpp | 4 +- .../monte_carlo_simulation_executor.cpp | 2 +- .../monte_carlo_simulation_executor.hpp | 12 +- .../simulation_object.cpp | 2 +- .../simulation_object.hpp | 6 +- .../relative_information.cpp | 58 +++---- .../relative_information.hpp | 26 +-- .../spacecraft/installed_components.cpp | 8 +- .../spacecraft/installed_components.hpp | 4 +- .../structure/initialize_structure.cpp | 8 +- .../structure/kinematics_parameters.cpp | 2 +- .../structure/kinematics_parameters.hpp | 14 +- .../structure/residual_magnetic_moment.hpp | 2 +- .../spacecraft/structure/surface.cpp | 2 +- .../spacecraft/structure/surface.hpp | 14 +- .../spacecraft/sample_components.cpp | 14 +- .../spacecraft/sample_components.hpp | 4 +- 188 files changed, 1528 insertions(+), 1528 deletions(-) diff --git a/src/components/base/sensor.hpp b/src/components/base/sensor.hpp index f3ab2e22b..398b24263 100644 --- a/src/components/base/sensor.hpp +++ b/src/components/base/sensor.hpp @@ -31,9 +31,9 @@ class Sensor { * @param [in] random_walk_standard_deviation_c: Standard deviation of random wark at the component frame * @param [in] random_walk_limit_c: Limit of random walk at the component frame */ - Sensor(const math::Matrix& scale_factor, const math::Vector& range_to_const_c, const math::Vector& range_to_zero_c, - const math::Vector& bias_noise_c, const math::Vector& normal_random_standard_deviation_c, const double random_walk_step_width_s, - const math::Vector& random_walk_standard_deviation_c, const math::Vector& random_walk_limit_c); + Sensor(const s2e::math::Matrix& scale_factor, const s2e::math::Vector& range_to_const_c, const s2e::math::Vector& range_to_zero_c, + const s2e::math::Vector& bias_noise_c, const s2e::math::Vector& normal_random_standard_deviation_c, const double random_walk_step_width_s, + const s2e::math::Vector& random_walk_standard_deviation_c, const s2e::math::Vector& random_walk_limit_c); /** * @fn ~Sensor * @brief Destructor @@ -41,7 +41,7 @@ class Sensor { ~Sensor(); protected: - math::Vector bias_noise_c_; //!< Constant bias noise at the component frame + s2e::math::Vector bias_noise_c_; //!< Constant bias noise at the component frame /** * @fn Measure @@ -49,12 +49,12 @@ class Sensor { * @param [in] true_value_c: True value at the component frame * @return Observed value with noise at the component frame */ - math::Vector Measure(const math::Vector true_value_c); + s2e::math::Vector Measure(const s2e::math::Vector true_value_c); private: - math::Matrix scale_factor_; //!< Scale factor matrix - math::Vector range_to_const_c_; //!< Output range limit to be constant output value at the component frame - math::Vector range_to_zero_c_; //!< Output range limit to be zero output value at the component frame + s2e::math::Matrix scale_factor_; //!< Scale factor matrix + s2e::math::Vector range_to_const_c_; //!< Output range limit to be constant output value at the component frame + s2e::math::Vector range_to_zero_c_; //!< Output range limit to be zero output value at the component frame randomization::NormalRand normal_random_noise_c_[N]; //!< Normal random RandomWalk random_walk_noise_c_; //!< Random Walk @@ -64,7 +64,7 @@ class Sensor { * @param [in] input_c: Input value at the component frame * @return Clipped value */ - math::Vector Clip(const math::Vector input_c); + s2e::math::Vector Clip(const s2e::math::Vector input_c); /** * @fn RangeCheck * @brief Check the range_to_const_c_ and range_to_zero_c_ is correct and fixed the values diff --git a/src/components/base/sensor_template_functions.hpp b/src/components/base/sensor_template_functions.hpp index 247093ce2..ef6f9a7e5 100644 --- a/src/components/base/sensor_template_functions.hpp +++ b/src/components/base/sensor_template_functions.hpp @@ -10,10 +10,10 @@ #include template -Sensor::Sensor(const math::Matrix& scale_factor, const math::Vector& range_to_const_c, const math::Vector& range_to_zero_c, - const math::Vector& bias_noise_c, const math::Vector& normal_random_standard_deviation_c, - const double random_walk_step_width_s, const math::Vector& random_walk_standard_deviation_c, - const math::Vector& random_walk_limit_c) +Sensor::Sensor(const s2e::math::Matrix& scale_factor, const s2e::math::Vector& range_to_const_c, const s2e::math::Vector& range_to_zero_c, + const s2e::math::Vector& bias_noise_c, const s2e::math::Vector& normal_random_standard_deviation_c, + const double random_walk_step_width_s, const s2e::math::Vector& random_walk_standard_deviation_c, + const s2e::math::Vector& random_walk_limit_c) : bias_noise_c_(bias_noise_c), scale_factor_(scale_factor), range_to_const_c_(range_to_const_c), @@ -29,8 +29,8 @@ template Sensor::~Sensor() {} template -math::Vector Sensor::Measure(const math::Vector true_value_c) { - math::Vector calc_value_c; +s2e::math::Vector Sensor::Measure(const s2e::math::Vector true_value_c) { + s2e::math::Vector calc_value_c; calc_value_c = scale_factor_ * true_value_c; calc_value_c += bias_noise_c_; for (size_t i = 0; i < N; ++i) { @@ -42,8 +42,8 @@ math::Vector Sensor::Measure(const math::Vector true_value_c) { } template -math::Vector Sensor::Clip(const math::Vector input_c) { - math::Vector output_c; +s2e::math::Vector Sensor::Clip(const s2e::math::Vector input_c) { + s2e::math::Vector output_c; for (size_t i = 0; i < N; ++i) { if (input_c[i] >= range_to_const_c_[i] && input_c[i] < range_to_zero_c_[i]) { output_c[i] = range_to_const_c_[i]; @@ -80,11 +80,11 @@ Sensor ReadSensorInformation(const std::string file_name, const double step_w IniAccess ini_file(file_name); std::string section = "SENSOR_BASE_" + component_name; - math::Vector scale_factor_vector; + s2e::math::Vector scale_factor_vector; ini_file.ReadVector(section.c_str(), "scale_factor_c", scale_factor_vector); - math::Matrix scale_factor_c; + s2e::math::Matrix scale_factor_c; if (scale_factor_vector.CalcNorm() == 0.0) { - scale_factor_c = math::MakeIdentityMatrix(); + scale_factor_c = s2e::math::MakeIdentityMatrix(); } else { for (size_t i = 0; i < N; i++) { for (size_t j = 0; j < N; j++) { @@ -94,26 +94,26 @@ Sensor ReadSensorInformation(const std::string file_name, const double step_w } std::string key_name; - math::Vector constant_bias_c; + s2e::math::Vector constant_bias_c; key_name = "constant_bias_c_" + unit; ini_file.ReadVector(section.c_str(), key_name.c_str(), constant_bias_c); - math::Vector normal_random_standard_deviation_c; + s2e::math::Vector normal_random_standard_deviation_c; key_name = "normal_random_standard_deviation_c_" + unit; ini_file.ReadVector(section.c_str(), key_name.c_str(), normal_random_standard_deviation_c); - math::Vector random_walk_standard_deviation_c; + s2e::math::Vector random_walk_standard_deviation_c; key_name = "random_walk_standard_deviation_c_" + unit; ini_file.ReadVector(section.c_str(), key_name.c_str(), random_walk_standard_deviation_c); - math::Vector random_walk_limit_c; + s2e::math::Vector random_walk_limit_c; key_name = "random_walk_limit_c_" + unit; ini_file.ReadVector(section.c_str(), key_name.c_str(), random_walk_limit_c); key_name = "range_to_constant_" + unit; double range_to_const = ini_file.ReadDouble(section.c_str(), key_name.c_str()); - math::Vector range_to_const_c{range_to_const}; + s2e::math::Vector range_to_const_c{range_to_const}; key_name = "range_to_zero_" + unit; double range_to_zero = ini_file.ReadDouble(section.c_str(), key_name.c_str()); - math::Vector range_to_zero_c{range_to_zero}; + s2e::math::Vector range_to_zero_c{range_to_zero}; Sensor sensor_base(scale_factor_c, range_to_const_c, range_to_zero_c, constant_bias_c, normal_random_standard_deviation_c, step_width_s, random_walk_standard_deviation_c, random_walk_limit_c); diff --git a/src/components/examples/example_change_structure.cpp b/src/components/examples/example_change_structure.cpp index 87e589700..bc28f4547 100644 --- a/src/components/examples/example_change_structure.cpp +++ b/src/components/examples/example_change_structure.cpp @@ -35,7 +35,7 @@ void ExampleChangeStructure::MainRoutine(const int time_count) { structure_->GetToSetSurfaces()[0].SetArea_m2(0.5); // Inertia Tensor - math::Matrix<3, 3> inertia_tensor_b_kgm2(0.0); + s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2(0.0); inertia_tensor_b_kgm2[0][0] = 0.2; inertia_tensor_b_kgm2[1][1] = 0.2; inertia_tensor_b_kgm2[2][2] = 0.2; diff --git a/src/components/ideal/angular_velocity_observer.hpp b/src/components/ideal/angular_velocity_observer.hpp index 3b162a36a..bf22777ac 100644 --- a/src/components/ideal/angular_velocity_observer.hpp +++ b/src/components/ideal/angular_velocity_observer.hpp @@ -57,10 +57,10 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public ILogg * @fn GetAngularVelocity_b_rad_s * @brief Return observed angular velocity */ - inline math::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; } + inline s2e::math::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; } protected: - math::Vector<3> angular_velocity_b_rad_s_{0.0}; //!< Observed angular velocity [rad/s] + s2e::math::Vector<3> angular_velocity_b_rad_s_{0.0}; //!< Observed angular velocity [rad/s] const Attitude& attitude_; //!< Dynamics information }; diff --git a/src/components/ideal/attitude_observer.cpp b/src/components/ideal/attitude_observer.cpp index 312a1930c..26ffee05b 100644 --- a/src/components/ideal/attitude_observer.cpp +++ b/src/components/ideal/attitude_observer.cpp @@ -18,14 +18,14 @@ void AttitudeObserver::MainRoutine(const int time_count) { UNUSED(time_count); // Error calculation - math::Vector<3> random_direction; + s2e::math::Vector<3> random_direction; random_direction[0] = direction_noise_; random_direction[1] = direction_noise_; random_direction[2] = direction_noise_; random_direction = random_direction.CalcNormalizedVector(); double error_angle_rad = angle_noise_; - math::Quaternion error_quaternion(random_direction, error_angle_rad); + s2e::math::Quaternion error_quaternion(random_direction, error_angle_rad); observed_quaternion_i2b_ = error_quaternion * attitude_.GetQuaternion_i2b(); } @@ -57,7 +57,7 @@ AttitudeObserver InitializeAttitudeObserver(ClockGenerator* clock_generator, con // AttitudeObserver double error_angle_standard_deviation_deg = ini_file.ReadDouble("ATTITUDE_OBSERVER", "error_angle_standard_deviation_deg"); - double error_angle_standard_deviation_rad = math::deg_to_rad * error_angle_standard_deviation_deg; + double error_angle_standard_deviation_rad = s2e::math::deg_to_rad * error_angle_standard_deviation_deg; AttitudeObserver attitude_observer(prescaler, clock_generator, error_angle_standard_deviation_rad, attitude); return attitude_observer; diff --git a/src/components/ideal/attitude_observer.hpp b/src/components/ideal/attitude_observer.hpp index 3244dce32..1efc18b71 100644 --- a/src/components/ideal/attitude_observer.hpp +++ b/src/components/ideal/attitude_observer.hpp @@ -57,10 +57,10 @@ class AttitudeObserver : public Component, public ILoggable { * @fn GetQuaternion_i2c * @brief Return observed quaternion from the inertial frame to the body-fixed frame */ - inline const math::Quaternion GetQuaternion_i2b() const { return observed_quaternion_i2b_; }; + inline const s2e::math::Quaternion GetQuaternion_i2b() const { return observed_quaternion_i2b_; }; protected: - math::Quaternion observed_quaternion_i2b_ = {0.0, 0.0, 0.0, 1.0}; //!< Observed quaternion + s2e::math::Quaternion observed_quaternion_i2b_ = {0.0, 0.0, 0.0, 1.0}; //!< Observed quaternion randomization::NormalRand angle_noise_; //!< Normal random for magnitude noise randomization::NormalRand direction_noise_; //!< Normal random for direction noise diff --git a/src/components/ideal/force_generator.cpp b/src/components/ideal/force_generator.cpp index 833703477..79a062ede 100644 --- a/src/components/ideal/force_generator.cpp +++ b/src/components/ideal/force_generator.cpp @@ -29,16 +29,16 @@ void ForceGenerator::MainRoutine(const int time_count) { double norm_ordered_force = ordered_force_b_N_.CalcNorm(); if (norm_ordered_force > 0.0 + DBL_EPSILON) { // Add noise only when the force is generated - math::Vector<3> true_direction = generated_force_b_N_.CalcNormalizedVector(); - math::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_); - math::Vector<3> converted_direction = error_quaternion.FrameConversion(true_direction); + s2e::math::Vector<3> true_direction = generated_force_b_N_.CalcNormalizedVector(); + s2e::math::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_); + s2e::math::Vector<3> converted_direction = error_quaternion.FrameConversion(true_direction); double force_norm_with_error = norm_ordered_force + magnitude_noise_; generated_force_b_N_ = force_norm_with_error * converted_direction; } // Convert frame - math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); - math::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh(); + s2e::math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + s2e::math::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh(); generated_force_i_N_ = q_i2b.InverseFrameConversion(generated_force_b_N_); generated_force_rtn_N_ = q_i2rtn.FrameConversion(generated_force_i_N_); } @@ -49,16 +49,16 @@ void ForceGenerator::PowerOffRoutine() { generated_force_rtn_N_ *= 0.0; } -void ForceGenerator::SetForce_i_N(const math::Vector<3> force_i_N) { - math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); +void ForceGenerator::SetForce_i_N(const s2e::math::Vector<3> force_i_N) { + s2e::math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); ordered_force_b_N_ = q_i2b.FrameConversion(force_i_N); } -void ForceGenerator::SetForce_rtn_N(const math::Vector<3> force_rtn_N) { - math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); - math::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh(); +void ForceGenerator::SetForce_rtn_N(const s2e::math::Vector<3> force_rtn_N) { + s2e::math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + s2e::math::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh(); - math::Vector<3> force_i_N = q_i2rtn.InverseFrameConversion(force_rtn_N); + s2e::math::Vector<3> force_i_N = q_i2rtn.InverseFrameConversion(force_rtn_N); ordered_force_b_N_ = q_i2b.FrameConversion(force_i_N); } @@ -85,14 +85,14 @@ std::string ForceGenerator::GetLogValue() const { return str_tmp; } -math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(math::Vector<3> true_direction, const double error_standard_deviation_rad) { - math::Vector<3> random_direction; +s2e::math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(s2e::math::Vector<3> true_direction, const double error_standard_deviation_rad) { + s2e::math::Vector<3> random_direction; random_direction[0] = direction_noise_; random_direction[1] = direction_noise_; random_direction[2] = direction_noise_; random_direction = random_direction.CalcNormalizedVector(); - math::Vector<3> rotation_axis; + s2e::math::Vector<3> rotation_axis; rotation_axis = OuterProduct(true_direction, random_direction); double norm_rotation_axis = rotation_axis.CalcNorm(); if (norm_rotation_axis < 0.0 + DBL_EPSILON) { @@ -101,7 +101,7 @@ math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(math::Vector<3 } double error_angle_rad = direction_noise_ * error_standard_deviation_rad; - math::Quaternion error_quaternion(rotation_axis, error_angle_rad); + s2e::math::Quaternion error_quaternion(rotation_axis, error_angle_rad); return error_quaternion; } @@ -117,7 +117,7 @@ ForceGenerator InitializeForceGenerator(ClockGenerator* clock_generator, const s char section[30] = "FORCE_GENERATOR"; double force_magnitude_standard_deviation_N = ini_file.ReadDouble(section, "force_magnitude_standard_deviation_N"); double force_direction_standard_deviation_deg = ini_file.ReadDouble(section, "force_direction_standard_deviation_deg"); - double force_direction_standard_deviation_rad = math::deg_to_rad * force_direction_standard_deviation_deg; + double force_direction_standard_deviation_rad = s2e::math::deg_to_rad * force_direction_standard_deviation_deg; ForceGenerator force_generator(prescaler, clock_generator, force_magnitude_standard_deviation_N, force_direction_standard_deviation_rad, dynamics); return force_generator; diff --git a/src/components/ideal/force_generator.hpp b/src/components/ideal/force_generator.hpp index d7cb07f49..9a07fe269 100644 --- a/src/components/ideal/force_generator.hpp +++ b/src/components/ideal/force_generator.hpp @@ -81,23 +81,23 @@ class ForceGenerator : public Component, public ILoggable { * @fn SetForce_b_N * @brief Set ordered force in the body fixed frame [N] */ - inline void SetForce_b_N(const math::Vector<3> force_b_N) { ordered_force_b_N_ = force_b_N; }; + inline void SetForce_b_N(const s2e::math::Vector<3> force_b_N) { ordered_force_b_N_ = force_b_N; }; /** * @fn SetForce_i_N * @brief Set ordered force in the inertial frame [N] */ - void SetForce_i_N(const math::Vector<3> force_i_N); + void SetForce_i_N(const s2e::math::Vector<3> force_i_N); /** * @fn SetForce_rtn_N * @brief Set ordered force in the RTN frame [N] */ - void SetForce_rtn_N(const math::Vector<3> force_rtn_N); + void SetForce_rtn_N(const s2e::math::Vector<3> force_rtn_N); protected: - math::Vector<3> ordered_force_b_N_{0.0}; //!< Ordered force in the body fixed frame [N] - math::Vector<3> generated_force_b_N_{0.0}; //!< Generated force in the body fixed frame [N] - math::Vector<3> generated_force_i_N_{0.0}; //!< Generated force in the inertial frame [N] - math::Vector<3> generated_force_rtn_N_{0.0}; //!< Generated force in the RTN frame [N] + s2e::math::Vector<3> ordered_force_b_N_{0.0}; //!< Ordered force in the body fixed frame [N] + s2e::math::Vector<3> generated_force_b_N_{0.0}; //!< Generated force in the body fixed frame [N] + s2e::math::Vector<3> generated_force_i_N_{0.0}; //!< Generated force in the inertial frame [N] + s2e::math::Vector<3> generated_force_rtn_N_{0.0}; //!< Generated force in the RTN frame [N] // Noise randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise @@ -110,7 +110,7 @@ class ForceGenerator : public Component, public ILoggable { * @param [in] true_direction: True direction * @param [in] error_standard_deviation_rad: Standard deviation of direction error [rad] */ - math::Quaternion GenerateDirectionNoiseQuaternion(math::Vector<3> true_direction, const double error_standard_deviation_rad); + s2e::math::Quaternion GenerateDirectionNoiseQuaternion(s2e::math::Vector<3> true_direction, const double error_standard_deviation_rad); const Dynamics* dynamics_; //!< Spacecraft dynamics information }; diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index e1b91d0c7..48a2157d2 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -9,7 +9,7 @@ #include OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, - const math::Vector<6> error_standard_deviation, const Orbit& orbit) + const s2e::math::Vector<6> error_standard_deviation, const Orbit& orbit) : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed()); @@ -20,11 +20,11 @@ void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); // Calc noise - math::Vector<3> position_error_i_m{0.0}; - math::Vector<3> position_error_rtn_m{0.0}; - math::Vector<3> velocity_error_i_m_s{0.0}; - math::Vector<3> velocity_error_rtn_m_s{0.0}; - math::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); + s2e::math::Vector<3> position_error_i_m{0.0}; + s2e::math::Vector<3> position_error_rtn_m{0.0}; + s2e::math::Vector<3> velocity_error_i_m_s{0.0}; + s2e::math::Vector<3> velocity_error_rtn_m_s{0.0}; + s2e::math::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); switch (noise_frame_) { case NoiseFrame::kInertial: for (size_t axis = 0; axis < 3; axis++) { @@ -93,7 +93,7 @@ OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std // Noise const NoiseFrame noise_frame = SetNoiseFrame(ini_file.ReadString("ORBIT_OBSERVER", "noise_frame")); - math::Vector<6> noise_standard_deviation; + s2e::math::Vector<6> noise_standard_deviation; ini_file.ReadVector("ORBIT_OBSERVER", "noise_standard_deviation", noise_standard_deviation); OrbitObserver orbit_observer(prescaler, clock_generator, noise_frame, noise_standard_deviation, orbit); diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 09437c079..ab3aa9887 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -37,7 +37,7 @@ class OrbitObserver : public Component, public ILoggable { * @param [in] error_standard_deviation: Position and Velocity standard deviation noise [m, m/s] * @param [in] orbit: Orbit information */ - OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, const math::Vector<6> error_standard_deviation, + OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, const s2e::math::Vector<6> error_standard_deviation, const Orbit& orbit); /** @@ -69,17 +69,17 @@ class OrbitObserver : public Component, public ILoggable { * @fn GetPosition_i_m * @brief Return observed position */ - inline const math::Vector<3> GetPosition_i_m() const { return observed_position_i_m_; }; + inline const s2e::math::Vector<3> GetPosition_i_m() const { return observed_position_i_m_; }; /** * @fn GetVelocity_i_m_s * @brief Return observed velocity */ - inline const math::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; }; + inline const s2e::math::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; }; protected: - math::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] - math::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] + s2e::math::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] + s2e::math::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] NoiseFrame noise_frame_; //!< Noise definition frame randomization::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] diff --git a/src/components/ideal/torque_generator.cpp b/src/components/ideal/torque_generator.cpp index d090f0d34..efcf0392d 100644 --- a/src/components/ideal/torque_generator.cpp +++ b/src/components/ideal/torque_generator.cpp @@ -29,9 +29,9 @@ void TorqueGenerator::MainRoutine(const int time_count) { double norm_ordered_torque = ordered_torque_b_Nm_.CalcNorm(); if (norm_ordered_torque > 0.0 + DBL_EPSILON) { // Add noise only when the torque is generated - math::Vector<3> true_direction = generated_torque_b_Nm_.CalcNormalizedVector(); - math::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_); - math::Vector<3> converted_direction = error_quaternion.FrameConversion(true_direction); + s2e::math::Vector<3> true_direction = generated_torque_b_Nm_.CalcNormalizedVector(); + s2e::math::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_); + s2e::math::Vector<3> converted_direction = error_quaternion.FrameConversion(true_direction); double torque_norm_with_error = norm_ordered_torque + magnitude_noise_; generated_torque_b_Nm_ = torque_norm_with_error * converted_direction; } @@ -58,14 +58,14 @@ std::string TorqueGenerator::GetLogValue() const { return str_tmp; } -math::Quaternion TorqueGenerator::GenerateDirectionNoiseQuaternion(math::Vector<3> true_direction, const double error_standard_deviation_rad) { - math::Vector<3> random_direction; +s2e::math::Quaternion TorqueGenerator::GenerateDirectionNoiseQuaternion(s2e::math::Vector<3> true_direction, const double error_standard_deviation_rad) { + s2e::math::Vector<3> random_direction; random_direction[0] = direction_noise_; random_direction[1] = direction_noise_; random_direction[2] = direction_noise_; random_direction = random_direction.CalcNormalizedVector(); - math::Vector<3> rotation_axis; + s2e::math::Vector<3> rotation_axis; rotation_axis = OuterProduct(true_direction, random_direction); double norm_rotation_axis = rotation_axis.CalcNorm(); if (norm_rotation_axis < 0.0 + DBL_EPSILON) { @@ -74,7 +74,7 @@ math::Quaternion TorqueGenerator::GenerateDirectionNoiseQuaternion(math::Vector< } double error_angle_rad = direction_noise_ * error_standard_deviation_rad; - math::Quaternion error_quaternion(rotation_axis, error_angle_rad); + s2e::math::Quaternion error_quaternion(rotation_axis, error_angle_rad); return error_quaternion; } @@ -90,7 +90,7 @@ TorqueGenerator InitializeTorqueGenerator(ClockGenerator* clock_generator, const char section[30] = "TORQUE_GENERATOR"; double torque_magnitude_standard_deviation_Nm = ini_file.ReadDouble(section, "torque_magnitude_standard_deviation_Nm"); double torque_direction_standard_deviation_deg = ini_file.ReadDouble(section, "torque_direction_standard_deviation_deg"); - double torque_direction_standard_deviation_rad = math::deg_to_rad * torque_direction_standard_deviation_deg; + double torque_direction_standard_deviation_rad = s2e::math::deg_to_rad * torque_direction_standard_deviation_deg; TorqueGenerator torque_generator(prescaler, clock_generator, torque_magnitude_standard_deviation_Nm, torque_direction_standard_deviation_rad, dynamics); diff --git a/src/components/ideal/torque_generator.hpp b/src/components/ideal/torque_generator.hpp index 6ddaf1c5d..c03aa4da1 100644 --- a/src/components/ideal/torque_generator.hpp +++ b/src/components/ideal/torque_generator.hpp @@ -71,11 +71,11 @@ class TorqueGenerator : public Component, public ILoggable { * @fn SetTorque_b_Nm * @brief Set ordered torque in the body fixed frame [Nm] */ - inline void SetTorque_b_Nm(const math::Vector<3> torque_b_Nm) { ordered_torque_b_Nm_ = torque_b_Nm; }; + inline void SetTorque_b_Nm(const s2e::math::Vector<3> torque_b_Nm) { ordered_torque_b_Nm_ = torque_b_Nm; }; protected: - math::Vector<3> ordered_torque_b_Nm_{0.0}; //!< Ordered torque in the body fixed frame [Nm] - math::Vector<3> generated_torque_b_Nm_{0.0}; //!< Generated torque in the body fixed frame [Nm] + s2e::math::Vector<3> ordered_torque_b_Nm_{0.0}; //!< Ordered torque in the body fixed frame [Nm] + s2e::math::Vector<3> generated_torque_b_Nm_{0.0}; //!< Generated torque in the body fixed frame [Nm] // Noise randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise @@ -88,7 +88,7 @@ class TorqueGenerator : public Component, public ILoggable { * @param [in] true_direction: True direction * @param [in] error_standard_deviation_rad: Standard deviation of direction error [rad] */ - math::Quaternion GenerateDirectionNoiseQuaternion(math::Vector<3> true_direction, const double error_standard_deviation_rad); + s2e::math::Quaternion GenerateDirectionNoiseQuaternion(s2e::math::Vector<3> true_direction, const double error_standard_deviation_rad); const Dynamics* dynamics_; //!< Spacecraft dynamics information }; diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index a07a52a0b..87e88abfd 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -12,9 +12,9 @@ #include GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, - const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg, - const math::Vector<3> position_noise_standard_deviation_ecef_m, - const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, + const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, + const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, + const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator), component_id_(component_id), @@ -32,9 +32,9 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, } GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, - const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m, - const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, + const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, + const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, + const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator, power_port), component_id_(component_id), @@ -56,8 +56,8 @@ void GnssReceiver::MainRoutine(const int time_count) { // Antenna checking // TODO: Use ECEF position only - math::Vector<3> position_true_eci = dynamics_->GetOrbit().GetPosition_i_m(); - math::Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + s2e::math::Vector<3> position_true_eci = dynamics_->GetOrbit().GetPosition_i_m(); + s2e::math::Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); CheckAntenna(position_true_eci, quaternion_i2b); if (is_gnss_visible_) { @@ -74,7 +74,7 @@ void GnssReceiver::MainRoutine(const int time_count) { ConvertJulianDayToGpsTime(simulation_time_->GetCurrentTime_jd()); } -void GnssReceiver::CheckAntenna(const math::Vector<3> position_true_eci_m, const math::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntenna(const s2e::math::Vector<3> position_true_eci_m, const s2e::math::Quaternion quaternion_i2b) { if (antenna_model_ == AntennaModel::kSimple) { CheckAntennaSimple(position_true_eci_m, quaternion_i2b); } else if (antenna_model_ == AntennaModel::kCone) { @@ -84,15 +84,15 @@ void GnssReceiver::CheckAntenna(const math::Vector<3> position_true_eci_m, const } } -void GnssReceiver::CheckAntennaSimple(const math::Vector<3> position_true_eci_m, const math::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntennaSimple(const s2e::math::Vector<3> position_true_eci_m, const s2e::math::Quaternion quaternion_i2b) { // Simplest model // GNSS satellites are visible when antenna directs anti-earth direction // Antenna normal vector at inertial frame - math::Vector<3> antenna_direction_c(0.0); + s2e::math::Vector<3> antenna_direction_c(0.0); antenna_direction_c[2] = 1.0; - math::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); - math::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); + s2e::math::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); + s2e::math::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); double inner = InnerProduct(position_true_eci_m, antenna_direction_i); if (inner <= 0.0) { @@ -102,18 +102,18 @@ void GnssReceiver::CheckAntennaSimple(const math::Vector<3> position_true_eci_m, } } -void GnssReceiver::CheckAntennaCone(const math::Vector<3> position_true_eci_m, const math::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntennaCone(const s2e::math::Vector<3> position_true_eci_m, const s2e::math::Quaternion quaternion_i2b) { // Cone model gnss_information_list_.clear(); // Antenna pointing direction vector at inertial frame - math::Vector<3> antenna_pointing_direction_c(0.0); + s2e::math::Vector<3> antenna_pointing_direction_c(0.0); antenna_pointing_direction_c[2] = 1.0; - math::Vector<3> antenna_pointing_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_pointing_direction_c); - math::Vector<3> antenna_pointing_direction_i = quaternion_i2b.InverseFrameConversion(antenna_pointing_direction_b); + s2e::math::Vector<3> antenna_pointing_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_pointing_direction_c); + s2e::math::Vector<3> antenna_pointing_direction_i = quaternion_i2b.InverseFrameConversion(antenna_pointing_direction_b); // Antenna position vector at inertial frame - math::Vector<3> antenna_position_i_m = position_true_eci_m + quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); + s2e::math::Vector<3> antenna_position_i_m = position_true_eci_m + quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); // initialize visible_satellite_number_ = 0; @@ -122,9 +122,9 @@ void GnssReceiver::CheckAntennaCone(const math::Vector<3> position_true_eci_m, c for (size_t i = 0; i < number_of_calculated_gnss_satellites; i++) { // compute direction from sat to gnss in body-fixed frame - math::Vector<3> gnss_satellite_position_i_m = gnss_satellites_->GetPosition_eci_m(i); - math::Vector<3> antenna_to_gnss_satellite_i_m = gnss_satellite_position_i_m - antenna_position_i_m; - math::Vector<3> antenna_to_gnss_satellite_direction_i = antenna_to_gnss_satellite_i_m.CalcNormalizedVector(); + s2e::math::Vector<3> gnss_satellite_position_i_m = gnss_satellites_->GetPosition_eci_m(i); + s2e::math::Vector<3> antenna_to_gnss_satellite_i_m = gnss_satellite_position_i_m - antenna_position_i_m; + s2e::math::Vector<3> antenna_to_gnss_satellite_direction_i = antenna_to_gnss_satellite_i_m.CalcNormalizedVector(); // Check GNSS satellites are visible from the receiver(not care antenna direction) bool is_gnss_satellite_visible_from_receiver = false; @@ -147,7 +147,7 @@ void GnssReceiver::CheckAntennaCone(const math::Vector<3> position_true_eci_m, c // Check GNSS satellites are in the antenna half width angle double inner2 = InnerProduct(antenna_pointing_direction_i, antenna_to_gnss_satellite_direction_i); - if (inner2 > cos(half_width_deg_ * math::deg_to_rad) && is_gnss_satellite_visible_from_receiver) { + if (inner2 > cos(half_width_deg_ * s2e::math::deg_to_rad) && is_gnss_satellite_visible_from_receiver) { // is visible visible_satellite_number_++; SetGnssInfo(antenna_to_gnss_satellite_i_m, quaternion_i2b, i); @@ -161,10 +161,10 @@ void GnssReceiver::CheckAntennaCone(const math::Vector<3> position_true_eci_m, c } } -void GnssReceiver::SetGnssInfo(const math::Vector<3> antenna_to_satellite_i_m, const math::Quaternion quaternion_i2b, +void GnssReceiver::SetGnssInfo(const s2e::math::Vector<3> antenna_to_satellite_i_m, const s2e::math::Quaternion quaternion_i2b, const std::size_t gnss_system_id) { - math::Vector<3> antenna_to_satellite_direction_b = quaternion_i2b.FrameConversion(antenna_to_satellite_i_m); - math::Vector<3> antenna_to_satellite_direction_c = quaternion_b2c_.FrameConversion(antenna_to_satellite_direction_b); + s2e::math::Vector<3> antenna_to_satellite_direction_b = quaternion_i2b.FrameConversion(antenna_to_satellite_i_m); + s2e::math::Vector<3> antenna_to_satellite_direction_c = quaternion_b2c_.FrameConversion(antenna_to_satellite_direction_b); double distance_m = antenna_to_satellite_i_m.CalcNorm(); double longitude_rad = AcTan(antenna_to_satellite_direction_c[1], antenna_to_satellite_direction_c[0]); @@ -175,7 +175,7 @@ void GnssReceiver::SetGnssInfo(const math::Vector<3> antenna_to_satellite_i_m, c gnss_information_list_.push_back(gnss_info_new); } -void GnssReceiver::AddNoise(const math::Vector<3> position_true_ecef_m, const math::Vector<3> velocity_true_ecef_m_s) { +void GnssReceiver::AddNoise(const s2e::math::Vector<3> position_true_ecef_m, const s2e::math::Vector<3> velocity_true_ecef_m_s) { for (size_t i = 0; i < 3; i++) { position_ecef_m_[i] = position_true_ecef_m[i] + position_random_noise_ecef_m_[i]; velocity_ecef_m_s_[i] = velocity_true_ecef_m_s[i] + velocity_random_noise_ecef_m_s_[i]; @@ -252,11 +252,11 @@ AntennaModel SetAntennaModel(const std::string antenna_model) { typedef struct _gnss_receiver_param { int prescaler; AntennaModel antenna_model; - math::Vector<3> antenna_pos_b; - math::Quaternion quaternion_b2c; + s2e::math::Vector<3> antenna_pos_b; + s2e::math::Quaternion quaternion_b2c; double half_width_deg; - math::Vector<3> position_noise_standard_deviation_ecef_m; - math::Vector<3> velocity_noise_standard_deviation_ecef_m_s; + s2e::math::Vector<3> position_noise_standard_deviation_ecef_m; + s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s; } GnssReceiverParam; GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const size_t component_id) { diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index f0fe73b06..006d70ab0 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -59,8 +59,8 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] simulation_time: Simulation time information */ GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, - const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg, - const math::Vector<3> position_noise_standard_deviation_ecef_m, const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, + const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, + const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn GnssReceiver @@ -79,9 +79,9 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] simulation_time: Simulation time information */ GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, - const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m, - const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, + const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, + const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, + const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); // Override functions for Component @@ -102,7 +102,7 @@ class GnssReceiver : public Component, public ILoggable { * @fn GetMeasuredPosition_ecef_m * @brief Return Observed position in the ECEF frame [m] */ - inline const math::Vector<3> GetMeasuredPosition_ecef_m(void) const { return position_ecef_m_; } + inline const s2e::math::Vector<3> GetMeasuredPosition_ecef_m(void) const { return position_ecef_m_; } /** * @fn GetMeasuredGeodeticPosition * @brief Return Observed position in the LLH frame [m] @@ -112,7 +112,7 @@ class GnssReceiver : public Component, public ILoggable { * @fn GetMeasuredVelocity_ecef_m_s * @brief Return Observed velocity in the ECEF frame [m/s] */ - inline const math::Vector<3> GetMeasuredVelocity_ecef_m_s(void) const { return velocity_ecef_m_s_; } + inline const s2e::math::Vector<3> GetMeasuredVelocity_ecef_m_s(void) const { return velocity_ecef_m_s_; } // Override ILoggable /** @@ -131,16 +131,16 @@ class GnssReceiver : public Component, public ILoggable { const size_t component_id_; //!< Receiver ID // Antenna - math::Vector<3> antenna_position_b_m_; //!< GNSS antenna position at the body-fixed frame [m] - math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (antenna frame) + s2e::math::Vector<3> antenna_position_b_m_; //!< GNSS antenna position at the body-fixed frame [m] + s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (antenna frame) double half_width_deg_ = 0.0; //!< Half width of the antenna cone model [deg] AntennaModel antenna_model_; //!< Antenna model // Simple position observation randomization::NormalRand position_random_noise_ecef_m_[3]; //!< Random noise for position at the ECEF frame [m] randomization::NormalRand velocity_random_noise_ecef_m_s_[3]; //!< Random noise for velocity at the ECEF frame [m] - math::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] - math::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] + s2e::math::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] + s2e::math::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] geodesy::GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame // Time observation @@ -166,7 +166,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame */ - void CheckAntenna(const math::Vector<3> position_true_i_m, const math::Quaternion quaternion_i2b); + void CheckAntenna(const s2e::math::Vector<3> position_true_i_m, const s2e::math::Quaternion quaternion_i2b); /** * @fn CheckAntennaSimple * @brief Check the antenna can detect GNSS signal with Simple mode @@ -174,7 +174,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame */ - void CheckAntennaSimple(const math::Vector<3> position_true_i_m, const math::Quaternion quaternion_i2b); + void CheckAntennaSimple(const s2e::math::Vector<3> position_true_i_m, const s2e::math::Quaternion quaternion_i2b); /** * @fn CheckAntennaCone * @brief Check the antenna can detect GNSS signal with Cone mode @@ -182,7 +182,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame */ - void CheckAntennaCone(const math::Vector<3> position_true_i_m, const math::Quaternion quaternion_i2b); + void CheckAntennaCone(const s2e::math::Vector<3> position_true_i_m, const s2e::math::Quaternion quaternion_i2b); /** * @fn SetGnssInfo * @brief Calculate and set the GnssInfo values of target GNSS satellite @@ -190,14 +190,14 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame * @param [in] gnss_system_id: ID of target GNSS satellite */ - void SetGnssInfo(const math::Vector<3> antenna_to_satellite_i_m, const math::Quaternion quaternion_i2b, const size_t gnss_system_id); + void SetGnssInfo(const s2e::math::Vector<3> antenna_to_satellite_i_m, const s2e::math::Quaternion quaternion_i2b, const size_t gnss_system_id); /** * @fn AddNoise * @brief Substitutional method for "Measure" in other sensor models inherited Sensor class * @param [in] position_true_ecef_m: True position of the spacecraft in the ECEF frame [m] * @param [in] velocity_true_ecef_m_s: True velocity of the spacecraft in the ECEF frame [m/s] */ - void AddNoise(const math::Vector<3> position_true_ecef_m, const math::Vector<3> velocity_true_ecef_m_s); + void AddNoise(const s2e::math::Vector<3> position_true_ecef_m, const s2e::math::Vector<3> velocity_true_ecef_m_s); /** * @fn ConvertJulianDayToGpsTime * @brief Convert Julian day to GPS time diff --git a/src/components/real/aocs/gyro_sensor.cpp b/src/components/real/aocs/gyro_sensor.cpp index 20e03edc9..244127fb1 100644 --- a/src/components/real/aocs/gyro_sensor.cpp +++ b/src/components/real/aocs/gyro_sensor.cpp @@ -8,11 +8,11 @@ #include GyroSensor::GyroSensor(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const Dynamics* dynamics) + const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics) : Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), dynamics_(dynamics) {} GyroSensor::GyroSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const Dynamics* dynamics) + const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics) : Component(prescaler, clock_generator, power_port), Sensor(sensor_base), sensor_id_(sensor_id), @@ -52,7 +52,7 @@ GyroSensor InitGyroSensor(ClockGenerator* clock_generator, int sensor_id, const const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); const char* GSection = section_name.c_str(); - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; gyro_conf.ReadQuaternion(GSection, "quaternion_b2c", quaternion_b2c); int prescaler = gyro_conf.ReadInt(GSection, "prescaler"); if (prescaler <= 1) prescaler = 1; @@ -73,7 +73,7 @@ GyroSensor InitGyroSensor(ClockGenerator* clock_generator, PowerPort* power_port const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); const char* GSection = section_name.c_str(); - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; gyro_conf.ReadQuaternion(GSection, "quaternion_b2c", quaternion_b2c); int prescaler = gyro_conf.ReadInt(GSection, "prescaler"); if (prescaler <= 1) prescaler = 1; diff --git a/src/components/real/aocs/gyro_sensor.hpp b/src/components/real/aocs/gyro_sensor.hpp index 139bf8597..cd0ec6d34 100644 --- a/src/components/real/aocs/gyro_sensor.hpp +++ b/src/components/real/aocs/gyro_sensor.hpp @@ -32,7 +32,7 @@ class GyroSensor : public Component, public Sensor, public ILogg * @param [in] dynamics: Dynamics information */ GyroSensor(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const Dynamics* dynamics); + const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics); /** * @fn GyroSensor * @brief Constructor with power port @@ -45,7 +45,7 @@ class GyroSensor : public Component, public Sensor, public ILogg * @param [in] dynamics: Dynamics information */ GyroSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const Dynamics* dynamics); + const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics); /** * @fn ~GyroSensor * @brief Destructor @@ -75,13 +75,13 @@ class GyroSensor : public Component, public Sensor, public ILogg * @fn GetMeasuredAngularVelocity_c_rad_s * @brief Return observed angular velocity of the component frame with respect to the inertial frame */ - inline const math::Vector& GetMeasuredAngularVelocity_c_rad_s(void) const { return angular_velocity_c_rad_s_; } + inline const s2e::math::Vector& GetMeasuredAngularVelocity_c_rad_s(void) const { return angular_velocity_c_rad_s_; } protected: - math::Vector angular_velocity_c_rad_s_{ + s2e::math::Vector angular_velocity_c_rad_s_{ 0.0}; //!< Observed angular velocity of the component frame with respect to the inertial frame [rad/s] unsigned int sensor_id_ = 0; //!< Sensor ID - math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame + s2e::math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame const Dynamics* dynamics_; //!< Dynamics information }; diff --git a/src/components/real/aocs/magnetometer.cpp b/src/components/real/aocs/magnetometer.cpp index d28be5bad..91236b2e4 100644 --- a/src/components/real/aocs/magnetometer.cpp +++ b/src/components/real/aocs/magnetometer.cpp @@ -8,14 +8,14 @@ #include Magnetometer::Magnetometer(int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) + const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), geomagnetic_field_(geomagnetic_field) {} Magnetometer::Magnetometer(int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) + const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator, power_port), Sensor(sensor_base), sensor_id_(sensor_id), @@ -57,7 +57,7 @@ Magnetometer InitMagnetometer(ClockGenerator* clock_generator, int sensor_id, co int prescaler = magsensor_conf.ReadInt(MSSection, "prescaler"); if (prescaler <= 1) prescaler = 1; - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; magsensor_conf.ReadQuaternion(MSSection, "quaternion_b2c", quaternion_b2c); // Sensor @@ -78,7 +78,7 @@ Magnetometer InitMagnetometer(ClockGenerator* clock_generator, PowerPort* power_ int prescaler = magsensor_conf.ReadInt(MSSection, "prescaler"); if (prescaler <= 1) prescaler = 1; - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; magsensor_conf.ReadQuaternion(MSSection, "quaternion_b2c", quaternion_b2c); // Sensor diff --git a/src/components/real/aocs/magnetometer.hpp b/src/components/real/aocs/magnetometer.hpp index deaccf1aa..d4d7502e6 100644 --- a/src/components/real/aocs/magnetometer.hpp +++ b/src/components/real/aocs/magnetometer.hpp @@ -32,7 +32,7 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] geomagnetic_field: Geomagnetic environment */ Magnetometer(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); + const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); /** * @fn Magnetometer * @brief Constructor with power port @@ -45,7 +45,7 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] geomagnetic_field: Geomagnetic environment */ Magnetometer(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); + const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); /** * @fn ~Magnetometer * @brief Destructor @@ -75,32 +75,32 @@ class Magnetometer : public Component, public Sensor, pu * @fn GetMeasuredMagneticField_c_nT * @brief Return observed magnetic field on the component frame */ - inline const math::Vector& GetMeasuredMagneticField_c_nT(void) const { return magnetic_field_c_nT_; } + inline const s2e::math::Vector& GetMeasuredMagneticField_c_nT(void) const { return magnetic_field_c_nT_; } /** * @fn SetConstantBiasNoise_c_nT * @brief Set constant bias noise at component frame [nT] * @param [in] constant_noise_c_nT: Constant bias noise at component frame [nT] */ - inline void SetConstantBiasNoise_c_nT(math::Vector constant_noise_c_nT) { bias_noise_c_ = constant_noise_c_nT; } + inline void SetConstantBiasNoise_c_nT(s2e::math::Vector constant_noise_c_nT) { bias_noise_c_ = constant_noise_c_nT; } /** * @fn AddConstantBiasNoise_c_nT * @brief Add constant bias noise at component frame [nT] * @param [in] constant_noise_c_nT: Additional constant bias noise at component frame [nT] */ - inline void AddConstantBiasNoise_c_nT(math::Vector constant_noise_c_nT) { bias_noise_c_ += constant_noise_c_nT; } + inline void AddConstantBiasNoise_c_nT(s2e::math::Vector constant_noise_c_nT) { bias_noise_c_ += constant_noise_c_nT; } /** * @fn GetConstantBiasNoise_c_nT * @brief Get constant bias noise at component frame [nT] */ - inline math::Vector GetConstantBiasNoise_c_nT() const { return bias_noise_c_; } + inline s2e::math::Vector GetConstantBiasNoise_c_nT() const { return bias_noise_c_; } protected: - math::Vector magnetic_field_c_nT_{0.0}; //!< Observed magnetic field on the component frame [nT] + s2e::math::Vector magnetic_field_c_nT_{0.0}; //!< Observed magnetic field on the component frame [nT] unsigned int sensor_id_ = 0; //!< Sensor ID - math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame + s2e::math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame const GeomagneticField* geomagnetic_field_; //!< Geomagnetic environment }; diff --git a/src/components/real/aocs/magnetorquer.cpp b/src/components/real/aocs/magnetorquer.cpp index 0f1a2bb22..909996b84 100644 --- a/src/components/real/aocs/magnetorquer.cpp +++ b/src/components/real/aocs/magnetorquer.cpp @@ -11,13 +11,13 @@ #include #include -Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, - const math::Matrix& scale_factor, - const math::Vector& max_magnetic_moment_c_Am2, const math::Vector& min_magnetic_moment_c_Am2, - const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, - const math::Vector& random_walk_standard_deviation_c_Am2, - const math::Vector& random_walk_limit_c_Am2, - const math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field) +Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, + const s2e::math::Matrix& scale_factor, + const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, + const s2e::math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, + const s2e::math::Vector& random_walk_standard_deviation_c_Am2, + const s2e::math::Vector& random_walk_limit_c_Am2, + const s2e::math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -34,12 +34,12 @@ Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, } Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const math::Quaternion& quaternion_b2c, const math::Matrix& scale_factor, - const math::Vector& max_magnetic_moment_c_Am2, const math::Vector& min_magnetic_moment_c_Am2, - const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, - const math::Vector& random_walk_standard_deviation_c_Am2, - const math::Vector& random_walk_limit_c_Am2, - const math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field) + const s2e::math::Quaternion& quaternion_b2c, const s2e::math::Matrix& scale_factor, + const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, + const s2e::math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, + const s2e::math::Vector& random_walk_standard_deviation_c_Am2, + const s2e::math::Vector& random_walk_limit_c_Am2, + const s2e::math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator, power_port), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -67,7 +67,7 @@ void Magnetorquer::PowerOffRoutine() { output_magnetic_moment_b_Am2_ *= 0.0; } -math::Vector Magnetorquer::CalcOutputTorque(void) { +s2e::math::Vector Magnetorquer::CalcOutputTorque(void) { for (size_t i = 0; i < kMtqDimension; ++i) { // Limit Check if (output_magnetic_moment_c_Am2_[i] > max_magnetic_moment_c_Am2_[i]) { @@ -121,33 +121,33 @@ Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, int actuator_id, int prescaler = magtorquer_conf.ReadInt(MTSection, "prescaler"); if (prescaler <= 1) prescaler = 1; - math::Vector sf_vec; + s2e::math::Vector sf_vec; magtorquer_conf.ReadVector(MTSection, "scale_factor_c", sf_vec); - math::Matrix scale_factor; + s2e::math::Matrix scale_factor; for (size_t i = 0; i < kMtqDimension; i++) { for (size_t j = 0; j < kMtqDimension; j++) { scale_factor[i][j] = sf_vec[i * kMtqDimension + j]; } } - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; magtorquer_conf.ReadQuaternion(MTSection, "quaternion_b2c", quaternion_b2c); - math::Vector max_magnetic_moment_c_Am2; + s2e::math::Vector max_magnetic_moment_c_Am2; magtorquer_conf.ReadVector(MTSection, "max_output_magnetic_moment_c_Am2", max_magnetic_moment_c_Am2); - math::Vector min_magnetic_moment_c_Am2; + s2e::math::Vector min_magnetic_moment_c_Am2; magtorquer_conf.ReadVector(MTSection, "min_output_magnetic_moment_c_Am2", min_magnetic_moment_c_Am2); - math::Vector bias_noise_c_Am2; + s2e::math::Vector bias_noise_c_Am2; magtorquer_conf.ReadVector(MTSection, "constant_bias_noise_c_Am2", bias_noise_c_Am2); double random_walk_step_width_s = component_step_time_s * (double)prescaler; - math::Vector random_walk_standard_deviation_c_Am2; + s2e::math::Vector random_walk_standard_deviation_c_Am2; magtorquer_conf.ReadVector(MTSection, "random_walk_standard_deviation_c_Am2", random_walk_standard_deviation_c_Am2); - math::Vector random_walk_limit_c_Am2; + s2e::math::Vector random_walk_limit_c_Am2; magtorquer_conf.ReadVector(MTSection, "random_walk_limit_c_Am2", random_walk_limit_c_Am2); - math::Vector normal_random_standard_deviation_c_Am2; + s2e::math::Vector normal_random_standard_deviation_c_Am2; magtorquer_conf.ReadVector(MTSection, "white_noise_standard_deviation_c_Am2", normal_random_standard_deviation_c_Am2); Magnetorquer magtorquer(prescaler, clock_generator, actuator_id, quaternion_b2c, scale_factor, max_magnetic_moment_c_Am2, min_magnetic_moment_c_Am2, @@ -166,33 +166,33 @@ Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, PowerPort* power_ int prescaler = magtorquer_conf.ReadInt(MTSection, "prescaler"); if (prescaler <= 1) prescaler = 1; - math::Vector sf_vec; + s2e::math::Vector sf_vec; magtorquer_conf.ReadVector(MTSection, "scale_factor_c", sf_vec); - math::Matrix scale_factor; + s2e::math::Matrix scale_factor; for (size_t i = 0; i < kMtqDimension; i++) { for (size_t j = 0; j < kMtqDimension; j++) { scale_factor[i][j] = sf_vec[i * kMtqDimension + j]; } } - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; magtorquer_conf.ReadQuaternion(MTSection, "quaternion_b2c", quaternion_b2c); - math::Vector max_magnetic_moment_c_Am2; + s2e::math::Vector max_magnetic_moment_c_Am2; magtorquer_conf.ReadVector(MTSection, "max_output_magnetic_moment_c_Am2", max_magnetic_moment_c_Am2); - math::Vector min_magnetic_moment_c_Am2; + s2e::math::Vector min_magnetic_moment_c_Am2; magtorquer_conf.ReadVector(MTSection, "min_output_magnetic_moment_c_Am2", min_magnetic_moment_c_Am2); - math::Vector bias_noise_c_Am2; + s2e::math::Vector bias_noise_c_Am2; magtorquer_conf.ReadVector(MTSection, "constant_bias_noise_c_Am2", bias_noise_c_Am2); double random_walk_step_width_s = component_step_time_s * (double)prescaler; - math::Vector random_walk_standard_deviation_c_Am2; + s2e::math::Vector random_walk_standard_deviation_c_Am2; magtorquer_conf.ReadVector(MTSection, "random_walk_standard_deviation_c_Am2", random_walk_standard_deviation_c_Am2); - math::Vector random_walk_limit_c_Am2; + s2e::math::Vector random_walk_limit_c_Am2; magtorquer_conf.ReadVector(MTSection, "random_walk_limit_c_Am2", random_walk_limit_c_Am2); - math::Vector normal_random_standard_deviation_c_Am2; + s2e::math::Vector normal_random_standard_deviation_c_Am2; magtorquer_conf.ReadVector(MTSection, "white_noise_standard_deviation_c_Am2", normal_random_standard_deviation_c_Am2); // PowerPort diff --git a/src/components/real/aocs/magnetorquer.hpp b/src/components/real/aocs/magnetorquer.hpp index 91b91864b..112125eb9 100644 --- a/src/components/real/aocs/magnetorquer.hpp +++ b/src/components/real/aocs/magnetorquer.hpp @@ -41,11 +41,11 @@ class Magnetorquer : public Component, public ILoggable { * @param [in] normal_random_standard_deviation_c_Am2: Standard deviation for the normal random noise in the component frame [Am2] * @param [in] geomagnetic_field: Geomagnetic environment */ - Magnetorquer(const int prescaler, ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, - const math::Matrix& scale_factor, const math::Vector& max_magnetic_moment_c_Am2, - const math::Vector& min_magnetic_moment_c_Am2, const math::Vector& bias_noise_c_Am2_, - double random_walk_step_width_s, const math::Vector& random_walk_standard_deviation_c_Am2, - const math::Vector& random_walk_limit_c_Am2, const math::Vector& normal_random_standard_deviation_c_Am2, + Magnetorquer(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, + const s2e::math::Matrix& scale_factor, const s2e::math::Vector& max_magnetic_moment_c_Am2, + const s2e::math::Vector& min_magnetic_moment_c_Am2, const s2e::math::Vector& bias_noise_c_Am2_, + double random_walk_step_width_s, const s2e::math::Vector& random_walk_standard_deviation_c_Am2, + const s2e::math::Vector& random_walk_limit_c_Am2, const s2e::math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field); /** * @fn Magnetorquer @@ -66,11 +66,11 @@ class Magnetorquer : public Component, public ILoggable { * @param [in] geomagnetic_field: Geomagnetic environment */ Magnetorquer(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const math::Quaternion& quaternion_b2c, const math::Matrix& scale_factor, - const math::Vector& max_magnetic_moment_c_Am2, const math::Vector& min_magnetic_moment_c_Am2, - const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, - const math::Vector& random_walk_standard_deviation_c_Am2, const math::Vector& random_walk_limit_c_Am2, - const math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field); + const s2e::math::Quaternion& quaternion_b2c, const s2e::math::Matrix& scale_factor, + const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, + const s2e::math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, + const s2e::math::Vector& random_walk_standard_deviation_c_Am2, const s2e::math::Vector& random_walk_limit_c_Am2, + const s2e::math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field); // Override functions for Component /** @@ -100,39 +100,39 @@ class Magnetorquer : public Component, public ILoggable { * @fn GetOutputTorque_b_Nm * @brief Return output torque in the body fixed frame [Nm] */ - inline const math::Vector& GetOutputTorque_b_Nm(void) const { return torque_b_Nm_; }; + inline const s2e::math::Vector& GetOutputTorque_b_Nm(void) const { return torque_b_Nm_; }; /** * @fn SetOutputMagneticMoment_c_Am2 * @brief Set output magnetic moment in the component frame [Am2] */ - inline void SetOutputMagneticMoment_c_Am2(const math::Vector mag_moment_c) { output_magnetic_moment_c_Am2_ = mag_moment_c; }; + inline void SetOutputMagneticMoment_c_Am2(const s2e::math::Vector mag_moment_c) { output_magnetic_moment_c_Am2_ = mag_moment_c; }; /** * @fn SetOutputMagneticMoment_b_Am2 * @brief Return output magnetic moment in the body fixed frame [Am2] */ - inline const math::Vector& SetOutputMagneticMoment_b_Am2(void) const { return output_magnetic_moment_b_Am2_; }; + inline const s2e::math::Vector& SetOutputMagneticMoment_b_Am2(void) const { return output_magnetic_moment_b_Am2_; }; /** * @fn GetOutputMagneticMoment_b_Am2 * @brief Return output magnetic moment in the body fixed frame [Am2] */ - inline const math::Vector& GetOutputMagneticMoment_c_Am2(void) const { return output_magnetic_moment_c_Am2_; }; + inline const s2e::math::Vector& GetOutputMagneticMoment_c_Am2(void) const { return output_magnetic_moment_c_Am2_; }; protected: const int component_id_ = 0; //!< Actuator ID const double kConvertNanoT2T = 1.0e-9; //!< Constant to convert nT to T - math::Vector torque_b_Nm_{0.0}; //!< Output torque in the body fixed frame [Nm] - math::Vector output_magnetic_moment_c_Am2_{0.0}; //!< Output output magnetic moment in the component frame [Am2] - math::Vector output_magnetic_moment_b_Am2_{0.0}; //!< Output output magnetic moment in the body fixed frame [Am2] - math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame - math::Quaternion quaternion_c2b_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from component frame to body frame - math::Matrix scale_factor_; //!< Scale factor matrix - math::Vector max_magnetic_moment_c_Am2_{100.0}; //!< Maximum magnetic moment in the component frame [Am2] - math::Vector min_magnetic_moment_c_Am2_{-100.0}; //!< Minimum magnetic moment in the component frame [Am2] - - math::Vector bias_noise_c_Am2_{0.0}; //!< Constant bias noise in the component frame [Am2] + s2e::math::Vector torque_b_Nm_{0.0}; //!< Output torque in the body fixed frame [Nm] + s2e::math::Vector output_magnetic_moment_c_Am2_{0.0}; //!< Output output magnetic moment in the component frame [Am2] + s2e::math::Vector output_magnetic_moment_b_Am2_{0.0}; //!< Output output magnetic moment in the body fixed frame [Am2] + s2e::math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame + s2e::math::Quaternion quaternion_c2b_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from component frame to body frame + s2e::math::Matrix scale_factor_; //!< Scale factor matrix + s2e::math::Vector max_magnetic_moment_c_Am2_{100.0}; //!< Maximum magnetic moment in the component frame [Am2] + s2e::math::Vector min_magnetic_moment_c_Am2_{-100.0}; //!< Minimum magnetic moment in the component frame [Am2] + + s2e::math::Vector bias_noise_c_Am2_{0.0}; //!< Constant bias noise in the component frame [Am2] RandomWalk random_walk_c_Am2_; //!< Random walk noise randomization::NormalRand random_noise_c_Am2_[kMtqDimension]; //!< Normal random noise @@ -143,7 +143,7 @@ class Magnetorquer : public Component, public ILoggable { * @brief Calculate output torque * @return Output torque in the body fixed frame [Nm] */ - math::Vector CalcOutputTorque(void); + s2e::math::Vector CalcOutputTorque(void); }; /** diff --git a/src/components/real/aocs/mtq_magnetometer_interference.cpp b/src/components/real/aocs/mtq_magnetometer_interference.cpp index 9aa2bb1e2..7b603abbc 100644 --- a/src/components/real/aocs/mtq_magnetometer_interference.cpp +++ b/src/components/real/aocs/mtq_magnetometer_interference.cpp @@ -18,8 +18,8 @@ MtqMagnetometerInterference::MtqMagnetometerInterference(const std::string file_ for (size_t degree = 1; degree <= polynomial_degree_; degree++) { const std::string key_name = "additional_bias_by_mtq_coefficients_" + std::to_string(static_cast(degree)); - math::Vector<9> additional_bias_by_mtq_coefficients_vec; - math::Matrix<3, 3> additional_bias_by_mtq_coefficients; + s2e::math::Vector<9> additional_bias_by_mtq_coefficients_vec; + s2e::math::Matrix<3, 3> additional_bias_by_mtq_coefficients; ini_file.ReadVector(section.c_str(), key_name.c_str(), additional_bias_by_mtq_coefficients_vec); for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { @@ -35,10 +35,10 @@ void MtqMagnetometerInterference::UpdateInterference(void) { magnetometer_.AddConstantBiasNoise_c_nT(-1.0 * previous_added_bias_c_nT_); // Calculate bias - math::Vector<3> additional_bias_c_nT{0.0}; - math::Vector<3> mtq_output_c_Am2 = magnetorquer_.GetOutputMagneticMoment_c_Am2(); + s2e::math::Vector<3> additional_bias_c_nT{0.0}; + s2e::math::Vector<3> mtq_output_c_Am2 = magnetorquer_.GetOutputMagneticMoment_c_Am2(); for (size_t degree = 1; degree <= polynomial_degree_; degree++) { - math::Vector<3> hadamard_mtq; + s2e::math::Vector<3> hadamard_mtq; for (size_t axis = 0; axis < 3; axis++) { hadamard_mtq[axis] = pow(mtq_output_c_Am2[axis], degree); } diff --git a/src/components/real/aocs/mtq_magnetometer_interference.hpp b/src/components/real/aocs/mtq_magnetometer_interference.hpp index fea4824eb..2c2c4c0d8 100644 --- a/src/components/real/aocs/mtq_magnetometer_interference.hpp +++ b/src/components/real/aocs/mtq_magnetometer_interference.hpp @@ -31,8 +31,8 @@ class MtqMagnetometerInterference { protected: size_t polynomial_degree_; //!< Polynomial degree - std::vector> additional_bias_by_mtq_coefficients_; //!< Polynomial coefficients of additional bias noise - math::Vector<3> previous_added_bias_c_nT_{0.0}; + std::vector> additional_bias_by_mtq_coefficients_; //!< Polynomial coefficients of additional bias noise + s2e::math::Vector<3> previous_added_bias_c_nT_{0.0}; Magnetometer& magnetometer_; //!< Magnetometer const Magnetorquer& magnetorquer_; //!< Magnetorquer diff --git a/src/components/real/aocs/reaction_wheel.cpp b/src/components/real/aocs/reaction_wheel.cpp index 54ab6149e..e0e2a1c13 100644 --- a/src/components/real/aocs/reaction_wheel.cpp +++ b/src/components/real/aocs/reaction_wheel.cpp @@ -13,7 +13,7 @@ ReactionWheel::ReactionWheel(const int prescaler, ClockGenerator* clock_generator, const int component_id, const double step_width_s, const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, - const math::Quaternion quaternion_b2c, const math::Vector<3> position_b_m, const double dead_time_s, + const s2e::math::Quaternion quaternion_b2c, const s2e::math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, bool drive_flag, const double init_velocity_rad_s) @@ -31,7 +31,7 @@ ReactionWheel::ReactionWheel(const int prescaler, ClockGenerator* clock_generato stop_limit_angular_velocity_rad_s_(stop_limit_angular_velocity_rad_s), drive_flag_(drive_flag), velocity_limit_rpm_(max_velocity_rpm_), - ode_angular_velocity_(step_width_s_, velocity_limit_rpm_ * math::rpm_to_rad_s, init_velocity_rad_s), + ode_angular_velocity_(step_width_s_, velocity_limit_rpm_ * s2e::math::rpm_to_rad_s, init_velocity_rad_s), rw_jitter_(rw_jitter), is_calculated_jitter_(is_calc_jitter_enabled), is_logged_jitter_(is_log_jitter_enabled) { @@ -40,7 +40,7 @@ ReactionWheel::ReactionWheel(const int prescaler, ClockGenerator* clock_generato ReactionWheel::ReactionWheel(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const double step_width_s, const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, - const math::Quaternion quaternion_b2c, const math::Vector<3> position_b_m, const double dead_time_s, + const s2e::math::Quaternion quaternion_b2c, const s2e::math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, const bool drive_flag, const double init_velocity_rad_s) @@ -58,7 +58,7 @@ ReactionWheel::ReactionWheel(const int prescaler, ClockGenerator* clock_generato stop_limit_angular_velocity_rad_s_(stop_limit_angular_velocity_rad_s), drive_flag_(drive_flag), velocity_limit_rpm_(max_velocity_rpm_), - ode_angular_velocity_(step_width_s, velocity_limit_rpm_ * math::rpm_to_rad_s, init_velocity_rad_s), + ode_angular_velocity_(step_width_s, velocity_limit_rpm_ * s2e::math::rpm_to_rad_s, init_velocity_rad_s), rw_jitter_(rw_jitter), is_calculated_jitter_(is_calc_jitter_enabled), is_logged_jitter_(is_log_jitter_enabled) { @@ -66,7 +66,7 @@ ReactionWheel::ReactionWheel(const int prescaler, ClockGenerator* clock_generato } void ReactionWheel::Initialize() { - rotation_axis_c_ = math::Vector<3>(0.0); + rotation_axis_c_ = s2e::math::Vector<3>(0.0); rotation_axis_c_[2] = 1.0; rotation_axis_b_ = quaternion_b2c_.InverseFrameConversion(rotation_axis_c_); @@ -77,7 +77,7 @@ void ReactionWheel::Initialize() { generated_angular_acceleration_rad_s2_ = 0.0; angular_velocity_rad_s_ = ode_angular_velocity_.GetAngularVelocity_rad_s(); - angular_velocity_rpm_ = angular_velocity_rad_s_ * math::rad_s_to_rpm; + angular_velocity_rpm_ = angular_velocity_rad_s_ * s2e::math::rad_s_to_rpm; // Turn on RW jitter calculation if (is_calculated_jitter_) { @@ -99,7 +99,7 @@ void ReactionWheel::FastUpdate() { } } -math::Vector<3> ReactionWheel::CalcTorque() { +s2e::math::Vector<3> ReactionWheel::CalcTorque() { if (!drive_flag_) // RW idle mode -> coasting mode { // Clear delay buffer @@ -116,7 +116,7 @@ math::Vector<3> ReactionWheel::CalcTorque() { if (abs_angular_velocity_rad_s < stop_limit_angular_velocity_rad_s_) { // Stop rotation rotation_direction = 0.0; - math::Vector<1> zero_rad_s{0.0}; + s2e::math::Vector<1> zero_rad_s{0.0}; ode_angular_velocity_.Setup(0.0, zero_rad_s); } else if (angular_velocity_rad_s_ > 0.0) { rotation_direction = -1.0; @@ -141,7 +141,7 @@ math::Vector<3> ReactionWheel::CalcTorque() { // Substitution double pre_angular_velocity_rad = angular_velocity_rad_s_; angular_velocity_rad_s_ = ode_angular_velocity_.GetAngularVelocity_rad_s(); - angular_velocity_rpm_ = angular_velocity_rad_s_ * math::rad_s_to_rpm; + angular_velocity_rpm_ = angular_velocity_rad_s_ * s2e::math::rad_s_to_rpm; generated_angular_acceleration_rad_s2_ = (angular_velocity_rad_s_ - pre_angular_velocity_rad) / step_width_s_; // Calc output torque by RW @@ -150,20 +150,20 @@ math::Vector<3> ReactionWheel::CalcTorque() { return output_torque_b_Nm_; } -const math::Vector<3> ReactionWheel::GetOutputTorque_b_Nm() const { +const s2e::math::Vector<3> ReactionWheel::GetOutputTorque_b_Nm() const { if (is_calculated_jitter_) { // Add jitter_force_b_N_-derived torque and jitter_torque_b_Nm_ to output_torque_b - return output_torque_b_Nm_ - math::OuterProduct(position_b_m_, rw_jitter_.GetJitterForce_b_N()) - rw_jitter_.GetJitterTorque_b_Nm(); + return output_torque_b_Nm_ - s2e::math::OuterProduct(position_b_m_, rw_jitter_.GetJitterForce_b_N()) - rw_jitter_.GetJitterTorque_b_Nm(); } else { return output_torque_b_Nm_; } } -const math::Vector<3> ReactionWheel::GetJitterForce_b_N() const { +const s2e::math::Vector<3> ReactionWheel::GetJitterForce_b_N() const { if (is_calculated_jitter_) { return rw_jitter_.GetJitterForce_b_N(); } else { - math::Vector<3> zero{0.0}; + s2e::math::Vector<3> zero{0.0}; return zero; } } @@ -188,7 +188,7 @@ void ReactionWheel::SetVelocityLimit_rpm(const double velocity_limit_rpm) { } else { velocity_limit_rpm_ = velocity_limit_rpm; } - ode_angular_velocity_.SetAngularVelocityLimit_rad_s(velocity_limit_rpm_ * math::rpm_to_rad_s); + ode_angular_velocity_.SetAngularVelocityLimit_rad_s(velocity_limit_rpm_ * s2e::math::rpm_to_rad_s); return; } @@ -238,8 +238,8 @@ double rotor_inertia_kgm2; double max_torque_Nm; double max_velocity_rpm; // Mounting -math::Quaternion quaternion_b2c; -math::Vector<3> position_b_m; +s2e::math::Quaternion quaternion_b2c; +s2e::math::Vector<3> position_b_m; // Time delay double dead_time_s; double time_constant_s; @@ -279,11 +279,11 @@ void InitParams(int actuator_id, std::string file_name, double compo_update_step rw_ini_file.ReadQuaternion(rw_section, "quaternion_b2c", quaternion_b2c); } else // direction_determination_mode == "DIRECTION" { - math::Vector<3> direction_b; + s2e::math::Vector<3> direction_b; rw_ini_file.ReadVector(rw_section, "direction_b", direction_b); - math::Vector<3> direction_c(0.0); + s2e::math::Vector<3> direction_c(0.0); direction_c[2] = 1.0; - math::Quaternion q(direction_b, direction_c); + s2e::math::Quaternion q(direction_b, direction_c); quaternion_b2c = q.Conjugate(); } rw_ini_file.ReadVector(rw_section, "position_b_m", position_b_m); diff --git a/src/components/real/aocs/reaction_wheel.hpp b/src/components/real/aocs/reaction_wheel.hpp index e9cde9dd6..5852b8a74 100644 --- a/src/components/real/aocs/reaction_wheel.hpp +++ b/src/components/real/aocs/reaction_wheel.hpp @@ -49,8 +49,8 @@ class ReactionWheel : public Component, public ILoggable { * @param [in] init_velocity_rad_s: Initial value of angular velocity of RW */ ReactionWheel(const int prescaler, ClockGenerator* clock_generator, const int component_id, const double step_width_s, - const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const math::Quaternion quaternion_b2c, - const math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, + const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const s2e::math::Quaternion quaternion_b2c, + const s2e::math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, const bool drive_flag = false, const double init_velocity_rad_s = 0.0); @@ -79,8 +79,8 @@ class ReactionWheel : public Component, public ILoggable { * @param [in] init_velocity_rad_s: Initial value of angular velocity of RW [rad/s] */ ReactionWheel(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const double step_width_s, - const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const math::Quaternion quaternion_b2c, - const math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, + const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const s2e::math::Quaternion quaternion_b2c, + const s2e::math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, const bool drive_flag = false, const double init_velocity_rad_s = 0.0); @@ -119,12 +119,12 @@ class ReactionWheel : public Component, public ILoggable { * @fn GetOutputTorque_b_Nm * @brief Return output torque in the body fixed frame [Nm] */ - const math::Vector<3> GetOutputTorque_b_Nm() const; + const s2e::math::Vector<3> GetOutputTorque_b_Nm() const; /** * @fn GetJitterForce_b_N * @brief Return output force by jitter in the body fixed frame [N] */ - inline const math::Vector<3> GetJitterForce_b_N() const; + inline const s2e::math::Vector<3> GetJitterForce_b_N() const; /** * @fn GetDriveFlag * @brief Return drive flag @@ -144,7 +144,7 @@ class ReactionWheel : public Component, public ILoggable { * @fn GetAngularMomentum_b_Nms * @brief Return angular momentum of RW [Nms] */ - inline const math::Vector<3> GetAngularMomentum_b_Nms() const { return angular_momentum_b_Nms_; }; + inline const s2e::math::Vector<3> GetAngularMomentum_b_Nms() const { return angular_momentum_b_Nms_; }; // Setter /** @@ -174,10 +174,10 @@ class ReactionWheel : public Component, public ILoggable { const double rotor_inertia_kgm2_; //!< Inertia of RW rotor [kgm2] const double max_torque_Nm_; //!< Maximum output torque [Nm] const double max_velocity_rpm_; //!< Maximum angular velocity of rotor [rpm] - const math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame - const math::Vector<3> position_b_m_; //!< Position of RW in the body fixed frame [m] - math::Vector<3> rotation_axis_c_; //!< Wheel rotation axis on the component frame. Constant as (0 0 1). (Output torque is minus direction) - math::Vector<3> rotation_axis_b_; //!< Wheel rotation vector in the body fixed frame. + const s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame + const s2e::math::Vector<3> position_b_m_; //!< Position of RW in the body fixed frame [m] + s2e::math::Vector<3> rotation_axis_c_; //!< Wheel rotation axis on the component frame. Constant as (0 0 1). (Output torque is minus direction) + s2e::math::Vector<3> rotation_axis_b_; //!< Wheel rotation vector in the body fixed frame. // Parameters for control delay const double step_width_s_; //!< step width for ReactionWheelOde [sec] @@ -199,8 +199,8 @@ class ReactionWheel : public Component, public ILoggable { double angular_velocity_rpm_ = 0.0; //!< Current angular velocity [rpm] double angular_velocity_rad_s_ = 0.0; //!< Current angular velocity [rad/s] // Output at body frame - math::Vector<3> output_torque_b_Nm_{0.0}; //!< Output torque in the body fixed frame [Nm] - math::Vector<3> angular_momentum_b_Nms_{0.0}; //!< Angular momentum of RW [Nms] + s2e::math::Vector<3> output_torque_b_Nm_{0.0}; //!< Output torque in the body fixed frame [Nm] + s2e::math::Vector<3> angular_momentum_b_Nms_{0.0}; //!< Angular momentum of RW [Nms] // ODE double velocity_limit_rpm_; //!< Velocity limit defined by users [RPM] @@ -216,7 +216,7 @@ class ReactionWheel : public Component, public ILoggable { * @fn CalcTorque * @brief Calculation of generated torque */ - math::Vector<3> CalcTorque(); + s2e::math::Vector<3> CalcTorque(); /** * @fn Initialize * @brief Initialize function diff --git a/src/components/real/aocs/reaction_wheel_jitter.cpp b/src/components/real/aocs/reaction_wheel_jitter.cpp index 189e364f7..5e4a2930b 100644 --- a/src/components/real/aocs/reaction_wheel_jitter.cpp +++ b/src/components/real/aocs/reaction_wheel_jitter.cpp @@ -11,14 +11,14 @@ ReactionWheelJitter::ReactionWheelJitter(std::vector> radial_force_harmonics_coefficients, std::vector> radial_torque_harmonics_coefficients, const double update_interval_s, - const math::Quaternion quaternion_b2c, const double structural_resonance_frequency_Hz, + const s2e::math::Quaternion quaternion_b2c, const double structural_resonance_frequency_Hz, const double damping_factor, const double bandwidth, const bool considers_structural_resonance) : radial_force_harmonics_coefficients_(radial_force_harmonics_coefficients), radial_torque_harmonics_coefficients_(radial_torque_harmonics_coefficients), update_interval_s_(update_interval_s), quaternion_b2c_(quaternion_b2c), structural_resonance_frequency_Hz_(structural_resonance_frequency_Hz), - structural_resonance_angular_frequency_Hz_(math::tau * structural_resonance_frequency_Hz), + structural_resonance_angular_frequency_Hz_(s2e::math::tau * structural_resonance_frequency_Hz), damping_factor_(damping_factor), bandwidth_(bandwidth), considers_structural_resonance_(considers_structural_resonance) { @@ -26,7 +26,7 @@ ReactionWheelJitter::ReactionWheelJitter(std::vector> radial // Generate random number for initial rotation phase std::random_device seed_gen; std::default_random_engine engine(seed_gen()); - std::uniform_real_distribution dist(0.0, math::tau); + std::uniform_real_distribution dist(0.0, s2e::math::tau); // Initialize RW rotation phase for (size_t i = 0; i < radial_force_harmonics_coefficients_.size(); i++) { jitter_force_rotation_phase_.push_back(dist(engine)); diff --git a/src/components/real/aocs/reaction_wheel_jitter.hpp b/src/components/real/aocs/reaction_wheel_jitter.hpp index dbf2e24c5..a6da8a503 100644 --- a/src/components/real/aocs/reaction_wheel_jitter.hpp +++ b/src/components/real/aocs/reaction_wheel_jitter.hpp @@ -36,7 +36,7 @@ class ReactionWheelJitter { */ ReactionWheelJitter(std::vector> radial_force_harmonics_coefficients, std::vector> radial_torque_harmonics_coefficients, const double update_interval_s, - const math::Quaternion quaternion_b2c, const double structural_resonance_frequency_Hz, const double damping_factor, + const s2e::math::Quaternion quaternion_b2c, const double structural_resonance_frequency_Hz, const double damping_factor, const double bandwidth, const bool considers_structural_resonance); /** * @fn ~ReactionWheelJitter @@ -55,24 +55,24 @@ class ReactionWheelJitter { * @fn GetJitterForce_b_N * @brief Return generated jitter force in the body fixed frame [N] */ - const math::Vector<3> GetJitterForce_b_N() const { return jitter_force_b_N_; } + const s2e::math::Vector<3> GetJitterForce_b_N() const { return jitter_force_b_N_; } /** * @fn GetJitterTorque_b_Nm * @brief Return generated jitter torque in the body fixed frame [Nm] */ - const math::Vector<3> GetJitterTorque_b_Nm() const { return jitter_torque_b_Nm_; } + const s2e::math::Vector<3> GetJitterTorque_b_Nm() const { return jitter_torque_b_Nm_; } /** * @fn GetJitterForce_c_N * @brief Return generated jitter force in the components frame [N] */ - const math::Vector<3> GetJitterForce_c_N() const { + const s2e::math::Vector<3> GetJitterForce_c_N() const { return considers_structural_resonance_ ? filtered_jitter_force_n_c_ : unfiltered_jitter_force_n_c_; } /** * @fn GetJitterTorque_c_Nm * @brief Return generated jitter torque in the component frame [Nm] */ - const math::Vector<3> GetJitterTorque_c_Nm() const { + const s2e::math::Vector<3> GetJitterTorque_c_Nm() const { return considers_structural_resonance_ ? filtered_jitter_torque_n_c_ : unfiltered_jitter_torque_n_c_; } @@ -81,7 +81,7 @@ class ReactionWheelJitter { std::vector> radial_torque_harmonics_coefficients_; //!< Coefficients for radial torque harmonics double update_interval_s_; //!< Jitter update interval [sec] - math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame + s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame double structural_resonance_frequency_Hz_; //!< Frequency of structural resonance [Hz] double structural_resonance_angular_frequency_Hz_; //!< Angular Frequency of structural resonance @@ -94,22 +94,22 @@ class ReactionWheelJitter { std::vector jitter_torque_rotation_phase_; //!< 2 * pi * h_i * Omega * t [rad] // Variables for solving difference equations in component frame - math::Vector<3> unfiltered_jitter_force_n_c_{0.0}; - math::Vector<3> unfiltered_jitter_force_n_1_c_{0.0}; - math::Vector<3> unfiltered_jitter_force_n_2_c_{0.0}; - math::Vector<3> unfiltered_jitter_torque_n_c_{0.0}; - math::Vector<3> unfiltered_jitter_torque_n_1_c_{0.0}; - math::Vector<3> unfiltered_jitter_torque_n_2_c_{0.0}; - math::Vector<3> filtered_jitter_force_n_c_{0.0}; - math::Vector<3> filtered_jitter_force_n_1_c_{0.0}; - math::Vector<3> filtered_jitter_force_n_2_c_{0.0}; - math::Vector<3> filtered_jitter_torque_n_c_{0.0}; - math::Vector<3> filtered_jitter_torque_n_1_c_{0.0}; - math::Vector<3> filtered_jitter_torque_n_2_c_{0.0}; + s2e::math::Vector<3> unfiltered_jitter_force_n_c_{0.0}; + s2e::math::Vector<3> unfiltered_jitter_force_n_1_c_{0.0}; + s2e::math::Vector<3> unfiltered_jitter_force_n_2_c_{0.0}; + s2e::math::Vector<3> unfiltered_jitter_torque_n_c_{0.0}; + s2e::math::Vector<3> unfiltered_jitter_torque_n_1_c_{0.0}; + s2e::math::Vector<3> unfiltered_jitter_torque_n_2_c_{0.0}; + s2e::math::Vector<3> filtered_jitter_force_n_c_{0.0}; + s2e::math::Vector<3> filtered_jitter_force_n_1_c_{0.0}; + s2e::math::Vector<3> filtered_jitter_force_n_2_c_{0.0}; + s2e::math::Vector<3> filtered_jitter_torque_n_c_{0.0}; + s2e::math::Vector<3> filtered_jitter_torque_n_1_c_{0.0}; + s2e::math::Vector<3> filtered_jitter_torque_n_2_c_{0.0}; double coefficients_[6]; //!< Coefficients of difference equation - math::Vector<3> jitter_force_b_N_{0.0}; //!< Generated jitter force in the body frame [N] - math::Vector<3> jitter_torque_b_Nm_{0.0}; //!< Generated jitter torque in the body frame [Nm] + s2e::math::Vector<3> jitter_force_b_N_{0.0}; //!< Generated jitter force in the body frame [N] + s2e::math::Vector<3> jitter_torque_b_Nm_{0.0}; //!< Generated jitter torque in the body frame [Nm] /** * @fn AddStructuralResonance diff --git a/src/components/real/aocs/reaction_wheel_ode.cpp b/src/components/real/aocs/reaction_wheel_ode.cpp index 48b440089..263e24460 100644 --- a/src/components/real/aocs/reaction_wheel_ode.cpp +++ b/src/components/real/aocs/reaction_wheel_ode.cpp @@ -8,10 +8,10 @@ ReactionWheelOde::ReactionWheelOde(const double step_width_s, const double velocity_limit_rad_s, const double initial_angular_velocity_rad_s) : OrdinaryDifferentialEquation<1>(step_width_s), velocity_limit_rad_s_(velocity_limit_rad_s) { - this->Setup(0.0, math::Vector<1>(initial_angular_velocity_rad_s)); + this->Setup(0.0, s2e::math::Vector<1>(initial_angular_velocity_rad_s)); } -void ReactionWheelOde::DerivativeFunction(double x, const math::Vector<1> &state, math::Vector<1> &rhs) { +void ReactionWheelOde::DerivativeFunction(double x, const s2e::math::Vector<1> &state, s2e::math::Vector<1> &rhs) { UNUSED(x); double angular_velocity_rad_s = state[0]; diff --git a/src/components/real/aocs/reaction_wheel_ode.hpp b/src/components/real/aocs/reaction_wheel_ode.hpp index 89bec07c3..5553ed389 100644 --- a/src/components/real/aocs/reaction_wheel_ode.hpp +++ b/src/components/real/aocs/reaction_wheel_ode.hpp @@ -12,7 +12,7 @@ * @file ReactionWheelOde * @brief Ordinary differential equation of angular velocity of reaction wheel with first-order lag */ -class ReactionWheelOde : public math::OrdinaryDifferentialEquation<1> { +class ReactionWheelOde : public s2e::math::OrdinaryDifferentialEquation<1> { public: /** * @fn ReactionWheelOde @@ -53,7 +53,7 @@ class ReactionWheelOde : public math::OrdinaryDifferentialEquation<1> { * @param [in] state: State vector * @param [out] rhs: Differentiated value of state vector */ - void DerivativeFunction(double x, const math::Vector<1>& state, math::Vector<1>& rhs) override; + void DerivativeFunction(double x, const s2e::math::Vector<1>& state, s2e::math::Vector<1>& rhs) override; double velocity_limit_rad_s_; double angular_acceleration_rad_s2_ = 0.0; //!< Angular acceleration [rad/s2] diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index 60a10d7a5..c6c0e5384 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -14,9 +14,9 @@ #include using namespace std; -using namespace math; +using namespace s2e::math; -StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, +StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, @@ -41,7 +41,7 @@ StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, con Initialize(); } StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, + const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const Dynamics* dynamics, @@ -67,21 +67,21 @@ StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, Pow } void StarSensor::Initialize() { - measured_quaternion_i2c_ = math::Quaternion(0.0, 0.0, 0.0, 1.0); + measured_quaternion_i2c_ = s2e::math::Quaternion(0.0, 0.0, 0.0, 1.0); // Decide delay buffer size max_delay_ = int(output_delay_ * 2 / step_time_s_); if (max_delay_ <= 0) max_delay_ = 1; - vector temp(max_delay_); + vector temp(max_delay_); delay_buffer_ = temp; // Initialize delay buffer for (int i = 0; i < max_delay_; ++i) { delay_buffer_[i] = measured_quaternion_i2c_; } - sight_direction_c_ = math::Vector<3>(0.0); - first_orthogonal_direction_c = math::Vector<3>(0.0); - second_orthogonal_direction_c = math::Vector<3>(0.0); + sight_direction_c_ = s2e::math::Vector<3>(0.0); + first_orthogonal_direction_c = s2e::math::Vector<3>(0.0); + second_orthogonal_direction_c = s2e::math::Vector<3>(0.0); sight_direction_c_[0] = 1.0; //(1,0,0)@Component coordinates, viewing direction first_orthogonal_direction_c[1] = 1.0; //(0,1,0)@Component coordinates, line-of-sight orthogonal direction second_orthogonal_direction_c[2] = 1.0; //(0,0,1)@Component coordinates, line-of-sight orthogonal direction @@ -110,10 +110,10 @@ void StarSensor::update(const LocalCelestialInformation* local_celestial_informa // Add noise on sight direction Quaternion q_sight(sight_direction_c_, sight_direction_noise_); // Random noise on orthogonal direction of sight. Range [0:2pi] - double rot = math::tau * double(rotation_noise_); + double rot = s2e::math::tau * double(rotation_noise_); // Calc observation error on orthogonal direction of sight - math::Vector<3> rot_axis = cos(rot) * first_orthogonal_direction_c + sin(rot) * second_orthogonal_direction_c; - math::Quaternion q_ortho(rot_axis, orthogonal_direction_noise_); + s2e::math::Vector<3> rot_axis = cos(rot) * first_orthogonal_direction_c + sin(rot) * second_orthogonal_direction_c; + s2e::math::Quaternion q_ortho(rot_axis, orthogonal_direction_noise_); // Judge errors AllJudgement(local_celestial_information, attitude); @@ -137,9 +137,9 @@ void StarSensor::AllJudgement(const LocalCelestialInformation* local_celestial_i error_flag_ = false; } -int StarSensor::SunJudgement(const math::Vector<3>& sun_b) { - math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); - math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); +int StarSensor::SunJudgement(const s2e::math::Vector<3>& sun_b) { + s2e::math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); + s2e::math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); double sun_angle_rad = CalAngleVector_rad(sun_b, sight_b); if (sun_angle_rad < sun_forbidden_angle_rad_) return 1; @@ -147,9 +147,9 @@ int StarSensor::SunJudgement(const math::Vector<3>& sun_b) { return 0; } -int StarSensor::EarthJudgement(const math::Vector<3>& earth_b) { - math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); - math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); +int StarSensor::EarthJudgement(const s2e::math::Vector<3>& earth_b) { + s2e::math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); + s2e::math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); double earth_size_rad = atan2(environment::earth_equatorial_radius_m, earth_b.CalcNorm()); // angles between sat<->earth_center & sat<->earth_edge double earth_center_angle_rad = CalAngleVector_rad(earth_b, sight_b); // angles between sat<->earth_center & sat_sight @@ -160,9 +160,9 @@ int StarSensor::EarthJudgement(const math::Vector<3>& earth_b) { return 0; } -int StarSensor::MoonJudgement(const math::Vector<3>& moon_b) { - math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); - math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); +int StarSensor::MoonJudgement(const s2e::math::Vector<3>& moon_b) { + s2e::math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); + s2e::math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); double moon_angle_rad = CalAngleVector_rad(moon_b, sight_b); if (moon_angle_rad < moon_forbidden_angle_rad_) return 1; @@ -170,7 +170,7 @@ int StarSensor::MoonJudgement(const math::Vector<3>& moon_b) { return 0; } -int StarSensor::CaptureRateJudgement(const math::Vector<3>& omega_b_rad_s) { +int StarSensor::CaptureRateJudgement(const s2e::math::Vector<3>& omega_b_rad_s) { double omega_norm = omega_b_rad_s.CalcNorm(); if (omega_norm > capture_rate_limit_rad_s_) return 1; @@ -199,8 +199,8 @@ std::string StarSensor::GetLogValue() const { } double StarSensor::CalAngleVector_rad(const Vector<3>& vector1, const Vector<3>& vector2) { - math::Vector<3> vect1_normal = vector1.CalcNormalizedVector(); - math::Vector<3> vect2_normal = vector2.CalcNormalizedVector(); + s2e::math::Vector<3> vect1_normal = vector1.CalcNormalizedVector(); + s2e::math::Vector<3> vect2_normal = vector2.CalcNormalizedVector(); double cosTheta = InnerProduct(vect1_normal, vect2_normal); // Calc cos value double theta_rad = acos(cosTheta); @@ -223,7 +223,7 @@ StarSensor InitStarSensor(ClockGenerator* clock_generator, int sensor_id, const int prescaler = STT_conf.ReadInt(STTSection, "prescaler"); if (prescaler <= 1) prescaler = 1; double step_time_s = component_step_time_s * prescaler; - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; STT_conf.ReadQuaternion(STTSection, "quaternion_b2c", quaternion_b2c); double standard_deviation_orthogonal_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_orthogonal_direction_rad"); double standard_deviation_sight_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_sight_direction_rad"); @@ -232,13 +232,13 @@ StarSensor InitStarSensor(ClockGenerator* clock_generator, int sensor_id, const double output_interval_sec = STT_conf.ReadDouble(STTSection, "output_interval_s"); int output_interval = max(int(output_interval_sec / step_time_s), 1); double sun_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "sun_exclusion_angle_deg"); - double sun_forbidden_angle_rad = sun_forbidden_angle_deg * math::pi / 180.0; + double sun_forbidden_angle_rad = sun_forbidden_angle_deg * s2e::math::pi / 180.0; double earth_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "earth_exclusion_angle_deg"); - double earth_forbidden_angle_rad = earth_forbidden_angle_deg * math::pi / 180.0; + double earth_forbidden_angle_rad = earth_forbidden_angle_deg * s2e::math::pi / 180.0; double moon_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "moon_exclusion_angle_deg"); - double moon_forbidden_angle_rad = moon_forbidden_angle_deg * math::pi / 180.0; + double moon_forbidden_angle_rad = moon_forbidden_angle_deg * s2e::math::pi / 180.0; double capture_rate_deg_s = STT_conf.ReadDouble(STTSection, "angular_rate_limit_deg_s"); - double capture_rate_rad_s = capture_rate_deg_s * math::pi / 180.0; + double capture_rate_rad_s = capture_rate_deg_s * s2e::math::pi / 180.0; StarSensor stt(prescaler, clock_generator, sensor_id, quaternion_b2c, standard_deviation_orthogonal_direction, standard_deviation_sight_direction, step_time_s, output_delay, output_interval, sun_forbidden_angle_rad, earth_forbidden_angle_rad, moon_forbidden_angle_rad, @@ -257,7 +257,7 @@ StarSensor InitStarSensor(ClockGenerator* clock_generator, PowerPort* power_port if (prescaler <= 1) prescaler = 1; double step_time_s = component_step_time_s * prescaler; - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; STT_conf.ReadQuaternion(STTSection, "quaternion_b2c", quaternion_b2c); double standard_deviation_orthogonal_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_orthogonal_direction_rad"); double standard_deviation_sight_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_sight_direction_rad"); @@ -266,13 +266,13 @@ StarSensor InitStarSensor(ClockGenerator* clock_generator, PowerPort* power_port double output_interval_sec = STT_conf.ReadDouble(STTSection, "output_interval_s"); int output_interval = max(int(output_interval_sec / step_time_s), 1); double sun_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "sun_exclusion_angle_deg"); - double sun_forbidden_angle_rad = sun_forbidden_angle_deg * math::pi / 180.0; + double sun_forbidden_angle_rad = sun_forbidden_angle_deg * s2e::math::pi / 180.0; double earth_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "earth_exclusion_angle_deg"); - double earth_forbidden_angle_rad = earth_forbidden_angle_deg * math::pi / 180.0; + double earth_forbidden_angle_rad = earth_forbidden_angle_deg * s2e::math::pi / 180.0; double moon_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "moon_exclusion_angle_deg"); - double moon_forbidden_angle_rad = moon_forbidden_angle_deg * math::pi / 180.0; + double moon_forbidden_angle_rad = moon_forbidden_angle_deg * s2e::math::pi / 180.0; double capture_rate_deg_s = STT_conf.ReadDouble(STTSection, "angular_rate_limit_deg_s"); - double capture_rate_rad_s = capture_rate_deg_s * math::pi / 180.0; + double capture_rate_rad_s = capture_rate_deg_s * s2e::math::pi / 180.0; power_port->InitializeWithInitializeFile(file_name); diff --git a/src/components/real/aocs/star_sensor.hpp b/src/components/real/aocs/star_sensor.hpp index 5965c5e9a..40f7c46a0 100644 --- a/src/components/real/aocs/star_sensor.hpp +++ b/src/components/real/aocs/star_sensor.hpp @@ -43,7 +43,7 @@ class StarSensor : public Component, public ILoggable { * @param [in] dynamics: Dynamics information * @param [in] local_environment: Local environment information */ - StarSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, + StarSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, @@ -69,7 +69,7 @@ class StarSensor : public Component, public ILoggable { * @param [in] local_environment: Local environment information */ StarSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, + const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const Dynamics* dynamics, @@ -98,7 +98,7 @@ class StarSensor : public Component, public ILoggable { * @fn GetMeasuredQuaternion_i2c * @brief Return observed quaternion from the inertial frame to the component frame */ - inline const math::Quaternion GetMeasuredQuaternion_i2c() const { return measured_quaternion_i2c_; }; + inline const s2e::math::Quaternion GetMeasuredQuaternion_i2c() const { return measured_quaternion_i2c_; }; /** * @fn GetErrorFlag * @brief Return error flag @@ -108,11 +108,11 @@ class StarSensor : public Component, public ILoggable { protected: // StarSensor general parameters const int component_id_; //!< Sensor ID - math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame - math::Quaternion measured_quaternion_i2c_ = {0.0, 0.0, 0.0, 1.0}; //!< StarSensor observed quaternion - math::Vector<3> sight_direction_c_; //!< Sight direction vector at component frame - math::Vector<3> first_orthogonal_direction_c; //!< The first orthogonal direction of sight at component frame - math::Vector<3> second_orthogonal_direction_c; //!< The second orthogonal direction of sight at component frame + s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame + s2e::math::Quaternion measured_quaternion_i2c_ = {0.0, 0.0, 0.0, 1.0}; //!< StarSensor observed quaternion + s2e::math::Vector<3> sight_direction_c_; //!< Sight direction vector at component frame + s2e::math::Vector<3> first_orthogonal_direction_c; //!< The first orthogonal direction of sight at component frame + s2e::math::Vector<3> second_orthogonal_direction_c; //!< The second orthogonal direction of sight at component frame // Noise parameters randomization::MinimalStandardLcgWithShuffle rotation_noise_; //!< Randomize object for orthogonal direction @@ -121,7 +121,7 @@ class StarSensor : public Component, public ILoggable { // Delay emulation parameters int max_delay_; //!< Max delay - std::vector delay_buffer_; //!< Buffer of quaternion for delay emulation + std::vector delay_buffer_; //!< Buffer of quaternion for delay emulation int buffer_position_; //!< Buffer position double step_time_s_; //!< Step time for delay calculation [sec] unsigned int output_delay_; //!< Output delay [0, max_delay_] [step_sec] @@ -153,7 +153,7 @@ class StarSensor : public Component, public ILoggable { * @param [in] local_celestial_information: Local celestial information * @param [in] attitude: Attitude information */ - math::Quaternion Measure(const LocalCelestialInformation* local_celestial_information, const Attitude* attitude); + s2e::math::Quaternion Measure(const LocalCelestialInformation* local_celestial_information, const Attitude* attitude); /** * @fn AllJudgement @@ -168,28 +168,28 @@ class StarSensor : public Component, public ILoggable { * @param [in] sun_b: Sun direction vector in the body fixed frame * @return 1: violated, 0: not violated */ - int SunJudgement(const math::Vector<3>& sun_b); + int SunJudgement(const s2e::math::Vector<3>& sun_b); /** * @fn EarthJudgement * @brief Judge violation of earth forbidden angle * @param [in] earth_b: Earth direction vector in the body fixed frame * @return 1: violated, 0: not violated */ - int EarthJudgement(const math::Vector<3>& earth_b); + int EarthJudgement(const s2e::math::Vector<3>& earth_b); /** * @fn MoonJudgement * @brief Judge violation of moon forbidden angle * @param [in] moon_b: Moon direction vector in the body fixed frame * @return 1: violated, 0: not violated */ - int MoonJudgement(const math::Vector<3>& moon_b); + int MoonJudgement(const s2e::math::Vector<3>& moon_b); /** * @fn CaptureRateJudgement * @brief Judge violation of angular velocity limit * @param [in] omega_b_rad_s: Angular velocity of spacecraft in the body fixed frame * @return 1: violated, 0: not violated */ - int CaptureRateJudgement(const math::Vector<3>& omega_b_rad_s); + int CaptureRateJudgement(const s2e::math::Vector<3>& omega_b_rad_s); /** * @fn CalAngleVector_rad * @brief Calculate angle between two vectors @@ -197,7 +197,7 @@ class StarSensor : public Component, public ILoggable { * @param [in] vector2: Second vector * @return Angle between two vectors [rad] */ - double CalAngleVector_rad(const math::Vector<3>& vector1, const math::Vector<3>& vector2); + double CalAngleVector_rad(const s2e::math::Vector<3>& vector1, const s2e::math::Vector<3>& vector2); /** * @fn Initialize diff --git a/src/components/real/aocs/sun_sensor.cpp b/src/components/real/aocs/sun_sensor.cpp index 6b47949ae..b9fbd3cae 100644 --- a/src/components/real/aocs/sun_sensor.cpp +++ b/src/components/real/aocs/sun_sensor.cpp @@ -14,7 +14,7 @@ using randomization::NormalRand; using namespace std; -SunSensor::SunSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, +SunSensor::SunSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) @@ -29,7 +29,7 @@ SunSensor::SunSensor(const int prescaler, ClockGenerator* clock_generator, const } SunSensor::SunSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, + const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) : Component(prescaler, clock_generator, power_port), @@ -59,8 +59,8 @@ void SunSensor::MainRoutine(const int time_count) { } void SunSensor::Measure() { - math::Vector<3> sun_pos_b = local_celestial_information_->GetPositionFromSpacecraft_b_m("SUN"); - math::Vector<3> sun_dir_b = sun_pos_b.CalcNormalizedVector(); + s2e::math::Vector<3> sun_pos_b = local_celestial_information_->GetPositionFromSpacecraft_b_m("SUN"); + s2e::math::Vector<3> sun_dir_b = sun_pos_b.CalcNormalizedVector(); sun_direction_true_c_ = quaternion_b2c_.FrameConversion(sun_dir_b); // Frame conversion from body to component @@ -87,7 +87,7 @@ void SunSensor::Measure() { measured_sun_direction_c_ = measured_sun_direction_c_.CalcNormalizedVector(); } else { - measured_sun_direction_c_ = math::Vector<3>(0); + measured_sun_direction_c_ = s2e::math::Vector<3>(0); alpha_rad_ = 0.0; beta_rad_ = 0.0; } @@ -96,7 +96,7 @@ void SunSensor::Measure() { } void SunSensor::SunDetectionJudgement() { - math::Vector<3> sun_direction_c = sun_direction_true_c_.CalcNormalizedVector(); + s2e::math::Vector<3> sun_direction_c = sun_direction_true_c_.CalcNormalizedVector(); double sun_angle_ = acos(sun_direction_c[2]); @@ -112,10 +112,10 @@ void SunSensor::SunDetectionJudgement() { } void SunSensor::CalcSolarIlluminance() { - math::Vector<3> sun_direction_c = sun_direction_true_c_.CalcNormalizedVector(); + s2e::math::Vector<3> sun_direction_c = sun_direction_true_c_.CalcNormalizedVector(); double sun_angle_ = acos(sun_direction_c[2]); - if (sun_angle_ > math::pi_2) { + if (sun_angle_ > s2e::math::pi_2) { solar_illuminance_W_m2_ = 0.0; return; } @@ -126,8 +126,8 @@ void SunSensor::CalcSolarIlluminance() { } double SunSensor::TanRange(double x) { - if (x > math::pi_2) x = math::pi - x; - if (x < -math::pi_2) x = -math::pi - x; + if (x > s2e::math::pi_2) x = s2e::math::pi - x; + if (x < -s2e::math::pi_2) x = -s2e::math::pi - x; return x; } @@ -161,20 +161,20 @@ SunSensor InitSunSensor(ClockGenerator* clock_generator, int ss_id, std::string int prescaler = ss_conf.ReadInt(Section, "prescaler"); if (prescaler <= 1) prescaler = 1; - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; ss_conf.ReadQuaternion(Section, "quaternion_b2c", quaternion_b2c); double detectable_angle_deg = 0.0, detectable_angle_rad = 0.0; detectable_angle_deg = ss_conf.ReadDouble(Section, "field_of_view_deg"); - detectable_angle_rad = math::pi / 180.0 * detectable_angle_deg; + detectable_angle_rad = s2e::math::pi / 180.0 * detectable_angle_deg; double nr_stddev = 0.0; nr_stddev = ss_conf.ReadDouble(Section, "white_noise_standard_deviation_deg"); - nr_stddev *= math::pi / 180.0; + nr_stddev *= s2e::math::pi / 180.0; double nr_bias_stddev = 0.0; nr_bias_stddev = ss_conf.ReadDouble(Section, "bias_standard_deviation_deg"); - nr_bias_stddev *= math::pi / 180.0; + nr_bias_stddev *= s2e::math::pi / 180.0; double intensity_lower_threshold_percent; intensity_lower_threshold_percent = ss_conf.ReadDouble(Section, "intensity_lower_threshold_percent"); @@ -194,20 +194,20 @@ SunSensor InitSunSensor(ClockGenerator* clock_generator, PowerPort* power_port, int prescaler = ss_conf.ReadInt(Section, "prescaler"); if (prescaler <= 1) prescaler = 1; - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; ss_conf.ReadQuaternion(Section, "quaternion_b2c", quaternion_b2c); double detectable_angle_deg = 0.0, detectable_angle_rad = 0.0; detectable_angle_deg = ss_conf.ReadDouble(Section, "field_of_view_deg"); - detectable_angle_rad = math::pi / 180.0 * detectable_angle_deg; + detectable_angle_rad = s2e::math::pi / 180.0 * detectable_angle_deg; double nr_stddev = 0.0; nr_stddev = ss_conf.ReadDouble(Section, "white_noise_standard_deviation_deg"); - nr_stddev *= math::pi / 180.0; + nr_stddev *= s2e::math::pi / 180.0; double nr_bias_stddev = 0.0; nr_bias_stddev = ss_conf.ReadDouble(Section, "bias_standard_deviation_deg"); - nr_bias_stddev *= math::pi / 180.0; + nr_bias_stddev *= s2e::math::pi / 180.0; double intensity_lower_threshold_percent; intensity_lower_threshold_percent = ss_conf.ReadDouble(Section, "intensity_lower_threshold_percent"); diff --git a/src/components/real/aocs/sun_sensor.hpp b/src/components/real/aocs/sun_sensor.hpp index e2c9623fd..81625a87c 100644 --- a/src/components/real/aocs/sun_sensor.hpp +++ b/src/components/real/aocs/sun_sensor.hpp @@ -35,7 +35,7 @@ class SunSensor : public Component, public ILoggable { * @param [in] srp_environment: Solar Radiation Pressure environment * @param [in] local_celestial_information: Local celestial information */ - SunSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, + SunSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); @@ -55,7 +55,7 @@ class SunSensor : public Component, public ILoggable { * @param [in] local_celestial_information: Local celestial information */ SunSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, + const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); @@ -80,19 +80,19 @@ class SunSensor : public Component, public ILoggable { // Getter inline bool GetSunDetectedFlag() const { return sun_detected_flag_; }; - inline const math::Vector<3> GetMeasuredSunDirection_c() const { return measured_sun_direction_c_; }; - inline const math::Vector<3> GetMeasuredSunDirection_b() const { return quaternion_b2c_.Conjugate().FrameConversion(measured_sun_direction_c_); }; + inline const s2e::math::Vector<3> GetMeasuredSunDirection_c() const { return measured_sun_direction_c_; }; + inline const s2e::math::Vector<3> GetMeasuredSunDirection_b() const { return quaternion_b2c_.Conjugate().FrameConversion(measured_sun_direction_c_); }; inline double GetSunAngleAlpha_rad() const { return alpha_rad_; }; inline double GetSunAngleBeta_rad() const { return beta_rad_; }; inline double GetSolarIlluminance_W_m2() const { return solar_illuminance_W_m2_; }; protected: const int component_id_; //!< Sensor ID - math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (Z-axis of the component is sight direction) + s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (Z-axis of the component is sight direction) double intensity_lower_threshold_percent_; //!< If the light intensity becomes smaller than this, it becomes impossible to get the sun direction - math::Vector<3> sun_direction_true_c_{0.0}; //!< True value of sun vector in the component frame - math::Vector<3> measured_sun_direction_c_{0.0}; //!< Measured sun vector in the component frame + s2e::math::Vector<3> sun_direction_true_c_{0.0}; //!< True value of sun vector in the component frame + s2e::math::Vector<3> measured_sun_direction_c_{0.0}; //!< Measured sun vector in the component frame double alpha_rad_ = 0.0; //!< Angle between Z-axis and the sun direction projected on XZ plane [rad] double beta_rad_ = 0.0; //!< Angle between Z-axis and the sun direction projected on YZ plane [rad] diff --git a/src/components/real/communication/antenna.cpp b/src/components/real/communication/antenna.cpp index b0713a76e..db044cb5d 100644 --- a/src/components/real/communication/antenna.cpp +++ b/src/components/real/communication/antenna.cpp @@ -11,7 +11,7 @@ #include #include -Antenna::Antenna(const int component_id, const math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, +Antenna::Antenna(const int component_id, const s2e::math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, const double frequency_MHz, const Vector<5> tx_parameters, const Vector<4> rx_parameters) : component_id_(component_id), is_transmitter_(is_transmitter), is_receiver_(is_receiver), frequency_MHz_(frequency_MHz) { quaternion_b2c_ = quaternion_b2c; @@ -45,7 +45,7 @@ Antenna::Antenna(const int component_id, const math::Quaternion& quaternion_b2c, } } -Antenna::Antenna(const int component_id, const math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, +Antenna::Antenna(const int component_id, const s2e::math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, const double frequency_MHz, const double tx_bitrate_bps, const double tx_output_power_W, const AntennaParameters tx_parameters, const double rx_system_noise_temperature_K, const AntennaParameters rx_parameters) : component_id_(component_id), diff --git a/src/components/real/communication/antenna.hpp b/src/components/real/communication/antenna.hpp index fbdaffb0a..78d8f48df 100644 --- a/src/components/real/communication/antenna.hpp +++ b/src/components/real/communication/antenna.hpp @@ -8,8 +8,8 @@ #include #include -using math::Quaternion; -using math::Vector; +using s2e::math::Quaternion; +using s2e::math::Vector; #include #include "./antenna_radiation_pattern.hpp" @@ -54,7 +54,7 @@ class Antenna { * @param [in] tx_parameters: output, gain, loss_feeder, loss_pointing for TX * @param [in] rx_parameters: gain, loss_feeder, loss_pointing, system_temperature for RX */ - Antenna(const int component_id, const math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, + Antenna(const int component_id, const s2e::math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, const double frequency_MHz, const Vector<5> tx_parameters, const Vector<4> rx_parameters); /** @@ -71,7 +71,7 @@ class Antenna { * @param [in] rx_system_noise_temperature_K: Receive system noise temperature [K] * @param [in] rx_parameters: RX antenna parameters */ - Antenna(const int component_id, const math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, + Antenna(const int component_id, const s2e::math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, const double frequency_MHz, const double tx_bitrate_bps, const double tx_output_power_W, const AntennaParameters tx_parameters, const double rx_system_noise_temperature_K, const AntennaParameters rx_parameters); /** diff --git a/src/components/real/communication/antenna_radiation_pattern.hpp b/src/components/real/communication/antenna_radiation_pattern.hpp index 422d168ab..93aff7df9 100644 --- a/src/components/real/communication/antenna_radiation_pattern.hpp +++ b/src/components/real/communication/antenna_radiation_pattern.hpp @@ -36,7 +36,7 @@ class AntennaRadiationPattern { * @param[in] phi_max_rad_: Maximum value of phi */ AntennaRadiationPattern(const std::string file_path, const size_t length_theta = 360, const size_t length_phi = 181, - const double theta_max_rad = math::tau, const double phi_max_rad = math::pi); + const double theta_max_rad = s2e::math::tau, const double phi_max_rad = s2e::math::pi); /** * @fn ~AntennaRadiationPattern @@ -56,8 +56,8 @@ class AntennaRadiationPattern { private: size_t length_theta_ = 360; //!< Length of grid for theta direction size_t length_phi_ = 181; //!< Length of grid for phi direction - double theta_max_rad_ = math::tau; //!< Maximum value of theta - double phi_max_rad_ = math::pi; //!< Maximum value of phi + double theta_max_rad_ = s2e::math::tau; //!< Maximum value of theta + double phi_max_rad_ = s2e::math::pi; //!< Maximum value of phi std::vector> gain_dBi_; //!< Antenna gain table [dBi] }; diff --git a/src/components/real/communication/ground_station_calculator.cpp b/src/components/real/communication/ground_station_calculator.cpp index 01b80ff25..2f4d1f19a 100644 --- a/src/components/real/communication/ground_station_calculator.cpp +++ b/src/components/real/communication/ground_station_calculator.cpp @@ -72,7 +72,7 @@ double GroundStationCalculator::CalcCn0OnGs(const Dynamics& dynamics, const Ante Vector<3> pos_gs2sc_i = sc_pos_i - gs_pos_i; double dist_sc_gs_km = pos_gs2sc_i.CalcNorm() / 1000.0; - double loss_space_dB = -20.0 * log10(4.0 * math::pi * dist_sc_gs_km / (300.0 / spacecraft_tx_antenna.GetFrequency_MHz() / 1000.0)); + double loss_space_dB = -20.0 * log10(4.0 * s2e::math::pi * dist_sc_gs_km / (300.0 / spacecraft_tx_antenna.GetFrequency_MHz() / 1000.0)); // GS direction on SC TX antenna frame Vector<3> sc_to_gs_i = gs_pos_i - sc_pos_i; diff --git a/src/components/real/mission/telescope.cpp b/src/components/real/mission/telescope.cpp index 31bc2919f..0e5a01c91 100644 --- a/src/components/real/mission/telescope.cpp +++ b/src/components/real/mission/telescope.cpp @@ -11,9 +11,9 @@ #include using namespace std; -using namespace math; +using namespace s2e::math; -Telescope::Telescope(ClockGenerator* clock_generator, const math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, +Telescope::Telescope(ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, const Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, @@ -38,8 +38,8 @@ Telescope::Telescope(ClockGenerator* clock_generator, const math::Quaternion& qu x_field_of_view_rad = x_number_of_pix_ * x_fov_per_pix_; y_field_of_view_rad = y_number_of_pix_ * y_fov_per_pix_; - assert(x_field_of_view_rad < math::pi_2); // Avoid the case that the field of view is over 90 degrees - assert(y_field_of_view_rad < math::pi_2); + assert(x_field_of_view_rad < s2e::math::pi_2); // Avoid the case that the field of view is over 90 degrees + assert(y_field_of_view_rad < s2e::math::pi_2); sight_direction_c_ = Vector<3>(0); sight_direction_c_[0] = 1; // (1,0,0) at component frame, Sight direction vector @@ -58,7 +58,7 @@ Telescope::Telescope(ClockGenerator* clock_generator, const math::Quaternion& qu } // Get initial spacecraft position in ECEF if (orbit_ != nullptr) { - math::Vector<3> initial_spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m(); + s2e::math::Vector<3> initial_spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m(); initial_ground_position_ecef_m_ = environment::earth_equatorial_radius_m * initial_spacecraft_position_ecef_m; initial_ground_position_ecef_m_ /= (orbit_->GetGeodeticPosition().GetAltitude_m() + environment::earth_equatorial_radius_m); } @@ -83,26 +83,26 @@ void Telescope::MainRoutine(const int time_count) { // sun_pos_c = quaternion_b2c_.FrameConversion(dynamics_->celestial_->GetPositionFromSpacecraft_b_m("SUN")); // earth_pos_c = quaternion_b2c_.FrameConversion(dynamics_->celestial_->GetPositionFromSpacecraft_b_m("EARTH")); // moon_pos_c = quaternion_b2c_.FrameConversion(dynamics_->celestial_->GetPositionFromSpacecraft_b_m("MOON")); - // angle_sun = CalcAngleTwoVectors_rad(sight_direction_c_, sun_pos_c) * 180/math::pi; - // angle_earth = CalcAngleTwoVectors_rad(sight_direction_c_, earth_pos_c) * 180 / math::pi; angle_moon = - // CalcAngleTwoVectors_rad(sight_direction_c_, moon_pos_c) * 180 / math::pi; + // angle_sun = CalcAngleTwoVectors_rad(sight_direction_c_, sun_pos_c) * 180/s2e::math::pi; + // angle_earth = CalcAngleTwoVectors_rad(sight_direction_c_, earth_pos_c) * 180 / s2e::math::pi; angle_moon = + // CalcAngleTwoVectors_rad(sight_direction_c_, moon_pos_c) * 180 / s2e::math::pi; //****************************************************************************** // Direction calculation of ground point ObserveGroundPositionDeviation(); } -bool Telescope::JudgeForbiddenAngle(const math::Vector<3>& target_b, const double forbidden_angle) { - math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); - math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); - double angle_rad = math::CalcAngleTwoVectors_rad(target_b, sight_b); +bool Telescope::JudgeForbiddenAngle(const s2e::math::Vector<3>& target_b, const double forbidden_angle) { + s2e::math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); + s2e::math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); + double angle_rad = s2e::math::CalcAngleTwoVectors_rad(target_b, sight_b); if (angle_rad < forbidden_angle) { return true; } else return false; } -void Telescope::Observe(math::Vector<2>& position_image_sensor, const math::Vector<3, double> target_b) { - math::Vector<3, double> target_c = quaternion_b2c_.FrameConversion(target_b); +void Telescope::Observe(s2e::math::Vector<2>& position_image_sensor, const s2e::math::Vector<3, double> target_b) { + s2e::math::Vector<3, double> target_c = quaternion_b2c_.FrameConversion(target_b); double arg_x = atan2(target_c[2], target_c[0]); // Angle from X-axis on XZ plane in the component frame double arg_y = atan2(target_c[1], target_c[0]); // Angle from X-axis on XY plane in the component frame @@ -122,8 +122,8 @@ void Telescope::ObserveStars() { size_t count = 0; // Counter for while loop while (star_list_in_sight.size() < number_of_logged_stars_) { - math::Vector<3> target_b = hipparcos_->GetStarDirection_b(count, quaternion_i2b); - math::Vector<3> target_c = quaternion_b2c_.FrameConversion(target_b); + s2e::math::Vector<3> target_b = hipparcos_->GetStarDirection_b(count, quaternion_i2b); + s2e::math::Vector<3> target_c = quaternion_b2c_.FrameConversion(target_b); double arg_x = atan2(target_c[2], target_c[0]); // Angle from X-axis on XZ plane in the component frame double arg_y = atan2(target_c[1], target_c[0]); // Angle from X-axis on XY plane in the component frame @@ -174,12 +174,12 @@ void Telescope::ObserveGroundPositionDeviation() { } Quaternion quaternion_i2b = attitude_->GetQuaternion_i2b(); - math::Vector<3> spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m(); - math::Vector<3> direction_ecef = (initial_ground_position_ecef_m_ - spacecraft_position_ecef_m).CalcNormalizedVector(); - math::Matrix<3, 3> dcm_ecef_to_i = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); - math::Vector<3> direction_i = (dcm_ecef_to_i * direction_ecef).CalcNormalizedVector(); - math::Vector<3> direction_b = quaternion_i2b.FrameConversion(direction_i); - math::Vector<3> target_c = quaternion_b2c_.FrameConversion(direction_b); + s2e::math::Vector<3> spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m(); + s2e::math::Vector<3> direction_ecef = (initial_ground_position_ecef_m_ - spacecraft_position_ecef_m).CalcNormalizedVector(); + s2e::math::Matrix<3, 3> dcm_ecef_to_i = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); + s2e::math::Vector<3> direction_i = (dcm_ecef_to_i * direction_ecef).CalcNormalizedVector(); + s2e::math::Vector<3> direction_b = quaternion_i2b.FrameConversion(direction_i); + s2e::math::Vector<3> target_c = quaternion_b2c_.FrameConversion(direction_b); // Ground position in the image sensor in the satellite frame double ground_angle_z_rad = atan2(target_c[2], target_c[0]); @@ -247,7 +247,7 @@ string Telescope::GetLogValue() const { Telescope InitTelescope(ClockGenerator* clock_generator, int sensor_id, const string file_name, const Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) { - using math::pi; + using s2e::math::pi; IniAccess Telescope_conf(file_name); const string st_sensor_id = std::to_string(static_cast(sensor_id)); @@ -260,7 +260,7 @@ Telescope InitTelescope(ClockGenerator* clock_generator, int sensor_id, const st strcat(TelescopeSection, cs); #endif - math::Quaternion quaternion_b2c; + s2e::math::Quaternion quaternion_b2c; Telescope_conf.ReadQuaternion(TelescopeSection, "quaternion_b2c", quaternion_b2c); double sun_forbidden_angle_deg = Telescope_conf.ReadDouble(TelescopeSection, "sun_exclusion_angle_deg"); diff --git a/src/components/real/mission/telescope.hpp b/src/components/real/mission/telescope.hpp index dec83517b..737a49c98 100644 --- a/src/components/real/mission/telescope.hpp +++ b/src/components/real/mission/telescope.hpp @@ -23,7 +23,7 @@ */ struct Star { HipparcosData hipparcos_data; //!< Hipparcos data - math::Vector<2> position_image_sensor; //!< Position of image sensor + s2e::math::Vector<2> position_image_sensor; //!< Position of image sensor }; /* @@ -50,7 +50,7 @@ class Telescope : public Component, public ILoggable { * @param [in] local_celestial_information: Local celestial information * @param [in] orbit: Orbit information */ - Telescope(ClockGenerator* clock_generator, const math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, + Telescope(ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, const Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr); @@ -67,8 +67,8 @@ class Telescope : public Component, public ILoggable { protected: private: - math::Quaternion quaternion_b2c_; //!< Quaternion from the body frame to component frame - math::Vector<3> sight_direction_c_; //!< Sight direction vector in the component frame + s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from the body frame to component frame + s2e::math::Vector<3> sight_direction_c_; //!< Sight direction vector in the component frame double sun_forbidden_angle_rad_; //!< Sun forbidden angle [rad] double earth_forbidden_angle_rad_; //!< Earth forbidden angle [rad] @@ -89,10 +89,10 @@ class Telescope : public Component, public ILoggable { size_t number_of_logged_stars_; //!< Number of logged stars - math::Vector<2> sun_position_image_sensor{-1}; //!< Position of the sun on the image plane - math::Vector<2> earth_position_image_sensor{-1}; //!< Position of the earth on the image plane - math::Vector<2> moon_position_image_sensor{-1}; //!< Position of the moon on the image plane - math::Vector<3> initial_ground_position_ecef_m_; //!< Initial spacecraft position + s2e::math::Vector<2> sun_position_image_sensor{-1}; //!< Position of the sun on the image plane + s2e::math::Vector<2> earth_position_image_sensor{-1}; //!< Position of the earth on the image plane + s2e::math::Vector<2> moon_position_image_sensor{-1}; //!< Position of the moon on the image plane + s2e::math::Vector<3> initial_ground_position_ecef_m_; //!< Initial spacecraft position std::vector star_list_in_sight; //!< Star information in the field of view @@ -102,7 +102,7 @@ class Telescope : public Component, public ILoggable { * @param [in] target_b: Direction vector of target on the body fixed frame * @param [in] forbidden_angle: Forbidden angle [rad] */ - bool JudgeForbiddenAngle(const math::Vector<3>& target_b, const double forbidden_angle); + bool JudgeForbiddenAngle(const s2e::math::Vector<3>& target_b, const double forbidden_angle); // Override functions for Component /** @@ -117,7 +117,7 @@ class Telescope : public Component, public ILoggable { * @param [out] position_image_sensor: Position on image sensor plane * @param [in] target_b: Direction vector of target on the body fixed frame */ - void Observe(math::Vector<2>& position_image_sensor, const math::Vector<3, double> target_b); + void Observe(s2e::math::Vector<2>& position_image_sensor, const s2e::math::Vector<3, double> target_b); /** * @fn ObserveStars * @brief Observe stars from Hipparcos catalogue @@ -147,9 +147,9 @@ class Telescope : public Component, public ILoggable { virtual std::string GetLogValue() const; // For debug ********************************************** - // math::Vector<3> sun_pos_c; - // math::Vector<3> earth_pos_c; - // math::Vector<3> moon_pos_c; + // s2e::math::Vector<3> sun_pos_c; + // s2e::math::Vector<3> earth_pos_c; + // s2e::math::Vector<3> moon_pos_c; // double angle_sun; // double angle_earth; // double angle_moon; diff --git a/src/components/real/power/csv_scenario_interface.cpp b/src/components/real/power/csv_scenario_interface.cpp index d0b5c1beb..7978d510c 100644 --- a/src/components/real/power/csv_scenario_interface.cpp +++ b/src/components/real/power/csv_scenario_interface.cpp @@ -36,8 +36,8 @@ void CsvScenarioInterface::Initialize(const std::string file_name) { bool CsvScenarioInterface::IsCsvScenarioEnabled() { return CsvScenarioInterface::is_csv_scenario_enabled_; } -math::Vector<3> CsvScenarioInterface::GetSunDirectionBody(const double time_query) { - math::Vector<3> sun_dir_b; +s2e::math::Vector<3> CsvScenarioInterface::GetSunDirectionBody(const double time_query) { + s2e::math::Vector<3> sun_dir_b; sun_dir_b[0] = GetValueFromBuffer("sun_dir_b_x", time_query); sun_dir_b[1] = GetValueFromBuffer("sun_dir_b_y", time_query); sun_dir_b[2] = GetValueFromBuffer("sun_dir_b_z", time_query); diff --git a/src/components/real/power/csv_scenario_interface.hpp b/src/components/real/power/csv_scenario_interface.hpp index 30f96ead1..e8177ca5f 100644 --- a/src/components/real/power/csv_scenario_interface.hpp +++ b/src/components/real/power/csv_scenario_interface.hpp @@ -36,7 +36,7 @@ class CsvScenarioInterface { * @brief Return sun direction vector in the body fixed frame * @param [in] time_query: Time query */ - static math::Vector<3> GetSunDirectionBody(const double time_query); + static s2e::math::Vector<3> GetSunDirectionBody(const double time_query); /** * @fn GetSunFlag * @brief Return sun flag diff --git a/src/components/real/power/solar_array_panel.cpp b/src/components/real/power/solar_array_panel.cpp index 43a22afa8..126f66da7 100644 --- a/src/components/real/power/solar_array_panel.cpp +++ b/src/components/real/power/solar_array_panel.cpp @@ -10,7 +10,7 @@ #include SolarArrayPanel::SolarArrayPanel(const int prescaler, ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, - double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, double component_step_time_s) : Component(prescaler, clock_generator), @@ -29,7 +29,7 @@ SolarArrayPanel::SolarArrayPanel(const int prescaler, ClockGenerator* clock_gene } SolarArrayPanel::SolarArrayPanel(const int prescaler, ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, - double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s) : Component(prescaler, clock_generator), component_id_(component_id), @@ -46,7 +46,7 @@ SolarArrayPanel::SolarArrayPanel(const int prescaler, ClockGenerator* clock_gene } SolarArrayPanel::SolarArrayPanel(ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, - math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) : Component(10, clock_generator), @@ -99,14 +99,14 @@ void SolarArrayPanel::MainRoutine(const int time_count) { if (CsvScenarioInterface::IsCsvScenarioEnabled()) { double time_query = compo_step_time_s_ * time_count; const auto solar_constant = srp_environment_->GetSolarConstant_W_m2(); - math::Vector<3> sun_direction_body = CsvScenarioInterface::GetSunDirectionBody(time_query); - math::Vector<3> normalized_sun_direction_body = sun_direction_body.CalcNormalizedVector(); + s2e::math::Vector<3> sun_direction_body = CsvScenarioInterface::GetSunDirectionBody(time_query); + s2e::math::Vector<3> normalized_sun_direction_body = sun_direction_body.CalcNormalizedVector(); power_generation_W_ = cell_efficiency_ * transmission_efficiency_ * solar_constant * (int)CsvScenarioInterface::GetSunFlag(time_query) * cell_area_m2_ * number_of_parallel_ * number_of_series_ * InnerProduct(normal_vector_, normalized_sun_direction_body); } else { const auto power_density = srp_environment_->GetPowerDensity_W_m2(); - math::Vector<3> sun_pos_b = local_celestial_information_->GetPositionFromSpacecraft_b_m("SUN"); - math::Vector<3> sun_dir_b = sun_pos_b.CalcNormalizedVector(); + s2e::math::Vector<3> sun_pos_b = local_celestial_information_->GetPositionFromSpacecraft_b_m("SUN"); + s2e::math::Vector<3> sun_dir_b = sun_pos_b.CalcNormalizedVector(); power_generation_W_ = cell_efficiency_ * transmission_efficiency_ * power_density * cell_area_m2_ * number_of_parallel_ * number_of_series_ * InnerProduct(normal_vector_, sun_dir_b); // TODO: Improve implementation. For example, update IV curve with sun direction and calculate generated power @@ -133,7 +133,7 @@ SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std:: double cell_area_m2; cell_area_m2 = sap_conf.ReadDouble(section_name.c_str(), "cell_area_m2"); - math::Vector<3> normal_vector; + s2e::math::Vector<3> normal_vector; sap_conf.ReadVector(section_name.c_str(), "normal_vector_b", normal_vector); double cell_efficiency; @@ -166,7 +166,7 @@ SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std:: double cell_area_m2; cell_area_m2 = sap_conf.ReadDouble(section_name.c_str(), "cell_area_m2"); - math::Vector<3> normal_vector; + s2e::math::Vector<3> normal_vector; sap_conf.ReadVector(section_name.c_str(), "normal_vector_b", normal_vector); double cell_efficiency; diff --git a/src/components/real/power/solar_array_panel.hpp b/src/components/real/power/solar_array_panel.hpp index 7ffa91195..db114a081 100644 --- a/src/components/real/power/solar_array_panel.hpp +++ b/src/components/real/power/solar_array_panel.hpp @@ -32,7 +32,7 @@ class SolarArrayPanel : public Component, public ILoggable { * @param [in] component_step_time_s: Component step time [sec] */ SolarArrayPanel(const int prescaler, ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, - double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, double component_step_time_s); /** @@ -51,7 +51,7 @@ class SolarArrayPanel : public Component, public ILoggable { * @param [in] component_step_time_s: Component step time [sec] */ SolarArrayPanel(const int prescaler, ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, - double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s); /** * @fn SolarArrayPanel @@ -69,7 +69,7 @@ class SolarArrayPanel : public Component, public ILoggable { * @param [in] local_celestial_information: Local celestial information */ SolarArrayPanel(ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, - math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); /** * @fn SolarArrayPanel @@ -111,7 +111,7 @@ class SolarArrayPanel : public Component, public ILoggable { const int number_of_series_; //!< Number of series connected solar cells const int number_of_parallel_; //!< Number of parallel connected solar cells const double cell_area_m2_; //!< Solar cell area [m^2] - const math::Vector<3> normal_vector_; //!< Normal vector of SolarArrayPanel on the body fixed frame + const s2e::math::Vector<3> normal_vector_; //!< Normal vector of SolarArrayPanel on the body fixed frame const double cell_efficiency_; //!< Power generation efficiency of solar cell const double transmission_efficiency_; //!< Efficiency of transmission to PCU diff --git a/src/components/real/propulsion/simple_thruster.cpp b/src/components/real/propulsion/simple_thruster.cpp index 70c5f2a1b..7917b5fb1 100644 --- a/src/components/real/propulsion/simple_thruster.cpp +++ b/src/components/real/propulsion/simple_thruster.cpp @@ -11,7 +11,7 @@ // Constructor SimpleThruster::SimpleThruster(const int prescaler, ClockGenerator* clock_generator, const int component_id, - const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, + const s2e::math::Vector<3> thruster_position_b_m, const s2e::math::Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, const Dynamics* dynamics) : Component(prescaler, clock_generator), @@ -26,7 +26,7 @@ SimpleThruster::SimpleThruster(const int prescaler, ClockGenerator* clock_genera } SimpleThruster::SimpleThruster(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, + const s2e::math::Vector<3> thruster_position_b_m, const s2e::math::Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, const Dynamics* dynamics) : Component(prescaler, clock_generator, power_port), @@ -66,9 +66,9 @@ void SimpleThruster::CalcThrust() { output_thrust_b_N_ = mag * CalcThrustDirection(); } -void SimpleThruster::CalcTorque(const math::Vector<3> center_of_mass_b_m) { - math::Vector<3> vector_center2thruster = thruster_position_b_m_ - center_of_mass_b_m; - math::Vector<3> torque = OuterProduct(vector_center2thruster, output_thrust_b_N_); +void SimpleThruster::CalcTorque(const s2e::math::Vector<3> center_of_mass_b_m) { + s2e::math::Vector<3> vector_center2thruster = thruster_position_b_m_ - center_of_mass_b_m; + s2e::math::Vector<3> torque = OuterProduct(vector_center2thruster, output_thrust_b_N_); output_torque_b_Nm_ = torque; } @@ -95,25 +95,25 @@ std::string SimpleThruster::GetLogValue() const { double SimpleThruster::CalcThrustMagnitude() { return duty_ * thrust_magnitude_max_N_; } -math::Vector<3> SimpleThruster::CalcThrustDirection() { - math::Vector<3> thrust_dir_b_true = thrust_direction_b_; +s2e::math::Vector<3> SimpleThruster::CalcThrustDirection() { + s2e::math::Vector<3> thrust_dir_b_true = thrust_direction_b_; if (direction_noise_standard_deviation_rad_ > 0.0 + DBL_EPSILON) { - math::Vector<3> ex; // Fixme: to use outer product to generate orthogonal vector + s2e::math::Vector<3> ex; // Fixme: to use outer product to generate orthogonal vector ex[0] = 1.0; ex[1] = 0.0; ex[2] = 0.0; int flag = rand() % 2; double make_axis_rot_rad; if (flag == 0) { - make_axis_rot_rad = math::pi * (double)rand() / RAND_MAX; + make_axis_rot_rad = s2e::math::pi * (double)rand() / RAND_MAX; } else { - make_axis_rot_rad = -math::pi * (double)rand() / RAND_MAX; + make_axis_rot_rad = -s2e::math::pi * (double)rand() / RAND_MAX; } - math::Quaternion make_axis_rot(thrust_dir_b_true, make_axis_rot_rad); - math::Vector<3> axis_rot = make_axis_rot.FrameConversion(ex); + s2e::math::Quaternion make_axis_rot(thrust_dir_b_true, make_axis_rot_rad); + s2e::math::Vector<3> axis_rot = make_axis_rot.FrameConversion(ex); - math::Quaternion err_rot(axis_rot, direction_random_noise_); // Generate error quaternion + s2e::math::Quaternion err_rot(axis_rot, direction_random_noise_); // Generate error quaternion thrust_dir_b_true = err_rot.FrameConversion(thrust_dir_b_true); // Add error } @@ -141,7 +141,7 @@ SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, int thruster_ magnitude_standard_deviation_N = thruster_conf.ReadDouble(Section, "thrust_error_standard_deviation_N"); double deg_err; - deg_err = thruster_conf.ReadDouble(Section, "direction_error_standard_deviation_deg") * math::pi / 180.0; + deg_err = thruster_conf.ReadDouble(Section, "direction_error_standard_deviation_deg") * s2e::math::pi / 180.0; SimpleThruster thruster(prescaler, clock_generator, thruster_id, thruster_pos, thruster_dir, max_magnitude_N, magnitude_standard_deviation_N, deg_err, structure, dynamics); @@ -169,7 +169,7 @@ SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, PowerPort* po magnitude_standard_deviation_N = thruster_conf.ReadDouble(Section, "thrust_error_standard_deviation_N"); double deg_err; - deg_err = thruster_conf.ReadDouble(Section, "direction_error_standard_deviation_deg") * math::pi / 180.0; + deg_err = thruster_conf.ReadDouble(Section, "direction_error_standard_deviation_deg") * s2e::math::pi / 180.0; power_port->InitializeWithInitializeFile(file_name); diff --git a/src/disturbances/air_drag.cpp b/src/disturbances/air_drag.cpp index d7d91cfac..e64593201 100644 --- a/src/disturbances/air_drag.cpp +++ b/src/disturbances/air_drag.cpp @@ -12,7 +12,7 @@ #include "../logger/log_utility.hpp" -AirDrag::AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, +AirDrag::AirDrag(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, const double molecular_temperature_K, const double molecular_weight_g_mol, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled), wall_temperature_K_(wall_temperature_K), @@ -26,15 +26,15 @@ AirDrag::AirDrag(const std::vector& surfaces, const math::Vector<3>& ce void AirDrag::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { double air_density_kg_m3 = local_environment.GetAtmosphere().GetAirDensity_kg_m3(); - math::Matrix<3, 3> dcm_ecef2eci = + s2e::math::Matrix<3, 3> dcm_ecef2eci = local_environment.GetCelestialInformation().GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); - math::Vector<3> relative_velocity_wrt_atmosphere_i_m_s = dcm_ecef2eci * dynamics.GetOrbit().GetVelocity_ecef_m_s(); - math::Quaternion quaternion_i2b = dynamics.GetAttitude().GetQuaternion_i2b(); - math::Vector<3> velocity_b_m_s = quaternion_i2b.FrameConversion(relative_velocity_wrt_atmosphere_i_m_s); + s2e::math::Vector<3> relative_velocity_wrt_atmosphere_i_m_s = dcm_ecef2eci * dynamics.GetOrbit().GetVelocity_ecef_m_s(); + s2e::math::Quaternion quaternion_i2b = dynamics.GetAttitude().GetQuaternion_i2b(); + s2e::math::Vector<3> velocity_b_m_s = quaternion_i2b.FrameConversion(relative_velocity_wrt_atmosphere_i_m_s); CalcTorqueForce(velocity_b_m_s, air_density_kg_m3); } -void AirDrag::CalcCoefficients(const math::Vector<3>& velocity_b_m_s, const double air_density_kg_m3) { +void AirDrag::CalcCoefficients(const s2e::math::Vector<3>& velocity_b_m_s, const double air_density_kg_m3) { double velocity_norm_m_s = velocity_b_m_s.CalcNorm(); CalcCnCt(velocity_b_m_s); for (size_t i = 0; i < surfaces_.size(); i++) { @@ -47,14 +47,14 @@ void AirDrag::CalcCoefficients(const math::Vector<3>& velocity_b_m_s, const doub double AirDrag::CalcFunctionPi(const double s) { double x; double erfs = erf(s); // ERF function is defined in math standard library - x = s * exp(-s * s) + sqrt(math::pi) * (s * s + 0.5) * (1.0 + erfs); + x = s * exp(-s * s) + sqrt(s2e::math::pi) * (s * s + 0.5) * (1.0 + erfs); return x; } double AirDrag::CalcFunctionChi(const double s) { double x; double erfs = erf(s); - x = exp(-s * s) + sqrt(math::pi) * s * (1.0 + erfs); + x = exp(-s * s) + sqrt(s2e::math::pi) * s * (1.0 + erfs); return x; } @@ -69,9 +69,9 @@ void AirDrag::CalcCnCt(const Vector<3>& velocity_b_m_s) { double speed_n = speed * cos_theta_[i]; double speed_t = speed * sin_theta_[i]; double diffuse = 1.0 - surfaces_[i].GetAirSpecularity(); - cn_[i] = (2.0 - diffuse) / sqrt(math::pi) * CalcFunctionPi(speed_n) / (speed * speed) + + cn_[i] = (2.0 - diffuse) / sqrt(s2e::math::pi) * CalcFunctionPi(speed_n) / (speed * speed) + diffuse / 2.0 * CalcFunctionChi(speed_n) / (speed * speed) * sqrt(wall_temperature_K_ / molecular_temperature_K_); - ct_[i] = diffuse * speed_t * CalcFunctionChi(speed_n) / (sqrt(math::pi) * speed * speed); + ct_[i] = diffuse * speed_t * CalcFunctionChi(speed_n) / (sqrt(s2e::math::pi) * speed * speed); } } diff --git a/src/disturbances/air_drag.hpp b/src/disturbances/air_drag.hpp index 6d5fe54fd..226e3595b 100644 --- a/src/disturbances/air_drag.hpp +++ b/src/disturbances/air_drag.hpp @@ -30,7 +30,7 @@ class AirDrag : public SurfaceForce { * @param [in] molecular_weight_g_mol: Molecular weight [g/mol] * @param [in] is_calculation_enabled: Calculation flag */ - AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, + AirDrag(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, const double molecular_temperature_K, const double molecular_weight_g_mol, const bool is_calculation_enabled = true); /** @@ -66,7 +66,7 @@ class AirDrag : public SurfaceForce { * @param [in] velocity_b_m_s: Spacecraft's velocity vector in the body frame [m/s] * @param [in] air_density_kg_m3: Air density around the spacecraft [kg/m^3] */ - void CalcCoefficients(const math::Vector<3>& velocity_b_m_s, const double air_density_kg_m3); + void CalcCoefficients(const s2e::math::Vector<3>& velocity_b_m_s, const double air_density_kg_m3); // internal function for calculation /** @@ -74,7 +74,7 @@ class AirDrag : public SurfaceForce { * @brief Calculate the Cn and Ct * @param [in] velocity_b_m_s: Spacecraft's velocity vector in the body frame [m/s] */ - void CalcCnCt(const math::Vector<3>& velocity_b_m_s); + void CalcCnCt(const s2e::math::Vector<3>& velocity_b_m_s); /** * @fn CalcFunctionPi * @brief Calculate The Pi function in the algorithm diff --git a/src/disturbances/disturbance.hpp b/src/disturbances/disturbance.hpp index 6cc662728..427c67fc5 100644 --- a/src/disturbances/disturbance.hpp +++ b/src/disturbances/disturbance.hpp @@ -23,10 +23,10 @@ class Disturbance : public ILoggable { */ Disturbance(const bool is_calculation_enabled = true, const bool is_attitude_dependent = true) : is_calculation_enabled_(is_calculation_enabled), is_attitude_dependent_(is_attitude_dependent) { - force_b_N_ = math::Vector<3>(0.0); - torque_b_Nm_ = math::Vector<3>(0.0); - acceleration_i_m_s2_ = math::Vector<3>(0.0); - acceleration_b_m_s2_ = math::Vector<3>(0.0); + force_b_N_ = s2e::math::Vector<3>(0.0); + torque_b_Nm_ = s2e::math::Vector<3>(0.0); + acceleration_i_m_s2_ = s2e::math::Vector<3>(0.0); + acceleration_b_m_s2_ = s2e::math::Vector<3>(0.0); } /** @@ -60,22 +60,22 @@ class Disturbance : public ILoggable { * @fn GetTorque_b_Nm * @brief Return the disturbance torque in the body frame [Nm] */ - virtual inline math::Vector<3> GetTorque_b_Nm() { return torque_b_Nm_; } + virtual inline s2e::math::Vector<3> GetTorque_b_Nm() { return torque_b_Nm_; } /** * @fn GetForce_b_N * @brief Return the disturbance force in the body frame [N] */ - virtual inline math::Vector<3> GetForce_b_N() { return force_b_N_; } + virtual inline s2e::math::Vector<3> GetForce_b_N() { return force_b_N_; } /** * @fn GetAcceleration_b_m_s2 * @brief Return the disturbance acceleration in the body frame [m/s2] */ - virtual inline math::Vector<3> GetAcceleration_b_m_s2() { return acceleration_b_m_s2_; } + virtual inline s2e::math::Vector<3> GetAcceleration_b_m_s2() { return acceleration_b_m_s2_; } /** * @fn GetAcceleration_i_m_s2 * @brief Return the disturbance acceleration in the inertial frame [m/s2] */ - virtual inline math::Vector<3> GetAcceleration_i_m_s2() { return acceleration_i_m_s2_; } + virtual inline s2e::math::Vector<3> GetAcceleration_i_m_s2() { return acceleration_i_m_s2_; } /** * @fn IsAttitudeDependent * @brief Return the attitude dependent flag @@ -85,10 +85,10 @@ class Disturbance : public ILoggable { protected: bool is_calculation_enabled_; //!< Flag to calculate the disturbance bool is_attitude_dependent_; //!< Flag to show the disturbance depends on attitude information - math::Vector<3> force_b_N_; //!< Disturbance force in the body frame [N] - math::Vector<3> torque_b_Nm_; //!< Disturbance torque in the body frame [Nm] - math::Vector<3> acceleration_b_m_s2_; //!< Disturbance acceleration in the body frame [m/s2] - math::Vector<3> acceleration_i_m_s2_; //!< Disturbance acceleration in the inertial frame [m/s2] + s2e::math::Vector<3> force_b_N_; //!< Disturbance force in the body frame [N] + s2e::math::Vector<3> torque_b_Nm_; //!< Disturbance torque in the body frame [Nm] + s2e::math::Vector<3> acceleration_b_m_s2_; //!< Disturbance acceleration in the body frame [m/s2] + s2e::math::Vector<3> acceleration_i_m_s2_; //!< Disturbance acceleration in the inertial frame [m/s2] }; #endif // S2E_DISTURBANCES_DISTURBANCE_HPP_ diff --git a/src/disturbances/disturbances.hpp b/src/disturbances/disturbances.hpp index 62af465ba..948cabe47 100644 --- a/src/disturbances/disturbances.hpp +++ b/src/disturbances/disturbances.hpp @@ -55,19 +55,19 @@ class Disturbances { * @fn GetTorque * @brief Return total disturbance torque in the body frame [Nm] */ - inline math::Vector<3> GetTorque_b_Nm() { return total_torque_b_Nm_; } + inline s2e::math::Vector<3> GetTorque_b_Nm() { return total_torque_b_Nm_; } /** * @fn GetTorque * @brief Return total disturbance force in the body frame [N] */ - inline math::Vector<3> GetForce_b_N() { return total_force_b_N_; } + inline s2e::math::Vector<3> GetForce_b_N() { return total_force_b_N_; } /** * @fn GetTorque * @brief Return total disturbance acceleration in the inertial frame [m/s2] */ - inline math::Vector<3> GetAcceleration_i_m_s2() { return total_acceleration_i_m_s2_; } + inline s2e::math::Vector<3> GetAcceleration_i_m_s2() { return total_acceleration_i_m_s2_; } private: std::string initialize_file_name_; //!< Initialization file name diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index 70b7cb534..0520730ff 100644 --- a/src/disturbances/geopotential.cpp +++ b/src/disturbances/geopotential.cpp @@ -20,8 +20,8 @@ Geopotential::Geopotential(const int degree, const std::string file_path, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, false), degree_(degree) { // Initialize - acceleration_ecef_m_s2_ = math::Vector<3>(0.0); - debug_pos_ecef_m_ = math::Vector<3>(0.0); + acceleration_ecef_m_s2_ = s2e::math::Vector<3>(0.0); + debug_pos_ecef_m_ = s2e::math::Vector<3>(0.0); // degree if (degree_ > 360) { degree_ = 360; @@ -83,8 +83,8 @@ void Geopotential::Update(const LocalEnvironment &local_environment, const Dynam UNUSED(time_ms_); #endif - math::Matrix<3, 3> trans_eci2ecef_ = local_environment.GetCelestialInformation().GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef(); - math::Matrix<3, 3> trans_ecef2eci = trans_eci2ecef_.Transpose(); + s2e::math::Matrix<3, 3> trans_eci2ecef_ = local_environment.GetCelestialInformation().GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef(); + s2e::math::Matrix<3, 3> trans_ecef2eci = trans_eci2ecef_.Transpose(); acceleration_i_m_s2_ = trans_ecef2eci * acceleration_ecef_m_s2_; } diff --git a/src/disturbances/geopotential.hpp b/src/disturbances/geopotential.hpp index 4903cb412..0f66a463d 100644 --- a/src/disturbances/geopotential.hpp +++ b/src/disturbances/geopotential.hpp @@ -67,7 +67,7 @@ class Geopotential : public Disturbance { Vector<3> acceleration_ecef_m_s2_; //!< Calculated acceleration in the ECEF frame [m/s2] // debug - math::Vector<3> debug_pos_ecef_m_; //!< Spacecraft position in ECEF frame [m] + s2e::math::Vector<3> debug_pos_ecef_m_; //!< Spacecraft position in ECEF frame [m] double time_ms_ = 0.0; //!< Calculation time [ms] /** diff --git a/src/disturbances/gravity_gradient.cpp b/src/disturbances/gravity_gradient.cpp index b52b4bee5..5eff2c05a 100644 --- a/src/disturbances/gravity_gradient.cpp +++ b/src/disturbances/gravity_gradient.cpp @@ -23,9 +23,9 @@ void GravityGradient::Update(const LocalEnvironment& local_environment, const Dy dynamics.GetAttitude().GetInertiaTensor_b_kgm2()); } -math::Vector<3> GravityGradient::CalcTorque_b_Nm(const math::Vector<3> earth_position_from_sc_b_m, const math::Matrix<3, 3> inertia_tensor_b_kgm2) { +s2e::math::Vector<3> GravityGradient::CalcTorque_b_Nm(const s2e::math::Vector<3> earth_position_from_sc_b_m, const s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2) { double r_norm_m = earth_position_from_sc_b_m.CalcNorm(); - math::Vector<3> u_b = earth_position_from_sc_b_m; // TODO: make undestructive normalize function for Vector + s2e::math::Vector<3> u_b = earth_position_from_sc_b_m; // TODO: make undestructive normalize function for Vector u_b /= r_norm_m; double coeff = 3.0 * gravity_constant_m3_s2_ / pow(r_norm_m, 3.0); diff --git a/src/disturbances/gravity_gradient.hpp b/src/disturbances/gravity_gradient.hpp index be957e19c..9ca8ff37f 100644 --- a/src/disturbances/gravity_gradient.hpp +++ b/src/disturbances/gravity_gradient.hpp @@ -65,7 +65,7 @@ class GravityGradient : public Disturbance { * @param [in] inertia_tensor_b_kgm2: Inertia Tensor at body frame [kg*m^2] * @return Calculated torque at body frame [Nm] */ - math::Vector<3> CalcTorque_b_Nm(const math::Vector<3> earth_position_from_sc_b_m, const math::Matrix<3, 3> inertia_tensor_b_kgm2); + s2e::math::Vector<3> CalcTorque_b_Nm(const s2e::math::Vector<3> earth_position_from_sc_b_m, const s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2); }; /** diff --git a/src/disturbances/lunar_gravity_field.cpp b/src/disturbances/lunar_gravity_field.cpp index bad7c5f7c..1baa1319b 100644 --- a/src/disturbances/lunar_gravity_field.cpp +++ b/src/disturbances/lunar_gravity_field.cpp @@ -20,8 +20,8 @@ LunarGravityField::LunarGravityField(const int degree, const std::string file_path, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, false), degree_(degree) { // Initialize - acceleration_mcmf_m_s2_ = math::Vector<3>(0.0); - debug_pos_mcmf_m_ = math::Vector<3>(0.0); + acceleration_mcmf_m_s2_ = s2e::math::Vector<3>(0.0); + debug_pos_mcmf_m_ = s2e::math::Vector<3>(0.0); debug_pos_mcmf_m_[0] = 2000000; debug_pos_mcmf_m_[1] = 2000000; debug_pos_mcmf_m_[2] = 2000000; @@ -88,10 +88,10 @@ bool LunarGravityField::ReadCoefficientsGrgm1200a(std::string file_name) { void LunarGravityField::Update(const LocalEnvironment &local_environment, const Dynamics &dynamics) { const CelestialInformation global_celestial_information = local_environment.GetCelestialInformation().GetGlobalInformation(); - math::Matrix<3, 3> dcm_mci2mcmf_ = global_celestial_information.GetMoonRotation().GetDcmJ2000ToMcmf(); + s2e::math::Matrix<3, 3> dcm_mci2mcmf_ = global_celestial_information.GetMoonRotation().GetDcmJ2000ToMcmf(); - math::Vector<3> spacecraft_position_mci_m = dynamics.GetOrbit().GetPosition_i_m(); - math::Vector<3> spacecraft_position_mcmf_m = dcm_mci2mcmf_ * spacecraft_position_mci_m; + s2e::math::Vector<3> spacecraft_position_mci_m = dynamics.GetOrbit().GetPosition_i_m(); + s2e::math::Vector<3> spacecraft_position_mcmf_m = dcm_mci2mcmf_ * spacecraft_position_mci_m; #ifdef DEBUG_LUNAR_GRAVITY_FIELD std::chrono::system_clock::time_point start, end; @@ -107,7 +107,7 @@ void LunarGravityField::Update(const LocalEnvironment &local_environment, const UNUSED(time_ms_); #endif - math::Matrix<3, 3> dcm_mcmf2i = dcm_mci2mcmf_.Transpose(); + s2e::math::Matrix<3, 3> dcm_mcmf2i = dcm_mci2mcmf_.Transpose(); acceleration_i_m_s2_ = dcm_mcmf2i * acceleration_mcmf_m_s2_; } diff --git a/src/disturbances/lunar_gravity_field.hpp b/src/disturbances/lunar_gravity_field.hpp index f1fa27c57..0a77b9c4b 100644 --- a/src/disturbances/lunar_gravity_field.hpp +++ b/src/disturbances/lunar_gravity_field.hpp @@ -71,7 +71,7 @@ class LunarGravityField : public Disturbance { Vector<3> acceleration_mcmf_m_s2_; //!< Calculated acceleration in the MCMF(Moon Centered Moon Fixed) frame [m/s2] // debug - math::Vector<3> debug_pos_mcmf_m_; //!< Spacecraft position in MCMF frame [m] + s2e::math::Vector<3> debug_pos_mcmf_m_; //!< Spacecraft position in MCMF frame [m] double time_ms_ = 0.0; //!< Calculation time [ms] /** diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index 901777c6a..9af45805c 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -31,8 +31,8 @@ void MagneticDisturbance::Update(const LocalEnvironment& local_environment, cons } void MagneticDisturbance::CalcRMM() { - static math::Vector<3> random_walk_std_dev(residual_magnetic_moment_.GetRandomWalkStandardDeviation_Am2()); - static math::Vector<3> random_walk_limit(residual_magnetic_moment_.GetRandomWalkLimit_Am2()); + static s2e::math::Vector<3> random_walk_std_dev(residual_magnetic_moment_.GetRandomWalkStandardDeviation_Am2()); + static s2e::math::Vector<3> random_walk_limit(residual_magnetic_moment_.GetRandomWalkLimit_Am2()); static RandomWalk<3> random_walk(0.1, random_walk_std_dev, random_walk_limit); // [FIXME] step width is constant static randomization::NormalRand normal_random(0.0, residual_magnetic_moment_.GetRandomNoiseStandardDeviation_Am2(), global_randomization.MakeSeed()); diff --git a/src/disturbances/magnetic_disturbance.hpp b/src/disturbances/magnetic_disturbance.hpp index 80aea05e3..e3be84928 100644 --- a/src/disturbances/magnetic_disturbance.hpp +++ b/src/disturbances/magnetic_disturbance.hpp @@ -50,7 +50,7 @@ class MagneticDisturbance : public Disturbance { private: const double kMagUnit_ = 1.0e-9; //!< Constant value to change the unit [nT] -> [T] - math::Vector<3> rmm_b_Am2_; //!< True RMM of the spacecraft in the body frame [Am2] + s2e::math::Vector<3> rmm_b_Am2_; //!< True RMM of the spacecraft in the body frame [Am2] const ResidualMagneticMoment& residual_magnetic_moment_; //!< RMM parameters /** @@ -64,7 +64,7 @@ class MagneticDisturbance : public Disturbance { * @param [in] magnetic_field_b_nT: Magnetic field vector at the body frame [nT] * @return Calculated disturbance torque in body frame [Nm] */ - math::Vector<3> CalcTorque_b_Nm(const math::Vector<3>& magnetic_field_b_nT); + s2e::math::Vector<3> CalcTorque_b_Nm(const s2e::math::Vector<3>& magnetic_field_b_nT); }; /** diff --git a/src/disturbances/solar_radiation_pressure_disturbance.cpp b/src/disturbances/solar_radiation_pressure_disturbance.cpp index b07d2a858..ed6386234 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.cpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.cpp @@ -11,17 +11,17 @@ #include "../logger/log_utility.hpp" SolarRadiationPressureDisturbance::SolarRadiationPressureDisturbance(const std::vector& surfaces, - const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) + const s2e::math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled) {} void SolarRadiationPressureDisturbance::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { UNUSED(dynamics); - math::Vector<3> sun_position_from_sc_b_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_b_m("SUN"); + s2e::math::Vector<3> sun_position_from_sc_b_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_b_m("SUN"); CalcTorqueForce(sun_position_from_sc_b_m, local_environment.GetSolarRadiationPressure().GetPressure_N_m2()); } -void SolarRadiationPressureDisturbance::CalcCoefficients(const math::Vector<3>& input_direction_b, const double item) { +void SolarRadiationPressureDisturbance::CalcCoefficients(const s2e::math::Vector<3>& input_direction_b, const double item) { UNUSED(input_direction_b); for (size_t i = 0; i < surfaces_.size(); i++) { // Calculate for each surface diff --git a/src/disturbances/solar_radiation_pressure_disturbance.hpp b/src/disturbances/solar_radiation_pressure_disturbance.hpp index 7ec4e9bbc..dc4f08419 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.hpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.hpp @@ -25,7 +25,7 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { * @param [in] center_of_gravity_b_m: Center of gravity position at the body frame [m] * @param [in] is_calculation_enabled: Calculation flag */ - SolarRadiationPressureDisturbance(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, + SolarRadiationPressureDisturbance(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled = true); /** @@ -55,7 +55,7 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { * @param [in] input_direction_b: Direction vector of the sun at the body frame * @param [in] item: Solar pressure [N/m^2] */ - void CalcCoefficients(const math::Vector<3>& input_direction_b, const double item); + void CalcCoefficients(const s2e::math::Vector<3>& input_direction_b, const double item); }; /** diff --git a/src/disturbances/surface_force.cpp b/src/disturbances/surface_force.cpp index 348944267..158e2a695 100644 --- a/src/disturbances/surface_force.cpp +++ b/src/disturbances/surface_force.cpp @@ -7,7 +7,7 @@ #include "../math_physics/math/vector.hpp" -SurfaceForce::SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) +SurfaceForce::SurfaceForce(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, true), surfaces_(surfaces), center_of_gravity_b_m_(center_of_gravity_b_m) { // Initialize vectors size_t num = surfaces_.size(); @@ -17,23 +17,23 @@ SurfaceForce::SurfaceForce(const std::vector& surfaces, const math::Vec sin_theta_.assign(num, 0.0); } -math::Vector<3> SurfaceForce::CalcTorqueForce(math::Vector<3>& input_direction_b, double item) { +s2e::math::Vector<3> SurfaceForce::CalcTorqueForce(s2e::math::Vector<3>& input_direction_b, double item) { CalcTheta(input_direction_b); CalcCoefficients(input_direction_b, item); - math::Vector<3> force_b_N(0.0); - math::Vector<3> torque_b_Nm(0.0); - math::Vector<3> input_b_normal = input_direction_b.CalcNormalizedVector(); + s2e::math::Vector<3> force_b_N(0.0); + s2e::math::Vector<3> torque_b_Nm(0.0); + s2e::math::Vector<3> input_b_normal = input_direction_b.CalcNormalizedVector(); for (size_t i = 0; i < surfaces_.size(); i++) { if (cos_theta_[i] > 0.0) { // if the surface faces to the disturbance source (sun or air) // calc direction of in-plane force - math::Vector<3> normal = surfaces_[i].GetNormal_b(); - math::Vector<3> ncu = OuterProduct(input_b_normal, normal); - math::Vector<3> ncu_normalized = ncu.CalcNormalizedVector(); - math::Vector<3> in_plane_force_direction = OuterProduct(ncu_normalized, normal); + s2e::math::Vector<3> normal = surfaces_[i].GetNormal_b(); + s2e::math::Vector<3> ncu = OuterProduct(input_b_normal, normal); + s2e::math::Vector<3> ncu_normalized = ncu.CalcNormalizedVector(); + s2e::math::Vector<3> in_plane_force_direction = OuterProduct(ncu_normalized, normal); // calc force - math::Vector<3> force_per_surface_b_N = -1.0 * normal_coefficients_[i] * normal + tangential_coefficients_[i] * in_plane_force_direction; + s2e::math::Vector<3> force_per_surface_b_N = -1.0 * normal_coefficients_[i] * normal + tangential_coefficients_[i] * in_plane_force_direction; force_b_N += force_per_surface_b_N; // calc torque torque_b_Nm += OuterProduct(surfaces_[i].GetPosition_b_m() - center_of_gravity_b_m_, force_per_surface_b_N); @@ -44,8 +44,8 @@ math::Vector<3> SurfaceForce::CalcTorqueForce(math::Vector<3>& input_direction_b return torque_b_Nm_; } -void SurfaceForce::CalcTheta(math::Vector<3>& input_direction_b) { - math::Vector<3> input_b_normal = input_direction_b.CalcNormalizedVector(); +void SurfaceForce::CalcTheta(s2e::math::Vector<3>& input_direction_b) { + s2e::math::Vector<3> input_b_normal = input_direction_b.CalcNormalizedVector(); for (size_t i = 0; i < surfaces_.size(); i++) { cos_theta_[i] = InnerProduct(surfaces_[i].GetNormal_b(), input_b_normal); diff --git a/src/disturbances/surface_force.hpp b/src/disturbances/surface_force.hpp index 2fa4a0ac3..2fa6fa7d3 100644 --- a/src/disturbances/surface_force.hpp +++ b/src/disturbances/surface_force.hpp @@ -28,7 +28,7 @@ class SurfaceForce : public Disturbance { * @param [in] center_of_gravity_b_m: Center of gravity position at the body frame [m] * @param [in] is_calculation_enabled: Calculation flag */ - SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled = true); + SurfaceForce(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled = true); /** * @fn ~SurfaceForce * @brief Destructor @@ -38,7 +38,7 @@ class SurfaceForce : public Disturbance { protected: // Spacecraft Structure parameters const std::vector& surfaces_; //!< List of surfaces - const math::Vector<3>& center_of_gravity_b_m_; //!< Position vector of the center of mass_kg at body frame [m] + const s2e::math::Vector<3>& center_of_gravity_b_m_; //!< Position vector of the center of mass_kg at body frame [m] // Internal calculated variables std::vector normal_coefficients_; //!< coefficients for out-plane force for each surface @@ -54,13 +54,13 @@ class SurfaceForce : public Disturbance { * @param [in] item: Parameter which decide the magnitude of the disturbances (e.g., Solar flux, air density) * @return Calculated disturbance torque in body frame [Nm] */ - math::Vector<3> CalcTorqueForce(math::Vector<3>& input_direction_b, double item); + s2e::math::Vector<3> CalcTorqueForce(s2e::math::Vector<3>& input_direction_b, double item); /** * @fn CalcTheta * @brief Calculate cosX and sinX * @param [in] input_direction_b: Direction of disturbance source at the body frame */ - void CalcTheta(math::Vector<3>& input_direction_b); + void CalcTheta(s2e::math::Vector<3>& input_direction_b); /** * @fn CalcCoefficients @@ -68,7 +68,7 @@ class SurfaceForce : public Disturbance { * @param [in] input_direction_b: Direction of disturbance source at the body frame * @param [in] item: Parameter which decide the magnitude of the disturbances (e.g., Solar flux, air density) */ - virtual void CalcCoefficients(const math::Vector<3>& input_direction_b, const double item) = 0; + virtual void CalcCoefficients(const s2e::math::Vector<3>& input_direction_b, const double item) = 0; }; #endif // S2E_DISTURBANCES_SURFACE_FORCE_HPP_ diff --git a/src/disturbances/third_body_gravity.cpp b/src/disturbances/third_body_gravity.cpp index 72bb2d24f..075415ecf 100644 --- a/src/disturbances/third_body_gravity.cpp +++ b/src/disturbances/third_body_gravity.cpp @@ -9,18 +9,18 @@ ThirdBodyGravity::ThirdBodyGravity(std::set third_body_list, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, false), third_body_list_(third_body_list) { - acceleration_i_m_s2_ = math::Vector<3>(0.0); + acceleration_i_m_s2_ = s2e::math::Vector<3>(0.0); } ThirdBodyGravity::~ThirdBodyGravity() {} void ThirdBodyGravity::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { - acceleration_i_m_s2_ = math::Vector<3>(0.0); // initialize + acceleration_i_m_s2_ = s2e::math::Vector<3>(0.0); // initialize - math::Vector<3> sc_position_i_m = dynamics.GetOrbit().GetPosition_i_m(); + s2e::math::Vector<3> sc_position_i_m = dynamics.GetOrbit().GetPosition_i_m(); for (auto third_body : third_body_list_) { - math::Vector<3> third_body_position_from_sc_i_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_i_m(third_body.c_str()); - math::Vector<3> third_body_pos_i_m = sc_position_i_m + third_body_position_from_sc_i_m; + s2e::math::Vector<3> third_body_position_from_sc_i_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_i_m(third_body.c_str()); + s2e::math::Vector<3> third_body_pos_i_m = sc_position_i_m + third_body_position_from_sc_i_m; double gravity_constant = local_environment.GetCelestialInformation().GetGlobalInformation().GetGravityConstant_m3_s2(third_body.c_str()); third_body_acceleration_i_m_s2_ = CalcAcceleration_i_m_s2(third_body_pos_i_m, third_body_position_from_sc_i_m, gravity_constant); @@ -28,8 +28,8 @@ void ThirdBodyGravity::Update(const LocalEnvironment& local_environment, const D } } -math::Vector<3> ThirdBodyGravity::CalcAcceleration_i_m_s2(const math::Vector<3> s, const math::Vector<3> sr, const double gravity_constant_m_s2) { - math::Vector<3> acceleration_i_m_s2; +s2e::math::Vector<3> ThirdBodyGravity::CalcAcceleration_i_m_s2(const s2e::math::Vector<3> s, const s2e::math::Vector<3> sr, const double gravity_constant_m_s2) { + s2e::math::Vector<3> acceleration_i_m_s2; double s_norm = s.CalcNorm(); double s_norm3 = s_norm * s_norm * s_norm; diff --git a/src/disturbances/third_body_gravity.hpp b/src/disturbances/third_body_gravity.hpp index 12b9f8b68..36fded28d 100644 --- a/src/disturbances/third_body_gravity.hpp +++ b/src/disturbances/third_body_gravity.hpp @@ -43,7 +43,7 @@ class ThirdBodyGravity : public Disturbance { private: std::set third_body_list_; //!< List of celestial bodies to calculate the third body disturbances - math::Vector<3> third_body_acceleration_i_m_s2_{0.0}; //!< Calculated third body disturbance acceleration in the inertial frame [m/s2] + s2e::math::Vector<3> third_body_acceleration_i_m_s2_{0.0}; //!< Calculated third body disturbance acceleration in the inertial frame [m/s2] // Override classes for ILoggable /** @@ -65,7 +65,7 @@ class ThirdBodyGravity : public Disturbance { * @param [in] GM: The gravitational constants of the third celestial body [m3/s2] * @return Third body disturbance acceleration in the inertial frame in unit [m/s2] */ - math::Vector<3> CalcAcceleration_i_m_s2(const math::Vector<3> s, const math::Vector<3> sr, const double gravity_constant_m_s2); + s2e::math::Vector<3> CalcAcceleration_i_m_s2(const s2e::math::Vector<3> s, const s2e::math::Vector<3> sr, const double gravity_constant_m_s2); }; /** diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index 461144660..35cc6b237 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -6,15 +6,15 @@ #include -Attitude::Attitude(const math::Matrix<3, 3>& inertia_tensor_kgm2, const std::string& simulation_object_name) +Attitude::Attitude(const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const std::string& simulation_object_name) : SimulationObject(simulation_object_name), inertia_tensor_kgm2_(inertia_tensor_kgm2) { - angular_velocity_b_rad_s_ = math::Vector<3>(0.0); - quaternion_i2b_ = math::Quaternion(0.0, 0.0, 0.0, 1.0); - torque_b_Nm_ = math::Vector<3>(0.0); - angular_momentum_spacecraft_b_Nms_ = math::Vector<3>(0.0); - angular_momentum_reaction_wheel_b_Nms_ = math::Vector<3>(0.0); - angular_momentum_total_b_Nms_ = math::Vector<3>(0.0); - angular_momentum_total_i_Nms_ = math::Vector<3>(0.0); + angular_velocity_b_rad_s_ = s2e::math::Vector<3>(0.0); + quaternion_i2b_ = s2e::math::Quaternion(0.0, 0.0, 0.0, 1.0); + torque_b_Nm_ = s2e::math::Vector<3>(0.0); + angular_momentum_spacecraft_b_Nms_ = s2e::math::Vector<3>(0.0); + angular_momentum_reaction_wheel_b_Nms_ = s2e::math::Vector<3>(0.0); + angular_momentum_total_b_Nms_ = s2e::math::Vector<3>(0.0); + angular_momentum_total_i_Nms_ = s2e::math::Vector<3>(0.0); angular_momentum_total_Nms_ = 0.0; kinetic_energy_J_ = 0.0; } @@ -50,15 +50,15 @@ void Attitude::SetParameters(const MonteCarloSimulationExecutor& mc_simulator) { void Attitude::CalcAngularMomentum(void) { angular_momentum_spacecraft_b_Nms_ = inertia_tensor_kgm2_ * angular_velocity_b_rad_s_; angular_momentum_total_b_Nms_ = angular_momentum_reaction_wheel_b_Nms_ + angular_momentum_spacecraft_b_Nms_; - math::Quaternion q_b2i = quaternion_i2b_.Conjugate(); + s2e::math::Quaternion q_b2i = quaternion_i2b_.Conjugate(); angular_momentum_total_i_Nms_ = q_b2i.FrameConversion(angular_momentum_total_b_Nms_); angular_momentum_total_Nms_ = angular_momentum_total_i_Nms_.CalcNorm(); - kinetic_energy_J_ = 0.5 * math::InnerProduct(angular_momentum_spacecraft_b_Nms_, angular_velocity_b_rad_s_); + kinetic_energy_J_ = 0.5 * s2e::math::InnerProduct(angular_momentum_spacecraft_b_Nms_, angular_velocity_b_rad_s_); } -math::Matrix<4, 4> CalcAngularVelocityMatrix(math::Vector<3> angular_velocity_b_rad_s) { - math::Matrix<4, 4> angular_velocity_matrix; +s2e::math::Matrix<4, 4> CalcAngularVelocityMatrix(s2e::math::Vector<3> angular_velocity_b_rad_s) { + s2e::math::Matrix<4, 4> angular_velocity_matrix; angular_velocity_matrix[0][0] = 0.0f; angular_velocity_matrix[0][1] = angular_velocity_b_rad_s[2]; diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index 130040b88..3485444d9 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -23,7 +23,7 @@ class Attitude : public ILoggable, public SimulationObject { * @brief Constructor * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - Attitude(const math::Matrix<3, 3>& inertia_tensor_kgm2, const std::string& simulation_object_name = "attitude"); + Attitude(const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const std::string& simulation_object_name = "attitude"); /** * @fn ~Attitude * @brief Destructor @@ -40,12 +40,12 @@ class Attitude : public ILoggable, public SimulationObject { * @fn GetAngularVelocity_b_rad_s * @brief Return angular velocity of spacecraft body-fixed frame with respect to the inertial frame [rad/s] */ - inline math::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; } + inline s2e::math::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; } /** * @fn GetQuaternion_i2b * @brief Return attitude quaternion from the inertial frame to the body fixed frame */ - inline math::Quaternion GetQuaternion_i2b() const { return quaternion_i2b_; } + inline s2e::math::Quaternion GetQuaternion_i2b() const { return quaternion_i2b_; } /** * @fn GetTotalAngularMomentNorm_Nms * @brief Return norm of total angular momentum of the spacecraft [Nms] @@ -60,34 +60,34 @@ class Attitude : public ILoggable, public SimulationObject { * @fn GetInertiaTensor_b_kgm2 * @brief Return inertia tensor [kg m^2] */ - inline math::Matrix<3, 3> GetInertiaTensor_b_kgm2() const { return inertia_tensor_kgm2_; } + inline s2e::math::Matrix<3, 3> GetInertiaTensor_b_kgm2() const { return inertia_tensor_kgm2_; } // Setter /** * @fn SetAngularVelocity_b_rad_s * @brief Set angular velocity of the body fixed frame with respect to the inertial frame [rad/s] */ - inline void SetAngularVelocity_b_rad_s(const math::Vector<3> angular_velocity_b_rad_s) { angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; } + inline void SetAngularVelocity_b_rad_s(const s2e::math::Vector<3> angular_velocity_b_rad_s) { angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; } /** * @fn SetQuaternion_i2b * @brief Set attitude quaternion from the inertial frame to the body frame */ - inline void SetQuaternion_i2b(const math::Quaternion quaternion_i2b) { quaternion_i2b_ = quaternion_i2b; } + inline void SetQuaternion_i2b(const s2e::math::Quaternion quaternion_i2b) { quaternion_i2b_ = quaternion_i2b; } /** * @fn SetTorque_b_Nm * @brief Set torque acting on the spacecraft on the body fixed frame [Nm] */ - inline void SetTorque_b_Nm(const math::Vector<3> torque_b_Nm) { torque_b_Nm_ = torque_b_Nm; } + inline void SetTorque_b_Nm(const s2e::math::Vector<3> torque_b_Nm) { torque_b_Nm_ = torque_b_Nm; } /** * @fn AddTorque_b_Nm * @brief Add torque acting on the spacecraft on the body fixed frame [Nm] */ - inline void AddTorque_b_Nm(const math::Vector<3> torque_b_Nm) { torque_b_Nm_ += torque_b_Nm; } + inline void AddTorque_b_Nm(const s2e::math::Vector<3> torque_b_Nm) { torque_b_Nm_ += torque_b_Nm; } /** * @fn SetRwAngularMomentum_b_Nms * @brief Set angular momentum of reaction wheel in the body fixed frame [Nms] */ - inline void SetRwAngularMomentum_b_Nms(const math::Vector<3> angular_momentum_rw_b_Nms) { + inline void SetRwAngularMomentum_b_Nms(const s2e::math::Vector<3> angular_momentum_rw_b_Nms) { angular_momentum_reaction_wheel_b_Nms_ = angular_momentum_rw_b_Nms; } @@ -116,15 +116,15 @@ class Attitude : public ILoggable, public SimulationObject { protected: bool is_calc_enabled_ = true; //!< Calculation flag double propagation_step_s_; //!< Propagation step [sec] - math::Vector<3> angular_velocity_b_rad_s_; //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s] - math::Quaternion quaternion_i2b_; //!< Attitude quaternion from the inertial frame to the body fixed frame - math::Vector<3> torque_b_Nm_; //!< Torque in the body fixed frame [Nm] - const math::Matrix<3, 3>& inertia_tensor_kgm2_; //!< Inertia tensor of the spacecraft [kg m^2] + s2e::math::Vector<3> angular_velocity_b_rad_s_; //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s] + s2e::math::Quaternion quaternion_i2b_; //!< Attitude quaternion from the inertial frame to the body fixed frame + s2e::math::Vector<3> torque_b_Nm_; //!< Torque in the body fixed frame [Nm] + const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2_; //!< Inertia tensor of the spacecraft [kg m^2] - math::Vector<3> angular_momentum_spacecraft_b_Nms_; //!< Angular momentum of spacecraft in the body fixed frame [Nms] - math::Vector<3> angular_momentum_reaction_wheel_b_Nms_; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] - math::Vector<3> angular_momentum_total_b_Nms_; //!< Total angular momentum of spacecraft in the body fixed frame [Nms] - math::Vector<3> angular_momentum_total_i_Nms_; //!< Total angular momentum of spacecraft in the inertial frame [Nms] + s2e::math::Vector<3> angular_momentum_spacecraft_b_Nms_; //!< Angular momentum of spacecraft in the body fixed frame [Nms] + s2e::math::Vector<3> angular_momentum_reaction_wheel_b_Nms_; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] + s2e::math::Vector<3> angular_momentum_total_b_Nms_; //!< Total angular momentum of spacecraft in the body fixed frame [Nms] + s2e::math::Vector<3> angular_momentum_total_i_Nms_; //!< Total angular momentum of spacecraft in the inertial frame [Nms] double angular_momentum_total_Nms_; //!< Norm of total angular momentum [Nms] double kinetic_energy_J_; //!< Rotational Kinetic Energy of Spacecraft [J] @@ -140,6 +140,6 @@ class Attitude : public ILoggable, public SimulationObject { * @brief Generate angular velocity matrix for kinematics calculation * @param [in] angular_velocity_b_rad_s: Angular velocity [rad/s] */ -math::Matrix<4, 4> CalcAngularVelocityMatrix(math::Vector<3> angular_velocity_b_rad_s); +s2e::math::Matrix<4, 4> CalcAngularVelocityMatrix(s2e::math::Vector<3> angular_velocity_b_rad_s); #endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_HPP_ diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index 5ff72c44e..29e130fef 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -9,8 +9,8 @@ #include #include -AttitudeRk4::AttitudeRk4(const math::Vector<3>& angular_velocity_b_rad_s, const math::Quaternion& quaternion_i2b, - const math::Matrix<3, 3>& inertia_tensor_kgm2, const math::Vector<3>& torque_b_Nm, const double propagation_step_s, +AttitudeRk4::AttitudeRk4(const s2e::math::Vector<3>& angular_velocity_b_rad_s, const s2e::math::Quaternion& quaternion_i2b, + const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const s2e::math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name) : Attitude(inertia_tensor_kgm2, simulation_object_name) { angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; @@ -18,7 +18,7 @@ AttitudeRk4::AttitudeRk4(const math::Vector<3>& angular_velocity_b_rad_s, const torque_b_Nm_ = torque_b_Nm; propagation_step_s_ = propagation_step_s; current_propagation_time_s_ = 0.0; - angular_momentum_reaction_wheel_b_Nms_ = math::Vector<3>(0.0); + angular_momentum_reaction_wheel_b_Nms_ = s2e::math::Vector<3>(0.0); previous_inertia_tensor_kgm2_ = inertia_tensor_kgm2_; inverse_inertia_tensor_ = CalcInverseMatrix(inertia_tensor_kgm2_); CalcAngularMomentum(); @@ -32,14 +32,14 @@ void AttitudeRk4::SetParameters(const MonteCarloSimulationExecutor& mc_simulator // TODO: Consider the following calculation is needed here? current_propagation_time_s_ = 0.0; - angular_momentum_reaction_wheel_b_Nms_ = math::Vector<3>(0.0); //!< Consider how to handle this variable + angular_momentum_reaction_wheel_b_Nms_ = s2e::math::Vector<3>(0.0); //!< Consider how to handle this variable CalcAngularMomentum(); } void AttitudeRk4::Propagate(const double end_time_s) { if (!is_calc_enabled_) return; - math::Matrix<3, 3> dot_inertia_tensor = (1.0 / (end_time_s - current_propagation_time_s_)) * (inertia_tensor_kgm2_ - previous_inertia_tensor_kgm2_); + s2e::math::Matrix<3, 3> dot_inertia_tensor = (1.0 / (end_time_s - current_propagation_time_s_)) * (inertia_tensor_kgm2_ - previous_inertia_tensor_kgm2_); torque_inertia_tensor_change_b_Nm_ = dot_inertia_tensor * angular_velocity_b_rad_s_; inverse_inertia_tensor_ = CalcInverseMatrix(inertia_tensor_kgm2_); @@ -55,29 +55,29 @@ void AttitudeRk4::Propagate(const double end_time_s) { CalcAngularMomentum(); } -math::Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(math::Vector<7> x, double t) { +s2e::math::Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(s2e::math::Vector<7> x, double t) { UNUSED(t); - math::Vector<7> dxdt; + s2e::math::Vector<7> dxdt; - math::Vector<3> omega_b; + s2e::math::Vector<3> omega_b; for (int i = 0; i < 3; i++) { omega_b[i] = x[i]; } - math::Vector<3> angular_momentum_total_b_Nms = (previous_inertia_tensor_kgm2_ * omega_b) + angular_momentum_reaction_wheel_b_Nms_; - math::Vector<3> rhs = - inverse_inertia_tensor_ * (torque_b_Nm_ - math::OuterProduct(omega_b, angular_momentum_total_b_Nms) - torque_inertia_tensor_change_b_Nm_); + s2e::math::Vector<3> angular_momentum_total_b_Nms = (previous_inertia_tensor_kgm2_ * omega_b) + angular_momentum_reaction_wheel_b_Nms_; + s2e::math::Vector<3> rhs = + inverse_inertia_tensor_ * (torque_b_Nm_ - s2e::math::OuterProduct(omega_b, angular_momentum_total_b_Nms) - torque_inertia_tensor_change_b_Nm_); for (int i = 0; i < 3; ++i) { dxdt[i] = rhs[i]; } - math::Vector<4> quaternion_i2b; + s2e::math::Vector<4> quaternion_i2b; for (int i = 0; i < 4; i++) { quaternion_i2b[i] = x[i + 3]; } - math::Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b) * quaternion_i2b; + s2e::math::Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b) * quaternion_i2b; for (int i = 0; i < 4; i++) { dxdt[i + 3] = d_quaternion[i]; @@ -87,7 +87,7 @@ math::Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(math::Vector<7> x, do } void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { - math::Vector<7> x; + s2e::math::Vector<7> x; for (int i = 0; i < 3; i++) { x[i] = angular_velocity_b_rad_s_[i]; } @@ -95,8 +95,8 @@ void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { x[i + 3] = quaternion_i2b_[i]; } - math::Vector<7> k1, k2, k3, k4; - math::Vector<7> xk2, xk3, xk4; + s2e::math::Vector<7> k1, k2, k3, k4; + s2e::math::Vector<7> xk2, xk3, xk4; k1 = AttitudeDynamicsAndKinematics(x, t); xk2 = x + (dt / 2.0) * k1; @@ -109,7 +109,7 @@ void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { k4 = AttitudeDynamicsAndKinematics(xk4, (t + dt)); - math::Vector<7> next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4); + s2e::math::Vector<7> next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4); for (int i = 0; i < 3; i++) { angular_velocity_b_rad_s_[i] = next_x[i]; diff --git a/src/dynamics/attitude/attitude_rk4.hpp b/src/dynamics/attitude/attitude_rk4.hpp index 5fe31d669..003989937 100644 --- a/src/dynamics/attitude/attitude_rk4.hpp +++ b/src/dynamics/attitude/attitude_rk4.hpp @@ -24,8 +24,8 @@ class AttitudeRk4 : public Attitude { * @param [in] propagation_step_s: Initial value of propagation step width [sec] * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - AttitudeRk4(const math::Vector<3>& angular_velocity_b_rad_s, const math::Quaternion& quaternion_i2b, const math::Matrix<3, 3>& inertia_tensor_kgm2, - const math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name = "attitude"); + AttitudeRk4(const s2e::math::Vector<3>& angular_velocity_b_rad_s, const s2e::math::Quaternion& quaternion_i2b, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, + const s2e::math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name = "attitude"); /** * @fn ~AttitudeRk4 * @brief Destructor @@ -48,9 +48,9 @@ class AttitudeRk4 : public Attitude { private: double current_propagation_time_s_; //!< current time [sec] - math::Matrix<3, 3> inverse_inertia_tensor_; //!< Inverse of inertia tensor - math::Matrix<3, 3> previous_inertia_tensor_kgm2_; //!< Previous inertia tensor [kgm2] - math::Vector<3> torque_inertia_tensor_change_b_Nm_; //!< Torque generated by inertia tensor change [Nm] + s2e::math::Matrix<3, 3> inverse_inertia_tensor_; //!< Inverse of inertia tensor + s2e::math::Matrix<3, 3> previous_inertia_tensor_kgm2_; //!< Previous inertia tensor [kgm2] + s2e::math::Vector<3> torque_inertia_tensor_change_b_Nm_; //!< Torque generated by inertia tensor change [Nm] /** * @fn AttitudeDynamicsAndKinematics @@ -58,7 +58,7 @@ class AttitudeRk4 : public Attitude { * @param [in] x: State vector (angular velocity and quaternion) * @param [in] t: Unused TODO: remove? */ - math::Vector<7> AttitudeDynamicsAndKinematics(math::Vector<7> x, double t); + s2e::math::Vector<7> AttitudeDynamicsAndKinematics(s2e::math::Vector<7> x, double t); /** * @fn RungeKuttaOneStep * @brief Equation for one step of Runge-Kutta method diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp index bb23cec85..47c8cad29 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp @@ -9,9 +9,9 @@ #include AttitudeWithCantileverVibration::AttitudeWithCantileverVibration( - const math::Vector<3>& angular_velocity_b_rad_s, const math::Quaternion& quaternion_i2b, const math::Matrix<3, 3>& inertia_tensor_kgm2, - const math::Matrix<3, 3>& inertia_tensor_cantilever_kgm2, const double damping_ratio_cantilever, - const double intrinsic_angular_velocity_cantilever_rad_s, const math::Vector<3>& torque_b_Nm, const double propagation_step_s, + const s2e::math::Vector<3>& angular_velocity_b_rad_s, const s2e::math::Quaternion& quaternion_i2b, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, + const s2e::math::Matrix<3, 3>& inertia_tensor_cantilever_kgm2, const double damping_ratio_cantilever, + const double intrinsic_angular_velocity_cantilever_rad_s, const s2e::math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name) : Attitude(inertia_tensor_kgm2, simulation_object_name), numerical_integrator_(propagation_step_s, attitude_ode_, @@ -21,7 +21,7 @@ AttitudeWithCantileverVibration::AttitudeWithCantileverVibration( torque_b_Nm_ = torque_b_Nm; propagation_step_s_ = propagation_step_s; current_propagation_time_s_ = 0.0; - angular_momentum_reaction_wheel_b_Nms_ = math::Vector<3>(0.0); + angular_momentum_reaction_wheel_b_Nms_ = s2e::math::Vector<3>(0.0); attitude_ode_.SetInertiaTensorCantilever_kgm2(inertia_tensor_cantilever_kgm2); attitude_ode_.SetPreviousInertiaTensor_kgm2(inertia_tensor_kgm2_); @@ -30,7 +30,7 @@ AttitudeWithCantileverVibration::AttitudeWithCantileverVibration( double spring_coefficient = pow(intrinsic_angular_velocity_cantilever_rad_s, 2.0); attitude_ode_.SetSpringCoefficient(spring_coefficient); attitude_ode_.SetInverseInertiaTensor(CalcInverseMatrix(inertia_tensor_kgm2_)); - math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever = + s2e::math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever = CalcInverseMatrix(inertia_tensor_kgm2_ - inertia_tensor_cantilever_kgm2) * inertia_tensor_kgm2_; attitude_ode_.SetInverseEquivalentInertiaTensorCantilever(inverse_equivalent_inertia_tensor_cantilever); attitude_ode_.SetTorque_b_Nm(torque_b_Nm_); @@ -65,23 +65,23 @@ void AttitudeWithCantileverVibration::SetParameters(const MonteCarloSimulationEx // TODO: Consider the following calculation is needed here? current_propagation_time_s_ = 0.0; - angular_momentum_reaction_wheel_b_Nms_ = math::Vector<3>(0.0); //!< TODO: Consider how to handle this variable + angular_momentum_reaction_wheel_b_Nms_ = s2e::math::Vector<3>(0.0); //!< TODO: Consider how to handle this variable CalcAngularMomentum(); } void AttitudeWithCantileverVibration::Propagate(const double end_time_s) { if (!is_calc_enabled_) return; - math::Matrix<3, 3> previous_inertia_tensor_kgm2 = attitude_ode_.GetPreviousInertiaTensor_kgm2(); + s2e::math::Matrix<3, 3> previous_inertia_tensor_kgm2 = attitude_ode_.GetPreviousInertiaTensor_kgm2(); assert(end_time_s - current_propagation_time_s_ > 1e-6); - math::Matrix<3, 3> dot_inertia_tensor = (1.0 / (end_time_s - current_propagation_time_s_)) * (inertia_tensor_kgm2_ - previous_inertia_tensor_kgm2); - math::Vector<3> torque_inertia_tensor_b_Nm = dot_inertia_tensor * angular_velocity_b_rad_s_; + s2e::math::Matrix<3, 3> dot_inertia_tensor = (1.0 / (end_time_s - current_propagation_time_s_)) * (inertia_tensor_kgm2_ - previous_inertia_tensor_kgm2); + s2e::math::Vector<3> torque_inertia_tensor_b_Nm = dot_inertia_tensor * angular_velocity_b_rad_s_; attitude_ode_.SetTorqueInertiaTensor_b_Nm(torque_inertia_tensor_b_Nm); attitude_ode_.SetInverseInertiaTensor(CalcInverseMatrix(inertia_tensor_kgm2_)); attitude_ode_.SetTorque_b_Nm(torque_b_Nm_); attitude_ode_.SetAngularMomentumReactionWheel_b_Nms(angular_momentum_reaction_wheel_b_Nms_); - math::Vector<13> state = attitude_ode_.SetStateFromPhysicalQuantities(angular_velocity_b_rad_s_, angular_velocity_cantilever_rad_s_, + s2e::math::Vector<13> state = attitude_ode_.SetStateFromPhysicalQuantities(angular_velocity_b_rad_s_, angular_velocity_cantilever_rad_s_, quaternion_i2b_, euler_angular_cantilever_rad_); numerical_integrator_.GetIntegrator()->SetState(propagation_step_s_, state); while (end_time_s - current_propagation_time_s_ - propagation_step_s_ > 1.0e-6) { diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp index 0ab64d619..95226c473 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp @@ -31,10 +31,10 @@ class AttitudeWithCantileverVibration : public Attitude { * @param [in] propagation_step_s: Initial value of propagation step width [sec] * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - AttitudeWithCantileverVibration(const math::Vector<3>& angular_velocity_b_rad_s, const math::Quaternion& quaternion_i2b, - const math::Matrix<3, 3>& inertia_tensor_kgm2, const math::Matrix<3, 3>& inertia_tensor_cantilever_kgm2, + AttitudeWithCantileverVibration(const s2e::math::Vector<3>& angular_velocity_b_rad_s, const s2e::math::Quaternion& quaternion_i2b, + const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const s2e::math::Matrix<3, 3>& inertia_tensor_cantilever_kgm2, const double damping_ratio_cantilever, const double intrinsic_angular_velocity_cantilever_rad_s, - const math::Vector<3>& torque_b_Nm, const double propagation_step_s, + const s2e::math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name = "attitude"); /** * @fn ~AttitudeWithCantileverVibration @@ -70,8 +70,8 @@ class AttitudeWithCantileverVibration : public Attitude { private: double current_propagation_time_s_; //!< current time [sec] - math::Vector<3> angular_velocity_cantilever_rad_s_{0.0}; //!< Angular velocity of the cantilever with respect to the body frame [rad/s] - math::Vector<3> euler_angular_cantilever_rad_{0.0}; //!< Euler angle of the cantilever with respect to the body frame [rad/s] + s2e::math::Vector<3> angular_velocity_cantilever_rad_s_{0.0}; //!< Angular velocity of the cantilever with respect to the body frame [rad/s] + s2e::math::Vector<3> euler_angular_cantilever_rad_{0.0}; //!< Euler angle of the cantilever with respect to the body frame [rad/s] numerical_integration::AttitudeWithCantileverVibrationOde attitude_ode_; numerical_integration::NumericalIntegratorManager<13> numerical_integrator_; diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index ae12dc2e7..982b1b446 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -7,9 +7,9 @@ #include #include -ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const math::Quaternion quaternion_i2b, - const math::Vector<3> main_target_direction_b, const math::Vector<3> sub_target_direction_b, - const math::Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, +ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const s2e::math::Quaternion quaternion_i2b, + const s2e::math::Vector<3> main_target_direction_b, const s2e::math::Vector<3> sub_target_direction_b, + const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, const std::string& simulation_object_name) : Attitude(inertia_tensor_kgm2, simulation_object_name), main_mode_(main_mode), @@ -53,7 +53,7 @@ void ControlledAttitude::Initialize(void) { } void ControlledAttitude::Propagate(const double end_time_s) { - math::Vector<3> main_direction_i, sub_direction_i; + s2e::math::Vector<3> main_direction_i, sub_direction_i; if (!is_calc_enabled_) return; if (main_mode_ == AttitudeControlMode::kInertialStabilize) { @@ -72,28 +72,28 @@ void ControlledAttitude::Propagate(const double end_time_s) { return; } -math::Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMode mode) { - math::Vector<3> direction; +s2e::math::Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMode mode) { + s2e::math::Vector<3> direction; if (mode == AttitudeControlMode::kSunPointing) { direction = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); // When the local_celestial_information is not initialized. FIXME: This is temporary codes for attitude initialize. if (direction.CalcNorm() == 0.0) { - math::Vector<3> sun_position_i_m = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("SUN"); - math::Vector<3> spacecraft_position_i_m = orbit_->GetPosition_i_m(); + s2e::math::Vector<3> sun_position_i_m = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("SUN"); + s2e::math::Vector<3> spacecraft_position_i_m = orbit_->GetPosition_i_m(); direction = sun_position_i_m - spacecraft_position_i_m; } } else if (mode == AttitudeControlMode::kEarthCenterPointing) { direction = local_celestial_information_->GetPositionFromSpacecraft_i_m("EARTH"); // When the local_celestial_information is not initialized. FIXME: This is temporary codes for attitude initialize. if (direction.CalcNorm() == 0.0) { - math::Vector<3> earth_position_i_m = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("EARTH"); - math::Vector<3> spacecraft_position_i_m = orbit_->GetPosition_i_m(); + s2e::math::Vector<3> earth_position_i_m = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("EARTH"); + s2e::math::Vector<3> spacecraft_position_i_m = orbit_->GetPosition_i_m(); direction = earth_position_i_m - spacecraft_position_i_m; } } else if (mode == AttitudeControlMode::kVelocityDirectionPointing) { direction = orbit_->GetVelocity_i_m_s(); } else if (mode == AttitudeControlMode::kGroundSpeedDirectionPointing) { - math::Matrix<3, 3> dcm_ecef2eci = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); + s2e::math::Matrix<3, 3> dcm_ecef2eci = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); direction = dcm_ecef2eci * orbit_->GetVelocity_ecef_m_s(); } else if (mode == AttitudeControlMode::kOrbitNormalPointing) { direction = OuterProduct(orbit_->GetPosition_i_m(), orbit_->GetVelocity_i_m_s()); @@ -102,29 +102,29 @@ math::Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMode mo return direction; } -void ControlledAttitude::PointingControl(const math::Vector<3> main_direction_i, const math::Vector<3> sub_direction_i) { +void ControlledAttitude::PointingControl(const s2e::math::Vector<3> main_direction_i, const s2e::math::Vector<3> sub_direction_i) { // Calc DCM ECI->Target - math::Matrix<3, 3> dcm_t2i = CalcDcm(main_direction_i, sub_direction_i); + s2e::math::Matrix<3, 3> dcm_t2i = CalcDcm(main_direction_i, sub_direction_i); // Calc DCM Target->body - math::Matrix<3, 3> dcm_t2b = CalcDcm(main_target_direction_b_, sub_target_direction_b_); + s2e::math::Matrix<3, 3> dcm_t2b = CalcDcm(main_target_direction_b_, sub_target_direction_b_); // Calc DCM ECI->body - math::Matrix<3, 3> dcm_i2b = dcm_t2b * dcm_t2i.Transpose(); + s2e::math::Matrix<3, 3> dcm_i2b = dcm_t2b * dcm_t2i.Transpose(); // Convert to Quaternion - quaternion_i2b_ = math::Quaternion::ConvertFromDcm(dcm_i2b); + quaternion_i2b_ = s2e::math::Quaternion::ConvertFromDcm(dcm_i2b); } -math::Matrix<3, 3> ControlledAttitude::CalcDcm(const math::Vector<3> main_direction, const math::Vector<3> sub_direction) { +s2e::math::Matrix<3, 3> ControlledAttitude::CalcDcm(const s2e::math::Vector<3> main_direction, const s2e::math::Vector<3> sub_direction) { // Calc basis vectors - math::Vector<3> ex, ey, ez; + s2e::math::Vector<3> ex, ey, ez; ex = main_direction; - math::Vector<3> tmp1 = OuterProduct(ex, sub_direction); - math::Vector<3> tmp2 = OuterProduct(tmp1, ex); + s2e::math::Vector<3> tmp1 = OuterProduct(ex, sub_direction); + s2e::math::Vector<3> tmp2 = OuterProduct(tmp1, ex); ey = tmp2.CalcNormalizedVector(); - math::Vector<3> tmp3 = OuterProduct(ex, ey); + s2e::math::Vector<3> tmp3 = OuterProduct(ex, ey); ez = tmp3.CalcNormalizedVector(); // Generate DCM - math::Matrix<3, 3> dcm; + s2e::math::Matrix<3, 3> dcm; for (int i = 0; i < 3; i++) { dcm[i][0] = ex[i]; dcm[i][1] = ey[i]; @@ -152,24 +152,24 @@ AttitudeControlMode ConvertStringToCtrlMode(const std::string mode) { } void ControlledAttitude::CalcAngularVelocity(const double current_time_s) { - math::Vector<3> controlled_torque_b_Nm(0.0); + s2e::math::Vector<3> controlled_torque_b_Nm(0.0); if (previous_calc_time_s_ > 0.0) { double time_diff_sec = current_time_s - previous_calc_time_s_; - math::Quaternion prev_q_b2i = previous_quaternion_i2b_.Conjugate(); - math::Quaternion q_diff = prev_q_b2i * quaternion_i2b_; + s2e::math::Quaternion prev_q_b2i = previous_quaternion_i2b_.Conjugate(); + s2e::math::Quaternion q_diff = prev_q_b2i * quaternion_i2b_; q_diff = (2.0 / time_diff_sec) * q_diff; - math::Vector<3> angular_acc_b_rad_s2_; + s2e::math::Vector<3> angular_acc_b_rad_s2_; for (int i = 0; i < 3; i++) { angular_velocity_b_rad_s_[i] = q_diff[i]; angular_acc_b_rad_s2_[i] = (previous_omega_b_rad_s_[i] - angular_velocity_b_rad_s_[i]) / time_diff_sec; } - math::Matrix<3, 3> inv_inertia_tensor = CalcInverseMatrix(inertia_tensor_kgm2_); + s2e::math::Matrix<3, 3> inv_inertia_tensor = CalcInverseMatrix(inertia_tensor_kgm2_); controlled_torque_b_Nm = inv_inertia_tensor * angular_acc_b_rad_s2_; } else { - angular_velocity_b_rad_s_ = math::Vector<3>(0.0); - controlled_torque_b_Nm = math::Vector<3>(0.0); + angular_velocity_b_rad_s_ = s2e::math::Vector<3>(0.0); + controlled_torque_b_Nm = s2e::math::Vector<3>(0.0); } // Add torque with disturbances AddTorque_b_Nm(controlled_torque_b_Nm); diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index ce26738d8..88204270b 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -54,9 +54,9 @@ class ControlledAttitude : public Attitude { * @param [in] orbit: Orbit * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const math::Quaternion quaternion_i2b, - const math::Vector<3> main_target_direction_b, const math::Vector<3> sub_target_direction_b, - const math::Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, + ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const s2e::math::Quaternion quaternion_i2b, + const s2e::math::Vector<3> main_target_direction_b, const s2e::math::Vector<3> sub_target_direction_b, + const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, const std::string& simulation_object_name = "attitude"); /** * @fn ~ControlledAttitude @@ -79,17 +79,17 @@ class ControlledAttitude : public Attitude { * @fn SetQuaternion_i2t * @brief Set quaternion for INERTIAL_STABILIZE mode */ - inline void SetQuaternion_i2t(const math::Quaternion quaternion_i2t) { quaternion_i2b_ = quaternion_i2t; } + inline void SetQuaternion_i2t(const s2e::math::Quaternion quaternion_i2t) { quaternion_i2b_ = quaternion_i2t; } /** * @fn SetMainTargetDirection_b * @brief Set main target direction on the body fixed frame */ - inline void SetMainTargetDirection_b(math::Vector<3> main_target_direction_b) { main_target_direction_b_ = main_target_direction_b; } + inline void SetMainTargetDirection_b(s2e::math::Vector<3> main_target_direction_b) { main_target_direction_b_ = main_target_direction_b; } /** * @fn SetSubTargetDirection_b * @brief Set sub target direction on the body fixed frame */ - inline void SetSubTargetDirection_b(math::Vector<3> sub_target_direction_b) { sub_target_direction_b_ = sub_target_direction_b; } + inline void SetSubTargetDirection_b(s2e::math::Vector<3> sub_target_direction_b) { sub_target_direction_b_ = sub_target_direction_b; } /** * @fn Propagate @@ -101,13 +101,13 @@ class ControlledAttitude : public Attitude { private: AttitudeControlMode main_mode_; //!< Main control mode AttitudeControlMode sub_mode_; //!< Sub control mode - math::Vector<3> main_target_direction_b_; //!< Main target direction on the body fixed frame - math::Vector<3> sub_target_direction_b_; //!< Sub target direction on tge body fixed frame + s2e::math::Vector<3> main_target_direction_b_; //!< Main target direction on the body fixed frame + s2e::math::Vector<3> sub_target_direction_b_; //!< Sub target direction on tge body fixed frame double previous_calc_time_s_ = -1.0; //!< Previous time of velocity calculation [sec] - math::Quaternion previous_quaternion_i2b_; //!< Previous quaternion - math::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s] + s2e::math::Quaternion previous_quaternion_i2b_; //!< Previous quaternion + s2e::math::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s] - const double kMinDirectionAngle_rad = 30.0 * math::deg_to_rad; //!< Minimum angle b/w main and sub direction + const double kMinDirectionAngle_rad = 30.0 * s2e::math::deg_to_rad; //!< Minimum angle b/w main and sub direction // TODO Change with ini file // Inputs @@ -126,14 +126,14 @@ class ControlledAttitude : public Attitude { * @param [in] mode: Attitude control mode * @return Target direction at the inertia frame */ - math::Vector<3> CalcTargetDirection_i(AttitudeControlMode mode); + s2e::math::Vector<3> CalcTargetDirection_i(AttitudeControlMode mode); /** * @fn PointingControl * @brief Calculate attitude quaternion * @param [in] main_direction_i: Main target direction in the inertial frame * @param [in] sub_direction_i: Sub target direction in the inertial frame */ - void PointingControl(const math::Vector<3> main_direction_i, const math::Vector<3> sub_direction_i); + void PointingControl(const s2e::math::Vector<3> main_direction_i, const s2e::math::Vector<3> sub_direction_i); /** * @fn CalcAngularVelocity * @brief Calculate angular velocity @@ -146,7 +146,7 @@ class ControlledAttitude : public Attitude { * @param [in] main_direction: Main target direction * @param [in] sub_direction: Sub target direction */ - math::Matrix<3, 3> CalcDcm(const math::Vector<3> main_direction, const math::Vector<3> sub_direction); + s2e::math::Matrix<3, 3> CalcDcm(const s2e::math::Vector<3> main_direction, const s2e::math::Vector<3> sub_direction); }; #endif // S2E_DYNAMICS_ATTITUDE_CONTROLLED_ATTITUDE_HPP_ diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index d4ed6e202..640cf98c6 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -7,7 +7,7 @@ #include Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* local_celestial_information, - const double step_width_s, const math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id) { + const double step_width_s, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id) { IniAccess ini_file(file_name); const char* section_ = "ATTITUDE"; std::string mc_name = "attitude" + std::to_string(spacecraft_id); @@ -16,9 +16,9 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel const std::string propagate_mode = ini_file.ReadString(section_, "propagate_mode"); const std::string initialize_mode = ini_file.ReadString(section_, "initialize_mode"); - math::Vector<3> omega_b; - math::Quaternion quaternion_i2b; - math::Vector<3> torque_b; + s2e::math::Vector<3> omega_b; + s2e::math::Quaternion quaternion_i2b; + s2e::math::Vector<3> torque_b; if (initialize_mode == "CONTROLLED") { // Initialize with Controlled attitude (attitude_tmp temporary used) IniAccess ini_file_ca(file_name); @@ -30,7 +30,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel AttitudeControlMode sub_mode = ConvertStringToCtrlMode(sub_mode_in); ini_file_ca.ReadQuaternion(section_, "initial_quaternion_i2b", quaternion_i2b); - math::Vector<3> main_target_direction_b, sub_target_direction_b; + s2e::math::Vector<3> main_target_direction_b, sub_target_direction_b; ini_file_ca.ReadVector(section_ca_, "main_pointing_direction_b", main_target_direction_b); ini_file_ca.ReadVector(section_ca_, "sub_pointing_direction_b", sub_target_direction_b); std::string mc_name_temp = section_ + std::to_string(spacecraft_id) + "_TEMP"; @@ -38,8 +38,8 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel inertia_tensor_kgm2, local_celestial_information, orbit, mc_name_temp); attitude_temp->Propagate(step_width_s); quaternion_i2b = attitude_temp->GetQuaternion_i2b(); - omega_b = math::Vector<3>(0.0); - torque_b = math::Vector<3>(0.0); + omega_b = s2e::math::Vector<3>(0.0); + torque_b = s2e::math::Vector<3>(0.0); } else { // Including the case: initialize_mode == "MANUAL" ini_file.ReadVector(section_, "initial_angular_velocity_b_rad_s", omega_b); @@ -54,8 +54,8 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel IniAccess ini_structure(ini_structure_name); const char* section_cantilever = "CANTILEVER_PARAMETERS"; - math::Matrix<3, 3> inertia_tensor_cantilever_kgm2; - math::Vector<9> inertia_vec; + s2e::math::Matrix<3, 3> inertia_tensor_cantilever_kgm2; + s2e::math::Vector<9> inertia_vec; ini_structure.ReadVector(section_cantilever, "inertia_tensor_cantilever_kgm2", inertia_vec); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { @@ -78,7 +78,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel AttitudeControlMode main_mode = ConvertStringToCtrlMode(main_mode_in); AttitudeControlMode sub_mode = ConvertStringToCtrlMode(sub_mode_in); ini_file_ca.ReadQuaternion(section_, "initial_quaternion_i2b", quaternion_i2b); - math::Vector<3> main_target_direction_b, sub_target_direction_b; + s2e::math::Vector<3> main_target_direction_b, sub_target_direction_b; ini_file_ca.ReadVector(section_ca_, "main_pointing_direction_b", main_target_direction_b); ini_file_ca.ReadVector(section_ca_, "sub_pointing_direction_b", sub_target_direction_b); diff --git a/src/dynamics/attitude/initialize_attitude.hpp b/src/dynamics/attitude/initialize_attitude.hpp index f9b1bebd4..0e3899c02 100644 --- a/src/dynamics/attitude/initialize_attitude.hpp +++ b/src/dynamics/attitude/initialize_attitude.hpp @@ -22,6 +22,6 @@ * @param [in] spacecraft_id: Satellite ID */ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* local_celestial_information, - const double step_width_s, const math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id); + const double step_width_s, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id); #endif // S2E_DYNAMICS_ATTITUDE_INITIALIZE_ATTITUDE_HPP_ diff --git a/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp index bd2fd8ae5..770a586db 100644 --- a/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp @@ -28,10 +28,10 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame * @param [in] euler_angule_cantilever_rad: Euler angle of the cantilever@ body-fixed frame [rad] */ - math::Vector<13> SetStateFromPhysicalQuantities(const math::Vector<3> angular_velocity_b_rad_s, - const math::Vector<3> angular_velocity_cantilever_rad_s, const math::Quaternion quaternion_i2b, - const math::Vector<3> euler_angule_cantilever_rad) const { - math::Vector<13> state; + s2e::math::Vector<13> SetStateFromPhysicalQuantities(const s2e::math::Vector<3> angular_velocity_b_rad_s, + const s2e::math::Vector<3> angular_velocity_cantilever_rad_s, const s2e::math::Quaternion quaternion_i2b, + const s2e::math::Vector<3> euler_angule_cantilever_rad) const { + s2e::math::Vector<13> state; for (size_t i = 0; i < 3; i++) { state[i] = angular_velocity_b_rad_s[i]; } @@ -52,9 +52,9 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { * @brief Set physical quantities from state acquired by calculation of the ordinary differential equation * @param [in] state: state variables used to calculate the ordinary differential equation */ - void SetPhysicalQuantitiesFromState(const math::Vector<13> state, math::Vector<3>& angular_velocity_b_rad_s, - math::Vector<3>& angular_velocity_cantilever_rad_s, math::Quaternion& quaternion_i2b, - math::Vector<3>& euler_angular_cantilever_rad) const { + void SetPhysicalQuantitiesFromState(const s2e::math::Vector<13> state, s2e::math::Vector<3>& angular_velocity_b_rad_s, + s2e::math::Vector<3>& angular_velocity_cantilever_rad_s, s2e::math::Quaternion& quaternion_i2b, + s2e::math::Vector<3>& euler_angular_cantilever_rad) const { for (size_t i = 0; i < 3; i++) { angular_velocity_b_rad_s[i] = state[i]; } @@ -69,27 +69,27 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { } } - math::Vector<13> DerivativeFunction(const double time_s, const math::Vector<13>& state) const override { + s2e::math::Vector<13> DerivativeFunction(const double time_s, const s2e::math::Vector<13>& state) const override { UNUSED(time_s); - math::Vector<13> output; + s2e::math::Vector<13> output; - math::Vector<3> omega_b_rad_s; - math::Vector<3> omega_cantilever_rad_s; - math::Quaternion quaternion_i2b; - math::Vector<3> euler_angle_cantilever_rad; + s2e::math::Vector<3> omega_b_rad_s; + s2e::math::Vector<3> omega_cantilever_rad_s; + s2e::math::Quaternion quaternion_i2b; + s2e::math::Vector<3> euler_angle_cantilever_rad; SetPhysicalQuantitiesFromState(state, omega_b_rad_s, omega_cantilever_rad_s, quaternion_i2b, euler_angle_cantilever_rad); - math::Vector<3> angular_momentum_total_b_Nms = (previous_inertia_tensor_kgm2_ * omega_b_rad_s) + angular_momentum_reaction_wheel_b_Nms_; - math::Vector<3> net_torque_b_Nm = torque_b_Nm_ - math::OuterProduct(omega_b_rad_s, angular_momentum_total_b_Nms) - torque_inertia_tensor_b_Nm_; + s2e::math::Vector<3> angular_momentum_total_b_Nms = (previous_inertia_tensor_kgm2_ * omega_b_rad_s) + angular_momentum_reaction_wheel_b_Nms_; + s2e::math::Vector<3> net_torque_b_Nm = torque_b_Nm_ - s2e::math::OuterProduct(omega_b_rad_s, angular_momentum_total_b_Nms) - torque_inertia_tensor_b_Nm_; - math::Vector<3> angular_accelaration_cantilever_rad_s2 = + s2e::math::Vector<3> angular_accelaration_cantilever_rad_s2 = -(inverse_equivalent_inertia_tensor_cantilever_ * (attenuation_coefficient_ * omega_cantilever_rad_s + spring_coefficient_ * euler_angle_cantilever_rad)) - inverse_inertia_tensor_ * net_torque_b_Nm; - math::Vector<3> rhs = inverse_inertia_tensor_ * (net_torque_b_Nm - inertia_tensor_cantilever_kgm2_ * angular_accelaration_cantilever_rad_s2); + s2e::math::Vector<3> rhs = inverse_inertia_tensor_ * (net_torque_b_Nm - inertia_tensor_cantilever_kgm2_ * angular_accelaration_cantilever_rad_s2); for (size_t i = 0; i < 3; ++i) { output[i] = rhs[i]; @@ -99,7 +99,7 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { output[i + 3] = angular_accelaration_cantilever_rad_s2[i]; } - math::Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b_rad_s) * (math::Vector<4>(quaternion_i2b)); + s2e::math::Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b_rad_s) * (s2e::math::Vector<4>(quaternion_i2b)); for (size_t i = 0; i < 4; i++) { output[i + 6] = d_quaternion[i]; @@ -110,7 +110,7 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { } // The function is used to output the derivative of each corresponding physical quantity. - output = SetStateFromPhysicalQuantities(rhs, angular_accelaration_cantilever_rad_s2, math::Quaternion(d_quaternion), omega_cantilever_rad_s); + output = SetStateFromPhysicalQuantities(rhs, angular_accelaration_cantilever_rad_s2, s2e::math::Quaternion(d_quaternion), omega_cantilever_rad_s); return output; } @@ -130,27 +130,27 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { * @fn GetTorqueInertiaTensor_b_Nm * @brief Get torque generated by inertia tensor [Nm] */ - inline math::Vector<3> GetTorqueInertiaTensor_b_Nm() { return torque_inertia_tensor_b_Nm_; } + inline s2e::math::Vector<3> GetTorqueInertiaTensor_b_Nm() { return torque_inertia_tensor_b_Nm_; } /** * @fn GetInverseInertiaTensor * @brief Get inverse of inertia tensor */ - inline math::Matrix<3, 3> GetInverseInertiaTensor() { return inverse_inertia_tensor_; } + inline s2e::math::Matrix<3, 3> GetInverseInertiaTensor() { return inverse_inertia_tensor_; } /** * @fn GetPreviousInertiaTensor_kgm2 * @brief Get previous inertia tensor [kgm2] */ - inline math::Matrix<3, 3> GetPreviousInertiaTensor_kgm2() { return previous_inertia_tensor_kgm2_; } + inline s2e::math::Matrix<3, 3> GetPreviousInertiaTensor_kgm2() { return previous_inertia_tensor_kgm2_; } /** * @fn GetInertiaTensorCantilever_kgm2 * @brief Get inertia tensor of the cantilever [kgm2] */ - inline math::Matrix<3, 3> GetInertiaTensorCantilever_kgm2() { return inertia_tensor_cantilever_kgm2_; } + inline s2e::math::Matrix<3, 3> GetInertiaTensorCantilever_kgm2() { return inertia_tensor_cantilever_kgm2_; } /** * @fn GetInverseEquivalentInertiaTensorCantilever * @brief Get inverse of inertia tensor of the cantilever */ - inline math::Matrix<3, 3> GetInverseEquivalentInertiaTensorCantilever() { return inverse_equivalent_inertia_tensor_cantilever_; } + inline s2e::math::Matrix<3, 3> GetInverseEquivalentInertiaTensorCantilever() { return inverse_equivalent_inertia_tensor_cantilever_; } // Setter /** @@ -167,58 +167,58 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { * @fn SetTorque_b_Nm * @brief Set torque acting on the spacecraft on the body fixed frame [Nm] */ - inline void SetTorque_b_Nm(const math::Vector<3> torque_b_Nm) { torque_b_Nm_ = torque_b_Nm; } + inline void SetTorque_b_Nm(const s2e::math::Vector<3> torque_b_Nm) { torque_b_Nm_ = torque_b_Nm; } /** * @fn SetTorqueInertiaTensor_b_Nm * @brief Set torque generated by inertia tensor [Nm] */ - inline void SetTorqueInertiaTensor_b_Nm(const math::Vector<3> torque_inertia_tensor_b_Nm) { + inline void SetTorqueInertiaTensor_b_Nm(const s2e::math::Vector<3> torque_inertia_tensor_b_Nm) { torque_inertia_tensor_b_Nm_ = torque_inertia_tensor_b_Nm; } /** * @fn SetAngularMomentumReactionWheel_b_Nms * @brief Set angular momentum of reaction wheel in the body fixed frame [Nms] */ - inline void SetAngularMomentumReactionWheel_b_Nms(const math::Vector<3> angular_momentum_reaction_wheel_b_Nms) { + inline void SetAngularMomentumReactionWheel_b_Nms(const s2e::math::Vector<3> angular_momentum_reaction_wheel_b_Nms) { angular_momentum_reaction_wheel_b_Nms_ = angular_momentum_reaction_wheel_b_Nms; } /** * @fn SetInverseInertiaTensor * @brief Set inverse of inertia tensor */ - inline void SetInverseInertiaTensor(const math::Matrix<3, 3> inverse_inertia_tensor) { inverse_inertia_tensor_ = inverse_inertia_tensor; } + inline void SetInverseInertiaTensor(const s2e::math::Matrix<3, 3> inverse_inertia_tensor) { inverse_inertia_tensor_ = inverse_inertia_tensor; } /** * @fn SetPreviousInertiaTensor_kgm2 * @brief Set previous inertia tensor [kgm2] */ - inline void SetPreviousInertiaTensor_kgm2(const math::Matrix<3, 3> previous_inertia_tensor_kgm2) { + inline void SetPreviousInertiaTensor_kgm2(const s2e::math::Matrix<3, 3> previous_inertia_tensor_kgm2) { previous_inertia_tensor_kgm2_ = previous_inertia_tensor_kgm2; } /** * @fn SetInertiaTensorCantilever_kgm2 * @brief Set inertia tensor of the cantilever [kgm2] */ - inline void SetInertiaTensorCantilever_kgm2(const math::Matrix<3, 3> inertia_tensor_cantilever_kgm2) { + inline void SetInertiaTensorCantilever_kgm2(const s2e::math::Matrix<3, 3> inertia_tensor_cantilever_kgm2) { inertia_tensor_cantilever_kgm2_ = inertia_tensor_cantilever_kgm2; } /** * @fn SetInverseEquivalentInertiaTensorCantilever * @brief Set inverse of inertia tensor of the cantilever */ - inline void SetInverseEquivalentInertiaTensorCantilever(const math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever) { + inline void SetInverseEquivalentInertiaTensorCantilever(const s2e::math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever) { inverse_equivalent_inertia_tensor_cantilever_ = inverse_equivalent_inertia_tensor_cantilever; } protected: double attenuation_coefficient_ = 0.0; //!< Attenuation coefficient double spring_coefficient_ = 0.0; //!< Spring coefficient - math::Vector<3> torque_b_Nm_{0.0}; //!< Torque in the body fixed frame [Nm] - math::Vector<3> torque_inertia_tensor_b_Nm_{0.0}; //!< Torque generated by inertia tensor [Nm] - math::Vector<3> angular_momentum_reaction_wheel_b_Nms_{0.0}; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] - math::Matrix<3, 3> inverse_inertia_tensor_{0.0}; //!< Inverse of inertia tensor - math::Matrix<3, 3> previous_inertia_tensor_kgm2_{0.0}; //!< Previous inertia tensor [kgm2] - math::Matrix<3, 3> inertia_tensor_cantilever_kgm2_{0.0}; //!< Inertia tensor of the cantilever [kgm2] - math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever_{0.0}; //!< Inverse of inertia tensor of the cantilever + s2e::math::Vector<3> torque_b_Nm_{0.0}; //!< Torque in the body fixed frame [Nm] + s2e::math::Vector<3> torque_inertia_tensor_b_Nm_{0.0}; //!< Torque generated by inertia tensor [Nm] + s2e::math::Vector<3> angular_momentum_reaction_wheel_b_Nms_{0.0}; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] + s2e::math::Matrix<3, 3> inverse_inertia_tensor_{0.0}; //!< Inverse of inertia tensor + s2e::math::Matrix<3, 3> previous_inertia_tensor_kgm2_{0.0}; //!< Previous inertia tensor [kgm2] + s2e::math::Matrix<3, 3> inertia_tensor_cantilever_kgm2_{0.0}; //!< Inertia tensor of the cantilever [kgm2] + s2e::math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever_{0.0}; //!< Inverse of inertia tensor of the cantilever }; } // namespace numerical_integration diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index ef54a9f29..d10a715bd 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -59,7 +59,7 @@ void Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestia } void Dynamics::ClearForceTorque(void) { - math::Vector<3> zero(0.0); + s2e::math::Vector<3> zero(0.0); attitude_->SetTorque_b_Nm(zero); orbit_->SetAcceleration_i_m_s2(zero); } diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index 327cc399a..2db4bfbbd 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -63,13 +63,13 @@ class Dynamics { * @brief Add input torque for the attitude dynamics propagation * @param [in] torque_b_Nm: Torque in the body fixed frame [Nm] */ - inline void AddTorque_b_Nm(math::Vector<3> torque_b_Nm) { attitude_->AddTorque_b_Nm(torque_b_Nm); } + inline void AddTorque_b_Nm(s2e::math::Vector<3> torque_b_Nm) { attitude_->AddTorque_b_Nm(torque_b_Nm); } /** * @fn AddForce_b_N * @brief Add input force for the orbit dynamics propagation * @param [in] force_b_N: Force in the body fixed frame [N] */ - inline void AddForce_b_N(math::Vector<3> force_b_N) { + inline void AddForce_b_N(s2e::math::Vector<3> force_b_N) { orbit_->AddForce_b_N(force_b_N, attitude_->GetQuaternion_i2b(), structure_->GetKinematicsParameters().GetMass_kg()); } /** @@ -77,7 +77,7 @@ class Dynamics { * @brief Add input acceleration for the orbit dynamics propagation * @param [in] acceleration_i_m_s2: Acceleration in the inertial fixed frame [N] */ - inline void AddAcceleration_i_m_s2(math::Vector<3> acceleration_i_m_s2) { orbit_->AddAcceleration_i_m_s2(acceleration_i_m_s2); } + inline void AddAcceleration_i_m_s2(s2e::math::Vector<3> acceleration_i_m_s2) { orbit_->AddAcceleration_i_m_s2(acceleration_i_m_s2); } /** * @fn ClearForceTorque diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index 9b67322f2..ccdbc265d 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -10,10 +10,10 @@ #include "../../math_physics/orbit/orbital_elements.hpp" EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double gravity_constant_m3_s2, - const double propagation_step_s, const double current_time_jd, const math::Vector<3> position_i_m, - const math::Vector<3> velocity_i_m_s, const double error_tolerance) + const double propagation_step_s, const double current_time_jd, const s2e::math::Vector<3> position_i_m, + const s2e::math::Vector<3> velocity_i_m_s, const double error_tolerance) : Orbit(celestial_information), - math::OrdinaryDifferentialEquation<6>(propagation_step_s), + s2e::math::OrdinaryDifferentialEquation<6>(propagation_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2), error_tolerance_(error_tolerance), propagation_step_s_(propagation_step_s) { @@ -60,9 +60,9 @@ void EnckeOrbitPropagation::Propagate(const double end_time_s, const double curr } // Functions for OrdinaryDifferentialEquation -void EnckeOrbitPropagation::DerivativeFunction(double t, const math::Vector<6>& state, math::Vector<6>& rhs) { +void EnckeOrbitPropagation::DerivativeFunction(double t, const s2e::math::Vector<6>& state, s2e::math::Vector<6>& rhs) { UNUSED(t); - math::Vector<3> difference_position_i_m_m, difference_acc_i_m_s2; + s2e::math::Vector<3> difference_position_i_m_m, difference_acc_i_m_s2; for (int i = 0; i < 3; i++) { difference_position_i_m_m[i] = state[i]; } @@ -83,7 +83,7 @@ void EnckeOrbitPropagation::DerivativeFunction(double t, const math::Vector<6>& } // Private Functions -void EnckeOrbitPropagation::Initialize(double current_time_jd, math::Vector<3> reference_position_i_m, math::Vector<3> reference_velocity_i_m_s) { +void EnckeOrbitPropagation::Initialize(double current_time_jd, s2e::math::Vector<3> reference_position_i_m, s2e::math::Vector<3> reference_velocity_i_m_s) { // General spacecraft_acceleration_i_m_s2_.FillUp(0.0); @@ -97,7 +97,7 @@ void EnckeOrbitPropagation::Initialize(double current_time_jd, math::Vector<3> r difference_position_i_m_.FillUp(0.0); difference_velocity_i_m_s_.FillUp(0.0); - math::Vector<6> zero(0.0f); + s2e::math::Vector<6> zero(0.0f); Setup(0.0, zero); UpdateSatOrbit(); @@ -111,11 +111,11 @@ void EnckeOrbitPropagation::UpdateSatOrbit() { TransformEcefToGeodetic(); } -double EnckeOrbitPropagation::CalcQFunction(math::Vector<3> difference_position_i_m) { +double EnckeOrbitPropagation::CalcQFunction(s2e::math::Vector<3> difference_position_i_m) { double r2; r2 = InnerProduct(spacecraft_position_i_m_, spacecraft_position_i_m_); - math::Vector<3> dr_2r; + s2e::math::Vector<3> dr_2r; dr_2r = difference_position_i_m - 2.0 * spacecraft_position_i_m_; double q = InnerProduct(difference_position_i_m, dr_2r) / r2; diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index a32052e2b..e5ad30c75 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -14,7 +14,7 @@ * @class EnckeOrbitPropagation * @brief Class to propagate spacecraft orbit with Encke's method */ -class EnckeOrbitPropagation : public Orbit, public math::OrdinaryDifferentialEquation<6> { +class EnckeOrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferentialEquation<6> { public: /** * @fn EnckeOrbitPropagation @@ -28,7 +28,7 @@ class EnckeOrbitPropagation : public Orbit, public math::OrdinaryDifferentialEqu * @param [in] error_tolerance: Error tolerance threshold */ EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double gravity_constant_m3_s2, const double propagation_step_s, - const double current_time_jd, const math::Vector<3> position_i_m, const math::Vector<3> velocity_i_m_s, + const double current_time_jd, const s2e::math::Vector<3> position_i_m, const s2e::math::Vector<3> velocity_i_m_s, const double error_tolerance); /** * @fn ~EnckeOrbitPropagation @@ -53,7 +53,7 @@ class EnckeOrbitPropagation : public Orbit, public math::OrdinaryDifferentialEqu * @param [in] state: Position and velocity as state vector * @param [out] rhs: Output of the function */ - virtual void DerivativeFunction(double t, const math::Vector<6>& state, math::Vector<6>& rhs); + virtual void DerivativeFunction(double t, const s2e::math::Vector<6>& state, s2e::math::Vector<6>& rhs); private: // General @@ -63,13 +63,13 @@ class EnckeOrbitPropagation : public Orbit, public math::OrdinaryDifferentialEqu double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 // reference orbit - math::Vector<3> reference_position_i_m_; //!< Reference orbit position in the inertial frame [m] - math::Vector<3> reference_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] + s2e::math::Vector<3> reference_position_i_m_; //!< Reference orbit position in the inertial frame [m] + s2e::math::Vector<3> reference_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] orbit::KeplerOrbit reference_kepler_orbit; //!< Reference Kepler orbital element // difference orbit - math::Vector<3> difference_position_i_m_; //!< Difference orbit position in the inertial frame [m] - math::Vector<3> difference_velocity_i_m_s_; //!< Difference orbit velocity in the inertial frame [m/s] + s2e::math::Vector<3> difference_position_i_m_; //!< Difference orbit position in the inertial frame [m] + s2e::math::Vector<3> difference_velocity_i_m_s_; //!< Difference orbit velocity in the inertial frame [m/s] // functions /** @@ -79,7 +79,7 @@ class EnckeOrbitPropagation : public Orbit, public math::OrdinaryDifferentialEqu * @param [in] reference_position_i_m: Initial value of reference orbit position in the inertial frame [m] * @param [in] reference_velocity_i_m_s: Initial value of reference orbit position in the inertial frame [m] */ - void Initialize(const double current_time_jd, const math::Vector<3> reference_position_i_m, const math::Vector<3> reference_velocity_i_m_s); + void Initialize(const double current_time_jd, const s2e::math::Vector<3> reference_position_i_m, const s2e::math::Vector<3> reference_velocity_i_m_s); /** * @fn UpdateSatOrbit * @brief Update satellite orbit @@ -90,7 +90,7 @@ class EnckeOrbitPropagation : public Orbit, public math::OrdinaryDifferentialEqu * @brief Calculate Q function * @param [in] difference_position_i_m: Difference of position in the inertial frame [m] */ - double CalcQFunction(const math::Vector<3> difference_position_i_m); + double CalcQFunction(const s2e::math::Vector<3> difference_position_i_m); }; #endif // S2E_DYNAMICS_ORBIT_ENCKE_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index 8ce7ab313..80252658b 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -26,9 +26,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string if (propagate_mode == "RK4") { // initialize RK4 orbit propagator - math::Vector<3> position_i_m; - math::Vector<3> velocity_i_m_s; - math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); + s2e::math::Vector<3> position_i_m; + s2e::math::Vector<3> velocity_i_m_s; + s2e::math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); for (size_t i = 0; i < 3; i++) { position_i_m[i] = pos_vel[i]; velocity_i_m_s[i] = pos_vel[i + 3]; @@ -49,9 +49,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string orbit::RelativeOrbitModel relative_dynamics_model_type = (orbit::RelativeOrbitModel)(conf.ReadInt(section_, "relative_dynamics_model_type")); orbit::StmModel stm_model_type = (orbit::StmModel)(conf.ReadInt(section_, "stm_model_type")); - math::Vector<3> init_relative_position_lvlh; + s2e::math::Vector<3> init_relative_position_lvlh; conf.ReadVector<3>(section_, "initial_relative_position_lvlh_m", init_relative_position_lvlh); - math::Vector<3> init_relative_velocity_lvlh; + s2e::math::Vector<3> init_relative_velocity_lvlh; conf.ReadVector<3>(section_, "initial_relative_velocity_lvlh_m_s", init_relative_velocity_lvlh); // There is a possibility that the orbit of the reference sat is not initialized when RelativeOrbit initialization is called To ensure that @@ -66,9 +66,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // TODO: init_mode_kepler should be removed in the next major update if (initialize_mode == OrbitInitializeMode::kInertialPositionAndVelocity) { // initialize with position and velocity - math::Vector<3> init_pos_m; + s2e::math::Vector<3> init_pos_m; conf.ReadVector<3>(section_, "initial_position_i_m", init_pos_m); - math::Vector<3> init_vel_m_s; + s2e::math::Vector<3> init_vel_m_s; conf.ReadVector<3>(section_, "initial_velocity_i_m_s", init_vel_m_s); oe = orbit::OrbitalElements(gravity_constant_m3_s2, current_time_jd, init_pos_m, init_vel_m_s); } else { @@ -85,9 +85,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string orbit = new KeplerOrbitPropagation(celestial_information, current_time_jd, kepler_orbit); } else if (propagate_mode == "ENCKE") { // initialize orbit for Encke's method - math::Vector<3> position_i_m; - math::Vector<3> velocity_i_m_s; - math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); + s2e::math::Vector<3> position_i_m; + s2e::math::Vector<3> velocity_i_m_s; + s2e::math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); for (size_t i = 0; i < 3; i++) { position_i_m[i] = pos_vel[i]; velocity_i_m_s[i] = pos_vel[i + 3]; @@ -100,9 +100,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string std::cerr << "ERROR: orbit propagation mode: " << propagate_mode << " is not defined!" << std::endl; std::cerr << "The orbit mode is automatically set as RK4" << std::endl; - math::Vector<3> position_i_m; - math::Vector<3> velocity_i_m_s; - math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); + s2e::math::Vector<3> position_i_m; + s2e::math::Vector<3> velocity_i_m_s; + s2e::math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); for (size_t i = 0; i < 3; i++) { position_i_m[i] = pos_vel[i]; velocity_i_m_s[i] = pos_vel[i + 3]; @@ -115,12 +115,12 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string return orbit; } -math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section) { +s2e::math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section) { auto conf = IniAccess(initialize_file); const char* section_ = section.c_str(); - math::Vector<3> position_i_m; - math::Vector<3> velocity_i_m_s; - math::Vector<6> pos_vel; + s2e::math::Vector<3> position_i_m; + s2e::math::Vector<3> velocity_i_m_s; + s2e::math::Vector<6> pos_vel; OrbitInitializeMode initialize_mode = SetOrbitInitializeMode(conf.ReadString(section_, "initialize_mode")); if (initialize_mode == OrbitInitializeMode::kOrbitalElements) { diff --git a/src/dynamics/orbit/initialize_orbit.hpp b/src/dynamics/orbit/initialize_orbit.hpp index 6b3aad69f..1bdbe44cd 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -35,6 +35,6 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string * @param [in] gravity_constant_m3_s2: Gravity constant [m3/s2] * @param [in] section: Section name */ -math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section = "ORBIT"); +s2e::math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section = "ORBIT"); #endif // S2E_DYNAMICS_ORBIT_INITIALIZE_ORBIT_HPP_ diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index b38aa9c98..d3058f5fd 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -4,16 +4,16 @@ */ #include "orbit.hpp" -math::Quaternion Orbit::CalcQuaternion_i2lvlh() const { - math::Vector<3> lvlh_x = spacecraft_position_i_m_; // x-axis in LVLH frame is position vector direction from geocenter to satellite - math::Vector<3> lvlh_ex = lvlh_x.CalcNormalizedVector(); - math::Vector<3> lvlh_z = +s2e::math::Quaternion Orbit::CalcQuaternion_i2lvlh() const { + s2e::math::Vector<3> lvlh_x = spacecraft_position_i_m_; // x-axis in LVLH frame is position vector direction from geocenter to satellite + s2e::math::Vector<3> lvlh_ex = lvlh_x.CalcNormalizedVector(); + s2e::math::Vector<3> lvlh_z = OuterProduct(spacecraft_position_i_m_, spacecraft_velocity_i_m_s_); // z-axis in LVLH frame is angular momentum vector direction of orbit - math::Vector<3> lvlh_ez = lvlh_z.CalcNormalizedVector(); - math::Vector<3> lvlh_y = OuterProduct(lvlh_z, lvlh_x); - math::Vector<3> lvlh_ey = lvlh_y.CalcNormalizedVector(); + s2e::math::Vector<3> lvlh_ez = lvlh_z.CalcNormalizedVector(); + s2e::math::Vector<3> lvlh_y = OuterProduct(lvlh_z, lvlh_x); + s2e::math::Vector<3> lvlh_ey = lvlh_y.CalcNormalizedVector(); - math::Matrix<3, 3> dcm_i2lvlh; + s2e::math::Matrix<3, 3> dcm_i2lvlh; dcm_i2lvlh[0][0] = lvlh_ex[0]; dcm_i2lvlh[0][1] = lvlh_ex[1]; dcm_i2lvlh[0][2] = lvlh_ex[2]; @@ -24,19 +24,19 @@ math::Quaternion Orbit::CalcQuaternion_i2lvlh() const { dcm_i2lvlh[2][1] = lvlh_ez[1]; dcm_i2lvlh[2][2] = lvlh_ez[2]; - math::Quaternion q_i2lvlh = math::Quaternion::ConvertFromDcm(dcm_i2lvlh); + s2e::math::Quaternion q_i2lvlh = s2e::math::Quaternion::ConvertFromDcm(dcm_i2lvlh); return q_i2lvlh.Normalize(); } void Orbit::TransformEciToEcef(void) { - math::Matrix<3, 3> dcm_i_to_xcxf = celestial_information_->GetEarthRotation().GetDcmJ2000ToEcef(); + s2e::math::Matrix<3, 3> dcm_i_to_xcxf = celestial_information_->GetEarthRotation().GetDcmJ2000ToEcef(); spacecraft_position_ecef_m_ = dcm_i_to_xcxf * spacecraft_position_i_m_; // convert velocity vector in ECI to the vector in ECEF - math::Vector<3> earth_angular_velocity_i_rad_s{0.0}; + s2e::math::Vector<3> earth_angular_velocity_i_rad_s{0.0}; earth_angular_velocity_i_rad_s[2] = environment::earth_mean_angular_velocity_rad_s; - math::Vector<3> we_cross_r = OuterProduct(earth_angular_velocity_i_rad_s, spacecraft_position_i_m_); - math::Vector<3> velocity_we_cross_r = spacecraft_velocity_i_m_s_ - we_cross_r; + s2e::math::Vector<3> we_cross_r = OuterProduct(earth_angular_velocity_i_rad_s, spacecraft_position_i_m_); + s2e::math::Vector<3> velocity_we_cross_r = spacecraft_velocity_i_m_s_ - we_cross_r; spacecraft_velocity_ecef_m_s_ = dcm_i_to_xcxf * velocity_we_cross_r; } diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index d3df8fb4a..01be5c87c 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -69,7 +69,7 @@ class Orbit : public ILoggable { * @brief Update attitude information * @param [in] quaternion_i2b: End time of simulation [sec] */ - inline void UpdateByAttitude(const math::Quaternion quaternion_i2b) { + inline void UpdateByAttitude(const s2e::math::Quaternion quaternion_i2b) { spacecraft_velocity_b_m_s_ = quaternion_i2b.FrameConversion(spacecraft_velocity_i_m_s_); } @@ -88,27 +88,27 @@ class Orbit : public ILoggable { * @fn GetPosition_i_m * @brief Return spacecraft position in the inertial frame [m] */ - inline math::Vector<3> GetPosition_i_m() const { return spacecraft_position_i_m_; } + inline s2e::math::Vector<3> GetPosition_i_m() const { return spacecraft_position_i_m_; } /** * @fn GetPosition_ecef_m * @brief Return spacecraft position in the ECEF frame [m] */ - inline math::Vector<3> GetPosition_ecef_m() const { return spacecraft_position_ecef_m_; } + inline s2e::math::Vector<3> GetPosition_ecef_m() const { return spacecraft_position_ecef_m_; } /** * @fn GetVelocity_i_m_s * @brief Return spacecraft velocity in the inertial frame [m/s] */ - inline math::Vector<3> GetVelocity_i_m_s() const { return spacecraft_velocity_i_m_s_; } + inline s2e::math::Vector<3> GetVelocity_i_m_s() const { return spacecraft_velocity_i_m_s_; } /** * @fn GetVelocity_b_m_s * @brief Return spacecraft velocity in the body fixed frame [m/s] */ - inline math::Vector<3> GetVelocity_b_m_s() const { return spacecraft_velocity_b_m_s_; } + inline s2e::math::Vector<3> GetVelocity_b_m_s() const { return spacecraft_velocity_b_m_s_; } /** * @fn GetVelocity_ecef_m_s * @brief Return spacecraft velocity in the ECEF frame [m/s] */ - inline math::Vector<3> GetVelocity_ecef_m_s() const { return spacecraft_velocity_ecef_m_s_; } + inline s2e::math::Vector<3> GetVelocity_ecef_m_s() const { return spacecraft_velocity_ecef_m_s_; } /** * @fn GetGeodeticPosition * @brief Return spacecraft position in the geodetic frame [m] @@ -119,8 +119,8 @@ class Orbit : public ILoggable { inline double GetLatitude_rad() const { return spacecraft_geodetic_position_.GetLatitude_rad(); } inline double GetLongitude_rad() const { return spacecraft_geodetic_position_.GetLongitude_rad(); } inline double GetAltitude_m() const { return spacecraft_geodetic_position_.GetAltitude_m(); } - inline math::Vector<3> GetLatLonAlt() const { - math::Vector<3> vec; + inline s2e::math::Vector<3> GetLatLonAlt() const { + s2e::math::Vector<3> vec; vec(0) = spacecraft_geodetic_position_.GetLatitude_rad(); vec(1) = spacecraft_geodetic_position_.GetLongitude_rad(); vec(2) = spacecraft_geodetic_position_.GetAltitude_m(); @@ -137,15 +137,15 @@ class Orbit : public ILoggable { * @fn SetAcceleration_i_m_s2 * @brief Set acceleration in the inertial frame [m/s2] */ - inline void SetAcceleration_i_m_s2(const math::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ = acceleration_i_m_s2; } + inline void SetAcceleration_i_m_s2(const s2e::math::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ = acceleration_i_m_s2; } /** * @fn AddForce_i_N * @brief Add force * @param [in] force_i: Force in the inertial frame [N] * @param [in] spacecraft_mass_kg: Mass of spacecraft [kg] */ - inline void AddForce_i_N(const math::Vector<3> force_i_N, double spacecraft_mass_kg) { - math::Vector<3> acceleration_i_m_s2 = force_i_N; + inline void AddForce_i_N(const s2e::math::Vector<3> force_i_N, double spacecraft_mass_kg) { + s2e::math::Vector<3> acceleration_i_m_s2 = force_i_N; acceleration_i_m_s2 /= spacecraft_mass_kg; // FIXME spacecraft_acceleration_i_m_s2_ += acceleration_i_m_s2; } @@ -153,7 +153,7 @@ class Orbit : public ILoggable { * @fn AddAcceleration_i_m_s2 * @brief Add acceleration in the inertial frame [m/s2] */ - inline void AddAcceleration_i_m_s2(const math::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ += acceleration_i_m_s2; } + inline void AddAcceleration_i_m_s2(const s2e::math::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ += acceleration_i_m_s2; } /** * @fn AddForce_i_N * @brief Add force @@ -161,7 +161,7 @@ class Orbit : public ILoggable { * @param [in] quaternion_i2b: Quaternion from the inertial frame to the body fixed frame * @param [in] spacecraft_mass_kg: Mass of spacecraft [kg] */ - inline void AddForce_b_N(const math::Vector<3> force_b_N, const math::Quaternion quaternion_i2b, const double spacecraft_mass_kg) { + inline void AddForce_b_N(const s2e::math::Vector<3> force_b_N, const s2e::math::Quaternion quaternion_i2b, const double spacecraft_mass_kg) { auto force_i = quaternion_i2b.InverseFrameConversion(force_b_N); AddForce_i_N(force_i, spacecraft_mass_kg); } @@ -170,7 +170,7 @@ class Orbit : public ILoggable { * @fn CalcQuaternion_i2lvlh * @brief Calculate and return quaternion from the inertial frame to the LVLH frame */ - math::Quaternion CalcQuaternion_i2lvlh() const; + s2e::math::Quaternion CalcQuaternion_i2lvlh() const; // Override ILoggable /** @@ -191,15 +191,15 @@ class Orbit : public ILoggable { bool is_calc_enabled_ = false; //!< Calculate flag OrbitPropagateMode propagate_mode_; //!< Propagation mode - math::Vector<3> spacecraft_position_i_m_; //!< Spacecraft position in the inertial frame [m] - math::Vector<3> spacecraft_position_ecef_m_; //!< Spacecraft position in the ECEF frame [m] + s2e::math::Vector<3> spacecraft_position_i_m_; //!< Spacecraft position in the inertial frame [m] + s2e::math::Vector<3> spacecraft_position_ecef_m_; //!< Spacecraft position in the ECEF frame [m] geodesy::GeodeticPosition spacecraft_geodetic_position_; //!< Spacecraft position in the Geodetic frame - math::Vector<3> spacecraft_velocity_i_m_s_; //!< Spacecraft velocity in the inertial frame [m/s] - math::Vector<3> spacecraft_velocity_b_m_s_; //!< Spacecraft velocity in the body frame [m/s] - math::Vector<3> spacecraft_velocity_ecef_m_s_; //!< Spacecraft velocity in the ECEF frame [m/s] + s2e::math::Vector<3> spacecraft_velocity_i_m_s_; //!< Spacecraft velocity in the inertial frame [m/s] + s2e::math::Vector<3> spacecraft_velocity_b_m_s_; //!< Spacecraft velocity in the body frame [m/s] + s2e::math::Vector<3> spacecraft_velocity_ecef_m_s_; //!< Spacecraft velocity in the ECEF frame [m/s] - math::Vector<3> spacecraft_acceleration_i_m_s2_; //!< Spacecraft acceleration in the inertial frame [m/s2] + s2e::math::Vector<3> spacecraft_acceleration_i_m_s2_; //!< Spacecraft acceleration in the inertial frame [m/s2] //!< NOTE: Clear to zero at the end of the Propagate function // Frame Conversion TODO: consider other planet diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index f60f52682..831e8d763 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -9,11 +9,11 @@ #include "rk4_orbit_propagation.hpp" RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, - int reference_spacecraft_id, math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, + int reference_spacecraft_id, s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, orbit::RelativeOrbitModel relative_dynamics_model_type, orbit::StmModel stm_model_type, RelativeInformation* relative_information) : Orbit(celestial_information), - math::OrdinaryDifferentialEquation<6>(time_step_s), + s2e::math::OrdinaryDifferentialEquation<6>(time_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2), reference_spacecraft_id_(reference_spacecraft_id), update_method_(update_method), @@ -30,7 +30,7 @@ RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, RelativeOrbit::~RelativeOrbit() {} -void RelativeOrbit::InitializeState(math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, +void RelativeOrbit::InitializeState(s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, double gravity_constant_m3_s2, double initial_time_s) { relative_position_lvlh_m_ = relative_position_lvlh_m; relative_velocity_lvlh_m_s_ = relative_velocity_lvlh_m_s; @@ -38,10 +38,10 @@ void RelativeOrbit::InitializeState(math::Vector<3> relative_position_lvlh_m, ma // Disturbance acceleration are not considered in relative orbit propagation spacecraft_acceleration_i_m_s2_ *= 0.0; - math::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); - math::Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); - math::Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); - math::Quaternion q_lvlh2i = q_i2lvlh.Conjugate(); + s2e::math::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); + s2e::math::Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); + s2e::math::Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); + s2e::math::Quaternion q_lvlh2i = q_i2lvlh.Conjugate(); spacecraft_position_i_m_ = q_lvlh2i.FrameConversion(relative_position_lvlh_m_) + reference_sat_position_i; spacecraft_velocity_i_m_s_ = q_lvlh2i.FrameConversion(relative_velocity_lvlh_m_s_) + reference_sat_velocity_i; @@ -108,10 +108,10 @@ void RelativeOrbit::Propagate(const double end_time_s, const double current_time PropagateStm(end_time_s); } - math::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); - math::Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); - math::Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); - math::Quaternion q_lvlh2i = q_i2lvlh.Conjugate(); + s2e::math::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); + s2e::math::Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); + s2e::math::Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); + s2e::math::Quaternion q_lvlh2i = q_i2lvlh.Conjugate(); spacecraft_position_i_m_ = q_lvlh2i.FrameConversion(relative_position_lvlh_m_) + reference_sat_position_i; spacecraft_velocity_i_m_s_ = q_lvlh2i.FrameConversion(relative_velocity_lvlh_m_s_) + reference_sat_velocity_i; @@ -138,7 +138,7 @@ void RelativeOrbit::PropagateRk4(double elapsed_sec) { } void RelativeOrbit::PropagateStm(double elapsed_sec) { - math::Vector<6> current_state; + s2e::math::Vector<6> current_state; CalculateStm(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), gravity_constant_m3_s2_, elapsed_sec); current_state = stm_ * initial_state_; @@ -151,8 +151,8 @@ void RelativeOrbit::PropagateStm(double elapsed_sec) { relative_velocity_lvlh_m_s_[2] = current_state[5]; } -void RelativeOrbit::DerivativeFunction(double t, const math::Vector<6>& state, - math::Vector<6>& rhs) // only for RK4 relative dynamics propagation +void RelativeOrbit::DerivativeFunction(double t, const s2e::math::Vector<6>& state, + s2e::math::Vector<6>& rhs) // only for RK4 relative dynamics propagation { rhs = system_matrix_ * state; (void)t; diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 3aae6e104..c281cd3c9 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -17,7 +17,7 @@ * @class RelativeOrbit * @brief Class to propagate relative orbit */ -class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> { +class RelativeOrbit : public Orbit, public s2e::math::OrdinaryDifferentialEquation<6> { public: /** * @enum RelativeOrbitUpdateMethod @@ -40,7 +40,7 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> * @param [in] relative_information: Relative information */ RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, int reference_spacecraft_id, - math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, + s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, orbit::RelativeOrbitModel relative_dynamics_model_type, orbit::StmModel stm_model_type, RelativeInformation* relative_information); /** * @fn ~RelativeOrbit @@ -73,12 +73,12 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] double propagation_step_s_; //!< Step width for RK4 [sec] - math::Matrix<6, 6> system_matrix_; //!< System matrix - math::Matrix<6, 6> stm_; //!< State transition matrix + s2e::math::Matrix<6, 6> system_matrix_; //!< System matrix + s2e::math::Matrix<6, 6> stm_; //!< State transition matrix - math::Vector<6> initial_state_; //!< Initial state (Position and Velocity) - math::Vector<3> relative_position_lvlh_m_; //!< Relative position in the LVLH frame - math::Vector<3> relative_velocity_lvlh_m_s_; //!< Relative velocity in the LVLH frame + s2e::math::Vector<6> initial_state_; //!< Initial state (Position and Velocity) + s2e::math::Vector<3> relative_position_lvlh_m_; //!< Relative position in the LVLH frame + s2e::math::Vector<3> relative_velocity_lvlh_m_s_; //!< Relative velocity in the LVLH frame RelativeOrbitUpdateMethod update_method_; //!< Update method orbit::RelativeOrbitModel relative_dynamics_model_type_; //!< Relative dynamics model type @@ -93,7 +93,7 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] initial_time_s: Initialize time [sec] */ - void InitializeState(math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, double gravity_constant_m3_s2, + void InitializeState(s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, double gravity_constant_m3_s2, double initial_time_s = 0); /** * @fn CalculateSystemMatrix diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index 38a6e39b2..dd2c5abfa 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -9,7 +9,7 @@ #include Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, - math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s) + s2e::math::Vector<3> position_i_m, s2e::math::Vector<3> velocity_i_m_s, double initial_time_s) : Orbit(celestial_information), OrdinaryDifferentialEquation<6>(time_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2) { propagate_mode_ = OrbitPropagateMode::kRk4; @@ -22,7 +22,7 @@ Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_i Rk4OrbitPropagation::~Rk4OrbitPropagation() {} -void Rk4OrbitPropagation::DerivativeFunction(double t, const math::Vector<6>& state, math::Vector<6>& rhs) { +void Rk4OrbitPropagation::DerivativeFunction(double t, const s2e::math::Vector<6>& state, s2e::math::Vector<6>& rhs) { double x = state[0], y = state[1], z = state[2]; double vx = state[3], vy = state[4], vz = state[5]; @@ -38,9 +38,9 @@ void Rk4OrbitPropagation::DerivativeFunction(double t, const math::Vector<6>& st (void)t; } -void Rk4OrbitPropagation::Initialize(math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s) { +void Rk4OrbitPropagation::Initialize(s2e::math::Vector<3> position_i_m, s2e::math::Vector<3> velocity_i_m_s, double initial_time_s) { // state vector [x,y,z,vx,vy,vz] - math::Vector<6> init_state; + s2e::math::Vector<6> init_state; init_state[0] = position_i_m[0]; init_state[1] = position_i_m[1]; init_state[2] = position_i_m[2]; diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index fde677ddd..83fc56bbe 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -15,7 +15,7 @@ * @class Rk4OrbitPropagation * @brief Class to propagate spacecraft orbit with Runge-Kutta-4 method */ -class Rk4OrbitPropagation : public Orbit, public math::OrdinaryDifferentialEquation<6> { +class Rk4OrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferentialEquation<6> { public: /** * @fn Rk4OrbitPropagation @@ -28,7 +28,7 @@ class Rk4OrbitPropagation : public Orbit, public math::OrdinaryDifferentialEquat * @param [in] initial_time_s: Initial time [sec] */ Rk4OrbitPropagation(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, - math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s = 0); + s2e::math::Vector<3> position_i_m, s2e::math::Vector<3> velocity_i_m_s, double initial_time_s = 0); /** * @fn ~Rk4OrbitPropagation * @brief Destructor @@ -43,7 +43,7 @@ class Rk4OrbitPropagation : public Orbit, public math::OrdinaryDifferentialEquat * @param [in] state: Position and velocity as state vector * @param [out] rhs: Output of the function */ - virtual void DerivativeFunction(double t, const math::Vector<6>& state, math::Vector<6>& rhs); + virtual void DerivativeFunction(double t, const s2e::math::Vector<6>& state, s2e::math::Vector<6>& rhs); // Override Orbit /** @@ -66,7 +66,7 @@ class Rk4OrbitPropagation : public Orbit, public math::OrdinaryDifferentialEquat * @param [in] velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] * @param [in] initial_time_s: Initial time [sec] */ - void Initialize(math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s = 0); + void Initialize(s2e::math::Vector<3> position_i_m, s2e::math::Vector<3> velocity_i_m_s, double initial_time_s = 0); }; #endif // S2E_DYNAMICS_ORBIT_RK4_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/thermal/heatload.cpp b/src/dynamics/thermal/heatload.cpp index 3d01aad08..15d32b25a 100644 --- a/src/dynamics/thermal/heatload.cpp +++ b/src/dynamics/thermal/heatload.cpp @@ -5,7 +5,7 @@ #include using namespace std; -using namespace math; +using namespace s2e::math; Heatload::Heatload(int node_id, std::vector time_table_s, std::vector internal_heatload_table_W) : node_id_(node_id), time_table_s_(time_table_s), internal_heatload_table_W_(internal_heatload_table_W) { diff --git a/src/dynamics/thermal/node.cpp b/src/dynamics/thermal/node.cpp index b34958a68..31f9774bd 100644 --- a/src/dynamics/thermal/node.cpp +++ b/src/dynamics/thermal/node.cpp @@ -10,10 +10,10 @@ #include using namespace std; -using namespace math; +using namespace s2e::math; Node::Node(const size_t node_id, const string node_name, const NodeType node_type, const size_t heater_id, const double temperature_ini_K, - const double capacity_J_K, const double alpha, const double area_m2, math::Vector<3> normal_vector_b) + const double capacity_J_K, const double alpha, const double area_m2, s2e::math::Vector<3> normal_vector_b) : node_id_(node_id), node_name_(node_name), heater_id_(heater_id), @@ -29,7 +29,7 @@ Node::Node(const size_t node_id, const string node_name, const NodeType node_typ Node::~Node() {} -double Node::CalcSolarRadiation_W(math::Vector<3> sun_direction_b, double solar_flux_W_m2) { +double Node::CalcSolarRadiation_W(s2e::math::Vector<3> sun_direction_b, double solar_flux_W_m2) { double cos_theta = InnerProduct(sun_direction_b, normal_vector_b_); // calculate sun_power @@ -147,7 +147,7 @@ Node InitNode(const std::vector& node_str) { capacity_J_K = stod(node_str[index_capacity]); alpha = stod(node_str[index_alpha]); area_m2 = stod(node_str[index_area]); - math::Vector<3> normal_v_b; + s2e::math::Vector<3> normal_v_b; for (size_t i = 0; i < 3; i++) { normal_v_b[i] = stod(node_str[index_normal_v_b_head + i]); } diff --git a/src/dynamics/thermal/node.hpp b/src/dynamics/thermal/node.hpp index de4653d3e..d579c80f6 100644 --- a/src/dynamics/thermal/node.hpp +++ b/src/dynamics/thermal/node.hpp @@ -45,7 +45,7 @@ class Node { * @param[in] normal_vector_b: Normal vector of face with possibility of solar incidence (Body frame) */ Node(const size_t node_id, const std::string node_name, const NodeType node_type, const size_t heater_id, const double temperature_ini_K, - const double capacity_J_K, const double alpha, const double area_m2, math::Vector<3> normal_vector_b); + const double capacity_J_K, const double alpha, const double area_m2, s2e::math::Vector<3> normal_vector_b); /** * @fn ~Node * @brief Destroy the Node object @@ -58,7 +58,7 @@ class Node { * @param sun_direction_b: Sun direction in body frame * @return double: Solar Radiation [W] */ - double CalcSolarRadiation_W(math::Vector<3> sun_direction_b, double solar_flux_W_m2); + double CalcSolarRadiation_W(s2e::math::Vector<3> sun_direction_b, double solar_flux_W_m2); // Getter /** @@ -136,7 +136,7 @@ class Node { double area_m2_; double solar_radiation_W_; NodeType node_type_; - math::Vector<3> normal_vector_b_; + s2e::math::Vector<3> normal_vector_b_; /** * @fn AssertNodeParams diff --git a/src/dynamics/thermal/temperature.cpp b/src/dynamics/thermal/temperature.cpp index b86e873fe..3c18bfa20 100644 --- a/src/dynamics/thermal/temperature.cpp +++ b/src/dynamics/thermal/temperature.cpp @@ -47,10 +47,10 @@ Temperature::Temperature() { Temperature::~Temperature() {} -void Temperature::Propagate(math::Vector<3> sun_position_b_m, const double time_end_s) { +void Temperature::Propagate(s2e::math::Vector<3> sun_position_b_m, const double time_end_s) { if (!is_calc_enabled_) return; double sun_distance_m = sun_position_b_m.CalcNorm(); - math::Vector<3> sun_direction_b; + s2e::math::Vector<3> sun_direction_b; for (size_t i = 0; i < 3; i++) { sun_direction_b[i] = sun_position_b_m[i] / sun_distance_m; } @@ -84,7 +84,7 @@ void Temperature::Propagate(math::Vector<3> sun_position_b_m, const double time_ } } -void Temperature::CalcRungeOneStep(double time_now_s, double time_step_s, math::Vector<3> sun_direction_b, size_t node_num) { +void Temperature::CalcRungeOneStep(double time_now_s, double time_step_s, s2e::math::Vector<3> sun_direction_b, size_t node_num) { vector temperatures_now_K(node_num); for (size_t i = 0; i < node_num; i++) { temperatures_now_K[i] = nodes_[i].GetTemperature_K(); @@ -120,7 +120,7 @@ void Temperature::CalcRungeOneStep(double time_now_s, double time_step_s, math:: } } -vector Temperature::CalcTemperatureDifferentials(vector temperatures_K, double t, math::Vector<3> sun_direction_b, size_t node_num) { +vector Temperature::CalcTemperatureDifferentials(vector temperatures_K, double t, s2e::math::Vector<3> sun_direction_b, size_t node_num) { // TODO: consider the following unused arguments are really needed UNUSED(temperatures_K); diff --git a/src/dynamics/thermal/temperature.hpp b/src/dynamics/thermal/temperature.hpp index 08eceb28f..ec5177d66 100644 --- a/src/dynamics/thermal/temperature.hpp +++ b/src/dynamics/thermal/temperature.hpp @@ -54,7 +54,7 @@ class Temperature : public ILoggable { * @param[in] sun_direction_b: Sun position in body frame [m] * @param[in] node_num: Number of nodes */ - void CalcRungeOneStep(double time_now_s, double time_step_s, math::Vector<3> sun_direction_b, size_t node_num); + void CalcRungeOneStep(double time_now_s, double time_step_s, s2e::math::Vector<3> sun_direction_b, size_t node_num); /** * @fn CalcTemperatureDifferentials * @brief Calculate differential of thermal equilibrium equation @@ -65,7 +65,7 @@ class Temperature : public ILoggable { * @param node_num: Number of nodes * @return std::vector: Differential of thermal equilibrium equation at time now */ - std::vector CalcTemperatureDifferentials(std::vector temperatures_K, double time_now_s, const math::Vector<3> sun_direction_b, + std::vector CalcTemperatureDifferentials(std::vector temperatures_K, double time_now_s, const s2e::math::Vector<3> sun_direction_b, size_t node_num); public: @@ -108,7 +108,7 @@ class Temperature : public ILoggable { * @param[in] sun_position_b_m: Sun position in body frame [m] * @param time_end_s: Time to finish propagation [s] */ - void Propagate(math::Vector<3> sun_position_b_m, const double time_end_s); + void Propagate(s2e::math::Vector<3> sun_position_b_m, const double time_end_s); // Getter /** diff --git a/src/environment/global/celestial_information.hpp b/src/environment/global/celestial_information.hpp index 50103ffca..6ffe57fbf 100644 --- a/src/environment/global/celestial_information.hpp +++ b/src/environment/global/celestial_information.hpp @@ -73,8 +73,8 @@ class CelestialInformation : public ILoggable { * @brief Return position from the center body in the inertial frame [m] * @param [in] id: ID of CelestialInformation list */ - inline math::Vector<3> GetPositionFromCenter_i_m(const unsigned int id) const { - math::Vector<3> pos(0.0); + inline s2e::math::Vector<3> GetPositionFromCenter_i_m(const unsigned int id) const { + s2e::math::Vector<3> pos(0.0); if (id > number_of_selected_bodies_) return pos; for (int i = 0; i < 3; i++) pos[i] = celestial_body_position_from_center_i_m_[id * 3 + i]; return pos; @@ -84,7 +84,7 @@ class CelestialInformation : public ILoggable { * @brief Return position from the center body in the inertial frame [m] * @param [in] body_name: Name of the body defined in the SPICE */ - inline math::Vector<3> GetPositionFromCenter_i_m(const char* body_name) const { + inline s2e::math::Vector<3> GetPositionFromCenter_i_m(const char* body_name) const { int id = CalcBodyIdFromName(body_name); return GetPositionFromCenter_i_m(id); } @@ -94,11 +94,11 @@ class CelestialInformation : public ILoggable { * @param [in] target_body_name: Name of the target body defined in the SPICE * @param [in] reference_body_name: Name of the reference body defined in the SPICE */ - inline math::Vector<3> GetPositionFromSelectedBody_i_m(const char* target_body_name, const char* reference_body_name) const { + inline s2e::math::Vector<3> GetPositionFromSelectedBody_i_m(const char* target_body_name, const char* reference_body_name) const { int target_id = CalcBodyIdFromName(target_body_name); - math::Vector<3> target_body_position_i_m = GetPositionFromCenter_i_m(target_id); + s2e::math::Vector<3> target_body_position_i_m = GetPositionFromCenter_i_m(target_id); int reference_id = CalcBodyIdFromName(reference_body_name); - math::Vector<3> reference_body_position_i_m = GetPositionFromCenter_i_m(reference_id); + s2e::math::Vector<3> reference_body_position_i_m = GetPositionFromCenter_i_m(reference_id); return target_body_position_i_m - reference_body_position_i_m; } @@ -108,8 +108,8 @@ class CelestialInformation : public ILoggable { * @brief Return velocity from the center body in the inertial frame [m/s] * @param [in] id: ID of CelestialInformation list */ - inline math::Vector<3> GetVelocityFromCenter_i_m_s(const unsigned int id) const { - math::Vector<3> vel(0.0); + inline s2e::math::Vector<3> GetVelocityFromCenter_i_m_s(const unsigned int id) const { + s2e::math::Vector<3> vel(0.0); if (id > number_of_selected_bodies_) return vel; for (int i = 0; i < 3; i++) vel[i] = celestial_body_velocity_from_center_i_m_s_[id * 3 + i]; return vel; @@ -119,7 +119,7 @@ class CelestialInformation : public ILoggable { * @brief Return velocity from the center body in the inertial frame [m/s] * @param [in] body_name: Name of the body defined in the SPICE */ - inline math::Vector<3> GetVelocityFromCenter_i_m_s(const char* body_name) const { + inline s2e::math::Vector<3> GetVelocityFromCenter_i_m_s(const char* body_name) const { int id = CalcBodyIdFromName(body_name); return GetVelocityFromCenter_i_m_s(id); } @@ -129,11 +129,11 @@ class CelestialInformation : public ILoggable { * @param [in] target_body_name: Name of the target body defined in the SPICE * @param [in] reference_body_name: Name of the reference body defined in the SPICE */ - inline math::Vector<3> GetVelocityFromSelectedBody_i_m_s(const char* target_body_name, const char* reference_body_name) const { + inline s2e::math::Vector<3> GetVelocityFromSelectedBody_i_m_s(const char* target_body_name, const char* reference_body_name) const { int target_id = CalcBodyIdFromName(target_body_name); - math::Vector<3> target_body_velocity_i_m_s = GetVelocityFromCenter_i_m_s(target_id); + s2e::math::Vector<3> target_body_velocity_i_m_s = GetVelocityFromCenter_i_m_s(target_id); int reference_id = CalcBodyIdFromName(reference_body_name); - math::Vector<3> reference_body_velocity_i_m_s = GetVelocityFromCenter_i_m_s(reference_id); + s2e::math::Vector<3> reference_body_velocity_i_m_s = GetVelocityFromCenter_i_m_s(reference_id); return target_body_velocity_i_m_s - reference_body_velocity_i_m_s; } @@ -160,8 +160,8 @@ class CelestialInformation : public ILoggable { * @brief Return 3 axis planetographic radii of a celestial body [m] * @param [in] id: ID of CelestialInformation list */ - inline math::Vector<3> GetRadii_m(const unsigned int id) const { - math::Vector<3> radii(0.0); + inline s2e::math::Vector<3> GetRadii_m(const unsigned int id) const { + s2e::math::Vector<3> radii(0.0); if (id > number_of_selected_bodies_) return radii; for (int i = 0; i < 3; i++) radii[i] = celestial_body_planetographic_radii_m_[id * 3 + i]; return radii; @@ -171,7 +171,7 @@ class CelestialInformation : public ILoggable { * @brief Return 3 axis planetographic radii of a celestial body [m] * @param [in] body_name: Name of the body defined in the SPICE */ - inline math::Vector<3> GetRadiiFromName_m(const char* body_name) const { + inline s2e::math::Vector<3> GetRadiiFromName_m(const char* body_name) const { int id = CalcBodyIdFromName(body_name); return GetRadii_m(id); } diff --git a/src/environment/global/earth_rotation.cpp b/src/environment/global/earth_rotation.cpp index dd552f04d..8950fd904 100644 --- a/src/environment/global/earth_rotation.cpp +++ b/src/environment/global/earth_rotation.cpp @@ -17,7 +17,7 @@ // Default constructor EarthRotation::EarthRotation(const EarthRotationMode rotation_mode) : rotation_mode_(rotation_mode) { - dcm_j2000_to_ecef_ = math::MakeIdentityMatrix<3>(); + dcm_j2000_to_ecef_ = s2e::math::MakeIdentityMatrix<3>(); dcm_teme_to_ecef_ = dcm_j2000_to_ecef_; InitializeParameters(); } @@ -32,80 +32,80 @@ void EarthRotation::InitializeParameters() { // Coefficients to compute mean obliquity of the ecliptic // The actual unit of the coefficients are [rad/century^i], where i is the index of the array - c_epsilon_rad_[0] = 23.4392911 * math::deg_to_rad; // [rad] - c_epsilon_rad_[1] = -46.8150000 * math::arcsec_to_rad; // [rad/century] - c_epsilon_rad_[2] = -5.9000e-4 * math::arcsec_to_rad; // [rad/century^2] - c_epsilon_rad_[3] = 1.8130e-3 * math::arcsec_to_rad; // [rad/century^3] + c_epsilon_rad_[0] = 23.4392911 * s2e::math::deg_to_rad; // [rad] + c_epsilon_rad_[1] = -46.8150000 * s2e::math::arcsec_to_rad; // [rad/century] + c_epsilon_rad_[2] = -5.9000e-4 * s2e::math::arcsec_to_rad; // [rad/century^2] + c_epsilon_rad_[3] = 1.8130e-3 * s2e::math::arcsec_to_rad; // [rad/century^3] // Coefficients to compute Delaunay angles // The actual unit of the coefficients are [rad/century^i], where i is the index of the array - c_lm_rad_[0] = 134.96340251 * math::deg_to_rad; // [rad] - c_lm_rad_[1] = 1717915923.21780000 * math::arcsec_to_rad; // [rad/century] - c_lm_rad_[2] = 31.87920000 * math::arcsec_to_rad; // [rad/century^2] - c_lm_rad_[3] = 0.05163500 * math::arcsec_to_rad; // [rad/century^3] - c_lm_rad_[4] = -0.00024470 * math::arcsec_to_rad; // [rad/century^4] - - c_ls_rad_[0] = 357.52910918 * math::deg_to_rad; // [rad] - c_ls_rad_[1] = 129596581.04810000 * math::arcsec_to_rad; // [rad/century] - c_ls_rad_[2] = -0.55320000 * math::arcsec_to_rad; // [rad/century^2] - c_ls_rad_[3] = 0.00013600 * math::arcsec_to_rad; // [rad/century^3] - c_ls_rad_[4] = -0.00001149 * math::arcsec_to_rad; // [rad/century^4] - - c_f_rad_[0] = 93.27209062 * math::deg_to_rad; // [rad] - c_f_rad_[1] = 1739527262.84780000 * math::arcsec_to_rad; // [rad/century] - c_f_rad_[2] = -12.75120000 * math::arcsec_to_rad; // [rad/century^2] - c_f_rad_[3] = -0.00103700 * math::arcsec_to_rad; // [rad/century^3] - c_f_rad_[4] = 0.00000417 * math::arcsec_to_rad; // [rad/century^4] - - c_d_rad_[0] = 297.85019547 * math::deg_to_rad; // [rad] - c_d_rad_[1] = 1602961601.20900000 * math::arcsec_to_rad; // [rad/century] - c_d_rad_[2] = -6.37060000 * math::arcsec_to_rad; // [rad/century^2] - c_d_rad_[3] = 0.00659300 * math::arcsec_to_rad; // [rad/century^3] - c_d_rad_[4] = -0.00003169 * math::arcsec_to_rad; // [rad/century^4] - - c_o_rad_[0] = 125.04455501 * math::deg_to_rad; // [rad] - c_o_rad_[1] = -6962890.54310000 * math::arcsec_to_rad; // [rad/century] - c_o_rad_[2] = 7.47220000 * math::arcsec_to_rad; // [rad/century^2] - c_o_rad_[3] = 0.00770200 * math::arcsec_to_rad; // [rad/century^3] - c_o_rad_[4] = -0.00005939 * math::arcsec_to_rad; // [rad/century^4] + c_lm_rad_[0] = 134.96340251 * s2e::math::deg_to_rad; // [rad] + c_lm_rad_[1] = 1717915923.21780000 * s2e::math::arcsec_to_rad; // [rad/century] + c_lm_rad_[2] = 31.87920000 * s2e::math::arcsec_to_rad; // [rad/century^2] + c_lm_rad_[3] = 0.05163500 * s2e::math::arcsec_to_rad; // [rad/century^3] + c_lm_rad_[4] = -0.00024470 * s2e::math::arcsec_to_rad; // [rad/century^4] + + c_ls_rad_[0] = 357.52910918 * s2e::math::deg_to_rad; // [rad] + c_ls_rad_[1] = 129596581.04810000 * s2e::math::arcsec_to_rad; // [rad/century] + c_ls_rad_[2] = -0.55320000 * s2e::math::arcsec_to_rad; // [rad/century^2] + c_ls_rad_[3] = 0.00013600 * s2e::math::arcsec_to_rad; // [rad/century^3] + c_ls_rad_[4] = -0.00001149 * s2e::math::arcsec_to_rad; // [rad/century^4] + + c_f_rad_[0] = 93.27209062 * s2e::math::deg_to_rad; // [rad] + c_f_rad_[1] = 1739527262.84780000 * s2e::math::arcsec_to_rad; // [rad/century] + c_f_rad_[2] = -12.75120000 * s2e::math::arcsec_to_rad; // [rad/century^2] + c_f_rad_[3] = -0.00103700 * s2e::math::arcsec_to_rad; // [rad/century^3] + c_f_rad_[4] = 0.00000417 * s2e::math::arcsec_to_rad; // [rad/century^4] + + c_d_rad_[0] = 297.85019547 * s2e::math::deg_to_rad; // [rad] + c_d_rad_[1] = 1602961601.20900000 * s2e::math::arcsec_to_rad; // [rad/century] + c_d_rad_[2] = -6.37060000 * s2e::math::arcsec_to_rad; // [rad/century^2] + c_d_rad_[3] = 0.00659300 * s2e::math::arcsec_to_rad; // [rad/century^3] + c_d_rad_[4] = -0.00003169 * s2e::math::arcsec_to_rad; // [rad/century^4] + + c_o_rad_[0] = 125.04455501 * s2e::math::deg_to_rad; // [rad] + c_o_rad_[1] = -6962890.54310000 * s2e::math::arcsec_to_rad; // [rad/century] + c_o_rad_[2] = 7.47220000 * s2e::math::arcsec_to_rad; // [rad/century^2] + c_o_rad_[3] = 0.00770200 * s2e::math::arcsec_to_rad; // [rad/century^3] + c_o_rad_[4] = -0.00005939 * s2e::math::arcsec_to_rad; // [rad/century^4] // Coefficients to compute nutation angles - c_d_epsilon_rad_[0] = 9.2050 * math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[1] = 0.5730 * math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[2] = -0.0900 * math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[3] = 0.0980 * math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[4] = 0.0070 * math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[5] = -0.0010 * math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[6] = 0.0220 * math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[7] = 0.0130 * math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[8] = -0.0100 * math::arcsec_to_rad; // [rad] - - c_d_psi_rad_[0] = -17.2060 * math::arcsec_to_rad; // [rad] - c_d_psi_rad_[1] = -1.3170 * math::arcsec_to_rad; // [rad] - c_d_psi_rad_[2] = 0.2070 * math::arcsec_to_rad; // [rad] - c_d_psi_rad_[3] = -0.2280 * math::arcsec_to_rad; // [rad] - c_d_psi_rad_[4] = 0.1480 * math::arcsec_to_rad; // [rad] - c_d_psi_rad_[5] = 0.0710 * math::arcsec_to_rad; // [rad] - c_d_psi_rad_[6] = -0.0520 * math::arcsec_to_rad; // [rad] - c_d_psi_rad_[7] = -0.0300 * math::arcsec_to_rad; // [rad] - c_d_psi_rad_[8] = 0.0220 * math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[0] = 9.2050 * s2e::math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[1] = 0.5730 * s2e::math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[2] = -0.0900 * s2e::math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[3] = 0.0980 * s2e::math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[4] = 0.0070 * s2e::math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[5] = -0.0010 * s2e::math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[6] = 0.0220 * s2e::math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[7] = 0.0130 * s2e::math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[8] = -0.0100 * s2e::math::arcsec_to_rad; // [rad] + + c_d_psi_rad_[0] = -17.2060 * s2e::math::arcsec_to_rad; // [rad] + c_d_psi_rad_[1] = -1.3170 * s2e::math::arcsec_to_rad; // [rad] + c_d_psi_rad_[2] = 0.2070 * s2e::math::arcsec_to_rad; // [rad] + c_d_psi_rad_[3] = -0.2280 * s2e::math::arcsec_to_rad; // [rad] + c_d_psi_rad_[4] = 0.1480 * s2e::math::arcsec_to_rad; // [rad] + c_d_psi_rad_[5] = 0.0710 * s2e::math::arcsec_to_rad; // [rad] + c_d_psi_rad_[6] = -0.0520 * s2e::math::arcsec_to_rad; // [rad] + c_d_psi_rad_[7] = -0.0300 * s2e::math::arcsec_to_rad; // [rad] + c_d_psi_rad_[8] = 0.0220 * s2e::math::arcsec_to_rad; // [rad] // Coefficients to compute precession angle // The actual unit of the coefficients are [rad/century^i], where i is the index of the array - c_zeta_rad_[0] = 2306.218100 * math::arcsec_to_rad; // [rad/century] - c_zeta_rad_[1] = 0.301880 * math::arcsec_to_rad; // [rad/century^2] - c_zeta_rad_[2] = 0.017998 * math::arcsec_to_rad; // [rad/century^3] + c_zeta_rad_[0] = 2306.218100 * s2e::math::arcsec_to_rad; // [rad/century] + c_zeta_rad_[1] = 0.301880 * s2e::math::arcsec_to_rad; // [rad/century^2] + c_zeta_rad_[2] = 0.017998 * s2e::math::arcsec_to_rad; // [rad/century^3] - c_theta_rad_[0] = 2004.310900 * math::arcsec_to_rad; // [rad/century] - c_theta_rad_[1] = -0.426650 * math::arcsec_to_rad; // [rad/century^2] - c_theta_rad_[2] = -0.041833 * math::arcsec_to_rad; // [rad/century^3] + c_theta_rad_[0] = 2004.310900 * s2e::math::arcsec_to_rad; // [rad/century] + c_theta_rad_[1] = -0.426650 * s2e::math::arcsec_to_rad; // [rad/century^2] + c_theta_rad_[2] = -0.041833 * s2e::math::arcsec_to_rad; // [rad/century^3] - c_z_rad_[0] = 2306.218100 * math::arcsec_to_rad; // [rad/century] - c_z_rad_[1] = 1.094680 * math::arcsec_to_rad; // [rad/century^2] - c_z_rad_[2] = 0.018203 * math::arcsec_to_rad; // [rad/century^3] + c_z_rad_[0] = 2306.218100 * s2e::math::arcsec_to_rad; // [rad/century] + c_z_rad_[1] = 1.094680 * s2e::math::arcsec_to_rad; // [rad/century^2] + c_z_rad_[2] = 0.018203 * s2e::math::arcsec_to_rad; // [rad/century^3] } else { // If the rotation mode is neither Simple nor Full, disable the rotation calculation and make the DCM a unit matrix - dcm_j2000_to_ecef_ = math::MakeIdentityMatrix<3>(); + dcm_j2000_to_ecef_ = s2e::math::MakeIdentityMatrix<3>(); } } @@ -125,10 +125,10 @@ void EarthRotation::Update(const double julian_date) { terrestrial_time_julian_century[i + 1] = terrestrial_time_julian_century[i] * terrestrial_time_julian_century[0]; } - math::Matrix<3, 3> dcm_precession; - math::Matrix<3, 3> dcm_nutation; - math::Matrix<3, 3> dcm_rotation; - math::Matrix<3, 3> dcm_polar_motion; + s2e::math::Matrix<3, 3> dcm_precession; + s2e::math::Matrix<3, 3> dcm_nutation; + s2e::math::Matrix<3, 3> dcm_rotation; + s2e::math::Matrix<3, 3> dcm_polar_motion; // Nutation + Precession dcm_precession = Precession(terrestrial_time_julian_century); dcm_nutation = Nutation(terrestrial_time_julian_century); // epsilon_rad_, d_epsilon_rad_, d_psi_rad_ are updated in this procedure @@ -154,9 +154,9 @@ void EarthRotation::Update(const double julian_date) { } } -math::Matrix<3, 3> EarthRotation::AxialRotation(const double gast_rad) { return math::MakeRotationMatrixZ(gast_rad); } +s2e::math::Matrix<3, 3> EarthRotation::AxialRotation(const double gast_rad) { return s2e::math::MakeRotationMatrixZ(gast_rad); } -math::Matrix<3, 3> EarthRotation::Nutation(const double (&t_tt_century)[4]) { +s2e::math::Matrix<3, 3> EarthRotation::Nutation(const double (&t_tt_century)[4]) { // Mean obliquity of the ecliptic epsilon_rad_ = c_epsilon_rad_[0]; for (int i = 0; i < 3; i++) { @@ -208,17 +208,17 @@ math::Matrix<3, 3> EarthRotation::Nutation(const double (&t_tt_century)[4]) { c_d_epsilon_rad_[7] * cos(2 * l_rad + lm_rad) + c_d_epsilon_rad_[8] * cos(2 * ld_rad - ls_rad); double epsi_mod_rad = epsilon_rad_ + d_epsilon_rad_; - math::Matrix<3, 3> x_epsi_1st = math::MakeRotationMatrixX(epsilon_rad_); - math::Matrix<3, 3> z_d_psi = math::MakeRotationMatrixZ(-d_psi_rad_); - math::Matrix<3, 3> x_epsi_2nd = math::MakeRotationMatrixX(-epsi_mod_rad); + s2e::math::Matrix<3, 3> x_epsi_1st = s2e::math::MakeRotationMatrixX(epsilon_rad_); + s2e::math::Matrix<3, 3> z_d_psi = s2e::math::MakeRotationMatrixZ(-d_psi_rad_); + s2e::math::Matrix<3, 3> x_epsi_2nd = s2e::math::MakeRotationMatrixX(-epsi_mod_rad); - math::Matrix<3, 3> dcm_nutation; + s2e::math::Matrix<3, 3> dcm_nutation; dcm_nutation = x_epsi_2nd * z_d_psi * x_epsi_1st; return dcm_nutation; } -math::Matrix<3, 3> EarthRotation::Precession(const double (&t_tt_century)[4]) { +s2e::math::Matrix<3, 3> EarthRotation::Precession(const double (&t_tt_century)[4]) { // Compute precession angles(zeta, theta, z) double zeta_rad = 0.0; for (int i = 0; i < 3; i++) { @@ -234,18 +234,18 @@ math::Matrix<3, 3> EarthRotation::Precession(const double (&t_tt_century)[4]) { } // Develop transformation matrix - math::Matrix<3, 3> z_zeta = math::MakeRotationMatrixZ(-zeta_rad); - math::Matrix<3, 3> y_theta = math::MakeRotationMatrixY(theta_rad); - math::Matrix<3, 3> z_z = math::MakeRotationMatrixZ(-z_rad); + s2e::math::Matrix<3, 3> z_zeta = s2e::math::MakeRotationMatrixZ(-zeta_rad); + s2e::math::Matrix<3, 3> y_theta = s2e::math::MakeRotationMatrixY(theta_rad); + s2e::math::Matrix<3, 3> z_z = s2e::math::MakeRotationMatrixZ(-z_rad); - math::Matrix<3, 3> dcm_precession; + s2e::math::Matrix<3, 3> dcm_precession; dcm_precession = z_z * y_theta * z_zeta; return dcm_precession; } -math::Matrix<3, 3> EarthRotation::PolarMotion(const double x_p, const double y_p) { - math::Matrix<3, 3> dcm_polar_motion; +s2e::math::Matrix<3, 3> EarthRotation::PolarMotion(const double x_p, const double y_p) { + s2e::math::Matrix<3, 3> dcm_polar_motion; dcm_polar_motion[0][0] = 1.0; dcm_polar_motion[0][1] = 0.0; diff --git a/src/environment/global/earth_rotation.hpp b/src/environment/global/earth_rotation.hpp index c12af3f2f..f4c7f7df7 100644 --- a/src/environment/global/earth_rotation.hpp +++ b/src/environment/global/earth_rotation.hpp @@ -45,20 +45,20 @@ class EarthRotation { * @fn GetDcmJ2000ToEcef * @brief Return the DCM between J2000 inertial frame and the Earth Centered Earth Fixed frame */ - inline const math::Matrix<3, 3> GetDcmJ2000ToEcef() const { return dcm_j2000_to_ecef_; }; + inline const s2e::math::Matrix<3, 3> GetDcmJ2000ToEcef() const { return dcm_j2000_to_ecef_; }; /** * @fn GetDcmTemeToEcef * @brief Return the DCM between TEME (Inertial frame used in SGP4) and the Earth Centered Earth Fixed frame */ - inline const math::Matrix<3, 3> GetDcmTemeToEcef() const { return dcm_teme_to_ecef_; }; + inline const s2e::math::Matrix<3, 3> GetDcmTemeToEcef() const { return dcm_teme_to_ecef_; }; private: double d_psi_rad_; //!< Nutation in obliquity [rad] double d_epsilon_rad_; //!< Nutation in longitude [rad] double epsilon_rad_; //!< Mean obliquity of the ecliptic [rad] - math::Matrix<3, 3> dcm_j2000_to_ecef_; //!< Direction Cosine Matrix J2000 to ECEF - math::Matrix<3, 3> dcm_teme_to_ecef_; //!< Direction Cosine Matrix TEME to ECEF + s2e::math::Matrix<3, 3> dcm_j2000_to_ecef_; //!< Direction Cosine Matrix J2000 to ECEF + s2e::math::Matrix<3, 3> dcm_teme_to_ecef_; //!< Direction Cosine Matrix TEME to ECEF EarthRotationMode rotation_mode_; //!< Designation of dynamics model // Definitions of coefficients @@ -94,7 +94,7 @@ class EarthRotation { * @param [in] gast_rad: Greenwich 'Apparent' Sidereal Time [rad] * @return Rotation matrix */ - math::Matrix<3, 3> AxialRotation(const double gast_rad); + s2e::math::Matrix<3, 3> AxialRotation(const double gast_rad); /** * @fn Nutation @@ -102,7 +102,7 @@ class EarthRotation { * @param [in] t_tt_century: nth power of julian century for terrestrial time * @return Rotation matrix */ - math::Matrix<3, 3> Nutation(const double (&t_tt_century)[4]); + s2e::math::Matrix<3, 3> Nutation(const double (&t_tt_century)[4]); /** * @fn Precession @@ -110,14 +110,14 @@ class EarthRotation { * @param [in] t_tt_century: nth power of julian century for terrestrial time * @return Rotation matrix */ - math::Matrix<3, 3> Precession(const double (&t_tt_century)[4]); + s2e::math::Matrix<3, 3> Precession(const double (&t_tt_century)[4]); /** * @fn PolarMotion * @brief Calculate movement of the coordinate axes due to Polar Motion * @note Currently, this function is not used. */ - math::Matrix<3, 3> PolarMotion(const double x_p, const double y_p); + s2e::math::Matrix<3, 3> PolarMotion(const double x_p, const double y_p); }; EarthRotationMode ConvertEarthRotationMode(const std::string mode); diff --git a/src/environment/global/gnss_satellites.cpp b/src/environment/global/gnss_satellites.cpp index 21749c9aa..c2a844d7a 100644 --- a/src/environment/global/gnss_satellites.cpp +++ b/src/environment/global/gnss_satellites.cpp @@ -45,7 +45,7 @@ void GnssSatellites::Initialize(const std::vector& sp3_files, con // Initialize clock std::vector temp; temp.assign(kNumberOfInterpolation, -1.0); - clock_.assign(number_of_calculated_gnss_satellites_, math::Interpolation(temp, temp)); + clock_.assign(number_of_calculated_gnss_satellites_, s2e::math::Interpolation(temp, temp)); // Initialize interpolation for (size_t i = 0; i < kNumberOfInterpolation; i++) { @@ -74,8 +74,8 @@ void GnssSatellites::Update(const SimulationTime& simulation_time) { return; } -math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_id, const time_system::EpochTime time) const { - if (gnss_satellite_id > number_of_calculated_gnss_satellites_) return math::Vector<3>(0.0); +s2e::math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_id, const time_system::EpochTime time) const { + if (gnss_satellite_id > number_of_calculated_gnss_satellites_) return s2e::math::Vector<3>(0.0); time_system::EpochTime target_time; @@ -86,10 +86,10 @@ math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_i } double diff_s = target_time.GetTimeWithFraction_s() - reference_time_.GetTimeWithFraction_s(); - if (diff_s < 0.0 || diff_s > 1e6) return math::Vector<3>(0.0); + if (diff_s < 0.0 || diff_s > 1e6) return s2e::math::Vector<3>(0.0); const double kOrbitalPeriodCorrection_s = 24 * 60 * 60 * 1.003; // See http://acc.igs.org/orbits/orbit-interp_gpssoln03.pdf - return orbit_[gnss_satellite_id].CalcPositionWithTrigonometric(diff_s, math::tau / kOrbitalPeriodCorrection_s); + return orbit_[gnss_satellite_id].CalcPositionWithTrigonometric(diff_s, s2e::math::tau / kOrbitalPeriodCorrection_s); } double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const time_system::EpochTime time) const { @@ -131,7 +131,7 @@ bool GnssSatellites::UpdateInterpolationInformation() { for (size_t gnss_id = 0; gnss_id < number_of_calculated_gnss_satellites_; gnss_id++) { time_system::EpochTime sp3_time = time_system::EpochTime(sp3_file.GetEpochData(reference_interpolation_id_)); double time_diff_s = sp3_time.GetTimeWithFraction_s() - reference_time_.GetTimeWithFraction_s(); - math::Vector<3> sp3_position_m = 1000.0 * sp3_file.GetSatellitePosition_km(reference_interpolation_id_, gnss_id); + s2e::math::Vector<3> sp3_position_m = 1000.0 * sp3_file.GetSatellitePosition_km(reference_interpolation_id_, gnss_id); orbit_[gnss_id].PushAndPopData(time_diff_s, sp3_position_m); clock_[gnss_id].PushAndPopData(time_diff_s, sp3_file.GetSatelliteClockOffset(reference_interpolation_id_, gnss_id)); diff --git a/src/environment/global/gnss_satellites.hpp b/src/environment/global/gnss_satellites.hpp index 77508f73b..bbb4aaac3 100644 --- a/src/environment/global/gnss_satellites.hpp +++ b/src/environment/global/gnss_satellites.hpp @@ -75,7 +75,7 @@ class GnssSatellites : public ILoggable { */ void Update(const SimulationTime& simulation_time); - inline math::Vector<3> GetPosition_eci_m(const size_t gnss_satellite_id) const { + inline s2e::math::Vector<3> GetPosition_eci_m(const size_t gnss_satellite_id) const { // TODO: Add target time for earth rotation calculation return earth_rotation_.GetDcmJ2000ToEcef().Transpose() * GetPosition_ecef_m(gnss_satellite_id); } @@ -87,7 +87,7 @@ class GnssSatellites : public ILoggable { * @param [in] time: Target time to get the GNSS satellite. When the argument is not set, the last updated time is used for the calculation. * @return GNSS satellite position at ECEF frame at the time. Or return zero vector when the arguments are out of range. */ - math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, const time_system::EpochTime time = time_system::EpochTime(0, 0.0)) const; + s2e::math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, const time_system::EpochTime time = time_system::EpochTime(0, 0.0)) const; /** * @fn GetGetClock_s @@ -121,7 +121,7 @@ class GnssSatellites : public ILoggable { time_system::EpochTime current_epoch_time_; //!< The last updated time std::vector orbit_; //!< GNSS satellite orbit with interpolation - std::vector clock_; //!< GNSS satellite clock offset with interpolation + std::vector clock_; //!< GNSS satellite clock offset with interpolation // References const EarthRotation& earth_rotation_; //!< Earth rotation diff --git a/src/environment/global/hipparcos_catalogue.cpp b/src/environment/global/hipparcos_catalogue.cpp index 7ef29494b..70e9fca25 100644 --- a/src/environment/global/hipparcos_catalogue.cpp +++ b/src/environment/global/hipparcos_catalogue.cpp @@ -50,10 +50,10 @@ bool HipparcosCatalogue::ReadContents(const std::string& file_name, const char d return true; } -math::Vector<3> HipparcosCatalogue::GetStarDirection_i(size_t rank) const { - math::Vector<3> direction_i; - double ra_rad = GetRightAscension_deg(rank) * math::deg_to_rad; - double de_rad = GetDeclination_deg(rank) * math::deg_to_rad; +s2e::math::Vector<3> HipparcosCatalogue::GetStarDirection_i(size_t rank) const { + s2e::math::Vector<3> direction_i; + double ra_rad = GetRightAscension_deg(rank) * s2e::math::deg_to_rad; + double de_rad = GetDeclination_deg(rank) * s2e::math::deg_to_rad; direction_i[0] = cos(ra_rad) * cos(de_rad); direction_i[1] = sin(ra_rad) * cos(de_rad); @@ -62,9 +62,9 @@ math::Vector<3> HipparcosCatalogue::GetStarDirection_i(size_t rank) const { return direction_i; } -math::Vector<3> HipparcosCatalogue::GetStarDirection_b(size_t rank, math::Quaternion quaternion_i2b) const { - math::Vector<3> direction_i; - math::Vector<3> direction_b; +s2e::math::Vector<3> HipparcosCatalogue::GetStarDirection_b(size_t rank, s2e::math::Quaternion quaternion_i2b) const { + s2e::math::Vector<3> direction_i; + s2e::math::Vector<3> direction_b; direction_i = GetStarDirection_i(rank); direction_b = quaternion_i2b.FrameConversion(direction_i); diff --git a/src/environment/global/hipparcos_catalogue.hpp b/src/environment/global/hipparcos_catalogue.hpp index d4a6afdd4..3114fdb3a 100644 --- a/src/environment/global/hipparcos_catalogue.hpp +++ b/src/environment/global/hipparcos_catalogue.hpp @@ -82,14 +82,14 @@ class HipparcosCatalogue : public ILoggable { *@brief Return direction vector of a star in the inertial frame *@param [in] rank: Rank of star magnitude in read catalogue */ - math::Vector<3> GetStarDirection_i(size_t rank) const; + s2e::math::Vector<3> GetStarDirection_i(size_t rank) const; /** *@fn GetStarDir_b *@brief Return direction vector of a star in the body-fixed frame *@param [in] rank: Rank of star magnitude in read catalogue *@param [in] quaternion_i2b: Quaternion from the inertial frame to the body-fixed frame */ - math::Vector<3> GetStarDirection_b(size_t rank, math::Quaternion quaternion_i2b) const; + s2e::math::Vector<3> GetStarDirection_b(size_t rank, s2e::math::Quaternion quaternion_i2b) const; // Override ILoggable /** diff --git a/src/environment/global/moon_rotation.cpp b/src/environment/global/moon_rotation.cpp index 1b8aea98d..75abc2e5b 100644 --- a/src/environment/global/moon_rotation.cpp +++ b/src/environment/global/moon_rotation.cpp @@ -12,13 +12,13 @@ MoonRotation::MoonRotation(const CelestialInformation& celestial_information, MoonRotationMode mode) : mode_(mode), celestial_information_(celestial_information) { - dcm_j2000_to_mcmf_ = math::MakeIdentityMatrix<3>(); + dcm_j2000_to_mcmf_ = s2e::math::MakeIdentityMatrix<3>(); } void MoonRotation::Update(const SimulationTime& simulation_time) { if (mode_ == MoonRotationMode::kSimple) { - math::Vector<3> moon_position_eci_m = celestial_information_.GetPositionFromSelectedBody_i_m("MOON", "EARTH"); - math::Vector<3> moon_velocity_eci_m_s = celestial_information_.GetVelocityFromSelectedBody_i_m_s("MOON", "EARTH"); + s2e::math::Vector<3> moon_position_eci_m = celestial_information_.GetPositionFromSelectedBody_i_m("MOON", "EARTH"); + s2e::math::Vector<3> moon_velocity_eci_m_s = celestial_information_.GetVelocityFromSelectedBody_i_m_s("MOON", "EARTH"); dcm_j2000_to_mcmf_ = planet_rotation::CalcDcmEciToPrincipalAxis(moon_position_eci_m, moon_velocity_eci_m_s); } else if (mode_ == MoonRotationMode::kIauMoon) { ConstSpiceChar from[] = "J2000"; @@ -32,7 +32,7 @@ void MoonRotation::Update(const SimulationTime& simulation_time) { } } } else { - dcm_j2000_to_mcmf_ = math::MakeIdentityMatrix<3>(); + dcm_j2000_to_mcmf_ = s2e::math::MakeIdentityMatrix<3>(); } } diff --git a/src/environment/global/moon_rotation.hpp b/src/environment/global/moon_rotation.hpp index fa9506757..5df9f247a 100644 --- a/src/environment/global/moon_rotation.hpp +++ b/src/environment/global/moon_rotation.hpp @@ -53,11 +53,11 @@ class MoonRotation { * @brief Return the DCM between J2000 inertial frame and the Moon Centered Moon Fixed frame * @note Because this is just a DCM, users need to consider the origin of the vector, which you want to convert with this matrix. */ - inline const math::Matrix<3, 3> GetDcmJ2000ToMcmf() const { return dcm_j2000_to_mcmf_; }; + inline const s2e::math::Matrix<3, 3> GetDcmJ2000ToMcmf() const { return dcm_j2000_to_mcmf_; }; private: MoonRotationMode mode_; //!< Rotation mode - math::Matrix<3, 3> dcm_j2000_to_mcmf_; //!< Direction Cosine Matrix J2000 to MCMF (Moon Centered Moon Fixed) + s2e::math::Matrix<3, 3> dcm_j2000_to_mcmf_; //!< Direction Cosine Matrix J2000 to MCMF (Moon Centered Moon Fixed) const CelestialInformation &celestial_information_; //!< Celestial Information to get moon orbit }; diff --git a/src/environment/local/atmosphere.cpp b/src/environment/local/atmosphere.cpp index b0792bca4..b46e855b2 100644 --- a/src/environment/local/atmosphere.cpp +++ b/src/environment/local/atmosphere.cpp @@ -66,7 +66,7 @@ double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const Orbit& manual_average_f107_, manual_ap_); } else if (model_ == "HARRIS_PRIESTER") { // Harris-Priester - math::Vector<3> sun_direction_eci = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("SUN").CalcNormalizedVector(); + s2e::math::Vector<3> sun_direction_eci = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("SUN").CalcNormalizedVector(); air_density_kg_m3_ = atmosphere::CalcAirDensityWithHarrisPriester_kg_m3(orbit.GetGeodeticPosition(), sun_direction_eci); } else { // No suitable model diff --git a/src/environment/local/geomagnetic_field.cpp b/src/environment/local/geomagnetic_field.cpp index d167ae9ea..971910b0a 100644 --- a/src/environment/local/geomagnetic_field.cpp +++ b/src/environment/local/geomagnetic_field.cpp @@ -23,7 +23,7 @@ GeomagneticField::GeomagneticField(const std::string igrf_file_name, const doubl } void GeomagneticField::CalcMagneticField(const double decimal_year, const double sidereal_day, const geodesy::GeodeticPosition position, - const math::Quaternion quaternion_i2b) { + const s2e::math::Quaternion quaternion_i2b) { if (!IsCalcEnabled) return; const double lat_rad = position.GetLatitude_rad(); @@ -40,8 +40,8 @@ void GeomagneticField::CalcMagneticField(const double decimal_year, const double } void GeomagneticField::AddNoise(double* magnetic_field_array_i_nT) { - static math::Vector<3> standard_deviation(random_walk_standard_deviation_nT_); - static math::Vector<3> limit(random_walk_limit_nT_); + static s2e::math::Vector<3> standard_deviation(random_walk_standard_deviation_nT_); + static s2e::math::Vector<3> limit(random_walk_limit_nT_); static RandomWalk<3> random_walk(0.1, standard_deviation, limit); static randomization::NormalRand white_noise(0.0, white_noise_standard_deviation_nT_, global_randomization.MakeSeed()); diff --git a/src/environment/local/geomagnetic_field.hpp b/src/environment/local/geomagnetic_field.hpp index f8c38b997..d00f46aa1 100644 --- a/src/environment/local/geomagnetic_field.hpp +++ b/src/environment/local/geomagnetic_field.hpp @@ -44,18 +44,18 @@ class GeomagneticField : public ILoggable { * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame */ void CalcMagneticField(const double decimal_year, const double sidereal_day, const geodesy::GeodeticPosition position, - const math::Quaternion quaternion_i2b); + const s2e::math::Quaternion quaternion_i2b); /** * @fn GetGeomagneticField_i_nT * @brief Return magnetic field vector in the inertial frame [nT] */ - inline math::Vector<3> GetGeomagneticField_i_nT() const { return magnetic_field_i_nT_; } + inline s2e::math::Vector<3> GetGeomagneticField_i_nT() const { return magnetic_field_i_nT_; } /** * @fn GetGeomagneticField_b_nT * @brief Return magnetic field vector in the body fixed frame [nT] */ - inline math::Vector<3> GetGeomagneticField_b_nT() const { return magnetic_field_b_nT_; } + inline s2e::math::Vector<3> GetGeomagneticField_b_nT() const { return magnetic_field_b_nT_; } // Override ILoggable /** @@ -70,8 +70,8 @@ class GeomagneticField : public ILoggable { virtual std::string GetLogValue() const; private: - math::Vector<3> magnetic_field_i_nT_; //!< Magnetic field vector at the inertial frame [nT] - math::Vector<3> magnetic_field_b_nT_; //!< Magnetic field vector at the spacecraft body fixed frame [nT] + s2e::math::Vector<3> magnetic_field_i_nT_; //!< Magnetic field vector at the inertial frame [nT] + s2e::math::Vector<3> magnetic_field_b_nT_; //!< Magnetic field vector at the spacecraft body fixed frame [nT] double random_walk_standard_deviation_nT_; //!< Standard deviation of Random Walk [nT] double random_walk_limit_nT_; //!< Limit of Random Walk [nT] double white_noise_standard_deviation_nT_; //!< Standard deviation of white noise [nT] diff --git a/src/environment/local/local_celestial_information.cpp b/src/environment/local/local_celestial_information.cpp index e73d23132..b34fcc6ed 100644 --- a/src/environment/local/local_celestial_information.cpp +++ b/src/environment/local/local_celestial_information.cpp @@ -43,11 +43,11 @@ LocalCelestialInformation::~LocalCelestialInformation() { delete[] celestial_body_velocity_from_spacecraft_b_m_s_; } -void LocalCelestialInformation::UpdateAllObjectsInformation(const math::Vector<3> spacecraft_position_from_center_i_m, - const math::Vector<3> spacecraft_velocity_from_center_i_m_s, - const math::Quaternion quaternion_i2b, - const math::Vector<3> spacecraft_angular_velocity_rad_s) { - math::Vector<3> celestial_body_position_i_m, celestial_body_velocity_i_m_s; +void LocalCelestialInformation::UpdateAllObjectsInformation(const s2e::math::Vector<3> spacecraft_position_from_center_i_m, + const s2e::math::Vector<3> spacecraft_velocity_from_center_i_m_s, + const s2e::math::Quaternion quaternion_i2b, + const s2e::math::Vector<3> spacecraft_angular_velocity_rad_s) { + s2e::math::Vector<3> celestial_body_position_i_m, celestial_body_velocity_i_m_s; for (int i = 0; i < global_celestial_information_->GetNumberOfSelectedBodies(); i++) { celestial_body_position_i_m = global_celestial_information_->GetPositionFromCenter_i_m(i); celestial_body_velocity_i_m_s = global_celestial_information_->GetVelocityFromCenter_i_m_s(i); @@ -62,8 +62,8 @@ void LocalCelestialInformation::UpdateAllObjectsInformation(const math::Vector<3 return; } -void LocalCelestialInformation::CalcAllPosVel_b(const math::Quaternion quaternion_i2b, const math::Vector<3> spacecraft_angular_velocity_rad_s) { - math::Vector<3> celestial_body_position_i_m, celestial_body_velocity_i_m_s; +void LocalCelestialInformation::CalcAllPosVel_b(const s2e::math::Quaternion quaternion_i2b, const s2e::math::Vector<3> spacecraft_angular_velocity_rad_s) { + s2e::math::Vector<3> celestial_body_position_i_m, celestial_body_velocity_i_m_s; double r_buf1_i[3], velocity_buf1_i[3], r_buf1_b[3], velocity_buf1_b[3]; double r_buf2_i[3], velocity_buf2_i[3], r_buf2_b[3], velocity_buf2_b[3]; for (int i = 0; i < global_celestial_information_->GetNumberOfSelectedBodies(); i++) { @@ -89,50 +89,50 @@ void LocalCelestialInformation::CalcAllPosVel_b(const math::Quaternion quaternio } } -void LocalCelestialInformation::ConvertInertialToBody(const double* input_i, double* output_b, math::Quaternion quaternion_i2b) { - math::Vector<3> temp_i; +void LocalCelestialInformation::ConvertInertialToBody(const double* input_i, double* output_b, s2e::math::Quaternion quaternion_i2b) { + s2e::math::Vector<3> temp_i; for (int i = 0; i < 3; i++) { temp_i[i] = input_i[i]; } - math::Vector<3> temp_b = quaternion_i2b.FrameConversion(temp_i); + s2e::math::Vector<3> temp_b = quaternion_i2b.FrameConversion(temp_i); for (int i = 0; i < 3; i++) { output_b[i] = temp_b[i]; } } void LocalCelestialInformation::ConvertVelocityInertialToBody(const double* position_i, const double* velocity_i, double* velocity_b, - const math::Quaternion quaternion_i2b, const math::Vector<3> angular_velocity_b) { + const s2e::math::Quaternion quaternion_i2b, const s2e::math::Vector<3> angular_velocity_b) { // copy input vector - math::Vector<3> vi; + s2e::math::Vector<3> vi; for (int i = 0; i < 3; i++) { vi[i] = velocity_i[i]; } - math::Vector<3> ri; + s2e::math::Vector<3> ri; for (int i = 0; i < 3; i++) { ri[i] = position_i[i]; } // convert body rate vector into that in inertial coordinate - math::Vector<3> wb; + s2e::math::Vector<3> wb; for (int i = 0; i < 3; i++) { wb[i] = angular_velocity_b[i]; } // compute cross term wxr - math::Vector<3> wxposition_i = OuterProduct(wb, ri); + s2e::math::Vector<3> wxposition_i = OuterProduct(wb, ri); // compute dr/dt + wxr for (int i = 0; i < 3; i++) { vi[i] = vi[i] - wxposition_i[i]; } // convert vector in inertial coordinate into that in body coordinate - math::Vector<3> temp_b = quaternion_i2b.FrameConversion(vi); + s2e::math::Vector<3> temp_b = quaternion_i2b.FrameConversion(vi); for (int i = 0; i < 3; i++) { velocity_b[i] = temp_b[i]; } } -math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_i_m(const char* body_name) const { - math::Vector<3> position; +s2e::math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_i_m(const char* body_name) const { + s2e::math::Vector<3> position; int index = 0; index = global_celestial_information_->CalcBodyIdFromName(body_name); for (int i = 0; i < 3; i++) { @@ -141,14 +141,14 @@ math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_i_m(const c return position; } -math::Vector<3> LocalCelestialInformation::GetCenterBodyPositionFromSpacecraft_i_m() const { +s2e::math::Vector<3> LocalCelestialInformation::GetCenterBodyPositionFromSpacecraft_i_m() const { std::string body_name = global_celestial_information_->GetCenterBodyName(); - math::Vector<3> position = GetPositionFromSpacecraft_i_m(body_name.c_str()); + s2e::math::Vector<3> position = GetPositionFromSpacecraft_i_m(body_name.c_str()); return position; } -math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_b_m(const char* body_name) const { - math::Vector<3> position; +s2e::math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_b_m(const char* body_name) const { + s2e::math::Vector<3> position; int index = 0; index = global_celestial_information_->CalcBodyIdFromName(body_name); for (int i = 0; i < 3; i++) { @@ -157,9 +157,9 @@ math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_b_m(const c return position; } -math::Vector<3> LocalCelestialInformation::GetCenterBodyPositionFromSpacecraft_b_m(void) const { +s2e::math::Vector<3> LocalCelestialInformation::GetCenterBodyPositionFromSpacecraft_b_m(void) const { std::string body_name = global_celestial_information_->GetCenterBodyName(); - math::Vector<3> position = GetPositionFromSpacecraft_b_m(body_name.c_str()); + s2e::math::Vector<3> position = GetPositionFromSpacecraft_b_m(body_name.c_str()); return position; } diff --git a/src/environment/local/local_celestial_information.hpp b/src/environment/local/local_celestial_information.hpp index dc10ab302..ec3a6cbb3 100644 --- a/src/environment/local/local_celestial_information.hpp +++ b/src/environment/local/local_celestial_information.hpp @@ -34,33 +34,33 @@ class LocalCelestialInformation : public ILoggable { * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame * @param [in] spacecraft_angular_velocity_rad_s: Spacecraft angular velocity with respect to the inertial frame [rad/s] */ - void UpdateAllObjectsInformation(const math::Vector<3> spacecraft_position_from_center_i_m, - const math::Vector<3> spacecraft_velocity_from_center_i_m_s, const math::Quaternion quaternion_i2b, - const math::Vector<3> spacecraft_angular_velocity_rad_s); + void UpdateAllObjectsInformation(const s2e::math::Vector<3> spacecraft_position_from_center_i_m, + const s2e::math::Vector<3> spacecraft_velocity_from_center_i_m_s, const s2e::math::Quaternion quaternion_i2b, + const s2e::math::Vector<3> spacecraft_angular_velocity_rad_s); /** * @fn GetPositionFromSpacecraft_i_m * @brief Return position of a selected body (Origin: Spacecraft, Frame: Inertial frame) * @param [in] body_name Celestial body name */ - math::Vector<3> GetPositionFromSpacecraft_i_m(const char* body_name) const; + s2e::math::Vector<3> GetPositionFromSpacecraft_i_m(const char* body_name) const; /** * @fn GetCenterBodyPositionFromSpacecraft_i_m * @brief Return position of the center body (Origin: Spacecraft, Frame: Inertial frame) */ - math::Vector<3> GetCenterBodyPositionFromSpacecraft_i_m(void) const; + s2e::math::Vector<3> GetCenterBodyPositionFromSpacecraft_i_m(void) const; /** * @fn GetPositionFromSpacecraft_b_m * @brief Return position of a selected body (Origin: Spacecraft, Frame: Body fixed frame) * @param [in] body_name Celestial body name */ - math::Vector<3> GetPositionFromSpacecraft_b_m(const char* body_name) const; + s2e::math::Vector<3> GetPositionFromSpacecraft_b_m(const char* body_name) const; /** * @fn GetCenterBodyPositionFromSpacecraft_b_m * @brief Return position of the center body (Origin: Spacecraft, Frame: Body fixed frame) */ - math::Vector<3> GetCenterBodyPositionFromSpacecraft_b_m(void) const; + s2e::math::Vector<3> GetCenterBodyPositionFromSpacecraft_b_m(void) const; /** * @fn GetGlobalInfo @@ -96,7 +96,7 @@ class LocalCelestialInformation : public ILoggable { * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame * @param [in] spacecraft_angular_velocity_rad_s: Spacecraft angular velocity with respect to the inertial frame [rad/s] */ - void CalcAllPosVel_b(const math::Quaternion quaternion_i2b, const math::Vector<3> spacecraft_angular_velocity_rad_s); + void CalcAllPosVel_b(const s2e::math::Quaternion quaternion_i2b, const s2e::math::Vector<3> spacecraft_angular_velocity_rad_s); /** * @fn ConvertInertialToBody @@ -105,7 +105,7 @@ class LocalCelestialInformation : public ILoggable { * @param [out] output_b: Output vector in the body fixed frame * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame */ - void ConvertInertialToBody(const double* input_i, double* output_b, const math::Quaternion quaternion_i2b); + void ConvertInertialToBody(const double* input_i, double* output_b, const s2e::math::Quaternion quaternion_i2b); /** * @fn ConvertVelocityInertialToBody @@ -116,8 +116,8 @@ class LocalCelestialInformation : public ILoggable { * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame * @param [in] angular_velocity_b: Spacecraft angular velocity with respect to the inertial frame [rad/s] */ - void ConvertVelocityInertialToBody(const double* position_i, const double* velocity_i, double* velocity_b, const math::Quaternion quaternion_i2b, - const math::Vector<3> angular_velocity_b); + void ConvertVelocityInertialToBody(const double* position_i, const double* velocity_i, double* velocity_b, const s2e::math::Quaternion quaternion_i2b, + const s2e::math::Vector<3> angular_velocity_b); }; #endif // S2E_ENVIRONMENT_LOCAL_LOCAL_CELESTIAL_INFORMATION_HPP_ diff --git a/src/environment/local/solar_radiation_pressure_environment.cpp b/src/environment/local/solar_radiation_pressure_environment.cpp index 181e1ea7a..abda33801 100644 --- a/src/environment/local/solar_radiation_pressure_environment.cpp +++ b/src/environment/local/solar_radiation_pressure_environment.cpp @@ -31,7 +31,7 @@ void SolarRadiationPressureEnvironment::UpdateAllStates() { } void SolarRadiationPressureEnvironment::UpdatePressure() { - const math::Vector<3> r_sc2sun_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); + const s2e::math::Vector<3> r_sc2sun_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); const double distance_sat_to_sun = r_sc2sun_eci.CalcNorm(); solar_radiation_pressure_N_m2_ = solar_constant_W_m2_ / environment::speed_of_light_m_s / pow(distance_sat_to_sun / environment::astronomical_unit_m, 2.0); @@ -61,8 +61,8 @@ void SolarRadiationPressureEnvironment::CalcShadowCoefficient(std::string shadow return; } - const math::Vector<3> r_sc2sun_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); - const math::Vector<3> r_sc2source_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m(shadow_source_name.c_str()); + const s2e::math::Vector<3> r_sc2sun_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); + const s2e::math::Vector<3> r_sc2source_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m(shadow_source_name.c_str()); const double shadow_source_radius_m = local_celestial_information_->GetGlobalInformation().GetMeanRadiusFromName_m(shadow_source_name.c_str()); @@ -71,7 +71,7 @@ void SolarRadiationPressureEnvironment::CalcShadowCoefficient(std::string shadow const double sd_source = asin(shadow_source_radius_m / r_sc2source_eci.CalcNorm()); // Apparent radius of the shadow source // Angle of deviation from shadow source center to sun center - math::Vector<3> r_source2sun_eci = r_sc2sun_eci - r_sc2source_eci; + s2e::math::Vector<3> r_source2sun_eci = r_sc2sun_eci - r_sc2source_eci; const double delta = acos(InnerProduct(r_sc2source_eci, r_sc2sun_eci - r_sc2source_eci) / r_sc2source_eci.CalcNorm() / r_source2sun_eci.CalcNorm()); // The angle between the center of the sun and the common chord const double x = (delta * delta + sd_sun * sd_sun - sd_source * sd_source) / (2.0 * delta); @@ -91,7 +91,7 @@ void SolarRadiationPressureEnvironment::CalcShadowCoefficient(std::string shadow } else if (fabs(a - b) <= c && c <= (a + b)) // spacecraft is in penumbra { double A = a * a * acos(x / a) + b * b * acos((c - x) / b) - c * y; // The area of the occulted segment of the apparent solar disk - shadow_coefficient_ *= 1.0 - A / (math::pi * a * a); + shadow_coefficient_ *= 1.0 - A / (s2e::math::pi * a * a); } else { // no occultation takes place if (c < (a + b)) { std::cout << "[Error SRP Environment]: The calculation error was occurred at the shadow calculation." << std::endl; diff --git a/src/logger/log_utility.hpp b/src/logger/log_utility.hpp index 7eba4b767..96ccf3c31 100644 --- a/src/logger/log_utility.hpp +++ b/src/logger/log_utility.hpp @@ -35,7 +35,7 @@ inline std::string WriteScalar(const std::string name, const std::string unit); * @param [in] precision: precision for the value (number of digit) */ template -inline std::string WriteVector(const math::Vector vector, const int precision = 6); +inline std::string WriteVector(const s2e::math::Vector vector, const int precision = 6); /** * @fn WriteVector * @brief Write header for vector value @@ -52,7 +52,7 @@ inline std::string WriteVector(const std::string name, const std::string frame, * @param [in] matrix: matrix value */ template -inline std::string WriteMatrix(const math::Matrix matrix, const int precision = 6); +inline std::string WriteMatrix(const s2e::math::Matrix matrix, const int precision = 6); /** * @fn WriteMatrix * @brief Write header for matrix value @@ -70,7 +70,7 @@ inline std::string WriteMatrix(const std::string name, const std::string frame, * @brief Write quaternion value * @param [in] quaternion: Quaternion */ -inline std::string WriteQuaternion(const math::Quaternion quaternion, const int precision = 6); +inline std::string WriteQuaternion(const s2e::math::Quaternion quaternion, const int precision = 6); /** * @fn WriteQuaternion * @brief Write header for quaternion @@ -91,7 +91,7 @@ std::string WriteScalar(const T scalar, const int precision) { std::string WriteScalar(const std::string name, const std::string unit) { return name + "[" + unit + "],"; } template -std::string WriteVector(const math::Vector vector, const int precision) { +std::string WriteVector(const s2e::math::Vector vector, const int precision) { std::stringstream str_tmp; for (size_t n = 0; n < NUM; n++) { @@ -115,7 +115,7 @@ std::string WriteVector(const std::string name, const std::string frame, const s } template -std::string WriteMatrix(const math::Matrix matrix, const int precision) { +std::string WriteMatrix(const s2e::math::Matrix matrix, const int precision) { std::stringstream str_tmp; for (size_t n = 0; n < ROW; n++) { @@ -138,7 +138,7 @@ std::string WriteMatrix(const std::string name, const std::string frame, const s return str_tmp.str(); } -std::string WriteQuaternion(const math::Quaternion quaternion, const int precision) { +std::string WriteQuaternion(const s2e::math::Quaternion quaternion, const int precision) { std::stringstream str_tmp; for (size_t i = 0; i < 4; i++) { diff --git a/src/math_physics/atmosphere/harris_priester_model.cpp b/src/math_physics/atmosphere/harris_priester_model.cpp index 3f2b022b3..ce88b5a54 100644 --- a/src/math_physics/atmosphere/harris_priester_model.cpp +++ b/src/math_physics/atmosphere/harris_priester_model.cpp @@ -28,27 +28,27 @@ double CalcScaleHeight_km(const std::map::const_iterator density */ double CalcApexDensity_g_km3(const std::map::const_iterator density_itr, const double altitude_km); -double CalcAirDensityWithHarrisPriester_kg_m3(const geodesy::GeodeticPosition geodetic_position, const math::Vector<3> sun_direction_eci, +double CalcAirDensityWithHarrisPriester_kg_m3(const geodesy::GeodeticPosition geodetic_position, const s2e::math::Vector<3> sun_direction_eci, const double f10_7, const double exponent_parameter) { // Altitude double altitude_km = geodetic_position.GetAltitude_m() / 1000.0; // Position - math::Vector<3> position_ecef_m = geodetic_position.CalcEcefPosition(); + s2e::math::Vector<3> position_ecef_m = geodetic_position.CalcEcefPosition(); // Phi: angle between the satellite position and apex of the diurnal bulge double sun_ra_rad; //!< Right ascension of the sun phi double sun_dec_rad; //!< Declination of the sun theta sun_ra_rad = atan2(sun_direction_eci[1], sun_direction_eci[0]); sun_dec_rad = atan2(sun_direction_eci[2], sqrt(sun_direction_eci[0] * sun_direction_eci[0] + sun_direction_eci[1] * sun_direction_eci[1])); - math::Vector<3> apex_direction; - const double lag_angle_rad = 30.0 * math::deg_to_rad; + s2e::math::Vector<3> apex_direction; + const double lag_angle_rad = 30.0 * s2e::math::deg_to_rad; double apex_ra_rad = sun_ra_rad + lag_angle_rad; apex_direction[0] = cos(sun_dec_rad) * cos(apex_ra_rad); apex_direction[1] = cos(sun_dec_rad) * sin(apex_ra_rad); apex_direction[2] = sin(sun_dec_rad); - double beta_rad = math::InnerProduct(position_ecef_m.CalcNormalizedVector(), apex_direction); + double beta_rad = s2e::math::InnerProduct(position_ecef_m.CalcNormalizedVector(), apex_direction); double cos_phi = pow(0.5 + beta_rad / 2.0, exponent_parameter / 2.0); // Find density coefficients from altitude diff --git a/src/math_physics/atmosphere/harris_priester_model.hpp b/src/math_physics/atmosphere/harris_priester_model.hpp index f9d8ffcc5..fb08c4977 100644 --- a/src/math_physics/atmosphere/harris_priester_model.hpp +++ b/src/math_physics/atmosphere/harris_priester_model.hpp @@ -20,7 +20,7 @@ namespace atmosphere { * @param [in] exponent_parameter: n in the equation. n=2 for low inclination orbit and n=6 for polar orbit. * @return Atmospheric density [kg/m^3] */ -double CalcAirDensityWithHarrisPriester_kg_m3(const geodesy::GeodeticPosition geodetic_position, const math::Vector<3> sun_direction_eci, +double CalcAirDensityWithHarrisPriester_kg_m3(const geodesy::GeodeticPosition geodetic_position, const s2e::math::Vector<3> sun_direction_eci, const double f10_7 = 100.0, const double exponent_parameter = 4); } // namespace atmosphere diff --git a/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp b/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp index c000ca49e..8401d233b 100644 --- a/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp +++ b/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp @@ -187,9 +187,9 @@ double CalcNRLMSISE00(double decyear, double latrad, double lonrad, double alt, input.year = 0; /* without effect */ input.sec = date[3] * 60.0 * 60.0 + date[4] * 60.0 + date[5]; input.alt = alt / 1000.0; - input.g_lat = latrad * math::rad_to_deg; - input.g_long = lonrad * math::rad_to_deg; - input.lst = input.sec / 3600.0 + lonrad * math::rad_to_deg / 15.0; + input.g_lat = latrad * s2e::math::rad_to_deg; + input.g_long = lonrad * s2e::math::rad_to_deg; + input.lst = input.sec / 3600.0 + lonrad * s2e::math::rad_to_deg / 15.0; if (is_manual_param) { input.f107 = manual_f107; diff --git a/src/math_physics/geodesy/geodetic_position.cpp b/src/math_physics/geodesy/geodetic_position.cpp index e7353b670..65969df15 100644 --- a/src/math_physics/geodesy/geodetic_position.cpp +++ b/src/math_physics/geodesy/geodetic_position.cpp @@ -25,7 +25,7 @@ GeodeticPosition::GeodeticPosition(const double latitude_rad, const double longi CalcQuaternionXcxfToLtc(); } -void GeodeticPosition::UpdateFromEcef(const math::Vector<3> position_ecef_m) { +void GeodeticPosition::UpdateFromEcef(const s2e::math::Vector<3> position_ecef_m) { const double earth_radius_m = environment::earth_equatorial_radius_m; const double flattening = environment::earth_flattening; @@ -46,7 +46,7 @@ void GeodeticPosition::UpdateFromEcef(const math::Vector<3> position_ecef_m) { altitude_m_ = r_m / cos(lat_tmp_rad) - c * earth_radius_m; - if (lat_tmp_rad > math::pi_2) lat_tmp_rad -= math::tau; + if (lat_tmp_rad > s2e::math::pi_2) lat_tmp_rad -= s2e::math::tau; latitude_rad_ = lat_tmp_rad; @@ -54,7 +54,7 @@ void GeodeticPosition::UpdateFromEcef(const math::Vector<3> position_ecef_m) { return; } -math::Vector<3> GeodeticPosition::CalcEcefPosition() const { +s2e::math::Vector<3> GeodeticPosition::CalcEcefPosition() const { const double earth_radius_m = environment::earth_equatorial_radius_m; const double flattening = environment::earth_flattening; @@ -63,7 +63,7 @@ math::Vector<3> GeodeticPosition::CalcEcefPosition() const { double c = 1.0 / sqrt(1.0 - e2 * sin(latitude_rad_) * sin(latitude_rad_)); double n = c * earth_radius_m; - math::Vector<3> pos_ecef_m; + s2e::math::Vector<3> pos_ecef_m; pos_ecef_m(0) = (n + altitude_m_) * cos(latitude_rad_) * cos(theta); pos_ecef_m(1) = (n + altitude_m_) * cos(latitude_rad_) * sin(theta); pos_ecef_m(2) = (n * (1.0 - e2) + altitude_m_) * sin(latitude_rad_); @@ -72,7 +72,7 @@ math::Vector<3> GeodeticPosition::CalcEcefPosition() const { } void GeodeticPosition::CalcQuaternionXcxfToLtc() { - math::Matrix<3, 3> dcm_xcxf_to_ltc; + s2e::math::Matrix<3, 3> dcm_xcxf_to_ltc; dcm_xcxf_to_ltc[0][0] = -sin(longitude_rad_); dcm_xcxf_to_ltc[0][1] = cos(longitude_rad_); dcm_xcxf_to_ltc[0][2] = 0; diff --git a/src/math_physics/geodesy/geodetic_position.hpp b/src/math_physics/geodesy/geodetic_position.hpp index 556744f69..a850c5936 100644 --- a/src/math_physics/geodesy/geodetic_position.hpp +++ b/src/math_physics/geodesy/geodetic_position.hpp @@ -36,13 +36,13 @@ class GeodeticPosition { * @brief Update geodetic position with position vector in the ECEF frame * @param [in] position_ecef_m: Position vector in the ECEF frame [m] */ - void UpdateFromEcef(const math::Vector<3> position_ecef_m); + void UpdateFromEcef(const s2e::math::Vector<3> position_ecef_m); /** * @fn CalcEcefPosition * @brief Calculate and return the ECEF position from the geodetic position */ - math::Vector<3> CalcEcefPosition() const; + s2e::math::Vector<3> CalcEcefPosition() const; // Getter /** @@ -64,14 +64,14 @@ class GeodeticPosition { * @fn GetQuaternionXcxfToLtc * @brief Conversion quaternion from XCXF (e.g. ECEF) to LTC frame */ - inline math::Quaternion GetQuaternionXcxfToLtc() const { return quaternion_xcxf_to_ltc_; } + inline s2e::math::Quaternion GetQuaternionXcxfToLtc() const { return quaternion_xcxf_to_ltc_; } private: double latitude_rad_; //!< Latitude [rad] South: -π/2 to 0, North: 0 to π/2 double longitude_rad_; //!< Longitude [rad] East: 0 to π, West: 2π to π (i.e., defined as 0 to 2π [rad] east of the Greenwich meridian) double altitude_m_; //!< Altitude [m] - math::Quaternion quaternion_xcxf_to_ltc_; //!< Conversion quaternion from XCXF (e.g. ECEF) to LTC (Local Topographic Coordinate) + s2e::math::Quaternion quaternion_xcxf_to_ltc_; //!< Conversion quaternion from XCXF (e.g. ECEF) to LTC (Local Topographic Coordinate) /** * @fn CalcQuaternionXcxfToLtc diff --git a/src/math_physics/gnss/antex_file_reader.cpp b/src/math_physics/gnss/antex_file_reader.cpp index c36cdacc4..9a80470d4 100644 --- a/src/math_physics/gnss/antex_file_reader.cpp +++ b/src/math_physics/gnss/antex_file_reader.cpp @@ -171,7 +171,7 @@ AntexPhaseCenterData AntexFileReader::ReadPhaseCenterData(std::ifstream& antex_f } // Phase center offset if (line.find("NORTH / EAST / UP") == ANTEX_LINE_TYPE_POSITION) { - math::Vector<3> offset{0.0}; + s2e::math::Vector<3> offset{0.0}; sscanf(line.c_str(), "%lf %lf %lf", &offset[0], &offset[1], &offset[2]); phase_center_data.SetPhaseCenterOffset_mm(offset); } diff --git a/src/math_physics/gnss/antex_file_reader.hpp b/src/math_physics/gnss/antex_file_reader.hpp index 13e126c55..11ac0dc7d 100644 --- a/src/math_physics/gnss/antex_file_reader.hpp +++ b/src/math_physics/gnss/antex_file_reader.hpp @@ -128,7 +128,7 @@ class AntexPhaseCenterData { * @fn SetPhaseCenterOffset_mm * @param[in] phase_center_offset_mm: Phase center offset vector [mm] */ - inline void SetPhaseCenterOffset_mm(const math::Vector<3> phase_center_offset_mm) { phase_center_offset_mm_ = phase_center_offset_mm; } + inline void SetPhaseCenterOffset_mm(const s2e::math::Vector<3> phase_center_offset_mm) { phase_center_offset_mm_ = phase_center_offset_mm; } /** * @fn SetGridInformation * @param[in] grid_information: Grid information @@ -152,7 +152,7 @@ class AntexPhaseCenterData { * @fn GetPhaseCenterOffset_mm * @return Phase center offset vector [mm] */ - inline math::Vector<3> GetPhaseCenterOffset_mm() const { return phase_center_offset_mm_; } + inline s2e::math::Vector<3> GetPhaseCenterOffset_mm() const { return phase_center_offset_mm_; } /** * @fn GetGridInformation * @return Grid information @@ -166,7 +166,7 @@ class AntexPhaseCenterData { private: std::string frequency_name_ = ""; //!< Frequency name - math::Vector<3> phase_center_offset_mm_{0.0}; //!< Phase center offset [mm] + s2e::math::Vector<3> phase_center_offset_mm_{0.0}; //!< Phase center offset [mm] AntexGridDefinition grid_information_; //!< Grid information std::vector> phase_center_variation_matrix_mm_; //!< Phase center variation [mm] (column, row definition: [azimuth][zenith]) }; diff --git a/src/math_physics/gnss/sp3_file_reader.cpp b/src/math_physics/gnss/sp3_file_reader.cpp index 7fbf0e831..b03fb4923 100644 --- a/src/math_physics/gnss/sp3_file_reader.cpp +++ b/src/math_physics/gnss/sp3_file_reader.cpp @@ -40,7 +40,7 @@ double Sp3FileReader::GetSatelliteClockOffset(const size_t epoch_id, const size_ return position_clock.clock_us_; } -math::Vector<3> Sp3FileReader::GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id) { +s2e::math::Vector<3> Sp3FileReader::GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id) { Sp3PositionClock position_clock = GetPositionClock(epoch_id, satellite_id); return position_clock.position_km_; } @@ -314,7 +314,7 @@ Sp3PositionClock Sp3FileReader::DecodePositionClockData(std::string line) { position_clock.satellite_id_ = line.substr(1, 3); // Position and clock - math::Vector<3> position_km; + s2e::math::Vector<3> position_km; for (size_t axis = 0; axis < 3; axis++) { position_km[axis] = stod(line.substr(4 + axis * 14, 14)); } @@ -323,7 +323,7 @@ Sp3PositionClock Sp3FileReader::DecodePositionClockData(std::string line) { // Standard deviations if (line.size() > 61) { - math::Vector<3> position_standard_deviation; + s2e::math::Vector<3> position_standard_deviation; for (size_t axis = 0; axis < 3; axis++) { try { position_standard_deviation[axis] = stod(line.substr(61 + axis * 3, 2)); @@ -384,7 +384,7 @@ Sp3VelocityClockRate Sp3FileReader::DecodeVelocityClockRateData(std::string line velocity_clock_rate.satellite_id_ = line.substr(1, 3); // Velocity and clock rate - math::Vector<3> velocity_dm_s; + s2e::math::Vector<3> velocity_dm_s; for (size_t axis = 0; axis < 3; axis++) { velocity_dm_s[axis] = stod(line.substr(4 + axis * 14, 14)); } @@ -393,7 +393,7 @@ Sp3VelocityClockRate Sp3FileReader::DecodeVelocityClockRateData(std::string line // Standard deviations if (line.size() > 60) { - math::Vector<3> velocity_standard_deviation; + s2e::math::Vector<3> velocity_standard_deviation; for (size_t axis = 0; axis < 3; axis++) { velocity_standard_deviation[axis] = stod(line.substr(61 + axis * 2, 2)); } diff --git a/src/math_physics/gnss/sp3_file_reader.hpp b/src/math_physics/gnss/sp3_file_reader.hpp index 15e4566dc..2f04b25fc 100644 --- a/src/math_physics/gnss/sp3_file_reader.hpp +++ b/src/math_physics/gnss/sp3_file_reader.hpp @@ -95,9 +95,9 @@ struct Sp3Header { */ struct Sp3PositionClock { std::string satellite_id_; //!< GNSS satellite ID - math::Vector<3> position_km_{SP3_BAD_POSITION_VALUE}; //!< Satellite position [km] + s2e::math::Vector<3> position_km_{SP3_BAD_POSITION_VALUE}; //!< Satellite position [km] double clock_us_ = SP3_BAD_CLOCK_VALUE; //!< Satellite clock offset [us] - math::Vector<3> position_standard_deviation_{0.0}; //!< Satellite position standard deviation [-] + s2e::math::Vector<3> position_standard_deviation_{0.0}; //!< Satellite position standard deviation [-] double clock_standard_deviation_ = 0.0; //!< Satellite clock offset standard deviation [-] bool clock_event_flag_ = false; //!< true when clock discontinuity is happened bool clock_prediction_flag_ = false; //!< true when clock data is predicted @@ -130,9 +130,9 @@ struct Sp3PositionClockCorrelation { */ struct Sp3VelocityClockRate { std::string satellite_id_; //!< GNSS satellite ID - math::Vector<3> velocity_dm_s_{0.0}; //!< Satellite velocity [dm/s] + s2e::math::Vector<3> velocity_dm_s_{0.0}; //!< Satellite velocity [dm/s] double clock_rate_ = 0.0; //!< Satellite clock offset change rate [-] - math::Vector<3> velocity_standard_deviation_{0.0}; //!< Satellite position standard deviation [-] + s2e::math::Vector<3> velocity_standard_deviation_{0.0}; //!< Satellite position standard deviation [-] double clock_rate_standard_deviation_ = 0.0; //!< Satellite clock offset standard deviation [-] }; @@ -178,7 +178,7 @@ class Sp3FileReader { time_system::DateTime GetEpochData(const size_t epoch_id) const; Sp3PositionClock GetPositionClock(const size_t epoch_id, const size_t satellite_id); double GetSatelliteClockOffset(const size_t epoch_id, const size_t satellite_id); - math::Vector<3> GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id); + s2e::math::Vector<3> GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id); size_t SearchNearestEpochId(const time_system::EpochTime time); diff --git a/src/math_physics/gravity/gravity_potential.cpp b/src/math_physics/gravity/gravity_potential.cpp index d6c2fda06..6b55c1558 100644 --- a/src/math_physics/gravity/gravity_potential.cpp +++ b/src/math_physics/gravity/gravity_potential.cpp @@ -28,8 +28,8 @@ GravityPotential::GravityPotential(const size_t degree, const std::vector GravityPotential::CalcAcceleration_xcxf_m_s2(const math::Vector<3> &position_xcxf_m) { - math::Vector<3> acceleration_xcxf_m_s2(0.0); +s2e::math::Vector<3> GravityPotential::CalcAcceleration_xcxf_m_s2(const s2e::math::Vector<3> &position_xcxf_m) { + s2e::math::Vector<3> acceleration_xcxf_m_s2(0.0); if (degree_ <= 0) return acceleration_xcxf_m_s2; // TODO: Consider this assertion is needed xcxf_x_m_ = position_xcxf_m[0]; @@ -95,8 +95,8 @@ math::Vector<3> GravityPotential::CalcAcceleration_xcxf_m_s2(const math::Vector< return acceleration_xcxf_m_s2; } -math::Matrix<3, 3> GravityPotential::CalcPartialDerivative_xcxf_s2(const math::Vector<3> &position_xcxf_m) { - math::Matrix<3, 3> partial_derivative(0.0); +s2e::math::Matrix<3, 3> GravityPotential::CalcPartialDerivative_xcxf_s2(const s2e::math::Vector<3> &position_xcxf_m) { + s2e::math::Matrix<3, 3> partial_derivative(0.0); if (degree_ <= 0) return partial_derivative; xcxf_x_m_ = position_xcxf_m[0]; diff --git a/src/math_physics/gravity/gravity_potential.hpp b/src/math_physics/gravity/gravity_potential.hpp index 94d0c8269..9e25980ba 100644 --- a/src/math_physics/gravity/gravity_potential.hpp +++ b/src/math_physics/gravity/gravity_potential.hpp @@ -48,7 +48,7 @@ class GravityPotential { * @param [in] position_xcxf_m: Position of the spacecraft in the XCXF frame [m] * @return Acceleration in XCXF frame [m/s2] */ - math::Vector<3> CalcAcceleration_xcxf_m_s2(const math::Vector<3> &position_xcxf_m); + s2e::math::Vector<3> CalcAcceleration_xcxf_m_s2(const s2e::math::Vector<3> &position_xcxf_m); /** * @fn CalcAcceleration_xcxf_m_s2 @@ -56,7 +56,7 @@ class GravityPotential { * @param [in] position_xcxf_m: Position of the spacecraft in the XCXF frame [m] * @return Partial derivative of acceleration in XCXF frame [-/s2] */ - math::Matrix<3, 3> CalcPartialDerivative_xcxf_s2(const math::Vector<3> &position_xcxf_m); + s2e::math::Matrix<3, 3> CalcPartialDerivative_xcxf_s2(const s2e::math::Vector<3> &position_xcxf_m); private: size_t degree_ = 0; //!< Maximum degree diff --git a/src/math_physics/gravity/test_gravity_potential.cpp b/src/math_physics/gravity/test_gravity_potential.cpp index 22450d621..1c5177463 100644 --- a/src/math_physics/gravity/test_gravity_potential.cpp +++ b/src/math_physics/gravity/test_gravity_potential.cpp @@ -25,8 +25,8 @@ TEST(GravityPotential, Acceleration) { GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); // Acceleration Calculation check - math::Vector<3> position_xcxf_m; - math::Vector<3> acceleration_xcxf_m_s2; + s2e::math::Vector<3> position_xcxf_m; + s2e::math::Vector<3> acceleration_xcxf_m_s2; const double accuracy = 1.0e-3; // Calc Acceleration @@ -67,8 +67,8 @@ TEST(GravityPotential, PartialDerivative1) { GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); // Calculation check - math::Vector<3> position_xcxf_m; - math::Matrix<3, 3> partial_derivative_xcxf_s2; + s2e::math::Vector<3> position_xcxf_m; + s2e::math::Matrix<3, 3> partial_derivative_xcxf_s2; const double accuracy = 1.0e-3; // Calc Partial Derivative @@ -79,22 +79,22 @@ TEST(GravityPotential, PartialDerivative1) { // Calc Acceleration and numerical partial derivatives double d_r = 1e-9; - math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; + s2e::math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; for (size_t i = 0; i < 3; i++) { - math::Vector<3> position_1_xcxf_m = position_xcxf_m; - math::Vector<3> position_2_xcxf_m = position_xcxf_m; + s2e::math::Vector<3> position_1_xcxf_m = position_xcxf_m; + s2e::math::Vector<3> position_2_xcxf_m = position_xcxf_m; position_1_xcxf_m[i] = position_xcxf_m[i] - d_r / 2.0; position_2_xcxf_m[i] = position_xcxf_m[i] + d_r / 2.0; - math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); - math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); - math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; + s2e::math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); + s2e::math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); + s2e::math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; for (size_t j = 0; j < 3; j++) { numerical_partial_derivative_xcxf_s2[i][j] = diff_acceleration_xcxf_m_s2[j] / d_r; } } // Compare numerical and analytical calculation - math::Matrix<3, 3> diff; + s2e::math::Matrix<3, 3> diff; for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { EXPECT_NEAR(numerical_partial_derivative_xcxf_s2[i][j], partial_derivative_xcxf_s2[i][j], accuracy); @@ -120,8 +120,8 @@ TEST(GravityPotential, PartialDerivative2) { GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); // Calculation check - math::Vector<3> position_xcxf_m; - math::Matrix<3, 3> partial_derivative_xcxf_s2; + s2e::math::Vector<3> position_xcxf_m; + s2e::math::Matrix<3, 3> partial_derivative_xcxf_s2; const double accuracy = 1.0e-3; // Calc Partial Derivative @@ -132,22 +132,22 @@ TEST(GravityPotential, PartialDerivative2) { // Calc Acceleration and numerical partial derivatives double d_r = 1e-9; - math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; + s2e::math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; for (size_t i = 0; i < 3; i++) { - math::Vector<3> position_1_xcxf_m = position_xcxf_m; - math::Vector<3> position_2_xcxf_m = position_xcxf_m; + s2e::math::Vector<3> position_1_xcxf_m = position_xcxf_m; + s2e::math::Vector<3> position_2_xcxf_m = position_xcxf_m; position_1_xcxf_m[i] = position_xcxf_m[i] - d_r / 2.0; position_2_xcxf_m[i] = position_xcxf_m[i] + d_r / 2.0; - math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); - math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); - math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; + s2e::math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); + s2e::math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); + s2e::math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; for (size_t j = 0; j < 3; j++) { numerical_partial_derivative_xcxf_s2[i][j] = diff_acceleration_xcxf_m_s2[j] / d_r; } } // Compare numerical and analytical calculation - math::Matrix<3, 3> diff; + s2e::math::Matrix<3, 3> diff; for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { EXPECT_NEAR(numerical_partial_derivative_xcxf_s2[i][j], partial_derivative_xcxf_s2[i][j], accuracy); diff --git a/src/math_physics/math/constants.hpp b/src/math_physics/math/constants.hpp index 9e6744015..7a6f0cf21 100644 --- a/src/math_physics/math/constants.hpp +++ b/src/math_physics/math/constants.hpp @@ -8,7 +8,7 @@ #include // std::is_floating_point_v -namespace math { +namespace s2e::math { // instead of C++20 std::numbers inline namespace numbers { @@ -47,6 +47,6 @@ DEFINE_MATH_CONSTANT(rad_s_to_rpm, 60.0L / tau) /* rad/s to rpm */ #undef DEFINE_MATH_CONSTANT } // namespace numbers -} // namespace math +} // namespace s2e::math #endif // S2E_LIBRARY_MATH_CONSTANTS_HPP_ diff --git a/src/math_physics/math/interpolation.cpp b/src/math_physics/math/interpolation.cpp index 4415c4dd5..2cab687a6 100644 --- a/src/math_physics/math/interpolation.cpp +++ b/src/math_physics/math/interpolation.cpp @@ -7,7 +7,7 @@ #include -namespace math { +namespace s2e::math { double Interpolation::CalcPolynomial(const double x) const { // Search nearest point @@ -94,4 +94,4 @@ size_t Interpolation::FindNearestPoint(const double x) const { return output; } -} // namespace math +} // namespace s2e::math diff --git a/src/math_physics/math/interpolation.hpp b/src/math_physics/math/interpolation.hpp index 7620ff338..f9815bc78 100644 --- a/src/math_physics/math/interpolation.hpp +++ b/src/math_physics/math/interpolation.hpp @@ -9,7 +9,7 @@ #include #include -namespace math { +namespace s2e::math { /** * @class Interpolation @@ -90,6 +90,6 @@ class Interpolation { size_t FindNearestPoint(const double x) const; }; -} // namespace math +} // namespace s2e::math #endif // S2E_LIBRARY_MATH_INTERPOLATION_HPP_ diff --git a/src/math_physics/math/matrix.hpp b/src/math_physics/math/matrix.hpp index 89429a8ca..a731e1d79 100644 --- a/src/math_physics/math/matrix.hpp +++ b/src/math_physics/math/matrix.hpp @@ -9,7 +9,7 @@ #include // for size_t #include // for ostream, cout -namespace math { +namespace s2e::math { /** * @class Matrix @@ -244,7 +244,7 @@ Matrix MakeRotationMatrixY(const double& theta_rad); template Matrix MakeRotationMatrixZ(const double& theta_rad); -} // namespace math +} // namespace s2e::math #include "matrix_template_functions.hpp" diff --git a/src/math_physics/math/matrix_template_functions.hpp b/src/math_physics/math/matrix_template_functions.hpp index dbfa9aa1b..56685a685 100644 --- a/src/math_physics/math/matrix_template_functions.hpp +++ b/src/math_physics/math/matrix_template_functions.hpp @@ -9,7 +9,7 @@ #include #include // for cout -namespace math { +namespace s2e::math { template Matrix::Matrix(const T& n) { @@ -186,6 +186,6 @@ Matrix MakeRotationMatrixZ(const double& theta_rad) { return m; } -} // namespace math +} // namespace s2e::math #endif // S2E_LIBRARY_MATH_MATRIX_TEMPLATE_FUNCTIONS_HPP_ diff --git a/src/math_physics/math/matrix_vector.hpp b/src/math_physics/math/matrix_vector.hpp index 7c76a07f5..1d4803ded 100644 --- a/src/math_physics/math/matrix_vector.hpp +++ b/src/math_physics/math/matrix_vector.hpp @@ -9,7 +9,7 @@ #include "matrix.hpp" #include "vector.hpp" -namespace math { +namespace s2e::math { /** * @fn operator* @@ -52,7 +52,7 @@ Matrix& LuDecomposition(Matrix& matrix, size_t index[]); template Vector& SolveLinearSystemWithLu(const Matrix& matrix, const size_t index[], Vector& vector); -} // namespace math +} // namespace s2e::math #include "matrix_vector_template_functions.hpp" diff --git a/src/math_physics/math/matrix_vector_template_functions.hpp b/src/math_physics/math/matrix_vector_template_functions.hpp index d16f08a2a..31289721b 100644 --- a/src/math_physics/math/matrix_vector_template_functions.hpp +++ b/src/math_physics/math/matrix_vector_template_functions.hpp @@ -8,7 +8,7 @@ #include // for invalid_argument. -namespace math { +namespace s2e::math { template Vector operator*(const Matrix& matrix, const Vector& vector) { @@ -139,6 +139,6 @@ Vector& SolveLinearSystemWithLu(const Matrix& a, const size_t index[], return b; } -} // namespace math +} // namespace s2e::math #endif // S2E_LIBRARY_MATH_MATRIX_VECTOR_TEMPLATE_FUNCTIONS_HPP_ diff --git a/src/math_physics/math/ordinary_differential_equation.hpp b/src/math_physics/math/ordinary_differential_equation.hpp index 36ff4ae86..fb0d81b36 100644 --- a/src/math_physics/math/ordinary_differential_equation.hpp +++ b/src/math_physics/math/ordinary_differential_equation.hpp @@ -8,7 +8,7 @@ #include "./vector.hpp" -namespace math { +namespace s2e::math { /** * @class OrdinaryDifferentialEquation @@ -102,7 +102,7 @@ class OrdinaryDifferentialEquation { * @fn GetState * @brief Return current state vector for inheriting class */ - inline math::Vector& GetState() { return state_; } + inline s2e::math::Vector& GetState() { return state_; } private: double independent_variable_; //!< Latest value of independent variable @@ -111,7 +111,7 @@ class OrdinaryDifferentialEquation { double step_width_s_; //!< Step width }; -} // namespace math +} // namespace s2e::math #include "./ordinary_differential_equation_template_functions.hpp" diff --git a/src/math_physics/math/ordinary_differential_equation_template_functions.hpp b/src/math_physics/math/ordinary_differential_equation_template_functions.hpp index 7c5dc3071..334be563a 100644 --- a/src/math_physics/math/ordinary_differential_equation_template_functions.hpp +++ b/src/math_physics/math/ordinary_differential_equation_template_functions.hpp @@ -5,7 +5,7 @@ #ifndef S2E_LIBRARY_MATH_ORDINARY_DIFFERENTIA_EQUATION_TEMPLATE_FUNCTIONS_HPP_ #define S2E_LIBRARY_MATH_ORDINARY_DIFFERENTIA_EQUATION_TEMPLATE_FUNCTIONS_HPP_ -namespace math { +namespace s2e::math { template OrdinaryDifferentialEquation::OrdinaryDifferentialEquation(double step_width_s) @@ -44,6 +44,6 @@ void OrdinaryDifferentialEquation::Update() { independent_variable_ += step_width_s_; // Update independent variable } -} // namespace math +} // namespace s2e::math #endif // S2E_LIBRARY_MATH_ORDINARY_DIFFERENTIA_EQUATION_TEMPLATE_FUNCTIONS_HPP_ diff --git a/src/math_physics/math/quaternion.cpp b/src/math_physics/math/quaternion.cpp index 27dacc275..d2f28f085 100644 --- a/src/math_physics/math/quaternion.cpp +++ b/src/math_physics/math/quaternion.cpp @@ -9,7 +9,7 @@ #include #include -namespace math { +namespace s2e::math { Quaternion::Quaternion(const Vector<3>& rotation_axis, const double rotation_angle_rad) { double half_rotation_angle_rad = rotation_angle_rad * 0.5; @@ -236,4 +236,4 @@ Vector<3> Quaternion::InverseFrameConversion(const Vector<3>& vector) const { Vector<4> Quaternion::ConvertToVector() { return quaternion_; } -} // namespace math +} // namespace s2e::math diff --git a/src/math_physics/math/quaternion.hpp b/src/math_physics/math/quaternion.hpp index 76720f773..1980ec8c1 100644 --- a/src/math_physics/math/quaternion.hpp +++ b/src/math_physics/math/quaternion.hpp @@ -9,7 +9,7 @@ #include "matrix.hpp" #include "vector.hpp" -namespace math { +namespace s2e::math { /** * @class Quaternion @@ -203,6 +203,6 @@ Quaternion operator*(const Quaternion& lhs, const Vector<3>& rhs); * @return Quaternion */ Quaternion operator*(const double& lhs, const Quaternion& rhs); -} // namespace math +} // namespace s2e::math #endif // S2E_LIBRARY_MATH_QUATERNION_HPP_ diff --git a/src/math_physics/math/s2e_math.cpp b/src/math_physics/math/s2e_math.cpp index 943e6da91..26020cd77 100644 --- a/src/math_physics/math/s2e_math.cpp +++ b/src/math_physics/math/s2e_math.cpp @@ -7,20 +7,20 @@ #include -namespace math { +namespace s2e::math { double WrapTo2Pi(const double angle_rad) { double angle_out = angle_rad; if (angle_out < 0.0) { while (angle_out < 0.0) { - angle_out += math::tau; + angle_out += s2e::math::tau; } - } else if (angle_out > math::tau) { - while (angle_out > math::tau) { - angle_out -= math::tau; + } else if (angle_out > s2e::math::tau) { + while (angle_out > s2e::math::tau) { + angle_out -= s2e::math::tau; } } else { // nothing to do } return angle_out; } -} // namespace math +} // namespace s2e::math diff --git a/src/math_physics/math/s2e_math.hpp b/src/math_physics/math/s2e_math.hpp index fb7bbccb6..8cafb64d4 100644 --- a/src/math_physics/math/s2e_math.hpp +++ b/src/math_physics/math/s2e_math.hpp @@ -8,7 +8,7 @@ #include -namespace math { +namespace s2e::math { /** * @fn WrapTo2Pi @@ -17,6 +17,6 @@ namespace math { */ double WrapTo2Pi(const double angle_rad); -} // namespace math +} // namespace s2e::math #endif // S2E_LIBRARY_MATH_S2E_MATH_HPP_ diff --git a/src/math_physics/math/test_interpolation.cpp b/src/math_physics/math/test_interpolation.cpp index 674b12419..a29f3b330 100644 --- a/src/math_physics/math/test_interpolation.cpp +++ b/src/math_physics/math/test_interpolation.cpp @@ -15,7 +15,7 @@ TEST(Interpolation, PolynomialLinearFunction) { std::vector x{0.0, 1.0, 2.0, 3.0, 4.0}; std::vector y{0.0, 2.0, 4.0, 6.0, 8.0}; - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); double xx = 0.4; EXPECT_DOUBLE_EQ(2.0 * xx, interpolation.CalcPolynomial(xx)); @@ -31,7 +31,7 @@ TEST(Interpolation, PolynomialLinearFunction) { TEST(Interpolation, PolynomialQuadraticFunction) { std::vector x{0.0, 1.0, 2.0, 3.0, 4.0}; std::vector y{0.0, 1.0, 4.0, 9.0, 16.0}; - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); double xx = 0.4; EXPECT_DOUBLE_EQ(pow(xx, 2.0), interpolation.CalcPolynomial(xx)); @@ -45,16 +45,16 @@ TEST(Interpolation, PolynomialQuadraticFunction) { * @brief Test for sin function with trigonometric interpolation */ TEST(Interpolation, TrigonometricSinFunction) { - std::vector x{0.0, math::pi_2, math::pi, math::pi * 3.0 / 2.0, math::tau}; + std::vector x{0.0, s2e::math::pi_2, s2e::math::pi, s2e::math::pi * 3.0 / 2.0, s2e::math::tau}; std::vector y; for (size_t i = 0; i < x.size(); i++) { y.push_back(sin(x[i])); } - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); - double xx = 0.4 * math::pi; + double xx = 0.4 * s2e::math::pi; EXPECT_DOUBLE_EQ(sin(xx), interpolation.CalcTrigonometric(xx)); - xx = 1.4 * math::pi; + xx = 1.4 * s2e::math::pi; EXPECT_DOUBLE_EQ(sin(xx), interpolation.CalcTrigonometric(xx)); } @@ -62,16 +62,16 @@ TEST(Interpolation, TrigonometricSinFunction) { * @brief Test for cos function with trigonometric interpolation */ TEST(Interpolation, TrigonometricCosFunction) { - std::vector x{0.0, 0.3 * math::pi_2, 0.6 * math::pi_2, 0.9 * math::pi_2, 1.2 * math::pi_2}; + std::vector x{0.0, 0.3 * s2e::math::pi_2, 0.6 * s2e::math::pi_2, 0.9 * s2e::math::pi_2, 1.2 * s2e::math::pi_2}; std::vector y; for (size_t i = 0; i < x.size(); i++) { y.push_back(cos(x[i])); } - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); - double xx = 0.1 * math::pi_2; + double xx = 0.1 * s2e::math::pi_2; EXPECT_NEAR(cos(xx), interpolation.CalcTrigonometric(xx), 1e-6); - xx = 0.8 * math::pi_2; + xx = 0.8 * s2e::math::pi_2; EXPECT_NEAR(cos(xx), interpolation.CalcTrigonometric(xx), 1e-6); } @@ -79,16 +79,16 @@ TEST(Interpolation, TrigonometricCosFunction) { * @brief Test for cos function with trigonometric interpolation */ TEST(Interpolation, TrigonometricCosSinFunctionOddDegree) { - std::vector x{0.0, 0.3 * math::pi_2, 0.6 * math::pi_2, 0.9 * math::pi_2, 1.2 * math::pi_2, 1.5 * math::pi_2}; + std::vector x{0.0, 0.3 * s2e::math::pi_2, 0.6 * s2e::math::pi_2, 0.9 * s2e::math::pi_2, 1.2 * s2e::math::pi_2, 1.5 * s2e::math::pi_2}; std::vector y; for (size_t i = 0; i < x.size(); i++) { y.push_back(cos(x[i]) + sin(x[i])); } - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); - double xx = 0.1 * math::pi_2; + double xx = 0.1 * s2e::math::pi_2; EXPECT_NEAR(cos(xx) + sin(xx), interpolation.CalcTrigonometric(xx), 1e-6); - xx = 0.8 * math::pi_2; + xx = 0.8 * s2e::math::pi_2; EXPECT_NEAR(cos(xx) + sin(xx), interpolation.CalcTrigonometric(xx), 1e-6); } @@ -98,7 +98,7 @@ TEST(Interpolation, TrigonometricCosSinFunctionOddDegree) { TEST(Interpolation, PushAndPop) { std::vector x{0.0, 1.0, 2.0, 3.0, 4.0}; std::vector y{0.0, 2.0, 4.0, 6.0, 8.0}; - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); EXPECT_EQ(x.size(), interpolation.GetDegree()); for (size_t i = 0; i < x.size(); i++) { diff --git a/src/math_physics/math/test_matrix.cpp b/src/math_physics/math/test_matrix.cpp index 53dee0b75..9b23c15d5 100644 --- a/src/math_physics/math/test_matrix.cpp +++ b/src/math_physics/math/test_matrix.cpp @@ -14,7 +14,7 @@ TEST(Matrix, ConstructorWithNumber) { const size_t R = 6; const size_t C = 3; double initialize_value = 2.0; - math::Matrix m(initialize_value); + s2e::math::Matrix m(initialize_value); for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -30,7 +30,7 @@ TEST(Matrix, ConstructorWithNumber) { TEST(Matrix, GetLength) { const size_t R = 6; const size_t C = 3; - math::Matrix m; + s2e::math::Matrix m; EXPECT_EQ(R, m.GetRowLength()); EXPECT_EQ(C, m.GetColumnLength()); @@ -43,8 +43,8 @@ TEST(Matrix, OperatorPlusEqual) { const size_t R = 6; const size_t C = 3; double initialize_value = 2.0; - math::Matrix m(initialize_value); - math::Matrix adding; + s2e::math::Matrix m(initialize_value); + s2e::math::Matrix adding; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -71,8 +71,8 @@ TEST(Matrix, OperatorMinusEqual) { const size_t R = 6; const size_t C = 3; double initialize_value = 2.0; - math::Matrix m(initialize_value); - math::Matrix subtracting; + s2e::math::Matrix m(initialize_value); + s2e::math::Matrix subtracting; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -99,7 +99,7 @@ TEST(Matrix, OperatorMultiplyEqual) { const size_t R = 6; const size_t C = 3; - math::Matrix m; + s2e::math::Matrix m; double multiplying = 2.0; for (size_t r = 0; r < R; r++) { @@ -125,7 +125,7 @@ TEST(Matrix, OperatorDivideEqual) { const size_t R = 6; const size_t C = 3; - math::Matrix m; + s2e::math::Matrix m; double dividing = 2.0; for (size_t r = 0; r < R; r++) { @@ -152,7 +152,7 @@ TEST(Matrix, FillUp) { const size_t C = 3; double value = 3.0; - math::Matrix m; + s2e::math::Matrix m; m.FillUp(value); @@ -169,7 +169,7 @@ TEST(Matrix, FillUp) { */ TEST(Matrix, CalcTrace) { const size_t N = 6; - math::Matrix m; + s2e::math::Matrix m; for (size_t r = 0; r < N; r++) { for (size_t c = 0; c < N; c++) { @@ -201,8 +201,8 @@ TEST(Matrix, OperatorPlus) { const size_t R = 6; const size_t C = 3; double initialize_value = -2.0; - math::Matrix m(initialize_value); - math::Matrix adding; + s2e::math::Matrix m(initialize_value); + s2e::math::Matrix adding; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -210,7 +210,7 @@ TEST(Matrix, OperatorPlus) { } } - math::Matrix added = m + adding; + s2e::math::Matrix added = m + adding; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -230,8 +230,8 @@ TEST(Matrix, OperatorMinus) { const size_t R = 6; const size_t C = 3; double initialize_value = 0.6; - math::Matrix m(initialize_value); - math::Matrix subtracting; + s2e::math::Matrix m(initialize_value); + s2e::math::Matrix subtracting; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -239,7 +239,7 @@ TEST(Matrix, OperatorMinus) { } } - math::Matrix subtracted = m - subtracting; + s2e::math::Matrix subtracted = m - subtracting; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -259,7 +259,7 @@ TEST(Matrix, OperatorMultiplyScalar) { const size_t R = 6; const size_t C = 3; - math::Matrix m; + s2e::math::Matrix m; double multiplying = 0.3; for (size_t r = 0; r < R; r++) { @@ -268,7 +268,7 @@ TEST(Matrix, OperatorMultiplyScalar) { } } - math::Matrix subtracted = multiplying * m; + s2e::math::Matrix subtracted = multiplying * m; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -287,8 +287,8 @@ TEST(Matrix, OperatorMultiplyMatrix) { const size_t R = 2; const size_t C = 3; - math::Matrix a; - math::Matrix b; + s2e::math::Matrix a; + s2e::math::Matrix b; a[0][0] = 1.0; a[0][1] = 2.0; @@ -304,7 +304,7 @@ TEST(Matrix, OperatorMultiplyMatrix) { b[2][0] = 5.0; b[2][1] = 6.0; - math::Matrix result = a * b; + s2e::math::Matrix result = a * b; EXPECT_DOUBLE_EQ(22.0, result[0][0]); EXPECT_DOUBLE_EQ(28.0, result[0][1]); @@ -319,14 +319,14 @@ TEST(Matrix, Transpose) { const size_t R = 6; const size_t C = 3; - math::Matrix m; + s2e::math::Matrix m; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { m[r][c] = r * c; } } - math::Matrix transposed = m.Transpose(); + s2e::math::Matrix transposed = m.Transpose(); for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -344,7 +344,7 @@ TEST(Matrix, Transpose) { TEST(Matrix, MakeIdentityMatrix) { const size_t N = 6; - math::Matrix m = math::MakeIdentityMatrix(); + s2e::math::Matrix m = s2e::math::MakeIdentityMatrix(); for (size_t r = 0; r < N; r++) { for (size_t c = 0; c < N; c++) { @@ -362,9 +362,9 @@ TEST(Matrix, MakeIdentityMatrix) { */ TEST(Matrix, MakeRotationMatrixX) { const size_t N = 3; - double theta_rad = -45.0 * math::deg_to_rad; + double theta_rad = -45.0 * s2e::math::deg_to_rad; - math::Matrix m = math::MakeRotationMatrixX(theta_rad); + s2e::math::Matrix m = s2e::math::MakeRotationMatrixX(theta_rad); EXPECT_DOUBLE_EQ(1.0, m[0][0]); EXPECT_DOUBLE_EQ(0.0, m[0][1]); @@ -382,9 +382,9 @@ TEST(Matrix, MakeRotationMatrixX) { */ TEST(Matrix, MakeRotationMatrixY) { const size_t N = 3; - double theta_rad = 120.0 * math::deg_to_rad; + double theta_rad = 120.0 * s2e::math::deg_to_rad; - math::Matrix m = math::MakeRotationMatrixY(theta_rad); + s2e::math::Matrix m = s2e::math::MakeRotationMatrixY(theta_rad); EXPECT_DOUBLE_EQ(cos(theta_rad), m[0][0]); EXPECT_DOUBLE_EQ(0.0, m[0][1]); @@ -402,9 +402,9 @@ TEST(Matrix, MakeRotationMatrixY) { */ TEST(Matrix, MakeRotationMatrixZ) { const size_t N = 3; - double theta_rad = 30.0 * math::deg_to_rad; + double theta_rad = 30.0 * s2e::math::deg_to_rad; - math::Matrix m = math::MakeRotationMatrixZ(theta_rad); + s2e::math::Matrix m = s2e::math::MakeRotationMatrixZ(theta_rad); EXPECT_DOUBLE_EQ(cos(theta_rad), m[0][0]); EXPECT_DOUBLE_EQ(sin(theta_rad), m[0][1]); diff --git a/src/math_physics/math/test_matrix_vector.cpp b/src/math_physics/math/test_matrix_vector.cpp index 78fc9babf..958a409b6 100644 --- a/src/math_physics/math/test_matrix_vector.cpp +++ b/src/math_physics/math/test_matrix_vector.cpp @@ -14,8 +14,8 @@ TEST(MatrixVector, MultiplyMatrixVector) { const size_t R = 3; const size_t C = 2; - math::Matrix m; - math::Vector v; + s2e::math::Matrix m; + s2e::math::Vector v; m[0][0] = 1.0; m[0][1] = 2.0; @@ -27,7 +27,7 @@ TEST(MatrixVector, MultiplyMatrixVector) { v[0] = 7.0; v[1] = 1.0; - math::Vector result = m * v; + s2e::math::Vector result = m * v; EXPECT_DOUBLE_EQ(9.0, result[0]); EXPECT_DOUBLE_EQ(-7.0, result[1]); @@ -40,7 +40,7 @@ TEST(MatrixVector, MultiplyMatrixVector) { TEST(MatrixVector, CalcInverseMatrix) { const size_t N = 3; - math::Matrix m; + s2e::math::Matrix m; m[0][0] = 1.0; m[0][1] = 1.0; @@ -52,7 +52,7 @@ TEST(MatrixVector, CalcInverseMatrix) { m[2][1] = -2.0; m[2][2] = 1.0; - math::Matrix inverse = math::CalcInverseMatrix(m); + s2e::math::Matrix inverse = s2e::math::CalcInverseMatrix(m); EXPECT_NEAR(-1.0, inverse[0][0], 1e-10); EXPECT_NEAR(-1.0, inverse[0][1], 1e-10); diff --git a/src/math_physics/math/test_quaternion.cpp b/src/math_physics/math/test_quaternion.cpp index 4995afccc..2821eccee 100644 --- a/src/math_physics/math/test_quaternion.cpp +++ b/src/math_physics/math/test_quaternion.cpp @@ -11,7 +11,7 @@ * @brief Test for constructor from four numbers */ TEST(Quaternion, ConstructorFourNumber) { - math::Quaternion q(0.5, 0.5, 0.5, 0.5); + s2e::math::Quaternion q(0.5, 0.5, 0.5, 0.5); EXPECT_DOUBLE_EQ(0.5, q[0]); EXPECT_DOUBLE_EQ(0.5, q[1]); @@ -23,8 +23,8 @@ TEST(Quaternion, ConstructorFourNumber) { * @brief Test for constructor from Vector */ TEST(Quaternion, ConstructorVector) { - math::Vector<4> v(0.5); - math::Quaternion q(v); + s2e::math::Vector<4> v(0.5); + s2e::math::Quaternion q(v); EXPECT_DOUBLE_EQ(0.5, q[0]); EXPECT_DOUBLE_EQ(0.5, q[1]); @@ -36,12 +36,12 @@ TEST(Quaternion, ConstructorVector) { * @brief Test for constructor from axis and rotation angle X rotation */ TEST(Quaternion, ConstructorAxisAndAngleX) { - math::Vector<3> axis; + s2e::math::Vector<3> axis; axis[0] = 1.0; axis[1] = 0.0; axis[2] = 0.0; - double theta_rad = 90 * math::deg_to_rad; - math::Quaternion q(axis, theta_rad); + double theta_rad = 90 * s2e::math::deg_to_rad; + s2e::math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -53,12 +53,12 @@ TEST(Quaternion, ConstructorAxisAndAngleX) { * @brief Test for constructor from axis and rotation angle Y rotation */ TEST(Quaternion, ConstructorAxisAndAngleY) { - math::Vector<3> axis; + s2e::math::Vector<3> axis; axis[0] = 0.0; axis[1] = 1.0; axis[2] = 0.0; - double theta_rad = 45 * math::deg_to_rad; - math::Quaternion q(axis, theta_rad); + double theta_rad = 45 * s2e::math::deg_to_rad; + s2e::math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -70,12 +70,12 @@ TEST(Quaternion, ConstructorAxisAndAngleY) { * @brief Test for constructor from axis and rotation angle Z rotation */ TEST(Quaternion, ConstructorAxisAndAngleZ) { - math::Vector<3> axis; + s2e::math::Vector<3> axis; axis[0] = 0.0; axis[1] = 0.0; axis[2] = 1.0; - double theta_rad = -60 * math::deg_to_rad; - math::Quaternion q(axis, theta_rad); + double theta_rad = -60 * s2e::math::deg_to_rad; + s2e::math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -87,12 +87,12 @@ TEST(Quaternion, ConstructorAxisAndAngleZ) { * @brief Test for constructor from axis and rotation angle All axes rotation */ TEST(Quaternion, ConstructorAxisAndAngleAll) { - math::Vector<3> axis; + s2e::math::Vector<3> axis; axis[0] = 1.0; axis[1] = 1.0; axis[2] = 1.0; - double theta_rad = 180 * math::deg_to_rad; - math::Quaternion q(axis, theta_rad); + double theta_rad = 180 * s2e::math::deg_to_rad; + s2e::math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -104,16 +104,16 @@ TEST(Quaternion, ConstructorAxisAndAngleAll) { * @brief Test for constructor from two vectors: No rotation */ TEST(Quaternion, ConstructorTwoVectorsNoRotation) { - math::Vector<3> before; + s2e::math::Vector<3> before; before[0] = 0.0; before[1] = 0.0; before[2] = 2.0; // To check normalization - math::Vector<3> after; + s2e::math::Vector<3> after; after[0] = 0.0; after[1] = 0.0; after[2] = 1.0; - math::Quaternion q(before, after); + s2e::math::Quaternion q(before, after); EXPECT_NEAR(0.0, q[0], 1e-5); EXPECT_NEAR(0.0, q[1], 1e-5); @@ -125,18 +125,18 @@ TEST(Quaternion, ConstructorTwoVectorsNoRotation) { * @brief Test for constructor from two vectors: X rotation */ TEST(Quaternion, ConstructorTwoVectorsX) { - math::Vector<3> before; + s2e::math::Vector<3> before; before[0] = 0.0; before[1] = 0.0; before[2] = 1.0; - math::Vector<3> after; + s2e::math::Vector<3> after; after[0] = 0.0; after[1] = 1.0; after[2] = 0.0; - math::Quaternion q(before, after); + s2e::math::Quaternion q(before, after); - double theta_rad = -90 * math::deg_to_rad; + double theta_rad = -90 * s2e::math::deg_to_rad; EXPECT_NEAR(sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(0.0, q[1], 1e-5); EXPECT_NEAR(0.0, q[2], 1e-5); @@ -147,18 +147,18 @@ TEST(Quaternion, ConstructorTwoVectorsX) { * @brief Test for constructor from two vectors: Y rotation */ TEST(Quaternion, ConstructorTwoVectorsY) { - math::Vector<3> before; + s2e::math::Vector<3> before; before[0] = 0.0; before[1] = 0.0; before[2] = 1.0; - math::Vector<3> after; + s2e::math::Vector<3> after; after[0] = 1.0; after[1] = 0.0; after[2] = 0.0; - math::Quaternion q(before, after); + s2e::math::Quaternion q(before, after); - double theta_rad = 90 * math::deg_to_rad; + double theta_rad = 90 * s2e::math::deg_to_rad; EXPECT_NEAR(0.0, q[0], 1e-5); EXPECT_NEAR(sin(theta_rad / 2.0), q[1], 1e-5); EXPECT_NEAR(0.0, q[2], 1e-5); @@ -169,18 +169,18 @@ TEST(Quaternion, ConstructorTwoVectorsY) { * @brief Test for constructor from two vectors: Z rotation */ TEST(Quaternion, ConstructorTwoVectorsZ) { - math::Vector<3> before; + s2e::math::Vector<3> before; before[0] = 1.0; before[1] = 0.0; before[2] = 0.0; - math::Vector<3> after; + s2e::math::Vector<3> after; after[0] = 0.0; after[1] = 1.0; after[2] = 0.0; - math::Quaternion q(before, after); + s2e::math::Quaternion q(before, after); - double theta_rad = 90 * math::deg_to_rad; + double theta_rad = 90 * s2e::math::deg_to_rad; EXPECT_NEAR(0.0, q[0], 1e-5); EXPECT_NEAR(0.0, q[1], 1e-5); EXPECT_NEAR(sin(theta_rad / 2.0), q[2], 1e-5); @@ -192,7 +192,7 @@ TEST(Quaternion, ConstructorTwoVectorsZ) { * @note TODO: Fix to nondestructive function */ TEST(Quaternion, Normalize) { - math::Quaternion q(1.0, 1.0, 1.0, 1.0); + s2e::math::Quaternion q(1.0, 1.0, 1.0, 1.0); EXPECT_DOUBLE_EQ(1.0, q[0]); EXPECT_DOUBLE_EQ(1.0, q[1]); EXPECT_DOUBLE_EQ(1.0, q[2]); @@ -209,9 +209,9 @@ TEST(Quaternion, Normalize) { * @brief Test for Conjugate */ TEST(Quaternion, Conjugate) { - math::Quaternion q(0.5, 0.5, 0.5, 0.5); + s2e::math::Quaternion q(0.5, 0.5, 0.5, 0.5); - math::Quaternion q_conjugate = q.Conjugate(); + s2e::math::Quaternion q_conjugate = q.Conjugate(); // Check nondestructive function EXPECT_DOUBLE_EQ(0.5, q[0]); @@ -229,10 +229,10 @@ TEST(Quaternion, Conjugate) { * @brief Test for ConvertToDcm Y rotation */ TEST(Quaternion, ConvertToDcmY) { - math::Quaternion q(0.0, 1.0, 0.0, 1.0); + s2e::math::Quaternion q(0.0, 1.0, 0.0, 1.0); q.Normalize(); - math::Matrix<3, 3> dcm = q.ConvertToDcm(); + s2e::math::Matrix<3, 3> dcm = q.ConvertToDcm(); // Check nondestructive function EXPECT_DOUBLE_EQ(0.0, q[0]); @@ -253,7 +253,7 @@ TEST(Quaternion, ConvertToDcmY) { EXPECT_NEAR(0.0, dcm[2][2], accuracy); // Inverse Conversion - math::Quaternion q_from_dcm = math::Quaternion::ConvertFromDcm(dcm); + s2e::math::Quaternion q_from_dcm = s2e::math::Quaternion::ConvertFromDcm(dcm); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_dcm[i], accuracy); } @@ -263,10 +263,10 @@ TEST(Quaternion, ConvertToDcmY) { * @brief Test for ConvertToDcm */ TEST(Quaternion, ConvertToDcm) { - math::Quaternion q(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); q.Normalize(); - math::Matrix<3, 3> dcm = q.ConvertToDcm(); + s2e::math::Matrix<3, 3> dcm = q.ConvertToDcm(); // Check nondestructive function const double accuracy = 1.0e-5; @@ -281,7 +281,7 @@ TEST(Quaternion, ConvertToDcm) { EXPECT_NEAR(0.4962963, dcm[2][2], accuracy); // Inverse Conversion - math::Quaternion q_from_dcm = math::Quaternion::ConvertFromDcm(dcm); + s2e::math::Quaternion q_from_dcm = s2e::math::Quaternion::ConvertFromDcm(dcm); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_dcm[i], accuracy); } @@ -291,10 +291,10 @@ TEST(Quaternion, ConvertToDcm) { * @brief Test for ConvertToEuler X rotation */ TEST(Quaternion, ConvertToEulerX) { - math::Quaternion q(1.0, 0.0, 0.0, 1.0); + s2e::math::Quaternion q(1.0, 0.0, 0.0, 1.0); q.Normalize(); - math::Vector<3> euler = q.ConvertToEuler(); + s2e::math::Vector<3> euler = q.ConvertToEuler(); // Check nondestructive function EXPECT_DOUBLE_EQ(1.0 / sqrt(2.0), q[0]); @@ -304,12 +304,12 @@ TEST(Quaternion, ConvertToEulerX) { // Check nondestructive function const double accuracy = 1.0e-7; - EXPECT_NEAR(90 * math::deg_to_rad, euler[0], accuracy); + EXPECT_NEAR(90 * s2e::math::deg_to_rad, euler[0], accuracy); EXPECT_NEAR(0.0, euler[1], accuracy); EXPECT_NEAR(0.0, euler[2], accuracy); // Inverse Conversion - math::Quaternion q_from_euler = math::Quaternion::ConvertFromEuler(euler); + s2e::math::Quaternion q_from_euler = s2e::math::Quaternion::ConvertFromEuler(euler); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_euler[i], accuracy); } @@ -319,10 +319,10 @@ TEST(Quaternion, ConvertToEulerX) { * @brief Test for ConvertToEuler */ TEST(Quaternion, ConvertToEuler) { - math::Quaternion q(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); q.Normalize(); - math::Vector<3> euler = q.ConvertToEuler(); + s2e::math::Vector<3> euler = q.ConvertToEuler(); // Check nondestructive function const double accuracy = 1.0e-7; @@ -331,7 +331,7 @@ TEST(Quaternion, ConvertToEuler) { EXPECT_NEAR(0.41012734, euler[2], accuracy); // Inverse Conversion - math::Quaternion q_from_euler = math::Quaternion::ConvertFromEuler(euler); + s2e::math::Quaternion q_from_euler = s2e::math::Quaternion::ConvertFromEuler(euler); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_euler[i], accuracy); } @@ -341,22 +341,22 @@ TEST(Quaternion, ConvertToEuler) { * @brief Test for FrameConversion Z rotation */ TEST(Quaternion, FrameConversionZ) { - math::Quaternion q(0.0, 0.0, 1.0, 1.0); + s2e::math::Quaternion q(0.0, 0.0, 1.0, 1.0); q.Normalize(); - math::Vector<3> v; + s2e::math::Vector<3> v; v[0] = 1.0; v[1] = 0.0; v[2] = 0.0; - math::Vector<3> v_frame_conv = q.FrameConversion(v); + s2e::math::Vector<3> v_frame_conv = q.FrameConversion(v); const double accuracy = 1.0e-7; EXPECT_NEAR(0.0, v_frame_conv[0], accuracy); EXPECT_NEAR(-1.0, v_frame_conv[1], accuracy); EXPECT_NEAR(0.0, v_frame_conv[2], accuracy); - math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); + s2e::math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); for (size_t i = 0; i < 3; i++) { EXPECT_NEAR(v[i], v_frame_conv_inv[i], accuracy); @@ -367,15 +367,15 @@ TEST(Quaternion, FrameConversionZ) { * @brief Test for FrameConversion */ TEST(Quaternion, FrameConversion) { - math::Quaternion q(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); q.Normalize(); - math::Vector<3> v; + s2e::math::Vector<3> v; v[0] = 1.0; v[1] = 0.0; v[2] = 0.0; - math::Vector<3> v_frame_conv = q.FrameConversion(v); - math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); + s2e::math::Vector<3> v_frame_conv = q.FrameConversion(v); + s2e::math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); const double accuracy = 1.0e-7; for (size_t i = 0; i < 3; i++) { @@ -387,9 +387,9 @@ TEST(Quaternion, FrameConversion) { * @brief Test for ConvertToVector */ TEST(Quaternion, ConvertToVector) { - math::Quaternion q(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); - math::Vector<4> v = q.ConvertToVector(); + s2e::math::Vector<4> v = q.ConvertToVector(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(q[i], v[i]); @@ -400,10 +400,10 @@ TEST(Quaternion, ConvertToVector) { * @brief Test for operator+ */ TEST(Quaternion, OperatorPlus) { - math::Quaternion q1(0.5, 0.3, 0.1, 1.0); - math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); + s2e::math::Quaternion q1(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); - math::Quaternion result = q1 + q2; + s2e::math::Quaternion result = q1 + q2; EXPECT_DOUBLE_EQ(0.2, result[0]); EXPECT_DOUBLE_EQ(0.4, result[1]); @@ -415,10 +415,10 @@ TEST(Quaternion, OperatorPlus) { * @brief Test for operator- */ TEST(Quaternion, OperatorMinus) { - math::Quaternion q1(0.5, 0.3, 0.1, 1.0); - math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); + s2e::math::Quaternion q1(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); - math::Quaternion result = q1 - q2; + s2e::math::Quaternion result = q1 - q2; EXPECT_DOUBLE_EQ(0.8, result[0]); EXPECT_DOUBLE_EQ(0.2, result[1]); @@ -430,10 +430,10 @@ TEST(Quaternion, OperatorMinus) { * @brief Test for operator* quaternion */ TEST(Quaternion, OperatorQuaternionMultiply) { - math::Quaternion q1(0.289271, -0.576012, -0.420972, 0.638212); - math::Quaternion q2(-0.0821846, 0.501761, 0.721995, -0.469259); + s2e::math::Quaternion q1(0.289271, -0.576012, -0.420972, 0.638212); + s2e::math::Quaternion q2(-0.0821846, 0.501761, 0.721995, -0.469259); - math::Quaternion result = q1 * q2; + s2e::math::Quaternion result = q1 * q2; const double accuracy = 1.0e-7; EXPECT_NEAR(-0.3928446703722, result[0], accuracy); @@ -446,10 +446,10 @@ TEST(Quaternion, OperatorQuaternionMultiply) { * @brief Test for operator* scalar */ TEST(Quaternion, OperatorScalarMultiply) { - math::Quaternion q(0.289271, -0.576012, -0.420972, 0.638212); + s2e::math::Quaternion q(0.289271, -0.576012, -0.420972, 0.638212); double scalar = 2.3; - math::Quaternion result = scalar * q; + s2e::math::Quaternion result = scalar * q; const double accuracy = 1.0e-7; EXPECT_NEAR(q[0] * 2.3, result[0], accuracy); diff --git a/src/math_physics/math/test_s2e_math.cpp b/src/math_physics/math/test_s2e_math.cpp index 949751a3b..7a293b967 100644 --- a/src/math_physics/math/test_s2e_math.cpp +++ b/src/math_physics/math/test_s2e_math.cpp @@ -14,18 +14,18 @@ TEST(S2eMath, WrapTo2Pi) { const double accuracy = 1.0e-7; double input_angle_rad = 0.0; - double wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); + double wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); EXPECT_NEAR(0.0, wrapped_angle_rad, accuracy); input_angle_rad = -1.0e-5; - wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); - EXPECT_NEAR(math::tau + input_angle_rad, wrapped_angle_rad, accuracy); + wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); + EXPECT_NEAR(s2e::math::tau + input_angle_rad, wrapped_angle_rad, accuracy); - input_angle_rad = math::tau + 1.0e-5; - wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); + input_angle_rad = s2e::math::tau + 1.0e-5; + wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); EXPECT_NEAR(1.0e-5, wrapped_angle_rad, accuracy); - input_angle_rad = 10 * math::tau + 1.0e-5; - wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); + input_angle_rad = 10 * s2e::math::tau + 1.0e-5; + wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); EXPECT_NEAR(1.0e-5, wrapped_angle_rad, accuracy); } diff --git a/src/math_physics/math/test_vector.cpp b/src/math_physics/math/test_vector.cpp index 5f636960d..e920dc0b8 100644 --- a/src/math_physics/math/test_vector.cpp +++ b/src/math_physics/math/test_vector.cpp @@ -13,7 +13,7 @@ TEST(Vector, ConstructorWithNumber) { const size_t N = 6; double initialize_value = 2.0; - math::Vector v(initialize_value); + s2e::math::Vector v(initialize_value); for (size_t i = 0; i < N; i++) { EXPECT_DOUBLE_EQ(initialize_value, v[i]); @@ -26,7 +26,7 @@ TEST(Vector, ConstructorWithNumber) { */ TEST(Vector, GetLength) { const size_t N = 6; - math::Vector v; + s2e::math::Vector v; EXPECT_EQ(N, v.GetLength()); } @@ -37,8 +37,8 @@ TEST(Vector, GetLength) { TEST(Vector, OperatorPlusEqual) { const size_t N = 6; double initialize_value = 2.0; - math::Vector v(initialize_value); - math::Vector adding; + s2e::math::Vector v(initialize_value); + s2e::math::Vector adding; for (size_t i = 0; i < N; i++) { adding[i] = double(i); @@ -60,8 +60,8 @@ TEST(Vector, OperatorPlusEqual) { TEST(Vector, OperatorMinusEqual) { const size_t N = 6; double initialize_value = 2.0; - math::Vector v(initialize_value); - math::Vector subtracting; + s2e::math::Vector v(initialize_value); + s2e::math::Vector subtracting; for (size_t i = 0; i < N; i++) { subtracting[i] = double(i); @@ -82,7 +82,7 @@ TEST(Vector, OperatorMinusEqual) { */ TEST(Vector, OperatorMultiplyEqual) { const size_t N = 6; - math::Vector v; + s2e::math::Vector v; double multiplying = 2.0; for (size_t i = 0; i < N; i++) { @@ -104,7 +104,7 @@ TEST(Vector, OperatorMultiplyEqual) { */ TEST(Vector, OperatorDivideEqual) { const size_t N = 6; - math::Vector v; + s2e::math::Vector v; double dividing = 3.0; for (size_t i = 0; i < N; i++) { @@ -126,13 +126,13 @@ TEST(Vector, OperatorDivideEqual) { */ TEST(Vector, OperatorNegative) { const size_t N = 6; - math::Vector v; + s2e::math::Vector v; for (size_t i = 0; i < N; i++) { v[i] = double(i); } - math::Vector v_negative = -v; + s2e::math::Vector v_negative = -v; for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -147,7 +147,7 @@ TEST(Vector, OperatorNegative) { */ TEST(Vector, FillUp) { const size_t N = 6; - math::Vector v; + s2e::math::Vector v; for (size_t i = 0; i < N; i++) { v[i] = double(i); @@ -171,14 +171,14 @@ TEST(Vector, FillUp) { TEST(Vector, OperatorPlus) { const size_t N = 6; double initialize_value = 2.0; - math::Vector v(initialize_value); - math::Vector adding; + s2e::math::Vector v(initialize_value); + s2e::math::Vector adding; for (size_t i = 0; i < N; i++) { adding[i] = double(i); } - math::Vector added = v + adding; + s2e::math::Vector added = v + adding; for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -195,14 +195,14 @@ TEST(Vector, OperatorPlus) { TEST(Vector, OperatorMinus) { const size_t N = 6; double initialize_value = 2.0; - math::Vector v(initialize_value); - math::Vector subtracting; + s2e::math::Vector v(initialize_value); + s2e::math::Vector subtracting; for (size_t i = 0; i < N; i++) { subtracting[i] = double(i); } - math::Vector subtracted = v - subtracting; + s2e::math::Vector subtracted = v - subtracting; for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -218,8 +218,8 @@ TEST(Vector, OperatorMinus) { */ TEST(Vector, InnerProduct) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; for (size_t i = 0; i < N; i++) { a[i] = double(i + 1); @@ -235,8 +235,8 @@ TEST(Vector, InnerProduct) { */ TEST(Vector, InnerProductZero) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -255,8 +255,8 @@ TEST(Vector, InnerProductZero) { */ TEST(Vector, OuterProductZero) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -266,7 +266,7 @@ TEST(Vector, OuterProductZero) { b[1] = 0.0; b[2] = 0.0; - math::Vector<3> result = OuterProduct(a, b); + s2e::math::Vector<3> result = OuterProduct(a, b); for (size_t i = 0; i < N; i++) { EXPECT_DOUBLE_EQ(0.0, result[i]); @@ -278,8 +278,8 @@ TEST(Vector, OuterProductZero) { */ TEST(Vector, OuterProductX) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 0.0; a[1] = 0.0; @@ -289,7 +289,7 @@ TEST(Vector, OuterProductX) { b[1] = 1.0; b[2] = 0.0; - math::Vector<3> result = OuterProduct(a, b); + s2e::math::Vector<3> result = OuterProduct(a, b); EXPECT_DOUBLE_EQ(-1.0, result[0]); EXPECT_DOUBLE_EQ(0.0, result[1]); @@ -301,8 +301,8 @@ TEST(Vector, OuterProductX) { */ TEST(Vector, OuterProductY) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 0.0; a[1] = 0.0; @@ -312,7 +312,7 @@ TEST(Vector, OuterProductY) { b[1] = 0.0; b[2] = 0.0; - math::Vector<3> result = OuterProduct(a, b); + s2e::math::Vector<3> result = OuterProduct(a, b); EXPECT_DOUBLE_EQ(0.0, result[0]); EXPECT_DOUBLE_EQ(1.0, result[1]); @@ -324,8 +324,8 @@ TEST(Vector, OuterProductY) { */ TEST(Vector, OuterProductZ) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -335,7 +335,7 @@ TEST(Vector, OuterProductZ) { b[1] = 1.0; b[2] = 0.0; - math::Vector<3> result = OuterProduct(a, b); + s2e::math::Vector<3> result = OuterProduct(a, b); EXPECT_DOUBLE_EQ(0.0, result[0]); EXPECT_DOUBLE_EQ(0.0, result[1]); @@ -347,7 +347,7 @@ TEST(Vector, OuterProductZ) { */ TEST(Vector, CalcNorm) { const size_t N = 10; - math::Vector v(1.0); + s2e::math::Vector v(1.0); double norm = v.CalcNorm(); @@ -364,9 +364,9 @@ TEST(Vector, CalcNorm) { */ TEST(Vector, Normalize) { const size_t N = 5; - math::Vector v(1.0); + s2e::math::Vector v(1.0); - math::Vector normalized = v.CalcNormalizedVector(); + s2e::math::Vector normalized = v.CalcNormalizedVector(); for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -381,8 +381,8 @@ TEST(Vector, Normalize) { */ TEST(Vector, CalcAngleTwoVectors90deg) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -394,7 +394,7 @@ TEST(Vector, CalcAngleTwoVectors90deg) { double angle_rad = CalcAngleTwoVectors_rad(a, b); - EXPECT_DOUBLE_EQ(90.0 * math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(90.0 * s2e::math::deg_to_rad, angle_rad); } /** @@ -402,7 +402,7 @@ TEST(Vector, CalcAngleTwoVectors90deg) { */ TEST(Vector, CalcAngleTwoVectors0deg) { const size_t N = 3; - math::Vector a; + s2e::math::Vector a; a[0] = 1.0; a[1] = 0.0; @@ -410,7 +410,7 @@ TEST(Vector, CalcAngleTwoVectors0deg) { double angle_rad = CalcAngleTwoVectors_rad(a, a); - EXPECT_DOUBLE_EQ(0.0 * math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(0.0 * s2e::math::deg_to_rad, angle_rad); } /** @@ -418,8 +418,8 @@ TEST(Vector, CalcAngleTwoVectors0deg) { */ TEST(Vector, CalcAngleTwoVectors45deg) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 0.0; a[1] = 1.0; @@ -431,7 +431,7 @@ TEST(Vector, CalcAngleTwoVectors45deg) { double angle_rad = CalcAngleTwoVectors_rad(a, b); - EXPECT_DOUBLE_EQ(45.0 * math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(45.0 * s2e::math::deg_to_rad, angle_rad); } /** @@ -439,11 +439,11 @@ TEST(Vector, CalcAngleTwoVectors45deg) { */ TEST(Vector, GenerateOrthogonalUnitVector) { const size_t N = 3; - math::Vector a(1.0); + s2e::math::Vector a(1.0); - math::Vector b = GenerateOrthogonalUnitVector(a); + s2e::math::Vector b = GenerateOrthogonalUnitVector(a); double angle_rad = CalcAngleTwoVectors_rad(a, b); - EXPECT_DOUBLE_EQ(90.0 * math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(90.0 * s2e::math::deg_to_rad, angle_rad); } diff --git a/src/math_physics/math/vector.cpp b/src/math_physics/math/vector.cpp index 88c214ba7..22a74e119 100644 --- a/src/math_physics/math/vector.cpp +++ b/src/math_physics/math/vector.cpp @@ -7,7 +7,7 @@ #include "constants.hpp" -namespace math { +namespace s2e::math { Vector<3, double> ConvertFrameOrthogonal2Polar(const Vector<3, double>& orthogonal) { Vector<3, double> polar; // vector on the polar coordinate polar.FillUp(0.0); @@ -51,4 +51,4 @@ Vector<3, double> GenerateOrthogonalUnitVector(const Vector<3, double>& v) { return (orthogonal_vector); } } -} // namespace math +} // namespace s2e::math diff --git a/src/math_physics/math/vector.hpp b/src/math_physics/math/vector.hpp index 5d290d87f..333c62056 100644 --- a/src/math_physics/math/vector.hpp +++ b/src/math_physics/math/vector.hpp @@ -12,7 +12,7 @@ #define dot InnerProduct #define cross OuterProduct -namespace math { +namespace s2e::math { /** * @class Vector * @brief Class for mathematical vector @@ -237,7 +237,7 @@ Vector<3, double> ConvertFrameOrthogonal2Polar(const Vector<3, double>& orthogon */ Vector<3, double> GenerateOrthogonalUnitVector(const Vector<3, double>& v); -} // namespace math +} // namespace s2e::math #include "vector_template_functions.hpp" diff --git a/src/math_physics/math/vector_template_functions.hpp b/src/math_physics/math/vector_template_functions.hpp index 80e51ce5f..9b9900388 100644 --- a/src/math_physics/math/vector_template_functions.hpp +++ b/src/math_physics/math/vector_template_functions.hpp @@ -8,7 +8,7 @@ #include -namespace math { +namespace s2e::math { template Vector::Vector(const T& n) { @@ -146,6 +146,6 @@ double CalcAngleTwoVectors_rad(const Vector& v1, const Vector { * @param [in] sigma: Sigma value (0 < sigma < 1) for interpolation * @return : interpolated state x(t0 + sigma * h) */ - math::Vector CalcInterpolationState(const double sigma) const override; + s2e::math::Vector CalcInterpolationState(const double sigma) const override; private: - std::vector> coefficients_; //!< Coefficients to calculate interpolation weights + std::vector> coefficients_; //!< Coefficients to calculate interpolation weights /** * @fn CalcInterpolationWeights * @brief Calculate weights for interpolation diff --git a/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp b/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp index 40b09a9c1..4190202b4 100644 --- a/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp +++ b/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp @@ -75,7 +75,7 @@ DormandPrince5::DormandPrince5(const double step_width, const InterfaceOde this->rk_matrix_[6][5] = 11.0 / 84.0; // Interpolation coefficients - math::Vector<5> coefficients_temp; + s2e::math::Vector<5> coefficients_temp; coefficients_temp[0] = 11282082432.0; coefficients_temp[1] = -32272833064.0; coefficients_temp[2] = 34969693132.0; @@ -123,10 +123,10 @@ DormandPrince5::DormandPrince5(const double step_width, const InterfaceOde } template -math::Vector DormandPrince5::CalcInterpolationState(const double sigma) const { +s2e::math::Vector DormandPrince5::CalcInterpolationState(const double sigma) const { std::vector interpolation_weights = CalcInterpolationWeights(sigma); - math::Vector interpolation_state = this->previous_state_; + s2e::math::Vector interpolation_state = this->previous_state_; for (size_t i = 0; i < this->number_of_stages_; i++) { interpolation_state = interpolation_state + (sigma * this->step_width_ * interpolation_weights[i]) * this->slope_[i]; } diff --git a/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp b/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp index ac13a861f..dfed2e7e1 100644 --- a/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp +++ b/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp @@ -14,15 +14,15 @@ void EmbeddedRungeKutta::Integrate() { this->CalcSlope(); this->previous_state_ = this->current_state_; - math::Vector lower_current_state = this->current_state_; //!< eta in the equation - math::Vector higher_current_state = this->current_state_; //!< eta_hat in the equation + s2e::math::Vector lower_current_state = this->current_state_; //!< eta in the equation + s2e::math::Vector higher_current_state = this->current_state_; //!< eta_hat in the equation for (size_t i = 0; i < this->number_of_stages_; i++) { lower_current_state = lower_current_state + this->weights_[i] * this->step_width_ * this->slope_[i]; higher_current_state = higher_current_state + higher_order_weights_[i] * this->step_width_ * this->slope_[i]; } // Error evaluation - math::Vector truncation_error = lower_current_state - higher_current_state; + s2e::math::Vector truncation_error = lower_current_state - higher_current_state; local_truncation_error_ = truncation_error.CalcNorm(); // State update diff --git a/src/math_physics/numerical_integration/interface_ode.hpp b/src/math_physics/numerical_integration/interface_ode.hpp index 1c30058e9..213598675 100644 --- a/src/math_physics/numerical_integration/interface_ode.hpp +++ b/src/math_physics/numerical_integration/interface_ode.hpp @@ -24,7 +24,7 @@ class InterfaceOde { * @param [in] state: State vector * @return Differentiated value of state vector */ - virtual math::Vector DerivativeFunction(const double independent_variable, const math::Vector& state) const = 0; + virtual s2e::math::Vector DerivativeFunction(const double independent_variable, const s2e::math::Vector& state) const = 0; }; } // namespace numerical_integration diff --git a/src/math_physics/numerical_integration/numerical_integrator.hpp b/src/math_physics/numerical_integration/numerical_integrator.hpp index 5f19d0185..83d78e85a 100644 --- a/src/math_physics/numerical_integration/numerical_integrator.hpp +++ b/src/math_physics/numerical_integration/numerical_integrator.hpp @@ -44,7 +44,7 @@ class NumericalIntegrator { * @fn SetState * @brief Set state information */ - inline void SetState(const double independent_variable, const math::Vector& state) { + inline void SetState(const double independent_variable, const s2e::math::Vector& state) { current_independent_variable_ = independent_variable; current_state_ = state; previous_state_ = state; @@ -54,7 +54,7 @@ class NumericalIntegrator { * @fn GetState * @brief Return current state vector */ - inline const math::Vector& GetState() const { return current_state_; } + inline const s2e::math::Vector& GetState() const { return current_state_; } /** * @fn CalcInterpolationState @@ -62,7 +62,7 @@ class NumericalIntegrator { * @param [in] sigma: Sigma value (0 < sigma < 1) for interpolation * @return : interpolated state x(t0 + sigma * h) */ - virtual math::Vector CalcInterpolationState(const double sigma) const = 0; + virtual s2e::math::Vector CalcInterpolationState(const double sigma) const = 0; protected: // Settings @@ -71,8 +71,8 @@ class NumericalIntegrator { // States const InterfaceOde& ode_; //!< Ordinary differential equation double current_independent_variable_; //!< Latest value of independent variable - math::Vector current_state_; //!< Latest state vector - math::Vector previous_state_; //!< Previous state vector + s2e::math::Vector current_state_; //!< Latest state vector + s2e::math::Vector previous_state_; //!< Previous state vector }; } // namespace numerical_integration diff --git a/src/math_physics/numerical_integration/ode_examples.hpp b/src/math_physics/numerical_integration/ode_examples.hpp index b0c9085b5..e6f7e3616 100644 --- a/src/math_physics/numerical_integration/ode_examples.hpp +++ b/src/math_physics/numerical_integration/ode_examples.hpp @@ -13,11 +13,11 @@ namespace numerical_integration { class ExampleLinearOde : public InterfaceOde<1> { public: - math::Vector<1> DerivativeFunction(const double time_s, const math::Vector<1>& state) const { + s2e::math::Vector<1> DerivativeFunction(const double time_s, const s2e::math::Vector<1>& state) const { UNUSED(time_s); UNUSED(state); - math::Vector<1> output(1.0); + s2e::math::Vector<1> output(1.0); return output; } }; @@ -35,11 +35,11 @@ class ExampleQuadraticOde : public InterfaceOde<1> { * @param [in] state: State vector * @return Differentiated value of state vector */ - virtual math::Vector<1> DerivativeFunction(const double time_s, const math::Vector<1>& state) const { + virtual s2e::math::Vector<1> DerivativeFunction(const double time_s, const s2e::math::Vector<1>& state) const { UNUSED(time_s); UNUSED(state); - math::Vector<1> output(0.0); + s2e::math::Vector<1> output(0.0); output[0] = 2.0 * time_s; return output; } @@ -51,10 +51,10 @@ class ExampleQuadraticOde : public InterfaceOde<1> { */ class Example1dPositionVelocityOde : public InterfaceOde<2> { public: - virtual math::Vector<2> DerivativeFunction(const double time_s, const math::Vector<2>& state) const { + virtual s2e::math::Vector<2> DerivativeFunction(const double time_s, const s2e::math::Vector<2>& state) const { UNUSED(time_s); - math::Vector<2> output(0.0); + s2e::math::Vector<2> output(0.0); output[0] = state[1]; output[1] = 0.0; return output; @@ -67,10 +67,10 @@ class Example1dPositionVelocityOde : public InterfaceOde<2> { */ class Example2dTwoBodyOrbitOde : public InterfaceOde<4> { public: - virtual math::Vector<4> DerivativeFunction(const double time_s, const math::Vector<4>& state) const { + virtual s2e::math::Vector<4> DerivativeFunction(const double time_s, const s2e::math::Vector<4>& state) const { UNUSED(time_s); - math::Vector<4> output(0.0); + s2e::math::Vector<4> output(0.0); output[0] = state[2]; output[1] = state[3]; double denominator = pow(state[0] * state[0] + state[1] * state[1], 3.0 / 2.0); diff --git a/src/math_physics/numerical_integration/runge_kutta.hpp b/src/math_physics/numerical_integration/runge_kutta.hpp index 18730c667..333d3cc4a 100644 --- a/src/math_physics/numerical_integration/runge_kutta.hpp +++ b/src/math_physics/numerical_integration/runge_kutta.hpp @@ -46,7 +46,7 @@ class RungeKutta : public NumericalIntegrator { std::vector nodes_; //!< Nodes vector for general RK (c vector in the equation) std::vector weights_; //!< Weights vector for general RK (b vector in the equation) std::vector> rk_matrix_; //!< Runge-Kutta matrix for general RK (a matrix in the equation) - std::vector> slope_; //!< Slope vector for general RK (k vector in the equation) + std::vector> slope_; //!< Slope vector for general RK (k vector in the equation) /** * @fn CalcSlope diff --git a/src/math_physics/numerical_integration/runge_kutta_4.hpp b/src/math_physics/numerical_integration/runge_kutta_4.hpp index b2f267d58..47fc6b6b5 100644 --- a/src/math_physics/numerical_integration/runge_kutta_4.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_4.hpp @@ -45,7 +45,7 @@ class RungeKutta4 : public RungeKutta { } // We did not implement the interpolation for RK4 - math::Vector CalcInterpolationState(const double sigma) const override { + s2e::math::Vector CalcInterpolationState(const double sigma) const override { UNUSED(sigma); return this->current_state_; } diff --git a/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp b/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp index 3056241ac..16974368f 100644 --- a/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp @@ -29,7 +29,7 @@ class RungeKuttaFehlberg : public EmbeddedRungeKutta { * @param [in] sigma: Sigma value (0 < sigma < 1) for interpolation * @return : interpolated state x(t0 + sigma * h) */ - math::Vector CalcInterpolationState(const double sigma) const override; + s2e::math::Vector CalcInterpolationState(const double sigma) const override; private: /** diff --git a/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp b/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp index d5ef9f7c0..04d5f8027 100644 --- a/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp @@ -64,15 +64,15 @@ RungeKuttaFehlberg::RungeKuttaFehlberg(const double step_width, const Interfa } template -math::Vector RungeKuttaFehlberg::CalcInterpolationState(const double sigma) const { +s2e::math::Vector RungeKuttaFehlberg::CalcInterpolationState(const double sigma) const { // Calc k7 (slope after state update) - math::Vector state_7 = + s2e::math::Vector state_7 = this->previous_state_ + this->step_width_ * (1.0 / 6.0 * this->slope_[0] + 1.0 / 6.0 * this->slope_[4] + 2.0 / 3.0 * this->slope_[5]); - math::Vector k7 = this->ode_.DerivativeFunction(this->current_independent_variable_, state_7); + s2e::math::Vector k7 = this->ode_.DerivativeFunction(this->current_independent_variable_, state_7); std::vector interpolation_weights = CalcInterpolationWeights(sigma); - math::Vector interpolation_state = this->previous_state_; + s2e::math::Vector interpolation_state = this->previous_state_; for (size_t i = 0; i < this->number_of_stages_; i++) { interpolation_state = interpolation_state + (sigma * this->step_width_ * interpolation_weights[i]) * this->slope_[i]; } diff --git a/src/math_physics/numerical_integration/runge_kutta_template.hpp b/src/math_physics/numerical_integration/runge_kutta_template.hpp index 99bab650c..577b24639 100644 --- a/src/math_physics/numerical_integration/runge_kutta_template.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_template.hpp @@ -22,10 +22,10 @@ void RungeKutta::Integrate() { template void RungeKutta::CalcSlope() { - slope_.assign(number_of_stages_, math::Vector(0.0)); + slope_.assign(number_of_stages_, s2e::math::Vector(0.0)); for (size_t i = 0; i < number_of_stages_; i++) { - math::Vector state = this->current_state_; + s2e::math::Vector state = this->current_state_; for (size_t j = 0; j < i; j++) { state = state + rk_matrix_[i][j] * this->step_width_ * slope_[j]; } diff --git a/src/math_physics/numerical_integration/test_runge_kutta.cpp b/src/math_physics/numerical_integration/test_runge_kutta.cpp index 376406850..d6ab81d28 100644 --- a/src/math_physics/numerical_integration/test_runge_kutta.cpp +++ b/src/math_physics/numerical_integration/test_runge_kutta.cpp @@ -18,7 +18,7 @@ TEST(NUMERICAL_INTEGRATION, Constructor) { numerical_integration::ExampleLinearOde ode; numerical_integration::RungeKutta4<1> linear_ode(0.1, ode); - math::Vector<1> state = linear_ode.GetState(); + s2e::math::Vector<1> state = linear_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); } @@ -30,7 +30,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearRk4) { numerical_integration::ExampleLinearOde ode; numerical_integration::RungeKutta4<1> rk4_ode(step_width_s, ode); - math::Vector<1> state = rk4_ode.GetState(); + s2e::math::Vector<1> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -51,7 +51,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearRkf) { numerical_integration::ExampleLinearOde ode; numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); - math::Vector<1> state = rkf_ode.GetState(); + s2e::math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -72,7 +72,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearDp5) { numerical_integration::ExampleLinearOde ode; numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); - math::Vector<1> state = dp5_ode.GetState(); + s2e::math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -93,7 +93,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRk4) { numerical_integration::ExampleLinearOde ode; numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode); - math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); + s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -115,7 +115,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRkf) { numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, numerical_integration::NumericalIntegrationMethod::kRkf); - math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); + s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -141,7 +141,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerDp5) { numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, numerical_integration::NumericalIntegrationMethod::kDp5); - math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); + s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -166,7 +166,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticRk4) { numerical_integration::ExampleQuadraticOde ode; numerical_integration::RungeKutta4<1> rk4_ode(step_width_s, ode); - math::Vector<1> state = rk4_ode.GetState(); + s2e::math::Vector<1> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -187,7 +187,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticRkf) { numerical_integration::ExampleQuadraticOde ode; numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); - math::Vector<1> state = rkf_ode.GetState(); + s2e::math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -208,7 +208,7 @@ TEST(NUMERICAL_INTEGRATION, InterpolationQuadraticRkf) { numerical_integration::ExampleQuadraticOde ode; numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); - math::Vector<1> state = rkf_ode.GetState(); + s2e::math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); rkf_ode.Integrate(); @@ -237,7 +237,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticDp5) { numerical_integration::ExampleQuadraticOde ode; numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); - math::Vector<1> state = dp5_ode.GetState(); + s2e::math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -258,7 +258,7 @@ TEST(NUMERICAL_INTEGRATION, InterpolationQuadraticDp5) { numerical_integration::ExampleQuadraticOde ode; numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); - math::Vector<1> state = dp5_ode.GetState(); + s2e::math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); dp5_ode.Integrate(); @@ -287,12 +287,12 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRk4) { numerical_integration::Example1dPositionVelocityOde ode; numerical_integration::RungeKutta4<2> rk4_ode(step_width_s, ode); - math::Vector<2> initial_state(0.0); + s2e::math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; initial_state[1] = 0.1; rk4_ode.SetState(0.0, initial_state); - math::Vector<2> state = rk4_ode.GetState(); + s2e::math::Vector<2> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(initial_state[0], state[0]); EXPECT_DOUBLE_EQ(initial_state[1], state[1]); @@ -301,7 +301,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRk4) { rk4_ode.Integrate(); } state = rk4_ode.GetState(); - math::Vector<2> estimated_result(0.0); + s2e::math::Vector<2> estimated_result(0.0); estimated_result[0] = (step_width_s * step_num) * initial_state[1] + initial_state[0]; estimated_result[1] = initial_state[1]; @@ -317,12 +317,12 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRkf) { numerical_integration::Example1dPositionVelocityOde ode; numerical_integration::RungeKuttaFehlberg<2> rkf_ode(step_width_s, ode); - math::Vector<2> initial_state(0.0); + s2e::math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; initial_state[1] = 0.1; rkf_ode.SetState(0.0, initial_state); - math::Vector<2> state = rkf_ode.GetState(); + s2e::math::Vector<2> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(initial_state[0], state[0]); EXPECT_DOUBLE_EQ(initial_state[1], state[1]); @@ -331,7 +331,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRkf) { rkf_ode.Integrate(); } state = rkf_ode.GetState(); - math::Vector<2> estimated_result(0.0); + s2e::math::Vector<2> estimated_result(0.0); estimated_result[0] = (step_width_s * step_num) * initial_state[1] + initial_state[0]; estimated_result[1] = initial_state[1]; @@ -347,12 +347,12 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityDp5) { numerical_integration::Example1dPositionVelocityOde ode; numerical_integration::DormandPrince5<2> dp5_ode(step_width_s, ode); - math::Vector<2> initial_state(0.0); + s2e::math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; initial_state[1] = 0.1; dp5_ode.SetState(0.0, initial_state); - math::Vector<2> state = dp5_ode.GetState(); + s2e::math::Vector<2> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(initial_state[0], state[0]); EXPECT_DOUBLE_EQ(initial_state[1], state[1]); @@ -361,7 +361,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityDp5) { dp5_ode.Integrate(); } state = dp5_ode.GetState(); - math::Vector<2> estimated_result(0.0); + s2e::math::Vector<2> estimated_result(0.0); estimated_result[0] = (step_width_s * step_num) * initial_state[1] + initial_state[0]; estimated_result[1] = initial_state[1]; @@ -379,7 +379,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - math::Vector<4> initial_state(0.0); + s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.1; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -389,9 +389,9 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - math::Vector<4> state_rk4 = rk4_ode.GetState(); - math::Vector<4> state_rkf = rkf_ode.GetState(); - math::Vector<4> state_dp5 = dp5_ode.GetState(); + s2e::math::Vector<4> state_rk4 = rk4_ode.GetState(); + s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); + s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rk4[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); @@ -409,8 +409,8 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - math::Vector<3> initial_position(0.0); - math::Vector<3> initial_velocity(0.0); + s2e::math::Vector<3> initial_position(0.0); + s2e::math::Vector<3> initial_velocity(0.0); initial_position[0] = initial_state[0]; initial_position[1] = initial_state[1]; @@ -449,7 +449,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - math::Vector<4> initial_state(0.0); + s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.9; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -459,9 +459,9 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - math::Vector<4> state_rk4 = rk4_ode.GetState(); - math::Vector<4> state_rkf = rkf_ode.GetState(); - math::Vector<4> state_dp5 = dp5_ode.GetState(); + s2e::math::Vector<4> state_rk4 = rk4_ode.GetState(); + s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); + s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rk4[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); @@ -479,8 +479,8 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - math::Vector<3> initial_position(0.0); - math::Vector<3> initial_velocity(0.0); + s2e::math::Vector<3> initial_position(0.0); + s2e::math::Vector<3> initial_velocity(0.0); initial_position[0] = initial_state[0]; initial_position[1] = initial_state[1]; @@ -518,7 +518,7 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - math::Vector<4> initial_state(0.0); + s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.1; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -527,8 +527,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - math::Vector<4> state_rkf = rkf_ode.GetState(); - math::Vector<4> state_dp5 = dp5_ode.GetState(); + s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); + s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_dp5[i]); @@ -543,8 +543,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - math::Vector<3> initial_position(0.0); - math::Vector<3> initial_velocity(0.0); + s2e::math::Vector<3> initial_position(0.0); + s2e::math::Vector<3> initial_velocity(0.0); // Final value initial_position[0] = initial_state[0]; @@ -613,7 +613,7 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - math::Vector<4> initial_state(0.0); + s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.8; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -622,8 +622,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - math::Vector<4> state_rkf = rkf_ode.GetState(); - math::Vector<4> state_dp5 = dp5_ode.GetState(); + s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); + s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_dp5[i]); @@ -638,8 +638,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - math::Vector<3> initial_position(0.0); - math::Vector<3> initial_velocity(0.0); + s2e::math::Vector<3> initial_position(0.0); + s2e::math::Vector<3> initial_velocity(0.0); // Final value initial_position[0] = initial_state[0]; diff --git a/src/math_physics/optics/gaussian_beam_base.cpp b/src/math_physics/optics/gaussian_beam_base.cpp index 86995e7e2..3b558612d 100644 --- a/src/math_physics/optics/gaussian_beam_base.cpp +++ b/src/math_physics/optics/gaussian_beam_base.cpp @@ -30,19 +30,19 @@ void GaussianBeamBase::SetTotalPower_W(const double total_power_W) { total_power_W_ = total_power_W; } -void GaussianBeamBase::SetPointingVector_i(const math::Vector<3> pointing_vector_i) { pointing_vector_i_ = pointing_vector_i; } +void GaussianBeamBase::SetPointingVector_i(const s2e::math::Vector<3> pointing_vector_i) { pointing_vector_i_ = pointing_vector_i; } -void GaussianBeamBase::SetBeamWaistPosition_i_m(const math::Vector<3> position_beam_waist_i_m) { position_beam_waist_i_m_ = position_beam_waist_i_m; } +void GaussianBeamBase::SetBeamWaistPosition_i_m(const s2e::math::Vector<3> position_beam_waist_i_m) { position_beam_waist_i_m_ = position_beam_waist_i_m; } double GaussianBeamBase::CalcBeamWidthRadius_m(double distance_from_beam_waist_m) { - double rayleigh_length_m = math::pi * radius_beam_waist_m_ * radius_beam_waist_m_ / wavelength_m_; + double rayleigh_length_m = s2e::math::pi * radius_beam_waist_m_ * radius_beam_waist_m_ / wavelength_m_; double beam_width_radius_m = radius_beam_waist_m_ * sqrt(1.0 + std::pow((distance_from_beam_waist_m / rayleigh_length_m), 2.0)); return beam_width_radius_m; } double GaussianBeamBase::CalcIntensity_W_m2(double distance_from_beam_waist_m, double deviation_from_optical_axis_m) { double beam_width_radius_m = CalcBeamWidthRadius_m(distance_from_beam_waist_m); - double peak_intensity_W_m2 = (2.0 * total_power_W_) / (math::pi * beam_width_radius_m * beam_width_radius_m); + double peak_intensity_W_m2 = (2.0 * total_power_W_) / (s2e::math::pi * beam_width_radius_m * beam_width_radius_m); double gaussian_dist = std::exp((-2.0 * deviation_from_optical_axis_m * deviation_from_optical_axis_m) / (beam_width_radius_m * beam_width_radius_m)); double intensity_W_m2 = peak_intensity_W_m2 * gaussian_dist; diff --git a/src/math_physics/optics/gaussian_beam_base.hpp b/src/math_physics/optics/gaussian_beam_base.hpp index 6f40fa8a2..010d4bb31 100644 --- a/src/math_physics/optics/gaussian_beam_base.hpp +++ b/src/math_physics/optics/gaussian_beam_base.hpp @@ -50,12 +50,12 @@ class GaussianBeamBase { * @fn SetPointingVector_i * @brief Set pointing direction vector in the inertial frame */ - void SetPointingVector_i(const math::Vector<3> pointing_vector_i); + void SetPointingVector_i(const s2e::math::Vector<3> pointing_vector_i); /** * @fn SetBeamWaistPosition_i_m * @brief Set position of beam waist in the inertial frame [m] (Not used?) */ - void SetBeamWaistPosition_i_m(const math::Vector<3> position_beam_waist_i_m); + void SetBeamWaistPosition_i_m(const s2e::math::Vector<3> position_beam_waist_i_m); // Getter /** @@ -77,12 +77,12 @@ class GaussianBeamBase { * @fn GetPointingVector_i * @brief Return pointing direction vector in the inertial frame */ - inline const math::Vector<3> GetPointingVector_i() const { return pointing_vector_i_; } + inline const s2e::math::Vector<3> GetPointingVector_i() const { return pointing_vector_i_; } /** * @fn GetBeamWaistPosition_i_m * @brief Return position of beam waist in the inertial frame [m] (Not used?) */ - inline const math::Vector<3> GetBeamWaistPosition_i_m() const { return position_beam_waist_i_m_; } + inline const s2e::math::Vector<3> GetBeamWaistPosition_i_m() const { return position_beam_waist_i_m_; } // Calculate functions /** @@ -105,8 +105,8 @@ class GaussianBeamBase { double wavelength_m_; //!< Wavelength [m] double radius_beam_waist_m_; //!< Radius of beam waist [m] double total_power_W_; //!< Total power [W] - math::Vector<3> pointing_vector_i_{0.0}; //!< Pointing direction vector in the inertial frame - math::Vector<3> position_beam_waist_i_m_{0.0}; //!< Position of beam waist in the inertial frame [m] (Not used?) + s2e::math::Vector<3> pointing_vector_i_{0.0}; //!< Pointing direction vector in the inertial frame + s2e::math::Vector<3> position_beam_waist_i_m_{0.0}; //!< Position of beam waist in the inertial frame [m] (Not used?) }; } // namespace optics diff --git a/src/math_physics/orbit/interpolation_orbit.cpp b/src/math_physics/orbit/interpolation_orbit.cpp index 33d2843a5..393b30166 100644 --- a/src/math_physics/orbit/interpolation_orbit.cpp +++ b/src/math_physics/orbit/interpolation_orbit.cpp @@ -12,13 +12,13 @@ InterpolationOrbit::InterpolationOrbit(const size_t degree) { time.assign(degree, -1.0); std::vector position; position.assign(degree, 0.0); - math::Interpolation temp(time, position); + s2e::math::Interpolation temp(time, position); for (size_t axis = 0; axis < 3; axis++) { interpolation_position_.push_back(temp); } } -bool InterpolationOrbit::PushAndPopData(const double time, const math::Vector<3> position) { +bool InterpolationOrbit::PushAndPopData(const double time, const s2e::math::Vector<3> position) { bool result; for (size_t axis = 0; axis < 3; axis++) { result = interpolation_position_[axis].PushAndPopData(time, position[axis]); @@ -29,8 +29,8 @@ bool InterpolationOrbit::PushAndPopData(const double time, const math::Vector<3> return true; } -math::Vector<3> InterpolationOrbit::CalcPositionWithTrigonometric(const double time, const double period) const { - math::Vector<3> output_position; +s2e::math::Vector<3> InterpolationOrbit::CalcPositionWithTrigonometric(const double time, const double period) const { + s2e::math::Vector<3> output_position; for (size_t axis = 0; axis < 3; axis++) { output_position[axis] = interpolation_position_[axis].CalcTrigonometric(time, period); } diff --git a/src/math_physics/orbit/interpolation_orbit.hpp b/src/math_physics/orbit/interpolation_orbit.hpp index cf83660ab..db215f534 100644 --- a/src/math_physics/orbit/interpolation_orbit.hpp +++ b/src/math_physics/orbit/interpolation_orbit.hpp @@ -32,7 +32,7 @@ class InterpolationOrbit { * @param [in] time: time of the new data * @param [in] position: Satellite position of the new data */ - bool PushAndPopData(const double time, const math::Vector<3> position); + bool PushAndPopData(const double time, const s2e::math::Vector<3> position); /** * @fn CalcPositionWithTrigonometric @@ -41,7 +41,7 @@ class InterpolationOrbit { * @param [in] period: Characteristic period * @return Calculated position */ - math::Vector<3> CalcPositionWithTrigonometric(const double time, const double period = 0.0) const; + s2e::math::Vector<3> CalcPositionWithTrigonometric(const double time, const double period = 0.0) const; // Getters /** @@ -68,7 +68,7 @@ class InterpolationOrbit { } private: - std::vector interpolation_position_; // 3D vector of interpolation + std::vector interpolation_position_; // 3D vector of interpolation }; } // namespace orbit diff --git a/src/math_physics/orbit/kepler_orbit.cpp b/src/math_physics/orbit/kepler_orbit.cpp index 2b3878703..389cc96ab 100644 --- a/src/math_physics/orbit/kepler_orbit.cpp +++ b/src/math_physics/orbit/kepler_orbit.cpp @@ -24,10 +24,10 @@ void KeplerOrbit::CalcConstKeplerMotion() { mean_motion_rad_s_ = sqrt(gravity_constant_m3_s2_ / a_m3); // DCM - math::Matrix<3, 3> dcm_arg_perigee = math::MakeRotationMatrixZ(-1.0 * oe_.GetArgPerigee_rad()); - math::Matrix<3, 3> dcm_inclination = math::MakeRotationMatrixX(-1.0 * oe_.GetInclination_rad()); - math::Matrix<3, 3> dcm_raan = math::MakeRotationMatrixZ(-1.0 * oe_.GetRaan_rad()); - math::Matrix<3, 3> dcm_inc_arg = dcm_inclination * dcm_arg_perigee; + s2e::math::Matrix<3, 3> dcm_arg_perigee = s2e::math::MakeRotationMatrixZ(-1.0 * oe_.GetArgPerigee_rad()); + s2e::math::Matrix<3, 3> dcm_inclination = s2e::math::MakeRotationMatrixX(-1.0 * oe_.GetInclination_rad()); + s2e::math::Matrix<3, 3> dcm_raan = s2e::math::MakeRotationMatrixZ(-1.0 * oe_.GetRaan_rad()); + s2e::math::Matrix<3, 3> dcm_inc_arg = dcm_inclination * dcm_arg_perigee; dcm_inplane_to_i_ = dcm_raan * dcm_inc_arg; } @@ -39,12 +39,12 @@ void KeplerOrbit::CalcOrbit(double time_jday) { double dt_s = (time_jday - oe_.GetEpoch_jday()) * (24.0 * 60.0 * 60.0); double mean_anomaly_rad = mean_motion_rad_s_ * dt_s; - double l_rad = math::WrapTo2Pi(mean_anomaly_rad); + double l_rad = s2e::math::WrapTo2Pi(mean_anomaly_rad); // Solve Kepler Equation double eccentric_anomaly_rad; eccentric_anomaly_rad = SolveKeplerNewtonMethod(e, l_rad, 1.0e-5, 10); - double u_rad = math::WrapTo2Pi(eccentric_anomaly_rad); + double u_rad = s2e::math::WrapTo2Pi(eccentric_anomaly_rad); // Calc position and velocity in the plane double cos_u = cos(u_rad); @@ -52,12 +52,12 @@ void KeplerOrbit::CalcOrbit(double time_jday) { double a_sqrt_e_m = a_m * sqrt(1.0 - e * e); double e_cos_u = 1.0 - e * cos_u; - math::Vector<3> pos_inplane_m; + s2e::math::Vector<3> pos_inplane_m; pos_inplane_m[0] = a_m * (cos_u - e); pos_inplane_m[1] = a_sqrt_e_m * sin_u; pos_inplane_m[2] = 0.0; - math::Vector<3> vel_inplane_m_s; + s2e::math::Vector<3> vel_inplane_m_s; vel_inplane_m_s[0] = -1.0 * a_m * n_rad_s * sin_u / e_cos_u; vel_inplane_m_s[1] = n_rad_s * a_sqrt_e_m * cos_u / e_cos_u; vel_inplane_m_s[2] = 0.0; diff --git a/src/math_physics/orbit/kepler_orbit.hpp b/src/math_physics/orbit/kepler_orbit.hpp index fec55f130..b76f15cac 100644 --- a/src/math_physics/orbit/kepler_orbit.hpp +++ b/src/math_physics/orbit/kepler_orbit.hpp @@ -47,22 +47,22 @@ class KeplerOrbit { * @fn GetPosition_i_m * @brief Return position vector in the inertial frame [m] */ - inline const math::Vector<3> GetPosition_i_m() const { return position_i_m_; } + inline const s2e::math::Vector<3> GetPosition_i_m() const { return position_i_m_; } /** * @fn GetVelocity_i_m_s * @brief Return velocity vector in the inertial frame [m/s] */ - inline const math::Vector<3> GetVelocity_i_m_s() const { return velocity_i_m_s_; } + inline const s2e::math::Vector<3> GetVelocity_i_m_s() const { return velocity_i_m_s_; } protected: - math::Vector<3> position_i_m_; //!< Position vector in the inertial frame [m] - math::Vector<3> velocity_i_m_s_; //!< Velocity vector in the inertial frame [m/s] + s2e::math::Vector<3> position_i_m_; //!< Position vector in the inertial frame [m] + s2e::math::Vector<3> velocity_i_m_s_; //!< Velocity vector in the inertial frame [m/s] private: double gravity_constant_m3_s2_; //!< Gravity constant of the center body [m3/s2] OrbitalElements oe_; //!< Orbital elements double mean_motion_rad_s_; //!< Mean motion of the orbit [rad/s] - math::Matrix<3, 3> dcm_inplane_to_i_; //!< Direction cosine matrix from the in-plane frame to the inertial frame + s2e::math::Matrix<3, 3> dcm_inplane_to_i_; //!< Direction cosine matrix from the in-plane frame to the inertial frame /** * @fn CalcConstKeplerMotion diff --git a/src/math_physics/orbit/orbital_elements.cpp b/src/math_physics/orbit/orbital_elements.cpp index 3fd2920b7..4c02fac73 100644 --- a/src/math_physics/orbit/orbital_elements.cpp +++ b/src/math_physics/orbit/orbital_elements.cpp @@ -24,20 +24,20 @@ OrbitalElements::OrbitalElements(const double epoch_jday, const double semi_majo epoch_jday_(epoch_jday) {} // initialize with position and velocity -OrbitalElements::OrbitalElements(const double gravity_constant_m3_s2, const double time_jday, const math::Vector<3> position_i_m, - const math::Vector<3> velocity_i_m_s) { +OrbitalElements::OrbitalElements(const double gravity_constant_m3_s2, const double time_jday, const s2e::math::Vector<3> position_i_m, + const s2e::math::Vector<3> velocity_i_m_s) { CalcOeFromPosVel(gravity_constant_m3_s2, time_jday, position_i_m, velocity_i_m_s); } OrbitalElements::~OrbitalElements() {} // Private Function -void OrbitalElements::CalcOeFromPosVel(const double gravity_constant_m3_s2, const double time_jday, const math::Vector<3> position_i_m, - const math::Vector<3> velocity_i_m_s) { +void OrbitalElements::CalcOeFromPosVel(const double gravity_constant_m3_s2, const double time_jday, const s2e::math::Vector<3> position_i_m, + const s2e::math::Vector<3> velocity_i_m_s) { // common variables double r_m = position_i_m.CalcNorm(); double v2_m2_s2 = InnerProduct(velocity_i_m_s, velocity_i_m_s); - math::Vector<3> h; //!< angular momentum vector + s2e::math::Vector<3> h; //!< angular momentum vector h = OuterProduct(position_i_m, velocity_i_m_s); double h_norm = h.CalcNorm(); @@ -45,7 +45,7 @@ void OrbitalElements::CalcOeFromPosVel(const double gravity_constant_m3_s2, cons semi_major_axis_m_ = gravity_constant_m3_s2 / (2.0 * gravity_constant_m3_s2 / r_m - v2_m2_s2); // inclination - math::Vector<3> h_direction = h.CalcNormalizedVector(); + s2e::math::Vector<3> h_direction = h.CalcNormalizedVector(); inclination_rad_ = acos(h_direction[2]); // RAAN @@ -77,10 +77,10 @@ void OrbitalElements::CalcOeFromPosVel(const double gravity_constant_m3_s2, cons // true anomaly f_rad and eccentric anomaly u_rad double phi_rad = atan2(y_p_m, x_p_m); double f_rad = phi_rad - arg_perigee_rad_; - f_rad = math::WrapTo2Pi(f_rad); + f_rad = s2e::math::WrapTo2Pi(f_rad); double u_rad = atan2(r_m * sin(f_rad) / sqrt(1.0 - eccentricity_ * eccentricity_), r_m * cos(f_rad) + semi_major_axis_m_ * eccentricity_); - u_rad = math::WrapTo2Pi(u_rad); + u_rad = s2e::math::WrapTo2Pi(u_rad); // epoch t0 double n_rad_s = sqrt(gravity_constant_m3_s2 / pow(semi_major_axis_m_, 3.0)); diff --git a/src/math_physics/orbit/orbital_elements.hpp b/src/math_physics/orbit/orbital_elements.hpp index 27424653e..66ccf0c05 100644 --- a/src/math_physics/orbit/orbital_elements.hpp +++ b/src/math_physics/orbit/orbital_elements.hpp @@ -41,8 +41,8 @@ class OrbitalElements { * @param[in] position_i_m: Position vector in the inertial frame [m] * @param[in] velocity_i_m_s: Velocity vector in the inertial frame [m/s] */ - OrbitalElements(const double gravity_constant_m3_s2, const double time_jday, const math::Vector<3> position_i_m, - const math::Vector<3> velocity_i_m_s); + OrbitalElements(const double gravity_constant_m3_s2, const double time_jday, const s2e::math::Vector<3> position_i_m, + const s2e::math::Vector<3> velocity_i_m_s); /** * @fn ~OrbitalElements * @brief Destructor @@ -102,8 +102,8 @@ class OrbitalElements { * @param[in] position_i_m: Position vector in the inertial frame [m] * @param[in] velocity_i_m_s: Velocity vector in the inertial frame [m/s] */ - void CalcOeFromPosVel(const double gravity_constant_m3_s2, const double time_jday, const math::Vector<3> position_i_m, - const math::Vector<3> velocity_i_m_s); + void CalcOeFromPosVel(const double gravity_constant_m3_s2, const double time_jday, const s2e::math::Vector<3> position_i_m, + const s2e::math::Vector<3> velocity_i_m_s); }; } // namespace orbit diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index a291e4633..b8d619734 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -6,8 +6,8 @@ namespace orbit { -math::Matrix<6, 6> CalcHillSystemMatrix(double orbit_radius_m, double gravity_constant_m3_s2) { - math::Matrix<6, 6> system_matrix; +s2e::math::Matrix<6, 6> CalcHillSystemMatrix(double orbit_radius_m, double gravity_constant_m3_s2) { + s2e::math::Matrix<6, 6> system_matrix; double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); system_matrix[0][0] = 0.0; @@ -50,8 +50,8 @@ math::Matrix<6, 6> CalcHillSystemMatrix(double orbit_radius_m, double gravity_co return system_matrix; } -math::Matrix<6, 6> CalcHcwStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s) { - math::Matrix<6, 6> stm; +s2e::math::Matrix<6, 6> CalcHcwStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s) { + s2e::math::Matrix<6, 6> stm; double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double t = elapsed_time_s; diff --git a/src/math_physics/orbit/relative_orbit_models.hpp b/src/math_physics/orbit/relative_orbit_models.hpp index 881d73bfb..62957b28c 100644 --- a/src/math_physics/orbit/relative_orbit_models.hpp +++ b/src/math_physics/orbit/relative_orbit_models.hpp @@ -31,7 +31,7 @@ enum class StmModel { kHcw = 0 }; * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] * @return System matrix */ -math::Matrix<6, 6> CalcHillSystemMatrix(const double orbit_radius_m, const double gravity_constant_m3_s2); +s2e::math::Matrix<6, 6> CalcHillSystemMatrix(const double orbit_radius_m, const double gravity_constant_m3_s2); // STMs /** @@ -42,7 +42,7 @@ math::Matrix<6, 6> CalcHillSystemMatrix(const double orbit_radius_m, const doubl * @param [in] elapsed_time_s: Elapsed time [s] * @return State Transition Matrix */ -math::Matrix<6, 6> CalcHcwStm(const double orbit_radius_m, const double gravity_constant_m3_s2, const double elapsed_time_s); +s2e::math::Matrix<6, 6> CalcHcwStm(const double orbit_radius_m, const double gravity_constant_m3_s2, const double elapsed_time_s); } // namespace orbit diff --git a/src/math_physics/orbit/test_interpolation_orbit.cpp b/src/math_physics/orbit/test_interpolation_orbit.cpp index d7fe51c67..b0972817d 100644 --- a/src/math_physics/orbit/test_interpolation_orbit.cpp +++ b/src/math_physics/orbit/test_interpolation_orbit.cpp @@ -33,7 +33,7 @@ TEST(InterpolationOrbit, PushAndPop) { EXPECT_EQ(degree, interpolation_orbit.GetDegree()); for (size_t i = 0; i < degree; i++) { double time = (double)i; - math::Vector<3> position{i * 2.0}; + s2e::math::Vector<3> position{i * 2.0}; bool ret = interpolation_orbit.PushAndPopData(time, position); EXPECT_TRUE(ret); } @@ -46,7 +46,7 @@ TEST(InterpolationOrbit, PushAndPop) { // False test double time = 2.0; - math::Vector<3> position{-100.0}; + s2e::math::Vector<3> position{-100.0}; bool ret = interpolation_orbit.PushAndPopData(time, position); EXPECT_FALSE(ret); } diff --git a/src/math_physics/planet_rotation/moon_rotation_utilities.cpp b/src/math_physics/planet_rotation/moon_rotation_utilities.cpp index cd6dc020a..0842aa301 100644 --- a/src/math_physics/planet_rotation/moon_rotation_utilities.cpp +++ b/src/math_physics/planet_rotation/moon_rotation_utilities.cpp @@ -12,22 +12,22 @@ namespace planet_rotation { -math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const math::Vector<3> moon_position_eci_m, const math::Vector<3> moon_velocity_eci_m_s) { - math::Matrix<3, 3> dcm_eci2me = CalcDcmEciToMeanEarth(moon_position_eci_m, moon_velocity_eci_m_s); - math::Matrix<3, 3> dcm_me2pa = CalcDcmMeanEarthToPrincipalAxis(); +s2e::math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const s2e::math::Vector<3> moon_position_eci_m, const s2e::math::Vector<3> moon_velocity_eci_m_s) { + s2e::math::Matrix<3, 3> dcm_eci2me = CalcDcmEciToMeanEarth(moon_position_eci_m, moon_velocity_eci_m_s); + s2e::math::Matrix<3, 3> dcm_me2pa = CalcDcmMeanEarthToPrincipalAxis(); return dcm_me2pa * dcm_eci2me; } -math::Matrix<3, 3> CalcDcmEciToMeanEarth(const math::Vector<3> moon_position_eci_m, const math::Vector<3> moon_velocity_eci_m_s) { - math::Vector<3> me_ex_eci = -1.0 * moon_position_eci_m.CalcNormalizedVector(); +s2e::math::Matrix<3, 3> CalcDcmEciToMeanEarth(const s2e::math::Vector<3> moon_position_eci_m, const s2e::math::Vector<3> moon_velocity_eci_m_s) { + s2e::math::Vector<3> me_ex_eci = -1.0 * moon_position_eci_m.CalcNormalizedVector(); - math::Vector<3> moon_orbit_norm = math::OuterProduct(moon_position_eci_m, moon_velocity_eci_m_s); - math::Vector<3> me_ez_eci = moon_orbit_norm.CalcNormalizedVector(); + s2e::math::Vector<3> moon_orbit_norm = s2e::math::OuterProduct(moon_position_eci_m, moon_velocity_eci_m_s); + s2e::math::Vector<3> me_ez_eci = moon_orbit_norm.CalcNormalizedVector(); - math::Vector<3> me_ey_eci = math::OuterProduct(me_ez_eci, me_ex_eci); + s2e::math::Vector<3> me_ey_eci = s2e::math::OuterProduct(me_ez_eci, me_ex_eci); - math::Matrix<3, 3> dcm_eci_to_me; + s2e::math::Matrix<3, 3> dcm_eci_to_me; for (size_t i = 0; i < 3; i++) { dcm_eci_to_me[0][i] = me_ex_eci[i]; dcm_eci_to_me[1][i] = me_ey_eci[i]; @@ -37,14 +37,14 @@ math::Matrix<3, 3> CalcDcmEciToMeanEarth(const math::Vector<3> moon_position_eci return dcm_eci_to_me; } -math::Matrix<3, 3> CalcDcmMeanEarthToPrincipalAxis() { +s2e::math::Matrix<3, 3> CalcDcmMeanEarthToPrincipalAxis() { // The correction values between DE430 Principal Axis and Mean Earth frame - const double theta_x_rad = 0.285 * math::arcsec_to_rad; - const double theta_y_rad = 78.580 * math::arcsec_to_rad; - const double theta_z_rad = 67.573 * math::arcsec_to_rad; + const double theta_x_rad = 0.285 * s2e::math::arcsec_to_rad; + const double theta_y_rad = 78.580 * s2e::math::arcsec_to_rad; + const double theta_z_rad = 67.573 * s2e::math::arcsec_to_rad; - math::Matrix<3, 3> dcm_me_pa = - math::MakeRotationMatrixZ(theta_z_rad) * math::MakeRotationMatrixY(theta_y_rad) * math::MakeRotationMatrixX(theta_x_rad); + s2e::math::Matrix<3, 3> dcm_me_pa = + s2e::math::MakeRotationMatrixZ(theta_z_rad) * s2e::math::MakeRotationMatrixY(theta_y_rad) * s2e::math::MakeRotationMatrixX(theta_x_rad); return dcm_me_pa; } diff --git a/src/math_physics/planet_rotation/moon_rotation_utilities.hpp b/src/math_physics/planet_rotation/moon_rotation_utilities.hpp index 9ce97f876..cd3c8b442 100644 --- a/src/math_physics/planet_rotation/moon_rotation_utilities.hpp +++ b/src/math_physics/planet_rotation/moon_rotation_utilities.hpp @@ -20,7 +20,7 @@ namespace planet_rotation { * @param[in] moon_position_eci_m: Moon position vector @ ECI frame [m] * @param[in] moon_velocity_eci_m_s: Moon velocity vector @ ECI frame [m/s] */ -math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const math::Vector<3> moon_position_eci_m, const math::Vector<3> moon_velocity_eci_m_s); +s2e::math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const s2e::math::Vector<3> moon_position_eci_m, const s2e::math::Vector<3> moon_velocity_eci_m_s); /** * @fn CalcDcmEciToMeanEarth @@ -28,13 +28,13 @@ math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const math::Vector<3> moon_position * @param[in] moon_position_eci_m: Moon position vector @ ECI frame [m] * @param[in] moon_velocity_eci_m_s: Moon velocity vector @ ECI frame [m/s] */ -math::Matrix<3, 3> CalcDcmEciToMeanEarth(const math::Vector<3> moon_position_eci_m, const math::Vector<3> moon_velocity_eci_m_s); +s2e::math::Matrix<3, 3> CalcDcmEciToMeanEarth(const s2e::math::Vector<3> moon_position_eci_m, const s2e::math::Vector<3> moon_velocity_eci_m_s); /** * @fn CalcDcmMeToPrincipalAxis * @brief Calculate DCM from ME (Mean Earth) moon fixed frame to PA (Principal Axis) moon fixed frame */ -math::Matrix<3, 3> CalcDcmMeanEarthToPrincipalAxis(); +s2e::math::Matrix<3, 3> CalcDcmMeanEarthToPrincipalAxis(); } // namespace planet_rotation diff --git a/src/math_physics/randomization/random_walk.hpp b/src/math_physics/randomization/random_walk.hpp index ecd269001..822d621aa 100644 --- a/src/math_physics/randomization/random_walk.hpp +++ b/src/math_physics/randomization/random_walk.hpp @@ -15,7 +15,7 @@ * @brief Class to calculate random wark value */ template -class RandomWalk : public math::OrdinaryDifferentialEquation { +class RandomWalk : public s2e::math::OrdinaryDifferentialEquation { public: /** * @fn RandomWalk @@ -24,7 +24,7 @@ class RandomWalk : public math::OrdinaryDifferentialEquation { * @param standard_deviation: Standard deviation of random walk excitation noise * @param limit: Limit of random walk */ - RandomWalk(double step_width_s, const math::Vector& standard_deviation, const math::Vector& limit); + RandomWalk(double step_width_s, const s2e::math::Vector& standard_deviation, const s2e::math::Vector& limit); /** * @fn DerivativeFunction @@ -33,10 +33,10 @@ class RandomWalk : public math::OrdinaryDifferentialEquation { * @param [in] state: State vector * @param [out] rhs: Differentiated value of state vector */ - virtual void DerivativeFunction(double x, const math::Vector& state, math::Vector& rhs); + virtual void DerivativeFunction(double x, const s2e::math::Vector& state, s2e::math::Vector& rhs); private: - math::Vector limit_; //!< Limit of random walk + s2e::math::Vector limit_; //!< Limit of random walk randomization::NormalRand normal_randomizer_[N]; //!< Random walk excitation noise }; diff --git a/src/math_physics/randomization/random_walk_template_functions.hpp b/src/math_physics/randomization/random_walk_template_functions.hpp index c316a3aab..ee224018c 100644 --- a/src/math_physics/randomization/random_walk_template_functions.hpp +++ b/src/math_physics/randomization/random_walk_template_functions.hpp @@ -10,8 +10,8 @@ #include template -RandomWalk::RandomWalk(double step_width_s, const math::Vector& standard_deviation, const math::Vector& limit) - : math::OrdinaryDifferentialEquation(step_width_s), limit_(limit) { +RandomWalk::RandomWalk(double step_width_s, const s2e::math::Vector& standard_deviation, const s2e::math::Vector& limit) + : s2e::math::OrdinaryDifferentialEquation(step_width_s), limit_(limit) { // Set standard deviation for (size_t i = 0; i < N; ++i) { normal_randomizer_[i].SetParameters(0.0, standard_deviation[i], global_randomization.MakeSeed()); @@ -19,7 +19,7 @@ RandomWalk::RandomWalk(double step_width_s, const math::Vector& standard_d } template -void RandomWalk::DerivativeFunction(double x, const math::Vector& state, math::Vector& rhs) { +void RandomWalk::DerivativeFunction(double x, const s2e::math::Vector& state, s2e::math::Vector& rhs) { UNUSED(x); // TODO: consider the x is really need for this function for (size_t i = 0; i < N; ++i) { diff --git a/src/setting_file_reader/initialize_file_access.cpp b/src/setting_file_reader/initialize_file_access.cpp index e25f89583..565a66f98 100644 --- a/src/setting_file_reader/initialize_file_access.cpp +++ b/src/setting_file_reader/initialize_file_access.cpp @@ -118,8 +118,8 @@ std::vector IniAccess::ReadVectorDouble(const char* section_name, const return data; } -void IniAccess::ReadQuaternion(const char* section_name, const char* key_name, math::Quaternion& data) { - math::Quaternion temp; +void IniAccess::ReadQuaternion(const char* section_name, const char* key_name, s2e::math::Quaternion& data) { + s2e::math::Quaternion temp; double norm = 0.0; for (int i = 0; i < 4; i++) { // Read Quaternion as new format diff --git a/src/setting_file_reader/initialize_file_access.hpp b/src/setting_file_reader/initialize_file_access.hpp index df49969ce..06bbc2866 100644 --- a/src/setting_file_reader/initialize_file_access.hpp +++ b/src/setting_file_reader/initialize_file_access.hpp @@ -110,7 +110,7 @@ class IniAccess { * @param[out] data: Read vector type data */ template - void ReadVector(const char* section_name, const char* key_name, math::Vector& data); + void ReadVector(const char* section_name, const char* key_name, s2e::math::Vector& data); /** * @fn ReadStrVector * @brief Read list of string type @@ -126,7 +126,7 @@ class IniAccess { * @param[in] key_name: Key name * @param[out] data: Read quaternion data */ - void ReadQuaternion(const char* section_name, const char* key_name, math::Quaternion& data); + void ReadQuaternion(const char* section_name, const char* key_name, s2e::math::Quaternion& data); /** * @fn ReadChar * @brief Read characters data @@ -208,7 +208,7 @@ class IniAccess { }; template -void IniAccess::ReadVector(const char* section_name, const char* key_name, math::Vector& data) { +void IniAccess::ReadVector(const char* section_name, const char* key_name, s2e::math::Vector& data) { for (size_t i = 0; i < NumElement; i++) { std::stringstream c_name; c_name << key_name << "(" << i << ")"; diff --git a/src/simulation/ground_station/ground_station.cpp b/src/simulation/ground_station/ground_station.cpp index 2f9483f2c..b1486b633 100644 --- a/src/simulation/ground_station/ground_station.cpp +++ b/src/simulation/ground_station/ground_station.cpp @@ -35,7 +35,7 @@ void GroundStation::Initialize(const SimulationConfiguration* configuration, con double latitude_deg = conf.ReadDouble(Section, "latitude_deg"); double longitude_deg = conf.ReadDouble(Section, "longitude_deg"); double height_m = conf.ReadDouble(Section, "height_m"); - geodetic_position_ = geodesy::GeodeticPosition(latitude_deg * math::deg_to_rad, longitude_deg * math::deg_to_rad, height_m); + geodetic_position_ = geodesy::GeodeticPosition(latitude_deg * s2e::math::deg_to_rad, longitude_deg * s2e::math::deg_to_rad, height_m); position_ecef_m_ = geodetic_position_.CalcEcefPosition(); elevation_limit_angle_deg_ = conf.ReadDouble(Section, "elevation_limit_angle_deg"); @@ -46,23 +46,23 @@ void GroundStation::Initialize(const SimulationConfiguration* configuration, con void GroundStation::LogSetup(Logger& logger) { logger.AddLogList(this); } void GroundStation::Update(const EarthRotation& celestial_rotation, const Spacecraft& spacecraft) { - math::Matrix<3, 3> dcm_ecef2eci = celestial_rotation.GetDcmJ2000ToEcef().Transpose(); + s2e::math::Matrix<3, 3> dcm_ecef2eci = celestial_rotation.GetDcmJ2000ToEcef().Transpose(); position_i_m_ = dcm_ecef2eci * position_ecef_m_; is_visible_[spacecraft.GetSpacecraftId()] = CalcIsVisible(spacecraft.GetDynamics().GetOrbit().GetPosition_ecef_m()); } -bool GroundStation::CalcIsVisible(const math::Vector<3> spacecraft_position_ecef_m) { - math::Quaternion q_ecef_to_ltc = geodetic_position_.GetQuaternionXcxfToLtc(); +bool GroundStation::CalcIsVisible(const s2e::math::Vector<3> spacecraft_position_ecef_m) { + s2e::math::Quaternion q_ecef_to_ltc = geodetic_position_.GetQuaternionXcxfToLtc(); - math::Vector<3> sc_pos_ltc = q_ecef_to_ltc.FrameConversion(spacecraft_position_ecef_m - position_ecef_m_); // Satellite position in LTC frame [m] + s2e::math::Vector<3> sc_pos_ltc = q_ecef_to_ltc.FrameConversion(spacecraft_position_ecef_m - position_ecef_m_); // Satellite position in LTC frame [m] sc_pos_ltc = sc_pos_ltc.CalcNormalizedVector(); - math::Vector<3> dir_gs_to_zenith = math::Vector<3>(0); + s2e::math::Vector<3> dir_gs_to_zenith = s2e::math::Vector<3>(0); dir_gs_to_zenith[2] = 1; // Judge the satellite position angle is over the minimum elevation - if (dot(sc_pos_ltc, dir_gs_to_zenith) > sin(elevation_limit_angle_deg_ * math::deg_to_rad)) { + if (dot(sc_pos_ltc, dir_gs_to_zenith) > sin(elevation_limit_angle_deg_ * s2e::math::deg_to_rad)) { return true; } else { return false; diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp index 44d6c211a..8ffaa5ad7 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp @@ -46,7 +46,7 @@ void InitializedMonteCarloParameters::GetRandomizedScalar(double& destination) c } } -void InitializedMonteCarloParameters::GetRandomizedQuaternion(math::Quaternion& destination) const { +void InitializedMonteCarloParameters::GetRandomizedQuaternion(s2e::math::Quaternion& destination) const { if (randomization_type_ == kNoRandomization) { ; } else if (4 > randomized_value_.size()) { @@ -120,7 +120,7 @@ void InitializedMonteCarloParameters::GenerateCartesianNormal() { } } -void InitializedMonteCarloParameters::CalcCircularNormalUniform(math::Vector<2>& destination, double r_mean, double r_sigma, double theta_min, +void InitializedMonteCarloParameters::CalcCircularNormalUniform(s2e::math::Vector<2>& destination, double r_mean, double r_sigma, double theta_min, double theta_max) { // r follows normal distribution, and θ follows uniform distribution in Circular frame double r = InitializedMonteCarloParameters::Generate1dNormal(r_mean, r_sigma); @@ -134,7 +134,7 @@ void InitializedMonteCarloParameters::GenerateCircularNormalUniform() { if (mean_or_min_.size() < dim || sigma_or_max_.size() < dim) { throw "Config parameters dimension unmatched."; } - math::Vector temp_vec; + s2e::math::Vector temp_vec; CalcCircularNormalUniform(temp_vec, mean_or_min_[0], sigma_or_max_[0], mean_or_min_[1], sigma_or_max_[1]); randomized_value_.clear(); @@ -143,7 +143,7 @@ void InitializedMonteCarloParameters::GenerateCircularNormalUniform() { } } -void InitializedMonteCarloParameters::CalcCircularNormalNormal(math::Vector<2>& destination, double r_mean, double r_sigma, double theta_mean, +void InitializedMonteCarloParameters::CalcCircularNormalNormal(s2e::math::Vector<2>& destination, double r_mean, double r_sigma, double theta_mean, double theta_sigma) { // r and θ follow normal distribution in Circular frame double r = InitializedMonteCarloParameters::Generate1dNormal(r_mean, r_sigma); @@ -157,7 +157,7 @@ void InitializedMonteCarloParameters::GenerateCircularNormalNormal() { if (mean_or_min_.size() < dim || sigma_or_max_.size() < dim) { throw "Config parameters dimension unmatched."; } - math::Vector temp_vec; + s2e::math::Vector temp_vec; CalcCircularNormalNormal(temp_vec, mean_or_min_[0], sigma_or_max_[0], mean_or_min_[1], sigma_or_max_[1]); randomized_value_.clear(); @@ -166,7 +166,7 @@ void InitializedMonteCarloParameters::GenerateCircularNormalNormal() { } } -void InitializedMonteCarloParameters::CalcSphericalNormalUniformUniform(math::Vector<3>& destination, double r_mean, double r_sigma, double theta_min, +void InitializedMonteCarloParameters::CalcSphericalNormalUniformUniform(s2e::math::Vector<3>& destination, double r_mean, double r_sigma, double theta_min, double theta_max, double phi_min, double phi_max) { // r follows normal distribution, and θ and φ follow uniform distribution in Spherical frame double r = InitializedMonteCarloParameters::Generate1dNormal(r_mean, r_sigma); @@ -182,7 +182,7 @@ void InitializedMonteCarloParameters::GenerateSphericalNormalUniformUniform() { if (mean_or_min_.size() < dim || sigma_or_max_.size() < dim) { throw "Config parameters dimension unmatched."; } - math::Vector temp_vec; + s2e::math::Vector temp_vec; CalcSphericalNormalUniformUniform(temp_vec, mean_or_min_[0], sigma_or_max_[0], mean_or_min_[1], sigma_or_max_[1], mean_or_min_[2], sigma_or_max_[2]); @@ -192,29 +192,29 @@ void InitializedMonteCarloParameters::GenerateSphericalNormalUniformUniform() { } } -void InitializedMonteCarloParameters::CalcSphericalNormalNormal(math::Vector<3>& destination, const math::Vector<3>& mean_vec) { +void InitializedMonteCarloParameters::CalcSphericalNormalNormal(s2e::math::Vector<3>& destination, const s2e::math::Vector<3>& mean_vec) { // r and θ follow normal distribution, and mean vector angle φ follows uniform distribution [0,2*pi] - math::Vector<3> mean_vec_dir; + s2e::math::Vector<3> mean_vec_dir; mean_vec_dir = 1.0 / mean_vec.CalcNorm() * mean_vec; // Unit vector of mean vector direction - math::Vector<3> x_axis(0.0), y_axis(0.0); + s2e::math::Vector<3> x_axis(0.0), y_axis(0.0); x_axis[0] = 1.0; y_axis[1] = 1.0; - math::Vector<3> op_x = OuterProduct(mean_vec_dir, x_axis); - math::Vector<3> op_y = OuterProduct(mean_vec_dir, y_axis); + s2e::math::Vector<3> op_x = OuterProduct(mean_vec_dir, x_axis); + s2e::math::Vector<3> op_y = OuterProduct(mean_vec_dir, y_axis); // An unit vector perpendicular with the mean vector // In case of the mean vector is parallel with X or Y axis, selecting the axis depend on the norm of outer product - math::Vector<3> normal_unit_vec = op_x.CalcNorm() > op_y.CalcNorm() ? op_x = op_x.CalcNormalizedVector() : op_y = op_y.CalcNormalizedVector(); + s2e::math::Vector<3> normal_unit_vec = op_x.CalcNorm() > op_y.CalcNorm() ? op_x = op_x.CalcNormalizedVector() : op_y = op_y.CalcNormalizedVector(); - double rotation_angle_of_normal_unit_vec = InitializedMonteCarloParameters::Generate1dUniform(0.0, math::tau); - math::Quaternion rotation_of_normal_unit_vec(mean_vec_dir, + double rotation_angle_of_normal_unit_vec = InitializedMonteCarloParameters::Generate1dUniform(0.0, s2e::math::tau); + s2e::math::Quaternion rotation_of_normal_unit_vec(mean_vec_dir, -rotation_angle_of_normal_unit_vec); // Use opposite sign to rotate the vector (not the frame) - math::Vector<3> rotation_axis = rotation_of_normal_unit_vec.FrameConversion(normal_unit_vec); // Axis of mean vector rotation + s2e::math::Vector<3> rotation_axis = rotation_of_normal_unit_vec.FrameConversion(normal_unit_vec); // Axis of mean vector rotation double rotation_angle_of_mean_vec = InitializedMonteCarloParameters::Generate1dNormal(0.0, sigma_or_max_[1]); - math::Quaternion rotation_of_mean_vec(rotation_axis, -rotation_angle_of_mean_vec); // Use opposite sign to rotate the vector (not the frame) - math::Vector<3> ret_vec = rotation_of_mean_vec.FrameConversion(mean_vec_dir); // Complete calculation of the direction + s2e::math::Quaternion rotation_of_mean_vec(rotation_axis, -rotation_angle_of_mean_vec); // Use opposite sign to rotate the vector (not the frame) + s2e::math::Vector<3> ret_vec = rotation_of_mean_vec.FrameConversion(mean_vec_dir); // Complete calculation of the direction ret_vec = InitializedMonteCarloParameters::Generate1dNormal(mean_vec.CalcNorm(), sigma_or_max_[0]) * ret_vec; // multiply norm @@ -228,7 +228,7 @@ void InitializedMonteCarloParameters::GenerateSphericalNormalNormal() { if (mean_or_min_.size() < dim || sigma_or_max_.size() < 2) { throw "Config parameters dimension unmatched."; } - math::Vector temp_vec, temp_mean_vec; + s2e::math::Vector temp_vec, temp_mean_vec; for (int i = 0; i < dim; i++) { temp_mean_vec[i] = mean_or_min_[i]; } @@ -240,32 +240,32 @@ void InitializedMonteCarloParameters::GenerateSphericalNormalNormal() { } } -void InitializedMonteCarloParameters::CalcQuaternionUniform(math::Quaternion& destination) { - // Perfectly Randomized math::Quaternion - math::Vector<3> x_axis(0.0); +void InitializedMonteCarloParameters::CalcQuaternionUniform(s2e::math::Quaternion& destination) { + // Perfectly Randomized s2e::math::Quaternion + s2e::math::Vector<3> x_axis(0.0); x_axis[0] = 1.0; // A direction vector converted from the X-axis by a quaternion may follows the uniform distribution in full sphere. - math::Quaternion first_cnv; - math::Vector<3> x_axis_cnvd; + s2e::math::Quaternion first_cnv; + s2e::math::Vector<3> x_axis_cnvd; double theta = acos(1 - (1 - (-1)) * InitializedMonteCarloParameters::Generate1dUniform(0.0, 1.0)); - double phi = InitializedMonteCarloParameters::Generate1dUniform(0, math::tau); + double phi = InitializedMonteCarloParameters::Generate1dUniform(0, s2e::math::tau); x_axis_cnvd[0] = sin(theta) * cos(phi); x_axis_cnvd[1] = sin(theta) * sin(phi); x_axis_cnvd[2] = cos(theta); double cos_angle_between = InnerProduct(x_axis, x_axis_cnvd); - math::Vector<3> op = OuterProduct(x_axis, x_axis_cnvd); + s2e::math::Vector<3> op = OuterProduct(x_axis, x_axis_cnvd); for (int i = 0; i < 3; i++) { first_cnv[i] = op[i]; } first_cnv[3] = cos_angle_between; // Generate randomized rotation angle around the X-axis - double rotation_angle = InitializedMonteCarloParameters::Generate1dUniform(0.0, math::tau); - math::Quaternion second_cnv(x_axis, rotation_angle); + double rotation_angle = InitializedMonteCarloParameters::Generate1dUniform(0.0, s2e::math::tau); + s2e::math::Quaternion second_cnv(x_axis, rotation_angle); - math::Quaternion ret_q = first_cnv * second_cnv; + s2e::math::Quaternion ret_q = first_cnv * second_cnv; for (int i = 0; i < 4; i++) { destination[i] = ret_q[i]; @@ -274,7 +274,7 @@ void InitializedMonteCarloParameters::CalcQuaternionUniform(math::Quaternion& de void InitializedMonteCarloParameters::GenerateQuaternionUniform() { const static int dim = 4; - math::Quaternion temp_q; + s2e::math::Quaternion temp_q; CalcQuaternionUniform(temp_q); randomized_value_.clear(); @@ -283,19 +283,19 @@ void InitializedMonteCarloParameters::GenerateQuaternionUniform() { } } -void InitializedMonteCarloParameters::CalcQuaternionNormal(math::Quaternion& destination, double theta_sigma) { +void InitializedMonteCarloParameters::CalcQuaternionNormal(s2e::math::Quaternion& destination, double theta_sigma) { // Angle from the default quaternion θ follows normal distribution // The rotation axis follows uniform distribution on full sphere - math::Vector<3> rot_axis; + s2e::math::Vector<3> rot_axis; double theta = acos(1 - (1 - (-1)) * InitializedMonteCarloParameters::Generate1dUniform(0.0, 1.0)); - double phi = InitializedMonteCarloParameters::Generate1dUniform(0, math::tau); + double phi = InitializedMonteCarloParameters::Generate1dUniform(0, s2e::math::tau); rot_axis[0] = sin(theta) * cos(phi); rot_axis[1] = sin(theta) * sin(phi); rot_axis[2] = cos(theta); double rotation_angle = InitializedMonteCarloParameters::Generate1dNormal(0.0, theta_sigma); - math::Quaternion ret_q(rot_axis, rotation_angle); + s2e::math::Quaternion ret_q(rot_axis, rotation_angle); for (int i = 0; i < 4; i++) { destination[i] = ret_q[i]; } @@ -307,7 +307,7 @@ void InitializedMonteCarloParameters::GenerateQuaternionNormal() { if (sigma_or_max_.size() < 1) { throw "Config parameters dimension unmatched."; } - math::Quaternion temp_q; + s2e::math::Quaternion temp_q; CalcQuaternionNormal(temp_q, sigma_or_max_[0]); randomized_value_.clear(); diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp index 534650c04..28159c98c 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp @@ -31,7 +31,7 @@ class InitializedMonteCarloParameters { kCircularNormalNormal, //!< r and θ follow normal distribution in Circular frame kSphericalNormalUniformUniform, //!< r follows normal distribution, and θ and φ follow uniform distribution in Spherical frame kSphericalNormalNormal, //!< r and θ follow normal distribution, and mean vector angle φ follows uniform distribution [0,2*pi] - kQuaternionUniform, //!< Perfectly Randomized math::Quaternion + kQuaternionUniform, //!< Perfectly Randomized s2e::math::Quaternion kQuaternionNormal, //!< Angle from the default quaternion θ follows normal distribution }; @@ -52,7 +52,7 @@ class InitializedMonteCarloParameters { * @brief Set randomization parameters */ template - void SetRandomConfiguration(const math::Vector& mean_or_min, const math::Vector& sigma_or_max, + void SetRandomConfiguration(const s2e::math::Vector& mean_or_min, const s2e::math::Vector& sigma_or_max, RandomizationType random_type); // Getter @@ -61,12 +61,12 @@ class InitializedMonteCarloParameters { * @brief Get randomized vector value results */ template - void GetRandomizedVector(math::Vector& destination) const; + void GetRandomizedVector(s2e::math::Vector& destination) const; /** * @fn GetRandomizedQuaternion * @brief Get randomized quaternion results */ - void GetRandomizedQuaternion(math::Quaternion& destination) const; + void GetRandomizedQuaternion(s2e::math::Quaternion& destination) const; /** * @fn GetRandomizedScalar * @brief Get randomized value results @@ -175,38 +175,38 @@ class InitializedMonteCarloParameters { * @fn CalcCircularNormalUniform * @brief Calculate randomized value with CircularNormalUniform mode */ - void CalcCircularNormalUniform(math::Vector<2>& destination, double r_mean, double r_sigma, double theta_min, double theta_max); + void CalcCircularNormalUniform(s2e::math::Vector<2>& destination, double r_mean, double r_sigma, double theta_min, double theta_max); /** * @fn CalcCircularNormalNormal * @brief Calculate randomized value with CircularNormalNormal mode */ - void CalcCircularNormalNormal(math::Vector<2>& destination, double r_mean, double r_sigma, double theta_mean, double theta_sigma); + void CalcCircularNormalNormal(s2e::math::Vector<2>& destination, double r_mean, double r_sigma, double theta_mean, double theta_sigma); /** * @fn CalcSphericalNormalUniformUniform * @brief Calculate randomized value with SphericalNormalUniformUniform mode */ - void CalcSphericalNormalUniformUniform(math::Vector<3>& destination, double r_mean, double r_sigma, double theta_min, double theta_max, + void CalcSphericalNormalUniformUniform(s2e::math::Vector<3>& destination, double r_mean, double r_sigma, double theta_min, double theta_max, double phi_min, double phi_max); /** * @fn CalcSphericalNormalNormal * @brief Calculate randomized value with SphericalNormalNormal mode */ - void CalcSphericalNormalNormal(math::Vector<3>& destination, const math::Vector<3>& mean_vec); + void CalcSphericalNormalNormal(s2e::math::Vector<3>& destination, const s2e::math::Vector<3>& mean_vec); /** * @fn CalcQuaternionUniform * @brief Calculate randomized value with QuaternionUniform mode */ - void CalcQuaternionUniform(math::Quaternion& destination); + void CalcQuaternionUniform(s2e::math::Quaternion& destination); /** * @fn CalcQuaternionNormal * @brief Calculate randomized value with QuaternionNormal mode */ - void CalcQuaternionNormal(math::Quaternion& destination, double theta_sigma); + void CalcQuaternionNormal(s2e::math::Quaternion& destination, double theta_sigma); }; template -void InitializedMonteCarloParameters::SetRandomConfiguration(const math::Vector& mean_or_min, - const math::Vector& sigma_or_max, +void InitializedMonteCarloParameters::SetRandomConfiguration(const s2e::math::Vector& mean_or_min, + const s2e::math::Vector& sigma_or_max, InitializedMonteCarloParameters::RandomizationType random_type) { randomization_type_ = random_type; mean_or_min_.clear(); @@ -220,7 +220,7 @@ void InitializedMonteCarloParameters::SetRandomConfiguration(const math::Vector< } template -void InitializedMonteCarloParameters::GetRandomizedVector(math::Vector& destination) const { +void InitializedMonteCarloParameters::GetRandomizedVector(s2e::math::Vector& destination) const { if (randomization_type_ == kNoRandomization) { ; } else if (NumElement > randomized_value_.size()) { diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp index 8c3ad1e39..f03c39754 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp @@ -79,12 +79,12 @@ MonteCarloSimulationExecutor* InitMonteCarloSimulation(std::string file_name) { // Read mean_or_min vector key_name = so_dot_ip_str + MonteCarloSimulationExecutor::separator_ + "mean_or_min"; - math::Vector<3> mean_or_min; + s2e::math::Vector<3> mean_or_min; ini_file.ReadVector(section, key_name.c_str(), mean_or_min); // Read sigma_or_max vector key_name = so_dot_ip_str + MonteCarloSimulationExecutor::separator_ + "sigma_or_max"; - math::Vector<3> sigma_or_max; + s2e::math::Vector<3> sigma_or_max; ini_file.ReadVector(section, key_name.c_str(), sigma_or_max); // Write randomize setting diff --git a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp index dead43808..1661c6e3a 100644 --- a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp +++ b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp @@ -47,7 +47,7 @@ void MonteCarloSimulationExecutor::GetInitializedMonteCarloParameterDouble(strin } void MonteCarloSimulationExecutor::GetInitializedMonteCarloParameterQuaternion(string so_name, string init_monte_carlo_parameter_name, - math::Quaternion& destination) const { + s2e::math::Quaternion& destination) const { if (!enabled_) return; { string name = so_name + MonteCarloSimulationExecutor::separator_ + init_monte_carlo_parameter_name; diff --git a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp index ad1fe87cc..7209b38a0 100644 --- a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp +++ b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp @@ -86,7 +86,7 @@ class MonteCarloSimulationExecutor { */ template void GetInitializedMonteCarloParameterVector(std::string so_name, std::string init_monte_carlo_parameter_name, - math::Vector& destination) const; + s2e::math::Vector& destination) const; /** * @fn GetInitializedMonteCarloParameterDouble * @brief Get randomized value and store it in dest @@ -97,7 +97,7 @@ class MonteCarloSimulationExecutor { * @brief Get randomized quaternion and store it in dest_quat */ void GetInitializedMonteCarloParameterQuaternion(std::string so_name, std::string init_monte_carlo_parameter_name, - math::Quaternion& destination) const; + s2e::math::Quaternion& destination) const; // Calculation /** @@ -126,7 +126,7 @@ class MonteCarloSimulationExecutor { * @brief Add initialized parameter */ void AddInitializedMonteCarloParameter(std::string so_name, std::string init_monte_carlo_parameter_name, - const math::Vector& mean_or_min, const math::Vector& sigma_or_max, + const s2e::math::Vector& mean_or_min, const s2e::math::Vector& sigma_or_max, InitializedMonteCarloParameters::RandomizationType random_type); /** @@ -138,7 +138,7 @@ class MonteCarloSimulationExecutor { template void MonteCarloSimulationExecutor::GetInitializedMonteCarloParameterVector(std::string so_name, std::string init_monte_carlo_parameter_name, - math::Vector& destination) const { + s2e::math::Vector& destination) const { if (!enabled_) return; std::string name = so_name + MonteCarloSimulationExecutor::separator_ + init_monte_carlo_parameter_name; if (init_parameter_list_.find(name) == init_parameter_list_.end()) { @@ -151,8 +151,8 @@ void MonteCarloSimulationExecutor::GetInitializedMonteCarloParameterVector(std:: template void MonteCarloSimulationExecutor::AddInitializedMonteCarloParameter(std::string so_name, std::string init_monte_carlo_parameter_name, - const math::Vector& mean_or_min, - const math::Vector& sigma_or_max, + const s2e::math::Vector& mean_or_min, + const s2e::math::Vector& sigma_or_max, InitializedMonteCarloParameters::RandomizationType random_type) { std::string name = so_name + MonteCarloSimulationExecutor::separator_ + init_monte_carlo_parameter_name; if (init_parameter_list_.find(name) == init_parameter_list_.end()) { diff --git a/src/simulation/monte_carlo_simulation/simulation_object.cpp b/src/simulation/monte_carlo_simulation/simulation_object.cpp index 9f41f0e15..a5b1e7b51 100644 --- a/src/simulation/monte_carlo_simulation/simulation_object.cpp +++ b/src/simulation/monte_carlo_simulation/simulation_object.cpp @@ -38,6 +38,6 @@ void SimulationObject::GetInitializedMonteCarloParameterDouble(const MonteCarloS } void SimulationObject::GetInitializedMonteCarloParameterQuaternion(const MonteCarloSimulationExecutor& monte_carlo_simulator, - std::string init_monte_carlo_parameter_name, math::Quaternion& destination) const { + std::string init_monte_carlo_parameter_name, s2e::math::Quaternion& destination) const { monte_carlo_simulator.GetInitializedMonteCarloParameterQuaternion(name_, init_monte_carlo_parameter_name, destination); } diff --git a/src/simulation/monte_carlo_simulation/simulation_object.hpp b/src/simulation/monte_carlo_simulation/simulation_object.hpp index 33708e59d..5a16163ea 100644 --- a/src/simulation/monte_carlo_simulation/simulation_object.hpp +++ b/src/simulation/monte_carlo_simulation/simulation_object.hpp @@ -40,7 +40,7 @@ class SimulationObject { */ template void GetInitializedMonteCarloParameterVector(const MonteCarloSimulationExecutor& monte_carlo_simulator, std::string init_monte_carlo_parameter_name, - math::Vector& destination) const; + s2e::math::Vector& destination) const; /** * @fn GetInitializedMonteCarloParameterDouble @@ -54,7 +54,7 @@ class SimulationObject { * @brief Get randomized quaternion and store it in destination */ void GetInitializedMonteCarloParameterQuaternion(const MonteCarloSimulationExecutor& monte_carlo_simulator, - std::string init_monte_carlo_parameter_name, math::Quaternion& destination) const; + std::string init_monte_carlo_parameter_name, s2e::math::Quaternion& destination) const; /** * @fn SetParameters @@ -80,7 +80,7 @@ class SimulationObject { template void SimulationObject::GetInitializedMonteCarloParameterVector(const MonteCarloSimulationExecutor& monte_carlo_simulator, std::string init_monte_carlo_parameter_name, - math::Vector& destination) const { + s2e::math::Vector& destination) const { monte_carlo_simulator.GetInitializedMonteCarloParameterVector(name_, init_monte_carlo_parameter_name, destination); } diff --git a/src/simulation/multiple_spacecraft/relative_information.cpp b/src/simulation/multiple_spacecraft/relative_information.cpp index f81227cb7..236a9ff19 100644 --- a/src/simulation/multiple_spacecraft/relative_information.cpp +++ b/src/simulation/multiple_spacecraft/relative_information.cpp @@ -13,8 +13,8 @@ void RelativeInformation::Update() { for (size_t target_spacecraft_id = 0; target_spacecraft_id < dynamics_database_.size(); target_spacecraft_id++) { for (size_t reference_spacecraft_id = 0; reference_spacecraft_id < dynamics_database_.size(); reference_spacecraft_id++) { // Position - math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); - math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); + s2e::math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); + s2e::math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); relative_position_list_i_m_[target_spacecraft_id][reference_spacecraft_id] = target_sat_pos_i - reference_sat_pos_i; relative_position_list_rtn_m_[target_spacecraft_id][reference_spacecraft_id] = CalcRelativePosition_rtn_m(target_spacecraft_id, reference_spacecraft_id); @@ -24,8 +24,8 @@ void RelativeInformation::Update() { relative_position_list_i_m_[target_spacecraft_id][reference_spacecraft_id].CalcNorm(); // Velocity - math::Vector<3> target_sat_vel_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); - math::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); + s2e::math::Vector<3> target_sat_vel_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); + s2e::math::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); relative_velocity_list_i_m_s_[target_spacecraft_id][reference_spacecraft_id] = target_sat_vel_i - reference_sat_vel_i; relative_velocity_list_rtn_m_s_[target_spacecraft_id][reference_spacecraft_id] = CalcRelativeVelocity_rtn_m_s(target_spacecraft_id, reference_spacecraft_id); @@ -111,55 +111,55 @@ std::string RelativeInformation::GetLogValue() const { void RelativeInformation::LogSetup(Logger& logger) { logger.AddLogList(this); } -math::Quaternion RelativeInformation::CalcRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { +s2e::math::Quaternion RelativeInformation::CalcRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { // Observer SC Body frame(obs_sat) -> ECI frame(i) - math::Quaternion q_reference_i2b = dynamics_database_.at(reference_spacecraft_id)->GetAttitude().GetQuaternion_i2b(); - math::Quaternion q_reference_b2i = q_reference_i2b.Conjugate(); + s2e::math::Quaternion q_reference_i2b = dynamics_database_.at(reference_spacecraft_id)->GetAttitude().GetQuaternion_i2b(); + s2e::math::Quaternion q_reference_b2i = q_reference_i2b.Conjugate(); // ECI frame(i) -> Target SC body frame(main_sat) - math::Quaternion q_target_i2b = dynamics_database_.at(target_spacecraft_id)->GetAttitude().GetQuaternion_i2b(); + s2e::math::Quaternion q_target_i2b = dynamics_database_.at(target_spacecraft_id)->GetAttitude().GetQuaternion_i2b(); return q_target_i2b * q_reference_b2i; } -math::Vector<3> RelativeInformation::CalcRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { - math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); - math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); - math::Vector<3> relative_pos_i = target_sat_pos_i - reference_sat_pos_i; +s2e::math::Vector<3> RelativeInformation::CalcRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { + s2e::math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); + s2e::math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); + s2e::math::Vector<3> relative_pos_i = target_sat_pos_i - reference_sat_pos_i; // RTN frame for the reference satellite - math::Quaternion q_i2rtn = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().CalcQuaternion_i2lvlh(); + s2e::math::Quaternion q_i2rtn = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().CalcQuaternion_i2lvlh(); - math::Vector<3> relative_pos_rtn = q_i2rtn.FrameConversion(relative_pos_i); + s2e::math::Vector<3> relative_pos_rtn = q_i2rtn.FrameConversion(relative_pos_i); return relative_pos_rtn; } -math::Vector<3> RelativeInformation::CalcRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { - math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); - math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); - math::Vector<3> relative_pos_i = target_sat_pos_i - reference_sat_pos_i; +s2e::math::Vector<3> RelativeInformation::CalcRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { + s2e::math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); + s2e::math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); + s2e::math::Vector<3> relative_pos_i = target_sat_pos_i - reference_sat_pos_i; // RTN frame for the reference satellite - math::Quaternion q_i2rtn = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().CalcQuaternion_i2lvlh(); + s2e::math::Quaternion q_i2rtn = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().CalcQuaternion_i2lvlh(); // Rotation vector of RTN frame - math::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); - math::Vector<3> target_sat_vel_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); - math::Vector<3> rot_vec_rtn_i = cross(reference_sat_pos_i, reference_sat_vel_i); + s2e::math::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); + s2e::math::Vector<3> target_sat_vel_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); + s2e::math::Vector<3> rot_vec_rtn_i = cross(reference_sat_pos_i, reference_sat_vel_i); double r2_ref = reference_sat_pos_i.CalcNorm() * reference_sat_pos_i.CalcNorm(); rot_vec_rtn_i /= r2_ref; - math::Vector<3> relative_vel_i = target_sat_vel_i - reference_sat_vel_i - cross(rot_vec_rtn_i, relative_pos_i); + s2e::math::Vector<3> relative_vel_i = target_sat_vel_i - reference_sat_vel_i - cross(rot_vec_rtn_i, relative_pos_i); - math::Vector<3> relative_vel_rtn = q_i2rtn.FrameConversion(relative_vel_i); + s2e::math::Vector<3> relative_vel_rtn = q_i2rtn.FrameConversion(relative_vel_i); return relative_vel_rtn; } void RelativeInformation::ResizeLists() { size_t size = dynamics_database_.size(); - relative_position_list_i_m_.assign(size, std::vector>(size, math::Vector<3>(0))); - relative_velocity_list_i_m_s_.assign(size, std::vector>(size, math::Vector<3>(0))); + relative_position_list_i_m_.assign(size, std::vector>(size, s2e::math::Vector<3>(0))); + relative_velocity_list_i_m_s_.assign(size, std::vector>(size, s2e::math::Vector<3>(0))); relative_distance_list_m_.assign(size, std::vector(size, 0.0)); - relative_position_list_rtn_m_.assign(size, std::vector>(size, math::Vector<3>(0))); - relative_velocity_list_rtn_m_s_.assign(size, std::vector>(size, math::Vector<3>(0))); - relative_attitude_quaternion_list_.assign(size, std::vector(size, math::Quaternion(0, 0, 0, 1))); + relative_position_list_rtn_m_.assign(size, std::vector>(size, s2e::math::Vector<3>(0))); + relative_velocity_list_rtn_m_s_.assign(size, std::vector>(size, s2e::math::Vector<3>(0))); + relative_attitude_quaternion_list_.assign(size, std::vector(size, s2e::math::Quaternion(0, 0, 0, 1))); } diff --git a/src/simulation/multiple_spacecraft/relative_information.hpp b/src/simulation/multiple_spacecraft/relative_information.hpp index 87a273641..2eb7e7789 100644 --- a/src/simulation/multiple_spacecraft/relative_information.hpp +++ b/src/simulation/multiple_spacecraft/relative_information.hpp @@ -73,7 +73,7 @@ class RelativeInformation : public ILoggable { * @param [in] target_spacecraft_id: ID of target spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - inline math::Quaternion GetRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { + inline s2e::math::Quaternion GetRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { return relative_attitude_quaternion_list_[target_spacecraft_id][reference_spacecraft_id]; } /** @@ -82,7 +82,7 @@ class RelativeInformation : public ILoggable { * @param [in] target_spacecraft_id: ID of target spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - inline math::Vector<3> GetRelativePosition_i_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { + inline s2e::math::Vector<3> GetRelativePosition_i_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { return relative_position_list_i_m_[target_spacecraft_id][reference_spacecraft_id]; } /** @@ -91,7 +91,7 @@ class RelativeInformation : public ILoggable { * @param [in] target_spacecraft_id: ID of target spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - inline math::Vector<3> GetRelativeVelocity_i_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { + inline s2e::math::Vector<3> GetRelativeVelocity_i_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { return relative_velocity_list_i_m_s_[target_spacecraft_id][reference_spacecraft_id]; } /** @@ -109,7 +109,7 @@ class RelativeInformation : public ILoggable { * @param [in] target_spacecraft_id: ID of target spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - inline math::Vector<3> GetRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { + inline s2e::math::Vector<3> GetRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { return relative_position_list_rtn_m_[target_spacecraft_id][reference_spacecraft_id]; } /** @@ -118,7 +118,7 @@ class RelativeInformation : public ILoggable { * @param [in] target_spacecraft_id: ID of target spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - inline math::Vector<3> GetRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { + inline s2e::math::Vector<3> GetRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { return relative_velocity_list_rtn_m_s_[target_spacecraft_id][reference_spacecraft_id]; } @@ -134,12 +134,12 @@ class RelativeInformation : public ILoggable { private: std::map dynamics_database_; //!< Dynamics database of all spacecraft - std::vector>> relative_position_list_i_m_; //!< Relative position list in the inertial frame in unit [m] - std::vector>> relative_velocity_list_i_m_s_; //!< Relative velocity list in the inertial frame in unit [m/s] - std::vector>> relative_position_list_rtn_m_; //!< Relative position list in the RTN frame in unit [m] - std::vector>> relative_velocity_list_rtn_m_s_; //!< Relative velocity list in the RTN frame in unit [m/s] + std::vector>> relative_position_list_i_m_; //!< Relative position list in the inertial frame in unit [m] + std::vector>> relative_velocity_list_i_m_s_; //!< Relative velocity list in the inertial frame in unit [m/s] + std::vector>> relative_position_list_rtn_m_; //!< Relative position list in the RTN frame in unit [m] + std::vector>> relative_velocity_list_rtn_m_s_; //!< Relative velocity list in the RTN frame in unit [m/s] std::vector> relative_distance_list_m_; //!< Relative distance list in unit [m] - std::vector> relative_attitude_quaternion_list_; //!< Relative attitude quaternion list + std::vector> relative_attitude_quaternion_list_; //!< Relative attitude quaternion list /** * @fn CalcRelativeAttitudeQuaternion @@ -147,21 +147,21 @@ class RelativeInformation : public ILoggable { * @param [in] target_spacecraft_id: ID of the spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - math::Quaternion CalcRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); + s2e::math::Quaternion CalcRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); /** * @fn CalcRelativePosition_rtn_m * @brief Calculate and return the relative position in RTN frame * @param [in] target_spacecraft_id: ID of the spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - math::Vector<3> CalcRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); + s2e::math::Vector<3> CalcRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); /** * @fn CalcRelativeVelocity_rtn_m_s * @brief Calculate and return the relative velocity in RTN frame * @param [in] target_spacecraft_id: ID of the spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - math::Vector<3> CalcRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); + s2e::math::Vector<3> CalcRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); /** * @fn ResizeLists diff --git a/src/simulation/spacecraft/installed_components.cpp b/src/simulation/spacecraft/installed_components.cpp index 7497df29e..6dcb6ee8c 100644 --- a/src/simulation/spacecraft/installed_components.cpp +++ b/src/simulation/spacecraft/installed_components.cpp @@ -7,13 +7,13 @@ #include -math::Vector<3> InstalledComponents::GenerateForce_b_N() { - math::Vector<3> force_b_N_(0.0); +s2e::math::Vector<3> InstalledComponents::GenerateForce_b_N() { + s2e::math::Vector<3> force_b_N_(0.0); return force_b_N_; } -math::Vector<3> InstalledComponents::GenerateTorque_b_Nm() { - math::Vector<3> torque_b_Nm_(0.0); +s2e::math::Vector<3> InstalledComponents::GenerateTorque_b_Nm() { + s2e::math::Vector<3> torque_b_Nm_(0.0); return torque_b_Nm_; } diff --git a/src/simulation/spacecraft/installed_components.hpp b/src/simulation/spacecraft/installed_components.hpp index 4a4550c55..daf41a7a8 100644 --- a/src/simulation/spacecraft/installed_components.hpp +++ b/src/simulation/spacecraft/installed_components.hpp @@ -26,14 +26,14 @@ class InstalledComponents { * @brief Return force generated by components in unit Newton in body fixed frame * @details Users need to override this function to add force generated by components */ - virtual math::Vector<3> GenerateForce_b_N(); + virtual s2e::math::Vector<3> GenerateForce_b_N(); /** * @fn GenerateTorque_b_Nm * @brief Return torque generated by components in unit Newton-meter in body fixed frame * @details Users need to override this function to add torque generated by components */ - virtual math::Vector<3> GenerateTorque_b_Nm(); + virtual s2e::math::Vector<3> GenerateTorque_b_Nm(); /** * @fn ComponentInterference diff --git a/src/simulation/spacecraft/structure/initialize_structure.cpp b/src/simulation/spacecraft/structure/initialize_structure.cpp index 0c2a49b35..b8e578398 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.cpp +++ b/src/simulation/spacecraft/structure/initialize_structure.cpp @@ -13,11 +13,11 @@ KinematicsParameters InitKinematicsParameters(std::string file_name) { auto conf = IniAccess(file_name); const char* section = "KINEMATIC_PARAMETERS"; - math::Vector<3> center_of_gravity_b_m; + s2e::math::Vector<3> center_of_gravity_b_m; conf.ReadVector(section, "center_of_gravity_b_m", center_of_gravity_b_m); double mass_kg = conf.ReadDouble(section, "mass_kg"); - math::Vector<9> inertia_vec; - math::Matrix<3, 3> inertia_tensor_b_kgm2; + s2e::math::Vector<9> inertia_vec; + s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2; conf.ReadVector(section, "inertia_tensor_kgm2", inertia_vec); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { @@ -110,7 +110,7 @@ ResidualMagneticMoment InitResidualMagneticMoment(std::string file_name) { auto conf = IniAccess(file_name); const char* section = "RESIDUAL_MAGNETIC_MOMENT"; - math::Vector<3> rmm_const_b; + s2e::math::Vector<3> rmm_const_b; conf.ReadVector(section, "rmm_constant_b_Am2", rmm_const_b); double rmm_rwdev = conf.ReadDouble(section, "rmm_random_walk_speed_Am2"); double random_walk_limit_Am2 = conf.ReadDouble(section, "rmm_random_walk_limit_Am2"); diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.cpp b/src/simulation/spacecraft/structure/kinematics_parameters.cpp index 67906710a..289c62cc7 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.cpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.cpp @@ -5,5 +5,5 @@ #include "kinematics_parameters.hpp" -KinematicsParameters::KinematicsParameters(math::Vector<3> center_of_gravity_b_m, double mass_kg, math::Matrix<3, 3> inertia_tensor_b_kgm2) +KinematicsParameters::KinematicsParameters(s2e::math::Vector<3> center_of_gravity_b_m, double mass_kg, s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2) : center_of_gravity_b_m_(center_of_gravity_b_m), mass_kg_(mass_kg), inertia_tensor_b_kgm2_(inertia_tensor_b_kgm2) {} \ No newline at end of file diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.hpp b/src/simulation/spacecraft/structure/kinematics_parameters.hpp index 52818f018..6cea84b20 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.hpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.hpp @@ -19,7 +19,7 @@ class KinematicsParameters { * @fn KinematicsParameters * @brief Constructor */ - KinematicsParameters(math::Vector<3> center_of_gravity_b_m, double mass_kg, math::Matrix<3, 3> inertia_tensor_b_kgm2); + KinematicsParameters(s2e::math::Vector<3> center_of_gravity_b_m, double mass_kg, s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2); /** * @fn ~KinematicsParameters * @brief Destructor @@ -31,7 +31,7 @@ class KinematicsParameters { * @fn GetCenterOfGravity_b_m * @brief Return Position vector of center of gravity at body frame [m] */ - inline const math::Vector<3>& GetCenterOfGravity_b_m() const { return center_of_gravity_b_m_; } + inline const s2e::math::Vector<3>& GetCenterOfGravity_b_m() const { return center_of_gravity_b_m_; } /** * @fn GetMass_kg * @brief Return Mass of the satellite [kg] @@ -41,7 +41,7 @@ class KinematicsParameters { * @fn GetInertiaTensor_b_kgm2 * @brief Return Inertia tensor at body frame [kgm2] */ - inline const math::Matrix<3, 3>& GetInertiaTensor_b_kgm2() const { return inertia_tensor_b_kgm2_; } + inline const s2e::math::Matrix<3, 3>& GetInertiaTensor_b_kgm2() const { return inertia_tensor_b_kgm2_; } // Setter /** @@ -49,7 +49,7 @@ class KinematicsParameters { * @brief Set center of gravity vector at the body frame [m] * @param [in] center_of_gravity_vector_b_m: Center of gravity vector at the body frame [m] */ - inline void SetCenterOfGravityVector_b_m(const math::Vector<3> center_of_gravity_vector_b_m) { + inline void SetCenterOfGravityVector_b_m(const s2e::math::Vector<3> center_of_gravity_vector_b_m) { center_of_gravity_b_m_ = center_of_gravity_vector_b_m; } /** @@ -75,15 +75,15 @@ class KinematicsParameters { * @brief Inertia tensor at body frame * @param [in] inertia_tensor_b_kgm2: Inertia tensor at body frame [kgm2] */ - inline void SetInertiaTensor_b_kgm2(const math::Matrix<3, 3> inertia_tensor_b_kgm2) { + inline void SetInertiaTensor_b_kgm2(const s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2) { // TODO add assertion check inertia_tensor_b_kgm2_ = inertia_tensor_b_kgm2; } private: - math::Vector<3> center_of_gravity_b_m_; //!< Position vector of center of gravity at body frame [m] + s2e::math::Vector<3> center_of_gravity_b_m_; //!< Position vector of center of gravity at body frame [m] double mass_kg_; //!< Mass of the satellite [kg] - math::Matrix<3, 3> inertia_tensor_b_kgm2_; //!< Inertia tensor at body frame [kgm2] + s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2_; //!< Inertia tensor at body frame [kgm2] }; #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_KINEMATICS_PARAMETERS_HPP_ diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp index 72000e51f..93d5e44ab 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp @@ -7,7 +7,7 @@ #define S2E_SIMULATION_SPACECRAFT_STRUCTURE_RESIDUAL_MAGNETIC_MOMENT_HPP_ #include -using math::Vector; +using s2e::math::Vector; /** * @class ResidualMagneticMoment diff --git a/src/simulation/spacecraft/structure/surface.cpp b/src/simulation/spacecraft/structure/surface.cpp index 06af72cde..58b0b4cbd 100644 --- a/src/simulation/spacecraft/structure/surface.cpp +++ b/src/simulation/spacecraft/structure/surface.cpp @@ -5,7 +5,7 @@ #include "surface.hpp" -Surface::Surface(const math::Vector<3> position_b_m, const math::Vector<3> normal_b, const double area_m2, const double reflectivity, +Surface::Surface(const s2e::math::Vector<3> position_b_m, const s2e::math::Vector<3> normal_b, const double area_m2, const double reflectivity, const double specularity, const double air_specularity) : position_b_m_(position_b_m), normal_b_(normal_b), diff --git a/src/simulation/spacecraft/structure/surface.hpp b/src/simulation/spacecraft/structure/surface.hpp index db54f219b..2b4b9439f 100644 --- a/src/simulation/spacecraft/structure/surface.hpp +++ b/src/simulation/spacecraft/structure/surface.hpp @@ -18,7 +18,7 @@ class Surface { * @fn Surface * @brief Constructor */ - Surface(const math::Vector<3> position_b_m, const math::Vector<3> normal_b, const double area_m2, const double reflectivity, + Surface(const s2e::math::Vector<3> position_b_m, const s2e::math::Vector<3> normal_b, const double area_m2, const double reflectivity, const double specularity, const double air_specularity); /** * @fn ~Surface @@ -31,12 +31,12 @@ class Surface { * @fn GetPosition_b_m * @brief Return position vector of geometric center of the surface in body frame and meter unit */ - inline const math::Vector<3>& GetPosition_b_m(void) const { return position_b_m_; } + inline const s2e::math::Vector<3>& GetPosition_b_m(void) const { return position_b_m_; } /** * @fn GetNormal_b * @brief Return normal vector of the surface in body frame */ - inline const math::Vector<3>& GetNormal_b(void) const { return normal_b_; } + inline const s2e::math::Vector<3>& GetNormal_b(void) const { return normal_b_; } /** * @fn GetArea_m2 * @brief Return area of the surface in meter^2 unit @@ -64,13 +64,13 @@ class Surface { * @brief Set position vector of geometric center of the surface in body frame [m] * @param[in] position_b_m: Position vector of geometric center of the surface in body frame [m] */ - inline void SetPosition_b_m(const math::Vector<3> position_b_m) { position_b_m_ = position_b_m; } + inline void SetPosition_b_m(const s2e::math::Vector<3> position_b_m) { position_b_m_ = position_b_m; } /** * @fn SetNormal * @brief Set normal vector of the surface in body frame * @param[in] normal_b: Normal vector of the surface in body frame */ - inline void SetNormal_b(const math::Vector<3> normal_b) { normal_b_ = normal_b.CalcNormalizedVector(); } + inline void SetNormal_b(const s2e::math::Vector<3> normal_b) { normal_b_ = normal_b.CalcNormalizedVector(); } /** * @fn SetArea_m2 * @brief Set area of the surface @@ -105,8 +105,8 @@ class Surface { } private: - math::Vector<3> position_b_m_; //!< Position vector of the surface @ Body Frame [m] - math::Vector<3> normal_b_; //!< Normal unit vector of the surface @ Body Frame [-] + s2e::math::Vector<3> position_b_m_; //!< Position vector of the surface @ Body Frame [m] + s2e::math::Vector<3> normal_b_; //!< Normal unit vector of the surface @ Body Frame [-] double area_m2_; //!< Area of the surface [m2] double reflectivity_; //!< Total reflectivity for solar wavelength (1.0 - solar absorption) double specularity_; //!< Ratio of specular reflection in the total reflected light diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index b45d960c3..5de0f8984 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -141,21 +141,21 @@ SampleComponents::SampleComponents(const Dynamics* dynamics, Structure* structur /**************/ // actuator debug output - // math::Vector mag_moment_c{0.01}; + // s2e::math::Vector mag_moment_c{0.01}; // magnetorquer_->SetOutputMagneticMoment_c_Am2(mag_moment_c); // reaction_wheel_->SetTargetTorque_rw_Nm(0.01); // reaction_wheel_->SetDriveFlag(true); // thruster_->SetDuty(0.9); // force generator debug output - // math::Vector<3> force_N; + // s2e::math::Vector<3> force_N; // force_N[0] = 1.0; // force_N[1] = 0.0; // force_N[2] = 0.0; // force_generator_->SetForce_b_N(force_N); // torque generator debug output - // math::Vector<3> torque_Nm; + // s2e::math::Vector<3> torque_Nm; // torque_Nm[0] = 0.1; // torque_Nm[1] = 0.0; // torque_Nm[2] = 0.0; @@ -188,15 +188,15 @@ SampleComponents::~SampleComponents() { delete hils_port_manager_; // delete after exp_hils } -math::Vector<3> SampleComponents::GenerateForce_b_N() { - math::Vector<3> force_b_N_(0.0); +s2e::math::Vector<3> SampleComponents::GenerateForce_b_N() { + s2e::math::Vector<3> force_b_N_(0.0); force_b_N_ += thruster_->GetOutputThrust_b_N(); force_b_N_ += force_generator_->GetGeneratedForce_b_N(); return force_b_N_; } -math::Vector<3> SampleComponents::GenerateTorque_b_Nm() { - math::Vector<3> torque_b_Nm_(0.0); +s2e::math::Vector<3> SampleComponents::GenerateTorque_b_Nm() { + s2e::math::Vector<3> torque_b_Nm_(0.0); torque_b_Nm_ += magnetorquer_->GetOutputTorque_b_Nm(); torque_b_Nm_ += reaction_wheel_->GetOutputTorque_b_Nm(); torque_b_Nm_ += thruster_->GetOutputTorque_b_Nm(); diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index d03044b3b..c8c3603d5 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -74,12 +74,12 @@ class SampleComponents : public InstalledComponents { * @fn GenerateForce_b_N * @brief Return force generated by components in unit Newton in body fixed frame */ - math::Vector<3> GenerateForce_b_N() override; + s2e::math::Vector<3> GenerateForce_b_N() override; /** * @fn GenerateTorque_b_Nm * @brief Return torque generated by components in unit Newton-meter in body fixed frame */ - math::Vector<3> GenerateTorque_b_Nm() override; + s2e::math::Vector<3> GenerateTorque_b_Nm() override; /** * @fn ComponentInterference * @brief Handle component interference effect From 70dfc4fc29b2f97916ed127cdb173c5cbe926456 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:05:02 -0700 Subject: [PATCH 02/49] Rename namespaec atmosphere to s2e::atmosphere --- src/environment/local/atmosphere.cpp | 4 ++-- src/math_physics/atmosphere/harris_priester_coefficients.hpp | 4 ++-- src/math_physics/atmosphere/harris_priester_model.cpp | 4 ++-- src/math_physics/atmosphere/harris_priester_model.hpp | 4 ++-- src/math_physics/atmosphere/simple_air_density_model.cpp | 4 ++-- src/math_physics/atmosphere/simple_air_density_model.hpp | 4 ++-- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/environment/local/atmosphere.cpp b/src/environment/local/atmosphere.cpp index b46e855b2..1ea0636a6 100644 --- a/src/environment/local/atmosphere.cpp +++ b/src/environment/local/atmosphere.cpp @@ -56,7 +56,7 @@ double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const Orbit& if (model_ == "STANDARD") { // Standard model double altitude_m = orbit.GetGeodeticPosition().GetAltitude_m(); - air_density_kg_m3_ = atmosphere::CalcAirDensityWithSimpleModel(altitude_m); + air_density_kg_m3_ = s2e::atmosphere::CalcAirDensityWithSimpleModel(altitude_m); } else if (model_ == "NRLMSISE00") { // NRLMSISE00 model double lat_rad = orbit.GetGeodeticPosition().GetLatitude_rad(); @@ -67,7 +67,7 @@ double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const Orbit& } else if (model_ == "HARRIS_PRIESTER") { // Harris-Priester s2e::math::Vector<3> sun_direction_eci = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("SUN").CalcNormalizedVector(); - air_density_kg_m3_ = atmosphere::CalcAirDensityWithHarrisPriester_kg_m3(orbit.GetGeodeticPosition(), sun_direction_eci); + air_density_kg_m3_ = s2e::atmosphere::CalcAirDensityWithHarrisPriester_kg_m3(orbit.GetGeodeticPosition(), sun_direction_eci); } else { // No suitable model return air_density_kg_m3_ = 0.0; diff --git a/src/math_physics/atmosphere/harris_priester_coefficients.hpp b/src/math_physics/atmosphere/harris_priester_coefficients.hpp index 3c467501b..7fd6802b7 100644 --- a/src/math_physics/atmosphere/harris_priester_coefficients.hpp +++ b/src/math_physics/atmosphere/harris_priester_coefficients.hpp @@ -7,7 +7,7 @@ #include -namespace atmosphere { +namespace s2e::atmosphere { // Height [km], density [g/km3] // TODO: Add other solar activities value @@ -28,6 +28,6 @@ const std::map harris_priester_max_density_table = { {640, 0.4121}, {660, 0.3325}, {680, 0.2691}, {700, 0.2185}, {720, 0.1779}, {740, 0.1452}, {760, 0.1190}, {780, 0.09776}, {800, 0.08059}, {840, 0.05741}, {880, 0.04210}, {920, 0.03130}, {960, 0.02360}, {1000, 0.01810}}; -} // namespace atmosphere +} // namespace s2e::atmosphere #endif // S2E_LIBRARY_HARRIS_COEFFICIENTS_HPP_ diff --git a/src/math_physics/atmosphere/harris_priester_model.cpp b/src/math_physics/atmosphere/harris_priester_model.cpp index ce88b5a54..4c04719b1 100644 --- a/src/math_physics/atmosphere/harris_priester_model.cpp +++ b/src/math_physics/atmosphere/harris_priester_model.cpp @@ -10,7 +10,7 @@ #include "harris_priester_coefficients.hpp" -namespace atmosphere { +namespace s2e::atmosphere { /** * @fn CalcScaleHeight_km @@ -88,4 +88,4 @@ double CalcApexDensity_g_km3(const std::map::const_iterator dens return apex_density_g_km3; } -} // namespace atmosphere +} // namespace s2e::atmosphere diff --git a/src/math_physics/atmosphere/harris_priester_model.hpp b/src/math_physics/atmosphere/harris_priester_model.hpp index fb08c4977..b999dbffd 100644 --- a/src/math_physics/atmosphere/harris_priester_model.hpp +++ b/src/math_physics/atmosphere/harris_priester_model.hpp @@ -9,7 +9,7 @@ #include #include -namespace atmosphere { +namespace s2e::atmosphere { /** * @fn CalcAirDensityWithHarrisPriester @@ -23,6 +23,6 @@ namespace atmosphere { double CalcAirDensityWithHarrisPriester_kg_m3(const geodesy::GeodeticPosition geodetic_position, const s2e::math::Vector<3> sun_direction_eci, const double f10_7 = 100.0, const double exponent_parameter = 4); -} // namespace atmosphere +} // namespace s2e::atmosphere #endif // S2E_LIBRARY_HARRIS_PRIESTER_HPP_ diff --git a/src/math_physics/atmosphere/simple_air_density_model.cpp b/src/math_physics/atmosphere/simple_air_density_model.cpp index 7e2b0a440..f616f674b 100644 --- a/src/math_physics/atmosphere/simple_air_density_model.cpp +++ b/src/math_physics/atmosphere/simple_air_density_model.cpp @@ -6,7 +6,7 @@ #include -namespace atmosphere { +namespace s2e::atmosphere { double CalcAirDensityWithSimpleModel(const double altitude_m) { double altitude_km = altitude_m / 1000.0; @@ -138,4 +138,4 @@ double CalcAirDensityWithSimpleModel(const double altitude_m) { return rho_kg_m3; } -} // namespace atmosphere +} // namespace s2e::atmosphere diff --git a/src/math_physics/atmosphere/simple_air_density_model.hpp b/src/math_physics/atmosphere/simple_air_density_model.hpp index b697a66e4..834807d96 100644 --- a/src/math_physics/atmosphere/simple_air_density_model.hpp +++ b/src/math_physics/atmosphere/simple_air_density_model.hpp @@ -5,7 +5,7 @@ #ifndef S2E_LIBRARY_ATMOSPHERE_SIMPLE_AIR_DENSITY_MODEL_HPP_ #define S2E_LIBRARY_ATMOSPHERE_SIMPLE_AIR_DENSITY_MODEL_HPP_ -namespace atmosphere { +namespace s2e::atmosphere { /** * @fn CalcAirDensityWithSimpleModel @@ -15,6 +15,6 @@ namespace atmosphere { */ double CalcAirDensityWithSimpleModel(const double altitude_m); -} // namespace atmosphere +} // namespace s2e::atmosphere #endif // S2E_LIBRARY_ATMOSPHERE_SIMPLE_AIR_DENSITY_MODEL_HPP_ From 4f102b4e7557542d4960939f957c5e0842c245b8 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:06:10 -0700 Subject: [PATCH 03/49] Rename namespaec control_utilities to s2e::control_utilities --- src/components/real/aocs/reaction_wheel.hpp | 2 +- src/math_physics/control_utilities/first_order_lag.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/components/real/aocs/reaction_wheel.hpp b/src/components/real/aocs/reaction_wheel.hpp index 5852b8a74..14c66a33e 100644 --- a/src/components/real/aocs/reaction_wheel.hpp +++ b/src/components/real/aocs/reaction_wheel.hpp @@ -183,7 +183,7 @@ class ReactionWheel : public Component, public ILoggable { const double step_width_s_; //!< step width for ReactionWheelOde [sec] const double dead_time_s_; //!< dead time [sec] std::vector acceleration_delay_buffer_; //!< Delay buffer for acceleration - control_utilities::FirstOrderLag delayed_acceleration_rad_s2_; //!< Delayed acceleration [rad/s2] + s2e::control_utilities::FirstOrderLag delayed_acceleration_rad_s2_; //!< Delayed acceleration [rad/s2] // Coasting friction // f_rad_s2 = v_rad_s * coefficients(0) + (v_rad_s)^2 * coefficients(1) + ... diff --git a/src/math_physics/control_utilities/first_order_lag.hpp b/src/math_physics/control_utilities/first_order_lag.hpp index 9a9d27ed3..3c61593be 100644 --- a/src/math_physics/control_utilities/first_order_lag.hpp +++ b/src/math_physics/control_utilities/first_order_lag.hpp @@ -6,7 +6,7 @@ #ifndef S2E_LIBRARY_CONTROL_UTILITIES_FIRST_ORDER_LAG_HPP_ #define S2E_LIBRARY_CONTROL_UTILITIES_FIRST_ORDER_LAG_HPP_ -namespace control_utilities { +namespace s2e::control_utilities { /** * @class FirstOderLag @@ -47,6 +47,6 @@ class FirstOrderLag { const double gain_; //!< Gain }; -} // namespace control_utilities +} // namespace s2e::control_utilities #endif // S2E_LIBRARY_CONTROL_UTILITIES_FIRST_ORDER_LAG_HPP_ From 99abe3c077cf9488c2ac5a552d38b63fda9d496a Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:07:41 -0700 Subject: [PATCH 04/49] Rename namespaec geodesy to s2e::geodesy --- src/components/real/aocs/gnss_receiver.hpp | 4 ++-- src/dynamics/orbit/orbit.hpp | 4 ++-- src/environment/local/geomagnetic_field.cpp | 2 +- src/environment/local/geomagnetic_field.hpp | 2 +- src/math_physics/atmosphere/harris_priester_model.cpp | 2 +- src/math_physics/atmosphere/harris_priester_model.hpp | 2 +- src/math_physics/geodesy/geodetic_position.cpp | 4 ++-- src/math_physics/geodesy/geodetic_position.hpp | 4 ++-- src/simulation/ground_station/ground_station.cpp | 2 +- src/simulation/ground_station/ground_station.hpp | 4 ++-- 10 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 006d70ab0..ad9f38103 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -107,7 +107,7 @@ class GnssReceiver : public Component, public ILoggable { * @fn GetMeasuredGeodeticPosition * @brief Return Observed position in the LLH frame [m] */ - inline const geodesy::GeodeticPosition GetMeasuredGeodeticPosition(void) const { return geodetic_position_; } + inline const s2e::geodesy::GeodeticPosition GetMeasuredGeodeticPosition(void) const { return geodetic_position_; } /** * @fn GetMeasuredVelocity_ecef_m_s * @brief Return Observed velocity in the ECEF frame [m/s] @@ -141,7 +141,7 @@ class GnssReceiver : public Component, public ILoggable { randomization::NormalRand velocity_random_noise_ecef_m_s_[3]; //!< Random noise for velocity at the ECEF frame [m] s2e::math::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] s2e::math::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] - geodesy::GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame + s2e::geodesy::GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame // Time observation UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index 01be5c87c..79a016b81 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -113,7 +113,7 @@ class Orbit : public ILoggable { * @fn GetGeodeticPosition * @brief Return spacecraft position in the geodetic frame [m] */ - inline geodesy::GeodeticPosition GetGeodeticPosition() const { return spacecraft_geodetic_position_; } + inline s2e::geodesy::GeodeticPosition GetGeodeticPosition() const { return spacecraft_geodetic_position_; } // TODO delete the following functions inline double GetLatitude_rad() const { return spacecraft_geodetic_position_.GetLatitude_rad(); } @@ -193,7 +193,7 @@ class Orbit : public ILoggable { s2e::math::Vector<3> spacecraft_position_i_m_; //!< Spacecraft position in the inertial frame [m] s2e::math::Vector<3> spacecraft_position_ecef_m_; //!< Spacecraft position in the ECEF frame [m] - geodesy::GeodeticPosition spacecraft_geodetic_position_; //!< Spacecraft position in the Geodetic frame + s2e::geodesy::GeodeticPosition spacecraft_geodetic_position_; //!< Spacecraft position in the Geodetic frame s2e::math::Vector<3> spacecraft_velocity_i_m_s_; //!< Spacecraft velocity in the inertial frame [m/s] s2e::math::Vector<3> spacecraft_velocity_b_m_s_; //!< Spacecraft velocity in the body frame [m/s] diff --git a/src/environment/local/geomagnetic_field.cpp b/src/environment/local/geomagnetic_field.cpp index 971910b0a..6fcdcc7f9 100644 --- a/src/environment/local/geomagnetic_field.cpp +++ b/src/environment/local/geomagnetic_field.cpp @@ -22,7 +22,7 @@ GeomagneticField::GeomagneticField(const std::string igrf_file_name, const doubl set_file_path(igrf_file_name_.c_str()); } -void GeomagneticField::CalcMagneticField(const double decimal_year, const double sidereal_day, const geodesy::GeodeticPosition position, +void GeomagneticField::CalcMagneticField(const double decimal_year, const double sidereal_day, const s2e::geodesy::GeodeticPosition position, const s2e::math::Quaternion quaternion_i2b) { if (!IsCalcEnabled) return; diff --git a/src/environment/local/geomagnetic_field.hpp b/src/environment/local/geomagnetic_field.hpp index d00f46aa1..3f2adf741 100644 --- a/src/environment/local/geomagnetic_field.hpp +++ b/src/environment/local/geomagnetic_field.hpp @@ -43,7 +43,7 @@ class GeomagneticField : public ILoggable { * @param [in] position: Position of target point to calculate the magnetic field * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame */ - void CalcMagneticField(const double decimal_year, const double sidereal_day, const geodesy::GeodeticPosition position, + void CalcMagneticField(const double decimal_year, const double sidereal_day, const s2e::geodesy::GeodeticPosition position, const s2e::math::Quaternion quaternion_i2b); /** diff --git a/src/math_physics/atmosphere/harris_priester_model.cpp b/src/math_physics/atmosphere/harris_priester_model.cpp index 4c04719b1..e38476335 100644 --- a/src/math_physics/atmosphere/harris_priester_model.cpp +++ b/src/math_physics/atmosphere/harris_priester_model.cpp @@ -28,7 +28,7 @@ double CalcScaleHeight_km(const std::map::const_iterator density */ double CalcApexDensity_g_km3(const std::map::const_iterator density_itr, const double altitude_km); -double CalcAirDensityWithHarrisPriester_kg_m3(const geodesy::GeodeticPosition geodetic_position, const s2e::math::Vector<3> sun_direction_eci, +double CalcAirDensityWithHarrisPriester_kg_m3(const s2e::geodesy::GeodeticPosition geodetic_position, const s2e::math::Vector<3> sun_direction_eci, const double f10_7, const double exponent_parameter) { // Altitude double altitude_km = geodetic_position.GetAltitude_m() / 1000.0; diff --git a/src/math_physics/atmosphere/harris_priester_model.hpp b/src/math_physics/atmosphere/harris_priester_model.hpp index b999dbffd..a5b05bc7e 100644 --- a/src/math_physics/atmosphere/harris_priester_model.hpp +++ b/src/math_physics/atmosphere/harris_priester_model.hpp @@ -20,7 +20,7 @@ namespace s2e::atmosphere { * @param [in] exponent_parameter: n in the equation. n=2 for low inclination orbit and n=6 for polar orbit. * @return Atmospheric density [kg/m^3] */ -double CalcAirDensityWithHarrisPriester_kg_m3(const geodesy::GeodeticPosition geodetic_position, const s2e::math::Vector<3> sun_direction_eci, +double CalcAirDensityWithHarrisPriester_kg_m3(const s2e::geodesy::GeodeticPosition geodetic_position, const s2e::math::Vector<3> sun_direction_eci, const double f10_7 = 100.0, const double exponent_parameter = 4); } // namespace s2e::atmosphere diff --git a/src/math_physics/geodesy/geodetic_position.cpp b/src/math_physics/geodesy/geodetic_position.cpp index 65969df15..50bac4071 100644 --- a/src/math_physics/geodesy/geodetic_position.cpp +++ b/src/math_physics/geodesy/geodetic_position.cpp @@ -10,7 +10,7 @@ #include #include -namespace geodesy { +namespace s2e::geodesy { GeodeticPosition::GeodeticPosition() { latitude_rad_ = 0.0; @@ -86,4 +86,4 @@ void GeodeticPosition::CalcQuaternionXcxfToLtc() { quaternion_xcxf_to_ltc_ = quaternion_xcxf_to_ltc_.ConvertFromDcm(dcm_xcxf_to_ltc); } -} // namespace geodesy +} // namespace s2e::geodesy diff --git a/src/math_physics/geodesy/geodetic_position.hpp b/src/math_physics/geodesy/geodetic_position.hpp index a850c5936..9687e0ea9 100644 --- a/src/math_physics/geodesy/geodetic_position.hpp +++ b/src/math_physics/geodesy/geodetic_position.hpp @@ -9,7 +9,7 @@ #include #include -namespace geodesy { +namespace s2e::geodesy { /** * @class GeodeticPosition @@ -80,6 +80,6 @@ class GeodeticPosition { void CalcQuaternionXcxfToLtc(); }; -} // namespace geodesy +} // namespace s2e::geodesy #endif // S2E_LIBRARY_GEODESY_GEODETIC_POSITION_HPP_ \ No newline at end of file diff --git a/src/simulation/ground_station/ground_station.cpp b/src/simulation/ground_station/ground_station.cpp index b1486b633..4a0110fc4 100644 --- a/src/simulation/ground_station/ground_station.cpp +++ b/src/simulation/ground_station/ground_station.cpp @@ -35,7 +35,7 @@ void GroundStation::Initialize(const SimulationConfiguration* configuration, con double latitude_deg = conf.ReadDouble(Section, "latitude_deg"); double longitude_deg = conf.ReadDouble(Section, "longitude_deg"); double height_m = conf.ReadDouble(Section, "height_m"); - geodetic_position_ = geodesy::GeodeticPosition(latitude_deg * s2e::math::deg_to_rad, longitude_deg * s2e::math::deg_to_rad, height_m); + geodetic_position_ = s2e::geodesy::GeodeticPosition(latitude_deg * s2e::math::deg_to_rad, longitude_deg * s2e::math::deg_to_rad, height_m); position_ecef_m_ = geodetic_position_.CalcEcefPosition(); elevation_limit_angle_deg_ = conf.ReadDouble(Section, "elevation_limit_angle_deg"); diff --git a/src/simulation/ground_station/ground_station.hpp b/src/simulation/ground_station/ground_station.hpp index 41a051d58..bdb320af9 100644 --- a/src/simulation/ground_station/ground_station.hpp +++ b/src/simulation/ground_station/ground_station.hpp @@ -67,7 +67,7 @@ class GroundStation : public ILoggable { * @fn GetGeodeticPosition * @brief Return ground station position in the geodetic frame */ - geodesy::GeodeticPosition GetGeodeticPosition() const { return geodetic_position_; } + s2e::geodesy::GeodeticPosition GetGeodeticPosition() const { return geodetic_position_; } /** * @fn GetPosition_ecef_m * @brief Return ground station position in the ECEF frame [m] @@ -92,7 +92,7 @@ class GroundStation : public ILoggable { protected: unsigned int ground_station_id_; //!< Ground station ID - geodesy::GeodeticPosition geodetic_position_; //!< Ground Station Position in the geodetic frame + s2e::geodesy::GeodeticPosition geodetic_position_; //!< Ground Station Position in the geodetic frame Vector<3> position_ecef_m_{0.0}; //!< Ground Station Position in the ECEF frame [m] Vector<3> position_i_m_{0.0}; //!< Ground Station Position in the inertial frame [m] double elevation_limit_angle_deg_; //!< Minimum elevation angle to work the ground station [deg] From f6ab6b59bf00f398acbf10e56c0e16057546a430 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:09:19 -0700 Subject: [PATCH 05/49] Rename namespaec gnss to s2e::gnss --- src/environment/global/gnss_satellites.cpp | 2 +- src/environment/global/gnss_satellites.hpp | 6 +++--- src/math_physics/gnss/antex_file_reader.cpp | 4 ++-- src/math_physics/gnss/antex_file_reader.hpp | 4 ++-- src/math_physics/gnss/bias_sinex_file_reader.cpp | 4 ++-- src/math_physics/gnss/bias_sinex_file_reader.hpp | 4 ++-- src/math_physics/gnss/gnss_satellite_number.cpp | 4 ++-- src/math_physics/gnss/gnss_satellite_number.hpp | 4 ++-- src/math_physics/gnss/igs_product_name_handling.hpp | 4 ++-- src/math_physics/gnss/sp3_file_reader.cpp | 4 ++-- src/math_physics/gnss/sp3_file_reader.hpp | 4 ++-- src/math_physics/gnss/test_antex_file_reader.cpp | 2 +- src/math_physics/gnss/test_bias_sinex_file_reader.cpp | 2 +- src/math_physics/gnss/test_gnss_satellite_number.cpp | 2 +- src/math_physics/gnss/test_igs_product_name_handling.cpp | 2 +- src/math_physics/gnss/test_sp3_file_reader.cpp | 2 +- 16 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/environment/global/gnss_satellites.cpp b/src/environment/global/gnss_satellites.cpp index c2a844d7a..ecc7683c8 100644 --- a/src/environment/global/gnss_satellites.cpp +++ b/src/environment/global/gnss_satellites.cpp @@ -15,7 +15,7 @@ #include "setting_file_reader/initialize_file_access.hpp" #include "utilities/macros.hpp" -using namespace gnss; +using namespace s2e::gnss; const size_t kNumberOfInterpolation = 9; diff --git a/src/environment/global/gnss_satellites.hpp b/src/environment/global/gnss_satellites.hpp index bbb4aaac3..24af47f6f 100644 --- a/src/environment/global/gnss_satellites.hpp +++ b/src/environment/global/gnss_satellites.hpp @@ -54,7 +54,7 @@ class GnssSatellites : public ILoggable { * @param [in] sp3_files: List of SP3 files * @param [in] start_time: The simulation start time */ - void Initialize(const std::vector& sp3_files, const time_system::EpochTime start_time); + void Initialize(const std::vector& sp3_files, const time_system::EpochTime start_time); /** * @fn IsCalcEnabled @@ -113,7 +113,7 @@ class GnssSatellites : public ILoggable { private: bool is_calc_enabled_ = false; //!< Flag to manage the GNSS satellite position calculation - std::vector sp3_files_; //!< List of SP3 files + std::vector sp3_files_; //!< List of SP3 files size_t number_of_calculated_gnss_satellites_; //!< Number of calculated GNSS satellites size_t sp3_file_id_; //!< Current SP3 file ID time_system::EpochTime reference_time_; //!< Reference start time of the SP3 handling @@ -133,7 +133,7 @@ class GnssSatellites : public ILoggable { * @param [in] current_time: Target time * @return true means no error, false means the time argument is out of range */ - bool GetCurrentSp3File(gnss::Sp3FileReader& current_sp3_file, const time_system::EpochTime current_time); + bool GetCurrentSp3File(s2e::gnss::Sp3FileReader& current_sp3_file, const time_system::EpochTime current_time); /** * @fn UpdateInterpolationInformation diff --git a/src/math_physics/gnss/antex_file_reader.cpp b/src/math_physics/gnss/antex_file_reader.cpp index 9a80470d4..2bcd93793 100644 --- a/src/math_physics/gnss/antex_file_reader.cpp +++ b/src/math_physics/gnss/antex_file_reader.cpp @@ -10,7 +10,7 @@ #include #include -namespace gnss { +namespace s2e::gnss { #define ANTEX_LINE_TYPE_POSITION (60) @@ -199,4 +199,4 @@ time_system::DateTime AntexFileReader::ReadDateTime(std::string line) { return time_system::DateTime(year, month, day, hour, minute, second); } -} // namespace gnss +} // namespace s2e::gnss diff --git a/src/math_physics/gnss/antex_file_reader.hpp b/src/math_physics/gnss/antex_file_reader.hpp index 11ac0dc7d..c25c49477 100644 --- a/src/math_physics/gnss/antex_file_reader.hpp +++ b/src/math_physics/gnss/antex_file_reader.hpp @@ -16,7 +16,7 @@ #include #include -namespace gnss { +namespace s2e::gnss { /** * @class AntexGridDefinition @@ -345,6 +345,6 @@ class AntexFileReader { time_system::DateTime ReadDateTime(std::string line); }; -} // namespace gnss +} // namespace s2e::gnss #endif // S2E_LIBRARY_ANTEX_FILE_READER_HPP_ diff --git a/src/math_physics/gnss/bias_sinex_file_reader.cpp b/src/math_physics/gnss/bias_sinex_file_reader.cpp index 252523ebf..b8a203c27 100644 --- a/src/math_physics/gnss/bias_sinex_file_reader.cpp +++ b/src/math_physics/gnss/bias_sinex_file_reader.cpp @@ -9,7 +9,7 @@ #include #include -namespace gnss { +namespace s2e::gnss { bool BiasSinexFileReader::ReadFile(const std::string file_name) { // File open @@ -245,4 +245,4 @@ void BiasSolutionData::SetTargetSignal(const std::string signal1, const std::str } } -} // namespace gnss +} // namespace s2e::gnss diff --git a/src/math_physics/gnss/bias_sinex_file_reader.hpp b/src/math_physics/gnss/bias_sinex_file_reader.hpp index 20e591d0c..694a9bcc5 100644 --- a/src/math_physics/gnss/bias_sinex_file_reader.hpp +++ b/src/math_physics/gnss/bias_sinex_file_reader.hpp @@ -10,7 +10,7 @@ #include #include -namespace gnss { +namespace s2e::gnss { /** * @enum BiasIdentifier @@ -186,6 +186,6 @@ class BiasSinexFileReader { void ReadBiasSolution(std::ifstream& bias_sinex_file); }; -} // namespace gnss +} // namespace s2e::gnss #endif // S2E_LIBRARY_BIAS_SINEX_FILE_READER_HPP_ \ No newline at end of file diff --git a/src/math_physics/gnss/gnss_satellite_number.cpp b/src/math_physics/gnss/gnss_satellite_number.cpp index 6eb8861a5..2241db843 100644 --- a/src/math_physics/gnss/gnss_satellite_number.cpp +++ b/src/math_physics/gnss/gnss_satellite_number.cpp @@ -5,7 +5,7 @@ #include "gnss_satellite_number.hpp" -namespace gnss { +namespace s2e::gnss { size_t ConvertGnssSatelliteNumberToIndex(const std::string satellite_number) { switch (satellite_number.front()) { @@ -61,4 +61,4 @@ std::string ConvertIndexToGnssSatelliteNumber(const size_t index) { return output; } -} // namespace gnss +} // namespace s2e::gnss diff --git a/src/math_physics/gnss/gnss_satellite_number.hpp b/src/math_physics/gnss/gnss_satellite_number.hpp index 35087c537..2b4e1f08b 100644 --- a/src/math_physics/gnss/gnss_satellite_number.hpp +++ b/src/math_physics/gnss/gnss_satellite_number.hpp @@ -10,7 +10,7 @@ #include -namespace gnss { +namespace s2e::gnss { // GNSS satellite number definition // TODO: Move to initialized file? @@ -46,6 +46,6 @@ size_t ConvertGnssSatelliteNumberToIndex(const std::string satellite_number); */ std::string ConvertIndexToGnssSatelliteNumber(const size_t index); -} // namespace gnss +} // namespace s2e::gnss #endif // S2E_LIBRARY_GNSS_GNSS_SATELLITE_NUMBER_HPP_ diff --git a/src/math_physics/gnss/igs_product_name_handling.hpp b/src/math_physics/gnss/igs_product_name_handling.hpp index e2352f76c..01c923fe7 100644 --- a/src/math_physics/gnss/igs_product_name_handling.hpp +++ b/src/math_physics/gnss/igs_product_name_handling.hpp @@ -10,7 +10,7 @@ #include -namespace gnss { +namespace s2e::gnss { /** * @fn GetOrbitClockFileName @@ -85,6 +85,6 @@ size_t IncrementYearDoy(const size_t year_doy) { return output; } -} // namespace gnss +} // namespace s2e::gnss #endif // S2E_LIBRARY_GNSS_IGS_PRODUCT_NAME_HANDLING_HPP_ diff --git a/src/math_physics/gnss/sp3_file_reader.cpp b/src/math_physics/gnss/sp3_file_reader.cpp index b03fb4923..70c170116 100644 --- a/src/math_physics/gnss/sp3_file_reader.cpp +++ b/src/math_physics/gnss/sp3_file_reader.cpp @@ -11,7 +11,7 @@ #include #include -namespace gnss { +namespace s2e::gnss { Sp3FileReader::Sp3FileReader(const std::string file_name) { ReadFile(file_name); } @@ -422,4 +422,4 @@ Sp3VelocityClockRateCorrelation Sp3FileReader::DecodeVelocityClockRateCorrelatio return correlation; } -} // namespace gnss +} // namespace s2e::gnss diff --git a/src/math_physics/gnss/sp3_file_reader.hpp b/src/math_physics/gnss/sp3_file_reader.hpp index 2f04b25fc..5068a4e82 100644 --- a/src/math_physics/gnss/sp3_file_reader.hpp +++ b/src/math_physics/gnss/sp3_file_reader.hpp @@ -17,7 +17,7 @@ #include #include -namespace gnss { +namespace s2e::gnss { #define SP3_BAD_CLOCK_VALUE (999999.999999) #define SP3_BAD_POSITION_VALUE (0.000000) @@ -236,6 +236,6 @@ class Sp3FileReader { Sp3VelocityClockRateCorrelation DecodeVelocityClockRateCorrelation(std::string line); }; -} // namespace gnss +} // namespace s2e::gnss #endif // S2E_LIBRARY_GNSS_SP3_FILE_READER_HPP_ diff --git a/src/math_physics/gnss/test_antex_file_reader.cpp b/src/math_physics/gnss/test_antex_file_reader.cpp index 320c598af..f64af1003 100644 --- a/src/math_physics/gnss/test_antex_file_reader.cpp +++ b/src/math_physics/gnss/test_antex_file_reader.cpp @@ -7,7 +7,7 @@ #include "antex_file_reader.hpp" -using namespace gnss; +using namespace s2e::gnss; /** * @brief Test Constructor diff --git a/src/math_physics/gnss/test_bias_sinex_file_reader.cpp b/src/math_physics/gnss/test_bias_sinex_file_reader.cpp index a681eedcc..c58f7ceae 100644 --- a/src/math_physics/gnss/test_bias_sinex_file_reader.cpp +++ b/src/math_physics/gnss/test_bias_sinex_file_reader.cpp @@ -2,7 +2,7 @@ #include "bias_sinex_file_reader.hpp" -using namespace gnss; +using namespace s2e::gnss; TEST(BiasSinex, Constructor) { // File read error check diff --git a/src/math_physics/gnss/test_gnss_satellite_number.cpp b/src/math_physics/gnss/test_gnss_satellite_number.cpp index 259920e29..0d4cae6f6 100644 --- a/src/math_physics/gnss/test_gnss_satellite_number.cpp +++ b/src/math_physics/gnss/test_gnss_satellite_number.cpp @@ -6,7 +6,7 @@ #include "gnss_satellite_number.hpp" -using namespace gnss; +using namespace s2e::gnss; /** * @brief Test satellite number to index diff --git a/src/math_physics/gnss/test_igs_product_name_handling.cpp b/src/math_physics/gnss/test_igs_product_name_handling.cpp index f1af19529..6b5d40270 100644 --- a/src/math_physics/gnss/test_igs_product_name_handling.cpp +++ b/src/math_physics/gnss/test_igs_product_name_handling.cpp @@ -7,7 +7,7 @@ #include "igs_product_name_handling.hpp" -using namespace gnss; +using namespace s2e::gnss; /** * @brief Test GetOrbitClockFinalFileName diff --git a/src/math_physics/gnss/test_sp3_file_reader.cpp b/src/math_physics/gnss/test_sp3_file_reader.cpp index 9ef514d41..2f9672ab0 100644 --- a/src/math_physics/gnss/test_sp3_file_reader.cpp +++ b/src/math_physics/gnss/test_sp3_file_reader.cpp @@ -6,7 +6,7 @@ #include "sp3_file_reader.hpp" -using namespace gnss; +using namespace s2e::gnss; /** * @brief Test Constructor From 46ceb8574912682ad16feeaf3edfc37b57f6cbcf Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:10:13 -0700 Subject: [PATCH 06/49] Rename namespaec gravity to s2e::gravity --- src/disturbances/geopotential.cpp | 2 +- src/disturbances/geopotential.hpp | 2 +- src/disturbances/lunar_gravity_field.cpp | 2 +- src/disturbances/lunar_gravity_field.hpp | 2 +- src/math_physics/gravity/gravity_potential.cpp | 4 ++-- src/math_physics/gravity/gravity_potential.hpp | 4 ++-- src/math_physics/gravity/test_gravity_potential.cpp | 2 +- 7 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index 0520730ff..b36df94d4 100644 --- a/src/disturbances/geopotential.cpp +++ b/src/disturbances/geopotential.cpp @@ -43,7 +43,7 @@ Geopotential::Geopotential(const int degree, const std::string file_path, const } } // Initialize GravityPotential - geopotential_ = gravity::GravityPotential(degree, c_, s_); + geopotential_ = s2e::gravity::GravityPotential(degree, c_, s_); } bool Geopotential::ReadCoefficientsEgm96(std::string file_name) { diff --git a/src/disturbances/geopotential.hpp b/src/disturbances/geopotential.hpp index 0f66a463d..0d350caae 100644 --- a/src/disturbances/geopotential.hpp +++ b/src/disturbances/geopotential.hpp @@ -60,7 +60,7 @@ class Geopotential : public Disturbance { virtual std::string GetLogValue() const; private: - gravity::GravityPotential geopotential_; + s2e::gravity::GravityPotential geopotential_; size_t degree_; //!< Maximum degree setting to calculate the geo-potential std::vector> c_; //!< Cosine coefficients std::vector> s_; //!< Sine coefficients diff --git a/src/disturbances/lunar_gravity_field.cpp b/src/disturbances/lunar_gravity_field.cpp index 1baa1319b..781ede97d 100644 --- a/src/disturbances/lunar_gravity_field.cpp +++ b/src/disturbances/lunar_gravity_field.cpp @@ -46,7 +46,7 @@ LunarGravityField::LunarGravityField(const int degree, const std::string file_pa } } // Initialize GravityPotential - lunar_potential_ = gravity::GravityPotential(degree, c_, s_, gravity_constants_km3_s2_ * 1e9, reference_radius_km_ * 1e3); + lunar_potential_ = s2e::gravity::GravityPotential(degree, c_, s_, gravity_constants_km3_s2_ * 1e9, reference_radius_km_ * 1e3); } bool LunarGravityField::ReadCoefficientsGrgm1200a(std::string file_name) { diff --git a/src/disturbances/lunar_gravity_field.hpp b/src/disturbances/lunar_gravity_field.hpp index 0a77b9c4b..dd5f049af 100644 --- a/src/disturbances/lunar_gravity_field.hpp +++ b/src/disturbances/lunar_gravity_field.hpp @@ -62,7 +62,7 @@ class LunarGravityField : public Disturbance { virtual std::string GetLogValue() const; private: - gravity::GravityPotential lunar_potential_; + s2e::gravity::GravityPotential lunar_potential_; double reference_radius_km_; double gravity_constants_km3_s2_; size_t degree_; //!< Maximum degree setting to calculate the geo-potential diff --git a/src/math_physics/gravity/gravity_potential.cpp b/src/math_physics/gravity/gravity_potential.cpp index 6b55c1558..f2adcaa7e 100644 --- a/src/math_physics/gravity/gravity_potential.cpp +++ b/src/math_physics/gravity/gravity_potential.cpp @@ -10,7 +10,7 @@ #include #include -namespace gravity { +namespace s2e::gravity { GravityPotential::GravityPotential(const size_t degree, const std::vector> cosine_coefficients, const std::vector> sine_coefficients, const double gravity_constants_m3_s2, @@ -278,4 +278,4 @@ void GravityPotential::v_w_nm_update(double *v_nm, double *w_nm, const double v_ return; } -} // namespace gravity +} // namespace s2e::gravity diff --git a/src/math_physics/gravity/gravity_potential.hpp b/src/math_physics/gravity/gravity_potential.hpp index 9e25980ba..23d6df4c9 100644 --- a/src/math_physics/gravity/gravity_potential.hpp +++ b/src/math_physics/gravity/gravity_potential.hpp @@ -12,7 +12,7 @@ #include "../math/matrix.hpp" #include "../math/vector.hpp" -namespace gravity { +namespace s2e::gravity { /** * @class GravityPotential @@ -85,6 +85,6 @@ class GravityPotential { void v_w_nm_update(double *v_nm, double *w_nm, const double v_prev, const double w_prev, const double v_prev2, const double w_prev2); }; -} // namespace gravity +} // namespace s2e::gravity #endif // S2E_LIBRARY_GRAVITY_GRAVITY_POTENTIAL_HPP_ diff --git a/src/math_physics/gravity/test_gravity_potential.cpp b/src/math_physics/gravity/test_gravity_potential.cpp index 1c5177463..54717f1c1 100644 --- a/src/math_physics/gravity/test_gravity_potential.cpp +++ b/src/math_physics/gravity/test_gravity_potential.cpp @@ -6,7 +6,7 @@ #include "gravity_potential.hpp" -using namespace gravity; +using namespace s2e::gravity; /** * @brief Test for Acceleration calculation From 5582b4d1c13843964ef66450542c094d1e2b8912 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:15:54 -0700 Subject: [PATCH 07/49] Rename namespaec numerical_integration to s2e::numerical_integration --- .../attitude_with_cantilever_vibration.cpp | 2 +- .../attitude_with_cantilever_vibration.hpp | 4 +- ...ode_attitude_with_cantilever_vibration.hpp | 4 +- .../dormand_prince_5.hpp | 4 +- .../dormand_prince_5_implementation.hpp | 4 +- .../embedded_runge_kutta.hpp | 4 +- .../embedded_runge_kutta_implementation.hpp | 4 +- .../numerical_integration/interface_ode.hpp | 4 +- .../numerical_integrator.hpp | 4 +- .../numerical_integrator_manager.hpp | 4 +- .../numerical_integration/ode_examples.hpp | 4 +- .../numerical_integration/runge_kutta.hpp | 4 +- .../numerical_integration/runge_kutta_4.hpp | 4 +- .../runge_kutta_fehlberg.hpp | 4 +- .../runge_kutta_fehlberg_implementation.hpp | 4 +- .../runge_kutta_template.hpp | 4 +- .../test_runge_kutta.cpp | 92 +++++++++---------- 17 files changed, 77 insertions(+), 77 deletions(-) diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp index 47c8cad29..fe58833da 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp @@ -15,7 +15,7 @@ AttitudeWithCantileverVibration::AttitudeWithCantileverVibration( const std::string& simulation_object_name) : Attitude(inertia_tensor_kgm2, simulation_object_name), numerical_integrator_(propagation_step_s, attitude_ode_, - numerical_integration::NumericalIntegrationMethod::kRk4) { //!< TODO: Set NumericalIntegrationMethod in *.ini file + s2e::numerical_integration::NumericalIntegrationMethod::kRk4) { //!< TODO: Set NumericalIntegrationMethod in *.ini file angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; quaternion_i2b_ = quaternion_i2b; torque_b_Nm_ = torque_b_Nm; diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp index 95226c473..76a8f974f 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp @@ -73,8 +73,8 @@ class AttitudeWithCantileverVibration : public Attitude { s2e::math::Vector<3> angular_velocity_cantilever_rad_s_{0.0}; //!< Angular velocity of the cantilever with respect to the body frame [rad/s] s2e::math::Vector<3> euler_angular_cantilever_rad_{0.0}; //!< Euler angle of the cantilever with respect to the body frame [rad/s] - numerical_integration::AttitudeWithCantileverVibrationOde attitude_ode_; - numerical_integration::NumericalIntegratorManager<13> numerical_integrator_; + s2e::numerical_integration::AttitudeWithCantileverVibrationOde attitude_ode_; + s2e::numerical_integration::NumericalIntegratorManager<13> numerical_integrator_; }; #endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_WITH_CANTILEVER_VIBRATION_HPP_ diff --git a/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp index 770a586db..f91e2be95 100644 --- a/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp @@ -11,7 +11,7 @@ #include "attitude.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { /** * @class AttitudeWithCantileverVibrationOde * @brief Class to implement Ordinary Differential Equations for Attitude with Cantilever Vibration @@ -221,6 +221,6 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { s2e::math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever_{0.0}; //!< Inverse of inertia tensor of the cantilever }; -} // namespace numerical_integration +} // namespace s2e::numerical_integration #endif // S2E_DYNAMICS_ATTITUDE_ODE_ATTITUDE_WITH_CANTILEVER_VIBRATION_HPP_ diff --git a/src/math_physics/numerical_integration/dormand_prince_5.hpp b/src/math_physics/numerical_integration/dormand_prince_5.hpp index 1c6090d66..90e5fb413 100644 --- a/src/math_physics/numerical_integration/dormand_prince_5.hpp +++ b/src/math_physics/numerical_integration/dormand_prince_5.hpp @@ -11,7 +11,7 @@ #include "embedded_runge_kutta.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { /** * @class DormandPrince5 @@ -46,7 +46,7 @@ class DormandPrince5 : public EmbeddedRungeKutta { std::vector CalcInterpolationWeights(const double sigma) const; }; -} // namespace numerical_integration +} // namespace s2e::numerical_integration #include "dormand_prince_5_implementation.hpp" diff --git a/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp b/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp index 4190202b4..3dc53411c 100644 --- a/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp +++ b/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp @@ -11,7 +11,7 @@ #include "dormand_prince_5.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { template DormandPrince5::DormandPrince5(const double step_width, const InterfaceOde& ode) : EmbeddedRungeKutta(step_width, ode) { @@ -149,6 +149,6 @@ std::vector DormandPrince5::CalcInterpolationWeights(const double sig return interpolation_weights; } -} // namespace numerical_integration +} // namespace s2e::numerical_integration #endif // S2E_LIBRARY_NUMERICAL_INTEGRATION_DORMAND_PRINCE_5_IMPLEMENTATION_HPP_ diff --git a/src/math_physics/numerical_integration/embedded_runge_kutta.hpp b/src/math_physics/numerical_integration/embedded_runge_kutta.hpp index 5e6fd605c..c453fc606 100644 --- a/src/math_physics/numerical_integration/embedded_runge_kutta.hpp +++ b/src/math_physics/numerical_integration/embedded_runge_kutta.hpp @@ -8,7 +8,7 @@ #include "runge_kutta.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { /** * @class EmbeddedRungeKutta @@ -51,7 +51,7 @@ class EmbeddedRungeKutta : public RungeKutta { double local_truncation_error_; //!< Norm of estimated local truncation error }; -} // namespace numerical_integration +} // namespace s2e::numerical_integration #include "embedded_runge_kutta_implementation.hpp" diff --git a/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp b/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp index dfed2e7e1..1a73d0775 100644 --- a/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp +++ b/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp @@ -7,7 +7,7 @@ #include "embedded_runge_kutta.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { template void EmbeddedRungeKutta::Integrate() { @@ -37,6 +37,6 @@ void EmbeddedRungeKutta::ControlStepWidth(const double error_tolerance) { this->step_width_ = updated_step_width; } -} // namespace numerical_integration +} // namespace s2e::numerical_integration #endif // S2E_LIBRARY_NUMERICAL_INTEGRATION_EMBEDDED_RUNGE_KUTTA_IMPLEMENTATION_HPP_ diff --git a/src/math_physics/numerical_integration/interface_ode.hpp b/src/math_physics/numerical_integration/interface_ode.hpp index 213598675..a050119b9 100644 --- a/src/math_physics/numerical_integration/interface_ode.hpp +++ b/src/math_physics/numerical_integration/interface_ode.hpp @@ -8,7 +8,7 @@ #include "../math/vector.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { /** * @class InterfaceOde @@ -27,6 +27,6 @@ class InterfaceOde { virtual s2e::math::Vector DerivativeFunction(const double independent_variable, const s2e::math::Vector& state) const = 0; }; -} // namespace numerical_integration +} // namespace s2e::numerical_integration #endif // S2E_LIBRARY_NUMERICAL_INTEGRATION_INTERFACE_ODE_HPP_ diff --git a/src/math_physics/numerical_integration/numerical_integrator.hpp b/src/math_physics/numerical_integration/numerical_integrator.hpp index 83d78e85a..e125ec810 100644 --- a/src/math_physics/numerical_integration/numerical_integrator.hpp +++ b/src/math_physics/numerical_integration/numerical_integrator.hpp @@ -11,7 +11,7 @@ #include "../math/vector.hpp" #include "interface_ode.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { /** * @class NumericalIntegrator @@ -75,6 +75,6 @@ class NumericalIntegrator { s2e::math::Vector previous_state_; //!< Previous state vector }; -} // namespace numerical_integration +} // namespace s2e::numerical_integration #endif // S2E_LIBRARY_NUMERICAL_INTEGRATION_NUMERICAL_INTEGRATOR_HPP_ diff --git a/src/math_physics/numerical_integration/numerical_integrator_manager.hpp b/src/math_physics/numerical_integration/numerical_integrator_manager.hpp index 8869f82b6..c37d3b375 100644 --- a/src/math_physics/numerical_integration/numerical_integrator_manager.hpp +++ b/src/math_physics/numerical_integration/numerical_integrator_manager.hpp @@ -12,7 +12,7 @@ #include "runge_kutta_4.hpp" #include "runge_kutta_fehlberg.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { /** * @enum NumericalIntegrationMethod @@ -62,6 +62,6 @@ class NumericalIntegratorManager { std::shared_ptr> integrator_; }; -} // namespace numerical_integration +} // namespace s2e::numerical_integration #endif // S2E_LIBRARY_NUMERICAL_INTEGRATION_NUMERICAL_INTEGRATION_HPP_ diff --git a/src/math_physics/numerical_integration/ode_examples.hpp b/src/math_physics/numerical_integration/ode_examples.hpp index e6f7e3616..f248f3d3b 100644 --- a/src/math_physics/numerical_integration/ode_examples.hpp +++ b/src/math_physics/numerical_integration/ode_examples.hpp @@ -9,7 +9,7 @@ #include "../../utilities/macros.hpp" #include "interface_ode.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { class ExampleLinearOde : public InterfaceOde<1> { public: @@ -86,6 +86,6 @@ class Example2dTwoBodyOrbitOde : public InterfaceOde<4> { } }; -} // namespace numerical_integration +} // namespace s2e::numerical_integration #endif // S2E_LIBRARY_NUMERICAL_INTEGRATION_EXAMPLE_ODE_HPP_s diff --git a/src/math_physics/numerical_integration/runge_kutta.hpp b/src/math_physics/numerical_integration/runge_kutta.hpp index 333d3cc4a..2bb9a2505 100644 --- a/src/math_physics/numerical_integration/runge_kutta.hpp +++ b/src/math_physics/numerical_integration/runge_kutta.hpp @@ -9,7 +9,7 @@ #include "numerical_integrator.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { /** * @class RungeKutta @@ -55,7 +55,7 @@ class RungeKutta : public NumericalIntegrator { void CalcSlope(); }; -} // namespace numerical_integration +} // namespace s2e::numerical_integration #include "runge_kutta_template.hpp" diff --git a/src/math_physics/numerical_integration/runge_kutta_4.hpp b/src/math_physics/numerical_integration/runge_kutta_4.hpp index 47fc6b6b5..d52106039 100644 --- a/src/math_physics/numerical_integration/runge_kutta_4.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_4.hpp @@ -10,7 +10,7 @@ #include "runge_kutta.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { /** * @class RungeKutta4 @@ -51,6 +51,6 @@ class RungeKutta4 : public RungeKutta { } }; -} // namespace numerical_integration +} // namespace s2e::numerical_integration #endif // S2E_LIBRARY_NUMERICAL_INTEGRATION_RUNGE_KUTTA_4_HPP_ diff --git a/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp b/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp index 16974368f..e22c969eb 100644 --- a/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp @@ -8,7 +8,7 @@ #include "embedded_runge_kutta.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { /** * @class RungeKuttaFehlberg @@ -41,7 +41,7 @@ class RungeKuttaFehlberg : public EmbeddedRungeKutta { std::vector CalcInterpolationWeights(const double sigma) const; }; -} // namespace numerical_integration +} // namespace s2e::numerical_integration #include "runge_kutta_fehlberg_implementation.hpp" diff --git a/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp b/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp index 04d5f8027..5ccfe3941 100644 --- a/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp @@ -8,7 +8,7 @@ #include "runge_kutta_fehlberg.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { template RungeKuttaFehlberg::RungeKuttaFehlberg(const double step_width, const InterfaceOde& ode) : EmbeddedRungeKutta(step_width, ode) { @@ -96,6 +96,6 @@ std::vector RungeKuttaFehlberg::CalcInterpolationWeights(const double return interpolation_weights; } -} // namespace numerical_integration +} // namespace s2e::numerical_integration #endif // S2E_LIBRARY_NUMERICAL_INTEGRATION_RUNGE_KUTTA_FEHLBERG_IMPLEMENTATION_HPP_ diff --git a/src/math_physics/numerical_integration/runge_kutta_template.hpp b/src/math_physics/numerical_integration/runge_kutta_template.hpp index 577b24639..296fe302f 100644 --- a/src/math_physics/numerical_integration/runge_kutta_template.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_template.hpp @@ -7,7 +7,7 @@ #include "./runge_kutta.hpp" -namespace numerical_integration { +namespace s2e::numerical_integration { template void RungeKutta::Integrate() { @@ -34,6 +34,6 @@ void RungeKutta::CalcSlope() { } } -} // namespace numerical_integration +} // namespace s2e::numerical_integration #endif // S2E_LIBRARY_NUMERICAL_INTEGRATION_RUNGE_KUTTA_TEMPLATE_HPP_ diff --git a/src/math_physics/numerical_integration/test_runge_kutta.cpp b/src/math_physics/numerical_integration/test_runge_kutta.cpp index d6ab81d28..3f66d0471 100644 --- a/src/math_physics/numerical_integration/test_runge_kutta.cpp +++ b/src/math_physics/numerical_integration/test_runge_kutta.cpp @@ -15,8 +15,8 @@ * @brief Test for constructor */ TEST(NUMERICAL_INTEGRATION, Constructor) { - numerical_integration::ExampleLinearOde ode; - numerical_integration::RungeKutta4<1> linear_ode(0.1, ode); + s2e::numerical_integration::ExampleLinearOde ode; + s2e::numerical_integration::RungeKutta4<1> linear_ode(0.1, ode); s2e::math::Vector<1> state = linear_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -27,8 +27,8 @@ TEST(NUMERICAL_INTEGRATION, Constructor) { */ TEST(NUMERICAL_INTEGRATION, IntegrateLinearRk4) { double step_width_s = 0.1; - numerical_integration::ExampleLinearOde ode; - numerical_integration::RungeKutta4<1> rk4_ode(step_width_s, ode); + s2e::numerical_integration::ExampleLinearOde ode; + s2e::numerical_integration::RungeKutta4<1> rk4_ode(step_width_s, ode); s2e::math::Vector<1> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -48,8 +48,8 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearRk4) { */ TEST(NUMERICAL_INTEGRATION, IntegrateLinearRkf) { double step_width_s = 0.1; - numerical_integration::ExampleLinearOde ode; - numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); + s2e::numerical_integration::ExampleLinearOde ode; + s2e::numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); s2e::math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -69,8 +69,8 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearRkf) { */ TEST(NUMERICAL_INTEGRATION, IntegrateLinearDp5) { double step_width_s = 0.1; - numerical_integration::ExampleLinearOde ode; - numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); + s2e::numerical_integration::ExampleLinearOde ode; + s2e::numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); s2e::math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -90,8 +90,8 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearDp5) { */ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRk4) { double step_width_s = 0.1; - numerical_integration::ExampleLinearOde ode; - numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode); + s2e::numerical_integration::ExampleLinearOde ode; + s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode); s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -111,9 +111,9 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRk4) { */ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRkf) { double step_width_s = 0.1; - numerical_integration::ExampleLinearOde ode; - numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, - numerical_integration::NumericalIntegrationMethod::kRkf); + s2e::numerical_integration::ExampleLinearOde ode; + s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, + s2e::numerical_integration::NumericalIntegrationMethod::kRkf); s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -137,9 +137,9 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRkf) { */ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerDp5) { double step_width_s = 0.1; - numerical_integration::ExampleLinearOde ode; - numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, - numerical_integration::NumericalIntegrationMethod::kDp5); + s2e::numerical_integration::ExampleLinearOde ode; + s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, + s2e::numerical_integration::NumericalIntegrationMethod::kDp5); s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -163,8 +163,8 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerDp5) { */ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticRk4) { double step_width_s = 0.1; - numerical_integration::ExampleQuadraticOde ode; - numerical_integration::RungeKutta4<1> rk4_ode(step_width_s, ode); + s2e::numerical_integration::ExampleQuadraticOde ode; + s2e::numerical_integration::RungeKutta4<1> rk4_ode(step_width_s, ode); s2e::math::Vector<1> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -184,8 +184,8 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticRk4) { */ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticRkf) { double step_width_s = 0.1; - numerical_integration::ExampleQuadraticOde ode; - numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); + s2e::numerical_integration::ExampleQuadraticOde ode; + s2e::numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); s2e::math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -205,8 +205,8 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticRkf) { */ TEST(NUMERICAL_INTEGRATION, InterpolationQuadraticRkf) { double step_width_s = 10.0; - numerical_integration::ExampleQuadraticOde ode; - numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); + s2e::numerical_integration::ExampleQuadraticOde ode; + s2e::numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); s2e::math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -234,8 +234,8 @@ TEST(NUMERICAL_INTEGRATION, InterpolationQuadraticRkf) { */ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticDp5) { double step_width_s = 0.1; - numerical_integration::ExampleQuadraticOde ode; - numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); + s2e::numerical_integration::ExampleQuadraticOde ode; + s2e::numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); s2e::math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -255,8 +255,8 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticDp5) { */ TEST(NUMERICAL_INTEGRATION, InterpolationQuadraticDp5) { double step_width_s = 10.0; - numerical_integration::ExampleQuadraticOde ode; - numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); + s2e::numerical_integration::ExampleQuadraticOde ode; + s2e::numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); s2e::math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -284,8 +284,8 @@ TEST(NUMERICAL_INTEGRATION, InterpolationQuadraticDp5) { */ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRk4) { double step_width_s = 0.1; - numerical_integration::Example1dPositionVelocityOde ode; - numerical_integration::RungeKutta4<2> rk4_ode(step_width_s, ode); + s2e::numerical_integration::Example1dPositionVelocityOde ode; + s2e::numerical_integration::RungeKutta4<2> rk4_ode(step_width_s, ode); s2e::math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; @@ -314,8 +314,8 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRk4) { */ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRkf) { double step_width_s = 0.1; - numerical_integration::Example1dPositionVelocityOde ode; - numerical_integration::RungeKuttaFehlberg<2> rkf_ode(step_width_s, ode); + s2e::numerical_integration::Example1dPositionVelocityOde ode; + s2e::numerical_integration::RungeKuttaFehlberg<2> rkf_ode(step_width_s, ode); s2e::math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; @@ -344,8 +344,8 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRkf) { */ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityDp5) { double step_width_s = 0.1; - numerical_integration::Example1dPositionVelocityOde ode; - numerical_integration::DormandPrince5<2> dp5_ode(step_width_s, ode); + s2e::numerical_integration::Example1dPositionVelocityOde ode; + s2e::numerical_integration::DormandPrince5<2> dp5_ode(step_width_s, ode); s2e::math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; @@ -374,10 +374,10 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityDp5) { */ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { double step_width_s = 0.1; - numerical_integration::Example2dTwoBodyOrbitOde ode; - numerical_integration::RungeKutta4<4> rk4_ode(step_width_s, ode); - numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); - numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); + s2e::numerical_integration::Example2dTwoBodyOrbitOde ode; + s2e::numerical_integration::RungeKutta4<4> rk4_ode(step_width_s, ode); + s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); + s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.1; @@ -444,10 +444,10 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { */ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { double step_width_s = 0.01; - numerical_integration::Example2dTwoBodyOrbitOde ode; - numerical_integration::RungeKutta4<4> rk4_ode(step_width_s, ode); - numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); - numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); + s2e::numerical_integration::Example2dTwoBodyOrbitOde ode; + s2e::numerical_integration::RungeKutta4<4> rk4_ode(step_width_s, ode); + s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); + s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.9; @@ -514,9 +514,9 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { */ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { double step_width_s = 1.0; - numerical_integration::Example2dTwoBodyOrbitOde ode; - numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); - numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); + s2e::numerical_integration::Example2dTwoBodyOrbitOde ode; + s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); + s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.1; @@ -609,9 +609,9 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { */ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { double step_width_s = 0.01; - numerical_integration::Example2dTwoBodyOrbitOde ode; - numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); - numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); + s2e::numerical_integration::Example2dTwoBodyOrbitOde ode; + s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); + s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.8; From 4727707772b9d1ada8c83a8cf08c048b2a6592de Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:18:08 -0700 Subject: [PATCH 08/49] Rename namespaec optics to s2e::optics --- src/math_physics/optics/gaussian_beam_base.cpp | 4 ++-- src/math_physics/optics/gaussian_beam_base.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/math_physics/optics/gaussian_beam_base.cpp b/src/math_physics/optics/gaussian_beam_base.cpp index 3b558612d..0cfc5e40b 100644 --- a/src/math_physics/optics/gaussian_beam_base.cpp +++ b/src/math_physics/optics/gaussian_beam_base.cpp @@ -8,7 +8,7 @@ #include #include -namespace optics { +namespace s2e::optics { GaussianBeamBase::GaussianBeamBase(double wavelength_m, double radius_beam_waist_m, double total_power_W) : wavelength_m_(wavelength_m), radius_beam_waist_m_(radius_beam_waist_m), total_power_W_(total_power_W) {} @@ -49,4 +49,4 @@ double GaussianBeamBase::CalcIntensity_W_m2(double distance_from_beam_waist_m, d return intensity_W_m2; } -} // namespace optics +} // namespace s2e::optics diff --git a/src/math_physics/optics/gaussian_beam_base.hpp b/src/math_physics/optics/gaussian_beam_base.hpp index 010d4bb31..f9fd71432 100644 --- a/src/math_physics/optics/gaussian_beam_base.hpp +++ b/src/math_physics/optics/gaussian_beam_base.hpp @@ -8,7 +8,7 @@ #include "../math/vector.hpp" -namespace optics { +namespace s2e::optics { /** * @class GaussianBeamBase @@ -109,6 +109,6 @@ class GaussianBeamBase { s2e::math::Vector<3> position_beam_waist_i_m_{0.0}; //!< Position of beam waist in the inertial frame [m] (Not used?) }; -} // namespace optics +} // namespace s2e::optics #endif // S2E_LIBRARY_OPTICS_GAUSSIAN_BEAM_BASE_HPP_ From 6d641f1f685645a4475cfa381a52cf26f97141cc Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:19:56 -0700 Subject: [PATCH 09/49] Rename namespaec orbit to s2e::orbit --- src/dynamics/orbit/encke_orbit_propagation.cpp | 4 ++-- src/dynamics/orbit/encke_orbit_propagation.hpp | 2 +- src/dynamics/orbit/initialize_orbit.cpp | 16 ++++++++-------- src/dynamics/orbit/kepler_orbit_propagation.hpp | 4 ++-- src/dynamics/orbit/relative_orbit.cpp | 16 ++++++++-------- src/dynamics/orbit/relative_orbit.hpp | 10 +++++----- src/environment/global/gnss_satellites.cpp | 2 +- src/environment/global/gnss_satellites.hpp | 2 +- .../numerical_integration/test_runge_kutta.cpp | 16 ++++++++-------- src/math_physics/orbit/interpolation_orbit.cpp | 4 ++-- src/math_physics/orbit/interpolation_orbit.hpp | 4 ++-- src/math_physics/orbit/kepler_orbit.cpp | 4 ++-- src/math_physics/orbit/kepler_orbit.hpp | 4 ++-- src/math_physics/orbit/orbital_elements.cpp | 4 ++-- src/math_physics/orbit/orbital_elements.hpp | 4 ++-- src/math_physics/orbit/relative_orbit_models.cpp | 4 ++-- src/math_physics/orbit/relative_orbit_models.hpp | 4 ++-- .../orbit/test_interpolation_orbit.cpp | 2 +- 18 files changed, 53 insertions(+), 53 deletions(-) diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index ccdbc265d..e25abbde4 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -90,8 +90,8 @@ void EnckeOrbitPropagation::Initialize(double current_time_jd, s2e::math::Vector // reference orbit reference_position_i_m_ = reference_position_i_m; reference_velocity_i_m_s_ = reference_velocity_i_m_s; - orbit::OrbitalElements oe_ref(gravity_constant_m3_s2_, current_time_jd, reference_position_i_m, reference_velocity_i_m_s); - reference_kepler_orbit = orbit::KeplerOrbit(gravity_constant_m3_s2_, oe_ref); + s2e::orbit::OrbitalElements oe_ref(gravity_constant_m3_s2_, current_time_jd, reference_position_i_m, reference_velocity_i_m_s); + reference_kepler_orbit = s2e::orbit::KeplerOrbit(gravity_constant_m3_s2_, oe_ref); // difference orbit difference_position_i_m_.FillUp(0.0); diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index e5ad30c75..a43db4e93 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -65,7 +65,7 @@ class EnckeOrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferenti // reference orbit s2e::math::Vector<3> reference_position_i_m_; //!< Reference orbit position in the inertial frame [m] s2e::math::Vector<3> reference_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] - orbit::KeplerOrbit reference_kepler_orbit; //!< Reference Kepler orbital element + s2e::orbit::KeplerOrbit reference_kepler_orbit; //!< Reference Kepler orbital element // difference orbit s2e::math::Vector<3> difference_position_i_m_; //!< Difference orbit position in the inertial frame [m] diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index 80252658b..9d071761f 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -46,8 +46,8 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // initialize orbit for relative dynamics of formation flying RelativeOrbit::RelativeOrbitUpdateMethod update_method = (RelativeOrbit::RelativeOrbitUpdateMethod)(conf.ReadInt(section_, "relative_orbit_update_method")); - orbit::RelativeOrbitModel relative_dynamics_model_type = (orbit::RelativeOrbitModel)(conf.ReadInt(section_, "relative_dynamics_model_type")); - orbit::StmModel stm_model_type = (orbit::StmModel)(conf.ReadInt(section_, "stm_model_type")); + s2e::orbit::RelativeOrbitModel relative_dynamics_model_type = (s2e::orbit::RelativeOrbitModel)(conf.ReadInt(section_, "relative_dynamics_model_type")); + s2e::orbit::StmModel stm_model_type = (s2e::orbit::StmModel)(conf.ReadInt(section_, "stm_model_type")); s2e::math::Vector<3> init_relative_position_lvlh; conf.ReadVector<3>(section_, "initial_relative_position_lvlh_m", init_relative_position_lvlh); @@ -62,7 +62,7 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string init_relative_velocity_lvlh, update_method, relative_dynamics_model_type, stm_model_type, relative_information); } else if (propagate_mode == "KEPLER") { // initialize orbit for Kepler propagation - orbit::OrbitalElements oe; + s2e::orbit::OrbitalElements oe; // TODO: init_mode_kepler should be removed in the next major update if (initialize_mode == OrbitInitializeMode::kInertialPositionAndVelocity) { // initialize with position and velocity @@ -70,7 +70,7 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string conf.ReadVector<3>(section_, "initial_position_i_m", init_pos_m); s2e::math::Vector<3> init_vel_m_s; conf.ReadVector<3>(section_, "initial_velocity_i_m_s", init_vel_m_s); - oe = orbit::OrbitalElements(gravity_constant_m3_s2, current_time_jd, init_pos_m, init_vel_m_s); + oe = s2e::orbit::OrbitalElements(gravity_constant_m3_s2, current_time_jd, init_pos_m, init_vel_m_s); } else { // initialize with orbital elements double semi_major_axis_m = conf.ReadDouble(section_, "semi_major_axis_m"); @@ -79,9 +79,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string double raan_rad = conf.ReadDouble(section_, "raan_rad"); double arg_perigee_rad = conf.ReadDouble(section_, "argument_of_perigee_rad"); double epoch_jday = conf.ReadDouble(section_, "epoch_jday"); - oe = orbit::OrbitalElements(epoch_jday, semi_major_axis_m, eccentricity, inclination_rad, raan_rad, arg_perigee_rad); + oe = s2e::orbit::OrbitalElements(epoch_jday, semi_major_axis_m, eccentricity, inclination_rad, raan_rad, arg_perigee_rad); } - orbit::KeplerOrbit kepler_orbit(gravity_constant_m3_s2, oe); + s2e::orbit::KeplerOrbit kepler_orbit(gravity_constant_m3_s2, oe); orbit = new KeplerOrbitPropagation(celestial_information, current_time_jd, kepler_orbit); } else if (propagate_mode == "ENCKE") { // initialize orbit for Encke's method @@ -130,8 +130,8 @@ s2e::math::Vector<6> InitializePosVel(std::string initialize_file, double curren double raan_rad = conf.ReadDouble(section_, "raan_rad"); double arg_perigee_rad = conf.ReadDouble(section_, "argument_of_perigee_rad"); double epoch_jday = conf.ReadDouble(section_, "epoch_jday"); - orbit::OrbitalElements oe(epoch_jday, semi_major_axis_m, eccentricity, inclination_rad, raan_rad, arg_perigee_rad); - orbit::KeplerOrbit kepler_orbit(gravity_constant_m3_s2, oe); + s2e::orbit::OrbitalElements oe(epoch_jday, semi_major_axis_m, eccentricity, inclination_rad, raan_rad, arg_perigee_rad); + s2e::orbit::KeplerOrbit kepler_orbit(gravity_constant_m3_s2, oe); kepler_orbit.CalcOrbit(current_time_jd); position_i_m = kepler_orbit.GetPosition_i_m(); diff --git a/src/dynamics/orbit/kepler_orbit_propagation.hpp b/src/dynamics/orbit/kepler_orbit_propagation.hpp index ec93197ab..9604c4bc1 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.hpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.hpp @@ -13,7 +13,7 @@ * @class KeplerOrbitPropagation * @brief Class to propagate spacecraft orbit with Kepler equation */ -class KeplerOrbitPropagation : public Orbit, public orbit::KeplerOrbit { +class KeplerOrbitPropagation : public Orbit, public s2e::orbit::KeplerOrbit { public: // Initialize with orbital elements /** @@ -23,7 +23,7 @@ class KeplerOrbitPropagation : public Orbit, public orbit::KeplerOrbit { * @param [in] current_time_jd: Current Julian day [day] * @param [in] kepler_orbit: Kepler orbit */ - KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_time_jd, orbit::KeplerOrbit kepler_orbit); + KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_time_jd, s2e::orbit::KeplerOrbit kepler_orbit); /** * @fn ~KeplerOrbitPropagation * @brief Destructor diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 831e8d763..5e6065776 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -10,8 +10,8 @@ RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, int reference_spacecraft_id, s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, - RelativeOrbitUpdateMethod update_method, orbit::RelativeOrbitModel relative_dynamics_model_type, - orbit::StmModel stm_model_type, RelativeInformation* relative_information) + RelativeOrbitUpdateMethod update_method, s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, + s2e::orbit::StmModel stm_model_type, RelativeInformation* relative_information) : Orbit(celestial_information), s2e::math::OrdinaryDifferentialEquation<6>(time_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2), @@ -66,12 +66,12 @@ void RelativeOrbit::InitializeState(s2e::math::Vector<3> relative_position_lvlh_ TransformEcefToGeodetic(); } -void RelativeOrbit::CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, +void RelativeOrbit::CalculateSystemMatrix(s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2) { switch (relative_dynamics_model_type) { - case orbit::RelativeOrbitModel::kHill: { + case s2e::orbit::RelativeOrbitModel::kHill: { double reference_sat_orbit_radius = reference_sat_orbit->GetPosition_i_m().CalcNorm(); - system_matrix_ = orbit::CalcHillSystemMatrix(reference_sat_orbit_radius, gravity_constant_m3_s2); + system_matrix_ = s2e::orbit::CalcHillSystemMatrix(reference_sat_orbit_radius, gravity_constant_m3_s2); } default: { // NOT REACHED @@ -80,12 +80,12 @@ void RelativeOrbit::CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dyn } } -void RelativeOrbit::CalculateStm(orbit::StmModel stm_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2, +void RelativeOrbit::CalculateStm(s2e::orbit::StmModel stm_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2, double elapsed_sec) { switch (stm_model_type) { - case orbit::StmModel::kHcw: { + case s2e::orbit::StmModel::kHcw: { double reference_sat_orbit_radius = reference_sat_orbit->GetPosition_i_m().CalcNorm(); - stm_ = orbit::CalcHcwStm(reference_sat_orbit_radius, gravity_constant_m3_s2, elapsed_sec); + stm_ = s2e::orbit::CalcHcwStm(reference_sat_orbit_radius, gravity_constant_m3_s2, elapsed_sec); } default: { // NOT REACHED diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index c281cd3c9..3a98c1249 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -41,7 +41,7 @@ class RelativeOrbit : public Orbit, public s2e::math::OrdinaryDifferentialEquati */ RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, int reference_spacecraft_id, s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, - orbit::RelativeOrbitModel relative_dynamics_model_type, orbit::StmModel stm_model_type, RelativeInformation* relative_information); + s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, s2e::orbit::StmModel stm_model_type, RelativeInformation* relative_information); /** * @fn ~RelativeOrbit * @brief Destructor @@ -81,8 +81,8 @@ class RelativeOrbit : public Orbit, public s2e::math::OrdinaryDifferentialEquati s2e::math::Vector<3> relative_velocity_lvlh_m_s_; //!< Relative velocity in the LVLH frame RelativeOrbitUpdateMethod update_method_; //!< Update method - orbit::RelativeOrbitModel relative_dynamics_model_type_; //!< Relative dynamics model type - orbit::StmModel stm_model_type_; //!< State Transition Matrix model type + s2e::orbit::RelativeOrbitModel relative_dynamics_model_type_; //!< Relative dynamics model type + s2e::orbit::StmModel stm_model_type_; //!< State Transition Matrix model type RelativeInformation* relative_information_; //!< Relative information /** @@ -102,7 +102,7 @@ class RelativeOrbit : public Orbit, public s2e::math::OrdinaryDifferentialEquati * @param [in] reference_sat_orbit: Orbit information of reference satellite * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] */ - void CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2); + void CalculateSystemMatrix(s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2); /** * @fn CalculateStm * @brief Calculate State Transition Matrix @@ -111,7 +111,7 @@ class RelativeOrbit : public Orbit, public s2e::math::OrdinaryDifferentialEquati * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] elapsed_sec: Elapsed time [sec] */ - void CalculateStm(orbit::StmModel stm_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2, double elapsed_sec); + void CalculateStm(s2e::orbit::StmModel stm_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2, double elapsed_sec); /** * @fn PropagateRk4 * @brief Propagate relative orbit with RK4 diff --git a/src/environment/global/gnss_satellites.cpp b/src/environment/global/gnss_satellites.cpp index ecc7683c8..07962c016 100644 --- a/src/environment/global/gnss_satellites.cpp +++ b/src/environment/global/gnss_satellites.cpp @@ -40,7 +40,7 @@ void GnssSatellites::Initialize(const std::vector& sp3_files, con reference_time_ = time_system::EpochTime(initial_sp3_file.GetEpochData(reference_interpolation_id_)); // Initialize orbit - orbit_.assign(number_of_calculated_gnss_satellites_, orbit::InterpolationOrbit(kNumberOfInterpolation)); + orbit_.assign(number_of_calculated_gnss_satellites_, s2e::orbit::InterpolationOrbit(kNumberOfInterpolation)); // Initialize clock std::vector temp; diff --git a/src/environment/global/gnss_satellites.hpp b/src/environment/global/gnss_satellites.hpp index 24af47f6f..f807d0a75 100644 --- a/src/environment/global/gnss_satellites.hpp +++ b/src/environment/global/gnss_satellites.hpp @@ -120,7 +120,7 @@ class GnssSatellites : public ILoggable { size_t reference_interpolation_id_ = 0; //!< Reference epoch ID of the interpolation time_system::EpochTime current_epoch_time_; //!< The last updated time - std::vector orbit_; //!< GNSS satellite orbit with interpolation + std::vector orbit_; //!< GNSS satellite orbit with interpolation std::vector clock_; //!< GNSS satellite clock offset with interpolation // References diff --git a/src/math_physics/numerical_integration/test_runge_kutta.cpp b/src/math_physics/numerical_integration/test_runge_kutta.cpp index 3f66d0471..b2571ba5c 100644 --- a/src/math_physics/numerical_integration/test_runge_kutta.cpp +++ b/src/math_physics/numerical_integration/test_runge_kutta.cpp @@ -416,8 +416,8 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { initial_position[1] = initial_state[1]; initial_velocity[0] = initial_state[2]; initial_velocity[1] = initial_state[3]; - orbit::OrbitalElements oe(1.0, 0.0, initial_position, initial_velocity); - orbit::KeplerOrbit kepler(1.0, oe); + s2e::orbit::OrbitalElements oe(1.0, 0.0, initial_position, initial_velocity); + s2e::orbit::KeplerOrbit kepler(1.0, oe); kepler.CalcOrbit((double)(step_num * step_width_s) / (24.0 * 60.0 * 60.0)); double error_tolerance = 2e-4; @@ -486,8 +486,8 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { initial_position[1] = initial_state[1]; initial_velocity[0] = initial_state[2]; initial_velocity[1] = initial_state[3]; - orbit::OrbitalElements oe(1.0, 0.0, initial_position, initial_velocity); - orbit::KeplerOrbit kepler(1.0, oe); + s2e::orbit::OrbitalElements oe(1.0, 0.0, initial_position, initial_velocity); + s2e::orbit::KeplerOrbit kepler(1.0, oe); kepler.CalcOrbit((double)(step_num * step_width_s) / (24.0 * 60.0 * 60.0)); double error_tolerance = 2e-1; @@ -551,8 +551,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { initial_position[1] = initial_state[1]; initial_velocity[0] = initial_state[2]; initial_velocity[1] = initial_state[3]; - orbit::OrbitalElements oe(1.0, 0.0, initial_position, initial_velocity); - orbit::KeplerOrbit kepler(1.0, oe); + s2e::orbit::OrbitalElements oe(1.0, 0.0, initial_position, initial_velocity); + s2e::orbit::KeplerOrbit kepler(1.0, oe); kepler.CalcOrbit((double)(step_num * step_width_s) / (24.0 * 60.0 * 60.0)); double error_tolerance = 5e-2; @@ -646,8 +646,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { initial_position[1] = initial_state[1]; initial_velocity[0] = initial_state[2]; initial_velocity[1] = initial_state[3]; - orbit::OrbitalElements oe(1.0, 0.0, initial_position, initial_velocity); - orbit::KeplerOrbit kepler(1.0, oe); + s2e::orbit::OrbitalElements oe(1.0, 0.0, initial_position, initial_velocity); + s2e::orbit::KeplerOrbit kepler(1.0, oe); kepler.CalcOrbit((double)(step_num * step_width_s) / (24.0 * 60.0 * 60.0)); double error_tolerance = 1e-5; diff --git a/src/math_physics/orbit/interpolation_orbit.cpp b/src/math_physics/orbit/interpolation_orbit.cpp index 393b30166..db373629a 100644 --- a/src/math_physics/orbit/interpolation_orbit.cpp +++ b/src/math_physics/orbit/interpolation_orbit.cpp @@ -5,7 +5,7 @@ #include "interpolation_orbit.hpp" -namespace orbit { +namespace s2e::orbit { InterpolationOrbit::InterpolationOrbit(const size_t degree) { std::vector time; @@ -37,4 +37,4 @@ s2e::math::Vector<3> InterpolationOrbit::CalcPositionWithTrigonometric(const dou return output_position; } -} // namespace orbit +} // namespace s2e::orbit diff --git a/src/math_physics/orbit/interpolation_orbit.hpp b/src/math_physics/orbit/interpolation_orbit.hpp index db215f534..1443ec959 100644 --- a/src/math_physics/orbit/interpolation_orbit.hpp +++ b/src/math_physics/orbit/interpolation_orbit.hpp @@ -10,7 +10,7 @@ #include #include -namespace orbit { +namespace s2e::orbit { /** * @class InterpolationOrbit @@ -71,6 +71,6 @@ class InterpolationOrbit { std::vector interpolation_position_; // 3D vector of interpolation }; -} // namespace orbit +} // namespace s2e::orbit #endif // S2E_LIBRARY_ORBIT_INTERPOLATION_ORBIT_HPP_ diff --git a/src/math_physics/orbit/kepler_orbit.cpp b/src/math_physics/orbit/kepler_orbit.cpp index 389cc96ab..1f7630ef2 100644 --- a/src/math_physics/orbit/kepler_orbit.cpp +++ b/src/math_physics/orbit/kepler_orbit.cpp @@ -7,7 +7,7 @@ #include "../math/matrix_vector.hpp" #include "../math/s2e_math.hpp" -namespace orbit { +namespace s2e::orbit { KeplerOrbit::KeplerOrbit() {} // Initialize with orbital elements @@ -100,4 +100,4 @@ double KeplerOrbit::SolveKeplerNewtonMethod(const double eccentricity, const dou return u_rad; } -} // namespace orbit +} // namespace s2e::orbit diff --git a/src/math_physics/orbit/kepler_orbit.hpp b/src/math_physics/orbit/kepler_orbit.hpp index b76f15cac..7a12ca760 100644 --- a/src/math_physics/orbit/kepler_orbit.hpp +++ b/src/math_physics/orbit/kepler_orbit.hpp @@ -10,7 +10,7 @@ #include "../math/vector.hpp" #include "./orbital_elements.hpp" -namespace orbit { +namespace s2e::orbit { /** * @class KeplerOrbit @@ -89,6 +89,6 @@ class KeplerOrbit { double SolveKeplerNewtonMethod(const double eccentricity, const double mean_anomaly_rad, const double angle_limit_rad, const int iteration_limit); }; -} // namespace orbit +} // namespace s2e::orbit #endif // S2E_LIBRARY_ORBIT_KEPLER_ORBIT_HPP_ diff --git a/src/math_physics/orbit/orbital_elements.cpp b/src/math_physics/orbit/orbital_elements.cpp index 4c02fac73..69817f80f 100644 --- a/src/math_physics/orbit/orbital_elements.cpp +++ b/src/math_physics/orbit/orbital_elements.cpp @@ -9,7 +9,7 @@ #include "../math/s2e_math.hpp" -namespace orbit { +namespace s2e::orbit { OrbitalElements::OrbitalElements() {} @@ -88,4 +88,4 @@ void OrbitalElements::CalcOeFromPosVel(const double gravity_constant_m3_s2, cons epoch_jday_ = time_jday - dt_s / (24.0 * 60.0 * 60.0); } -} // namespace orbit +} // namespace s2e::orbit diff --git a/src/math_physics/orbit/orbital_elements.hpp b/src/math_physics/orbit/orbital_elements.hpp index 66ccf0c05..b177fda41 100644 --- a/src/math_physics/orbit/orbital_elements.hpp +++ b/src/math_physics/orbit/orbital_elements.hpp @@ -8,7 +8,7 @@ #include "../math/vector.hpp" -namespace orbit { +namespace s2e::orbit { /** * @class OrbitalElements @@ -106,6 +106,6 @@ class OrbitalElements { const s2e::math::Vector<3> velocity_i_m_s); }; -} // namespace orbit +} // namespace s2e::orbit #endif // S2E_LIBRARY_ORBIT_ORBITAL_ELEMENTS_HPP_ diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index b8d619734..0c6e358b6 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -4,7 +4,7 @@ */ #include "relative_orbit_models.hpp" -namespace orbit { +namespace s2e::orbit { s2e::math::Matrix<6, 6> CalcHillSystemMatrix(double orbit_radius_m, double gravity_constant_m3_s2) { s2e::math::Matrix<6, 6> system_matrix; @@ -94,4 +94,4 @@ s2e::math::Matrix<6, 6> CalcHcwStm(double orbit_radius_m, double gravity_constan return stm; } -} // namespace orbit +} // namespace s2e::orbit diff --git a/src/math_physics/orbit/relative_orbit_models.hpp b/src/math_physics/orbit/relative_orbit_models.hpp index 62957b28c..1a12bf634 100644 --- a/src/math_physics/orbit/relative_orbit_models.hpp +++ b/src/math_physics/orbit/relative_orbit_models.hpp @@ -9,7 +9,7 @@ #include "../math/matrix.hpp" #include "../math/vector.hpp" -namespace orbit { +namespace s2e::orbit { /** * @enum RelativeOrbitModel @@ -44,6 +44,6 @@ s2e::math::Matrix<6, 6> CalcHillSystemMatrix(const double orbit_radius_m, const */ s2e::math::Matrix<6, 6> CalcHcwStm(const double orbit_radius_m, const double gravity_constant_m3_s2, const double elapsed_time_s); -} // namespace orbit +} // namespace s2e::orbit #endif // S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_MODEL_HPP_ diff --git a/src/math_physics/orbit/test_interpolation_orbit.cpp b/src/math_physics/orbit/test_interpolation_orbit.cpp index b0972817d..f83a3f0cf 100644 --- a/src/math_physics/orbit/test_interpolation_orbit.cpp +++ b/src/math_physics/orbit/test_interpolation_orbit.cpp @@ -8,7 +8,7 @@ #include "interpolation_orbit.hpp" -using namespace orbit; +using namespace s2e::orbit; /** * @brief Test for Constructor function From 63d04608d5a1598152661f66f079a510759b86e1 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:22:26 -0700 Subject: [PATCH 10/49] Rename namespaec planet_rotation to s2e::planet_rotation --- src/environment/global/moon_rotation.cpp | 2 +- src/math_physics/planet_rotation/moon_rotation_utilities.cpp | 4 ++-- src/math_physics/planet_rotation/moon_rotation_utilities.hpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/environment/global/moon_rotation.cpp b/src/environment/global/moon_rotation.cpp index 75abc2e5b..a49e236ec 100644 --- a/src/environment/global/moon_rotation.cpp +++ b/src/environment/global/moon_rotation.cpp @@ -19,7 +19,7 @@ void MoonRotation::Update(const SimulationTime& simulation_time) { if (mode_ == MoonRotationMode::kSimple) { s2e::math::Vector<3> moon_position_eci_m = celestial_information_.GetPositionFromSelectedBody_i_m("MOON", "EARTH"); s2e::math::Vector<3> moon_velocity_eci_m_s = celestial_information_.GetVelocityFromSelectedBody_i_m_s("MOON", "EARTH"); - dcm_j2000_to_mcmf_ = planet_rotation::CalcDcmEciToPrincipalAxis(moon_position_eci_m, moon_velocity_eci_m_s); + dcm_j2000_to_mcmf_ = s2e::planet_rotation::CalcDcmEciToPrincipalAxis(moon_position_eci_m, moon_velocity_eci_m_s); } else if (mode_ == MoonRotationMode::kIauMoon) { ConstSpiceChar from[] = "J2000"; ConstSpiceChar to[] = "IAU_MOON"; diff --git a/src/math_physics/planet_rotation/moon_rotation_utilities.cpp b/src/math_physics/planet_rotation/moon_rotation_utilities.cpp index 0842aa301..f2d30c96c 100644 --- a/src/math_physics/planet_rotation/moon_rotation_utilities.cpp +++ b/src/math_physics/planet_rotation/moon_rotation_utilities.cpp @@ -10,7 +10,7 @@ #include -namespace planet_rotation { +namespace s2e::planet_rotation { s2e::math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const s2e::math::Vector<3> moon_position_eci_m, const s2e::math::Vector<3> moon_velocity_eci_m_s) { s2e::math::Matrix<3, 3> dcm_eci2me = CalcDcmEciToMeanEarth(moon_position_eci_m, moon_velocity_eci_m_s); @@ -49,4 +49,4 @@ s2e::math::Matrix<3, 3> CalcDcmMeanEarthToPrincipalAxis() { return dcm_me_pa; } -} // namespace planet_rotation +} // namespace s2e::planet_rotation diff --git a/src/math_physics/planet_rotation/moon_rotation_utilities.hpp b/src/math_physics/planet_rotation/moon_rotation_utilities.hpp index cd3c8b442..d7e3eff30 100644 --- a/src/math_physics/planet_rotation/moon_rotation_utilities.hpp +++ b/src/math_physics/planet_rotation/moon_rotation_utilities.hpp @@ -12,7 +12,7 @@ #include "math_physics/math/matrix.hpp" #include "math_physics/math/vector.hpp" -namespace planet_rotation { +namespace s2e::planet_rotation { /** * @fn CalcDcmEciToPrincipalAxis @@ -36,6 +36,6 @@ s2e::math::Matrix<3, 3> CalcDcmEciToMeanEarth(const s2e::math::Vector<3> moon_po */ s2e::math::Matrix<3, 3> CalcDcmMeanEarthToPrincipalAxis(); -} // namespace planet_rotation +} // namespace s2e::planet_rotation #endif // S2E_LIBRARY_PLANET_ROTATION_MOON_MEAN_EARTH_PRINCIPAL_AXIS_FRAME_HPP_ From ba23a62cf15a51a934a2e4ca55d757966afc79b5 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:25:04 -0700 Subject: [PATCH 11/49] Rename namespaec randomization to s2e::randomization --- src/components/base/sensor.hpp | 2 +- src/components/ideal/attitude_observer.hpp | 4 ++-- src/components/ideal/force_generator.hpp | 4 ++-- src/components/ideal/orbit_observer.hpp | 2 +- src/components/ideal/torque_generator.hpp | 4 ++-- src/components/real/aocs/gnss_receiver.hpp | 4 ++-- src/components/real/aocs/magnetorquer.hpp | 2 +- src/components/real/aocs/star_sensor.hpp | 6 +++--- src/components/real/aocs/sun_sensor.cpp | 2 +- src/components/real/aocs/sun_sensor.hpp | 4 ++-- src/components/real/propulsion/simple_thruster.hpp | 4 ++-- src/disturbances/magnetic_disturbance.cpp | 2 +- src/environment/local/atmosphere.cpp | 2 +- src/environment/local/geomagnetic_field.cpp | 2 +- src/math_physics/randomization/global_randomization.hpp | 2 +- .../minimal_standard_linear_congruential_generator.cpp | 2 +- .../minimal_standard_linear_congruential_generator.hpp | 4 ++-- ..._standard_linear_congruential_generator_with_shuffle.cpp | 2 +- ..._standard_linear_congruential_generator_with_shuffle.hpp | 4 ++-- src/math_physics/randomization/normal_randomization.cpp | 2 +- src/math_physics/randomization/normal_randomization.hpp | 6 +++--- src/math_physics/randomization/random_walk.hpp | 2 +- 22 files changed, 34 insertions(+), 34 deletions(-) diff --git a/src/components/base/sensor.hpp b/src/components/base/sensor.hpp index 398b24263..43fabd0cf 100644 --- a/src/components/base/sensor.hpp +++ b/src/components/base/sensor.hpp @@ -55,7 +55,7 @@ class Sensor { s2e::math::Matrix scale_factor_; //!< Scale factor matrix s2e::math::Vector range_to_const_c_; //!< Output range limit to be constant output value at the component frame s2e::math::Vector range_to_zero_c_; //!< Output range limit to be zero output value at the component frame - randomization::NormalRand normal_random_noise_c_[N]; //!< Normal random + s2e::randomization::NormalRand normal_random_noise_c_[N]; //!< Normal random RandomWalk random_walk_noise_c_; //!< Random Walk /** diff --git a/src/components/ideal/attitude_observer.hpp b/src/components/ideal/attitude_observer.hpp index 1efc18b71..1b80659d0 100644 --- a/src/components/ideal/attitude_observer.hpp +++ b/src/components/ideal/attitude_observer.hpp @@ -62,8 +62,8 @@ class AttitudeObserver : public Component, public ILoggable { protected: s2e::math::Quaternion observed_quaternion_i2b_ = {0.0, 0.0, 0.0, 1.0}; //!< Observed quaternion - randomization::NormalRand angle_noise_; //!< Normal random for magnitude noise - randomization::NormalRand direction_noise_; //!< Normal random for direction noise + s2e::randomization::NormalRand angle_noise_; //!< Normal random for magnitude noise + s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise const Attitude& attitude_; //!< Attitude information }; diff --git a/src/components/ideal/force_generator.hpp b/src/components/ideal/force_generator.hpp index 9a07fe269..607e0a6fd 100644 --- a/src/components/ideal/force_generator.hpp +++ b/src/components/ideal/force_generator.hpp @@ -100,8 +100,8 @@ class ForceGenerator : public Component, public ILoggable { s2e::math::Vector<3> generated_force_rtn_N_{0.0}; //!< Generated force in the RTN frame [N] // Noise - randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise - randomization::NormalRand direction_noise_; //!< Normal random for direction noise + s2e::randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise + s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad] /** diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index ab3aa9887..6c1119986 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -82,7 +82,7 @@ class OrbitObserver : public Component, public ILoggable { s2e::math::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] NoiseFrame noise_frame_; //!< Noise definition frame - randomization::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] + s2e::randomization::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] // Observed variables const Orbit& orbit_; //!< Orbit information diff --git a/src/components/ideal/torque_generator.hpp b/src/components/ideal/torque_generator.hpp index c03aa4da1..3b8606464 100644 --- a/src/components/ideal/torque_generator.hpp +++ b/src/components/ideal/torque_generator.hpp @@ -78,8 +78,8 @@ class TorqueGenerator : public Component, public ILoggable { s2e::math::Vector<3> generated_torque_b_Nm_{0.0}; //!< Generated torque in the body fixed frame [Nm] // Noise - randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise - randomization::NormalRand direction_noise_; //!< Normal random for direction noise + s2e::randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise + s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad] /** diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index ad9f38103..14a4dbc86 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -137,8 +137,8 @@ class GnssReceiver : public Component, public ILoggable { AntennaModel antenna_model_; //!< Antenna model // Simple position observation - randomization::NormalRand position_random_noise_ecef_m_[3]; //!< Random noise for position at the ECEF frame [m] - randomization::NormalRand velocity_random_noise_ecef_m_s_[3]; //!< Random noise for velocity at the ECEF frame [m] + s2e::randomization::NormalRand position_random_noise_ecef_m_[3]; //!< Random noise for position at the ECEF frame [m] + s2e::randomization::NormalRand velocity_random_noise_ecef_m_s_[3]; //!< Random noise for velocity at the ECEF frame [m] s2e::math::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] s2e::math::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] s2e::geodesy::GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame diff --git a/src/components/real/aocs/magnetorquer.hpp b/src/components/real/aocs/magnetorquer.hpp index 112125eb9..0af9f30e7 100644 --- a/src/components/real/aocs/magnetorquer.hpp +++ b/src/components/real/aocs/magnetorquer.hpp @@ -134,7 +134,7 @@ class Magnetorquer : public Component, public ILoggable { s2e::math::Vector bias_noise_c_Am2_{0.0}; //!< Constant bias noise in the component frame [Am2] RandomWalk random_walk_c_Am2_; //!< Random walk noise - randomization::NormalRand random_noise_c_Am2_[kMtqDimension]; //!< Normal random noise + s2e::randomization::NormalRand random_noise_c_Am2_[kMtqDimension]; //!< Normal random noise const GeomagneticField* geomagnetic_field_; //!< Geomagnetic environment diff --git a/src/components/real/aocs/star_sensor.hpp b/src/components/real/aocs/star_sensor.hpp index 40f7c46a0..1120f9eeb 100644 --- a/src/components/real/aocs/star_sensor.hpp +++ b/src/components/real/aocs/star_sensor.hpp @@ -115,9 +115,9 @@ class StarSensor : public Component, public ILoggable { s2e::math::Vector<3> second_orthogonal_direction_c; //!< The second orthogonal direction of sight at component frame // Noise parameters - randomization::MinimalStandardLcgWithShuffle rotation_noise_; //!< Randomize object for orthogonal direction - randomization::NormalRand orthogonal_direction_noise_; //!< Random noise for orthogonal direction of sight - randomization::NormalRand sight_direction_noise_; //!< Random noise for sight direction + s2e::randomization::MinimalStandardLcgWithShuffle rotation_noise_; //!< Randomize object for orthogonal direction + s2e::randomization::NormalRand orthogonal_direction_noise_; //!< Random noise for orthogonal direction of sight + s2e::randomization::NormalRand sight_direction_noise_; //!< Random noise for sight direction // Delay emulation parameters int max_delay_; //!< Max delay diff --git a/src/components/real/aocs/sun_sensor.cpp b/src/components/real/aocs/sun_sensor.cpp index b9fbd3cae..0b230e787 100644 --- a/src/components/real/aocs/sun_sensor.cpp +++ b/src/components/real/aocs/sun_sensor.cpp @@ -7,7 +7,7 @@ #include #include -using randomization::NormalRand; +using s2e::randomization::NormalRand; #include #include #include diff --git a/src/components/real/aocs/sun_sensor.hpp b/src/components/real/aocs/sun_sensor.hpp index 81625a87c..532dfb18f 100644 --- a/src/components/real/aocs/sun_sensor.hpp +++ b/src/components/real/aocs/sun_sensor.hpp @@ -100,8 +100,8 @@ class SunSensor : public Component, public ILoggable { double detectable_angle_rad_; //!< half angle (>0) [rad] bool sun_detected_flag_ = false; //!< Sun detected flag // Noise parameters - randomization::NormalRand random_noise_alpha_; //!< Normal random for alpha angle - randomization::NormalRand random_noise_beta_; //!< Normal random for beta angle + s2e::randomization::NormalRand random_noise_alpha_; //!< Normal random for alpha angle + s2e::randomization::NormalRand random_noise_beta_; //!< Normal random for beta angle double bias_noise_alpha_rad_ = 0.0; //!< Constant bias for alpha angle (Value is calculated by random number generator) double bias_noise_beta_rad_ = 0.0; //!< Constant bias for beta angle (Value is calculated by random number generator) diff --git a/src/components/real/propulsion/simple_thruster.hpp b/src/components/real/propulsion/simple_thruster.hpp index f2466fcc9..f297b8af2 100644 --- a/src/components/real/propulsion/simple_thruster.hpp +++ b/src/components/real/propulsion/simple_thruster.hpp @@ -114,8 +114,8 @@ class SimpleThruster : public Component, public ILoggable { double duty_ = 0.0; //!< PWM Duty [0.0 : 1.0] double thrust_magnitude_max_N_ = 0.0; //!< Maximum thrust magnitude [N] double direction_noise_standard_deviation_rad_ = 0.0; //!< Standard deviation of thrust direction error [rad] - randomization::NormalRand magnitude_random_noise_; //!< Normal random for thrust magnitude error - randomization::NormalRand direction_random_noise_; //!< Normal random for thrust direction error + s2e::randomization::NormalRand magnitude_random_noise_; //!< Normal random for thrust magnitude error + s2e::randomization::NormalRand direction_random_noise_; //!< Normal random for thrust direction error // outputs Vector<3> output_thrust_b_N_{0.0}; //!< Generated thrust on the body fixed frame [N] Vector<3> output_torque_b_Nm_{0.0}; //!< Generated torque on the body fixed frame [Nm] diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index 9af45805c..0336b1182 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -34,7 +34,7 @@ void MagneticDisturbance::CalcRMM() { static s2e::math::Vector<3> random_walk_std_dev(residual_magnetic_moment_.GetRandomWalkStandardDeviation_Am2()); static s2e::math::Vector<3> random_walk_limit(residual_magnetic_moment_.GetRandomWalkLimit_Am2()); static RandomWalk<3> random_walk(0.1, random_walk_std_dev, random_walk_limit); // [FIXME] step width is constant - static randomization::NormalRand normal_random(0.0, residual_magnetic_moment_.GetRandomNoiseStandardDeviation_Am2(), + static s2e::randomization::NormalRand normal_random(0.0, residual_magnetic_moment_.GetRandomNoiseStandardDeviation_Am2(), global_randomization.MakeSeed()); rmm_b_Am2_ = residual_magnetic_moment_.GetConstantValue_b_Am2(); diff --git a/src/environment/local/atmosphere.cpp b/src/environment/local/atmosphere.cpp index 1ea0636a6..ceb1a9139 100644 --- a/src/environment/local/atmosphere.cpp +++ b/src/environment/local/atmosphere.cpp @@ -78,7 +78,7 @@ double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const Orbit& double Atmosphere::AddNoise(const double rho_kg_m3) { // RandomWalk rw(rho_kg_m3*rw_stepwidth_,rho_kg_m3*rw_stddev_,rho_kg_m3*rw_limit_); - randomization::NormalRand nr(0.0, rho_kg_m3 * gauss_standard_deviation_rate_, global_randomization.MakeSeed()); + s2e::randomization::NormalRand nr(0.0, rho_kg_m3 * gauss_standard_deviation_rate_, global_randomization.MakeSeed()); double nrd = nr; return rho_kg_m3 + nrd; diff --git a/src/environment/local/geomagnetic_field.cpp b/src/environment/local/geomagnetic_field.cpp index 6fcdcc7f9..df45b492c 100644 --- a/src/environment/local/geomagnetic_field.cpp +++ b/src/environment/local/geomagnetic_field.cpp @@ -44,7 +44,7 @@ void GeomagneticField::AddNoise(double* magnetic_field_array_i_nT) { static s2e::math::Vector<3> limit(random_walk_limit_nT_); static RandomWalk<3> random_walk(0.1, standard_deviation, limit); - static randomization::NormalRand white_noise(0.0, white_noise_standard_deviation_nT_, global_randomization.MakeSeed()); + static s2e::randomization::NormalRand white_noise(0.0, white_noise_standard_deviation_nT_, global_randomization.MakeSeed()); for (int i = 0; i < 3; ++i) { magnetic_field_array_i_nT[i] += random_walk[i] + white_noise; diff --git a/src/math_physics/randomization/global_randomization.hpp b/src/math_physics/randomization/global_randomization.hpp index 24c88ec2b..67c52b6f6 100644 --- a/src/math_physics/randomization/global_randomization.hpp +++ b/src/math_physics/randomization/global_randomization.hpp @@ -33,7 +33,7 @@ class GlobalRandomization { private: static const unsigned int kMaxSeed = 0xffffffff; //!< Maximum value of seed - randomization::MinimalStandardLcg base_randomizer_; //!< Base of global randomization + s2e::randomization::MinimalStandardLcg base_randomizer_; //!< Base of global randomization long seed_; //!< Seed of global randomization }; diff --git a/src/math_physics/randomization/minimal_standard_linear_congruential_generator.cpp b/src/math_physics/randomization/minimal_standard_linear_congruential_generator.cpp index ccd179507..c1821e93b 100644 --- a/src/math_physics/randomization/minimal_standard_linear_congruential_generator.cpp +++ b/src/math_physics/randomization/minimal_standard_linear_congruential_generator.cpp @@ -5,7 +5,7 @@ */ #include "minimal_standard_linear_congruential_generator.hpp" -using randomization::MinimalStandardLcg; +using s2e::randomization::MinimalStandardLcg; #include diff --git a/src/math_physics/randomization/minimal_standard_linear_congruential_generator.hpp b/src/math_physics/randomization/minimal_standard_linear_congruential_generator.hpp index 9120faffd..01ff61947 100644 --- a/src/math_physics/randomization/minimal_standard_linear_congruential_generator.hpp +++ b/src/math_physics/randomization/minimal_standard_linear_congruential_generator.hpp @@ -7,7 +7,7 @@ #ifndef S2E_LIBRARY_RANDOMIZATION_MINIMAL_STANDARD_LINEAR_CONGRUENTIAL_GENERATOR_HPP_ #define S2E_LIBRARY_RANDOMIZATION_MINIMAL_STANDARD_LINEAR_CONGRUENTIAL_GENERATOR_HPP_ -namespace randomization { +namespace s2e::randomization { /** * @class MinimalStandardLcg @@ -52,6 +52,6 @@ class MinimalStandardLcg { long seed_; //!< Seed of randomization }; -} // namespace randomization +} // namespace s2e::randomization #endif // S2E_LIBRARY_RANDOMIZATION_MINIMAL_STANDARD_LINEAR_CONGRUENTIAL_GENERATOR_HPP_ diff --git a/src/math_physics/randomization/minimal_standard_linear_congruential_generator_with_shuffle.cpp b/src/math_physics/randomization/minimal_standard_linear_congruential_generator_with_shuffle.cpp index ea0a6bf43..3bdd51de9 100644 --- a/src/math_physics/randomization/minimal_standard_linear_congruential_generator_with_shuffle.cpp +++ b/src/math_physics/randomization/minimal_standard_linear_congruential_generator_with_shuffle.cpp @@ -5,7 +5,7 @@ */ #include "minimal_standard_linear_congruential_generator_with_shuffle.hpp" -using randomization::MinimalStandardLcgWithShuffle; +using s2e::randomization::MinimalStandardLcgWithShuffle; MinimalStandardLcgWithShuffle::MinimalStandardLcgWithShuffle() : table_position_(0) { Initialize(); } diff --git a/src/math_physics/randomization/minimal_standard_linear_congruential_generator_with_shuffle.hpp b/src/math_physics/randomization/minimal_standard_linear_congruential_generator_with_shuffle.hpp index 5c016291f..3d5913bb7 100644 --- a/src/math_physics/randomization/minimal_standard_linear_congruential_generator_with_shuffle.hpp +++ b/src/math_physics/randomization/minimal_standard_linear_congruential_generator_with_shuffle.hpp @@ -11,7 +11,7 @@ #include "minimal_standard_linear_congruential_generator.hpp" -namespace randomization { +namespace s2e::randomization { /** * @class MinimalStandardLcgWithShuffle @@ -58,6 +58,6 @@ class MinimalStandardLcgWithShuffle { double mixing_table_[kTableSize]; //!< Mixing table }; -} // namespace randomization +} // namespace s2e::randomization #endif // S2E_LIBRARY_RANDOMIZATION_MINIMAL_STANDARD_LINEAR_CONGRUENTIAL_GENERATOR_WITH_SHUFFLE_HPP_ diff --git a/src/math_physics/randomization/normal_randomization.cpp b/src/math_physics/randomization/normal_randomization.cpp index 0e5c9e36c..b136700cb 100644 --- a/src/math_physics/randomization/normal_randomization.cpp +++ b/src/math_physics/randomization/normal_randomization.cpp @@ -4,7 +4,7 @@ * @note Ref: NUMERICAL RECIPES in C, p.216-p.217 */ #include "normal_randomization.hpp" -using randomization::NormalRand; +using s2e::randomization::NormalRand; #include //DBL_EPSILON #include //sqrt, log; diff --git a/src/math_physics/randomization/normal_randomization.hpp b/src/math_physics/randomization/normal_randomization.hpp index 81e688091..b230680a3 100644 --- a/src/math_physics/randomization/normal_randomization.hpp +++ b/src/math_physics/randomization/normal_randomization.hpp @@ -8,9 +8,9 @@ #define S2E_LIBRARY_RANDOMIZATION_NORMAL_RANDOMIZATION_HPP_ #include "minimal_standard_linear_congruential_generator_with_shuffle.hpp" -using randomization::MinimalStandardLcgWithShuffle; +using s2e::randomization::MinimalStandardLcgWithShuffle; -namespace randomization { +namespace s2e::randomization { /** * @class NormalRand @@ -106,6 +106,6 @@ class NormalRand { bool is_empty_; //!< Flag to show the holder_ has available value }; -} // namespace randomization +} // namespace s2e::randomization #endif // S2E_LIBRARY_RANDOMIZATION_NORMAL_RANDOMIZATION_HPP_ diff --git a/src/math_physics/randomization/random_walk.hpp b/src/math_physics/randomization/random_walk.hpp index 822d621aa..9f05aff9f 100644 --- a/src/math_physics/randomization/random_walk.hpp +++ b/src/math_physics/randomization/random_walk.hpp @@ -37,7 +37,7 @@ class RandomWalk : public s2e::math::OrdinaryDifferentialEquation { private: s2e::math::Vector limit_; //!< Limit of random walk - randomization::NormalRand normal_randomizer_[N]; //!< Random walk excitation noise + s2e::randomization::NormalRand normal_randomizer_[N]; //!< Random walk excitation noise }; #include "random_walk_template_functions.hpp" // template function definisions. From 2539d242cb326d561ecf6f239dde703ddb966de1 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:32:40 -0700 Subject: [PATCH 12/49] Rename namespaec randomization to s2e::randomization --- src/components/base/sensor_template_functions.hpp | 2 +- src/components/ideal/orbit_observer.cpp | 2 +- src/components/real/aocs/gnss_receiver.cpp | 8 ++++---- src/components/real/aocs/star_sensor.cpp | 12 ++++++------ src/components/real/aocs/sun_sensor.cpp | 2 +- src/disturbances/magnetic_disturbance.cpp | 2 +- src/environment/local/atmosphere.cpp | 2 +- src/environment/local/geomagnetic_field.cpp | 2 +- .../randomization/global_randomization.cpp | 6 +++++- .../randomization/global_randomization.hpp | 4 ++++ .../randomization/random_walk_template_functions.hpp | 2 +- 11 files changed, 26 insertions(+), 18 deletions(-) diff --git a/src/components/base/sensor_template_functions.hpp b/src/components/base/sensor_template_functions.hpp index ef6f9a7e5..b0e4f0300 100644 --- a/src/components/base/sensor_template_functions.hpp +++ b/src/components/base/sensor_template_functions.hpp @@ -20,7 +20,7 @@ Sensor::Sensor(const s2e::math::Matrix& scale_factor, const s2e::math:: range_to_zero_c_(range_to_zero_c), random_walk_noise_c_(random_walk_step_width_s, random_walk_standard_deviation_c, random_walk_limit_c) { for (size_t i = 0; i < N; i++) { - normal_random_noise_c_[i].SetParameters(0.0, normal_random_standard_deviation_c[i], global_randomization.MakeSeed()); + normal_random_noise_c_[i].SetParameters(0.0, normal_random_standard_deviation_c[i], s2e::randomization::global_randomization.MakeSeed()); } RangeCheck(); } diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 48a2157d2..f724d0ce9 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -12,7 +12,7 @@ OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generato const s2e::math::Vector<6> error_standard_deviation, const Orbit& orbit) : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { - normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed()); + normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], s2e::randomization::global_randomization.MakeSeed()); } } diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 87e88abfd..f5c28e8cd 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -26,8 +26,8 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) { for (size_t i = 0; i < 3; i++) { - position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], global_randomization.MakeSeed()); - velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], global_randomization.MakeSeed()); + position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], s2e::randomization::global_randomization.MakeSeed()); + velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], s2e::randomization::global_randomization.MakeSeed()); } } @@ -46,8 +46,8 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) { for (size_t i = 0; i < 3; i++) { - position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], global_randomization.MakeSeed()); - velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], global_randomization.MakeSeed()); + position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], s2e::randomization::global_randomization.MakeSeed()); + velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], s2e::randomization::global_randomization.MakeSeed()); } } diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index c6c0e5384..193805226 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -24,9 +24,9 @@ StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, con : Component(prescaler, clock_generator), component_id_(component_id), quaternion_b2c_(quaternion_b2c), - rotation_noise_(global_randomization.MakeSeed()), - orthogonal_direction_noise_(0.0, standard_deviation_orthogonal_direction, global_randomization.MakeSeed()), - sight_direction_noise_(0.0, standard_deviation_sight_direction, global_randomization.MakeSeed()), + rotation_noise_(s2e::randomization::global_randomization.MakeSeed()), + orthogonal_direction_noise_(0.0, standard_deviation_orthogonal_direction, s2e::randomization::global_randomization.MakeSeed()), + sight_direction_noise_(0.0, standard_deviation_sight_direction, s2e::randomization::global_randomization.MakeSeed()), buffer_position_(0), step_time_s_(step_time_s), output_delay_(output_delay), @@ -49,9 +49,9 @@ StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, Pow : Component(prescaler, clock_generator, power_port), component_id_(component_id), quaternion_b2c_(quaternion_b2c), - rotation_noise_(global_randomization.MakeSeed()), - orthogonal_direction_noise_(0.0, standard_deviation_orthogonal_direction, global_randomization.MakeSeed()), - sight_direction_noise_(0.0, standard_deviation_sight_direction, global_randomization.MakeSeed()), + rotation_noise_(s2e::randomization::global_randomization.MakeSeed()), + orthogonal_direction_noise_(0.0, standard_deviation_orthogonal_direction, s2e::randomization::global_randomization.MakeSeed()), + sight_direction_noise_(0.0, standard_deviation_sight_direction, s2e::randomization::global_randomization.MakeSeed()), buffer_position_(0), step_time_s_(step_time_s), output_delay_(output_delay), diff --git a/src/components/real/aocs/sun_sensor.cpp b/src/components/real/aocs/sun_sensor.cpp index 0b230e787..3070355e4 100644 --- a/src/components/real/aocs/sun_sensor.cpp +++ b/src/components/real/aocs/sun_sensor.cpp @@ -44,7 +44,7 @@ SunSensor::SunSensor(const int prescaler, ClockGenerator* clock_generator, Power void SunSensor::Initialize(const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad) { // Bias - NormalRand nr(0.0, bias_noise_standard_deviation_rad, global_randomization.MakeSeed()); + NormalRand nr(0.0, bias_noise_standard_deviation_rad, s2e::randomization::global_randomization.MakeSeed()); bias_noise_alpha_rad_ += nr; bias_noise_beta_rad_ += nr; diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index 0336b1182..054032b9c 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -35,7 +35,7 @@ void MagneticDisturbance::CalcRMM() { static s2e::math::Vector<3> random_walk_limit(residual_magnetic_moment_.GetRandomWalkLimit_Am2()); static RandomWalk<3> random_walk(0.1, random_walk_std_dev, random_walk_limit); // [FIXME] step width is constant static s2e::randomization::NormalRand normal_random(0.0, residual_magnetic_moment_.GetRandomNoiseStandardDeviation_Am2(), - global_randomization.MakeSeed()); + s2e::randomization::global_randomization.MakeSeed()); rmm_b_Am2_ = residual_magnetic_moment_.GetConstantValue_b_Am2(); for (int i = 0; i < 3; ++i) { diff --git a/src/environment/local/atmosphere.cpp b/src/environment/local/atmosphere.cpp index ceb1a9139..ae789fdb3 100644 --- a/src/environment/local/atmosphere.cpp +++ b/src/environment/local/atmosphere.cpp @@ -78,7 +78,7 @@ double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const Orbit& double Atmosphere::AddNoise(const double rho_kg_m3) { // RandomWalk rw(rho_kg_m3*rw_stepwidth_,rho_kg_m3*rw_stddev_,rho_kg_m3*rw_limit_); - s2e::randomization::NormalRand nr(0.0, rho_kg_m3 * gauss_standard_deviation_rate_, global_randomization.MakeSeed()); + s2e::randomization::NormalRand nr(0.0, rho_kg_m3 * gauss_standard_deviation_rate_, s2e::randomization::global_randomization.MakeSeed()); double nrd = nr; return rho_kg_m3 + nrd; diff --git a/src/environment/local/geomagnetic_field.cpp b/src/environment/local/geomagnetic_field.cpp index df45b492c..48cdc57c0 100644 --- a/src/environment/local/geomagnetic_field.cpp +++ b/src/environment/local/geomagnetic_field.cpp @@ -44,7 +44,7 @@ void GeomagneticField::AddNoise(double* magnetic_field_array_i_nT) { static s2e::math::Vector<3> limit(random_walk_limit_nT_); static RandomWalk<3> random_walk(0.1, standard_deviation, limit); - static s2e::randomization::NormalRand white_noise(0.0, white_noise_standard_deviation_nT_, global_randomization.MakeSeed()); + static s2e::randomization::NormalRand white_noise(0.0, white_noise_standard_deviation_nT_, s2e::randomization::global_randomization.MakeSeed()); for (int i = 0; i < 3; ++i) { magnetic_field_array_i_nT[i] += random_walk[i] + white_noise; diff --git a/src/math_physics/randomization/global_randomization.cpp b/src/math_physics/randomization/global_randomization.cpp index 7a7282902..5c1bdef1a 100644 --- a/src/math_physics/randomization/global_randomization.cpp +++ b/src/math_physics/randomization/global_randomization.cpp @@ -5,6 +5,8 @@ #include "global_randomization.hpp" +namespace s2e::randomization { + GlobalRandomization global_randomization; GlobalRandomization::GlobalRandomization() { seed_ = 0xdeadbeef; } @@ -21,4 +23,6 @@ long GlobalRandomization::MakeSeed() { seed = 0xdeadbeef; } return seed; -} \ No newline at end of file +} + +} // namespace s2e::randomization diff --git a/src/math_physics/randomization/global_randomization.hpp b/src/math_physics/randomization/global_randomization.hpp index 67c52b6f6..fd9dac4a0 100644 --- a/src/math_physics/randomization/global_randomization.hpp +++ b/src/math_physics/randomization/global_randomization.hpp @@ -8,6 +8,8 @@ #include "./minimal_standard_linear_congruential_generator.hpp" +namespace s2e::randomization { + /** * @class global_randomization.hpp * @brief Class to manage global randomization @@ -39,4 +41,6 @@ class GlobalRandomization { extern GlobalRandomization global_randomization; //!< Global randomization +} // namespace s2e::randomization + #endif // S2E_LIBRARY_RANDOMIZATION_GLOBAL_RANDOMIZATION_HPP_ diff --git a/src/math_physics/randomization/random_walk_template_functions.hpp b/src/math_physics/randomization/random_walk_template_functions.hpp index ee224018c..4c4a77ca6 100644 --- a/src/math_physics/randomization/random_walk_template_functions.hpp +++ b/src/math_physics/randomization/random_walk_template_functions.hpp @@ -14,7 +14,7 @@ RandomWalk::RandomWalk(double step_width_s, const s2e::math::Vector& stand : s2e::math::OrdinaryDifferentialEquation(step_width_s), limit_(limit) { // Set standard deviation for (size_t i = 0; i < N; ++i) { - normal_randomizer_[i].SetParameters(0.0, standard_deviation[i], global_randomization.MakeSeed()); + normal_randomizer_[i].SetParameters(0.0, standard_deviation[i], s2e::randomization::global_randomization.MakeSeed()); } } From 2c597240f7793993d6d3837eb187a83aebd8bd31 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 14 Sep 2024 00:33:44 -0700 Subject: [PATCH 13/49] Rename namespaec time_system to s2e::time_system --- src/environment/global/gnss_satellites.cpp | 26 +++++++++---------- src/environment/global/gnss_satellites.hpp | 12 ++++----- src/math_physics/gnss/antex_file_reader.cpp | 4 +-- src/math_physics/gnss/antex_file_reader.hpp | 14 +++++----- src/math_physics/gnss/sp3_file_reader.cpp | 18 ++++++------- src/math_physics/gnss/sp3_file_reader.hpp | 14 +++++----- .../time_system/date_time_format.cpp | 4 +-- .../time_system/date_time_format.hpp | 4 +-- src/math_physics/time_system/epoch_time.cpp | 4 +-- src/math_physics/time_system/epoch_time.hpp | 4 +-- src/math_physics/time_system/gps_time.cpp | 4 +-- src/math_physics/time_system/gps_time.hpp | 4 +-- .../time_system/test_date_time_format.cpp | 2 +- .../time_system/test_epoch_time.cpp | 2 +- .../time_system/test_gps_time.cpp | 2 +- 15 files changed, 59 insertions(+), 59 deletions(-) diff --git a/src/environment/global/gnss_satellites.cpp b/src/environment/global/gnss_satellites.cpp index 07962c016..11b551228 100644 --- a/src/environment/global/gnss_satellites.cpp +++ b/src/environment/global/gnss_satellites.cpp @@ -19,7 +19,7 @@ using namespace s2e::gnss; const size_t kNumberOfInterpolation = 9; -void GnssSatellites::Initialize(const std::vector& sp3_files, const time_system::EpochTime start_time) { +void GnssSatellites::Initialize(const std::vector& sp3_files, const s2e::time_system::EpochTime start_time) { sp3_files_ = sp3_files; current_epoch_time_ = start_time; @@ -37,7 +37,7 @@ void GnssSatellites::Initialize(const std::vector& sp3_files, con if (nearest_epoch_id >= half_interpolation_number) { reference_interpolation_id_ = nearest_epoch_id - half_interpolation_number; } - reference_time_ = time_system::EpochTime(initial_sp3_file.GetEpochData(reference_interpolation_id_)); + reference_time_ = s2e::time_system::EpochTime(initial_sp3_file.GetEpochData(reference_interpolation_id_)); // Initialize orbit orbit_.assign(number_of_calculated_gnss_satellites_, s2e::orbit::InterpolationOrbit(kNumberOfInterpolation)); @@ -60,9 +60,9 @@ void GnssSatellites::Update(const SimulationTime& simulation_time) { // Get time UTC current_utc = simulation_time.GetCurrentUtc(); - time_system::DateTime current_date_time((size_t)current_utc.year, (size_t)current_utc.month, (size_t)current_utc.day, (size_t)current_utc.hour, + s2e::time_system::DateTime current_date_time((size_t)current_utc.year, (size_t)current_utc.month, (size_t)current_utc.day, (size_t)current_utc.hour, (size_t)current_utc.minute, current_utc.second); - current_epoch_time_ = time_system::EpochTime(current_date_time); + current_epoch_time_ = s2e::time_system::EpochTime(current_date_time); // Check interpolation update double diff_s = current_epoch_time_.GetTimeWithFraction_s() - reference_time_.GetTimeWithFraction_s(); @@ -74,10 +74,10 @@ void GnssSatellites::Update(const SimulationTime& simulation_time) { return; } -s2e::math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_id, const time_system::EpochTime time) const { +s2e::math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time) const { if (gnss_satellite_id > number_of_calculated_gnss_satellites_) return s2e::math::Vector<3>(0.0); - time_system::EpochTime target_time; + s2e::time_system::EpochTime target_time; if (time.GetTime_s() == 0) { target_time = current_epoch_time_; @@ -92,10 +92,10 @@ s2e::math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satell return orbit_[gnss_satellite_id].CalcPositionWithTrigonometric(diff_s, s2e::math::tau / kOrbitalPeriodCorrection_s); } -double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const time_system::EpochTime time) const { +double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time) const { if (gnss_satellite_id > number_of_calculated_gnss_satellites_) return 0.0; - time_system::EpochTime target_time; + s2e::time_system::EpochTime target_time; if (time.GetTime_s() == 0) { target_time = current_epoch_time_; @@ -109,9 +109,9 @@ double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const time_sys return clock_[gnss_satellite_id].CalcPolynomial(diff_s) * 1e-6; } -bool GnssSatellites::GetCurrentSp3File(Sp3FileReader& current_sp3_file, const time_system::EpochTime current_time) { +bool GnssSatellites::GetCurrentSp3File(Sp3FileReader& current_sp3_file, const s2e::time_system::EpochTime current_time) { for (size_t i = 0; i < sp3_files_.size(); i++) { - time_system::EpochTime sp3_start_time(sp3_files_[i].GetStartEpochDateTime()); + s2e::time_system::EpochTime sp3_start_time(sp3_files_[i].GetStartEpochDateTime()); double diff_s = current_time.GetTimeWithFraction_s() - sp3_start_time.GetTimeWithFraction_s(); if (diff_s < 0.0) { // Error @@ -129,7 +129,7 @@ bool GnssSatellites::UpdateInterpolationInformation() { Sp3FileReader sp3_file = sp3_files_[sp3_file_id_]; for (size_t gnss_id = 0; gnss_id < number_of_calculated_gnss_satellites_; gnss_id++) { - time_system::EpochTime sp3_time = time_system::EpochTime(sp3_file.GetEpochData(reference_interpolation_id_)); + s2e::time_system::EpochTime sp3_time = s2e::time_system::EpochTime(sp3_file.GetEpochData(reference_interpolation_id_)); double time_diff_s = sp3_time.GetTimeWithFraction_s() - reference_time_.GetTimeWithFraction_s(); s2e::math::Vector<3> sp3_position_m = 1000.0 * sp3_file.GetSatellitePosition_km(reference_interpolation_id_, gnss_id); @@ -226,10 +226,10 @@ GnssSatellites* InitGnssSatellites(const std::string file_name, const EarthRotat } // - time_system::DateTime start_date_time((size_t)simulation_time.GetStartYear(), (size_t)simulation_time.GetStartMonth(), + s2e::time_system::DateTime start_date_time((size_t)simulation_time.GetStartYear(), (size_t)simulation_time.GetStartMonth(), (size_t)simulation_time.GetStartDay(), (size_t)simulation_time.GetStartHour(), (size_t)simulation_time.GetStartMinute(), simulation_time.GetStartSecond()); - time_system::EpochTime start_epoch_time(start_date_time); + s2e::time_system::EpochTime start_epoch_time(start_date_time); gnss_satellites->Initialize(sp3_file_readers, start_epoch_time); return gnss_satellites; diff --git a/src/environment/global/gnss_satellites.hpp b/src/environment/global/gnss_satellites.hpp index f807d0a75..6051ce4f5 100644 --- a/src/environment/global/gnss_satellites.hpp +++ b/src/environment/global/gnss_satellites.hpp @@ -54,7 +54,7 @@ class GnssSatellites : public ILoggable { * @param [in] sp3_files: List of SP3 files * @param [in] start_time: The simulation start time */ - void Initialize(const std::vector& sp3_files, const time_system::EpochTime start_time); + void Initialize(const std::vector& sp3_files, const s2e::time_system::EpochTime start_time); /** * @fn IsCalcEnabled @@ -87,7 +87,7 @@ class GnssSatellites : public ILoggable { * @param [in] time: Target time to get the GNSS satellite. When the argument is not set, the last updated time is used for the calculation. * @return GNSS satellite position at ECEF frame at the time. Or return zero vector when the arguments are out of range. */ - s2e::math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, const time_system::EpochTime time = time_system::EpochTime(0, 0.0)) const; + s2e::math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time = s2e::time_system::EpochTime(0, 0.0)) const; /** * @fn GetGetClock_s @@ -96,7 +96,7 @@ class GnssSatellites : public ILoggable { * @param [in] time: Target time to get the GNSS satellite. When the argument is not set, the last updated time is used for the calculation. * @return GNSS satellite clock offset at the time. Or return zero when the arguments are out of range. */ - double GetClock_s(const size_t gnss_satellite_id, const time_system::EpochTime time = time_system::EpochTime(0, 0.0)) const; + double GetClock_s(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time = s2e::time_system::EpochTime(0, 0.0)) const; // Override ILoggable /** @@ -116,9 +116,9 @@ class GnssSatellites : public ILoggable { std::vector sp3_files_; //!< List of SP3 files size_t number_of_calculated_gnss_satellites_; //!< Number of calculated GNSS satellites size_t sp3_file_id_; //!< Current SP3 file ID - time_system::EpochTime reference_time_; //!< Reference start time of the SP3 handling + s2e::time_system::EpochTime reference_time_; //!< Reference start time of the SP3 handling size_t reference_interpolation_id_ = 0; //!< Reference epoch ID of the interpolation - time_system::EpochTime current_epoch_time_; //!< The last updated time + s2e::time_system::EpochTime current_epoch_time_; //!< The last updated time std::vector orbit_; //!< GNSS satellite orbit with interpolation std::vector clock_; //!< GNSS satellite clock offset with interpolation @@ -133,7 +133,7 @@ class GnssSatellites : public ILoggable { * @param [in] current_time: Target time * @return true means no error, false means the time argument is out of range */ - bool GetCurrentSp3File(s2e::gnss::Sp3FileReader& current_sp3_file, const time_system::EpochTime current_time); + bool GetCurrentSp3File(s2e::gnss::Sp3FileReader& current_sp3_file, const s2e::time_system::EpochTime current_time); /** * @fn UpdateInterpolationInformation diff --git a/src/math_physics/gnss/antex_file_reader.cpp b/src/math_physics/gnss/antex_file_reader.cpp index 2bcd93793..c27c34ff7 100644 --- a/src/math_physics/gnss/antex_file_reader.cpp +++ b/src/math_physics/gnss/antex_file_reader.cpp @@ -192,11 +192,11 @@ AntexPhaseCenterData AntexFileReader::ReadPhaseCenterData(std::ifstream& antex_f return phase_center_data; } -time_system::DateTime AntexFileReader::ReadDateTime(std::string line) { +s2e::time_system::DateTime AntexFileReader::ReadDateTime(std::string line) { size_t year, month, day, hour, minute; double second; sscanf(line.c_str(), "%zu %2zu %2zu %2zu %2zu %10lf", &year, &month, &day, &hour, &minute, &second); - return time_system::DateTime(year, month, day, hour, minute, second); + return s2e::time_system::DateTime(year, month, day, hour, minute, second); } } // namespace s2e::gnss diff --git a/src/math_physics/gnss/antex_file_reader.hpp b/src/math_physics/gnss/antex_file_reader.hpp index c25c49477..179616bb9 100644 --- a/src/math_physics/gnss/antex_file_reader.hpp +++ b/src/math_physics/gnss/antex_file_reader.hpp @@ -203,12 +203,12 @@ class AntexSatelliteData { * @fn SetValidStartTime * @param[in] valid_start_time: Valid start time */ - inline void SetValidStartTime(const time_system::DateTime valid_start_time) { valid_start_time_ = valid_start_time; }; + inline void SetValidStartTime(const s2e::time_system::DateTime valid_start_time) { valid_start_time_ = valid_start_time; }; /** * @fn SetValidEndTime * @param[in] valid_end_time: Valid end time */ - inline void SetValidEndTime(const time_system::DateTime valid_end_time) { valid_end_time_ = valid_end_time; }; + inline void SetValidEndTime(const s2e::time_system::DateTime valid_end_time) { valid_end_time_ = valid_end_time; }; /** * @fn SetNumberOfFrequency * @param[in] number_of_frequency: Number of frequency @@ -235,12 +235,12 @@ class AntexSatelliteData { * @fn GetValidStartTime * @return Valid start time */ - inline time_system::DateTime GetValidStartTime() const { return valid_start_time_; }; + inline s2e::time_system::DateTime GetValidStartTime() const { return valid_start_time_; }; /** * @fn GetValidEndTime * @return Valid end time */ - inline time_system::DateTime GetValidEndTime() const { return valid_end_time_; }; + inline s2e::time_system::DateTime GetValidEndTime() const { return valid_end_time_; }; /** * @fn GetNumberOfFrequency * @return Number of frequency @@ -256,8 +256,8 @@ class AntexSatelliteData { private: std::string antenna_type_; //!< Antenna type std::string serial_number_; //!< Serial number or satellite code - time_system::DateTime valid_start_time_; //!< Valid start time - time_system::DateTime valid_end_time_; //!< Valid end time (The latest data does not have the end time) + s2e::time_system::DateTime valid_start_time_; //!< Valid start time + s2e::time_system::DateTime valid_end_time_; //!< Valid end time (The latest data does not have the end time) size_t number_of_frequency_ = 1; //!< Number of frequency std::vector phase_center_data_; //!< Phase center data for each frequency }; @@ -342,7 +342,7 @@ class AntexFileReader { * @param[in] line: A single line in ANTEX file * @return Read date time */ - time_system::DateTime ReadDateTime(std::string line); + s2e::time_system::DateTime ReadDateTime(std::string line); }; } // namespace s2e::gnss diff --git a/src/math_physics/gnss/sp3_file_reader.cpp b/src/math_physics/gnss/sp3_file_reader.cpp index 70c170116..212661e73 100644 --- a/src/math_physics/gnss/sp3_file_reader.cpp +++ b/src/math_physics/gnss/sp3_file_reader.cpp @@ -15,9 +15,9 @@ namespace s2e::gnss { Sp3FileReader::Sp3FileReader(const std::string file_name) { ReadFile(file_name); } -time_system::DateTime Sp3FileReader::GetEpochData(const size_t epoch_id) const { +s2e::time_system::DateTime Sp3FileReader::GetEpochData(const size_t epoch_id) const { if (epoch_id > epoch_.size()) { - time_system::DateTime zero; + s2e::time_system::DateTime zero; return zero; } return epoch_[epoch_id]; @@ -69,7 +69,7 @@ bool Sp3FileReader::ReadFile(const std::string file_name) { size_t year, month, day, hour, minute; double second; sscanf(line.substr(3, 28).c_str(), "%zu %2zu %2zu %2zu %2zu %12lf", &year, &month, &day, &hour, &minute, &second); - epoch_.push_back(time_system::DateTime(year, month, day, hour, minute, second)); + epoch_.push_back(s2e::time_system::DateTime(year, month, day, hour, minute, second)); // Orbit and Clock information for (size_t satellite_id = 0; satellite_id < header_.number_of_satellites_; satellite_id++) { @@ -117,7 +117,7 @@ bool Sp3FileReader::ReadFile(const std::string file_name) { } // Test - time_system::DateTime test = epoch_[0]; + s2e::time_system::DateTime test = epoch_[0]; test = epoch_[1]; std::vector test_p = position_clock_[0]; test_p = position_clock_[1]; @@ -126,7 +126,7 @@ bool Sp3FileReader::ReadFile(const std::string file_name) { return true; } -size_t Sp3FileReader::SearchNearestEpochId(const time_system::EpochTime time) { +size_t Sp3FileReader::SearchNearestEpochId(const s2e::time_system::EpochTime time) { size_t nearest_epoch_id = 0; // Get header info @@ -134,10 +134,10 @@ size_t Sp3FileReader::SearchNearestEpochId(const time_system::EpochTime time) { const double interval_s = header_.epoch_interval_s_; // Check range - time_system::EpochTime start_epoch(epoch_[0]); + s2e::time_system::EpochTime start_epoch(epoch_[0]); if (start_epoch > time) { nearest_epoch_id = 0; - } else if ((time_system::EpochTime)(epoch_[num_epoch - 1]) < time) { + } else if ((s2e::time_system::EpochTime)(epoch_[num_epoch - 1]) < time) { nearest_epoch_id = num_epoch - 1; } else { // Calc nearest point double diff_s = time.GetTimeWithFraction_s() - start_epoch.GetTimeWithFraction_s(); @@ -171,7 +171,7 @@ size_t Sp3FileReader::ReadHeader(std::ifstream& sp3_file) { size_t year, month, day, hour, minute; double second; sscanf(line.substr(3, 28).c_str(), "%zu %2zu %2zu %2zu %2zu %12lf", &year, &month, &day, &hour, &minute, &second); - header_.start_epoch_ = time_system::DateTime(year, month, day, hour, minute, second); + header_.start_epoch_ = s2e::time_system::DateTime(year, month, day, hour, minute, second); header_.number_of_epoch_ = std::stoi(line.substr(32, 7)); header_.used_data_ = line.substr(40, 5); header_.coordinate_system_ = line.substr(46, 5); @@ -199,7 +199,7 @@ size_t Sp3FileReader::ReadHeader(std::ifstream& sp3_file) { return 0; } // Read contents - header_.start_gps_time_ = time_system::GpsTime(std::stoi(line.substr(3, 4)), std::stod(line.substr(8, 15))); + header_.start_gps_time_ = s2e::time_system::GpsTime(std::stoi(line.substr(3, 4)), std::stod(line.substr(8, 15))); header_.epoch_interval_s_ = std::stod(line.substr(24, 14)); header_.start_time_mjday_ = std::stoi(line.substr(39, 5)); header_.start_time_mjday_fractional_day_ = std::stod(line.substr(45, 15)); diff --git a/src/math_physics/gnss/sp3_file_reader.hpp b/src/math_physics/gnss/sp3_file_reader.hpp index 5068a4e82..076ebd4d4 100644 --- a/src/math_physics/gnss/sp3_file_reader.hpp +++ b/src/math_physics/gnss/sp3_file_reader.hpp @@ -52,7 +52,7 @@ struct Sp3Header { // 1st line information // version -> not implemented yet Sp3Mode mode_; //!< position or velocity - time_system::DateTime start_epoch_; //!< Time of start epoch + s2e::time_system::DateTime start_epoch_; //!< Time of start epoch size_t number_of_epoch_ = 0; //!< Number of epoch in the SP3 file std::string used_data_; //!< Used data to generate the SP3 file std::string coordinate_system_; //!< Coordinate system for the position and velocity data @@ -60,7 +60,7 @@ struct Sp3Header { std::string agency_name_; //!< Agency name who generates the SP3 file // 2nd line information - time_system::GpsTime start_gps_time_; //!< Start time of orbit + s2e::time_system::GpsTime start_gps_time_; //!< Start time of orbit double epoch_interval_s_ = 1.0; //!< Epoch interval (0.0, 100000.0) size_t start_time_mjday_; //!< Start time of the orbit data (44244 = 6th Jan. 1980) [Modified Julian day] double start_time_mjday_fractional_day_ = 0.0; //!< Fractional part of the start time [0.0, 1.0) [day] @@ -172,19 +172,19 @@ class Sp3FileReader { inline Sp3Header GetHeader() const { return header_; } inline size_t GetNumberOfEpoch() const { return header_.number_of_epoch_; } inline size_t GetNumberOfSatellites() const { return header_.number_of_satellites_; } - inline time_system::DateTime GetStartEpochDateTime() const { return header_.start_epoch_; } - inline time_system::GpsTime GetStartEpochGpsTime() const { return header_.start_gps_time_; } + inline s2e::time_system::DateTime GetStartEpochDateTime() const { return header_.start_epoch_; } + inline s2e::time_system::GpsTime GetStartEpochGpsTime() const { return header_.start_gps_time_; } // Data - time_system::DateTime GetEpochData(const size_t epoch_id) const; + s2e::time_system::DateTime GetEpochData(const size_t epoch_id) const; Sp3PositionClock GetPositionClock(const size_t epoch_id, const size_t satellite_id); double GetSatelliteClockOffset(const size_t epoch_id, const size_t satellite_id); s2e::math::Vector<3> GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id); - size_t SearchNearestEpochId(const time_system::EpochTime time); + size_t SearchNearestEpochId(const s2e::time_system::EpochTime time); private: Sp3Header header_; //!< SP3 header information - std::vector epoch_; //!< Epoch data list + std::vector epoch_; //!< Epoch data list // Orbit and clock data (Use as position_clock_[satellite_id][epoch_id]) std::map> position_clock_; //!< Position and Clock data diff --git a/src/math_physics/time_system/date_time_format.cpp b/src/math_physics/time_system/date_time_format.cpp index 069c7247f..d470bfef5 100644 --- a/src/math_physics/time_system/date_time_format.cpp +++ b/src/math_physics/time_system/date_time_format.cpp @@ -11,7 +11,7 @@ #include #include -namespace time_system { +namespace s2e::time_system { DateTime::DateTime(const std::string date_time) { sscanf(date_time.c_str(), "%zu/%zu/%zu %zu:%zu:%lf", &year_, &month_, &day_, &hour_, &minute_, &second_); @@ -55,4 +55,4 @@ std::string DateTime::GetAsString() const { return output; } -} // namespace time_system +} // namespace s2e::time_system diff --git a/src/math_physics/time_system/date_time_format.hpp b/src/math_physics/time_system/date_time_format.hpp index 25084e979..540c6fefa 100644 --- a/src/math_physics/time_system/date_time_format.hpp +++ b/src/math_physics/time_system/date_time_format.hpp @@ -10,7 +10,7 @@ #include "epoch_time.hpp" -namespace time_system { +namespace s2e::time_system { class EpochTime; @@ -62,6 +62,6 @@ class DateTime { double second_; //!< Second [0.0, 60.0) }; -} // namespace time_system +} // namespace s2e::time_system #endif // S2E_LIBRARY_TIME_SYSTEM_DATE_TIME_FORMAT_HPP_ diff --git a/src/math_physics/time_system/epoch_time.cpp b/src/math_physics/time_system/epoch_time.cpp index b3b6ec730..cee1a4068 100644 --- a/src/math_physics/time_system/epoch_time.cpp +++ b/src/math_physics/time_system/epoch_time.cpp @@ -7,7 +7,7 @@ #include -namespace time_system { +namespace s2e::time_system { EpochTime::EpochTime(const DateTime date_time) { // No leap second calculation @@ -76,4 +76,4 @@ EpochTime EpochTime::operator-(const EpochTime& right_side) const { return result; } -} // namespace time_system +} // namespace s2e::time_system diff --git a/src/math_physics/time_system/epoch_time.hpp b/src/math_physics/time_system/epoch_time.hpp index 7f7dfe871..72723c066 100644 --- a/src/math_physics/time_system/epoch_time.hpp +++ b/src/math_physics/time_system/epoch_time.hpp @@ -10,7 +10,7 @@ #include "date_time_format.hpp" -namespace time_system { +namespace s2e::time_system { class DateTime; @@ -69,6 +69,6 @@ class EpochTime { double fraction_s_; //!< Fraction of second under 1 sec [0, 1) }; -} // namespace time_system +} // namespace s2e::time_system #endif // S2E_LIBRARY_TIME_SYSTEM_EPOCH_TIME_HPP_ diff --git a/src/math_physics/time_system/gps_time.cpp b/src/math_physics/time_system/gps_time.cpp index cb3a5fec9..5bed94290 100644 --- a/src/math_physics/time_system/gps_time.cpp +++ b/src/math_physics/time_system/gps_time.cpp @@ -5,7 +5,7 @@ #include "gps_time.hpp" -namespace time_system { +namespace s2e::time_system { const DateTime GpsTime::kEpochOfGpsTimeInDateTime_ = DateTime("1980/1/6 00:00:00.0"); const EpochTime GpsTime::kEpochOfGpsTimeInEpochTime_ = EpochTime(kEpochOfGpsTimeInDateTime_); @@ -26,4 +26,4 @@ void GpsTime::CalcEpochTime() { epoch_time_ = kEpochOfGpsTimeInEpochTime_ + time_diff; } -} // namespace time_system +} // namespace s2e::time_system diff --git a/src/math_physics/time_system/gps_time.hpp b/src/math_physics/time_system/gps_time.hpp index f86242754..fdc42d5a8 100644 --- a/src/math_physics/time_system/gps_time.hpp +++ b/src/math_physics/time_system/gps_time.hpp @@ -11,7 +11,7 @@ #include "date_time_format.hpp" #include "epoch_time.hpp" -namespace time_system { +namespace s2e::time_system { /** * @class GpsTime @@ -97,6 +97,6 @@ class GpsTime { void CalcEpochTime(); }; -} // namespace time_system +} // namespace s2e::time_system #endif // S2E_LIBRARY_TIME_SYSTEM_GPS_TIME_HPP_ diff --git a/src/math_physics/time_system/test_date_time_format.cpp b/src/math_physics/time_system/test_date_time_format.cpp index 4ac1a3840..935e92776 100644 --- a/src/math_physics/time_system/test_date_time_format.cpp +++ b/src/math_physics/time_system/test_date_time_format.cpp @@ -2,7 +2,7 @@ #include "date_time_format.hpp" -using namespace time_system; +using namespace s2e::time_system; /** * @brief Test Constructor with value diff --git a/src/math_physics/time_system/test_epoch_time.cpp b/src/math_physics/time_system/test_epoch_time.cpp index 6cb5f0c1b..e1281ca11 100644 --- a/src/math_physics/time_system/test_epoch_time.cpp +++ b/src/math_physics/time_system/test_epoch_time.cpp @@ -3,7 +3,7 @@ #include "date_time_format.hpp" #include "epoch_time.hpp" -using namespace time_system; +using namespace s2e::time_system; TEST(EpochTime, ConstructorNominal) { EpochTime time(1000000000, 0.250); diff --git a/src/math_physics/time_system/test_gps_time.cpp b/src/math_physics/time_system/test_gps_time.cpp index 425556774..737d2f7c6 100644 --- a/src/math_physics/time_system/test_gps_time.cpp +++ b/src/math_physics/time_system/test_gps_time.cpp @@ -2,7 +2,7 @@ #include "gps_time.hpp" -using namespace time_system; +using namespace s2e::time_system; /** * @brief Test Constructor From 6a437ec9155b6abd2d3ddf345bcf2d3d0975d9a8 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 13:55:03 +0900 Subject: [PATCH 14/49] Add s2e::components namespace for base --- src/components/base/component.cpp | 4 ++++ src/components/base/component.hpp | 4 ++++ src/components/base/gpio_connection_with_obc.cpp | 4 ++++ src/components/base/gpio_connection_with_obc.hpp | 4 ++++ src/components/base/i2c_controller.cpp | 5 +++++ src/components/base/i2c_controller.hpp | 4 ++++ src/components/base/i2c_target_communication_with_obc.cpp | 4 ++++ src/components/base/i2c_target_communication_with_obc.hpp | 4 ++++ src/components/base/interface_gpio_component.hpp | 4 ++++ src/components/base/interface_tickable.hpp | 4 ++++ src/components/base/sensor.hpp | 4 ++++ src/components/base/sensor_template_functions.hpp | 4 ++++ src/components/base/uart_communication_with_obc.cpp | 4 ++++ src/components/base/uart_communication_with_obc.hpp | 4 ++++ 14 files changed, 57 insertions(+) diff --git a/src/components/base/component.cpp b/src/components/base/component.cpp index 82fdf14c9..df61cb1ce 100644 --- a/src/components/base/component.cpp +++ b/src/components/base/component.cpp @@ -5,6 +5,8 @@ #include "component.hpp" +namespace s2e::components { + Component::Component(const unsigned int prescaler, ClockGenerator* clock_generator, const unsigned int fast_prescaler) : clock_generator_(clock_generator) { power_port_ = new PowerPort(); @@ -48,3 +50,5 @@ void Component::FastTick(const unsigned int fast_count) { PowerOffRoutine(); } } + +} // namespace s2e::components diff --git a/src/components/base/component.hpp b/src/components/base/component.hpp index 91432e1f3..72dbf6c6c 100644 --- a/src/components/base/component.hpp +++ b/src/components/base/component.hpp @@ -12,6 +12,8 @@ #include "interface_tickable.hpp" +namespace s2e::components { + /** * @class Component * @brief Base class for component emulation. All components have to inherit this. @@ -88,4 +90,6 @@ class Component : public ITickable { PowerPort* power_port_; //!< Power port }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_BASE_COMPONENT_HPP_ diff --git a/src/components/base/gpio_connection_with_obc.cpp b/src/components/base/gpio_connection_with_obc.cpp index 7a3024a66..7b87659b0 100644 --- a/src/components/base/gpio_connection_with_obc.cpp +++ b/src/components/base/gpio_connection_with_obc.cpp @@ -6,6 +6,8 @@ #include "gpio_connection_with_obc.hpp" +namespace s2e::components { + GpioConnectionWithObc::GpioConnectionWithObc(const std::vector port_id, OnBoardComputer* obc) : port_id_(port_id), obc_(obc) { for (size_t i = 0; i < port_id_.size(); i++) { obc_->GpioConnectPort(port_id_[i]); @@ -17,3 +19,5 @@ GpioConnectionWithObc::~GpioConnectionWithObc() {} bool GpioConnectionWithObc::Read(const int index) { return obc_->GpioComponentRead(port_id_[index]); } void GpioConnectionWithObc::Write(const int index, const bool is_high) { obc_->GpioComponentWrite(port_id_[index], is_high); } + +} // namespace s2e::components diff --git a/src/components/base/gpio_connection_with_obc.hpp b/src/components/base/gpio_connection_with_obc.hpp index 397456674..b066b1f38 100644 --- a/src/components/base/gpio_connection_with_obc.hpp +++ b/src/components/base/gpio_connection_with_obc.hpp @@ -9,6 +9,8 @@ #include "../real/cdh/on_board_computer.hpp" +namespace s2e::components { + /** * @class GpioConnectionWithObc * @brief Base class for GPIO communication with OBC flight software @@ -50,4 +52,6 @@ class GpioConnectionWithObc { OnBoardComputer* obc_; //!< The communication target OBC }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_BASE_GPIO_CONNECTION_WITH_OBC_HPP_ diff --git a/src/components/base/i2c_controller.cpp b/src/components/base/i2c_controller.cpp index 64e1429ba..6cc80c056 100644 --- a/src/components/base/i2c_controller.cpp +++ b/src/components/base/i2c_controller.cpp @@ -7,6 +7,9 @@ #include #include + +namespace s2e::components { + I2cController::I2cController(const unsigned int hils_port_id, const unsigned int baud_rate, const unsigned int tx_buffer_size, const unsigned int rx_buffer_size, HilsPortManager* hils_port_manager) : hils_port_id_(hils_port_id), @@ -48,3 +51,5 @@ int I2cController::SendCommand(const unsigned char length) { if (simulation_mode_ != SimulationMode::kHils) return -1; return hils_port_manager_->I2cControllerSend(hils_port_id_, &tx_buffer_.front(), 0, length); } + +} // namespace s2e::components diff --git a/src/components/base/i2c_controller.hpp b/src/components/base/i2c_controller.hpp index 62f59e480..bd1e5e2d2 100644 --- a/src/components/base/i2c_controller.hpp +++ b/src/components/base/i2c_controller.hpp @@ -9,6 +9,8 @@ #include "../../simulation/hils/hils_port_manager.hpp" #include "uart_communication_with_obc.hpp" +namespace s2e::components { + /** * @class I2cController * @brief This class simulates the I2C Controller communication with the I2C Target. @@ -64,4 +66,6 @@ class I2cController { HilsPortManager* hils_port_manager_; //!< HILS port manager }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_BASE_I2C_CONTROLLER_HPP_ diff --git a/src/components/base/i2c_target_communication_with_obc.cpp b/src/components/base/i2c_target_communication_with_obc.cpp index a4b7cc4fe..a916b0df7 100644 --- a/src/components/base/i2c_target_communication_with_obc.cpp +++ b/src/components/base/i2c_target_communication_with_obc.cpp @@ -6,6 +6,8 @@ #include +namespace s2e::components { + I2cTargetCommunicationWithObc::I2cTargetCommunicationWithObc(const unsigned int sils_port_id, const unsigned char i2c_address, OnBoardComputer* obc) : sils_port_id_(sils_port_id), i2c_address_(i2c_address), obc_(obc) { #ifdef USE_HILS @@ -163,3 +165,5 @@ int I2cTargetCommunicationWithObc::StoreTelemetry(const unsigned int stored_fram } return 0; } + +} // namespace s2e::components diff --git a/src/components/base/i2c_target_communication_with_obc.hpp b/src/components/base/i2c_target_communication_with_obc.hpp index 12ff9a4ac..4d017aafe 100644 --- a/src/components/base/i2c_target_communication_with_obc.hpp +++ b/src/components/base/i2c_target_communication_with_obc.hpp @@ -10,6 +10,8 @@ #include "../real/cdh/on_board_computer.hpp" #include "uart_communication_with_obc.hpp" +namespace s2e::components { + /** * @class I2cTargetCommunicationWithObc * @brief Base class for I2C communication as target side with OBC flight software @@ -121,4 +123,6 @@ class I2cTargetCommunicationWithObc { HilsPortManager* hils_port_manager_; //!< HILS port manager }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_BASE_I2C_TARGET_COMMUNICATION_WITH_OBC_HPP_ diff --git a/src/components/base/interface_gpio_component.hpp b/src/components/base/interface_gpio_component.hpp index 7ed129e7b..9df3db260 100644 --- a/src/components/base/interface_gpio_component.hpp +++ b/src/components/base/interface_gpio_component.hpp @@ -6,6 +6,8 @@ #ifndef S2E_COMPONENTS_BASE_CLASSES_INTERFACE_GPIO_COMPONENT_HPP_ #define S2E_COMPONENTS_BASE_CLASSES_INTERFACE_GPIO_COMPONENT_HPP_ +namespace s2e::components { + /** * @class IGPIOCompo * @brief Interface class for components which have GPIO port @@ -27,4 +29,6 @@ class IGPIOCompo { virtual void GpioStateChanged(const int port_id, const bool is_positive_edge) = 0; }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_BASE_CLASSES_INTERFACE_GPIO_COMPONENT_HPP_ diff --git a/src/components/base/interface_tickable.hpp b/src/components/base/interface_tickable.hpp index 3d7171aeb..401c5d87b 100644 --- a/src/components/base/interface_tickable.hpp +++ b/src/components/base/interface_tickable.hpp @@ -6,6 +6,8 @@ #ifndef S2E_COMPONENTS_BASE_CLASSES_INTERFACE_TICKABLE_HPP_ #define S2E_COMPONENTS_BASE_CLASSES_INTERFACE_TICKABLE_HPP_ +namespace s2e::components { + /** * @class ITickable * @brief Interface class for time update of components @@ -40,4 +42,6 @@ class ITickable { bool needs_fast_update_ = false; //!< Whether or not high-frequency disturbances need to be calculated }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_BASE_CLASSES_INTERFACE_TICKABLE_HPP_ diff --git a/src/components/base/sensor.hpp b/src/components/base/sensor.hpp index 43fabd0cf..38f2a9075 100644 --- a/src/components/base/sensor.hpp +++ b/src/components/base/sensor.hpp @@ -11,6 +11,8 @@ #include #include +namespace s2e::components { + /** * @class Sensor * @brief Base class for sensor emulation to add noises @@ -85,6 +87,8 @@ template Sensor ReadSensorInformation(const std::string file_name, const double step_width_s, const std::string component_name, const std::string unit = ""); +} // namespace s2e::components + #include "./sensor_template_functions.hpp" #endif // S2E_COMPONENTS_BASE_SENSOR_HPP_ diff --git a/src/components/base/sensor_template_functions.hpp b/src/components/base/sensor_template_functions.hpp index b0e4f0300..31c761393 100644 --- a/src/components/base/sensor_template_functions.hpp +++ b/src/components/base/sensor_template_functions.hpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::components { + template Sensor::Sensor(const s2e::math::Matrix& scale_factor, const s2e::math::Vector& range_to_const_c, const s2e::math::Vector& range_to_zero_c, const s2e::math::Vector& bias_noise_c, const s2e::math::Vector& normal_random_standard_deviation_c, @@ -121,4 +123,6 @@ Sensor ReadSensorInformation(const std::string file_name, const double step_w return sensor_base; } +} // namespace s2e::components + #endif // S2E_COMPONENTS_BASE_SENSOR_TEMPLATE_FUNCTIONS_HPP_ diff --git a/src/components/base/uart_communication_with_obc.cpp b/src/components/base/uart_communication_with_obc.cpp index 47df421d4..41bb777fd 100644 --- a/src/components/base/uart_communication_with_obc.cpp +++ b/src/components/base/uart_communication_with_obc.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::components { + UartCommunicationWithObc::UartCommunicationWithObc(const unsigned int sils_port_id, OnBoardComputer* obc) : sils_port_id_(sils_port_id), obc_(obc) { #ifdef USE_HILS simulation_mode_ = SimulationMode::kError; @@ -173,3 +175,5 @@ int UartCommunicationWithObc::SendTelemetry(const unsigned int offset) { return -1; } } + +} // namespace s2e::components diff --git a/src/components/base/uart_communication_with_obc.hpp b/src/components/base/uart_communication_with_obc.hpp index 0db74815b..6cb45c5df 100644 --- a/src/components/base/uart_communication_with_obc.hpp +++ b/src/components/base/uart_communication_with_obc.hpp @@ -10,6 +10,8 @@ #include "../real/cdh/on_board_computer.hpp" +namespace s2e::components { + /** * @enum SimulationMode * @brief Simulation mode (SILS or HILS) @@ -129,4 +131,6 @@ class UartCommunicationWithObc { virtual int GenerateTelemetry() = 0; }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_BASE_UART_COMMUNICATION_WITH_OBC_HPP_ From 684d0d602592daa5accd22f07d963ccd8f75379d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 13:56:55 +0900 Subject: [PATCH 15/49] Add s2e::components namespace for examples --- src/components/examples/example_change_structure.cpp | 4 ++++ src/components/examples/example_change_structure.hpp | 4 ++++ src/components/examples/example_i2c_controller_for_hils.cpp | 4 ++++ src/components/examples/example_i2c_controller_for_hils.hpp | 4 ++++ src/components/examples/example_i2c_target_for_hils.cpp | 4 ++++ src/components/examples/example_i2c_target_for_hils.hpp | 4 ++++ .../examples/example_serial_communication_for_hils.cpp | 4 ++++ .../examples/example_serial_communication_for_hils.hpp | 4 ++++ .../examples/example_serial_communication_with_obc.cpp | 4 ++++ .../examples/example_serial_communication_with_obc.hpp | 4 ++++ 10 files changed, 40 insertions(+) diff --git a/src/components/examples/example_change_structure.cpp b/src/components/examples/example_change_structure.cpp index bc28f4547..9f1d6d051 100644 --- a/src/components/examples/example_change_structure.cpp +++ b/src/components/examples/example_change_structure.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::components { + ExampleChangeStructure::ExampleChangeStructure(ClockGenerator* clock_generator, Structure* structure) : Component(1, clock_generator), structure_(structure) {} @@ -54,3 +56,5 @@ std::string ExampleChangeStructure::GetLogValue() const { return str_tmp; } + +} // namespace s2e::components diff --git a/src/components/examples/example_change_structure.hpp b/src/components/examples/example_change_structure.hpp index 31d656c92..93d749b61 100644 --- a/src/components/examples/example_change_structure.hpp +++ b/src/components/examples/example_change_structure.hpp @@ -11,6 +11,8 @@ #include "../base/component.hpp" +namespace s2e::components { + /** * @class ExampleChangeStructure * @brief Class to show an example to change satellite structure information @@ -53,4 +55,6 @@ class ExampleChangeStructure : public Component, public ILoggable { Structure* structure_; //!< Structure information }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_EXAMPLES_EXAMPLE_CHANGE_STRUCTURE_HPP_ diff --git a/src/components/examples/example_i2c_controller_for_hils.cpp b/src/components/examples/example_i2c_controller_for_hils.cpp index 1924ec1c8..b4f2bace1 100644 --- a/src/components/examples/example_i2c_controller_for_hils.cpp +++ b/src/components/examples/example_i2c_controller_for_hils.cpp @@ -4,6 +4,8 @@ */ #include "example_i2c_controller_for_hils.hpp" +namespace s2e::components { + ExampleI2cControllerForHils::ExampleI2cControllerForHils(const int prescaler, ClockGenerator* clock_generator, const unsigned int hils_port_id, const unsigned int baud_rate, const unsigned int tx_buffer_size, const unsigned int rx_buffer_size, HilsPortManager* hils_port_manager) @@ -54,3 +56,5 @@ void ExampleI2cControllerForHils::Receive() { std::cout << std::endl; return; } + +} // namespace s2e::components diff --git a/src/components/examples/example_i2c_controller_for_hils.hpp b/src/components/examples/example_i2c_controller_for_hils.hpp index 51b948708..1e5796c39 100644 --- a/src/components/examples/example_i2c_controller_for_hils.hpp +++ b/src/components/examples/example_i2c_controller_for_hils.hpp @@ -11,6 +11,8 @@ #include "../base/component.hpp" #include "../base/i2c_controller.hpp" +namespace s2e::components { + /** * @class ExampleI2cControllerForHils * @brief Example of component emulation for I2C controller side communication in HILS environment @@ -65,4 +67,6 @@ class ExampleI2cControllerForHils : public Component, public I2cController { static const uint8_t kCmdFooter_ = 0x50; //!< 'P' Footer for SC18IM700 }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_EXAMPLES_EXAMPLE_I2C_CONTROLLER_FOR_HILS_HPP_ diff --git a/src/components/examples/example_i2c_target_for_hils.cpp b/src/components/examples/example_i2c_target_for_hils.cpp index 3ec63ed3e..0fc2a2380 100644 --- a/src/components/examples/example_i2c_target_for_hils.cpp +++ b/src/components/examples/example_i2c_target_for_hils.cpp @@ -5,6 +5,8 @@ #include "example_i2c_target_for_hils.hpp" +namespace s2e::components { + ExampleI2cTargetForHils::ExampleI2cTargetForHils(const int prescaler, ClockGenerator* clock_generator, const int sils_port_id, unsigned char i2c_address, OnBoardComputer* obc, const unsigned int hils_port_id, HilsPortManager* hils_port_manager) @@ -39,3 +41,5 @@ void ExampleI2cTargetForHils::MainRoutine(const int time_count) { return; } + +} // namespace s2e::components diff --git a/src/components/examples/example_i2c_target_for_hils.hpp b/src/components/examples/example_i2c_target_for_hils.hpp index 9c486995f..ca03d3690 100644 --- a/src/components/examples/example_i2c_target_for_hils.hpp +++ b/src/components/examples/example_i2c_target_for_hils.hpp @@ -11,6 +11,8 @@ #include "../base/component.hpp" #include "../base/i2c_target_communication_with_obc.hpp" +namespace s2e::components { + /** * @class ExampleI2cTargetForHils * @brief Example of component emulation for I2C target side communication in HILS environment @@ -54,4 +56,6 @@ class ExampleI2cTargetForHils : public Component, public I2cTargetCommunicationW const unsigned char kNumAlphabet = 26; //!< Number of alphabet }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_EXAMPLES_EXAMPLE_I2C_TARGET_FOR_HILS_HPP_ diff --git a/src/components/examples/example_serial_communication_for_hils.cpp b/src/components/examples/example_serial_communication_for_hils.cpp index fe0b9b3d8..d4890360d 100644 --- a/src/components/examples/example_serial_communication_for_hils.cpp +++ b/src/components/examples/example_serial_communication_for_hils.cpp @@ -6,6 +6,8 @@ #include +namespace s2e::components { + ExampleSerialCommunicationForHils::ExampleSerialCommunicationForHils(ClockGenerator* clock_generator, const int sils_port_id, OnBoardComputer* obc, const unsigned int hils_port_id, const unsigned int baud_rate, HilsPortManager* hils_port_manager, const int mode_id) @@ -59,3 +61,5 @@ void ExampleSerialCommunicationForHils::MainRoutine(const int time_count) { ReceiveCommand(0, kMemorySize); SendTelemetry(0); } + +} // namespace s2e::components diff --git a/src/components/examples/example_serial_communication_for_hils.hpp b/src/components/examples/example_serial_communication_for_hils.hpp index 682d4afe0..2393fa2c5 100644 --- a/src/components/examples/example_serial_communication_for_hils.hpp +++ b/src/components/examples/example_serial_communication_for_hils.hpp @@ -11,6 +11,8 @@ #include "../base/component.hpp" #include "../base/uart_communication_with_obc.hpp" +namespace s2e::components { + /** * @class ExampleSerialCommunicationForHils * @brief Example of component emulation for communication in HILS environment @@ -71,4 +73,6 @@ class ExampleSerialCommunicationForHils : public Component, public UartCommunica int GenerateTelemetry() override; }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_EXAMPLES_EXAMPLE_SERIAL_COMMUNICATION_FOR_HILS_HPP_ diff --git a/src/components/examples/example_serial_communication_with_obc.cpp b/src/components/examples/example_serial_communication_with_obc.cpp index 4042fbcb1..b44138520 100644 --- a/src/components/examples/example_serial_communication_with_obc.cpp +++ b/src/components/examples/example_serial_communication_with_obc.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::components { + ExampleSerialCommunicationWithObc::ExampleSerialCommunicationWithObc(ClockGenerator* clock_generator, int port_id, OnBoardComputer* obc) : Component(1000, clock_generator), UartCommunicationWithObc(port_id, obc) { Initialize(); @@ -54,3 +56,5 @@ void ExampleSerialCommunicationWithObc::MainRoutine(const int time_count) { void ExampleSerialCommunicationWithObc::GpioStateChanged(const int port_id, const bool is_positive_edge) { printf("interrupted. port id = %d, is positive edge = %d./n", port_id, is_positive_edge); } + +} // namespace s2e::components diff --git a/src/components/examples/example_serial_communication_with_obc.hpp b/src/components/examples/example_serial_communication_with_obc.hpp index a83eb4bcf..9a61c1ed0 100644 --- a/src/components/examples/example_serial_communication_with_obc.hpp +++ b/src/components/examples/example_serial_communication_with_obc.hpp @@ -12,6 +12,8 @@ #include "../base/interface_gpio_component.hpp" #include "../base/uart_communication_with_obc.hpp" +namespace s2e::components { + /** * @class ExampleSerialCommunicationWithObc * @brief Example of component emulation with communication between OBC flight software @@ -89,4 +91,6 @@ class ExampleSerialCommunicationWithObc : public Component, public UartCommunica int Initialize(); }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_EXAMPLES_EXAMPLE_SERIAL_COMMUNICATION_WITH_OBC_HPP_P_ From 085a95f9666efe6321bb4c7b0312b5e50c43d537 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 13:58:37 +0900 Subject: [PATCH 16/49] Add s2e::components namespace for ideal --- src/components/ideal/angular_velocity_observer.cpp | 4 ++++ src/components/ideal/angular_velocity_observer.hpp | 4 ++++ src/components/ideal/attitude_observer.cpp | 4 ++++ src/components/ideal/attitude_observer.hpp | 4 ++++ src/components/ideal/force_generator.cpp | 4 ++++ src/components/ideal/force_generator.hpp | 4 ++++ src/components/ideal/orbit_observer.cpp | 4 ++++ src/components/ideal/orbit_observer.hpp | 4 ++++ src/components/ideal/torque_generator.cpp | 4 ++++ src/components/ideal/torque_generator.hpp | 4 ++++ 10 files changed, 40 insertions(+) diff --git a/src/components/ideal/angular_velocity_observer.cpp b/src/components/ideal/angular_velocity_observer.cpp index 6ff33272f..d6a2bbcf3 100644 --- a/src/components/ideal/angular_velocity_observer.cpp +++ b/src/components/ideal/angular_velocity_observer.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::components { + AngularVelocityObserver::AngularVelocityObserver(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const Attitude& attitude) : Component(prescaler, clock_generator), Sensor(sensor_base), attitude_(attitude) {} @@ -46,3 +48,5 @@ AngularVelocityObserver InitializeAngularVelocityObserver(ClockGenerator* clock_ return observer; } + +} // namespace s2e::components diff --git a/src/components/ideal/angular_velocity_observer.hpp b/src/components/ideal/angular_velocity_observer.hpp index bf22777ac..47bb4a404 100644 --- a/src/components/ideal/angular_velocity_observer.hpp +++ b/src/components/ideal/angular_velocity_observer.hpp @@ -12,6 +12,8 @@ #include "../base/component.hpp" #include "../base/sensor.hpp" +namespace s2e::components { + /* * @class AngularVelocityObserver * @brief Ideal component which can observe angular velocity @@ -75,4 +77,6 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public ILogg AngularVelocityObserver InitializeAngularVelocityObserver(ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s, const Attitude& attitude); +} // namespace s2e::components + #endif // S2E_COMPONENTS_IDEAL_ANGULAR_VELOCITY_OBSERVER_HPP_ diff --git a/src/components/ideal/attitude_observer.cpp b/src/components/ideal/attitude_observer.cpp index 26ffee05b..533a636c8 100644 --- a/src/components/ideal/attitude_observer.cpp +++ b/src/components/ideal/attitude_observer.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::components { + AttitudeObserver::AttitudeObserver(const int prescaler, ClockGenerator* clock_generator, const double standard_deviation_rad, const Attitude& attitude) : Component(prescaler, clock_generator), angle_noise_(0.0, standard_deviation_rad), attitude_(attitude) { @@ -62,3 +64,5 @@ AttitudeObserver InitializeAttitudeObserver(ClockGenerator* clock_generator, con return attitude_observer; } + +} // namespace s2e::components diff --git a/src/components/ideal/attitude_observer.hpp b/src/components/ideal/attitude_observer.hpp index 1b80659d0..0bf1e973f 100644 --- a/src/components/ideal/attitude_observer.hpp +++ b/src/components/ideal/attitude_observer.hpp @@ -13,6 +13,8 @@ #include "../base/component.hpp" +namespace s2e::components { + /* * @class AttitudeObserver * @brief Ideal component which can observe attitude @@ -77,4 +79,6 @@ class AttitudeObserver : public Component, public ILoggable { */ AttitudeObserver InitializeAttitudeObserver(ClockGenerator* clock_generator, const std::string file_name, const Attitude& attitude); +} // namespace s2e::components + #endif // S2E_COMPONENTS_IDEAL_ATTITUDE_OBSERVER_HPP_ diff --git a/src/components/ideal/force_generator.cpp b/src/components/ideal/force_generator.cpp index 79a062ede..59e788ba3 100644 --- a/src/components/ideal/force_generator.cpp +++ b/src/components/ideal/force_generator.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::components { + // Constructor ForceGenerator::ForceGenerator(const int prescaler, ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_N, const double direction_error_standard_deviation_rad, const Dynamics* dynamics) @@ -122,3 +124,5 @@ ForceGenerator InitializeForceGenerator(ClockGenerator* clock_generator, const s return force_generator; } + +} // namespace s2e::components diff --git a/src/components/ideal/force_generator.hpp b/src/components/ideal/force_generator.hpp index 607e0a6fd..46724d9f4 100644 --- a/src/components/ideal/force_generator.hpp +++ b/src/components/ideal/force_generator.hpp @@ -12,6 +12,8 @@ #include #include +namespace s2e::components { + /* * @class ForceGenerator * @brief Ideal component which can generate for control algorithm test @@ -124,4 +126,6 @@ class ForceGenerator : public Component, public ILoggable { */ ForceGenerator InitializeForceGenerator(ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics); +} // namespace s2e::components + #endif // S2E_COMPONENTS_IDEAL_FORCE_GENERATOR_HPP_ diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index f724d0ce9..881610793 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::components { + OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, const s2e::math::Vector<6> error_standard_deviation, const Orbit& orbit) : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) { @@ -100,3 +102,5 @@ OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std return orbit_observer; } + +} // namespace s2e::components diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 6c1119986..36ef2a447 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -13,6 +13,8 @@ #include "../base/component.hpp" +namespace s2e::components { + /** * @enum NoiseFrame * @brief Noise definition frame @@ -105,4 +107,6 @@ NoiseFrame SetNoiseFrame(const std::string noise_frame); */ OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit); +} // namespace s2e::components + #endif // S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ diff --git a/src/components/ideal/torque_generator.cpp b/src/components/ideal/torque_generator.cpp index efcf0392d..600f65e5a 100644 --- a/src/components/ideal/torque_generator.cpp +++ b/src/components/ideal/torque_generator.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::components { + // Constructor TorqueGenerator::TorqueGenerator(const int prescaler, ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_Nm, const double direction_error_standard_deviation_rad, const Dynamics* dynamics) @@ -96,3 +98,5 @@ TorqueGenerator InitializeTorqueGenerator(ClockGenerator* clock_generator, const return torque_generator; } + +} // namespace s2e::components diff --git a/src/components/ideal/torque_generator.hpp b/src/components/ideal/torque_generator.hpp index 3b8606464..51cbe39ee 100644 --- a/src/components/ideal/torque_generator.hpp +++ b/src/components/ideal/torque_generator.hpp @@ -12,6 +12,8 @@ #include #include +namespace s2e::components { + /* * @class TorqueGenerator * @brief Ideal component which can generate for control algorithm test @@ -102,4 +104,6 @@ class TorqueGenerator : public Component, public ILoggable { */ TorqueGenerator InitializeTorqueGenerator(ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics); +} // namespace s2e::components + #endif // S2E_COMPONENTS_IDEAL_TORQUE_GENERATOR_HPP_ From 61aa36f0df027adcf2176056dd684358558ff25e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:02:00 +0900 Subject: [PATCH 17/49] Add s2e::components namespace for ports --- src/components/ports/gpio_port.cpp | 4 ++++ src/components/ports/gpio_port.hpp | 4 ++++ src/components/ports/hils_i2c_target_port.cpp | 4 ++++ src/components/ports/hils_i2c_target_port.hpp | 4 ++++ src/components/ports/hils_uart_port.cpp | 4 ++++ src/components/ports/hils_uart_port.hpp | 4 ++++ src/components/ports/i2c_port.cpp | 4 ++++ src/components/ports/i2c_port.hpp | 4 ++++ src/components/ports/power_port.cpp | 4 ++++ src/components/ports/power_port.hpp | 4 ++++ src/components/ports/uart_port.cpp | 4 ++++ src/components/ports/uart_port.hpp | 4 ++++ 12 files changed, 48 insertions(+) diff --git a/src/components/ports/gpio_port.cpp b/src/components/ports/gpio_port.cpp index 4a155cc57..bf91c2f8c 100644 --- a/src/components/ports/gpio_port.cpp +++ b/src/components/ports/gpio_port.cpp @@ -5,6 +5,8 @@ #include "gpio_port.hpp" +namespace s2e::components { + GpioPort::GpioPort(const unsigned int port_id, IGPIOCompo* component) : kPortId(port_id) { high_low_state_ = GPIO_LOW; component_ = component; @@ -24,3 +26,5 @@ int GpioPort::DigitalWrite(const bool is_high) { } bool GpioPort::DigitalRead() { return high_low_state_; } + +} // namespace s2e::components diff --git a/src/components/ports/gpio_port.hpp b/src/components/ports/gpio_port.hpp index b1c38bb2a..3feb8af86 100644 --- a/src/components/ports/gpio_port.hpp +++ b/src/components/ports/gpio_port.hpp @@ -8,6 +8,8 @@ #include +namespace s2e::components { + #define GPIO_HIGH true #define GPIO_LOW false @@ -51,4 +53,6 @@ class GpioPort { bool high_low_state_; //!< GPIO High/Low state }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_PORTS_GPIO_PORT_HPP_ diff --git a/src/components/ports/hils_i2c_target_port.cpp b/src/components/ports/hils_i2c_target_port.cpp index c4797e31e..6d4cd71cb 100644 --- a/src/components/ports/hils_i2c_target_port.cpp +++ b/src/components/ports/hils_i2c_target_port.cpp @@ -5,6 +5,8 @@ #include "hils_i2c_target_port.hpp" +namespace s2e::components { + // #define HILS_I2C_TARGET_PORT_SHOW_DEBUG_DATA //!< Remove comment when you want to show the debug message // FIXME: The magic number. This is depending on the converter. @@ -105,3 +107,5 @@ int HilsI2cTargetPort::Send(const unsigned char data_length) // to I2C-USB Targ } int HilsI2cTargetPort::GetStoredFrameCounter() { return stored_frame_counter_; } + +} // namespace s2e::components diff --git a/src/components/ports/hils_i2c_target_port.hpp b/src/components/ports/hils_i2c_target_port.hpp index 16bee3cd4..54cb76531 100644 --- a/src/components/ports/hils_i2c_target_port.hpp +++ b/src/components/ports/hils_i2c_target_port.hpp @@ -10,6 +10,8 @@ #include "hils_uart_port.hpp" +namespace s2e::components { + const unsigned int kDefaultCommandSize = 0xff; //!< Default command size const unsigned int kDefaultTxSize = 0xff; //!< Default TX size @@ -107,4 +109,6 @@ class HilsI2cTargetPort : public HilsUartPort { std::map command_buffer_; }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_PORTS_HILS_I2C_TARGET_PORT_HPP_ diff --git a/src/components/ports/hils_uart_port.cpp b/src/components/ports/hils_uart_port.cpp index be3d1e16c..bc811269b 100644 --- a/src/components/ports/hils_uart_port.cpp +++ b/src/components/ports/hils_uart_port.cpp @@ -8,6 +8,8 @@ #include "hils_uart_port.hpp" +namespace s2e::components { + // # define HILS_UART_PORT_SHOW_DEBUG_DATA HilsUartPort::HilsUartPort(const unsigned int port_id, const unsigned int baud_rate, const unsigned int tx_buffer_size, @@ -186,3 +188,5 @@ int HilsUartPort::DiscardOutBuffer() { } return 0; } + +} // namespace s2e::components diff --git a/src/components/ports/hils_uart_port.hpp b/src/components/ports/hils_uart_port.hpp index 1be6a9b8f..be2fc9bbd 100644 --- a/src/components/ports/hils_uart_port.hpp +++ b/src/components/ports/hils_uart_port.hpp @@ -14,6 +14,8 @@ #include +namespace s2e::components { + typedef cli::array bytearray; //!< System::Byte: an 8-bit unsigned integer /** @@ -111,4 +113,6 @@ class HilsUartPort { int DiscardOutBuffer(); }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_PORTS_HILS_UART_PORT_HPP_ diff --git a/src/components/ports/i2c_port.cpp b/src/components/ports/i2c_port.cpp index 715fd9538..8b4e1cfa1 100644 --- a/src/components/ports/i2c_port.cpp +++ b/src/components/ports/i2c_port.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::components { + I2cPort::I2cPort(void) {} I2cPort::I2cPort(const unsigned char max_register_number) : max_register_number_(max_register_number) {} @@ -93,3 +95,5 @@ unsigned char I2cPort::ReadCommand(const unsigned char i2c_address, unsigned cha } return length; } + +} // namespace s2e::components \ No newline at end of file diff --git a/src/components/ports/i2c_port.hpp b/src/components/ports/i2c_port.hpp index 57143b992..044431c0e 100644 --- a/src/components/ports/i2c_port.hpp +++ b/src/components/ports/i2c_port.hpp @@ -8,6 +8,8 @@ #include +namespace s2e::components { + /** * @class I2cPort * @brief Class to emulate I2C(Inter-Integrated Circuit) communication port @@ -108,4 +110,6 @@ class I2cPort { std::map, unsigned char> command_buffer_; }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_PORTS_I2C_PORT_HPP_ diff --git a/src/components/ports/power_port.cpp b/src/components/ports/power_port.cpp index 1a3393ec3..ac763115e 100644 --- a/src/components/ports/power_port.cpp +++ b/src/components/ports/power_port.cpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::components { + PowerPort::PowerPort() : kPortId(-1), current_limit_A_(10.0), minimum_voltage_V_(3.3), assumed_power_consumption_W_(0.0) { is_on_ = true; // power on to work the component Initialize(); @@ -74,3 +76,5 @@ void PowerPort::InitializeWithInitializeFile(const std::string file_name) { double assumed_power_consumption_W = initialize_file.ReadDouble(section_name.c_str(), "assumed_power_consumption_W"); this->SetAssumedPowerConsumption_W(assumed_power_consumption_W); } + +} // namespace s2e::components diff --git a/src/components/ports/power_port.hpp b/src/components/ports/power_port.hpp index e890d25b5..98d0a74d6 100644 --- a/src/components/ports/power_port.hpp +++ b/src/components/ports/power_port.hpp @@ -8,6 +8,8 @@ #include +namespace s2e::components { + /** * @class PowerPort * @brief Class to emulate electrical power port @@ -133,4 +135,6 @@ class PowerPort { void Initialize(void); }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_PORTS_POWER_PORT_HPP_ diff --git a/src/components/ports/uart_port.cpp b/src/components/ports/uart_port.cpp index a2ad5303a..a88f98699 100644 --- a/src/components/ports/uart_port.cpp +++ b/src/components/ports/uart_port.cpp @@ -5,6 +5,8 @@ #include "uart_port.hpp" +namespace s2e::components { + UartPort::UartPort() : UartPort(kDefaultBufferSize, kDefaultBufferSize) {} UartPort::UartPort(const unsigned int rx_buffer_size, const unsigned int tx_buffer_size) { @@ -36,3 +38,5 @@ int UartPort::ReadTx(unsigned char* buffer, const unsigned int offset, const uns int UartPort::ReadRx(unsigned char* buffer, const unsigned int offset, const unsigned int data_length) { return rx_buffer_->Read(buffer, offset, data_length); } + +} // namespace s2e::components diff --git a/src/components/ports/uart_port.hpp b/src/components/ports/uart_port.hpp index b50f90c28..eb5e6683c 100644 --- a/src/components/ports/uart_port.hpp +++ b/src/components/ports/uart_port.hpp @@ -8,6 +8,8 @@ #include +namespace s2e::components { + /** * @class UartPort * @brief Class to emulate UART communication port @@ -78,4 +80,6 @@ class UartPort { RingBuffer* tx_buffer_; //!< Transmit buffer (OBC-> Component) }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_PORTS_UART_PORT_HPP_ From 50aa0554eee2482e997041aff87a73f943c8204d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:16:16 +0900 Subject: [PATCH 18/49] Add s2e::components namespace for real --- src/components/real/aocs/gnss_receiver.cpp | 4 ++++ src/components/real/aocs/gnss_receiver.hpp | 4 ++++ src/components/real/aocs/gyro_sensor.cpp | 4 ++++ src/components/real/aocs/gyro_sensor.hpp | 4 ++++ src/components/real/aocs/magnetometer.cpp | 4 ++++ src/components/real/aocs/magnetometer.hpp | 4 ++++ src/components/real/aocs/magnetorquer.cpp | 4 ++++ src/components/real/aocs/magnetorquer.hpp | 4 ++++ src/components/real/aocs/mtq_magnetometer_interference.cpp | 4 ++++ src/components/real/aocs/mtq_magnetometer_interference.hpp | 4 ++++ src/components/real/aocs/reaction_wheel.cpp | 4 ++++ src/components/real/aocs/reaction_wheel.hpp | 4 ++++ src/components/real/aocs/reaction_wheel_jitter.cpp | 4 ++++ src/components/real/aocs/reaction_wheel_jitter.hpp | 4 ++++ src/components/real/aocs/reaction_wheel_ode.cpp | 4 ++++ src/components/real/aocs/reaction_wheel_ode.hpp | 4 ++++ src/components/real/aocs/star_sensor.cpp | 4 ++++ src/components/real/aocs/star_sensor.hpp | 4 ++++ src/components/real/aocs/sun_sensor.cpp | 4 ++++ src/components/real/aocs/sun_sensor.hpp | 4 ++++ src/components/real/cdh/c2a_communication.hpp | 4 ++++ src/components/real/cdh/on_board_computer.cpp | 4 ++++ src/components/real/cdh/on_board_computer.hpp | 4 ++++ src/components/real/cdh/on_board_computer_with_c2a.cpp | 6 +++++- src/components/real/cdh/on_board_computer_with_c2a.hpp | 4 ++++ src/components/real/communication/antenna.cpp | 4 ++++ src/components/real/communication/antenna.hpp | 4 ++++ .../real/communication/antenna_radiation_pattern.cpp | 4 ++++ .../real/communication/antenna_radiation_pattern.hpp | 4 ++++ .../real/communication/ground_station_calculator.cpp | 4 ++++ .../real/communication/ground_station_calculator.hpp | 4 ++++ .../real/communication/wings_command_sender_to_c2a.cpp | 4 ++++ .../real/communication/wings_command_sender_to_c2a.hpp | 4 ++++ src/components/real/mission/telescope.cpp | 4 ++++ src/components/real/mission/telescope.hpp | 4 ++++ src/components/real/power/battery.cpp | 4 ++++ src/components/real/power/battery.hpp | 4 ++++ src/components/real/power/csv_scenario_interface.cpp | 4 ++++ src/components/real/power/csv_scenario_interface.hpp | 4 ++++ src/components/real/power/pcu_initial_study.cpp | 4 ++++ src/components/real/power/pcu_initial_study.hpp | 4 ++++ src/components/real/power/power_control_unit.cpp | 4 ++++ src/components/real/power/power_control_unit.hpp | 4 ++++ src/components/real/power/solar_array_panel.cpp | 4 ++++ src/components/real/power/solar_array_panel.hpp | 4 ++++ src/components/real/propulsion/simple_thruster.cpp | 4 ++++ src/components/real/propulsion/simple_thruster.hpp | 4 ++++ 47 files changed, 189 insertions(+), 1 deletion(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index f5c28e8cd..0d899b9e8 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -11,6 +11,8 @@ #include #include +namespace s2e::components { + GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, @@ -311,3 +313,5 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_ gr_param.velocity_noise_standard_deviation_ecef_m_s, dynamics, gnss_satellites, simulation_time); return gnss_r; } + +} // namespace s2e::components diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 14a4dbc86..1ea260d0f 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -16,6 +16,8 @@ #include "../../base/component.hpp" +namespace s2e::components { + /** * @enum AntennaModel * @brief Antenna pattern model to emulate GNSS antenna @@ -240,4 +242,6 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t comp GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_AOCS_GNSS_RECEIVER_HPP_ diff --git a/src/components/real/aocs/gyro_sensor.cpp b/src/components/real/aocs/gyro_sensor.cpp index 244127fb1..7af914f24 100644 --- a/src/components/real/aocs/gyro_sensor.cpp +++ b/src/components/real/aocs/gyro_sensor.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::components { + GyroSensor::GyroSensor(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics) : Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), dynamics_(dynamics) {} @@ -88,3 +90,5 @@ GyroSensor InitGyroSensor(ClockGenerator* clock_generator, PowerPort* power_port GyroSensor gyro(prescaler, clock_generator, power_port, sensor_base, sensor_id, quaternion_b2c, dynamics); return gyro; } + +} // namespace s2e::components diff --git a/src/components/real/aocs/gyro_sensor.hpp b/src/components/real/aocs/gyro_sensor.hpp index cd0ec6d34..fd6ede443 100644 --- a/src/components/real/aocs/gyro_sensor.hpp +++ b/src/components/real/aocs/gyro_sensor.hpp @@ -13,6 +13,8 @@ #include "../../base/component.hpp" #include "../../base/sensor.hpp" +namespace s2e::components { + const size_t kGyroDimension = 3; //!< Dimension of gyro sensor /** @@ -110,4 +112,6 @@ GyroSensor InitGyroSensor(ClockGenerator* clock_generator, int sensor_id, const GyroSensor InitGyroSensor(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const Dynamics* dynamics); +} //namespace s2e::components + #endif // S2E_COMPONENTS_REAL_AOCS_GYRO_SENSOR_HPP_ diff --git a/src/components/real/aocs/magnetometer.cpp b/src/components/real/aocs/magnetometer.cpp index 91236b2e4..1326a7ba4 100644 --- a/src/components/real/aocs/magnetometer.cpp +++ b/src/components/real/aocs/magnetometer.cpp @@ -7,6 +7,8 @@ #include #include +namespace s2e::components { + Magnetometer::Magnetometer(int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator), @@ -91,3 +93,5 @@ Magnetometer InitMagnetometer(ClockGenerator* clock_generator, PowerPort* power_ Magnetometer magsensor(prescaler, clock_generator, power_port, sensor_base, sensor_id, quaternion_b2c, geomagnetic_field); return magsensor; } + +} // namespace s2e::components diff --git a/src/components/real/aocs/magnetometer.hpp b/src/components/real/aocs/magnetometer.hpp index d4d7502e6..34dce8647 100644 --- a/src/components/real/aocs/magnetometer.hpp +++ b/src/components/real/aocs/magnetometer.hpp @@ -13,6 +13,8 @@ #include "../../base/component.hpp" #include "../../base/sensor.hpp" +namespace s2e::components { + const size_t kMagnetometerDimension = 3; //!< Dimension of magnetometer /** @@ -129,4 +131,6 @@ Magnetometer InitMagnetometer(ClockGenerator* clock_generator, int sensor_id, co Magnetometer InitMagnetometer(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_AOCS_MAGNETOMETER_HPP_ diff --git a/src/components/real/aocs/magnetorquer.cpp b/src/components/real/aocs/magnetorquer.cpp index 909996b84..a213539a7 100644 --- a/src/components/real/aocs/magnetorquer.cpp +++ b/src/components/real/aocs/magnetorquer.cpp @@ -11,6 +11,8 @@ #include #include +namespace s2e::components { + Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const s2e::math::Matrix& scale_factor, const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, @@ -203,3 +205,5 @@ Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, PowerPort* power_ random_walk_limit_c_Am2, normal_random_standard_deviation_c_Am2, geomagnetic_field); return magtorquer; } + +} // namespace s2e::components diff --git a/src/components/real/aocs/magnetorquer.hpp b/src/components/real/aocs/magnetorquer.hpp index 0af9f30e7..fb7ad153e 100644 --- a/src/components/real/aocs/magnetorquer.hpp +++ b/src/components/real/aocs/magnetorquer.hpp @@ -16,6 +16,8 @@ #include "../../base/component.hpp" +namespace s2e::components { + const size_t kMtqDimension = 3; //!< Dimension of magnetorquer /** @@ -170,4 +172,6 @@ Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, int actuator_id, Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_AOCS_MAGNETORQUER_HPP_ diff --git a/src/components/real/aocs/mtq_magnetometer_interference.cpp b/src/components/real/aocs/mtq_magnetometer_interference.cpp index 7b603abbc..b2f23f474 100644 --- a/src/components/real/aocs/mtq_magnetometer_interference.cpp +++ b/src/components/real/aocs/mtq_magnetometer_interference.cpp @@ -7,6 +7,8 @@ #include "setting_file_reader/initialize_file_access.hpp" +namespace s2e::components { + MtqMagnetometerInterference::MtqMagnetometerInterference(const std::string file_name, Magnetometer& magnetometer, const Magnetorquer& magnetorquer, const size_t initialize_id) : magnetometer_(magnetometer), magnetorquer_(magnetorquer) { @@ -49,3 +51,5 @@ void MtqMagnetometerInterference::UpdateInterference(void) { magnetometer_.AddConstantBiasNoise_c_nT(additional_bias_c_nT); previous_added_bias_c_nT_ = additional_bias_c_nT; } + +} // namespace s2e::components diff --git a/src/components/real/aocs/mtq_magnetometer_interference.hpp b/src/components/real/aocs/mtq_magnetometer_interference.hpp index 2c2c4c0d8..715cbce47 100644 --- a/src/components/real/aocs/mtq_magnetometer_interference.hpp +++ b/src/components/real/aocs/mtq_magnetometer_interference.hpp @@ -9,6 +9,8 @@ #include "magnetometer.hpp" #include "magnetorquer.hpp" +namespace s2e::components { + /** * @class MtqMagnetometerInterference * @brief Class for MTQ Magnetometer interference @@ -38,4 +40,6 @@ class MtqMagnetometerInterference { const Magnetorquer& magnetorquer_; //!< Magnetorquer }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_AOCS_MTQ_MAGNETOMETER_INTERFERENCE_HPP_ diff --git a/src/components/real/aocs/reaction_wheel.cpp b/src/components/real/aocs/reaction_wheel.cpp index e0e2a1c13..918ed9578 100644 --- a/src/components/real/aocs/reaction_wheel.cpp +++ b/src/components/real/aocs/reaction_wheel.cpp @@ -11,6 +11,8 @@ #include #include +namespace s2e::components { + ReactionWheel::ReactionWheel(const int prescaler, ClockGenerator* clock_generator, const int component_id, const double step_width_s, const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const s2e::math::Quaternion quaternion_b2c, const s2e::math::Vector<3> position_b_m, const double dead_time_s, @@ -353,3 +355,5 @@ ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, PowerPort* powe return rw; } + +} // namespace s2e::components diff --git a/src/components/real/aocs/reaction_wheel.hpp b/src/components/real/aocs/reaction_wheel.hpp index 14c66a33e..4735f6193 100644 --- a/src/components/real/aocs/reaction_wheel.hpp +++ b/src/components/real/aocs/reaction_wheel.hpp @@ -17,6 +17,8 @@ #include "reaction_wheel_jitter.hpp" #include "reaction_wheel_ode.hpp" +namespace s2e::components { + /* * @class ReactionWheel * @brief Class to emulate Reaction Wheel @@ -246,4 +248,6 @@ ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, int actuator_id ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, std::string file_name, double compo_update_step_s); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_AOCS_REACTION_WHEEL_HPP_ diff --git a/src/components/real/aocs/reaction_wheel_jitter.cpp b/src/components/real/aocs/reaction_wheel_jitter.cpp index 5e4a2930b..d0b1408fe 100644 --- a/src/components/real/aocs/reaction_wheel_jitter.cpp +++ b/src/components/real/aocs/reaction_wheel_jitter.cpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::components { + ReactionWheelJitter::ReactionWheelJitter(std::vector> radial_force_harmonics_coefficients, std::vector> radial_torque_harmonics_coefficients, const double update_interval_s, const s2e::math::Quaternion quaternion_b2c, const double structural_resonance_frequency_Hz, @@ -126,3 +128,5 @@ void ReactionWheelJitter::CalcCoefficients() { coefficients_[5] = 4.0 - 4.0 * damping_factor_ * update_interval_s_ * structural_resonance_angular_frequency_Hz_ + pow(update_interval_s_, 2.0) * pow(structural_resonance_angular_frequency_Hz_, 2.0); } + +} // namespace s2e::components diff --git a/src/components/real/aocs/reaction_wheel_jitter.hpp b/src/components/real/aocs/reaction_wheel_jitter.hpp index a6da8a503..ea2fcec93 100644 --- a/src/components/real/aocs/reaction_wheel_jitter.hpp +++ b/src/components/real/aocs/reaction_wheel_jitter.hpp @@ -11,6 +11,8 @@ #include #include +namespace s2e::components { + /* * @class ReactionWheelJitter * @brief Class to calculate RW high-frequency jitter effect @@ -128,4 +130,6 @@ class ReactionWheelJitter { void CalcCoefficients(); }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_AOCS_REACTION_WHEEL_JITTER_HPP_ diff --git a/src/components/real/aocs/reaction_wheel_ode.cpp b/src/components/real/aocs/reaction_wheel_ode.cpp index 263e24460..d8b34ee86 100644 --- a/src/components/real/aocs/reaction_wheel_ode.cpp +++ b/src/components/real/aocs/reaction_wheel_ode.cpp @@ -6,6 +6,8 @@ #include +namespace s2e::components { + ReactionWheelOde::ReactionWheelOde(const double step_width_s, const double velocity_limit_rad_s, const double initial_angular_velocity_rad_s) : OrdinaryDifferentialEquation<1>(step_width_s), velocity_limit_rad_s_(velocity_limit_rad_s) { this->Setup(0.0, s2e::math::Vector<1>(initial_angular_velocity_rad_s)); @@ -28,3 +30,5 @@ void ReactionWheelOde::DerivativeFunction(double x, const s2e::math::Vector<1> & rhs[0] = angular_acceleration_rad_s2_; } + +} // namespace s2e::components diff --git a/src/components/real/aocs/reaction_wheel_ode.hpp b/src/components/real/aocs/reaction_wheel_ode.hpp index 5553ed389..552c2cc04 100644 --- a/src/components/real/aocs/reaction_wheel_ode.hpp +++ b/src/components/real/aocs/reaction_wheel_ode.hpp @@ -8,6 +8,8 @@ #include +namespace s2e::components { + /* * @file ReactionWheelOde * @brief Ordinary differential equation of angular velocity of reaction wheel with first-order lag @@ -59,4 +61,6 @@ class ReactionWheelOde : public s2e::math::OrdinaryDifferentialEquation<1> { double angular_acceleration_rad_s2_ = 0.0; //!< Angular acceleration [rad/s2] }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_AOCS_REACTION_WHEEL_ODE_HPP_ diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index 193805226..83b105096 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -16,6 +16,8 @@ using namespace std; using namespace s2e::math; +namespace s2e::components { + StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, @@ -281,3 +283,5 @@ StarSensor InitStarSensor(ClockGenerator* clock_generator, PowerPort* power_port moon_forbidden_angle_rad, capture_rate_rad_s, dynamics, local_environment); return stt; } + +} // namespace s2e::components diff --git a/src/components/real/aocs/star_sensor.hpp b/src/components/real/aocs/star_sensor.hpp index 1120f9eeb..888f91b18 100644 --- a/src/components/real/aocs/star_sensor.hpp +++ b/src/components/real/aocs/star_sensor.hpp @@ -18,6 +18,8 @@ #include "../../base/component.hpp" #include "dynamics/dynamics.hpp" +namespace s2e::components { + /* * @class StarSensor * @brief Class to emulate star tracker @@ -232,4 +234,6 @@ StarSensor InitStarSensor(ClockGenerator* clock_generator, int sensor_id, const StarSensor InitStarSensor(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const Dynamics* dynamics, const LocalEnvironment* local_environment); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_AOCS_STAR_SENSOR_HPP_ diff --git a/src/components/real/aocs/sun_sensor.cpp b/src/components/real/aocs/sun_sensor.cpp index 3070355e4..773aba273 100644 --- a/src/components/real/aocs/sun_sensor.cpp +++ b/src/components/real/aocs/sun_sensor.cpp @@ -14,6 +14,8 @@ using s2e::randomization::NormalRand; using namespace std; +namespace s2e::components { + SunSensor::SunSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, @@ -218,3 +220,5 @@ SunSensor InitSunSensor(ClockGenerator* clock_generator, PowerPort* power_port, intensity_lower_threshold_percent, srp_environment, local_celestial_information); return ss; } + +} // namespace s2e::components diff --git a/src/components/real/aocs/sun_sensor.hpp b/src/components/real/aocs/sun_sensor.hpp index 532dfb18f..08c8982d2 100644 --- a/src/components/real/aocs/sun_sensor.hpp +++ b/src/components/real/aocs/sun_sensor.hpp @@ -15,6 +15,8 @@ #include "../../base/component.hpp" +namespace s2e::components { + /* * @class SunSensor * @brief Class to emulate sun sensor @@ -165,4 +167,6 @@ SunSensor InitSunSensor(ClockGenerator* clock_generator, int sensor_id, const st SunSensor InitSunSensor(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_AOCS_SUN_SENSOR_HPP_ diff --git a/src/components/real/cdh/c2a_communication.hpp b/src/components/real/cdh/c2a_communication.hpp index c749d90e3..4517a3355 100644 --- a/src/components/real/cdh/c2a_communication.hpp +++ b/src/components/real/cdh/c2a_communication.hpp @@ -6,6 +6,8 @@ #ifndef C2A_COMMUNICATION_H_ #define C2A_COMMUNICATION_H_ +namespace s2e::components { + // If the character encoding of C2A is UTF-8, the following functions are not necessary, // and users can directory use SendFromObc_C2A and ReceivedByObc_C2A UART // TODO: Delete these functions since C2A is changed to use UTF-8 @@ -23,4 +25,6 @@ int OBC_C2A_I2cReadRegister(int port_id, const unsigned char i2c_address, unsign int OBC_C2A_GpioWrite(int port_id, const bool is_high); bool OBC_C2A_GpioRead(int port_id); // return false when the port_id is not used +} // namespace s2e::components + #endif // C2A_COMMUNICATION_H_ diff --git a/src/components/real/cdh/on_board_computer.cpp b/src/components/real/cdh/on_board_computer.cpp index ad23fadf4..33a75efd9 100644 --- a/src/components/real/cdh/on_board_computer.cpp +++ b/src/components/real/cdh/on_board_computer.cpp @@ -4,6 +4,8 @@ */ #include "on_board_computer.hpp" +namespace s2e::components { + OnBoardComputer::OnBoardComputer(ClockGenerator* clock_generator) : Component(1, clock_generator) { Initialize(); } OnBoardComputer::OnBoardComputer(int prescaler, ClockGenerator* clock_generator, PowerPort* power_port) @@ -132,3 +134,5 @@ bool OnBoardComputer::GpioComponentRead(int port_id) { if (port == nullptr) return false; return port->DigitalRead(); } + +} // namespace s2e::components diff --git a/src/components/real/cdh/on_board_computer.hpp b/src/components/real/cdh/on_board_computer.hpp index 641f1566c..1943de045 100644 --- a/src/components/real/cdh/on_board_computer.hpp +++ b/src/components/real/cdh/on_board_computer.hpp @@ -13,6 +13,8 @@ #include "../../base/component.hpp" +namespace s2e::components { + /* * @class OnBoardComputer * @brief Class to emulate on board computer @@ -207,4 +209,6 @@ class OnBoardComputer : public Component { std::map gpio_ports_; //!< GPIO ports }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_CDH_OBC_HPP_ diff --git a/src/components/real/cdh/on_board_computer_with_c2a.cpp b/src/components/real/cdh/on_board_computer_with_c2a.cpp index 8deb53bec..c0514d03f 100644 --- a/src/components/real/cdh/on_board_computer_with_c2a.cpp +++ b/src/components/real/cdh/on_board_computer_with_c2a.cpp @@ -26,6 +26,8 @@ #endif // c2a-core version header #endif // USE_C2A +namespace s2e::components { + std::map ObcWithC2a::com_ports_c2a_; std::map ObcWithC2a::i2c_com_ports_c2a_; std::map ObcWithC2a::gpio_ports_c2a_; @@ -255,4 +257,6 @@ bool ObcWithC2a::GpioRead_C2A(int port_id) { int OBC_C2A_GpioWrite(int port_id, const bool is_high) { return ObcWithC2a::GpioWrite_C2A(port_id, is_high); } -bool OBC_C2A_GpioRead(int port_id) { return ObcWithC2a::GpioRead_C2A(port_id); } \ No newline at end of file +bool OBC_C2A_GpioRead(int port_id) { return ObcWithC2a::GpioRead_C2A(port_id); } + +} // namespace s2e::components diff --git a/src/components/real/cdh/on_board_computer_with_c2a.hpp b/src/components/real/cdh/on_board_computer_with_c2a.hpp index bc9cc22e8..c7b5fe968 100644 --- a/src/components/real/cdh/on_board_computer_with_c2a.hpp +++ b/src/components/real/cdh/on_board_computer_with_c2a.hpp @@ -11,6 +11,8 @@ #include "c2a_communication.hpp" #include "on_board_computer.hpp" +namespace s2e::components { + /* * @class ObcWithC2a * @brief Class to emulate on board computer with C2A flight software @@ -271,4 +273,6 @@ class ObcWithC2a : public OnBoardComputer { static std::map gpio_ports_c2a_; //!< GPIO ports }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_CDH_OBC_C2A_HPP_ diff --git a/src/components/real/communication/antenna.cpp b/src/components/real/communication/antenna.cpp index db044cb5d..fb22a00b8 100644 --- a/src/components/real/communication/antenna.cpp +++ b/src/components/real/communication/antenna.cpp @@ -11,6 +11,8 @@ #include #include +namespace s2e::components { + Antenna::Antenna(const int component_id, const s2e::math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, const double frequency_MHz, const Vector<5> tx_parameters, const Vector<4> rx_parameters) : component_id_(component_id), is_transmitter_(is_transmitter), is_receiver_(is_receiver), frequency_MHz_(frequency_MHz) { @@ -164,3 +166,5 @@ Antenna InitAntenna(const int antenna_id, const std::string file_name) { rx_system_noise_temperature_K, rx_parameters); return antenna; } + +} // namespace s2e::components diff --git a/src/components/real/communication/antenna.hpp b/src/components/real/communication/antenna.hpp index 78d8f48df..28023cff6 100644 --- a/src/components/real/communication/antenna.hpp +++ b/src/components/real/communication/antenna.hpp @@ -14,6 +14,8 @@ using s2e::math::Vector; #include "./antenna_radiation_pattern.hpp" +namespace s2e::components { + /* * @enum AntennaGainModel * @brief Antenna gain model definition @@ -166,4 +168,6 @@ AntennaGainModel SetAntennaGainModel(const std::string gain_model_name); */ Antenna InitAntenna(const int antenna_id, const std::string file_name); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_COMMUNICATION_ANTENNA_HPP_ diff --git a/src/components/real/communication/antenna_radiation_pattern.cpp b/src/components/real/communication/antenna_radiation_pattern.cpp index a82929c11..71566f080 100644 --- a/src/components/real/communication/antenna_radiation_pattern.cpp +++ b/src/components/real/communication/antenna_radiation_pattern.cpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::components { + AntennaRadiationPattern::AntennaRadiationPattern() { gain_dBi_.assign(length_theta_, std::vector(length_phi_, 0.0)); } AntennaRadiationPattern::AntennaRadiationPattern(const std::string file_path, const size_t length_theta, const size_t length_phi, @@ -37,3 +39,5 @@ double AntennaRadiationPattern::GetGain_dBi(const double theta_rad, const double return gain_dBi_[theta_idx][phi_idx]; } + +} // namespace s2e::components diff --git a/src/components/real/communication/antenna_radiation_pattern.hpp b/src/components/real/communication/antenna_radiation_pattern.hpp index 93aff7df9..3d2382862 100644 --- a/src/components/real/communication/antenna_radiation_pattern.hpp +++ b/src/components/real/communication/antenna_radiation_pattern.hpp @@ -10,6 +10,8 @@ #include #include +namespace s2e::components { + /* * @class AntennaRadiationPattern * @brief Antenna radiation pattern @@ -62,4 +64,6 @@ class AntennaRadiationPattern { std::vector> gain_dBi_; //!< Antenna gain table [dBi] }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_COMMUNICATION_ANTENNA_RADIATION_PATTERN_HPP_ diff --git a/src/components/real/communication/ground_station_calculator.cpp b/src/components/real/communication/ground_station_calculator.cpp index 2f4d1f19a..f53afc30a 100644 --- a/src/components/real/communication/ground_station_calculator.cpp +++ b/src/components/real/communication/ground_station_calculator.cpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::components { + GroundStationCalculator::GroundStationCalculator(const double loss_polarization_dB, const double loss_atmosphere_dB, const double loss_rainfall_dB, const double loss_others_dB, const double ebn0_dB, const double hardware_deterioration_dB, const double coding_gain_dB, const double margin_requirement_dB, const double downlink_bitrate_bps) @@ -136,3 +138,5 @@ GroundStationCalculator InitGsCalculator(const std::string file_name) { hardware_deterioration_dB, coding_gain_dB, margin_requirement_dB, downlink_bitrate_bps); return gs_calculator; } + +} // namespace s2e::components diff --git a/src/components/real/communication/ground_station_calculator.hpp b/src/components/real/communication/ground_station_calculator.hpp index a2d5eb75a..3d20f5a0b 100644 --- a/src/components/real/communication/ground_station_calculator.hpp +++ b/src/components/real/communication/ground_station_calculator.hpp @@ -13,6 +13,8 @@ #include #include +namespace s2e::components { + /* * @class GroundStationCalculator * @brief Emulation of analysis and calculation for Ground Stations @@ -144,4 +146,6 @@ class GroundStationCalculator : public ILoggable { GroundStationCalculator InitGsCalculator(const std::string file_name); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_COMMUNICATION_GROUND_STATION_CALCULATOR_HPP_ diff --git a/src/components/real/communication/wings_command_sender_to_c2a.cpp b/src/components/real/communication/wings_command_sender_to_c2a.cpp index 4d04ee911..f2ce172e0 100644 --- a/src/components/real/communication/wings_command_sender_to_c2a.cpp +++ b/src/components/real/communication/wings_command_sender_to_c2a.cpp @@ -26,6 +26,8 @@ #endif // c2a-core version header #endif // USE_C2A +namespace s2e::components { + void WingsCommandSenderToC2a::MainRoutine(const int time_count) { UNUSED(time_count); if (is_enabled_ == false) return; @@ -148,3 +150,5 @@ WingsCommandSenderToC2a InitWingsCommandSenderToC2a(ClockGenerator* clock_genera return WingsCommandSenderToC2a(prescaler, clock_generator, step_width_s, c2a_command_data_base_file, wings_operation_file, is_enabled); } + +} // namespace s2e::components diff --git a/src/components/real/communication/wings_command_sender_to_c2a.hpp b/src/components/real/communication/wings_command_sender_to_c2a.hpp index 1ae74b1c1..14e1eda9f 100644 --- a/src/components/real/communication/wings_command_sender_to_c2a.hpp +++ b/src/components/real/communication/wings_command_sender_to_c2a.hpp @@ -11,6 +11,8 @@ #include "../../base/component.hpp" +namespace s2e::components { + /* * @class C2aCommandSender * @brief A component to send C2A command using WINGS operation file @@ -76,4 +78,6 @@ class WingsCommandSenderToC2a : public Component { WingsCommandSenderToC2a InitWingsCommandSenderToC2a(ClockGenerator* clock_generator, const double compo_update_step_s, const std::string initialize_file); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_COMMUNICATION_C2A_COMMAND_SENDER_HPP_ diff --git a/src/components/real/mission/telescope.cpp b/src/components/real/mission/telescope.cpp index 0e5a01c91..7d76de5c1 100644 --- a/src/components/real/mission/telescope.cpp +++ b/src/components/real/mission/telescope.cpp @@ -13,6 +13,8 @@ using namespace std; using namespace s2e::math; +namespace s2e::components { + Telescope::Telescope(ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, @@ -285,3 +287,5 @@ Telescope InitTelescope(ClockGenerator* clock_generator, int sensor_id, const st orbit); return telescope; } + +} // namespace s2e::components diff --git a/src/components/real/mission/telescope.hpp b/src/components/real/mission/telescope.hpp index 737a49c98..e2d75c397 100644 --- a/src/components/real/mission/telescope.hpp +++ b/src/components/real/mission/telescope.hpp @@ -17,6 +17,8 @@ #include "../../base/component.hpp" +namespace s2e::components { + /* * @struct Star * @brief Information of stars in the telescope's field of view @@ -170,4 +172,6 @@ Telescope InitTelescope(ClockGenerator* clock_generator, int sensor_id, const st const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_MISSION_TELESCOPE_HPP_P_ diff --git a/src/components/real/power/battery.cpp b/src/components/real/power/battery.cpp index f647fb940..a8f1bc4d0 100644 --- a/src/components/real/power/battery.cpp +++ b/src/components/real/power/battery.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::components { + Battery::Battery(const int prescaler, ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, double cell_capacity_Ah, const std::vector cell_discharge_curve_coefficients, double initial_dod, double cc_charge_c_rate, double cv_charge_voltage_V, double battery_resistance_Ohm, double component_step_time_s) @@ -130,3 +132,5 @@ Battery InitBAT(ClockGenerator* clock_generator, int bat_id, const std::string f return battery; } + +} // namespace s2e::components diff --git a/src/components/real/power/battery.hpp b/src/components/real/power/battery.hpp index 39ee34c44..24f43c464 100644 --- a/src/components/real/power/battery.hpp +++ b/src/components/real/power/battery.hpp @@ -11,6 +11,8 @@ #include "../../base/component.hpp" +namespace s2e::components { + /* * @class Battery * @brief Component emulation of battery @@ -144,4 +146,6 @@ class Battery : public Component, public ILoggable { */ Battery InitBAT(ClockGenerator* clock_generator, int bat_id, const std::string file_name, double component_step_time_s); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_POWER_BATTERY_HPP_P_ diff --git a/src/components/real/power/csv_scenario_interface.cpp b/src/components/real/power/csv_scenario_interface.cpp index 7978d510c..f0efc14fb 100644 --- a/src/components/real/power/csv_scenario_interface.cpp +++ b/src/components/real/power/csv_scenario_interface.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::components { + bool CsvScenarioInterface::is_csv_scenario_enabled_; std::map CsvScenarioInterface::buffer_line_id_; std::map CsvScenarioInterface::buffers_; @@ -92,3 +94,5 @@ double CsvScenarioInterface::GetValueFromBuffer(const std::string buffer_name, c output = itr->second; return output; } + +} // namespace s2e::components diff --git a/src/components/real/power/csv_scenario_interface.hpp b/src/components/real/power/csv_scenario_interface.hpp index e8177ca5f..f105dd773 100644 --- a/src/components/real/power/csv_scenario_interface.hpp +++ b/src/components/real/power/csv_scenario_interface.hpp @@ -11,6 +11,8 @@ #include #include +namespace s2e::components { + typedef std::map DoubleBuffer; /* @@ -78,4 +80,6 @@ class CsvScenarioInterface { static std::map buffers_; //!< Buffer }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_POWER_CSV_SCENARIO_INTERFACE_HPP_ diff --git a/src/components/real/power/pcu_initial_study.cpp b/src/components/real/power/pcu_initial_study.cpp index 77f6fda37..df8527352 100644 --- a/src/components/real/power/pcu_initial_study.cpp +++ b/src/components/real/power/pcu_initial_study.cpp @@ -10,6 +10,8 @@ #include #include +namespace s2e::components { + PcuInitialStudy::PcuInitialStudy(const int prescaler, ClockGenerator* clock_generator, const std::vector saps, Battery* battery, double component_step_time_s) : Component(prescaler, clock_generator), @@ -124,3 +126,5 @@ PcuInitialStudy InitPCU_InitialStudy(ClockGenerator* clock_generator, int pcu_id return pcu; } + +} // namespace s2e::components diff --git a/src/components/real/power/pcu_initial_study.hpp b/src/components/real/power/pcu_initial_study.hpp index f59966774..f9dd8ccf8 100644 --- a/src/components/real/power/pcu_initial_study.hpp +++ b/src/components/real/power/pcu_initial_study.hpp @@ -13,6 +13,8 @@ #include "battery.hpp" #include "solar_array_panel.hpp" +namespace s2e::components { + class PcuInitialStudy : public Component, public ILoggable { public: /** @@ -95,4 +97,6 @@ class PcuInitialStudy : public Component, public ILoggable { PcuInitialStudy InitPCU_InitialStudy(ClockGenerator* clock_generator, int pcu_id, const std::string file_name, const std::vector saps, Battery* battery, double component_step_time_s); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_POWER_PCU_INITIAL_STUDY_HPP_ diff --git a/src/components/real/power/power_control_unit.cpp b/src/components/real/power/power_control_unit.cpp index 055300e2c..8e908c2af 100644 --- a/src/components/real/power/power_control_unit.cpp +++ b/src/components/real/power/power_control_unit.cpp @@ -4,6 +4,8 @@ */ #include "power_control_unit.hpp" +namespace s2e::components { + PowerControlUnit::PowerControlUnit(ClockGenerator* clock_generator) : Component(1, clock_generator) {} PowerControlUnit::PowerControlUnit(int prescaler, ClockGenerator* clock_generator) : Component(prescaler, clock_generator) {} @@ -52,3 +54,5 @@ std::string PowerControlUnit::GetLogValue() const { std::string str_tmp = ""; return str_tmp; } + +} // namespace s2e::components diff --git a/src/components/real/power/power_control_unit.hpp b/src/components/real/power/power_control_unit.hpp index 3e77101c2..f4765e74f 100644 --- a/src/components/real/power/power_control_unit.hpp +++ b/src/components/real/power/power_control_unit.hpp @@ -12,6 +12,8 @@ #include "../../base/component.hpp" +namespace s2e::components { + /* * @class PowerControlUnit * @brief Component emulation of Power Control Unit @@ -95,4 +97,6 @@ class PowerControlUnit : public Component, public ILoggable { std::map power_ports_; //!< Power port list }; +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_POWER_POWER_CONTROL_UNIT_HPP_ diff --git a/src/components/real/power/solar_array_panel.cpp b/src/components/real/power/solar_array_panel.cpp index 126f66da7..67e459f6e 100644 --- a/src/components/real/power/solar_array_panel.cpp +++ b/src/components/real/power/solar_array_panel.cpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::components { + SolarArrayPanel::SolarArrayPanel(const int prescaler, ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, @@ -180,3 +182,5 @@ SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std:: return sap; } + +} // namespace s2e::components diff --git a/src/components/real/power/solar_array_panel.hpp b/src/components/real/power/solar_array_panel.hpp index db114a081..6fb7aacac 100644 --- a/src/components/real/power/solar_array_panel.hpp +++ b/src/components/real/power/solar_array_panel.hpp @@ -13,6 +13,8 @@ #include "../../base/component.hpp" +namespace s2e::components { + class SolarArrayPanel : public Component, public ILoggable { public: /** @@ -157,4 +159,6 @@ SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std:: SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_POWER_SOLAR_ARRAY_PANEL_HPP_ diff --git a/src/components/real/propulsion/simple_thruster.cpp b/src/components/real/propulsion/simple_thruster.cpp index 7917b5fb1..3cfdc6da7 100644 --- a/src/components/real/propulsion/simple_thruster.cpp +++ b/src/components/real/propulsion/simple_thruster.cpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::components { + // Constructor SimpleThruster::SimpleThruster(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Vector<3> thruster_position_b_m, const s2e::math::Vector<3> thrust_direction_b, const double max_magnitude_N, @@ -177,3 +179,5 @@ SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, PowerPort* po magnitude_standard_deviation_N, deg_err, structure, dynamics); return thruster; } + +} // namespace s2e::components diff --git a/src/components/real/propulsion/simple_thruster.hpp b/src/components/real/propulsion/simple_thruster.hpp index f297b8af2..ab1e0734a 100644 --- a/src/components/real/propulsion/simple_thruster.hpp +++ b/src/components/real/propulsion/simple_thruster.hpp @@ -15,6 +15,8 @@ #include "../../base/component.hpp" +namespace s2e::components { + /* * @class SimpleThruster * @brief Component emulation of simple thruster @@ -179,4 +181,6 @@ SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, int thruster_ SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, const Structure* structure, const Dynamics* dynamics); +} // namespace s2e::components + #endif // S2E_COMPONENTS_REAL_PROPULSION_SIMPLE_THRUSTER_HPP_ From 9f6593b0fd9bbb55cd445dbfdd0cda51b2d9fbf5 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:20:01 +0900 Subject: [PATCH 19/49] Add s2e::disturbances namespace --- src/disturbances/air_drag.cpp | 4 ++++ src/disturbances/air_drag.hpp | 4 ++++ src/disturbances/disturbance.hpp | 4 ++++ src/disturbances/disturbances.cpp | 4 ++++ src/disturbances/disturbances.hpp | 4 ++++ src/disturbances/geopotential.cpp | 6 +++++- src/disturbances/geopotential.hpp | 5 +++++ src/disturbances/gravity_gradient.cpp | 4 ++++ src/disturbances/gravity_gradient.hpp | 4 ++++ src/disturbances/lunar_gravity_field.cpp | 4 ++++ src/disturbances/lunar_gravity_field.hpp | 5 +++++ src/disturbances/magnetic_disturbance.cpp | 4 ++++ src/disturbances/magnetic_disturbance.hpp | 4 ++++ src/disturbances/solar_radiation_pressure_disturbance.cpp | 4 ++++ src/disturbances/solar_radiation_pressure_disturbance.hpp | 4 ++++ src/disturbances/surface_force.cpp | 4 ++++ src/disturbances/surface_force.hpp | 4 ++++ src/disturbances/third_body_gravity.cpp | 4 ++++ src/disturbances/third_body_gravity.hpp | 4 ++++ 19 files changed, 79 insertions(+), 1 deletion(-) diff --git a/src/disturbances/air_drag.cpp b/src/disturbances/air_drag.cpp index e64593201..87884ba9b 100644 --- a/src/disturbances/air_drag.cpp +++ b/src/disturbances/air_drag.cpp @@ -12,6 +12,8 @@ #include "../logger/log_utility.hpp" +namespace s2e::disturbances { + AirDrag::AirDrag(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, const double molecular_temperature_K, const double molecular_weight_g_mol, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled), @@ -109,3 +111,5 @@ AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, const Vector<3>& center_of_gravity_b_m); +} // namespace s2e::disturbances + #endif // S2E_DISTURBANCES_AIR_DRAG_HPP_ diff --git a/src/disturbances/disturbance.hpp b/src/disturbances/disturbance.hpp index 427c67fc5..58966c35a 100644 --- a/src/disturbances/disturbance.hpp +++ b/src/disturbances/disturbance.hpp @@ -9,6 +9,8 @@ #include "../environment/local/local_environment.hpp" #include "../math_physics/math/vector.hpp" +namespace s2e::disturbances { + /** * @class Disturbance * @brief Base class for a disturbance @@ -91,4 +93,6 @@ class Disturbance : public ILoggable { s2e::math::Vector<3> acceleration_i_m_s2_; //!< Disturbance acceleration in the inertial frame [m/s2] }; +} // namespace s2e::disturbances + #endif // S2E_DISTURBANCES_DISTURBANCE_HPP_ diff --git a/src/disturbances/disturbances.cpp b/src/disturbances/disturbances.cpp index 682790896..84380248b 100644 --- a/src/disturbances/disturbances.cpp +++ b/src/disturbances/disturbances.cpp @@ -15,6 +15,8 @@ #include "solar_radiation_pressure_disturbance.hpp" #include "third_body_gravity.hpp" +namespace s2e::disturbances { + Disturbances::Disturbances(const SimulationConfiguration* simulation_configuration, const int spacecraft_id, const Structure* structure, const GlobalEnvironment* global_environment) { InitializeInstances(simulation_configuration, spacecraft_id, structure, global_environment); @@ -96,3 +98,5 @@ void Disturbances::InitializeForceAndTorque() { } void Disturbances::InitializeAcceleration() { total_acceleration_i_m_s2_ = Vector<3>(0.0); } + +} // namespace s2e::disturbances diff --git a/src/disturbances/disturbances.hpp b/src/disturbances/disturbances.hpp index 948cabe47..ab1d488fe 100644 --- a/src/disturbances/disturbances.hpp +++ b/src/disturbances/disturbances.hpp @@ -14,6 +14,8 @@ class Logger; +namespace s2e::disturbances { + /** * @class Disturbances * @brief Class to manage all disturbances @@ -99,4 +101,6 @@ class Disturbances { void InitializeAcceleration(); }; +} // namespace s2e::disturbances + #endif // S2E_DISTURBANCES_DISTURBANCES_HPP_ diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index b36df94d4..16123cf5d 100644 --- a/src/disturbances/geopotential.cpp +++ b/src/disturbances/geopotential.cpp @@ -15,6 +15,8 @@ #include "../logger/log_utility.hpp" #include "../utilities/macros.hpp" +namespace s2e::disturbances { + // #define DEBUG_GEOPOTENTIAL Geopotential::Geopotential(const int degree, const std::string file_path, const bool is_calculation_enabled) @@ -125,4 +127,6 @@ Geopotential InitGeopotential(const std::string initialize_file_path) { geopotential_disturbance.is_log_enabled_ = conf.ReadEnable(section, INI_LOG_LABEL); return geopotential_disturbance; -} \ No newline at end of file +} + +} // namespace s2e::disturbances diff --git a/src/disturbances/geopotential.hpp b/src/disturbances/geopotential.hpp index 0d350caae..95ec16968 100644 --- a/src/disturbances/geopotential.hpp +++ b/src/disturbances/geopotential.hpp @@ -11,6 +11,9 @@ #include "../math_physics/gravity/gravity_potential.hpp" #include "../math_physics/math/vector.hpp" #include "disturbance.hpp" + +namespace s2e::disturbances { + /** * @class Geopotential * @brief Class to calculate the high-order earth gravity acceleration @@ -85,4 +88,6 @@ class Geopotential : public Disturbance { */ Geopotential InitGeopotential(const std::string initialize_file_path); +} // namespace s2e::disturbances + #endif // S2E_DISTURBANCES_GEOPOTENTIAL_HPP_ diff --git a/src/disturbances/gravity_gradient.cpp b/src/disturbances/gravity_gradient.cpp index 5eff2c05a..b04b2c8d7 100644 --- a/src/disturbances/gravity_gradient.cpp +++ b/src/disturbances/gravity_gradient.cpp @@ -11,6 +11,8 @@ #include "../logger/log_utility.hpp" +namespace s2e::disturbances { + GravityGradient::GravityGradient(const bool is_calculation_enabled) : GravityGradient(environment::earth_gravitational_constant_m3_s2, is_calculation_enabled) {} @@ -70,3 +72,5 @@ GravityGradient InitGravityGradient(const std::string initialize_file_path, cons return gg_disturbance; } + +} // namespace s2e::disturbances diff --git a/src/disturbances/gravity_gradient.hpp b/src/disturbances/gravity_gradient.hpp index 9ca8ff37f..c54e502dd 100644 --- a/src/disturbances/gravity_gradient.hpp +++ b/src/disturbances/gravity_gradient.hpp @@ -14,6 +14,8 @@ #include "../math_physics/math/vector.hpp" #include "disturbance.hpp" +namespace s2e::disturbances { + /** * @class GravityGradient * @brief Class to calculate the gravity gradient torque @@ -83,4 +85,6 @@ GravityGradient InitGravityGradient(const std::string initialize_file_path); */ GravityGradient InitGravityGradient(const std::string initialize_file_path, const double gravity_constant_m3_s2); +} // namespace s2e::disturbances + #endif // S2E_DISTURBANCES_GRAVITY_GRADIENT_HPP_ diff --git a/src/disturbances/lunar_gravity_field.cpp b/src/disturbances/lunar_gravity_field.cpp index 781ede97d..747a0e065 100644 --- a/src/disturbances/lunar_gravity_field.cpp +++ b/src/disturbances/lunar_gravity_field.cpp @@ -15,6 +15,8 @@ #include "../logger/log_utility.hpp" #include "../utilities/macros.hpp" +namespace s2e::disturbances { + // #define DEBUG_LUNAR_GRAVITY_FIELD LunarGravityField::LunarGravityField(const int degree, const std::string file_path, const bool is_calculation_enabled) @@ -150,3 +152,5 @@ LunarGravityField InitLunarGravityField(const std::string initialize_file_path) return lunar_gravity_field; } + +} // namespace s2e::disturbances diff --git a/src/disturbances/lunar_gravity_field.hpp b/src/disturbances/lunar_gravity_field.hpp index dd5f049af..c809bc4fb 100644 --- a/src/disturbances/lunar_gravity_field.hpp +++ b/src/disturbances/lunar_gravity_field.hpp @@ -11,6 +11,9 @@ #include "../math_physics/gravity/gravity_potential.hpp" #include "../math_physics/math/vector.hpp" #include "disturbance.hpp" + +namespace s2e::disturbances { + /** * @class LunarGravityField * @brief Class to calculate the high-order earth gravity acceleration @@ -89,4 +92,6 @@ class LunarGravityField : public Disturbance { */ LunarGravityField InitLunarGravityField(const std::string initialize_file_path); +} // namespace s2e::disturbances + #endif // S2E_DISTURBANCES_LUNAR_GRAVITY_FIELD_HPP_ diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index 054032b9c..fec275450 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -13,6 +13,8 @@ #include "../math_physics/randomization/normal_randomization.hpp" #include "../math_physics/randomization/random_walk.hpp" +namespace s2e::disturbances { + MagneticDisturbance::MagneticDisturbance(const ResidualMagneticMoment& rmm_params, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, true), residual_magnetic_moment_(rmm_params) { rmm_b_Am2_ = residual_magnetic_moment_.GetConstantValue_b_Am2(); @@ -72,3 +74,5 @@ MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_pa return mag_disturbance; } + +} // namespace s2e::disturbances diff --git a/src/disturbances/magnetic_disturbance.hpp b/src/disturbances/magnetic_disturbance.hpp index e3be84928..ab3bb7375 100644 --- a/src/disturbances/magnetic_disturbance.hpp +++ b/src/disturbances/magnetic_disturbance.hpp @@ -13,6 +13,8 @@ #include "../simulation/spacecraft/structure/residual_magnetic_moment.hpp" #include "disturbance.hpp" +namespace s2e::disturbances { + /** * @class MagneticDisturbance * @brief Class to calculate the magnetic disturbance torque @@ -75,4 +77,6 @@ class MagneticDisturbance : public Disturbance { */ MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const ResidualMagneticMoment& rmm_params); +} // namespace s2e::disturbances + #endif // S2E_DISTURBANCES_MAGNETIC_DISTURBANCE_HPP_ diff --git a/src/disturbances/solar_radiation_pressure_disturbance.cpp b/src/disturbances/solar_radiation_pressure_disturbance.cpp index ed6386234..490938542 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.cpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.cpp @@ -10,6 +10,8 @@ #include "../logger/log_utility.hpp" +namespace s2e::disturbances { + SolarRadiationPressureDisturbance::SolarRadiationPressureDisturbance(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled) {} @@ -65,3 +67,5 @@ SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const st return srp_disturbance; } + +} // namespace s2e::disturbances diff --git a/src/disturbances/solar_radiation_pressure_disturbance.hpp b/src/disturbances/solar_radiation_pressure_disturbance.hpp index dc4f08419..e427e6dc3 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.hpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.hpp @@ -12,6 +12,8 @@ #include "../math_physics/math/vector.hpp" #include "surface_force.hpp" +namespace s2e::disturbances { + /** * @class SolarRadiationPressureDisturbance * @brief Class to calculate the solar radiation pressure disturbance force and torque @@ -68,4 +70,6 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const std::string initialize_file_path, const std::vector& surfaces, const Vector<3>& center_of_gravity_b_m); +} // namespace s2e::disturbances + #endif // S2E_DISTURBANCES_SOLAR_RADIATION_PRESSURE_DISTURBANCE_HPP_ diff --git a/src/disturbances/surface_force.cpp b/src/disturbances/surface_force.cpp index 158e2a695..1bf0b1dfe 100644 --- a/src/disturbances/surface_force.cpp +++ b/src/disturbances/surface_force.cpp @@ -7,6 +7,8 @@ #include "../math_physics/math/vector.hpp" +namespace s2e::disturbances { + SurfaceForce::SurfaceForce(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, true), surfaces_(surfaces), center_of_gravity_b_m_(center_of_gravity_b_m) { // Initialize vectors @@ -52,3 +54,5 @@ void SurfaceForce::CalcTheta(s2e::math::Vector<3>& input_direction_b) { sin_theta_[i] = sqrt(1.0 - cos_theta_[i] * cos_theta_[i]); } } + +} // namespace s2e::disturbances diff --git a/src/disturbances/surface_force.hpp b/src/disturbances/surface_force.hpp index 2fa6fa7d3..943daa9e6 100644 --- a/src/disturbances/surface_force.hpp +++ b/src/disturbances/surface_force.hpp @@ -15,6 +15,8 @@ #include "../simulation/spacecraft/structure/surface.hpp" #include "disturbance.hpp" +namespace s2e::disturbances { + /** * @class ThirdBodyGravity * @brief Class to calculate third body gravity disturbance @@ -71,4 +73,6 @@ class SurfaceForce : public Disturbance { virtual void CalcCoefficients(const s2e::math::Vector<3>& input_direction_b, const double item) = 0; }; +} // namespace s2e::disturbances + #endif // S2E_DISTURBANCES_SURFACE_FORCE_HPP_ diff --git a/src/disturbances/third_body_gravity.cpp b/src/disturbances/third_body_gravity.cpp index 075415ecf..eebb2ec05 100644 --- a/src/disturbances/third_body_gravity.cpp +++ b/src/disturbances/third_body_gravity.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::disturbances { + ThirdBodyGravity::ThirdBodyGravity(std::set third_body_list, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, false), third_body_list_(third_body_list) { acceleration_i_m_s2_ = s2e::math::Vector<3>(0.0); @@ -99,3 +101,5 @@ ThirdBodyGravity InitThirdBodyGravity(const std::string initialize_file_path, co return third_body_disturbance; } + +} // namespace s2e::disturbances diff --git a/src/disturbances/third_body_gravity.hpp b/src/disturbances/third_body_gravity.hpp index 36fded28d..cf45a8593 100644 --- a/src/disturbances/third_body_gravity.hpp +++ b/src/disturbances/third_body_gravity.hpp @@ -14,6 +14,8 @@ #include "../math_physics/math/vector.hpp" #include "disturbance.hpp" +namespace s2e::disturbances { + /** * @class ThirdBodyGravity * @brief Class to calculate third body gravity disturbance @@ -76,4 +78,6 @@ class ThirdBodyGravity : public Disturbance { */ ThirdBodyGravity InitThirdBodyGravity(const std::string initialize_file_path, const std::string ini_path_celes); +} // namespace s2e::disturbances + #endif // S2E_DISTURBANCES_THIRD_BODY_GRAVITY_HPP_ From 54c007e65b22be19bb2f8079ece56cf9b1564256 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:23:56 +0900 Subject: [PATCH 20/49] Add s2e::dynamics::attitude namespace --- src/dynamics/attitude/attitude.cpp | 4 ++++ src/dynamics/attitude/attitude.hpp | 4 ++++ src/dynamics/attitude/attitude_rk4.cpp | 4 ++++ src/dynamics/attitude/attitude_rk4.hpp | 4 ++++ .../attitude/attitude_with_cantilever_vibration.cpp | 4 ++++ .../attitude/attitude_with_cantilever_vibration.hpp | 4 ++++ src/dynamics/attitude/controlled_attitude.cpp | 4 ++++ src/dynamics/attitude/controlled_attitude.hpp | 4 ++++ src/dynamics/attitude/initialize_attitude.cpp | 4 ++++ src/dynamics/attitude/initialize_attitude.hpp | 4 ++++ .../attitude/ode_attitude_with_cantilever_vibration.hpp | 7 +++++-- 11 files changed, 45 insertions(+), 2 deletions(-) diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index 35cc6b237..4777184de 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -6,6 +6,8 @@ #include +namespace s2e::dynamics::attitude { + Attitude::Attitude(const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const std::string& simulation_object_name) : SimulationObject(simulation_object_name), inertia_tensor_kgm2_(inertia_tensor_kgm2) { angular_velocity_b_rad_s_ = s2e::math::Vector<3>(0.0); @@ -79,3 +81,5 @@ s2e::math::Matrix<4, 4> CalcAngularVelocityMatrix(s2e::math::Vector<3> angular_v return angular_velocity_matrix; } + +} // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index 3485444d9..1dbcfa809 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -12,6 +12,8 @@ #include #include +namespace s2e::dynamics::attitude { + /** * @class Attitude * @brief Base class for attitude of spacecraft @@ -142,4 +144,6 @@ class Attitude : public ILoggable, public SimulationObject { */ s2e::math::Matrix<4, 4> CalcAngularVelocityMatrix(s2e::math::Vector<3> angular_velocity_b_rad_s); +} // namespace s2e::dynamics::attitude + #endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_HPP_ diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index 29e130fef..bac023c33 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::dynamics::attitude { + AttitudeRk4::AttitudeRk4(const s2e::math::Vector<3>& angular_velocity_b_rad_s, const s2e::math::Quaternion& quaternion_i2b, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const s2e::math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name) @@ -119,3 +121,5 @@ void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { } quaternion_i2b_.Normalize(); } + +} // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/attitude_rk4.hpp b/src/dynamics/attitude/attitude_rk4.hpp index 003989937..1e71fca19 100644 --- a/src/dynamics/attitude/attitude_rk4.hpp +++ b/src/dynamics/attitude/attitude_rk4.hpp @@ -8,6 +8,8 @@ #include "attitude.hpp" +namespace s2e::dynamics::attitude { + /** * @class AttitudeRk4 * @brief Class to calculate spacecraft attitude with Runge-Kutta method @@ -68,4 +70,6 @@ class AttitudeRk4 : public Attitude { void RungeKuttaOneStep(double t, double dt); }; +} // namespace s2e::dynamics::attitude + #endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_RK4_HPP_ diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp index fe58833da..2b2ff73e3 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::dynamics::attitude { + AttitudeWithCantileverVibration::AttitudeWithCantileverVibration( const s2e::math::Vector<3>& angular_velocity_b_rad_s, const s2e::math::Quaternion& quaternion_i2b, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const s2e::math::Matrix<3, 3>& inertia_tensor_cantilever_kgm2, const double damping_ratio_cantilever, @@ -99,3 +101,5 @@ void AttitudeWithCantileverVibration::Propagate(const double end_time_s) { attitude_ode_.SetPreviousInertiaTensor_kgm2(inertia_tensor_kgm2_); CalcAngularMomentum(); } + +} // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp index 76a8f974f..dbc96391d 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp @@ -12,6 +12,8 @@ #include "attitude.hpp" #include "ode_attitude_with_cantilever_vibration.hpp" +namespace s2e::dynamics::attitude { + /** * @class AttitudeWithCantileverVibration * @brief Class to calculate spacecraft attitude with cantilever vibration @@ -77,4 +79,6 @@ class AttitudeWithCantileverVibration : public Attitude { s2e::numerical_integration::NumericalIntegratorManager<13> numerical_integrator_; }; +} // namespace s2e::dynamics::attitude + #endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_WITH_CANTILEVER_VIBRATION_HPP_ diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index 982b1b446..04937978c 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -7,6 +7,8 @@ #include #include +namespace s2e::dynamics::attitude { + ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const s2e::math::Quaternion quaternion_i2b, const s2e::math::Vector<3> main_target_direction_b, const s2e::math::Vector<3> sub_target_direction_b, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, @@ -178,3 +180,5 @@ void ControlledAttitude::CalcAngularVelocity(const double current_time_s) { previous_quaternion_i2b_ = quaternion_i2b_; previous_omega_b_rad_s_ = angular_velocity_b_rad_s_; } + +} // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index 88204270b..97154f376 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -13,6 +13,8 @@ #include "../orbit/orbit.hpp" #include "attitude.hpp" +namespace s2e::dynamics::attitude { + /** * @enum AttitudeControlMode * @brief Attitude control mode @@ -149,4 +151,6 @@ class ControlledAttitude : public Attitude { s2e::math::Matrix<3, 3> CalcDcm(const s2e::math::Vector<3> main_direction, const s2e::math::Vector<3> sub_direction); }; +} // namespace s2e::dynamics::attitude + #endif // S2E_DYNAMICS_ATTITUDE_CONTROLLED_ATTITUDE_HPP_ diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index 640cf98c6..02e19ad09 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -6,6 +6,8 @@ #include +namespace s2e::dynamics::attitude { + Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* local_celestial_information, const double step_width_s, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id) { IniAccess ini_file(file_name); @@ -93,3 +95,5 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel return attitude; } + +} // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/initialize_attitude.hpp b/src/dynamics/attitude/initialize_attitude.hpp index 0e3899c02..fae3b99ab 100644 --- a/src/dynamics/attitude/initialize_attitude.hpp +++ b/src/dynamics/attitude/initialize_attitude.hpp @@ -11,6 +11,8 @@ #include "attitude_with_cantilever_vibration.hpp" #include "controlled_attitude.hpp" +namespace s2e::dynamics::attitude { + /** * @fn InitAttitude * @brief Initialize function for Attitude @@ -24,4 +26,6 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* local_celestial_information, const double step_width_s, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id); +} // namespace s2e::dynamics::attitude + #endif // S2E_DYNAMICS_ATTITUDE_INITIALIZE_ATTITUDE_HPP_ diff --git a/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp index f91e2be95..b0cc7711c 100644 --- a/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp @@ -11,7 +11,10 @@ #include "attitude.hpp" -namespace s2e::numerical_integration { +namespace s2e::dynamics::attitude { + +using s2e::numerical_integration + /** * @class AttitudeWithCantileverVibrationOde * @brief Class to implement Ordinary Differential Equations for Attitude with Cantilever Vibration @@ -221,6 +224,6 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { s2e::math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever_{0.0}; //!< Inverse of inertia tensor of the cantilever }; -} // namespace s2e::numerical_integration +} // namespace s2e::dynamics::attitude #endif // S2E_DYNAMICS_ATTITUDE_ODE_ATTITUDE_WITH_CANTILEVER_VIBRATION_HPP_ From 4e9a3049265a29feae41f0333cdb9c599251bc95 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:26:15 +0900 Subject: [PATCH 21/49] Add s2e::dynamics::orbit namespace --- src/dynamics/orbit/encke_orbit_propagation.cpp | 4 ++++ src/dynamics/orbit/encke_orbit_propagation.hpp | 4 ++++ src/dynamics/orbit/initialize_orbit.cpp | 4 ++++ src/dynamics/orbit/initialize_orbit.hpp | 4 ++++ src/dynamics/orbit/kepler_orbit_propagation.cpp | 4 ++++ src/dynamics/orbit/kepler_orbit_propagation.hpp | 4 ++++ src/dynamics/orbit/orbit.cpp | 4 ++++ src/dynamics/orbit/orbit.hpp | 4 ++++ src/dynamics/orbit/relative_orbit.cpp | 4 ++++ src/dynamics/orbit/relative_orbit.hpp | 4 ++++ src/dynamics/orbit/rk4_orbit_propagation.cpp | 4 ++++ src/dynamics/orbit/rk4_orbit_propagation.hpp | 4 ++++ src/dynamics/orbit/sgp4_orbit_propagation.cpp | 4 ++++ src/dynamics/orbit/sgp4_orbit_propagation.hpp | 4 ++++ 14 files changed, 56 insertions(+) diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index e25abbde4..b846fa37c 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -9,6 +9,8 @@ #include "../../math_physics/orbit/orbital_elements.hpp" +namespace s2e::dynamics::orbit { + EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double gravity_constant_m3_s2, const double propagation_step_s, const double current_time_jd, const s2e::math::Vector<3> position_i_m, const s2e::math::Vector<3> velocity_i_m_s, const double error_tolerance) @@ -124,3 +126,5 @@ double EnckeOrbitPropagation::CalcQFunction(s2e::math::Vector<3> difference_posi return q_func; } + +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index a43db4e93..a06a207f1 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -10,6 +10,8 @@ #include "../../math_physics/orbit/kepler_orbit.hpp" #include "orbit.hpp" +namespace s2e::dynamics::orbit { + /** * @class EnckeOrbitPropagation * @brief Class to propagate spacecraft orbit with Encke's method @@ -93,4 +95,6 @@ class EnckeOrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferenti double CalcQFunction(const s2e::math::Vector<3> difference_position_i_m); }; +} // namespace s2e::dynamics::orbit + #endif // S2E_DYNAMICS_ORBIT_ENCKE_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index 9d071761f..5b8329708 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -12,6 +12,8 @@ #include "rk4_orbit_propagation.hpp" #include "sgp4_orbit_propagation.hpp" +namespace s2e::dynamics::orbit { + Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, double current_time_jd, double gravity_constant_m3_s2, std::string section, RelativeInformation* relative_information) { auto conf = IniAccess(initialize_file); @@ -148,3 +150,5 @@ s2e::math::Vector<6> InitializePosVel(std::string initialize_file, double curren return pos_vel; } + +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/initialize_orbit.hpp b/src/dynamics/orbit/initialize_orbit.hpp index 1bdbe44cd..59a03d8a0 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -12,6 +12,8 @@ class RelativeInformation; +namespace s2e::dynamics::orbit { + /** * @fn InitOrbit * @brief Initialize function for Orbit class @@ -37,4 +39,6 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string */ s2e::math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section = "ORBIT"); +} // namespace s2e::dynamics::orbit + #endif // S2E_DYNAMICS_ORBIT_INITIALIZE_ORBIT_HPP_ diff --git a/src/dynamics/orbit/kepler_orbit_propagation.cpp b/src/dynamics/orbit/kepler_orbit_propagation.cpp index 905c56e39..d745a2c3a 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.cpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.cpp @@ -8,6 +8,8 @@ #include "../../math_physics/math/s2e_math.hpp" +namespace s2e::dynamics::orbit { + KeplerOrbitPropagation::KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_time_jd, KeplerOrbit kepler_orbit) : Orbit(celestial_information), KeplerOrbit(kepler_orbit) { @@ -32,3 +34,5 @@ void KeplerOrbitPropagation::UpdateState(const double current_time_jd) { TransformEciToEcef(); TransformEcefToGeodetic(); } + +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/kepler_orbit_propagation.hpp b/src/dynamics/orbit/kepler_orbit_propagation.hpp index 9604c4bc1..117f8edc1 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.hpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.hpp @@ -9,6 +9,8 @@ #include "../../math_physics/orbit/kepler_orbit.hpp" #include "orbit.hpp" +namespace s2e::dynamics::orbit { + /** * @class KeplerOrbitPropagation * @brief Class to propagate spacecraft orbit with Kepler equation @@ -48,4 +50,6 @@ class KeplerOrbitPropagation : public Orbit, public s2e::orbit::KeplerOrbit { void UpdateState(const double current_time_jd); }; +} // namespace s2e::dynamics::orbit + #endif // S2E_DYNAMICS_ORBIT_KEPLER_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index d3058f5fd..c2b94493d 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -4,6 +4,8 @@ */ #include "orbit.hpp" +namespace s2e::dynamics::orbit { + s2e::math::Quaternion Orbit::CalcQuaternion_i2lvlh() const { s2e::math::Vector<3> lvlh_x = spacecraft_position_i_m_; // x-axis in LVLH frame is position vector direction from geocenter to satellite s2e::math::Vector<3> lvlh_ex = lvlh_x.CalcNormalizedVector(); @@ -93,3 +95,5 @@ std::string Orbit::GetLogValue() const { return str_tmp; } + +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index 79a016b81..dabd75f59 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -16,6 +16,8 @@ #include #include +namespace s2e::dynamics::orbit { + /** * @enum OrbitPropagateMode * @brief Propagation mode of orbit @@ -217,4 +219,6 @@ class Orbit : public ILoggable { OrbitInitializeMode SetOrbitInitializeMode(const std::string initialize_mode); +} // namespace s2e::dynamics::orbit + #endif // S2E_DYNAMICS_ORBIT_ORBIT_HPP_ diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 5e6065776..4dbb9838a 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -8,6 +8,8 @@ #include "rk4_orbit_propagation.hpp" +namespace s2e::dynamics::orbit { + RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, int reference_spacecraft_id, s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, @@ -157,3 +159,5 @@ void RelativeOrbit::DerivativeFunction(double t, const s2e::math::Vector<6>& sta rhs = system_matrix_ * state; (void)t; } + +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 3a98c1249..609994168 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -13,6 +13,8 @@ #include "orbit.hpp" +namespace s2e::dynamics::orbit { + /** * @class RelativeOrbit * @brief Class to propagate relative orbit @@ -126,4 +128,6 @@ class RelativeOrbit : public Orbit, public s2e::math::OrdinaryDifferentialEquati void PropagateStm(double elapsed_sec); }; +} // namespace s2e::dynamics::orbit + #endif // S2E_DYNAMICS_ORBIT_RELATIVE_ORBIT_HPP_ diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index dd2c5abfa..b73971d46 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::dynamics::orbit { + Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, s2e::math::Vector<3> position_i_m, s2e::math::Vector<3> velocity_i_m_s, double initial_time_s) : Orbit(celestial_information), OrdinaryDifferentialEquation<6>(time_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2) { @@ -86,3 +88,5 @@ void Rk4OrbitPropagation::Propagate(const double end_time_s, const double curren TransformEciToEcef(); TransformEcefToGeodetic(); } + +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index 83fc56bbe..dbdc33b0d 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -11,6 +11,8 @@ #include "orbit.hpp" +namespace s2e::dynamics::orbit { + /** * @class Rk4OrbitPropagation * @brief Class to propagate spacecraft orbit with Runge-Kutta-4 method @@ -69,4 +71,6 @@ class Rk4OrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferential void Initialize(s2e::math::Vector<3> position_i_m, s2e::math::Vector<3> velocity_i_m_s, double initial_time_s = 0); }; +} // namespace s2e::dynamics::orbit + #endif // S2E_DYNAMICS_ORBIT_RK4_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index 30451f568..f15c8b56c 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::dynamics::orbit { + Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, const int wgs_setting, const double current_time_jd) : Orbit(celestial_information) { @@ -66,3 +68,5 @@ void Sgp4OrbitPropagation::Propagate(const double end_time_s, const double curre TransformEciToEcef(); TransformEcefToGeodetic(); } + +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.hpp b/src/dynamics/orbit/sgp4_orbit_propagation.hpp index 42b69681c..a31be3690 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.hpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.hpp @@ -13,6 +13,8 @@ #include "orbit.hpp" +namespace s2e::dynamics::orbit { + /** * @class Sgp4OrbitPropagation * @brief Class to propagate spacecraft orbit with SGP4 method with TLE @@ -45,4 +47,6 @@ class Sgp4OrbitPropagation : public Orbit { elsetrec sgp4_data_; //!< Structure data for SGP4 library }; +} // namespace s2e::dynamics::orbit + #endif // S2E_DYNAMICS_ORBIT_SGP4_ORBIT_PROPAGATION_HPP_ From 70ec6612d7a1990b54692585db9506fbb21f5634 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:29:37 +0900 Subject: [PATCH 22/49] Add s2e::dynamics::thermal namespace --- src/dynamics/thermal/heater.cpp | 4 ++++ src/dynamics/thermal/heater.hpp | 4 ++++ src/dynamics/thermal/heater_controller.cpp | 4 ++++ src/dynamics/thermal/heater_controller.hpp | 4 ++++ src/dynamics/thermal/heatload.cpp | 4 ++++ src/dynamics/thermal/heatload.hpp | 4 ++++ src/dynamics/thermal/node.cpp | 4 ++++ src/dynamics/thermal/node.hpp | 4 ++++ src/dynamics/thermal/temperature.cpp | 4 ++++ src/dynamics/thermal/temperature.hpp | 4 ++++ 10 files changed, 40 insertions(+) diff --git a/src/dynamics/thermal/heater.cpp b/src/dynamics/thermal/heater.cpp index d47ce224a..6444ae329 100644 --- a/src/dynamics/thermal/heater.cpp +++ b/src/dynamics/thermal/heater.cpp @@ -5,6 +5,8 @@ using namespace std; +namespace s2e::dynamics::thermal { + Heater::Heater(const size_t heater_id, const double power_rating_W) : heater_id_(heater_id), power_rating_W_(power_rating_W) { AssertHeaterParams(); heater_status_ = HeaterStatus::kOff; @@ -75,3 +77,5 @@ Heater InitHeater(const std::vector& heater_str) { Heater heater(heater_id, power_rating_W); return heater; } + +} // namespace s2e::dynamics::thermal diff --git a/src/dynamics/thermal/heater.hpp b/src/dynamics/thermal/heater.hpp index 42ea9774f..ac1f47c30 100644 --- a/src/dynamics/thermal/heater.hpp +++ b/src/dynamics/thermal/heater.hpp @@ -11,6 +11,8 @@ #include #include +namespace s2e::dynamics::thermal { + /** * @enum HeaterStatus * @brief Status of heater (On/Off) @@ -99,4 +101,6 @@ class Heater { */ Heater InitHeater(const std::vector& heater_str); +} // namespace s2e::dynamics::thermal + #endif // S2E_DYNAMICS_THERMAL_HEATER_HPP_ diff --git a/src/dynamics/thermal/heater_controller.cpp b/src/dynamics/thermal/heater_controller.cpp index 93c0fc811..6c346041e 100644 --- a/src/dynamics/thermal/heater_controller.cpp +++ b/src/dynamics/thermal/heater_controller.cpp @@ -4,6 +4,8 @@ #include using namespace std; +namespace s2e::dynamics::thermal { + HeaterController::HeaterController(const double lower_threshold_degC, const double upper_threshold_degC) : lower_threshold_degC_(lower_threshold_degC), upper_threshold_degC_(upper_threshold_degC) { AssertHeaterControllerParams(); @@ -52,3 +54,5 @@ HeaterController InitHeaterController(const std::vector& heater_str HeaterController heater_controller(lower_threshold_degC, upper_threshold_degC); return heater_controller; } + +} // namespace s2e::dynamics::thermal diff --git a/src/dynamics/thermal/heater_controller.hpp b/src/dynamics/thermal/heater_controller.hpp index 4c2353fb0..638e8d9e2 100644 --- a/src/dynamics/thermal/heater_controller.hpp +++ b/src/dynamics/thermal/heater_controller.hpp @@ -13,6 +13,8 @@ #include "heater.hpp" +namespace s2e::dynamics::thermal { + class HeaterController { public: /** @@ -89,4 +91,6 @@ class HeaterController { */ HeaterController InitHeaterController(const std::vector& heater_str); +} // namespace s2e::dynamics::thermal + #endif // S2E_DYNAMICS_THERMAL_HEATER_CONTROLLER_HPP_ diff --git a/src/dynamics/thermal/heatload.cpp b/src/dynamics/thermal/heatload.cpp index f8e5d98a6..949bfa361 100644 --- a/src/dynamics/thermal/heatload.cpp +++ b/src/dynamics/thermal/heatload.cpp @@ -7,6 +7,8 @@ using namespace std; using namespace s2e::math; +namespace s2e::dynamics::thermal { + Heatload::Heatload(int node_id, std::vector time_table_s, std::vector internal_heatload_table_W) : node_id_(node_id), time_table_s_(time_table_s), internal_heatload_table_W_(internal_heatload_table_W) { elapsed_time_s_ = 0; @@ -100,3 +102,5 @@ Heatload InitHeatload(const std::vector& time_str, const std::vecto return Heatload(node_id, time_table_s, internal_heatload_table_W); } + +} // namespace s2e::dynamics::thermal diff --git a/src/dynamics/thermal/heatload.hpp b/src/dynamics/thermal/heatload.hpp index ef17d51e4..0a3106a25 100644 --- a/src/dynamics/thermal/heatload.hpp +++ b/src/dynamics/thermal/heatload.hpp @@ -10,6 +10,8 @@ #include #include +namespace s2e::dynamics::thermal { + /** * @class Heatload * @brief Class for calculating heatload value for Node class object at elapsed time @@ -127,4 +129,6 @@ class Heatload { */ Heatload InitHeatload(const std::vector& time_str, const std::vector& internal_heatload_str); +} // namespace s2e::dynamics::thermal + #endif // S2E_DYNAMICS_THERMAL_HEATLOAD_HPP_ diff --git a/src/dynamics/thermal/node.cpp b/src/dynamics/thermal/node.cpp index 8ec1a5849..992fe489d 100644 --- a/src/dynamics/thermal/node.cpp +++ b/src/dynamics/thermal/node.cpp @@ -12,6 +12,8 @@ using namespace std; using namespace s2e::math; +namespace s2e::dynamics::thermal { + Node::Node(const size_t node_id, const string node_name, const NodeType node_type, const size_t heater_id, const double temperature_ini_K, const double capacity_J_K, const double alpha, const double area_m2, s2e::math::Vector<3> normal_vector_b) : node_id_(node_id), @@ -186,3 +188,5 @@ Node InitNode(const std::vector& node_str) { Node node(node_id, node_label, node_type, heater_id, temperature_K, capacity_J_K, alpha, area_m2, normal_v_b); return node; } + +} // namespace s2e::dynamics::thermal diff --git a/src/dynamics/thermal/node.hpp b/src/dynamics/thermal/node.hpp index e6267f79d..b75e6ad72 100644 --- a/src/dynamics/thermal/node.hpp +++ b/src/dynamics/thermal/node.hpp @@ -11,6 +11,8 @@ #include #include +namespace s2e::dynamics::thermal { + /** * @enum NodeType * @brief Type of node @@ -170,4 +172,6 @@ class Node { */ Node InitNode(const std::vector& node_str); +}namespace s2e::dynamics::thermal + #endif // S2E_DYNAMICS_THERMAL_NODE_HPP_ diff --git a/src/dynamics/thermal/temperature.cpp b/src/dynamics/thermal/temperature.cpp index 0025e27f0..aec2acc7a 100644 --- a/src/dynamics/thermal/temperature.cpp +++ b/src/dynamics/thermal/temperature.cpp @@ -14,6 +14,8 @@ using namespace std; +namespace s2e::dynamics::thermal { + Temperature::Temperature(const vector> conductance_matrix_W_K, const vector> radiation_matrix_m2, vector nodes, vector heatloads, vector heaters, vector heater_controllers, const size_t node_num, const double propagation_step_s, const SolarRadiationPressureEnvironment* srp_environment, const EarthAlbedo* earth_albedo, @@ -388,3 +390,5 @@ Temperature* InitTemperature(const std::string file_name, const double rk_prop_s rk_prop_step_s, srp_environment, earth_albedo, is_calc_enabled, solar_calc_setting, debug); return temperature; } + +} // namespace s2e::dynamics::thermal diff --git a/src/dynamics/thermal/temperature.hpp b/src/dynamics/thermal/temperature.hpp index 4958cc06b..c0aaec67c 100644 --- a/src/dynamics/thermal/temperature.hpp +++ b/src/dynamics/thermal/temperature.hpp @@ -17,6 +17,8 @@ #include "heatload.hpp" #include "node.hpp" +namespace s2e::dynamics::thermal { + /** * @enum SolarCalcSetting * @brief Whether to calculate solar radiation inside simulation @@ -166,4 +168,6 @@ class Temperature : public ILoggable { Temperature* InitTemperature(const std::string file_name, const double rk_prop_step_s, const SolarRadiationPressureEnvironment* srp_environment, const EarthAlbedo* earth_albedo); +} // namespace s2e::dynamics::thermal + #endif // S2E_DYNAMICS_THERMAL_TEMPERATURE_HPP_ From 21675ce3e4e2536f375fcb52114c5c1eccd5fc4c Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:31:40 +0900 Subject: [PATCH 23/49] Add s2e::dynamics namespace --- src/dynamics/dynamics.cpp | 4 ++++ src/dynamics/dynamics.hpp | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index 8cf774ad8..98276524c 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -7,6 +7,8 @@ #include "../simulation/multiple_spacecraft/relative_information.hpp" +namespace s2e::dynamics { + Dynamics::Dynamics(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, const LocalEnvironment* local_environment, const int spacecraft_id, Structure* structure, RelativeInformation* relative_information) @@ -65,3 +67,5 @@ void Dynamics::LogSetup(Logger& logger) { logger.AddLogList(orbit_); logger.AddLogList(temperature_); } + +} // namespace s2e::dynamics diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index 2db4bfbbd..27797a506 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -20,6 +20,8 @@ class RelativeInformation; class LocalEnvironment; +namespace s2e::dynamics { + /** * @class Dynamics * @brief Class to manage dynamics of spacecraft @@ -127,4 +129,6 @@ class Dynamics { Structure* structure, RelativeInformation* relative_information = (RelativeInformation*)nullptr); }; +} // namespace s2e::dynamics + #endif // S2E_DYNAMICS_DYNAMICS_HPP_ From 62f53388925437d6f4579846f0b3f66e3ead632f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:36:59 +0900 Subject: [PATCH 24/49] Add s2e::environment namespace --- src/environment/global/celestial_information.cpp | 4 ++++ src/environment/global/celestial_information.hpp | 4 ++++ src/environment/global/clock_generator.cpp | 4 ++++ src/environment/global/clock_generator.hpp | 4 ++++ src/environment/global/earth_rotation.cpp | 4 ++++ src/environment/global/earth_rotation.hpp | 4 ++++ src/environment/global/global_environment.cpp | 4 ++++ src/environment/global/global_environment.hpp | 4 ++++ src/environment/global/gnss_satellites.cpp | 4 ++++ src/environment/global/gnss_satellites.hpp | 4 ++++ src/environment/global/hipparcos_catalogue.cpp | 4 ++++ src/environment/global/hipparcos_catalogue.hpp | 4 ++++ src/environment/global/moon_rotation.cpp | 6 +++++- src/environment/global/moon_rotation.hpp | 4 ++++ src/environment/global/physical_constants.hpp | 4 ++-- src/environment/global/simulation_time.cpp | 4 ++++ src/environment/global/simulation_time.hpp | 4 ++++ src/environment/local/atmosphere.cpp | 4 ++++ src/environment/local/atmosphere.hpp | 4 ++++ src/environment/local/earth_albedo.cpp | 4 ++++ src/environment/local/earth_albedo.hpp | 4 ++++ src/environment/local/geomagnetic_field.cpp | 4 ++++ src/environment/local/geomagnetic_field.hpp | 4 ++++ src/environment/local/local_celestial_information.cpp | 4 ++++ src/environment/local/local_celestial_information.hpp | 4 ++++ src/environment/local/local_environment.cpp | 4 ++++ src/environment/local/local_environment.hpp | 4 ++++ .../local/solar_radiation_pressure_environment.cpp | 4 ++++ .../local/solar_radiation_pressure_environment.hpp | 4 ++++ 29 files changed, 115 insertions(+), 3 deletions(-) diff --git a/src/environment/global/celestial_information.cpp b/src/environment/global/celestial_information.cpp index c51461e92..4eb09cf05 100644 --- a/src/environment/global/celestial_information.cpp +++ b/src/environment/global/celestial_information.cpp @@ -18,6 +18,8 @@ #include "logger/log_utility.hpp" #include "setting_file_reader/initialize_file_access.hpp" +namespace s2e::environment { + CelestialInformation::CelestialInformation(const std::string inertial_frame_name, const std::string aberration_correction_setting, const std::string center_body_name, const unsigned int number_of_selected_body, int* selected_body_ids, const std::vector rotation_mode_list) @@ -244,3 +246,5 @@ CelestialInformation* InitCelestialInformation(std::string file_name) { return celestial_info; } + +} // namespace s2e::environment diff --git a/src/environment/global/celestial_information.hpp b/src/environment/global/celestial_information.hpp index 6ffe57fbf..eeb9ff5f2 100644 --- a/src/environment/global/celestial_information.hpp +++ b/src/environment/global/celestial_information.hpp @@ -17,6 +17,8 @@ class MoonRotation; +namespace s2e::environment { + /** * @class CelestialInformation * @brief Class to manage the information related with the celestial bodies @@ -281,4 +283,6 @@ class CelestialInformation : public ILoggable { */ CelestialInformation* InitCelestialInformation(std::string file_name); +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_GLOBAL_CELESTIAL_INFORMATION_HPP_ diff --git a/src/environment/global/clock_generator.cpp b/src/environment/global/clock_generator.cpp index a196efac0..f2a1b7c8f 100644 --- a/src/environment/global/clock_generator.cpp +++ b/src/environment/global/clock_generator.cpp @@ -5,6 +5,8 @@ #include "clock_generator.hpp" +namespace s2e::environment { + ClockGenerator::~ClockGenerator() {} void ClockGenerator::RegisterComponent(ITickable* tickable) { components_.push_back(tickable); } @@ -38,3 +40,5 @@ void ClockGenerator::UpdateComponents(const SimulationTime* simulation_time) { TickToComponents(); } } + +} // namespace s2e::environment diff --git a/src/environment/global/clock_generator.hpp b/src/environment/global/clock_generator.hpp index b09b80c4a..91ef4c383 100644 --- a/src/environment/global/clock_generator.hpp +++ b/src/environment/global/clock_generator.hpp @@ -11,6 +11,8 @@ #include "simulation_time.hpp" +namespace s2e::environment { + /** * @class ClockGenerator * @brief Class to generate clock for classes which have ITickable @@ -57,4 +59,6 @@ class ClockGenerator { unsigned int timer_count_; //!< Timer count TODO: change to long? }; +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_GLOBAL_CLOCK_GENERATOR_HPP_ diff --git a/src/environment/global/earth_rotation.cpp b/src/environment/global/earth_rotation.cpp index 8950fd904..fd62d7b10 100644 --- a/src/environment/global/earth_rotation.cpp +++ b/src/environment/global/earth_rotation.cpp @@ -15,6 +15,8 @@ #include "math_physics/orbit/sgp4/sgp4ext.h" // for jday() #include "math_physics/orbit/sgp4/sgp4unit.h" // for gstime() +namespace s2e::environment { + // Default constructor EarthRotation::EarthRotation(const EarthRotationMode rotation_mode) : rotation_mode_(rotation_mode) { dcm_j2000_to_ecef_ = s2e::math::MakeIdentityMatrix<3>(); @@ -275,3 +277,5 @@ EarthRotationMode ConvertEarthRotationMode(const std::string mode) { return rotation_mode; } + +} // namespace s2e::environment diff --git a/src/environment/global/earth_rotation.hpp b/src/environment/global/earth_rotation.hpp index f4c7f7df7..b9680e478 100644 --- a/src/environment/global/earth_rotation.hpp +++ b/src/environment/global/earth_rotation.hpp @@ -11,6 +11,8 @@ #include "math_physics/math/matrix.hpp" +namespace s2e::environment { + /** * @enum EarthRotationMode * @brief Definition of calculation mode of earth rotation @@ -122,4 +124,6 @@ class EarthRotation { EarthRotationMode ConvertEarthRotationMode(const std::string mode); +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_GLOBAL_EARTH_ROTATION_HPP_ diff --git a/src/environment/global/global_environment.cpp b/src/environment/global/global_environment.cpp index 0e89b2346..b97b78da7 100644 --- a/src/environment/global/global_environment.cpp +++ b/src/environment/global/global_environment.cpp @@ -7,6 +7,8 @@ #include "setting_file_reader/initialize_file_access.hpp" +namespace s2e::environment { + GlobalEnvironment::GlobalEnvironment(const SimulationConfiguration* simulation_configuration) { Initialize(simulation_configuration); } GlobalEnvironment::~GlobalEnvironment() { @@ -43,4 +45,6 @@ void GlobalEnvironment::LogSetup(Logger& logger) { logger.AddLogList(gnss_satellites_); } +} // namespace s2e::environment + void GlobalEnvironment::Reset(void) { simulation_time_->ResetClock(); } diff --git a/src/environment/global/global_environment.hpp b/src/environment/global/global_environment.hpp index b8bb56890..37c51cbfb 100644 --- a/src/environment/global/global_environment.hpp +++ b/src/environment/global/global_environment.hpp @@ -13,6 +13,8 @@ #include "simulation/simulation_configuration.hpp" #include "simulation_time.hpp" +namespace s2e::environment { + /** * @class GlobalEnvironment * @brief Class to manage the global environment @@ -82,4 +84,6 @@ class GlobalEnvironment { GnssSatellites* gnss_satellites_; //!< GNSS satellites }; +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_GLOBAL_GLOBAL_ENVIRONMENT_HPP_ diff --git a/src/environment/global/gnss_satellites.cpp b/src/environment/global/gnss_satellites.cpp index 11b551228..d16fefc26 100644 --- a/src/environment/global/gnss_satellites.cpp +++ b/src/environment/global/gnss_satellites.cpp @@ -17,6 +17,8 @@ using namespace s2e::gnss; +namespace s2e::environment { + const size_t kNumberOfInterpolation = 9; void GnssSatellites::Initialize(const std::vector& sp3_files, const s2e::time_system::EpochTime start_time) { @@ -234,3 +236,5 @@ GnssSatellites* InitGnssSatellites(const std::string file_name, const EarthRotat return gnss_satellites; } + +} // namespace s2e::environment diff --git a/src/environment/global/gnss_satellites.hpp b/src/environment/global/gnss_satellites.hpp index 6051ce4f5..88994a500 100644 --- a/src/environment/global/gnss_satellites.hpp +++ b/src/environment/global/gnss_satellites.hpp @@ -21,6 +21,8 @@ #include "math_physics/math/vector.hpp" #include "simulation_time.hpp" +namespace s2e::environment { + /** * @class GnssSatellites * @brief Class to calculate GNSS satellite position and clock @@ -153,4 +155,6 @@ class GnssSatellites : public ILoggable { */ GnssSatellites* InitGnssSatellites(const std::string file_name, const EarthRotation& earth_rotation, const SimulationTime& simulation_time); +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_GLOBAL_GNSS_SATELLITES_HPP_ diff --git a/src/environment/global/hipparcos_catalogue.cpp b/src/environment/global/hipparcos_catalogue.cpp index 70e9fca25..72ad7dfdf 100644 --- a/src/environment/global/hipparcos_catalogue.cpp +++ b/src/environment/global/hipparcos_catalogue.cpp @@ -14,6 +14,8 @@ #include "math_physics/math/constants.hpp" #include "setting_file_reader/initialize_file_access.hpp" +namespace s2e::environment { + HipparcosCatalogue::HipparcosCatalogue(double max_magnitude, std::string catalogue_path) : max_magnitude_(max_magnitude), catalogue_path_(catalogue_path) {} @@ -99,3 +101,5 @@ HipparcosCatalogue* InitHipparcosCatalogue(std::string file_name) { return hipparcos_catalogue_; } + +} // namespace s2e::environment diff --git a/src/environment/global/hipparcos_catalogue.hpp b/src/environment/global/hipparcos_catalogue.hpp index 3114fdb3a..dd20fee9d 100644 --- a/src/environment/global/hipparcos_catalogue.hpp +++ b/src/environment/global/hipparcos_catalogue.hpp @@ -12,6 +12,8 @@ #include "math_physics/math/quaternion.hpp" #include "math_physics/math/vector.hpp" +namespace s2e::environment { + /** *@struct HipparcosData *@brief Hipparcos catalogue data @@ -118,4 +120,6 @@ class HipparcosCatalogue : public ILoggable { */ HipparcosCatalogue* InitHipparcosCatalogue(std::string file_name); +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_GLOBAL_HIPPARCOS_CATALOGUE_HPP_ diff --git a/src/environment/global/moon_rotation.cpp b/src/environment/global/moon_rotation.cpp index a49e236ec..c8843e3a0 100644 --- a/src/environment/global/moon_rotation.cpp +++ b/src/environment/global/moon_rotation.cpp @@ -10,6 +10,8 @@ #include #include +namespace s2e::environment { + MoonRotation::MoonRotation(const CelestialInformation& celestial_information, MoonRotationMode mode) : mode_(mode), celestial_information_(celestial_information) { dcm_j2000_to_mcmf_ = s2e::math::MakeIdentityMatrix<3>(); @@ -50,4 +52,6 @@ MoonRotationMode ConvertMoonRotationMode(const std::string mode) { } return rotation_mode; -} \ No newline at end of file +} + +} // namespace s2e::environment diff --git a/src/environment/global/moon_rotation.hpp b/src/environment/global/moon_rotation.hpp index 5df9f247a..ebdc7c4a3 100644 --- a/src/environment/global/moon_rotation.hpp +++ b/src/environment/global/moon_rotation.hpp @@ -13,6 +13,8 @@ class CelestialInformation; +namespace s2e::environment { + /** * @enum MoonRotationMode * @brief Definition of calculation mode of moon rotation @@ -62,4 +64,6 @@ class MoonRotation { const CelestialInformation &celestial_information_; //!< Celestial Information to get moon orbit }; +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_GLOBAL_MOON_ROTATION_HPP_ diff --git a/src/environment/global/physical_constants.hpp b/src/environment/global/physical_constants.hpp index c2acd7de0..2c279b389 100644 --- a/src/environment/global/physical_constants.hpp +++ b/src/environment/global/physical_constants.hpp @@ -8,7 +8,7 @@ #include // std::is_floating_point_v -namespace environment { +namespace s2e::environment { template using enable_if_float = std::enable_if_t, T>; @@ -39,7 +39,7 @@ DEFINE_PHYSICAL_CONSTANT(earth_flattening, 3.352797e-3L) / #undef DEFINE_PHYSICAL_CONSTANT -} // namespace environment +} // namespace s2e::environment inline double degC2K(double degC) { return (degC - environment::absolute_zero_degC); } inline double K2degC(double K) { return (K + environment::absolute_zero_degC); } diff --git a/src/environment/global/simulation_time.cpp b/src/environment/global/simulation_time.cpp index 149b88f42..e44d5a416 100644 --- a/src/environment/global/simulation_time.cpp +++ b/src/environment/global/simulation_time.cpp @@ -21,6 +21,8 @@ using namespace std; +namespace s2e::environment { + SimulationTime::SimulationTime(const double end_sec, const double step_sec, const double attitude_update_interval_sec, const double attitude_rk_step_sec, const double orbit_update_interval_sec, const double orbit_rk_step_sec, const double thermal_update_interval_sec, const double thermal_rk_step_sec, const double compo_propagate_step_sec, @@ -275,3 +277,5 @@ SimulationTime* InitSimulationTime(std::string file_name) { return simTime; } + +} // namespace s2e::environment diff --git a/src/environment/global/simulation_time.hpp b/src/environment/global/simulation_time.hpp index f993b5ec8..27892afdf 100644 --- a/src/environment/global/simulation_time.hpp +++ b/src/environment/global/simulation_time.hpp @@ -19,6 +19,8 @@ #include "math_physics/orbit/sgp4/sgp4io.h" #include "math_physics/orbit/sgp4/sgp4unit.h" +namespace s2e::environment { + /** *@struct TimeState *@brief State of timing controller @@ -334,4 +336,6 @@ class SimulationTime : public ILoggable { */ SimulationTime* InitSimulationTime(std::string file_name); +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_GLOBAL_SIMULATION_TIME_HPP_ diff --git a/src/environment/local/atmosphere.cpp b/src/environment/local/atmosphere.cpp index ae789fdb3..cfada183c 100644 --- a/src/environment/local/atmosphere.cpp +++ b/src/environment/local/atmosphere.cpp @@ -14,6 +14,8 @@ #include "math_physics/randomization/random_walk.hpp" #include "setting_file_reader/initialize_file_access.hpp" +namespace s2e::environment { + Atmosphere::Atmosphere(const std::string model, const std::string space_weather_file_name, const double gauss_standard_deviation_rate, const bool is_manual_param, const double manual_f107, const double manual_f107a, const double manual_ap, const LocalCelestialInformation* local_celestial_information, const SimulationTime* simulation_time) @@ -133,3 +135,5 @@ Atmosphere InitAtmosphere(const std::string initialize_file_path, const LocalCel return atmosphere; } + +} // namespace s2e::environment diff --git a/src/environment/local/atmosphere.hpp b/src/environment/local/atmosphere.hpp index 8489891d8..2c383b78c 100644 --- a/src/environment/local/atmosphere.hpp +++ b/src/environment/local/atmosphere.hpp @@ -15,6 +15,8 @@ #include "math_physics/atmosphere/wrapper_nrlmsise00.hpp" #include "math_physics/math/vector.hpp" +namespace s2e::environment { + /** * @class Atmosphere * @brief Class to calculate earth's atmospheric density @@ -116,4 +118,6 @@ class Atmosphere : public ILoggable { Atmosphere InitAtmosphere(const std::string initialize_file_path, const LocalCelestialInformation* local_celestial_information, const SimulationTime* simulation_time); +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_LOCAL_ATMOSPHERE_HPP_ diff --git a/src/environment/local/earth_albedo.cpp b/src/environment/local/earth_albedo.cpp index 5aca6b575..fb3672a8e 100644 --- a/src/environment/local/earth_albedo.cpp +++ b/src/environment/local/earth_albedo.cpp @@ -13,6 +13,8 @@ #include "math_physics/math/vector.hpp" #include "setting_file_reader/initialize_file_access.hpp" +namespace s2e::environment { + EarthAlbedo::EarthAlbedo(LocalCelestialInformation* local_celestial_information, SolarRadiationPressureEnvironment* srp_environment) : local_celestial_information_(local_celestial_information), srp_environment_(srp_environment) {} @@ -57,3 +59,5 @@ EarthAlbedo InitEarthAlbedo(std::string initialize_file_path, LocalCelestialInfo return earth_albedo; } + +} // namespace s2e::environment diff --git a/src/environment/local/earth_albedo.hpp b/src/environment/local/earth_albedo.hpp index e07ad7b46..724cba2a7 100644 --- a/src/environment/local/earth_albedo.hpp +++ b/src/environment/local/earth_albedo.hpp @@ -10,6 +10,8 @@ #include "environment/local/local_celestial_information.hpp" #include "solar_radiation_pressure_environment.hpp" +namespace s2e::environment { + /** * @class EarthAlbedo * @brief Class to calculate Solar Radiation Pressure @@ -102,4 +104,6 @@ class EarthAlbedo : public ILoggable { EarthAlbedo InitEarthAlbedo(std::string initialize_file_path, LocalCelestialInformation* local_celestial_information, SolarRadiationPressureEnvironment* srp_environment); +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_LOCAL_EARTH_ALBEDO_HPP_ diff --git a/src/environment/local/geomagnetic_field.cpp b/src/environment/local/geomagnetic_field.cpp index 48cdc57c0..008f3277c 100644 --- a/src/environment/local/geomagnetic_field.cpp +++ b/src/environment/local/geomagnetic_field.cpp @@ -11,6 +11,8 @@ #include "math_physics/randomization/random_walk.hpp" #include "setting_file_reader/initialize_file_access.hpp" +namespace s2e::environment { + GeomagneticField::GeomagneticField(const std::string igrf_file_name, const double random_walk_srandard_deviation_nT, const double random_walk_limit_nT, const double white_noise_standard_deviation_nT) : magnetic_field_i_nT_(0.0), @@ -85,3 +87,5 @@ GeomagneticField InitGeomagneticField(std::string initialize_file_path) { return geomagnetic_field; } + +} // namespace s2e::environment diff --git a/src/environment/local/geomagnetic_field.hpp b/src/environment/local/geomagnetic_field.hpp index 3f2adf741..100baf480 100644 --- a/src/environment/local/geomagnetic_field.hpp +++ b/src/environment/local/geomagnetic_field.hpp @@ -11,6 +11,8 @@ #include "math_physics/math/quaternion.hpp" #include "math_physics/math/vector.hpp" +namespace s2e::environment { + /** * @class GeomagneticField * @brief Class to calculate magnetic field of the earth @@ -92,4 +94,6 @@ class GeomagneticField : public ILoggable { */ GeomagneticField InitGeomagneticField(std::string initialize_file_path); +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_LOCAL_GEOMAGNETIC_FIELD_HPP_ diff --git a/src/environment/local/local_celestial_information.cpp b/src/environment/local/local_celestial_information.cpp index b34fcc6ed..fde483b84 100644 --- a/src/environment/local/local_celestial_information.cpp +++ b/src/environment/local/local_celestial_information.cpp @@ -14,6 +14,8 @@ #include "logger/log_utility.hpp" +namespace s2e::environment { + LocalCelestialInformation::LocalCelestialInformation(const CelestialInformation* global_celestial_information) : global_celestial_information_(global_celestial_information) { int num_of_state = global_celestial_information_->GetNumberOfSelectedBodies() * 3; @@ -197,3 +199,5 @@ std::string LocalCelestialInformation::GetLogValue() const { } return str_tmp; } + +} // namespace s2e::environment diff --git a/src/environment/local/local_celestial_information.hpp b/src/environment/local/local_celestial_information.hpp index ec3a6cbb3..26e1c354b 100644 --- a/src/environment/local/local_celestial_information.hpp +++ b/src/environment/local/local_celestial_information.hpp @@ -8,6 +8,8 @@ #include "../global/celestial_information.hpp" +namespace s2e::environment { + /** * @class LocalCelestialInformation * @brief Class to manage celestial body information in the spacecraft body frame @@ -120,4 +122,6 @@ class LocalCelestialInformation : public ILoggable { const s2e::math::Vector<3> angular_velocity_b); }; +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_LOCAL_LOCAL_CELESTIAL_INFORMATION_HPP_ diff --git a/src/environment/local/local_environment.cpp b/src/environment/local/local_environment.cpp index 7cd898db9..d12f5e882 100644 --- a/src/environment/local/local_environment.cpp +++ b/src/environment/local/local_environment.cpp @@ -8,6 +8,8 @@ #include "dynamics/orbit/orbit.hpp" #include "setting_file_reader/initialize_file_access.hpp" +namespace s2e::environment + LocalEnvironment::LocalEnvironment(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id) { Initialize(simulation_configuration, global_environment, spacecraft_id); @@ -76,3 +78,5 @@ void LocalEnvironment::LogSetup(Logger& logger) { logger.AddLogList(atmosphere_); logger.AddLogList(celestial_information_); } + +} // namespace s2e::environment diff --git a/src/environment/local/local_environment.hpp b/src/environment/local/local_environment.hpp index a7d0b7cac..807ffa100 100644 --- a/src/environment/local/local_environment.hpp +++ b/src/environment/local/local_environment.hpp @@ -17,6 +17,8 @@ class Dynamics; +namespace s2e::environment { + /** * @class LocalEnvironment * @brief Class to manage local environments @@ -94,4 +96,6 @@ class LocalEnvironment { void Initialize(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id); }; +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_LOCAL_LOCAL_ENVIRONMENT_HPP_ diff --git a/src/environment/local/solar_radiation_pressure_environment.cpp b/src/environment/local/solar_radiation_pressure_environment.cpp index abda33801..1ab74ebdc 100644 --- a/src/environment/local/solar_radiation_pressure_environment.cpp +++ b/src/environment/local/solar_radiation_pressure_environment.cpp @@ -13,6 +13,8 @@ #include "math_physics/math/vector.hpp" #include "setting_file_reader/initialize_file_access.hpp" +namespace s2e::environment { + SolarRadiationPressureEnvironment::SolarRadiationPressureEnvironment(LocalCelestialInformation* local_celestial_information) : local_celestial_information_(local_celestial_information) { solar_radiation_pressure_N_m2_ = solar_constant_W_m2_ / environment::speed_of_light_m_s; @@ -119,3 +121,5 @@ SolarRadiationPressureEnvironment InitSolarRadiationPressureEnvironment(std::str return srp_env; } + +} // namespace s2e::environment diff --git a/src/environment/local/solar_radiation_pressure_environment.hpp b/src/environment/local/solar_radiation_pressure_environment.hpp index 8fbc73250..a768ce136 100644 --- a/src/environment/local/solar_radiation_pressure_environment.hpp +++ b/src/environment/local/solar_radiation_pressure_environment.hpp @@ -9,6 +9,8 @@ #include "environment/global/physical_constants.hpp" #include "environment/local/local_celestial_information.hpp" +namespace s2e::environment { + /** * @class SolarRadiationPressureEnvironment * @brief Class to calculate Solar Radiation Pressure @@ -122,4 +124,6 @@ class SolarRadiationPressureEnvironment : public ILoggable { SolarRadiationPressureEnvironment InitSolarRadiationPressureEnvironment(std::string initialize_file_path, LocalCelestialInformation* local_celestial_information); +} // namespace s2e::environment + #endif // S2E_ENVIRONMENT_LOCAL_SOLAR_RADIATION_PRESSURE_ENVIRONMENT_HPP_ From 1f847517244b106c3a48244b72d3969eedf8b5c6 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:38:17 +0900 Subject: [PATCH 25/49] Add s2e::logger namespace --- src/logger/initialize_log.cpp | 4 ++++ src/logger/initialize_log.hpp | 4 ++++ src/logger/log_utility.hpp | 4 ++++ src/logger/loggable.hpp | 4 ++++ src/logger/logger.cpp | 4 ++++ src/logger/logger.hpp | 4 ++++ 6 files changed, 24 insertions(+) diff --git a/src/logger/initialize_log.cpp b/src/logger/initialize_log.cpp index 239cfe3d4..3dec812af 100644 --- a/src/logger/initialize_log.cpp +++ b/src/logger/initialize_log.cpp @@ -7,6 +7,8 @@ #include "../setting_file_reader/initialize_file_access.hpp" +namespace s2e::logger { + Logger* InitLog(std::string file_name) { IniAccess ini_file(file_name); @@ -28,3 +30,5 @@ Logger* InitMonteCarloLog(std::string file_name, bool enable) { return log; } + +} // namespace s2e::logger diff --git a/src/logger/initialize_log.hpp b/src/logger/initialize_log.hpp index 0d70fc1e8..630425134 100644 --- a/src/logger/initialize_log.hpp +++ b/src/logger/initialize_log.hpp @@ -8,6 +8,8 @@ #include +namespace s2e::logger { + /** * @fn InitLog * @brief Initialize normal logger (default.csv) @@ -23,4 +25,6 @@ Logger* InitLog(std::string file_name); */ Logger* InitMonteCarloLog(std::string file_name, bool enable); +} // namespace s2e::logger + #endif // S2E_LIBRARY_LOGGER_INITIALIZE_LOG_HPP_ diff --git a/src/logger/log_utility.hpp b/src/logger/log_utility.hpp index 96ccf3c31..54599ee07 100644 --- a/src/logger/log_utility.hpp +++ b/src/logger/log_utility.hpp @@ -12,6 +12,8 @@ #include #include +namespace s2e::logger { + /** * @fn WriteScalar * @brief Write scalar value @@ -156,4 +158,6 @@ std::string WriteQuaternion(const std::string name, const std::string frame) { return str_tmp.str(); } +} // namespace s2e::logger + #endif // S2E_LIBRARY_LOGGER_LOG_UTILITY_HPP_ diff --git a/src/logger/loggable.hpp b/src/logger/loggable.hpp index ab1b22a8b..064bbf952 100644 --- a/src/logger/loggable.hpp +++ b/src/logger/loggable.hpp @@ -10,6 +10,8 @@ #include "log_utility.hpp" // This is not necessary but include here for convenience +namespace s2e::logger { + /** * @class ILoggable * @brief Abstract class to manage logging @@ -34,4 +36,6 @@ class ILoggable { bool is_log_enabled_ = true; //!< Log enable flag }; +} // namespace s2e::logger + #endif // S2E_LIBRARY_LOGGER_LOGGABLE_HPP_ diff --git a/src/logger/logger.cpp b/src/logger/logger.cpp index f4795d537..0469f38e9 100644 --- a/src/logger/logger.cpp +++ b/src/logger/logger.cpp @@ -13,6 +13,8 @@ bool Logger::is_directory_created_ = false; namespace fs = std::filesystem; +namespace s2e::logger { + Logger::Logger(const std::string &file_name, const fs::path &data_path, const fs::path &ini_file_name, const bool is_ini_save_enabled, const bool is_enabled) : is_enabled_(is_enabled), is_ini_save_enabled_(is_ini_save_enabled) { @@ -103,3 +105,5 @@ void Logger::CopyFileToLogDirectory(const fs::path &ini_file_name) { fs::copy_file(ini_file_name, to_file_name); return; } + +} // namespace s2e::logger diff --git a/src/logger/logger.hpp b/src/logger/logger.hpp index e9ac5b1f0..fa85e21ed 100644 --- a/src/logger/logger.hpp +++ b/src/logger/logger.hpp @@ -15,6 +15,8 @@ #include "loggable.hpp" +namespace s2e::logger { + /** * @class Logger * @brief Class to manage log output file @@ -121,4 +123,6 @@ class Logger { std::filesystem::path CreateDirectory(const std::filesystem::path &data_path, const std::string &time); }; +} // namespace s2e::logger + #endif // S2E_LIBRARY_LOGGER_LOGGER_HPP_ From fdf93e2c24e0b62aea51907637aabb3ffdb31c14 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:41:07 +0900 Subject: [PATCH 26/49] Add s2e::setting_file_reader namespace --- src/setting_file_reader/c2a_command_database.cpp | 4 ++++ src/setting_file_reader/c2a_command_database.hpp | 4 ++++ src/setting_file_reader/initialize_file_access.cpp | 4 ++++ src/setting_file_reader/initialize_file_access.hpp | 4 ++++ src/setting_file_reader/wings_operation_file.cpp | 4 ++++ src/setting_file_reader/wings_operation_file.hpp | 4 ++++ 6 files changed, 24 insertions(+) diff --git a/src/setting_file_reader/c2a_command_database.cpp b/src/setting_file_reader/c2a_command_database.cpp index 650c11cf5..72e8bdf18 100644 --- a/src/setting_file_reader/c2a_command_database.cpp +++ b/src/setting_file_reader/c2a_command_database.cpp @@ -11,6 +11,8 @@ #include #include +namespace s2e::setting_file_reader { + C2aCommandInformation::C2aCommandInformation(const std::string cmd_db_line) { if (cmd_db_line.find("*") == 0) return; if (cmd_db_line.find(",") != 0) return; @@ -158,3 +160,5 @@ void DecodeC2aCommandArgument(const C2aArgumentType type, const std::string argu break; } } + +} // namespace s2e::setting_file_reader diff --git a/src/setting_file_reader/c2a_command_database.hpp b/src/setting_file_reader/c2a_command_database.hpp index 9fd2e9125..fce12028f 100644 --- a/src/setting_file_reader/c2a_command_database.hpp +++ b/src/setting_file_reader/c2a_command_database.hpp @@ -12,6 +12,8 @@ #include #include +namespace s2e::setting_file_reader { + /** * @enum C2aArgumentType * @brief Argument type used in C2A command @@ -115,4 +117,6 @@ class C2aCommandDatabase { */ void DecodeC2aCommandArgument(const C2aArgumentType type, const std::string argument_string, uint8_t* param, size_t& size_param); +} // namespace s2e::setting_file_reader + #endif // S2E_LIBRARY_INITIALIZE_C2A_COMMAND_DATABASE_HPP_ diff --git a/src/setting_file_reader/initialize_file_access.cpp b/src/setting_file_reader/initialize_file_access.cpp index 565a66f98..9397057ad 100644 --- a/src/setting_file_reader/initialize_file_access.cpp +++ b/src/setting_file_reader/initialize_file_access.cpp @@ -14,6 +14,8 @@ #include "../utilities/macros.hpp" +namespace s2e::setting_file_reader { + #ifdef WIN32 IniAccess::IniAccess(const std::string file_path) : file_path_(file_path) { // strcpy_s(file_path_char_, (size_t)_countof(file_path_char_), file_path_.c_str()); @@ -283,3 +285,5 @@ void IniAccess::ReadCsvString(std::vector>& output_valu output_value.push_back(temp); } } + +} // namespace s2e::setting_file_reader diff --git a/src/setting_file_reader/initialize_file_access.hpp b/src/setting_file_reader/initialize_file_access.hpp index 06bbc2866..48ff1acd1 100644 --- a/src/setting_file_reader/initialize_file_access.hpp +++ b/src/setting_file_reader/initialize_file_access.hpp @@ -27,6 +27,8 @@ #include #include +namespace s2e::setting_file_reader { + /** * @class IniAccess * @brief Class to read and get parameters for the `ini` format file @@ -216,4 +218,6 @@ void IniAccess::ReadVector(const char* section_name, const char* key_name, s2e:: } } +} // namespace s2e::setting_file_reader + #endif // S2E_LIBRARY_INITIALIZE_INITIALIZE_FILE_ACCESS_HPP_ diff --git a/src/setting_file_reader/wings_operation_file.cpp b/src/setting_file_reader/wings_operation_file.cpp index 628c22fcf..4b9b56d0a 100644 --- a/src/setting_file_reader/wings_operation_file.cpp +++ b/src/setting_file_reader/wings_operation_file.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::setting_file_reader { + WingsOperationFile::WingsOperationFile(const std::string file_path) { // File open std::ifstream file(file_path); @@ -45,3 +47,5 @@ std::string WingsOperationFile::GetLatestLine() { return line; } + +} // namespace s2e::setting_file_reader diff --git a/src/setting_file_reader/wings_operation_file.hpp b/src/setting_file_reader/wings_operation_file.hpp index 11cb2a9b0..848010aac 100644 --- a/src/setting_file_reader/wings_operation_file.hpp +++ b/src/setting_file_reader/wings_operation_file.hpp @@ -11,6 +11,8 @@ #include "c2a_command_database.hpp" +namespace s2e::setting_file_reader { + /** * @class WingsOperationFile * @brief A class to handle WINGS operation file @@ -35,4 +37,6 @@ class WingsOperationFile { size_t line_pointer_ = 0; //!< Line pointer }; +} // namespace s2e::setting_file_reader + #endif // S2E_LIBRARY_INITIALIZE_WINGS_OPERATION_FILE_HPP_ From a4adecd467c03114842db80489ad53474d5becbc Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:49:04 +0900 Subject: [PATCH 27/49] Add s2e::simulation namespace --- src/simulation/case/simulation_case.cpp | 6 +++++- src/simulation/case/simulation_case.hpp | 4 ++++ src/simulation/ground_station/ground_station.cpp | 4 ++++ src/simulation/ground_station/ground_station.hpp | 4 ++++ src/simulation/hils/hils_port_manager.cpp | 4 ++++ src/simulation/hils/hils_port_manager.hpp | 4 ++++ .../initialize_monte_carlo_parameters.cpp | 4 ++++ .../initialize_monte_carlo_parameters.hpp | 4 ++++ .../initialize_monte_carlo_simulation.cpp | 4 ++++ .../initialize_monte_carlo_simulation.hpp | 4 ++++ .../monte_carlo_simulation_executor.cpp | 4 ++++ .../monte_carlo_simulation_executor.hpp | 4 ++++ src/simulation/monte_carlo_simulation/simulation_object.cpp | 4 ++++ src/simulation/monte_carlo_simulation/simulation_object.hpp | 4 ++++ .../multiple_spacecraft/inter_spacecraft_communication.cpp | 4 ++++ .../multiple_spacecraft/inter_spacecraft_communication.hpp | 4 ++++ src/simulation/multiple_spacecraft/relative_information.cpp | 4 ++++ src/simulation/multiple_spacecraft/relative_information.hpp | 4 ++++ src/simulation/simulation_configuration.hpp | 4 ++++ src/simulation/spacecraft/installed_components.cpp | 4 ++++ src/simulation/spacecraft/installed_components.hpp | 4 ++++ src/simulation/spacecraft/spacecraft.cpp | 4 ++++ src/simulation/spacecraft/spacecraft.hpp | 4 ++++ .../spacecraft/structure/initialize_structure.cpp | 4 ++++ .../spacecraft/structure/initialize_structure.hpp | 4 ++++ .../spacecraft/structure/kinematics_parameters.cpp | 6 +++++- .../spacecraft/structure/kinematics_parameters.hpp | 4 ++++ .../spacecraft/structure/residual_magnetic_moment.cpp | 4 ++++ .../spacecraft/structure/residual_magnetic_moment.hpp | 4 ++++ src/simulation/spacecraft/structure/structure.cpp | 4 ++++ src/simulation/spacecraft/structure/structure.hpp | 4 ++++ src/simulation/spacecraft/structure/surface.cpp | 4 ++++ src/simulation/spacecraft/structure/surface.hpp | 4 ++++ 33 files changed, 134 insertions(+), 2 deletions(-) diff --git a/src/simulation/case/simulation_case.cpp b/src/simulation/case/simulation_case.cpp index 1a19dd003..f9e869937 100644 --- a/src/simulation/case/simulation_case.cpp +++ b/src/simulation/case/simulation_case.cpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::simulation { + SimulationCase::SimulationCase(const std::string initialize_base_file) { // Initialize Log simulation_configuration_.main_logger_ = InitLog(initialize_base_file); @@ -104,4 +106,6 @@ void SimulationCase::InitializeSimulationConfiguration(const std::string initial // Global Environment global_environment_ = new GlobalEnvironment(&simulation_configuration_); global_environment_->LogSetup(*(simulation_configuration_.main_logger_)); -} \ No newline at end of file +} + +} // namespace s2e::simulation diff --git a/src/simulation/case/simulation_case.hpp b/src/simulation/case/simulation_case.hpp index 68f73f66c..53d37f8a9 100644 --- a/src/simulation/case/simulation_case.hpp +++ b/src/simulation/case/simulation_case.hpp @@ -13,6 +13,8 @@ #include "../simulation_configuration.hpp" class Logger; +namespace s2e::simulation { + /** * @class SimulationCase * @brief Base class to define simulation scenario @@ -98,4 +100,6 @@ class SimulationCase : public ILoggable { virtual void UpdateTargetObjects() = 0; }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_CASE_SIMULATION_CASE_HPP_ diff --git a/src/simulation/ground_station/ground_station.cpp b/src/simulation/ground_station/ground_station.cpp index 4a0110fc4..12ee9ae4a 100644 --- a/src/simulation/ground_station/ground_station.cpp +++ b/src/simulation/ground_station/ground_station.cpp @@ -13,6 +13,8 @@ #include #include +namespace s2e::simulation { + GroundStation::GroundStation(const SimulationConfiguration* configuration, const unsigned int ground_station_id) : ground_station_id_(ground_station_id) { Initialize(configuration, ground_station_id_); @@ -90,3 +92,5 @@ std::string GroundStation::GetLogValue() const { str_tmp += WriteVector(position_i_m_); return str_tmp; } + +} // namespace s2e::simulation diff --git a/src/simulation/ground_station/ground_station.hpp b/src/simulation/ground_station/ground_station.hpp index bdb320af9..42a3b8cac 100644 --- a/src/simulation/ground_station/ground_station.hpp +++ b/src/simulation/ground_station/ground_station.hpp @@ -12,6 +12,8 @@ #include "../simulation_configuration.hpp" +namespace s2e::simulation { + /** * @class GroundStation * @brief Base class of ground station @@ -109,4 +111,6 @@ class GroundStation : public ILoggable { bool CalcIsVisible(const Vector<3> spacecraft_position_ecef_m); }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_GROUND_STATION_GROUND_STATION_HPP_ diff --git a/src/simulation/hils/hils_port_manager.cpp b/src/simulation/hils/hils_port_manager.cpp index 40e4d36fe..4eda6559f 100644 --- a/src/simulation/hils/hils_port_manager.cpp +++ b/src/simulation/hils/hils_port_manager.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::simulation { + // #define HILS_PORT_MANAGER_SHOW_DEBUG_DATA HilsPortManager::HilsPortManager() {} @@ -258,3 +260,5 @@ int HilsPortManager::I2cControllerReceive(unsigned int port_id, unsigned char* b int HilsPortManager::I2cControllerSend(unsigned int port_id, const unsigned char* buffer, int offset, int length) { return UartSend(port_id, buffer, offset, length); } + +} // namespace s2e::simulation diff --git a/src/simulation/hils/hils_port_manager.hpp b/src/simulation/hils/hils_port_manager.hpp index 9fbeaf2c7..06d00b2f7 100644 --- a/src/simulation/hils/hils_port_manager.hpp +++ b/src/simulation/hils/hils_port_manager.hpp @@ -12,6 +12,8 @@ #endif #include +namespace s2e::simulation { + /** * @class HilsPortManager * @brief Class to manage COM ports for HILS test @@ -168,4 +170,6 @@ class HilsPortManager { #endif }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_HILS_HILS_PORT_MANAGER_HPP_ diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp index 8ffaa5ad7..0020cffc5 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp @@ -9,6 +9,8 @@ using namespace std; +namespace s2e::simulation { + random_device InitializedMonteCarloParameters::randomizer_; mt19937 InitializedMonteCarloParameters::mt_; uniform_real_distribution<>* InitializedMonteCarloParameters::uniform_distribution_; @@ -315,3 +317,5 @@ void InitializedMonteCarloParameters::GenerateQuaternionNormal() { randomized_value_.push_back(temp_q[i]); } } + +} // namespace s2e::simulation diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp index 28159c98c..1570aaa1a 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp @@ -13,6 +13,8 @@ #include #include +namespace s2e::simulation { + /** * @class InitializedMonteCarloParameters * @brief Initialized parameters for Monte-Carlo simulation @@ -232,4 +234,6 @@ void InitializedMonteCarloParameters::GetRandomizedVector(s2e::math::Vector #include +namespace s2e::simulation { + #define MAX_CHAR_NUM 256 MonteCarloSimulationExecutor* InitMonteCarloSimulation(std::string file_name) { @@ -93,3 +95,5 @@ MonteCarloSimulationExecutor* InitMonteCarloSimulation(std::string file_name) { return monte_carlo_simulator; } + +} // namespace s2e::simulation diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.hpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.hpp index e4dcd60a2..fd7678cd4 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.hpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.hpp @@ -9,10 +9,14 @@ #include "initialize_monte_carlo_parameters.hpp" #include "monte_carlo_simulation_executor.hpp" +namespace s2e::simulation { + /** * @fn InitMonteCarloSimulation * @brief Initialize function for Monte-Carlo Simulator */ MonteCarloSimulationExecutor* InitMonteCarloSimulation(std::string file_name); +} // namespace s2e::simulation + #endif // S2E_SIMULATION_MONTE_CARLO_SIMULATION_INITIALIZE_MONTE_CARLO_SIMULATION_HPP_ diff --git a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp index 1661c6e3a..dda7abfd0 100644 --- a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp +++ b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp @@ -7,6 +7,8 @@ using std::string; +namespace s2e::simulation { + MonteCarloSimulationExecutor::MonteCarloSimulationExecutor(unsigned long long total_num_of_executions) : total_number_of_executions_(total_num_of_executions) { number_of_executions_done_ = 0; @@ -69,3 +71,5 @@ void MonteCarloSimulationExecutor::RandomizeAllParameters() { void MonteCarloSimulationExecutor::SetSeed(unsigned long seed, bool is_deterministic) { InitializedMonteCarloParameters::SetSeed(seed, is_deterministic); } + +} // namespace s2e::simulation diff --git a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp index 7209b38a0..97626e47e 100644 --- a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp +++ b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp @@ -12,6 +12,8 @@ // #include "simulation_object.hpp" #include "initialize_monte_carlo_parameters.hpp" +namespace s2e::simulation { + /** * @class MonteCarloSimulationExecutor * @brief Monte-Carlo Simulation Executor class @@ -166,4 +168,6 @@ void MonteCarloSimulationExecutor::AddInitializedMonteCarloParameter(std::string } } +} // namespace s2e::simulation + #endif // S2E_SIMULATION_MONTE_CARLO_SIMULATION_MONTE_CARLO_SIMULATION_EXECUTOR_HPP_ diff --git a/src/simulation/monte_carlo_simulation/simulation_object.cpp b/src/simulation/monte_carlo_simulation/simulation_object.cpp index a5b1e7b51..0f41bf32d 100644 --- a/src/simulation/monte_carlo_simulation/simulation_object.cpp +++ b/src/simulation/monte_carlo_simulation/simulation_object.cpp @@ -5,6 +5,8 @@ #include "simulation_object.hpp" +namespace s2e::simulation { + std::map SimulationObject::object_list_; SimulationObject::SimulationObject(std::string name) : name_(name) { @@ -41,3 +43,5 @@ void SimulationObject::GetInitializedMonteCarloParameterQuaternion(const MonteCa std::string init_monte_carlo_parameter_name, s2e::math::Quaternion& destination) const { monte_carlo_simulator.GetInitializedMonteCarloParameterQuaternion(name_, init_monte_carlo_parameter_name, destination); } + +} // namespace s2e::simulation diff --git a/src/simulation/monte_carlo_simulation/simulation_object.hpp b/src/simulation/monte_carlo_simulation/simulation_object.hpp index 5a16163ea..cb1c2df09 100644 --- a/src/simulation/monte_carlo_simulation/simulation_object.hpp +++ b/src/simulation/monte_carlo_simulation/simulation_object.hpp @@ -16,6 +16,8 @@ #include "initialize_monte_carlo_parameters.hpp" #include "monte_carlo_simulation_executor.hpp" +namespace s2e::simulation { + /** * @class SimulationObject * @brief Class to manage randomization of variables for Monte-Carlo simulation @@ -84,4 +86,6 @@ void SimulationObject::GetInitializedMonteCarloParameterVector(const MonteCarloS monte_carlo_simulator.GetInitializedMonteCarloParameterVector(name_, init_monte_carlo_parameter_name, destination); } +} // namespace s2e::simulation + #endif // S2E_SIMULATION_MONTE_CARLO_SIMULATION_SIMULATION_OBJECT_HPP_ diff --git a/src/simulation/multiple_spacecraft/inter_spacecraft_communication.cpp b/src/simulation/multiple_spacecraft/inter_spacecraft_communication.cpp index 0324d2e2e..1fd56c363 100644 --- a/src/simulation/multiple_spacecraft/inter_spacecraft_communication.cpp +++ b/src/simulation/multiple_spacecraft/inter_spacecraft_communication.cpp @@ -7,8 +7,12 @@ #include +namespace s2e::simulation { + InterSpacecraftCommunication::InterSpacecraftCommunication(const SimulationConfiguration* simulation_configuration) { UNUSED(simulation_configuration); } InterSpacecraftCommunication::~InterSpacecraftCommunication() {} + +} // namespace s2e::simulation diff --git a/src/simulation/multiple_spacecraft/inter_spacecraft_communication.hpp b/src/simulation/multiple_spacecraft/inter_spacecraft_communication.hpp index 11b32008e..f667d60c9 100644 --- a/src/simulation/multiple_spacecraft/inter_spacecraft_communication.hpp +++ b/src/simulation/multiple_spacecraft/inter_spacecraft_communication.hpp @@ -8,6 +8,8 @@ #include "../simulation_configuration.hpp" +namespace s2e::simulation { + /** * @class InterSpacecraftCommunication * @brief Base class of inter satellite communication @@ -28,4 +30,6 @@ class InterSpacecraftCommunication { private: }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_MULTIPLE_SPACECRAFT_INTER_SPACECRAFT_COMMUNICATION_HPP_ diff --git a/src/simulation/multiple_spacecraft/relative_information.cpp b/src/simulation/multiple_spacecraft/relative_information.cpp index 236a9ff19..b5c85d32d 100644 --- a/src/simulation/multiple_spacecraft/relative_information.cpp +++ b/src/simulation/multiple_spacecraft/relative_information.cpp @@ -5,6 +5,8 @@ #include "relative_information.hpp" +namespace s2e::simulation { + RelativeInformation::RelativeInformation() {} RelativeInformation::~RelativeInformation() {} @@ -163,3 +165,5 @@ void RelativeInformation::ResizeLists() { relative_velocity_list_rtn_m_s_.assign(size, std::vector>(size, s2e::math::Vector<3>(0))); relative_attitude_quaternion_list_.assign(size, std::vector(size, s2e::math::Quaternion(0, 0, 0, 1))); } + +} // namespace s2e::simulation diff --git a/src/simulation/multiple_spacecraft/relative_information.hpp b/src/simulation/multiple_spacecraft/relative_information.hpp index 2eb7e7789..a5cbe1630 100644 --- a/src/simulation/multiple_spacecraft/relative_information.hpp +++ b/src/simulation/multiple_spacecraft/relative_information.hpp @@ -12,6 +12,8 @@ #include "../../logger/loggable.hpp" #include "../../logger/logger.hpp" +namespace s2e::simulation { + /** * @class RelativeInformation * @brief Base class to manage relative information between spacecraft @@ -170,4 +172,6 @@ class RelativeInformation : public ILoggable { void ResizeLists(); }; +} // namespace s2e::simulation + #endif // S2E_MULTIPLE_SPACECRAFT_RELATIVE_INFORMATION_HPP_ diff --git a/src/simulation/simulation_configuration.hpp b/src/simulation/simulation_configuration.hpp index 28b6f74f0..599b6ec4e 100644 --- a/src/simulation/simulation_configuration.hpp +++ b/src/simulation/simulation_configuration.hpp @@ -11,6 +11,8 @@ #include "../logger/logger.hpp" +namespace s2e::simulation { + /** * @struct SimulationConfiguration * @brief Simulation setting information @@ -35,4 +37,6 @@ struct SimulationConfiguration { ~SimulationConfiguration() { delete main_logger_; } }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_SIMULATION_CONFIGURATION_HPP_ diff --git a/src/simulation/spacecraft/installed_components.cpp b/src/simulation/spacecraft/installed_components.cpp index 6dcb6ee8c..ff7ce9b5c 100644 --- a/src/simulation/spacecraft/installed_components.cpp +++ b/src/simulation/spacecraft/installed_components.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::simulation { + s2e::math::Vector<3> InstalledComponents::GenerateForce_b_N() { s2e::math::Vector<3> force_b_N_(0.0); return force_b_N_; @@ -18,3 +20,5 @@ s2e::math::Vector<3> InstalledComponents::GenerateTorque_b_Nm() { } void InstalledComponents::LogSetup(Logger& logger) { UNUSED(logger); } + +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/installed_components.hpp b/src/simulation/spacecraft/installed_components.hpp index daf41a7a8..6e5a3c61c 100644 --- a/src/simulation/spacecraft/installed_components.hpp +++ b/src/simulation/spacecraft/installed_components.hpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::simulation { + /** * @class InstalledComponents * @brief Base class to express components list installed on a spacecraft @@ -49,4 +51,6 @@ class InstalledComponents { virtual void LogSetup(Logger& logger); }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_SPACECRAFT_INSTALLED_COMPONENTS_HPP_ diff --git a/src/simulation/spacecraft/spacecraft.cpp b/src/simulation/spacecraft/spacecraft.cpp index 61a7762d4..c80950dd5 100644 --- a/src/simulation/spacecraft/spacecraft.cpp +++ b/src/simulation/spacecraft/spacecraft.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::simulation { + Spacecraft::Spacecraft(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id, RelativeInformation* relative_information) : spacecraft_id_(spacecraft_id) { @@ -74,3 +76,5 @@ void Spacecraft::Update(const SimulationTime* simulation_time) { } void Spacecraft::Clear(void) { dynamics_->ClearForceTorque(); } + +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/spacecraft.hpp b/src/simulation/spacecraft/spacecraft.hpp index 2d9552f24..a59ea4eaa 100644 --- a/src/simulation/spacecraft/spacecraft.hpp +++ b/src/simulation/spacecraft/spacecraft.hpp @@ -15,6 +15,8 @@ #include "installed_components.hpp" #include "structure/structure.hpp" +namespace s2e::simulation { + /** * @class Spacecraft * @brief Base class to express Spacecraft @@ -107,4 +109,6 @@ class Spacecraft { const unsigned int spacecraft_id_; //!< ID of the spacecraft }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_SPACECRAFT_SPACECRAFT_HPP_ diff --git a/src/simulation/spacecraft/structure/initialize_structure.cpp b/src/simulation/spacecraft/structure/initialize_structure.cpp index b8e578398..543203f75 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.cpp +++ b/src/simulation/spacecraft/structure/initialize_structure.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::simulation { + #define MIN_VAL 1e-6 KinematicsParameters InitKinematicsParameters(std::string file_name) { auto conf = IniAccess(file_name); @@ -119,3 +121,5 @@ ResidualMagneticMoment InitResidualMagneticMoment(std::string file_name) { ResidualMagneticMoment rmm_params(rmm_const_b, rmm_rwdev, random_walk_limit_Am2, random_noise_standard_deviation_Am2); return rmm_params; } + +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/initialize_structure.hpp b/src/simulation/spacecraft/structure/initialize_structure.hpp index 59311219a..53065ff2e 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.hpp +++ b/src/simulation/spacecraft/structure/initialize_structure.hpp @@ -10,6 +10,8 @@ #include +namespace s2e::simulation { + /** * @fn InitKinematicsParameters * @brief Initialize the kinematics parameters with an ini file @@ -26,4 +28,6 @@ std::vector InitSurfaces(std::string file_name); */ ResidualMagneticMoment InitResidualMagneticMoment(std::string file_name); +} // namespace s2e::simulation + #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_INITIALIZE_STRUCTURE_HPP_ diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.cpp b/src/simulation/spacecraft/structure/kinematics_parameters.cpp index 289c62cc7..9318c92e7 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.cpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.cpp @@ -5,5 +5,9 @@ #include "kinematics_parameters.hpp" +namespace s2e::simulation { + KinematicsParameters::KinematicsParameters(s2e::math::Vector<3> center_of_gravity_b_m, double mass_kg, s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2) - : center_of_gravity_b_m_(center_of_gravity_b_m), mass_kg_(mass_kg), inertia_tensor_b_kgm2_(inertia_tensor_b_kgm2) {} \ No newline at end of file + : center_of_gravity_b_m_(center_of_gravity_b_m), mass_kg_(mass_kg), inertia_tensor_b_kgm2_(inertia_tensor_b_kgm2) {} + +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.hpp b/src/simulation/spacecraft/structure/kinematics_parameters.hpp index 6cea84b20..c7d3db504 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.hpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.hpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::simulation { + /** * @class KinematicsParameters * @brief Class for spacecraft Kinematics information @@ -86,4 +88,6 @@ class KinematicsParameters { s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2_; //!< Inertia tensor at body frame [kgm2] }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_KINEMATICS_PARAMETERS_HPP_ diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp index 0fe90e09f..f88ef8b39 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp @@ -5,9 +5,13 @@ #include "residual_magnetic_moment.hpp" +namespace s2e::simulation { + ResidualMagneticMoment::ResidualMagneticMoment(const Vector<3> constant_value_b_Am2_, const double random_walk_standard_deviation_Am2, const double random_walk_limit_Am2, const double random_noise_standard_deviation_Am2) : constant_value_b_Am2_(constant_value_b_Am2_), random_walk_standard_deviation_Am2_(random_walk_standard_deviation_Am2), random_walk_limit_Am2_(random_walk_limit_Am2), random_noise_standard_deviation_Am2_(random_noise_standard_deviation_Am2) {} + +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp index 93d5e44ab..750db9a55 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp @@ -9,6 +9,8 @@ #include using s2e::math::Vector; +namespace s2e::simulation { + /** * @class ResidualMagneticMoment * @brief Class for spacecraft RMM (Residual Magnetic Moment) @@ -70,4 +72,6 @@ class ResidualMagneticMoment { double random_noise_standard_deviation_Am2_; //!< Standard deviation of white noise of RMM [Am2] }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_RESIDUAL_MAGNETIC_MOMENT_HPP_ \ No newline at end of file diff --git a/src/simulation/spacecraft/structure/structure.cpp b/src/simulation/spacecraft/structure/structure.cpp index bf4510005..2958437de 100644 --- a/src/simulation/spacecraft/structure/structure.cpp +++ b/src/simulation/spacecraft/structure/structure.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::simulation { + Structure::Structure(const SimulationConfiguration* simulation_configuration, const int spacecraft_id) { Initialize(simulation_configuration, spacecraft_id); } @@ -28,3 +30,5 @@ void Structure::Initialize(const SimulationConfiguration* simulation_configurati surfaces_ = InitSurfaces(ini_fname); residual_magnetic_moment_ = new ResidualMagneticMoment(InitResidualMagneticMoment(ini_fname)); } + +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/structure.hpp b/src/simulation/spacecraft/structure/structure.hpp index acc0b5d36..c56943f58 100644 --- a/src/simulation/spacecraft/structure/structure.hpp +++ b/src/simulation/spacecraft/structure/structure.hpp @@ -13,6 +13,8 @@ #include "residual_magnetic_moment.hpp" #include "surface.hpp" +namespace s2e::simulation { + /** * @class Structure * @brief Class for spacecraft structure information @@ -74,4 +76,6 @@ class Structure { ResidualMagneticMoment* residual_magnetic_moment_; //!< Residual Magnetic Moment }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_STRUCTURE_HPP_ diff --git a/src/simulation/spacecraft/structure/surface.cpp b/src/simulation/spacecraft/structure/surface.cpp index 58b0b4cbd..9c6c1326c 100644 --- a/src/simulation/spacecraft/structure/surface.cpp +++ b/src/simulation/spacecraft/structure/surface.cpp @@ -5,6 +5,8 @@ #include "surface.hpp" +namespace s2e::simulation { + Surface::Surface(const s2e::math::Vector<3> position_b_m, const s2e::math::Vector<3> normal_b, const double area_m2, const double reflectivity, const double specularity, const double air_specularity) : position_b_m_(position_b_m), @@ -13,3 +15,5 @@ Surface::Surface(const s2e::math::Vector<3> position_b_m, const s2e::math::Vecto reflectivity_(reflectivity), specularity_(specularity), air_specularity_(air_specularity) {} + +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/surface.hpp b/src/simulation/spacecraft/structure/surface.hpp index 2b4b9439f..01367ee95 100644 --- a/src/simulation/spacecraft/structure/surface.hpp +++ b/src/simulation/spacecraft/structure/surface.hpp @@ -8,6 +8,8 @@ #include +namespace s2e::simulation { + /** * @class Surface * @brief Class for spacecraft surface @@ -113,4 +115,6 @@ class Surface { double air_specularity_; //!< Specularity for air drag }; +} // namespace s2e::simulation + #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_SURFACE_HPP_ From 2091a7070f053e0fe8a5580b3bcd123cb50ce1c9 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:52:05 +0900 Subject: [PATCH 28/49] Add s2e::utilities namespace --- src/s2e.cpp | 4 ++++ src/utilities/com_port_interface.cpp | 4 ++++ src/utilities/com_port_interface.hpp | 5 +++++ src/utilities/endian.cpp | 6 +++++- src/utilities/endian.hpp | 4 ++++ src/utilities/macros.hpp | 4 ++++ src/utilities/quantization.cpp | 4 ++++ src/utilities/quantization.hpp | 4 ++++ src/utilities/ring_buffer.cpp | 4 ++++ src/utilities/ring_buffer.hpp | 4 ++++ src/utilities/slip.cpp | 4 ++++ src/utilities/slip.hpp | 4 ++++ 12 files changed, 50 insertions(+), 1 deletion(-) diff --git a/src/s2e.cpp b/src/s2e.cpp index 960452763..cc1585976 100644 --- a/src/s2e.cpp +++ b/src/s2e.cpp @@ -23,6 +23,8 @@ // #include "interface/hils/COSMOSWrapper.h" // #include "interface/hils/HardwareMessage.h" +namespace s2e { + void print_path(std::string path) { #ifdef WIN32 std::cout << path << std::endl; @@ -79,3 +81,5 @@ int main(int argc, char *argv[]) return EXIT_SUCCESS; } + +} // namaspace s2e diff --git a/src/utilities/com_port_interface.cpp b/src/utilities/com_port_interface.cpp index ab15eb3ab..70209cde8 100644 --- a/src/utilities/com_port_interface.cpp +++ b/src/utilities/com_port_interface.cpp @@ -7,6 +7,8 @@ #include "com_port_interface.hpp" +namespace s2e::utilities { + ComPortInterface::ComPortInterface(int port_id, int baudrate, unsigned int tx_buffer_size, unsigned int rx_buffer_size) : kPortId(port_id), kPortName(PortName(port_id)), kBaudRate(baudrate), kTxBufferSize(tx_buffer_size), kRxBufferSize(rx_buffer_size) { // Allocate managed memory @@ -114,3 +116,5 @@ int ComPortInterface::DiscardInBuffer() { } return 0; } + +} // namespace s2e::utilities diff --git a/src/utilities/com_port_interface.hpp b/src/utilities/com_port_interface.hpp index 03ccd568b..aca6d663d 100644 --- a/src/utilities/com_port_interface.hpp +++ b/src/utilities/com_port_interface.hpp @@ -17,6 +17,9 @@ using namespace System; using namespace System::Runtime::InteropServices; + +namespace s2e::utilities { + typedef cli::array bytearray; /** @@ -105,4 +108,6 @@ class ComPortInterface { msclr::gcroot rx_buf_; //!< RX Buffer }; +} // namespace s2e::utilities + #endif // S2E_LIBRARY_COMMUNICATION_COM_PORT_INTERFACE_HPP_ diff --git a/src/utilities/endian.cpp b/src/utilities/endian.cpp index a20149b0c..8286204ea 100644 --- a/src/utilities/endian.cpp +++ b/src/utilities/endian.cpp @@ -7,6 +7,8 @@ #include +namespace s2e::utilities { + void *endian_memcpy(void *dst, const void *src, size_t size) { #ifdef IS_LITTLE_ENDIAN uint8_t *src_ = (uint8_t *)src; @@ -25,4 +27,6 @@ void *endian_memcpy(void *dst, const void *src, size_t size) { #else return memcpy(dst, src, size); #endif // IS_LITTLE_ENDIAN -} \ No newline at end of file +} + +} // namespace s2e::utilities diff --git a/src/utilities/endian.hpp b/src/utilities/endian.hpp index 27195efd1..966800f36 100644 --- a/src/utilities/endian.hpp +++ b/src/utilities/endian.hpp @@ -11,6 +11,8 @@ #include "endian_define.hpp" // for IS_LITTLE_ENDIAN +namespace s2e::utilities { + /** * @fn endian_memcpy * @brief Memory copy considering endian @@ -20,4 +22,6 @@ */ void *endian_memcpy(void *dst, const void *src, size_t count); +} // namespace s2e::utilities + #endif // S2E_LIBRARY_UTILITIES_ENDIAN_HPP_ diff --git a/src/utilities/macros.hpp b/src/utilities/macros.hpp index 308db3cd8..f78b16fe0 100644 --- a/src/utilities/macros.hpp +++ b/src/utilities/macros.hpp @@ -6,6 +6,10 @@ #ifndef S2E_LIBRARY_UTILITIES_MACROS_HPP_ #define S2E_LIBRARY_UTILITIES_MACROS_HPP_ +namespace s2e::utilities { + #define UNUSED(x) (void)(x) //!< Macro to avoid unused warnings +} // namespace s2e::utilities + #endif // S2E_LIBRARY_UTILITIES_MACROS_HPP_ diff --git a/src/utilities/quantization.cpp b/src/utilities/quantization.cpp index 114c0e2fe..9b8aa00d5 100644 --- a/src/utilities/quantization.cpp +++ b/src/utilities/quantization.cpp @@ -5,9 +5,13 @@ #include "quantization.hpp" +namespace s2e::utilities { + double quantization(const double continuous_number, const double resolution) { int bin_num = (int)((double)continuous_number / resolution); return (double)bin_num * resolution; } float quantization_float(const double continuous_number, const double resolution) { return (float)quantization(continuous_number, resolution); } + +} // namespace s2e::utilities diff --git a/src/utilities/quantization.hpp b/src/utilities/quantization.hpp index f23eebdeb..47bdd4f71 100644 --- a/src/utilities/quantization.hpp +++ b/src/utilities/quantization.hpp @@ -6,6 +6,8 @@ #ifndef S2E_LIBRARY_UTILITIES_QUANTIZATION_HPP_ #define S2E_LIBRARY_UTILITIES_QUANTIZATION_HPP_ +namespace s2e::utilities { + /** * @fn quantization * @brief Default constructor without any initialization @@ -24,4 +26,6 @@ double quantization(const double continuous_number, const double resolution); */ float quantization_float(const double continuous_number, const double resolution); +} // namespace s2e::utilities + #endif // S2E_LIBRARY_UTILITIES_QUANTIZATION_HPP_ diff --git a/src/utilities/ring_buffer.cpp b/src/utilities/ring_buffer.cpp index d89f10204..feb9ec73e 100644 --- a/src/utilities/ring_buffer.cpp +++ b/src/utilities/ring_buffer.cpp @@ -10,6 +10,8 @@ #include #include +namespace s2e::utilities { + RingBuffer::RingBuffer(int buffer_size) : buffer_size_(buffer_size) { buffer_ = new byte[buffer_size]; write_pointer_ = 0; @@ -44,3 +46,5 @@ int RingBuffer::Read(byte* buffer, const unsigned int offset, const unsigned int return read_count; } + +} // namespace s2e::utilities diff --git a/src/utilities/ring_buffer.hpp b/src/utilities/ring_buffer.hpp index 3e133de26..95a45d60f 100644 --- a/src/utilities/ring_buffer.hpp +++ b/src/utilities/ring_buffer.hpp @@ -6,6 +6,8 @@ #ifndef S2E_LIBRARY_UTILITIES_RING_BUFFER_HPP_ #define S2E_LIBRARY_UTILITIES_RING_BUFFER_HPP_ +namespace s2e::utilities { + typedef unsigned char byte; /** @@ -52,4 +54,6 @@ class RingBuffer { unsigned int write_pointer_; //!< Write pointer }; +} // namespace s2e::utilities + #endif // S2E_LIBRARY_UTILITIES_RING_BUFFER_HPP_ diff --git a/src/utilities/slip.cpp b/src/utilities/slip.cpp index 6f2192409..beeef8e53 100644 --- a/src/utilities/slip.cpp +++ b/src/utilities/slip.cpp @@ -8,6 +8,8 @@ #include #include +namespace s2e::utilities { + static uint8_t kSlipFend_ = 0xc0; //!< FEND: Frame End static uint8_t kSlipFesc_ = 0xdb; //!< FESC: Frame Escape static uint8_t kSlipTfend_ = 0xdc; //!< TFEND: Transposed Frame End @@ -77,3 +79,5 @@ std::vector encode_slip_with_header(const std::vector in) { out.insert(out.begin(), kSlipFend_); return out; } + +} // namespace s2e::utilities diff --git a/src/utilities/slip.hpp b/src/utilities/slip.hpp index b5355bb5d..e001295b3 100644 --- a/src/utilities/slip.hpp +++ b/src/utilities/slip.hpp @@ -10,6 +10,8 @@ #include +namespace s2e::utilities { + /** * @fn decode_slip * @brief Decode SLIP data @@ -40,4 +42,6 @@ std::vector encode_slip(std::vector in); */ std::vector encode_slip_with_header(std::vector in); +} // namespace s2e::utilities + #endif // S2E_LIBRARY_UTILITIES_SLIP_HPP_ From 27bbb1a9b1fb7c0f6cfe9435b494435e14057b0f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:52:21 +0900 Subject: [PATCH 29/49] Fix typo --- src/s2e.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/s2e.cpp b/src/s2e.cpp index cc1585976..7861f38af 100644 --- a/src/s2e.cpp +++ b/src/s2e.cpp @@ -82,4 +82,4 @@ int main(int argc, char *argv[]) return EXIT_SUCCESS; } -} // namaspace s2e +} // namespace s2e From ad7eb1cda6fabb4de133041d5a49ef9c1b01b829 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 14:59:12 +0900 Subject: [PATCH 30/49] Add s2e::sample namespace --- src/simulation_sample/case/sample_case.cpp | 6 +++++- src/simulation_sample/case/sample_case.hpp | 6 +++++- .../ground_station/sample_ground_station.cpp | 4 ++++ .../ground_station/sample_ground_station.hpp | 4 ++++ .../ground_station/sample_ground_station_components.cpp | 4 ++++ .../ground_station/sample_ground_station_components.hpp | 4 ++++ src/simulation_sample/spacecraft/sample_components.cpp | 4 ++++ src/simulation_sample/spacecraft/sample_components.hpp | 4 ++++ .../spacecraft/sample_port_configuration.hpp | 4 ++++ src/simulation_sample/spacecraft/sample_spacecraft.cpp | 4 ++++ src/simulation_sample/spacecraft/sample_spacecraft.hpp | 4 ++++ 11 files changed, 46 insertions(+), 2 deletions(-) diff --git a/src/simulation_sample/case/sample_case.cpp b/src/simulation_sample/case/sample_case.cpp index ce6376ebe..259b677b5 100644 --- a/src/simulation_sample/case/sample_case.cpp +++ b/src/simulation_sample/case/sample_case.cpp @@ -5,7 +5,9 @@ #include "sample_case.hpp" -SampleCase::SampleCase(std::string initialise_base_file) : SimulationCase(initialise_base_file) {} +namespace s2e::sample { + +SampleCase::SampleCase(std::string initialise_base_file) : s2e::simulation::SimulationCase(initialise_base_file) {} SampleCase::~SampleCase() { delete sample_spacecraft_; @@ -47,3 +49,5 @@ std::string SampleCase::GetLogValue() const { return str_tmp; } + +} // namespace s2e::sample diff --git a/src/simulation_sample/case/sample_case.hpp b/src/simulation_sample/case/sample_case.hpp index a4664a190..bbc86be45 100644 --- a/src/simulation_sample/case/sample_case.hpp +++ b/src/simulation_sample/case/sample_case.hpp @@ -11,11 +11,13 @@ #include "../ground_station/sample_ground_station.hpp" #include "../spacecraft/sample_spacecraft.hpp" +namespace s2e::sample { + /** * @class SampleCase * @brief An example of user defined simulation class */ -class SampleCase : public SimulationCase { +class SampleCase : public s2e::simulation::SimulationCase { public: /** * @fn SampleCase @@ -57,4 +59,6 @@ class SampleCase : public SimulationCase { void UpdateTargetObjects(); }; +} // namespace s2e::sample + #endif // S2E_SIMULATION_SAMPLE_CASE_SAMPLE_CASE_HPP_ diff --git a/src/simulation_sample/ground_station/sample_ground_station.cpp b/src/simulation_sample/ground_station/sample_ground_station.cpp index 1aa881352..7760419e9 100644 --- a/src/simulation_sample/ground_station/sample_ground_station.cpp +++ b/src/simulation_sample/ground_station/sample_ground_station.cpp @@ -7,6 +7,8 @@ #include "sample_ground_station_components.hpp" +namespace s2e::sample { + SampleGroundStation::SampleGroundStation(const SimulationConfiguration* configuration, const unsigned int ground_station_id) : GroundStation(configuration, ground_station_id) { components_ = new SampleGsComponents(configuration); @@ -23,3 +25,5 @@ void SampleGroundStation::Update(const EarthRotation& celestial_rotation, const GroundStation::Update(celestial_rotation, spacecraft); components_->GetGsCalculator()->Update(spacecraft, spacecraft.GetInstalledComponents().GetAntenna(), *this, *(components_->GetAntenna())); } + +} // namespace s2e::sample diff --git a/src/simulation_sample/ground_station/sample_ground_station.hpp b/src/simulation_sample/ground_station/sample_ground_station.hpp index 8fe089bba..b392c1b66 100644 --- a/src/simulation_sample/ground_station/sample_ground_station.hpp +++ b/src/simulation_sample/ground_station/sample_ground_station.hpp @@ -16,6 +16,8 @@ class SampleGsComponents; +namespace s2e::sample { + /** * @class SampleGroundStation * @brief An example of user defined ground station class @@ -49,4 +51,6 @@ class SampleGroundStation : public GroundStation { SampleGsComponents* components_; //!< Ground station related components }; +} // namespace s2e::sample + #endif // S2E_SIMULATION_SAMPLE_GROUND_STATION_SAMPLE_GROUND_STATION_HPP_ diff --git a/src/simulation_sample/ground_station/sample_ground_station_components.cpp b/src/simulation_sample/ground_station/sample_ground_station_components.cpp index bf7fbdbb6..f799b9bc6 100644 --- a/src/simulation_sample/ground_station/sample_ground_station_components.cpp +++ b/src/simulation_sample/ground_station/sample_ground_station_components.cpp @@ -6,6 +6,8 @@ #include +namespace s2e::sample { + SampleGsComponents::SampleGsComponents(const SimulationConfiguration* configuration) : configuration_(configuration) { IniAccess iniAccess = IniAccess(configuration_->ground_station_file_list_[0]); @@ -26,3 +28,5 @@ void SampleGsComponents::CompoLogSetUp(Logger& logger) { // logger.AddLogList(ant_); logger.AddLogList(gs_calculator_); } + +} // namespace s2e::sample diff --git a/src/simulation_sample/ground_station/sample_ground_station_components.hpp b/src/simulation_sample/ground_station/sample_ground_station_components.hpp index 15f1632bb..dbf87cd42 100644 --- a/src/simulation_sample/ground_station/sample_ground_station_components.hpp +++ b/src/simulation_sample/ground_station/sample_ground_station_components.hpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::sample { + /** * @class SampleGsComponents * @brief An example of ground station related components list class @@ -49,4 +51,6 @@ class SampleGsComponents { const SimulationConfiguration* configuration_; //!< Simulation setting }; +} // namespace s2e::sample + #endif // S2E_SIMULATION_SAMPLE_GROUND_STATION_SAMPLE_GROUND_STATION_COMPONENTS_HPP_ diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index 5de0f8984..956b0408c 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -9,6 +9,8 @@ #include "sample_port_configuration.hpp" +namespace s2e::sample { + SampleComponents::SampleComponents(const Dynamics* dynamics, Structure* structure, const LocalEnvironment* local_environment, const GlobalEnvironment* global_environment, const SimulationConfiguration* configuration, ClockGenerator* clock_generator, const unsigned int spacecraft_id) @@ -222,3 +224,5 @@ void SampleComponents::LogSetup(Logger& logger) { logger.AddLogList(attitude_observer_); logger.AddLogList(orbit_observer_); } + +} // namespace s2e::sample diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index c8c3603d5..619043697 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -50,6 +50,8 @@ class AngularVelocityObserver; class AttitudeObserver; class Telescope; +namespace s2e::sample { + /** * @class SampleComponents * @brief An example of user side components management class @@ -143,4 +145,6 @@ class SampleComponents : public InstalledComponents { const GlobalEnvironment* global_environment_; //!< Global environment information }; +} // namespace s2e::sample + #endif // S2E_SIMULATION_SAMPLE_SPACECRAFT_SAMPLE_COMPONENTS_HPP_ diff --git a/src/simulation_sample/spacecraft/sample_port_configuration.hpp b/src/simulation_sample/spacecraft/sample_port_configuration.hpp index 413e22fce..bfa1a57db 100644 --- a/src/simulation_sample/spacecraft/sample_port_configuration.hpp +++ b/src/simulation_sample/spacecraft/sample_port_configuration.hpp @@ -6,6 +6,8 @@ #ifndef S2E_SIMULATION_SAMPLE_SPACECRAFT_SAMPLE_PORT_CONFIGURATION_HPP_ #define S2E_SIMULATION_SAMPLE_SPACECRAFT_SAMPLE_PORT_CONFIGURATION_HPP_ +namespace s2e::sample { + /** * @enum PowerPortConfig * @brief ID list of electrical power switch ports @@ -27,4 +29,6 @@ enum class UARTPortConfig { kUartComponentMax //!< Maximum port number. Do not remove. Place on the bottom. }; +} // namespace s2e::sample + #endif // S2E_SIMULATION_SAMPLE_SPACECRAFT_SAMPLE_PORT_CONFIGURATION_HPP_ diff --git a/src/simulation_sample/spacecraft/sample_spacecraft.cpp b/src/simulation_sample/spacecraft/sample_spacecraft.cpp index 09c7df86c..3b46777ec 100644 --- a/src/simulation_sample/spacecraft/sample_spacecraft.cpp +++ b/src/simulation_sample/spacecraft/sample_spacecraft.cpp @@ -10,6 +10,8 @@ #include "sample_components.hpp" +namespace s2e::sample { + SampleSpacecraft::SampleSpacecraft(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const unsigned int spacecraft_id) : Spacecraft(simulation_configuration, global_environment, spacecraft_id) { @@ -17,3 +19,5 @@ SampleSpacecraft::SampleSpacecraft(const SimulationConfiguration* simulation_con new SampleComponents(dynamics_, structure_, local_environment_, global_environment, simulation_configuration, &clock_generator_, spacecraft_id); components_ = sample_components_; } + +} // namespace s2e::sample diff --git a/src/simulation_sample/spacecraft/sample_spacecraft.hpp b/src/simulation_sample/spacecraft/sample_spacecraft.hpp index 1436dbfe1..27b09addf 100644 --- a/src/simulation_sample/spacecraft/sample_spacecraft.hpp +++ b/src/simulation_sample/spacecraft/sample_spacecraft.hpp @@ -12,6 +12,8 @@ class SampleComponents; +namespace s2e::sample { + /** * @class SampleSpacecraft * @brief An example of user side spacecraft class @@ -35,4 +37,6 @@ class SampleSpacecraft : public Spacecraft { SampleComponents* sample_components_; }; +} // namespace s2e::sample + #endif // S2E_SIMULATION_SAMPLE_SPACECRAFT_SAMPLE_SPACECRAFT_HPP_ From ccaf910d1bd65187dc71fbaa6f92af96d426f081 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 15:21:32 +0900 Subject: [PATCH 31/49] Update namespace info in simulation sample --- src/simulation_sample/case/sample_case.cpp | 6 +- src/simulation_sample/case/sample_case.hpp | 2 +- .../ground_station/sample_ground_station.cpp | 10 +- .../ground_station/sample_ground_station.hpp | 10 +- .../sample_ground_station_components.cpp | 10 +- .../sample_ground_station_components.hpp | 12 +-- .../spacecraft/sample_components.cpp | 22 +++-- .../spacecraft/sample_components.hpp | 94 +++++++++---------- .../spacecraft/sample_spacecraft.cpp | 4 +- .../spacecraft/sample_spacecraft.hpp | 4 +- 10 files changed, 88 insertions(+), 86 deletions(-) diff --git a/src/simulation_sample/case/sample_case.cpp b/src/simulation_sample/case/sample_case.cpp index 259b677b5..5487e0e08 100644 --- a/src/simulation_sample/case/sample_case.cpp +++ b/src/simulation_sample/case/sample_case.cpp @@ -7,7 +7,7 @@ namespace s2e::sample { -SampleCase::SampleCase(std::string initialise_base_file) : s2e::simulation::SimulationCase(initialise_base_file) {} +SampleCase::SampleCase(std::string initialise_base_file) : simulation::SimulationCase(initialise_base_file) {} SampleCase::~SampleCase() { delete sample_spacecraft_; @@ -37,7 +37,7 @@ void SampleCase::UpdateTargetObjects() { std::string SampleCase::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteScalar("time", "s"); + str_tmp += logger::WriteScalar("time", "s"); return str_tmp; } @@ -45,7 +45,7 @@ std::string SampleCase::GetLogHeader() const { std::string SampleCase::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteScalar(global_environment_->GetSimulationTime().GetElapsedTime_s()); + str_tmp += logger::WriteScalar(global_environment_->GetSimulationTime().GetElapsedTime_s()); return str_tmp; } diff --git a/src/simulation_sample/case/sample_case.hpp b/src/simulation_sample/case/sample_case.hpp index bbc86be45..66302d589 100644 --- a/src/simulation_sample/case/sample_case.hpp +++ b/src/simulation_sample/case/sample_case.hpp @@ -17,7 +17,7 @@ namespace s2e::sample { * @class SampleCase * @brief An example of user defined simulation class */ -class SampleCase : public s2e::simulation::SimulationCase { +class SampleCase : public simulation::SimulationCase { public: /** * @fn SampleCase diff --git a/src/simulation_sample/ground_station/sample_ground_station.cpp b/src/simulation_sample/ground_station/sample_ground_station.cpp index 7760419e9..5489d4423 100644 --- a/src/simulation_sample/ground_station/sample_ground_station.cpp +++ b/src/simulation_sample/ground_station/sample_ground_station.cpp @@ -9,20 +9,20 @@ namespace s2e::sample { -SampleGroundStation::SampleGroundStation(const SimulationConfiguration* configuration, const unsigned int ground_station_id) - : GroundStation(configuration, ground_station_id) { +SampleGroundStation::SampleGroundStation(const simulation::SimulationConfiguration* configuration, const unsigned int ground_station_id) + : simulation::GroundStation(configuration, ground_station_id) { components_ = new SampleGsComponents(configuration); } SampleGroundStation::~SampleGroundStation() { delete components_; } void SampleGroundStation::LogSetup(Logger& logger) { - GroundStation::LogSetup(logger); + simulation::GroundStation::LogSetup(logger); components_->CompoLogSetUp(logger); } -void SampleGroundStation::Update(const EarthRotation& celestial_rotation, const SampleSpacecraft& spacecraft) { - GroundStation::Update(celestial_rotation, spacecraft); +void SampleGroundStation::Update(const environment::EarthRotation& celestial_rotation, const SampleSpacecraft& spacecraft) { + simulation::GroundStation::Update(celestial_rotation, spacecraft); components_->GetGsCalculator()->Update(spacecraft, spacecraft.GetInstalledComponents().GetAntenna(), *this, *(components_->GetAntenna())); } diff --git a/src/simulation_sample/ground_station/sample_ground_station.hpp b/src/simulation_sample/ground_station/sample_ground_station.hpp index b392c1b66..9acc8d613 100644 --- a/src/simulation_sample/ground_station/sample_ground_station.hpp +++ b/src/simulation_sample/ground_station/sample_ground_station.hpp @@ -14,21 +14,21 @@ #include "../spacecraft/sample_spacecraft.hpp" -class SampleGsComponents; - namespace s2e::sample { +class SampleGsComponents; + /** * @class SampleGroundStation * @brief An example of user defined ground station class */ -class SampleGroundStation : public GroundStation { +class SampleGroundStation : public simulation::GroundStation { public: /** * @fn SampleGroundStation * @brief Constructor */ - SampleGroundStation(const SimulationConfiguration* configuration, const unsigned int ground_station_id); + SampleGroundStation(const simulation::SimulationConfiguration* configuration, const unsigned int ground_station_id); /** * @fn ~SampleGroundStation * @brief Destructor @@ -44,7 +44,7 @@ class SampleGroundStation : public GroundStation { * @fn Update * @brief Override function of Update in GroundStation class */ - virtual void Update(const EarthRotation& celestial_rotation, const SampleSpacecraft& spacecraft); + virtual void Update(const environment::EarthRotation& celestial_rotation, const SampleSpacecraft& spacecraft); private: using GroundStation::Update; diff --git a/src/simulation_sample/ground_station/sample_ground_station_components.cpp b/src/simulation_sample/ground_station/sample_ground_station_components.cpp index f799b9bc6..c18528bb2 100644 --- a/src/simulation_sample/ground_station/sample_ground_station_components.cpp +++ b/src/simulation_sample/ground_station/sample_ground_station_components.cpp @@ -8,15 +8,15 @@ namespace s2e::sample { -SampleGsComponents::SampleGsComponents(const SimulationConfiguration* configuration) : configuration_(configuration) { - IniAccess iniAccess = IniAccess(configuration_->ground_station_file_list_[0]); +SampleGsComponents::SampleGsComponents(const simulation::SimulationConfiguration* configuration) : configuration_(configuration) { + setting_file_reader::IniAccess iniAccess = setting_file_reader::IniAccess(configuration_->ground_station_file_list_[0]); std::string ant_ini_path = iniAccess.ReadString("COMPONENT_FILES", "ground_station_antenna_file"); configuration_->main_logger_->CopyFileToLogDirectory(ant_ini_path); - antenna_ = new Antenna(InitAntenna(1, ant_ini_path)); + antenna_ = new components::Antenna(components::InitAntenna(1, ant_ini_path)); std::string gs_calculator_ini_path = iniAccess.ReadString("COMPONENT_FILES", "ground_station_calculator_file"); configuration_->main_logger_->CopyFileToLogDirectory(gs_calculator_ini_path); - gs_calculator_ = new GroundStationCalculator(InitGsCalculator(gs_calculator_ini_path)); + gs_calculator_ = new components::GroundStationCalculator(components::InitGsCalculator(gs_calculator_ini_path)); } SampleGsComponents::~SampleGsComponents() { @@ -24,7 +24,7 @@ SampleGsComponents::~SampleGsComponents() { delete gs_calculator_; } -void SampleGsComponents::CompoLogSetUp(Logger& logger) { +void SampleGsComponents::CompoLogSetUp(logger::Logger& logger) { // logger.AddLogList(ant_); logger.AddLogList(gs_calculator_); } diff --git a/src/simulation_sample/ground_station/sample_ground_station_components.hpp b/src/simulation_sample/ground_station/sample_ground_station_components.hpp index dbf87cd42..1de5eec24 100644 --- a/src/simulation_sample/ground_station/sample_ground_station_components.hpp +++ b/src/simulation_sample/ground_station/sample_ground_station_components.hpp @@ -21,7 +21,7 @@ class SampleGsComponents { * @fn SampleGsComponents * @brief Constructor */ - SampleGsComponents(const SimulationConfiguration* configuration); + SampleGsComponents(const simulation::SimulationConfiguration* configuration); /** * @fn ~SampleGsComponents * @brief Destructor @@ -38,17 +38,17 @@ class SampleGsComponents { * @fn GetAntenna * @brief Return antenna */ - inline Antenna* GetAntenna() const { return antenna_; } + inline components::Antenna* GetAntenna() const { return antenna_; } /** * @fn GetGsCalculator * @brief Return ground station calculator */ - inline GroundStationCalculator* GetGsCalculator() const { return gs_calculator_; } + inline components::GroundStationCalculator* GetGsCalculator() const { return gs_calculator_; } private: - Antenna* antenna_; //!< Antenna on ground station - GroundStationCalculator* gs_calculator_; //!< Ground station calculation algorithm - const SimulationConfiguration* configuration_; //!< Simulation setting + components::Antenna* antenna_; //!< Antenna on ground station + components::GroundStationCalculator* gs_calculator_; //!< Ground station calculation algorithm + const simulation::SimulationConfiguration* configuration_; //!< Simulation setting }; } // namespace s2e::sample diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index 956b0408c..38debc042 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -11,15 +11,17 @@ namespace s2e::sample { -SampleComponents::SampleComponents(const Dynamics* dynamics, Structure* structure, const LocalEnvironment* local_environment, - const GlobalEnvironment* global_environment, const SimulationConfiguration* configuration, - ClockGenerator* clock_generator, const unsigned int spacecraft_id) +using namespace components; + +SampleComponents::SampleComponents(const dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, + const environment::GlobalEnvironment* global_environment, const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, + const unsigned int spacecraft_id) : configuration_(configuration), dynamics_(dynamics), structure_(structure), local_environment_(local_environment), global_environment_(global_environment) { - IniAccess iniAccess = IniAccess(configuration_->spacecraft_file_list_[spacecraft_id]); + setting_file_reader::IniAccess iniAccess = setting_file_reader::IniAccess(configuration_->spacecraft_file_list_[spacecraft_id]); // PCU power port connection pcu_ = new PowerControlUnit(clock_generator); @@ -29,7 +31,7 @@ SampleComponents::SampleComponents(const Dynamics* dynamics, Structure* structur // Components obc_ = new OnBoardComputer(1, clock_generator, pcu_->GetPowerPort(0)); - hils_port_manager_ = new HilsPortManager(); + hils_port_manager_ = new simulation::HilsPortManager(); // GyroSensor std::string file_name = iniAccess.ReadString("COMPONENT_FILES", "gyro_file"); @@ -190,15 +192,15 @@ SampleComponents::~SampleComponents() { delete hils_port_manager_; // delete after exp_hils } -s2e::math::Vector<3> SampleComponents::GenerateForce_b_N() { - s2e::math::Vector<3> force_b_N_(0.0); +math::Vector<3> SampleComponents::GenerateForce_b_N() { + math::Vector<3> force_b_N_(0.0); force_b_N_ += thruster_->GetOutputThrust_b_N(); force_b_N_ += force_generator_->GetGeneratedForce_b_N(); return force_b_N_; } -s2e::math::Vector<3> SampleComponents::GenerateTorque_b_Nm() { - s2e::math::Vector<3> torque_b_Nm_(0.0); +math::Vector<3> SampleComponents::GenerateTorque_b_Nm() { + math::Vector<3> torque_b_Nm_(0.0); torque_b_Nm_ += magnetorquer_->GetOutputTorque_b_Nm(); torque_b_Nm_ += reaction_wheel_->GetOutputTorque_b_Nm(); torque_b_Nm_ += thruster_->GetOutputTorque_b_Nm(); @@ -208,7 +210,7 @@ s2e::math::Vector<3> SampleComponents::GenerateTorque_b_Nm() { void SampleComponents::ComponentInterference() { mtq_magnetometer_interference_->UpdateInterference(); } -void SampleComponents::LogSetup(Logger& logger) { +void SampleComponents::LogSetup(logger::Logger& logger) { logger.AddLogList(gyro_sensor_); logger.AddLogList(magnetometer_); logger.AddLogList(star_sensor_); diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index 619043697..756e3fc9b 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -34,36 +34,36 @@ #include #include -class OnBoardComputer; -class PowerControlUnit; -class GyroSensor; -class Magnetometer; -class StarSensor; -class SunSensor; -class GnssReceiver; -class Magnetorquer; -class ReactionWheel; -class SimpleThruster; -class ForceGenerator; -class TorqueGenerator; -class AngularVelocityObserver; -class AttitudeObserver; -class Telescope; - namespace s2e::sample { +class components::OnBoardComputer; +class components::PowerControlUnit; +class components::GyroSensor; +class components::Magnetometer; +class components::StarSensor; +class components::SunSensor; +class components::GnssReceiver; +class components::Magnetorquer; +class components::ReactionWheel; +class components::SimpleThruster; +class components::ForceGenerator; +class components::TorqueGenerator; +class components::AngularVelocityObserver; +class components::AttitudeObserver; +class components::Telescope; + /** * @class SampleComponents * @brief An example of user side components management class */ -class SampleComponents : public InstalledComponents { +class SampleComponents : public simulation::InstalledComponents { public: /** * @fn SampleComponents * @brief Constructor */ - SampleComponents(const Dynamics* dynamics, Structure* structure, const LocalEnvironment* local_environment, - const GlobalEnvironment* global_environment, const SimulationConfiguration* configuration, ClockGenerator* clock_generator, + SampleComponents(const dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, + const environment::GlobalEnvironment* global_environment, const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, const unsigned int spacecraft_id); /** * @fn ~SampleComponents @@ -76,12 +76,12 @@ class SampleComponents : public InstalledComponents { * @fn GenerateForce_b_N * @brief Return force generated by components in unit Newton in body fixed frame */ - s2e::math::Vector<3> GenerateForce_b_N() override; + math::Vector<3> GenerateForce_b_N() override; /** * @fn GenerateTorque_b_Nm * @brief Return torque generated by components in unit Newton-meter in body fixed frame */ - s2e::math::Vector<3> GenerateTorque_b_Nm() override; + math::Vector<3> GenerateTorque_b_Nm() override; /** * @fn ComponentInterference * @brief Handle component interference effect @@ -92,41 +92,41 @@ class SampleComponents : public InstalledComponents { * @fn LogSetup * @brief Setup the logger for components */ - void LogSetup(Logger& logger) override; + void LogSetup(logger::Logger& logger) override; // Getter - inline Antenna& GetAntenna() const { return *antenna_; } + inline components::Antenna& GetAntenna() const { return *antenna_; } private: - PowerControlUnit* pcu_; //!< Power Control Unit - OnBoardComputer* obc_; //!< Onboard Computer - HilsPortManager* hils_port_manager_; //!< Port manager for HILS test + components::PowerControlUnit* pcu_; //!< Power Control Unit + components::OnBoardComputer* obc_; //!< Onboard Computer + simulation::HilsPortManager* hils_port_manager_; //!< Port manager for HILS test // AOCS - GyroSensor* gyro_sensor_; //!< GyroSensor sensor - Magnetometer* magnetometer_; //!< Magnetometer - StarSensor* star_sensor_; //!< Star sensor - SunSensor* sun_sensor_; //!< Sun sensor - GnssReceiver* gnss_receiver_; //!< GNSS receiver - Magnetorquer* magnetorquer_; //!< Magnetorquer - ReactionWheel* reaction_wheel_; //!< Reaction Wheel - SimpleThruster* thruster_; //!< Thruster + components::GyroSensor* gyro_sensor_; //!< GyroSensor sensor + components::Magnetometer* magnetometer_; //!< Magnetometer + components::StarSensor* star_sensor_; //!< Star sensor + components::SunSensor* sun_sensor_; //!< Sun sensor + components::GnssReceiver* gnss_receiver_; //!< GNSS receiver + components::Magnetorquer* magnetorquer_; //!< Magnetorquer + components::ReactionWheel* reaction_wheel_; //!< Reaction Wheel + components::SimpleThruster* thruster_; //!< Thruster // Ideal component - ForceGenerator* force_generator_; //!< Ideal Force Generator - TorqueGenerator* torque_generator_; //!< Ideal Torque Generator - AngularVelocityObserver* angular_velocity_observer_; //!< Ideal Angular velocity observer - AttitudeObserver* attitude_observer_; //!< Ideal Attitude observer - OrbitObserver* orbit_observer_; //!< Ideal Orbit observer + components::ForceGenerator* force_generator_; //!< Ideal Force Generator + components::TorqueGenerator* torque_generator_; //!< Ideal Torque Generator + components::AngularVelocityObserver* angular_velocity_observer_; //!< Ideal Angular velocity observer + components::AttitudeObserver* attitude_observer_; //!< Ideal Attitude observer + components::OrbitObserver* orbit_observer_; //!< Ideal Orbit observer // Mission - Telescope* telescope_; //!< Telescope + components::Telescope* telescope_; //!< Telescope // CommGs - Antenna* antenna_; //!< Antenna + components::Antenna* antenna_; //!< Antenna // Component Interference - MtqMagnetometerInterference* mtq_magnetometer_interference_; //!< Additional Bias noise by MTQ-Magnetometer interference + components::MtqMagnetometerInterference* mtq_magnetometer_interference_; //!< Additional Bias noise by MTQ-Magnetometer interference // Examples // ExampleChangeStructure* change_structure_; //!< Change structure @@ -138,11 +138,11 @@ class SampleComponents : public InstalledComponents { */ // States - const SimulationConfiguration* configuration_; //!< Simulation settings - const Dynamics* dynamics_; //!< Dynamics information of the spacecraft - Structure* structure_; //!< Structure information of the spacecraft - const LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft - const GlobalEnvironment* global_environment_; //!< Global environment information + const simulation::SimulationConfiguration* configuration_; //!< Simulation settings + const dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft + simulation::Structure* structure_; //!< Structure information of the spacecraft + const environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft + const environment::GlobalEnvironment* global_environment_; //!< Global environment information }; } // namespace s2e::sample diff --git a/src/simulation_sample/spacecraft/sample_spacecraft.cpp b/src/simulation_sample/spacecraft/sample_spacecraft.cpp index 3b46777ec..bfc1bdcc9 100644 --- a/src/simulation_sample/spacecraft/sample_spacecraft.cpp +++ b/src/simulation_sample/spacecraft/sample_spacecraft.cpp @@ -12,9 +12,9 @@ namespace s2e::sample { -SampleSpacecraft::SampleSpacecraft(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, +SampleSpacecraft::SampleSpacecraft(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const unsigned int spacecraft_id) - : Spacecraft(simulation_configuration, global_environment, spacecraft_id) { + : simulation::Spacecraft(simulation_configuration, global_environment, spacecraft_id) { sample_components_ = new SampleComponents(dynamics_, structure_, local_environment_, global_environment, simulation_configuration, &clock_generator_, spacecraft_id); components_ = sample_components_; diff --git a/src/simulation_sample/spacecraft/sample_spacecraft.hpp b/src/simulation_sample/spacecraft/sample_spacecraft.hpp index 27b09addf..43f9ff564 100644 --- a/src/simulation_sample/spacecraft/sample_spacecraft.hpp +++ b/src/simulation_sample/spacecraft/sample_spacecraft.hpp @@ -18,13 +18,13 @@ namespace s2e::sample { * @class SampleSpacecraft * @brief An example of user side spacecraft class */ -class SampleSpacecraft : public Spacecraft { +class SampleSpacecraft : public simulation::Spacecraft { public: /** * @fn SampleSpacecraft * @brief Constructor */ - SampleSpacecraft(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, + SampleSpacecraft(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const unsigned int spacecraft_id); /** From e8b37f531f04446a85a5f18ee6511abc91f3dcf8 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 15:37:04 +0900 Subject: [PATCH 32/49] Update namespace info in simulation --- src/simulation/case/simulation_case.cpp | 12 +++++----- src/simulation/case/simulation_case.hpp | 6 ++--- .../ground_station/ground_station.cpp | 14 +++++------ .../ground_station/ground_station.hpp | 20 ++++++++-------- src/simulation/hils/hils_port_manager.cpp | 20 ++++++++-------- src/simulation/hils/hils_port_manager.hpp | 4 ++-- .../initialize_monte_carlo_parameters.hpp | 24 +++++++++---------- .../initialize_monte_carlo_simulation.cpp | 2 +- .../relative_information.cpp | 18 +++++++------- .../relative_information.hpp | 10 ++++---- src/simulation/simulation_configuration.hpp | 2 +- .../spacecraft/installed_components.cpp | 2 +- .../spacecraft/installed_components.hpp | 2 +- src/simulation/spacecraft/spacecraft.cpp | 14 +++++------ src/simulation/spacecraft/spacecraft.hpp | 22 ++++++++--------- .../structure/initialize_structure.cpp | 6 ++--- .../structure/residual_magnetic_moment.hpp | 7 +++--- .../spacecraft/structure/structure.cpp | 2 +- 18 files changed, 93 insertions(+), 94 deletions(-) diff --git a/src/simulation/case/simulation_case.cpp b/src/simulation/case/simulation_case.cpp index f9e869937..cda5c0786 100644 --- a/src/simulation/case/simulation_case.cpp +++ b/src/simulation/case/simulation_case.cpp @@ -13,7 +13,7 @@ namespace s2e::simulation { SimulationCase::SimulationCase(const std::string initialize_base_file) { // Initialize Log - simulation_configuration_.main_logger_ = InitLog(initialize_base_file); + simulation_configuration_.main_logger_ = logger::InitLog(initialize_base_file); // Initialize Simulation Configuration InitializeSimulationConfiguration(initialize_base_file); @@ -23,16 +23,16 @@ SimulationCase::SimulationCase(const std::string initialize_base_file, const Mon const std::string log_path) { if (monte_carlo_simulator.IsEnabled() == false) { // Monte Carlo simulation is disabled - simulation_configuration_.main_logger_ = InitLog(initialize_base_file); + simulation_configuration_.main_logger_ = logger::InitLog(initialize_base_file); } else { // Monte Carlo Simulation is enabled std::string log_file_name = "default" + std::to_string(monte_carlo_simulator.GetNumberOfExecutionsDone()) + ".csv"; - IniAccess ini_file(initialize_base_file); + setting_file_reader::IniAccess ini_file(initialize_base_file); bool save_ini_files = ini_file.ReadEnable("SIMULATION_SETTINGS", "save_initialize_files"); simulation_configuration_.main_logger_ = - new Logger(log_file_name, log_path, initialize_base_file, save_ini_files, monte_carlo_simulator.GetSaveLogHistoryFlag()); + new logger::Logger(log_file_name, log_path, initialize_base_file, save_ini_files, monte_carlo_simulator.GetSaveLogHistoryFlag()); } // Initialize Simulation Configuration InitializeSimulationConfiguration(initialize_base_file); @@ -87,7 +87,7 @@ std::string SimulationCase::GetLogValue() const { void SimulationCase::InitializeSimulationConfiguration(const std::string initialize_base_file) { // Initialize - IniAccess simulation_base_ini = IniAccess(initialize_base_file); + setting_file_reader::IniAccess simulation_base_ini = setting_file_reader::IniAccess(initialize_base_file); const char* section = "SIMULATION_SETTINGS"; simulation_configuration_.initialize_base_file_name_ = initialize_base_file; @@ -104,7 +104,7 @@ void SimulationCase::InitializeSimulationConfiguration(const std::string initial simulation_configuration_.gnss_file_ = simulation_base_ini.ReadString(section, "gnss_file"); // Global Environment - global_environment_ = new GlobalEnvironment(&simulation_configuration_); + global_environment_ = new environment::GlobalEnvironment(&simulation_configuration_); global_environment_->LogSetup(*(simulation_configuration_.main_logger_)); } diff --git a/src/simulation/case/simulation_case.hpp b/src/simulation/case/simulation_case.hpp index 53d37f8a9..47800dee7 100644 --- a/src/simulation/case/simulation_case.hpp +++ b/src/simulation/case/simulation_case.hpp @@ -19,7 +19,7 @@ namespace s2e::simulation { * @class SimulationCase * @brief Base class to define simulation scenario */ -class SimulationCase : public ILoggable { +class SimulationCase : public logger::ILoggable { public: /** * @fn SimulationCase @@ -74,11 +74,11 @@ class SimulationCase : public ILoggable { * @fn GetGlobalEnvironment * @brief Return global environment */ - inline const GlobalEnvironment& GetGlobalEnvironment() const { return *global_environment_; } + inline const environment::GlobalEnvironment& GetGlobalEnvironment() const { return *global_environment_; } protected: SimulationConfiguration simulation_configuration_; //!< Simulation setting - GlobalEnvironment* global_environment_; //!< Global Environment + environment::GlobalEnvironment* global_environment_; //!< Global Environment /** * @fn InitializeSimulationConfiguration diff --git a/src/simulation/ground_station/ground_station.cpp b/src/simulation/ground_station/ground_station.cpp index 12ee9ae4a..5340c30dc 100644 --- a/src/simulation/ground_station/ground_station.cpp +++ b/src/simulation/ground_station/ground_station.cpp @@ -28,7 +28,7 @@ GroundStation::~GroundStation() {} void GroundStation::Initialize(const SimulationConfiguration* configuration, const unsigned int ground_station_id) { std::string gs_ini_path = configuration->ground_station_file_list_[0]; - auto conf = IniAccess(gs_ini_path); + auto conf = setting_file_reader::IniAccess(gs_ini_path); const char* section_base = "GROUND_STATION_"; const std::string section_tmp = section_base + std::to_string(static_cast(ground_station_id)); @@ -45,9 +45,9 @@ void GroundStation::Initialize(const SimulationConfiguration* configuration, con configuration->main_logger_->CopyFileToLogDirectory(gs_ini_path); } -void GroundStation::LogSetup(Logger& logger) { logger.AddLogList(this); } +void GroundStation::LogSetup(logger::Logger& logger) { logger.AddLogList(this); } -void GroundStation::Update(const EarthRotation& celestial_rotation, const Spacecraft& spacecraft) { +void GroundStation::Update(const environment::EarthRotation& celestial_rotation, const Spacecraft& spacecraft) { s2e::math::Matrix<3, 3> dcm_ecef2eci = celestial_rotation.GetDcmJ2000ToEcef().Transpose(); position_i_m_ = dcm_ecef2eci * position_ecef_m_; @@ -77,9 +77,9 @@ std::string GroundStation::GetLogHeader() const { std::string head = "ground_station" + std::to_string(ground_station_id_) + "_"; for (unsigned int i = 0; i < number_of_spacecraft_; i++) { std::string legend = head + "sc" + std::to_string(i) + "_visible_flag"; - str_tmp += WriteScalar(legend); + str_tmp += logger::WriteScalar(legend); } - str_tmp += WriteVector("ground_station_position", "eci", "m", 3); + str_tmp += logger::WriteVector("ground_station_position", "eci", "m", 3); return str_tmp; } @@ -87,9 +87,9 @@ std::string GroundStation::GetLogValue() const { std::string str_tmp = ""; for (unsigned int i = 0; i < number_of_spacecraft_; i++) { - str_tmp += WriteScalar(is_visible_.at(i)); + str_tmp += logger::WriteScalar(is_visible_.at(i)); } - str_tmp += WriteVector(position_i_m_); + str_tmp += logger::WriteVector(position_i_m_); return str_tmp; } diff --git a/src/simulation/ground_station/ground_station.hpp b/src/simulation/ground_station/ground_station.hpp index 42a3b8cac..12ca04fec 100644 --- a/src/simulation/ground_station/ground_station.hpp +++ b/src/simulation/ground_station/ground_station.hpp @@ -18,7 +18,7 @@ namespace s2e::simulation { * @class GroundStation * @brief Base class of ground station */ -class GroundStation : public ILoggable { +class GroundStation : public logger::ILoggable { public: /** * @fn GroundStation @@ -40,12 +40,12 @@ class GroundStation : public ILoggable { * @fn LogSetup * @brief Virtual function to log output setting for ground station related components */ - virtual void LogSetup(Logger& logger); + virtual void LogSetup(logger::Logger& logger); /** * @fn Update * @brief Virtual function of main routine */ - virtual void Update(const EarthRotation& celestial_rotation, const Spacecraft& spacecraft); + virtual void Update(const environment::EarthRotation& celestial_rotation, const Spacecraft& spacecraft); // Override functions for ILoggable /** @@ -69,17 +69,17 @@ class GroundStation : public ILoggable { * @fn GetGeodeticPosition * @brief Return ground station position in the geodetic frame */ - s2e::geodesy::GeodeticPosition GetGeodeticPosition() const { return geodetic_position_; } + geodesy::GeodeticPosition GetGeodeticPosition() const { return geodetic_position_; } /** * @fn GetPosition_ecef_m * @brief Return ground station position in the ECEF frame [m] */ - Vector<3> GetPosition_ecef_m() const { return position_ecef_m_; } + math::Vector<3> GetPosition_ecef_m() const { return position_ecef_m_; } /** * @fn GetPosition_i_m * @brief Return ground station position in the inertial frame [m] */ - Vector<3> GetPosition_i_m() const { return position_i_m_; } + math::Vector<3> GetPosition_i_m() const { return position_i_m_; } /** * @fn GetElevationLimitAngle_deg * @brief Return ground station elevation limit angle [deg] @@ -94,9 +94,9 @@ class GroundStation : public ILoggable { protected: unsigned int ground_station_id_; //!< Ground station ID - s2e::geodesy::GeodeticPosition geodetic_position_; //!< Ground Station Position in the geodetic frame - Vector<3> position_ecef_m_{0.0}; //!< Ground Station Position in the ECEF frame [m] - Vector<3> position_i_m_{0.0}; //!< Ground Station Position in the inertial frame [m] + geodesy::GeodeticPosition geodetic_position_; //!< Ground Station Position in the geodetic frame + math::Vector<3> position_ecef_m_{0.0}; //!< Ground Station Position in the ECEF frame [m] + math::Vector<3> position_i_m_{0.0}; //!< Ground Station Position in the inertial frame [m] double elevation_limit_angle_deg_; //!< Minimum elevation angle to work the ground station [deg] std::map is_visible_; //!< Visible flag for each spacecraft ID (not care antenna) @@ -108,7 +108,7 @@ class GroundStation : public ILoggable { * @param [in] spacecraft_position_ecef_m: spacecraft position in ECEF frame [m] * @return True when the satellite is visible from the ground station */ - bool CalcIsVisible(const Vector<3> spacecraft_position_ecef_m); + bool CalcIsVisible(const math::Vector<3> spacecraft_position_ecef_m); }; } // namespace s2e::simulation diff --git a/src/simulation/hils/hils_port_manager.cpp b/src/simulation/hils/hils_port_manager.cpp index 4eda6559f..ee0e7f1e1 100644 --- a/src/simulation/hils/hils_port_manager.cpp +++ b/src/simulation/hils/hils_port_manager.cpp @@ -47,7 +47,7 @@ int HilsPortManager::UartCloseComPort(unsigned int port_id) { } uart_ports_[port_id]->ClosePort(); - HilsUartPort* port = uart_ports_.at(port_id); + components::HilsUartPort* port = uart_ports_.at(port_id); delete port; uart_ports_.erase(port_id); return 0; @@ -60,7 +60,7 @@ int HilsPortManager::UartCloseComPort(unsigned int port_id) { int HilsPortManager::UartReceive(unsigned int port_id, unsigned char* buffer, int offset, int length) { #ifdef USE_HILS - HilsUartPort* port = uart_ports_[port_id]; + components::HilsUartPort* port = uart_ports_[port_id]; if (port == nullptr) return -1; int ret = port->ReadRx(buffer, offset, length); #ifdef HILS_PORT_MANAGER_SHOW_DEBUG_DATA @@ -85,7 +85,7 @@ int HilsPortManager::UartReceive(unsigned int port_id, unsigned char* buffer, in int HilsPortManager::UartSend(unsigned int port_id, const unsigned char* buffer, int offset, int length) { #ifdef USE_HILS - HilsUartPort* port = uart_ports_[port_id]; + components::HilsUartPort* port = uart_ports_[port_id]; if (port == nullptr) return -1; int ret = port->WriteTx(buffer, offset, length); #ifdef HILS_PORT_MANAGER_SHOW_DEBUG_DATA @@ -132,7 +132,7 @@ int HilsPortManager::I2cTargetCloseComPort(unsigned int port_id) { return -1; } i2c_ports_[port_id]->ClosePort(); - HilsI2cTargetPort* port = i2c_ports_.at(port_id); + components::HilsI2cTargetPort* port = i2c_ports_.at(port_id); delete port; i2c_ports_.erase(port_id); return 0; @@ -146,7 +146,7 @@ int HilsPortManager::I2cTargetCloseComPort(unsigned int port_id) { int HilsPortManager::I2cTargetWriteRegister(unsigned int port_id, const unsigned char register_address, const unsigned char* data, const unsigned char length) { #ifdef USE_HILS - HilsI2cTargetPort* port = i2c_ports_[port_id]; + components::HilsI2cTargetPort* port = i2c_ports_[port_id]; if (port == nullptr) return -1; for (unsigned char i = 0; i < length; i++) { port->WriteRegister(register_address + i, data[i]); @@ -165,7 +165,7 @@ int HilsPortManager::I2cTargetWriteRegister(unsigned int port_id, const unsigned int HilsPortManager::I2cTargetReadRegister(unsigned int port_id, const unsigned char register_address, unsigned char* data, const unsigned char length) { #ifdef USE_HILS - HilsI2cTargetPort* port = i2c_ports_[port_id]; + components::HilsI2cTargetPort* port = i2c_ports_[port_id]; if (port == nullptr) return -1; for (unsigned char i = 0; i < length; i++) { data[i] = port->ReadRegister(register_address + i); @@ -183,7 +183,7 @@ int HilsPortManager::I2cTargetReadRegister(unsigned int port_id, const unsigned int HilsPortManager::I2cTargetReadCommand(unsigned int port_id, unsigned char* data, const unsigned char length) { #ifdef USE_HILS - HilsI2cTargetPort* port = i2c_ports_[port_id]; + components::HilsI2cTargetPort* port = i2c_ports_[port_id]; if (port == nullptr) return -1; port->ReadCommand(data, length); return 0; @@ -198,7 +198,7 @@ int HilsPortManager::I2cTargetReadCommand(unsigned int port_id, unsigned char* d int HilsPortManager::I2cTargetReceive(unsigned int port_id) { #ifdef USE_HILS - HilsI2cTargetPort* port = i2c_ports_[port_id]; + components::HilsI2cTargetPort* port = i2c_ports_[port_id]; if (port == nullptr) return -1; int ret = port->Receive(); #ifdef HILS_PORT_MANAGER_SHOW_DEBUG_DATA @@ -216,7 +216,7 @@ int HilsPortManager::I2cTargetReceive(unsigned int port_id) { int HilsPortManager::I2cTargetSend(unsigned int port_id, const unsigned char length) { #ifdef USE_HILS - HilsI2cTargetPort* port = i2c_ports_[port_id]; + components::HilsI2cTargetPort* port = i2c_ports_[port_id]; if (port == nullptr) return -1; int ret = port->Send(length); #ifdef HILS_PORT_MANAGER_SHOW_DEBUG_DATA @@ -235,7 +235,7 @@ int HilsPortManager::I2cTargetSend(unsigned int port_id, const unsigned char len int HilsPortManager::I2cTargetGetStoredFrameCounter(unsigned int port_id) { #ifdef USE_HILS - HilsI2cTargetPort* port = i2c_ports_[port_id]; + components::HilsI2cTargetPort* port = i2c_ports_[port_id]; if (port == nullptr) return -1; return port->GetStoredFrameCounter(); #else diff --git a/src/simulation/hils/hils_port_manager.hpp b/src/simulation/hils/hils_port_manager.hpp index 06d00b2f7..8753429e6 100644 --- a/src/simulation/hils/hils_port_manager.hpp +++ b/src/simulation/hils/hils_port_manager.hpp @@ -165,8 +165,8 @@ class HilsPortManager { private: #ifdef USE_HILS - std::map uart_ports_; //!< UART ports - std::map i2c_ports_; //!< I2C ports + std::map uart_ports_; //!< UART ports + std::map i2c_ports_; //!< I2C ports #endif }; diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp index 1570aaa1a..6b7493ee4 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp @@ -54,7 +54,7 @@ class InitializedMonteCarloParameters { * @brief Set randomization parameters */ template - void SetRandomConfiguration(const s2e::math::Vector& mean_or_min, const s2e::math::Vector& sigma_or_max, + void SetRandomConfiguration(const math::Vector& mean_or_min, const math::Vector& sigma_or_max, RandomizationType random_type); // Getter @@ -63,12 +63,12 @@ class InitializedMonteCarloParameters { * @brief Get randomized vector value results */ template - void GetRandomizedVector(s2e::math::Vector& destination) const; + void GetRandomizedVector(math::Vector& destination) const; /** * @fn GetRandomizedQuaternion * @brief Get randomized quaternion results */ - void GetRandomizedQuaternion(s2e::math::Quaternion& destination) const; + void GetRandomizedQuaternion(math::Quaternion& destination) const; /** * @fn GetRandomizedScalar * @brief Get randomized value results @@ -177,38 +177,38 @@ class InitializedMonteCarloParameters { * @fn CalcCircularNormalUniform * @brief Calculate randomized value with CircularNormalUniform mode */ - void CalcCircularNormalUniform(s2e::math::Vector<2>& destination, double r_mean, double r_sigma, double theta_min, double theta_max); + void CalcCircularNormalUniform(math::Vector<2>& destination, double r_mean, double r_sigma, double theta_min, double theta_max); /** * @fn CalcCircularNormalNormal * @brief Calculate randomized value with CircularNormalNormal mode */ - void CalcCircularNormalNormal(s2e::math::Vector<2>& destination, double r_mean, double r_sigma, double theta_mean, double theta_sigma); + void CalcCircularNormalNormal(math::Vector<2>& destination, double r_mean, double r_sigma, double theta_mean, double theta_sigma); /** * @fn CalcSphericalNormalUniformUniform * @brief Calculate randomized value with SphericalNormalUniformUniform mode */ - void CalcSphericalNormalUniformUniform(s2e::math::Vector<3>& destination, double r_mean, double r_sigma, double theta_min, double theta_max, + void CalcSphericalNormalUniformUniform(math::Vector<3>& destination, double r_mean, double r_sigma, double theta_min, double theta_max, double phi_min, double phi_max); /** * @fn CalcSphericalNormalNormal * @brief Calculate randomized value with SphericalNormalNormal mode */ - void CalcSphericalNormalNormal(s2e::math::Vector<3>& destination, const s2e::math::Vector<3>& mean_vec); + void CalcSphericalNormalNormal(math::Vector<3>& destination, const math::Vector<3>& mean_vec); /** * @fn CalcQuaternionUniform * @brief Calculate randomized value with QuaternionUniform mode */ - void CalcQuaternionUniform(s2e::math::Quaternion& destination); + void CalcQuaternionUniform(math::Quaternion& destination); /** * @fn CalcQuaternionNormal * @brief Calculate randomized value with QuaternionNormal mode */ - void CalcQuaternionNormal(s2e::math::Quaternion& destination, double theta_sigma); + void CalcQuaternionNormal(math::Quaternion& destination, double theta_sigma); }; template -void InitializedMonteCarloParameters::SetRandomConfiguration(const s2e::math::Vector& mean_or_min, - const s2e::math::Vector& sigma_or_max, +void InitializedMonteCarloParameters::SetRandomConfiguration(const math::Vector& mean_or_min, + const math::Vector& sigma_or_max, InitializedMonteCarloParameters::RandomizationType random_type) { randomization_type_ = random_type; mean_or_min_.clear(); @@ -222,7 +222,7 @@ void InitializedMonteCarloParameters::SetRandomConfiguration(const s2e::math::Ve } template -void InitializedMonteCarloParameters::GetRandomizedVector(s2e::math::Vector& destination) const { +void InitializedMonteCarloParameters::GetRandomizedVector(math::Vector& destination) const { if (randomization_type_ == kNoRandomization) { ; } else if (NumElement > randomized_value_.size()) { diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp index 91560ea1b..720d678eb 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp @@ -13,7 +13,7 @@ namespace s2e::simulation { #define MAX_CHAR_NUM 256 MonteCarloSimulationExecutor* InitMonteCarloSimulation(std::string file_name) { - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); const char* section = "MONTE_CARLO_EXECUTION"; unsigned long long total_num_of_executions = ini_file.ReadInt(section, "number_of_executions"); diff --git a/src/simulation/multiple_spacecraft/relative_information.cpp b/src/simulation/multiple_spacecraft/relative_information.cpp index b5c85d32d..782b50295 100644 --- a/src/simulation/multiple_spacecraft/relative_information.cpp +++ b/src/simulation/multiple_spacecraft/relative_information.cpp @@ -53,28 +53,28 @@ std::string RelativeInformation::GetLogHeader() const { std::string str_tmp = ""; for (size_t target_spacecraft_id = 0; target_spacecraft_id < dynamics_database_.size(); target_spacecraft_id++) { for (size_t reference_spacecraft_id = 0; reference_spacecraft_id < target_spacecraft_id; reference_spacecraft_id++) { - str_tmp += WriteVector( + str_tmp += logger::WriteVector( "satellite" + std::to_string(target_spacecraft_id) + "_position_from_satellite" + std::to_string(reference_spacecraft_id), "i", "m", 3); } } for (size_t target_spacecraft_id = 0; target_spacecraft_id < dynamics_database_.size(); target_spacecraft_id++) { for (size_t reference_spacecraft_id = 0; reference_spacecraft_id < target_spacecraft_id; reference_spacecraft_id++) { - str_tmp += WriteVector( + str_tmp += logger::WriteVector( "satellite" + std::to_string(target_spacecraft_id) + "_velocity_from_satellite" + std::to_string(reference_spacecraft_id), "i", "m/s", 3); } } for (size_t target_spacecraft_id = 0; target_spacecraft_id < dynamics_database_.size(); target_spacecraft_id++) { for (size_t reference_spacecraft_id = 0; reference_spacecraft_id < target_spacecraft_id; reference_spacecraft_id++) { - str_tmp += WriteVector( + str_tmp += logger::WriteVector( "satellite" + std::to_string(target_spacecraft_id) + "_position_from_satellite" + std::to_string(reference_spacecraft_id), "rtn", "m", 3); } } for (size_t target_spacecraft_id = 0; target_spacecraft_id < dynamics_database_.size(); target_spacecraft_id++) { for (size_t reference_spacecraft_id = 0; reference_spacecraft_id < target_spacecraft_id; reference_spacecraft_id++) { - str_tmp += WriteVector( + str_tmp += logger::WriteVector( "satellite" + std::to_string(target_spacecraft_id) + "_velocity_from_satellite" + std::to_string(reference_spacecraft_id), "rtn", "m/s", 3); } } @@ -86,32 +86,32 @@ std::string RelativeInformation::GetLogValue() const { std::string str_tmp = ""; for (size_t target_spacecraft_id = 0; target_spacecraft_id < dynamics_database_.size(); target_spacecraft_id++) { for (size_t reference_spacecraft_id = 0; reference_spacecraft_id < target_spacecraft_id; reference_spacecraft_id++) { - str_tmp += WriteVector(GetRelativePosition_i_m(target_spacecraft_id, reference_spacecraft_id)); + str_tmp += logger::WriteVector(GetRelativePosition_i_m(target_spacecraft_id, reference_spacecraft_id)); } } for (size_t target_spacecraft_id = 0; target_spacecraft_id < dynamics_database_.size(); target_spacecraft_id++) { for (size_t reference_spacecraft_id = 0; reference_spacecraft_id < target_spacecraft_id; reference_spacecraft_id++) { - str_tmp += WriteVector(GetRelativeVelocity_i_m_s(target_spacecraft_id, reference_spacecraft_id)); + str_tmp += logger::WriteVector(GetRelativeVelocity_i_m_s(target_spacecraft_id, reference_spacecraft_id)); } } for (size_t target_spacecraft_id = 0; target_spacecraft_id < dynamics_database_.size(); target_spacecraft_id++) { for (size_t reference_spacecraft_id = 0; reference_spacecraft_id < target_spacecraft_id; reference_spacecraft_id++) { - str_tmp += WriteVector(GetRelativePosition_rtn_m(target_spacecraft_id, reference_spacecraft_id)); + str_tmp += logger::WriteVector(GetRelativePosition_rtn_m(target_spacecraft_id, reference_spacecraft_id)); } } for (size_t target_spacecraft_id = 0; target_spacecraft_id < dynamics_database_.size(); target_spacecraft_id++) { for (size_t reference_spacecraft_id = 0; reference_spacecraft_id < target_spacecraft_id; reference_spacecraft_id++) { - str_tmp += WriteVector(GetRelativeVelocity_rtn_m_s(target_spacecraft_id, reference_spacecraft_id)); + str_tmp += logger::WriteVector(GetRelativeVelocity_rtn_m_s(target_spacecraft_id, reference_spacecraft_id)); } } return str_tmp; } -void RelativeInformation::LogSetup(Logger& logger) { logger.AddLogList(this); } +void RelativeInformation::LogSetup(logger::Logger& logger) { logger.AddLogList(this); } s2e::math::Quaternion RelativeInformation::CalcRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { // Observer SC Body frame(obs_sat) -> ECI frame(i) diff --git a/src/simulation/multiple_spacecraft/relative_information.hpp b/src/simulation/multiple_spacecraft/relative_information.hpp index a5cbe1630..b68cab097 100644 --- a/src/simulation/multiple_spacecraft/relative_information.hpp +++ b/src/simulation/multiple_spacecraft/relative_information.hpp @@ -18,7 +18,7 @@ namespace s2e::simulation { * @class RelativeInformation * @brief Base class to manage relative information between spacecraft */ -class RelativeInformation : public ILoggable { +class RelativeInformation : public logger::ILoggable { public: /** * @fn RelativeInformation @@ -42,7 +42,7 @@ class RelativeInformation : public ILoggable { * @param [in] spacecraft_id: ID of target spacecraft * @param [in] dynamics: Dynamics information of the target spacecraft */ - void RegisterDynamicsInfo(const size_t spacecraft_id, const Dynamics* dynamics); + void RegisterDynamicsInfo(const size_t spacecraft_id, const dynamics::Dynamics* dynamics); /** * @fn RegisterDynamicsInfo * @brief Remove dynamics information of target spacecraft @@ -66,7 +66,7 @@ class RelativeInformation : public ILoggable { * @fn LogSetup * @brief Logging setup for relative information */ - void LogSetup(Logger& logger); + void LogSetup(logger::Logger& logger); // Getter /** @@ -129,12 +129,12 @@ class RelativeInformation : public ILoggable { * @brief Return the dynamics information of a spacecraft * @param [in] target_spacecraft_id: ID of the spacecraft */ - inline const Dynamics* GetReferenceSatDynamics(const size_t reference_spacecraft_id) const { + inline const dynamics::Dynamics* GetReferenceSatDynamics(const size_t reference_spacecraft_id) const { return dynamics_database_.at(reference_spacecraft_id); }; private: - std::map dynamics_database_; //!< Dynamics database of all spacecraft + std::map dynamics_database_; //!< Dynamics database of all spacecraft std::vector>> relative_position_list_i_m_; //!< Relative position list in the inertial frame in unit [m] std::vector>> relative_velocity_list_i_m_s_; //!< Relative velocity list in the inertial frame in unit [m/s] diff --git a/src/simulation/simulation_configuration.hpp b/src/simulation/simulation_configuration.hpp index 599b6ec4e..d8ca3078f 100644 --- a/src/simulation/simulation_configuration.hpp +++ b/src/simulation/simulation_configuration.hpp @@ -19,7 +19,7 @@ namespace s2e::simulation { */ struct SimulationConfiguration { std::string initialize_base_file_name_; //!< Base file name for initialization - Logger* main_logger_; //!< Main logger + logger::Logger* main_logger_; //!< Main logger unsigned int number_of_simulated_spacecraft_; //!< Number of simulated spacecraft std::vector spacecraft_file_list_; //!< File name list for spacecraft initialization diff --git a/src/simulation/spacecraft/installed_components.cpp b/src/simulation/spacecraft/installed_components.cpp index ff7ce9b5c..ebee0e571 100644 --- a/src/simulation/spacecraft/installed_components.cpp +++ b/src/simulation/spacecraft/installed_components.cpp @@ -19,6 +19,6 @@ s2e::math::Vector<3> InstalledComponents::GenerateTorque_b_Nm() { return torque_b_Nm_; } -void InstalledComponents::LogSetup(Logger& logger) { UNUSED(logger); } +void InstalledComponents::LogSetup(logger::Logger& logger) { UNUSED(logger); } } // namespace s2e::simulation diff --git a/src/simulation/spacecraft/installed_components.hpp b/src/simulation/spacecraft/installed_components.hpp index 6e5a3c61c..f357daca3 100644 --- a/src/simulation/spacecraft/installed_components.hpp +++ b/src/simulation/spacecraft/installed_components.hpp @@ -48,7 +48,7 @@ class InstalledComponents { * @brief Setup the logger for components * @details Users need to override this function to add logger for components */ - virtual void LogSetup(Logger& logger); + virtual void LogSetup(logger::Logger& logger); }; } // namespace s2e::simulation diff --git a/src/simulation/spacecraft/spacecraft.cpp b/src/simulation/spacecraft/spacecraft.cpp index c80950dd5..d99b1a1a5 100644 --- a/src/simulation/spacecraft/spacecraft.cpp +++ b/src/simulation/spacecraft/spacecraft.cpp @@ -10,7 +10,7 @@ namespace s2e::simulation { -Spacecraft::Spacecraft(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id, +Spacecraft::Spacecraft(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const int spacecraft_id, RelativeInformation* relative_information) : spacecraft_id_(spacecraft_id) { Initialize(simulation_configuration, global_environment, spacecraft_id, relative_information); @@ -27,14 +27,14 @@ Spacecraft::~Spacecraft() { delete components_; } -void Spacecraft::Initialize(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, +void Spacecraft::Initialize(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const int spacecraft_id, RelativeInformation* relative_information) { clock_generator_.ClearTimerCount(); structure_ = new Structure(simulation_configuration, spacecraft_id); - local_environment_ = new LocalEnvironment(simulation_configuration, global_environment, spacecraft_id); - dynamics_ = new Dynamics(simulation_configuration, &(global_environment->GetSimulationTime()), local_environment_, spacecraft_id, structure_, + local_environment_ = new environment::LocalEnvironment(simulation_configuration, global_environment, spacecraft_id); + dynamics_ = new dynamics::Dynamics(simulation_configuration, &(global_environment->GetSimulationTime()), local_environment_, spacecraft_id, structure_, relative_information); - disturbances_ = new Disturbances(simulation_configuration, spacecraft_id, structure_, global_environment); + disturbances_ = new disturbances::Disturbances(simulation_configuration, spacecraft_id, structure_, global_environment); simulation_configuration->main_logger_->CopyFileToLogDirectory(simulation_configuration->spacecraft_file_list_[spacecraft_id]); @@ -44,14 +44,14 @@ void Spacecraft::Initialize(const SimulationConfiguration* simulation_configurat } } -void Spacecraft::LogSetup(Logger& logger) { +void Spacecraft::LogSetup(logger::Logger& logger) { dynamics_->LogSetup(logger); local_environment_->LogSetup(logger); disturbances_->LogSetup(logger); components_->LogSetup(logger); } -void Spacecraft::Update(const SimulationTime* simulation_time) { +void Spacecraft::Update(const environment::SimulationTime* simulation_time) { dynamics_->ClearForceTorque(); // Update local environment and disturbance diff --git a/src/simulation/spacecraft/spacecraft.hpp b/src/simulation/spacecraft/spacecraft.hpp index a59ea4eaa..88f612246 100644 --- a/src/simulation/spacecraft/spacecraft.hpp +++ b/src/simulation/spacecraft/spacecraft.hpp @@ -27,7 +27,7 @@ class Spacecraft { * @fn Spacecraft * @brief Constructor for single satellite simulation */ - Spacecraft(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id, + Spacecraft(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const int spacecraft_id, RelativeInformation* relative_information = nullptr); /** @@ -45,14 +45,14 @@ class Spacecraft { * @fn Initialize * @brief Initialize function for multiple spacecraft simulation */ - virtual void Initialize(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, + virtual void Initialize(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const int spacecraft_id, RelativeInformation* relative_information = nullptr); /** * @fn Update * @brief Update all states related with the spacecraft */ - virtual void Update(const SimulationTime* simulation_time); + virtual void Update(const environment::SimulationTime* simulation_time); /** * @fn Clear @@ -64,24 +64,24 @@ class Spacecraft { * @fn LogSetup * @brief Logger setting for the spacecraft specific information */ - virtual void LogSetup(Logger& logger); + virtual void LogSetup(logger::Logger& logger); // Getters /** * @fn GetDynamics * @brief Get dynamics of the spacecraft */ - inline const Dynamics& GetDynamics() const { return *dynamics_; } + inline const dynamics::Dynamics& GetDynamics() const { return *dynamics_; } /** * @fn GetLocalEnvironment * @brief Get local environment around the spacecraft */ - inline const LocalEnvironment& GetLocalEnvironment() const { return *local_environment_; } + inline const environment::LocalEnvironment& GetLocalEnvironment() const { return *local_environment_; } /** * @fn GetDisturbances * @brief Get disturbance acting of the spacecraft */ - inline const Disturbances& GetDisturbances() const { return *disturbances_; } + inline const disturbances::Disturbances& GetDisturbances() const { return *disturbances_; } /** * @fn GetStructure * @brief Get structure of the spacecraft @@ -99,11 +99,11 @@ class Spacecraft { inline unsigned int GetSpacecraftId() const { return spacecraft_id_; } protected: - ClockGenerator clock_generator_; //!< Origin of clock for the spacecraft - Dynamics* dynamics_; //!< Dynamics information of the spacecraft + environment::ClockGenerator clock_generator_; //!< Origin of clock for the spacecraft + dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft RelativeInformation* relative_information_; //!< Relative information with respect to the other spacecraft - LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft - Disturbances* disturbances_; //!< Disturbance information acting on the spacecraft + environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft + disturbances::Disturbances* disturbances_; //!< Disturbance information acting on the spacecraft Structure* structure_; //!< Structure information of the spacecraft InstalledComponents* components_; //!< Components information installed on the spacecraft const unsigned int spacecraft_id_; //!< ID of the spacecraft diff --git a/src/simulation/spacecraft/structure/initialize_structure.cpp b/src/simulation/spacecraft/structure/initialize_structure.cpp index 543203f75..5eaa7c863 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.cpp +++ b/src/simulation/spacecraft/structure/initialize_structure.cpp @@ -12,7 +12,7 @@ namespace s2e::simulation { #define MIN_VAL 1e-6 KinematicsParameters InitKinematicsParameters(std::string file_name) { - auto conf = IniAccess(file_name); + auto conf = setting_file_reader::IniAccess(file_name); const char* section = "KINEMATIC_PARAMETERS"; s2e::math::Vector<3> center_of_gravity_b_m; @@ -34,7 +34,7 @@ KinematicsParameters InitKinematicsParameters(std::string file_name) { std::vector InitSurfaces(std::string file_name) { using std::cout; - auto conf = IniAccess(file_name); + auto conf = setting_file_reader::IniAccess(file_name); const char* section = "SURFACES"; const int num_surface = conf.ReadInt(section, "number_of_surfaces"); @@ -109,7 +109,7 @@ std::vector InitSurfaces(std::string file_name) { } ResidualMagneticMoment InitResidualMagneticMoment(std::string file_name) { - auto conf = IniAccess(file_name); + auto conf = setting_file_reader::IniAccess(file_name); const char* section = "RESIDUAL_MAGNETIC_MOMENT"; s2e::math::Vector<3> rmm_const_b; diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp index 750db9a55..b66d7c90d 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp @@ -7,7 +7,6 @@ #define S2E_SIMULATION_SPACECRAFT_STRUCTURE_RESIDUAL_MAGNETIC_MOMENT_HPP_ #include -using s2e::math::Vector; namespace s2e::simulation { @@ -57,16 +56,16 @@ class ResidualMagneticMoment { * @brief Set Constant value of RMM at body frame [Am2] * @param [in] rmm_const_b_Am2: Constant value of RMM at the body frame [Am2] */ - inline void SetRmmConstant_b_Am2(const Vector<3> rmm_const_b_Am2) { constant_value_b_Am2_ = rmm_const_b_Am2; } + inline void SetRmmConstant_b_Am2(const math::Vector<3> rmm_const_b_Am2) { constant_value_b_Am2_ = rmm_const_b_Am2; } /** * @fn AddRMMConst_b_Am2 * @brief Add Constant value of RMM at body frame [Am2] * @param [in] rmm_const_b_Am2: Constant value of RMM at the body frame [Am2] */ - inline void AddRmmConstant_b_Am2(const Vector<3> rmm_const_b_Am2) { constant_value_b_Am2_ += rmm_const_b_Am2; } + inline void AddRmmConstant_b_Am2(const math::Vector<3> rmm_const_b_Am2) { constant_value_b_Am2_ += rmm_const_b_Am2; } private: - Vector<3> constant_value_b_Am2_; //!< Constant value of RMM at body frame [Am2] + math::Vector<3> constant_value_b_Am2_; //!< Constant value of RMM at body frame [Am2] double random_walk_standard_deviation_Am2_; //!< Random walk standard deviation of RMM [Am2] double random_walk_limit_Am2_; //!< Random walk limit of RMM [Am2] double random_noise_standard_deviation_Am2_; //!< Standard deviation of white noise of RMM [Am2] diff --git a/src/simulation/spacecraft/structure/structure.cpp b/src/simulation/spacecraft/structure/structure.cpp index 2958437de..ffbe55e05 100644 --- a/src/simulation/spacecraft/structure/structure.cpp +++ b/src/simulation/spacecraft/structure/structure.cpp @@ -21,7 +21,7 @@ Structure::~Structure() { void Structure::Initialize(const SimulationConfiguration* simulation_configuration, const int spacecraft_id) { // Read file name - IniAccess conf = IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); + setting_file_reader::IniAccess conf = setting_file_reader::IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); std::string ini_fname = conf.ReadString("SETTING_FILES", "structure_file"); // Save ini file simulation_configuration->main_logger_->CopyFileToLogDirectory(ini_fname); From 09970898af93b8e4ee7de0e5d3945559d4eec002 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 15:37:50 +0900 Subject: [PATCH 33/49] Update namespace info in s2e --- src/s2e.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/s2e.cpp b/src/s2e.cpp index 7861f38af..853b04bee 100644 --- a/src/s2e.cpp +++ b/src/s2e.cpp @@ -71,7 +71,7 @@ int main(int argc, char *argv[]) std::cout << "\tIni file: "; print_path(ini_file); - auto simulation_case = SampleCase(ini_file); + auto simulation_case = sample::SampleCase(ini_file); simulation_case.Initialize(); simulation_case.Main(); From 10c18e1236b69248968b39167e180dfaf0f86540 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 15:44:10 +0900 Subject: [PATCH 34/49] Update namespace info in components/base --- src/components/base/component.cpp | 4 ++-- src/components/base/component.hpp | 6 +++--- src/components/base/i2c_controller.cpp | 2 +- src/components/base/i2c_controller.hpp | 4 ++-- src/components/base/i2c_target_communication_with_obc.cpp | 4 ++-- src/components/base/i2c_target_communication_with_obc.hpp | 6 +++--- src/components/base/uart_communication_with_obc.cpp | 6 +++--- src/components/base/uart_communication_with_obc.hpp | 8 ++++---- src/components/examples/example_change_structure.cpp | 6 +++--- src/components/examples/example_change_structure.hpp | 6 +++--- .../examples/example_i2c_controller_for_hils.cpp | 4 ++-- .../examples/example_i2c_controller_for_hils.hpp | 4 ++-- src/components/examples/example_i2c_target_for_hils.cpp | 4 ++-- src/components/examples/example_i2c_target_for_hils.hpp | 4 ++-- .../examples/example_serial_communication_for_hils.cpp | 4 ++-- .../examples/example_serial_communication_for_hils.hpp | 4 ++-- .../examples/example_serial_communication_with_obc.cpp | 4 ++-- .../examples/example_serial_communication_with_obc.hpp | 4 ++-- 18 files changed, 42 insertions(+), 42 deletions(-) diff --git a/src/components/base/component.cpp b/src/components/base/component.cpp index df61cb1ce..65f9f88ae 100644 --- a/src/components/base/component.cpp +++ b/src/components/base/component.cpp @@ -7,7 +7,7 @@ namespace s2e::components { -Component::Component(const unsigned int prescaler, ClockGenerator* clock_generator, const unsigned int fast_prescaler) +Component::Component(const unsigned int prescaler, environment::ClockGenerator* clock_generator, const unsigned int fast_prescaler) : clock_generator_(clock_generator) { power_port_ = new PowerPort(); clock_generator_->RegisterComponent(this); @@ -15,7 +15,7 @@ Component::Component(const unsigned int prescaler, ClockGenerator* clock_generat fast_prescaler_ = (fast_prescaler > 0) ? fast_prescaler : 1; } -Component::Component(const unsigned int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const unsigned int fast_prescaler) +Component::Component(const unsigned int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const unsigned int fast_prescaler) : clock_generator_(clock_generator), power_port_(power_port) { clock_generator_->RegisterComponent(this); prescaler_ = (prescaler > 0) ? prescaler : 1; diff --git a/src/components/base/component.hpp b/src/components/base/component.hpp index 72dbf6c6c..25c035415 100644 --- a/src/components/base/component.hpp +++ b/src/components/base/component.hpp @@ -29,7 +29,7 @@ class Component : public ITickable { * @param [in] clock_generator: Clock generator * @param [in] fast_prescaler: Frequency scale factor for fast update (used only for component faster than component update period) */ - Component(const unsigned int prescaler, ClockGenerator* clock_generator, const unsigned int fast_prescaler = 1); + Component(const unsigned int prescaler, environment::ClockGenerator* clock_generator, const unsigned int fast_prescaler = 1); /** * @fn Component * @brief Constructor with power port @@ -38,7 +38,7 @@ class Component : public ITickable { * @param [in] power_port: Power port * @param [in] fast_prescaler: Frequency scale factor for fast update (used only for component faster than component update period) */ - Component(const unsigned int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const unsigned int fast_prescaler = 1); + Component(const unsigned int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const unsigned int fast_prescaler = 1); /** * @fn Component * @brief Copy constructor @@ -86,7 +86,7 @@ class Component : public ITickable { */ virtual void PowerOffRoutine(){}; - ClockGenerator* clock_generator_; //!< Clock generator + environment::ClockGenerator* clock_generator_; //!< Clock generator PowerPort* power_port_; //!< Power port }; diff --git a/src/components/base/i2c_controller.cpp b/src/components/base/i2c_controller.cpp index 6cc80c056..400c88ae3 100644 --- a/src/components/base/i2c_controller.cpp +++ b/src/components/base/i2c_controller.cpp @@ -11,7 +11,7 @@ namespace s2e::components { I2cController::I2cController(const unsigned int hils_port_id, const unsigned int baud_rate, const unsigned int tx_buffer_size, - const unsigned int rx_buffer_size, HilsPortManager* hils_port_manager) + const unsigned int rx_buffer_size, simulation::HilsPortManager* hils_port_manager) : hils_port_id_(hils_port_id), baud_rate_(baud_rate), tx_buffer_size_(tx_buffer_size), diff --git a/src/components/base/i2c_controller.hpp b/src/components/base/i2c_controller.hpp index bd1e5e2d2..f300a9710 100644 --- a/src/components/base/i2c_controller.hpp +++ b/src/components/base/i2c_controller.hpp @@ -30,7 +30,7 @@ class I2cController { * @param [in] hils_port_manager: HILS port manager */ I2cController(const unsigned int hils_port_id, const unsigned int baud_rate, const unsigned int tx_buffer_size, const unsigned int rx_buffer_size, - HilsPortManager* hils_port_manager); + simulation::HilsPortManager* hils_port_manager); /** * @fn ~I2cController * @brief Destructor @@ -63,7 +63,7 @@ class I2cController { unsigned int rx_buffer_size_; //!< RX (Target to Controller) buffer size SimulationMode simulation_mode_ = SimulationMode::kError; //!< Simulation mode (SILS or HILS) - HilsPortManager* hils_port_manager_; //!< HILS port manager + simulation::HilsPortManager* hils_port_manager_; //!< HILS port manager }; } // namespace s2e::components diff --git a/src/components/base/i2c_target_communication_with_obc.cpp b/src/components/base/i2c_target_communication_with_obc.cpp index a916b0df7..7aefe1704 100644 --- a/src/components/base/i2c_target_communication_with_obc.cpp +++ b/src/components/base/i2c_target_communication_with_obc.cpp @@ -20,7 +20,7 @@ I2cTargetCommunicationWithObc::I2cTargetCommunicationWithObc(const unsigned int } I2cTargetCommunicationWithObc::I2cTargetCommunicationWithObc(const unsigned int hils_port_id, const unsigned char i2c_address, - HilsPortManager* hils_port_manager) + simulation::HilsPortManager* hils_port_manager) : hils_port_id_(hils_port_id), i2c_address_(i2c_address), hils_port_manager_(hils_port_manager) { #ifdef USE_HILS simulation_mode_ = SimulationMode::kHils; @@ -36,7 +36,7 @@ I2cTargetCommunicationWithObc::I2cTargetCommunicationWithObc(const unsigned int I2cTargetCommunicationWithObc::I2cTargetCommunicationWithObc(const unsigned int sils_port_id, const unsigned int hils_port_id, const unsigned char i2c_address, OnBoardComputer* obc, - HilsPortManager* hils_port_manager) + simulation::HilsPortManager* hils_port_manager) : sils_port_id_(sils_port_id), hils_port_id_(hils_port_id), i2c_address_(i2c_address), obc_(obc), hils_port_manager_(hils_port_manager) { #ifdef USE_HILS simulation_mode_ = SimulationMode::kHils; diff --git a/src/components/base/i2c_target_communication_with_obc.hpp b/src/components/base/i2c_target_communication_with_obc.hpp index 4d017aafe..61a1b3e34 100644 --- a/src/components/base/i2c_target_communication_with_obc.hpp +++ b/src/components/base/i2c_target_communication_with_obc.hpp @@ -34,7 +34,7 @@ class I2cTargetCommunicationWithObc { * @param [in] i2c_address: I2C address for the target * @param [in] hils_port_manager: HILS port manager */ - I2cTargetCommunicationWithObc(const unsigned int hils_port_id, const unsigned char i2c_address, HilsPortManager* hils_port_manager); + I2cTargetCommunicationWithObc(const unsigned int hils_port_id, const unsigned char i2c_address, simulation::HilsPortManager* hils_port_manager); /** * @fn I2cTargetCommunicationWithObc * @brief Constructor for both SILS and HILS mode @@ -45,7 +45,7 @@ class I2cTargetCommunicationWithObc { * @param [in] hils_port_manager: HILS port manager */ I2cTargetCommunicationWithObc(const unsigned int sils_port_id, const unsigned int hils_port_id, const unsigned char i2c_address, - OnBoardComputer* obc, HilsPortManager* hils_port_manager); + OnBoardComputer* obc, simulation::HilsPortManager* hils_port_manager); /** * @fn I2cTargetCommunicationWithObc * @brief Prevent double freeing of memory when this class is copied @@ -120,7 +120,7 @@ class I2cTargetCommunicationWithObc { SimulationMode simulation_mode_ = SimulationMode::kError; //!< Simulation mode OnBoardComputer* obc_; //!< Communication target OBC - HilsPortManager* hils_port_manager_; //!< HILS port manager + simulation::HilsPortManager* hils_port_manager_; //!< HILS port manager }; } // namespace s2e::components diff --git a/src/components/base/uart_communication_with_obc.cpp b/src/components/base/uart_communication_with_obc.cpp index 41bb777fd..075b67944 100644 --- a/src/components/base/uart_communication_with_obc.cpp +++ b/src/components/base/uart_communication_with_obc.cpp @@ -35,7 +35,7 @@ UartCommunicationWithObc::UartCommunicationWithObc(const unsigned int sils_port_ InitializeObcComBase(); } -UartCommunicationWithObc::UartCommunicationWithObc(const unsigned int hils_port_id, const unsigned int baud_rate, HilsPortManager* hils_port_manager) +UartCommunicationWithObc::UartCommunicationWithObc(const unsigned int hils_port_id, const unsigned int baud_rate, simulation::HilsPortManager* hils_port_manager) : hils_port_id_(hils_port_id), baud_rate_(baud_rate), hils_port_manager_(hils_port_manager) { #ifdef USE_HILS simulation_mode_ = SimulationMode::kHils; @@ -49,7 +49,7 @@ UartCommunicationWithObc::UartCommunicationWithObc(const unsigned int hils_port_ } UartCommunicationWithObc::UartCommunicationWithObc(const unsigned int hils_port_id, const unsigned int baud_rate, const unsigned int tx_buffer_size, - const unsigned int rx_buffer_size, HilsPortManager* hils_port_manager) + const unsigned int rx_buffer_size, simulation::HilsPortManager* hils_port_manager) : hils_port_id_(hils_port_id), baud_rate_(baud_rate), tx_buffer_size_(tx_buffer_size), @@ -67,7 +67,7 @@ UartCommunicationWithObc::UartCommunicationWithObc(const unsigned int hils_port_ } UartCommunicationWithObc::UartCommunicationWithObc(const int sils_port_id, OnBoardComputer* obc, const unsigned int hils_port_id, - const unsigned int baud_rate, HilsPortManager* hils_port_manager) + const unsigned int baud_rate, simulation::HilsPortManager* hils_port_manager) : sils_port_id_(sils_port_id), hils_port_id_(hils_port_id), baud_rate_(baud_rate), obc_(obc), hils_port_manager_(hils_port_manager) { #ifdef USE_HILS simulation_mode_ = SimulationMode::kHils; diff --git a/src/components/base/uart_communication_with_obc.hpp b/src/components/base/uart_communication_with_obc.hpp index 6cb45c5df..caad9f1c9 100644 --- a/src/components/base/uart_communication_with_obc.hpp +++ b/src/components/base/uart_communication_with_obc.hpp @@ -56,7 +56,7 @@ class UartCommunicationWithObc { * @param [in] baud_rate: Baud rate of HILS communication port * @param [in] hils_port_manager: HILS port manager */ - UartCommunicationWithObc(const unsigned int hils_port_id, const unsigned int baud_rate, HilsPortManager* hils_port_manager); + UartCommunicationWithObc(const unsigned int hils_port_id, const unsigned int baud_rate, simulation::HilsPortManager* hils_port_manager); /** * @fn UartCommunicationWithObc * @brief Constructor for HILS mode @@ -67,7 +67,7 @@ class UartCommunicationWithObc { * @param [in] hils_port_manager: HILS port manager */ UartCommunicationWithObc(const unsigned int hils_port_id, const unsigned int baud_rate, const unsigned int tx_buffer_size, - const unsigned int rx_buffer_size, HilsPortManager* hils_port_manager); + const unsigned int rx_buffer_size, simulation::HilsPortManager* hils_port_manager); /** * @fn UartCommunicationWithObc * @brief Constructor for both SILS and HILS mode @@ -79,7 +79,7 @@ class UartCommunicationWithObc { * @param [in] hils_port_manager: HILS port manager */ UartCommunicationWithObc(const int sils_port_id, OnBoardComputer* obc, const unsigned int hils_port_id, const unsigned int baud_rate, - HilsPortManager* hils_port_manager); + simulation::HilsPortManager* hils_port_manager); /** * @fn ~UartCommunicationWithObc * @brief Destructor @@ -110,7 +110,7 @@ class UartCommunicationWithObc { SimulationMode simulation_mode_ = SimulationMode::kError; //!< Simulation mode OnBoardComputer* obc_; //!< Communication target OBC - HilsPortManager* hils_port_manager_; //!< HILS port manager + simulation::HilsPortManager* hils_port_manager_; //!< HILS port manager /** * @fn InitializeObcComBase diff --git a/src/components/examples/example_change_structure.cpp b/src/components/examples/example_change_structure.cpp index 9f1d6d051..0ee982771 100644 --- a/src/components/examples/example_change_structure.cpp +++ b/src/components/examples/example_change_structure.cpp @@ -9,7 +9,7 @@ namespace s2e::components { -ExampleChangeStructure::ExampleChangeStructure(ClockGenerator* clock_generator, Structure* structure) +ExampleChangeStructure::ExampleChangeStructure(environment::ClockGenerator* clock_generator, simulation::Structure* structure) : Component(1, clock_generator), structure_(structure) {} ExampleChangeStructure::~ExampleChangeStructure() {} @@ -20,14 +20,14 @@ void ExampleChangeStructure::MainRoutine(const int time_count) { structure_->GetToSetKinematicsParameters().SetMass_kg(100.0); // Center of gravity - Vector<3> cg(0.0); + math::Vector<3> cg(0.0); cg[0] = 0.01; cg[1] = -0.01; cg[2] = 0.02; structure_->GetToSetKinematicsParameters().SetCenterOfGravityVector_b_m(cg); // RMM - Vector<3> rmm(0.0); + math::Vector<3> rmm(0.0); rmm[0] = 0.1; rmm[1] = -0.1; rmm[2] = 0.2; diff --git a/src/components/examples/example_change_structure.hpp b/src/components/examples/example_change_structure.hpp index 93d749b61..bdc4f74c5 100644 --- a/src/components/examples/example_change_structure.hpp +++ b/src/components/examples/example_change_structure.hpp @@ -17,7 +17,7 @@ namespace s2e::components { * @class ExampleChangeStructure * @brief Class to show an example to change satellite structure information */ -class ExampleChangeStructure : public Component, public ILoggable { +class ExampleChangeStructure : public Component, public logger::ILoggable { public: /** * @fn ExampleChangeStructure @@ -25,7 +25,7 @@ class ExampleChangeStructure : public Component, public ILoggable { * @param [in] clock_generator: Clock generator * @param [in] structure: Structure information */ - ExampleChangeStructure(ClockGenerator* clock_generator, Structure* structure); + ExampleChangeStructure(environment::ClockGenerator* clock_generator, simulation::Structure* structure); /** * @fn ~ChangeStructure * @brief Destructor @@ -52,7 +52,7 @@ class ExampleChangeStructure : public Component, public ILoggable { virtual std::string GetLogValue() const override; protected: - Structure* structure_; //!< Structure information + simulation::Structure* structure_; //!< Structure information }; } // namespace s2e::components diff --git a/src/components/examples/example_i2c_controller_for_hils.cpp b/src/components/examples/example_i2c_controller_for_hils.cpp index b4f2bace1..c67f5fe8e 100644 --- a/src/components/examples/example_i2c_controller_for_hils.cpp +++ b/src/components/examples/example_i2c_controller_for_hils.cpp @@ -6,9 +6,9 @@ namespace s2e::components { -ExampleI2cControllerForHils::ExampleI2cControllerForHils(const int prescaler, ClockGenerator* clock_generator, const unsigned int hils_port_id, +ExampleI2cControllerForHils::ExampleI2cControllerForHils(const int prescaler, environment::ClockGenerator* clock_generator, const unsigned int hils_port_id, const unsigned int baud_rate, const unsigned int tx_buffer_size, - const unsigned int rx_buffer_size, HilsPortManager* hils_port_manager) + const unsigned int rx_buffer_size, simulation::HilsPortManager* hils_port_manager) : Component(prescaler, clock_generator), I2cController(hils_port_id, baud_rate, tx_buffer_size, rx_buffer_size, hils_port_manager) {} ExampleI2cControllerForHils::~ExampleI2cControllerForHils() {} diff --git a/src/components/examples/example_i2c_controller_for_hils.hpp b/src/components/examples/example_i2c_controller_for_hils.hpp index 1e5796c39..84537cf6a 100644 --- a/src/components/examples/example_i2c_controller_for_hils.hpp +++ b/src/components/examples/example_i2c_controller_for_hils.hpp @@ -33,8 +33,8 @@ class ExampleI2cControllerForHils : public Component, public I2cController { * @param [in] rx_buffer_size: RX (Target to Controller) buffer size * @param [in] hils_port_manager: HILS port manager */ - ExampleI2cControllerForHils(const int prescaler, ClockGenerator* clock_generator, const unsigned int hils_port_id, const unsigned int baud_rate, - const unsigned int tx_buffer_size, const unsigned int rx_buffer_size, HilsPortManager* hils_port_manager); + ExampleI2cControllerForHils(const int prescaler, environment::ClockGenerator* clock_generator, const unsigned int hils_port_id, const unsigned int baud_rate, + const unsigned int tx_buffer_size, const unsigned int rx_buffer_size, simulation::HilsPortManager* hils_port_manager); /** * @fn ~ExampleI2cControllerForHils * @brief Destructor diff --git a/src/components/examples/example_i2c_target_for_hils.cpp b/src/components/examples/example_i2c_target_for_hils.cpp index 0fc2a2380..b278d7313 100644 --- a/src/components/examples/example_i2c_target_for_hils.cpp +++ b/src/components/examples/example_i2c_target_for_hils.cpp @@ -7,9 +7,9 @@ namespace s2e::components { -ExampleI2cTargetForHils::ExampleI2cTargetForHils(const int prescaler, ClockGenerator* clock_generator, const int sils_port_id, +ExampleI2cTargetForHils::ExampleI2cTargetForHils(const int prescaler, environment::ClockGenerator* clock_generator, const int sils_port_id, unsigned char i2c_address, OnBoardComputer* obc, const unsigned int hils_port_id, - HilsPortManager* hils_port_manager) + simulation::HilsPortManager* hils_port_manager) : Component(prescaler, clock_generator), I2cTargetCommunicationWithObc(sils_port_id, hils_port_id, i2c_address, obc, hils_port_manager) {} ExampleI2cTargetForHils::~ExampleI2cTargetForHils() {} diff --git a/src/components/examples/example_i2c_target_for_hils.hpp b/src/components/examples/example_i2c_target_for_hils.hpp index ca03d3690..0e6faf2bd 100644 --- a/src/components/examples/example_i2c_target_for_hils.hpp +++ b/src/components/examples/example_i2c_target_for_hils.hpp @@ -34,8 +34,8 @@ class ExampleI2cTargetForHils : public Component, public I2cTargetCommunicationW * @param [in] hils_port_id: ID of HILS communication port * @param [in] hils_port_manager: HILS port manager */ - ExampleI2cTargetForHils(const int prescaler, ClockGenerator* clock_generator, const int sils_port_id, unsigned char i2c_address, - OnBoardComputer* obc, const unsigned int hils_port_id, HilsPortManager* hils_port_manager); + ExampleI2cTargetForHils(const int prescaler, environment::ClockGenerator* clock_generator, const int sils_port_id, unsigned char i2c_address, + OnBoardComputer* obc, const unsigned int hils_port_id, simulation::HilsPortManager* hils_port_manager); /** * @fn ~ExampleI2cTargetForHils * @brief Destructor diff --git a/src/components/examples/example_serial_communication_for_hils.cpp b/src/components/examples/example_serial_communication_for_hils.cpp index d4890360d..3c84e0b2b 100644 --- a/src/components/examples/example_serial_communication_for_hils.cpp +++ b/src/components/examples/example_serial_communication_for_hils.cpp @@ -8,9 +8,9 @@ namespace s2e::components { -ExampleSerialCommunicationForHils::ExampleSerialCommunicationForHils(ClockGenerator* clock_generator, const int sils_port_id, OnBoardComputer* obc, +ExampleSerialCommunicationForHils::ExampleSerialCommunicationForHils(environment::ClockGenerator* clock_generator, const int sils_port_id, OnBoardComputer* obc, const unsigned int hils_port_id, const unsigned int baud_rate, - HilsPortManager* hils_port_manager, const int mode_id) + simulation::HilsPortManager* hils_port_manager, const int mode_id) : Component(300, clock_generator), UartCommunicationWithObc(sils_port_id, obc, hils_port_id, baud_rate, hils_port_manager), mode_id_(mode_id) {} ExampleSerialCommunicationForHils::~ExampleSerialCommunicationForHils() {} diff --git a/src/components/examples/example_serial_communication_for_hils.hpp b/src/components/examples/example_serial_communication_for_hils.hpp index 2393fa2c5..aeac14f2e 100644 --- a/src/components/examples/example_serial_communication_for_hils.hpp +++ b/src/components/examples/example_serial_communication_for_hils.hpp @@ -36,8 +36,8 @@ class ExampleSerialCommunicationForHils : public Component, public UartCommunica * @param [in] hils_port_manager: HILS port manager * @param [in] mode_id: Mode ID to select sender(0) or responder(1) */ - ExampleSerialCommunicationForHils(ClockGenerator* clock_generator, const int sils_port_id, OnBoardComputer* obc, const unsigned int hils_port_id, - const unsigned int baud_rate, HilsPortManager* hils_port_manager, const int mode_id); + ExampleSerialCommunicationForHils(environment::ClockGenerator* clock_generator, const int sils_port_id, OnBoardComputer* obc, const unsigned int hils_port_id, + const unsigned int baud_rate, simulation::HilsPortManager* hils_port_manager, const int mode_id); /** * @fn ~ExampleSerialCommunicationForHils * @brief Destructor diff --git a/src/components/examples/example_serial_communication_with_obc.cpp b/src/components/examples/example_serial_communication_with_obc.cpp index b44138520..3017ea96d 100644 --- a/src/components/examples/example_serial_communication_with_obc.cpp +++ b/src/components/examples/example_serial_communication_with_obc.cpp @@ -9,11 +9,11 @@ namespace s2e::components { -ExampleSerialCommunicationWithObc::ExampleSerialCommunicationWithObc(ClockGenerator* clock_generator, int port_id, OnBoardComputer* obc) +ExampleSerialCommunicationWithObc::ExampleSerialCommunicationWithObc(environment::ClockGenerator* clock_generator, int port_id, OnBoardComputer* obc) : Component(1000, clock_generator), UartCommunicationWithObc(port_id, obc) { Initialize(); } -ExampleSerialCommunicationWithObc::ExampleSerialCommunicationWithObc(ClockGenerator* clock_generator, int port_id, int prescaler, +ExampleSerialCommunicationWithObc::ExampleSerialCommunicationWithObc(environment::ClockGenerator* clock_generator, int port_id, int prescaler, OnBoardComputer* obc) : Component(prescaler, clock_generator), UartCommunicationWithObc(port_id, obc) { Initialize(); diff --git a/src/components/examples/example_serial_communication_with_obc.hpp b/src/components/examples/example_serial_communication_with_obc.hpp index 9a61c1ed0..d202fbc15 100644 --- a/src/components/examples/example_serial_communication_with_obc.hpp +++ b/src/components/examples/example_serial_communication_with_obc.hpp @@ -37,7 +37,7 @@ class ExampleSerialCommunicationWithObc : public Component, public UartCommunica * @param [in] port_id: Port ID for communication line b/w OnBoardComputer * @param [in] obc: The communication target OBC */ - ExampleSerialCommunicationWithObc(ClockGenerator* clock_generator, int port_id, OnBoardComputer* obc); + ExampleSerialCommunicationWithObc(environment::ClockGenerator* clock_generator, int port_id, OnBoardComputer* obc); /** * @fn ExampleSerialCommunicationWithObc * @brief Constructor @@ -46,7 +46,7 @@ class ExampleSerialCommunicationWithObc : public Component, public UartCommunica * @param [in] prescaler: Frequency scale factor for update * @param [in] obc: The communication target OBC */ - ExampleSerialCommunicationWithObc(ClockGenerator* clock_generator, int port_id, int prescaler, OnBoardComputer* obc); + ExampleSerialCommunicationWithObc(environment::ClockGenerator* clock_generator, int port_id, int prescaler, OnBoardComputer* obc); /** * @fn ~SerialCommunicationWithObc * @brief Destructor From 922b4abb296fbbc1cc5b395802e765522f476d58 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 27 Sep 2024 15:47:14 +0900 Subject: [PATCH 35/49] Update namespace info in components --- .../ideal/angular_velocity_observer.cpp | 4 ++-- .../ideal/angular_velocity_observer.hpp | 12 ++++++------ src/components/ideal/attitude_observer.cpp | 4 ++-- src/components/ideal/attitude_observer.hpp | 12 ++++++------ src/components/ideal/force_generator.cpp | 4 ++-- src/components/ideal/force_generator.hpp | 12 ++++++------ src/components/ideal/orbit_observer.cpp | 4 ++-- src/components/ideal/orbit_observer.hpp | 12 ++++++------ src/components/ideal/torque_generator.cpp | 4 ++-- src/components/ideal/torque_generator.hpp | 12 ++++++------ src/components/real/aocs/gnss_receiver.cpp | 8 ++++---- src/components/real/aocs/gnss_receiver.hpp | 16 ++++++++-------- src/components/real/aocs/gyro_sensor.cpp | 8 ++++---- src/components/real/aocs/gyro_sensor.hpp | 16 ++++++++-------- src/components/real/aocs/magnetometer.cpp | 8 ++++---- src/components/real/aocs/magnetometer.hpp | 16 ++++++++-------- src/components/real/aocs/magnetorquer.cpp | 8 ++++---- src/components/real/aocs/magnetorquer.hpp | 16 ++++++++-------- src/components/real/aocs/reaction_wheel.cpp | 8 ++++---- src/components/real/aocs/reaction_wheel.hpp | 16 ++++++++-------- src/components/real/aocs/star_sensor.cpp | 8 ++++---- src/components/real/aocs/star_sensor.hpp | 16 ++++++++-------- src/components/real/aocs/sun_sensor.cpp | 8 ++++---- src/components/real/aocs/sun_sensor.hpp | 16 ++++++++-------- src/components/real/cdh/on_board_computer.cpp | 6 +++--- src/components/real/cdh/on_board_computer.hpp | 6 +++--- .../real/cdh/on_board_computer_with_c2a.cpp | 6 +++--- .../real/cdh/on_board_computer_with_c2a.hpp | 6 +++--- .../ground_station_calculator.hpp | 8 ++++---- .../wings_command_sender_to_c2a.cpp | 2 +- .../wings_command_sender_to_c2a.hpp | 4 ++-- src/components/real/mission/telescope.cpp | 4 ++-- src/components/real/mission/telescope.hpp | 12 ++++++------ src/components/real/power/battery.cpp | 6 +++--- src/components/real/power/battery.hpp | 14 +++++++------- .../real/power/pcu_initial_study.cpp | 6 +++--- .../real/power/pcu_initial_study.hpp | 14 +++++++------- .../real/power/power_control_unit.cpp | 4 ++-- .../real/power/power_control_unit.hpp | 12 ++++++------ .../real/power/solar_array_panel.cpp | 10 +++++----- .../real/power/solar_array_panel.hpp | 18 +++++++++--------- .../real/propulsion/simple_thruster.cpp | 8 ++++---- .../real/propulsion/simple_thruster.hpp | 16 ++++++++-------- src/disturbances/air_drag.hpp | 6 +++--- src/disturbances/disturbance.hpp | 2 +- src/disturbances/geopotential.hpp | 6 +++--- src/disturbances/gravity_gradient.hpp | 6 +++--- src/disturbances/lunar_gravity_field.hpp | 6 +++--- src/disturbances/magnetic_disturbance.hpp | 6 +++--- .../solar_radiation_pressure_disturbance.hpp | 6 +++--- src/disturbances/third_body_gravity.hpp | 2 +- src/dynamics/attitude/attitude.hpp | 8 ++++---- .../attitude_with_cantilever_vibration.hpp | 6 +++--- src/dynamics/orbit/orbit.hpp | 8 ++++---- src/dynamics/thermal/temperature.hpp | 2 +- .../global/celestial_information.hpp | 8 ++++---- src/environment/global/gnss_satellites.hpp | 8 ++++---- src/environment/global/hipparcos_catalogue.hpp | 8 ++++---- src/environment/global/simulation_time.hpp | 8 ++++---- src/environment/local/atmosphere.hpp | 8 ++++---- src/environment/local/earth_albedo.hpp | 8 ++++---- src/environment/local/geomagnetic_field.hpp | 8 ++++---- .../local/local_celestial_information.hpp | 8 ++++---- .../solar_radiation_pressure_environment.hpp | 8 ++++---- 64 files changed, 273 insertions(+), 273 deletions(-) diff --git a/src/components/ideal/angular_velocity_observer.cpp b/src/components/ideal/angular_velocity_observer.cpp index d6a2bbcf3..7e3bea5d9 100644 --- a/src/components/ideal/angular_velocity_observer.cpp +++ b/src/components/ideal/angular_velocity_observer.cpp @@ -9,7 +9,7 @@ namespace s2e::components { -AngularVelocityObserver::AngularVelocityObserver(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const Attitude& attitude) +AngularVelocityObserver::AngularVelocityObserver(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const Attitude& attitude) : Component(prescaler, clock_generator), Sensor(sensor_base), attitude_(attitude) {} void AngularVelocityObserver::MainRoutine(const int time_count) { @@ -34,7 +34,7 @@ std::string AngularVelocityObserver::GetLogValue() const { return str_tmp; } -AngularVelocityObserver InitializeAngularVelocityObserver(ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s, +AngularVelocityObserver InitializeAngularVelocityObserver(environment::ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s, const Attitude& attitude) { IniAccess ini_file(file_name); diff --git a/src/components/ideal/angular_velocity_observer.hpp b/src/components/ideal/angular_velocity_observer.hpp index 47bb4a404..67d421f9b 100644 --- a/src/components/ideal/angular_velocity_observer.hpp +++ b/src/components/ideal/angular_velocity_observer.hpp @@ -18,7 +18,7 @@ namespace s2e::components { * @class AngularVelocityObserver * @brief Ideal component which can observe angular velocity */ -class AngularVelocityObserver : public Component, public Sensor<3>, public ILoggable { +class AngularVelocityObserver : public Component, public Sensor<3>, public logger::ILoggable { public: /** * @fn AngularVelocityObserver @@ -28,7 +28,7 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public ILogg * @param [in] sensor_base: Sensor base information * @param [in] dynamics: Dynamics information */ - AngularVelocityObserver(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const Attitude& attitude); + AngularVelocityObserver(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const Attitude& attitude); /** * @fn ~AngularVelocityObserver * @brief Destructor @@ -42,15 +42,15 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public ILogg */ void MainRoutine(const int time_count) override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const override; @@ -74,7 +74,7 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public ILogg * @param [in] component_step_time_s: Component step time [sec] * @param [in] dynamics: Dynamics information */ -AngularVelocityObserver InitializeAngularVelocityObserver(ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s, +AngularVelocityObserver InitializeAngularVelocityObserver(environment::ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s, const Attitude& attitude); } // namespace s2e::components diff --git a/src/components/ideal/attitude_observer.cpp b/src/components/ideal/attitude_observer.cpp index 533a636c8..1cb6e6126 100644 --- a/src/components/ideal/attitude_observer.cpp +++ b/src/components/ideal/attitude_observer.cpp @@ -10,7 +10,7 @@ namespace s2e::components { -AttitudeObserver::AttitudeObserver(const int prescaler, ClockGenerator* clock_generator, const double standard_deviation_rad, +AttitudeObserver::AttitudeObserver(const int prescaler, environment::ClockGenerator* clock_generator, const double standard_deviation_rad, const Attitude& attitude) : Component(prescaler, clock_generator), angle_noise_(0.0, standard_deviation_rad), attitude_(attitude) { direction_noise_.SetParameters(0.0, 1.0); @@ -49,7 +49,7 @@ std::string AttitudeObserver::GetLogValue() const { return str_tmp; } -AttitudeObserver InitializeAttitudeObserver(ClockGenerator* clock_generator, const std::string file_name, const Attitude& attitude) { +AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const Attitude& attitude) { // General IniAccess ini_file(file_name); diff --git a/src/components/ideal/attitude_observer.hpp b/src/components/ideal/attitude_observer.hpp index 0bf1e973f..8ce9697e5 100644 --- a/src/components/ideal/attitude_observer.hpp +++ b/src/components/ideal/attitude_observer.hpp @@ -19,7 +19,7 @@ namespace s2e::components { * @class AttitudeObserver * @brief Ideal component which can observe attitude */ -class AttitudeObserver : public Component, public ILoggable { +class AttitudeObserver : public Component, public logger::ILoggable { public: /** * @fn AttitudeObserver @@ -28,7 +28,7 @@ class AttitudeObserver : public Component, public ILoggable { * @param [in] clock_generator: Clock generator * @param [in] attitude: Attitude information */ - AttitudeObserver(const int prescaler, ClockGenerator* clock_generator, const double standard_deviation_rad, const Attitude& attitude); + AttitudeObserver(const int prescaler, environment::ClockGenerator* clock_generator, const double standard_deviation_rad, const Attitude& attitude); /** * @fn ~AttitudeObserver @@ -43,15 +43,15 @@ class AttitudeObserver : public Component, public ILoggable { */ void MainRoutine(const int time_count) override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const override; @@ -77,7 +77,7 @@ class AttitudeObserver : public Component, public ILoggable { * @param [in] file_name: Path to the initialize file * @param [in] attitude: Attitude information */ -AttitudeObserver InitializeAttitudeObserver(ClockGenerator* clock_generator, const std::string file_name, const Attitude& attitude); +AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const Attitude& attitude); } // namespace s2e::components diff --git a/src/components/ideal/force_generator.cpp b/src/components/ideal/force_generator.cpp index 59e788ba3..f458160d7 100644 --- a/src/components/ideal/force_generator.cpp +++ b/src/components/ideal/force_generator.cpp @@ -11,7 +11,7 @@ namespace s2e::components { // Constructor -ForceGenerator::ForceGenerator(const int prescaler, ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_N, +ForceGenerator::ForceGenerator(const int prescaler, environment::ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_N, const double direction_error_standard_deviation_rad, const Dynamics* dynamics) : Component(prescaler, clock_generator), magnitude_noise_(0.0, magnitude_error_standard_deviation_N), @@ -107,7 +107,7 @@ s2e::math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(s2e::math return error_quaternion; } -ForceGenerator InitializeForceGenerator(ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics) { +ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics) { // General IniAccess ini_file(file_name); diff --git a/src/components/ideal/force_generator.hpp b/src/components/ideal/force_generator.hpp index 46724d9f4..03c4bd09b 100644 --- a/src/components/ideal/force_generator.hpp +++ b/src/components/ideal/force_generator.hpp @@ -18,7 +18,7 @@ namespace s2e::components { * @class ForceGenerator * @brief Ideal component which can generate for control algorithm test */ -class ForceGenerator : public Component, public ILoggable { +class ForceGenerator : public Component, public logger::ILoggable { public: /** * @fn ForceGenerator @@ -29,7 +29,7 @@ class ForceGenerator : public Component, public ILoggable { * @param [in] direction_error_standard_deviation_rad: Standard deviation of direction error [rad] * @param [in] dynamics: Dynamics information */ - ForceGenerator(const int prescaler, ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_N, + ForceGenerator(const int prescaler, environment::ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_N, const double direction_error_standard_deviation_rad, const Dynamics* dynamics); /** * @fn ~ForceGenerator @@ -49,15 +49,15 @@ class ForceGenerator : public Component, public ILoggable { */ void PowerOffRoutine(); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; @@ -124,7 +124,7 @@ class ForceGenerator : public Component, public ILoggable { * @param [in] file_name: Path to initialize file * @param [in] dynamics: Dynamics information */ -ForceGenerator InitializeForceGenerator(ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics); +ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics); } // namespace s2e::components diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 881610793..0851de4d8 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -10,7 +10,7 @@ namespace s2e::components { -OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, +OrbitObserver::OrbitObserver(const int prescaler, environment::ClockGenerator* clock_generator, const NoiseFrame noise_frame, const s2e::math::Vector<6> error_standard_deviation, const Orbit& orbit) : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { @@ -85,7 +85,7 @@ NoiseFrame SetNoiseFrame(const std::string noise_frame) { } } -OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { +OrbitObserver InitializeOrbitObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { // General IniAccess ini_file(file_name); diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 36ef2a447..58a2cf3c9 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -28,7 +28,7 @@ enum class NoiseFrame { * @class OrbitObserver * @brief Ideal component which can observe orbit */ -class OrbitObserver : public Component, public ILoggable { +class OrbitObserver : public Component, public logger::ILoggable { public: /** * @fn OrbitObserver @@ -39,7 +39,7 @@ class OrbitObserver : public Component, public ILoggable { * @param [in] error_standard_deviation: Position and Velocity standard deviation noise [m, m/s] * @param [in] orbit: Orbit information */ - OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, const s2e::math::Vector<6> error_standard_deviation, + OrbitObserver(const int prescaler, environment::ClockGenerator* clock_generator, const NoiseFrame noise_frame, const s2e::math::Vector<6> error_standard_deviation, const Orbit& orbit); /** @@ -55,15 +55,15 @@ class OrbitObserver : public Component, public ILoggable { */ void MainRoutine(const int time_count) override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const override; @@ -105,7 +105,7 @@ NoiseFrame SetNoiseFrame(const std::string noise_frame); * @param [in] file_name: Path to the initialize file * @param [in] orbit: Orbit information */ -OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit); +OrbitObserver InitializeOrbitObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit); } // namespace s2e::components diff --git a/src/components/ideal/torque_generator.cpp b/src/components/ideal/torque_generator.cpp index 600f65e5a..a47ede7c9 100644 --- a/src/components/ideal/torque_generator.cpp +++ b/src/components/ideal/torque_generator.cpp @@ -11,7 +11,7 @@ namespace s2e::components { // Constructor -TorqueGenerator::TorqueGenerator(const int prescaler, ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_Nm, +TorqueGenerator::TorqueGenerator(const int prescaler, environment::ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_Nm, const double direction_error_standard_deviation_rad, const Dynamics* dynamics) : Component(prescaler, clock_generator), magnitude_noise_(0.0, magnitude_error_standard_deviation_Nm), @@ -80,7 +80,7 @@ s2e::math::Quaternion TorqueGenerator::GenerateDirectionNoiseQuaternion(s2e::mat return error_quaternion; } -TorqueGenerator InitializeTorqueGenerator(ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics) { +TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics) { // General IniAccess ini_file(file_name); diff --git a/src/components/ideal/torque_generator.hpp b/src/components/ideal/torque_generator.hpp index 51cbe39ee..357f7b22c 100644 --- a/src/components/ideal/torque_generator.hpp +++ b/src/components/ideal/torque_generator.hpp @@ -18,7 +18,7 @@ namespace s2e::components { * @class TorqueGenerator * @brief Ideal component which can generate for control algorithm test */ -class TorqueGenerator : public Component, public ILoggable { +class TorqueGenerator : public Component, public logger::ILoggable { public: /** * @fn TorqueGenerator @@ -29,7 +29,7 @@ class TorqueGenerator : public Component, public ILoggable { * @param [in] direction_error_standard_deviation_rad: Standard deviation of direction error [rad] * @param [in] dynamics: Dynamics information */ - TorqueGenerator(const int prescaler, ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_Nm, + TorqueGenerator(const int prescaler, environment::ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_Nm, const double direction_error_standard_deviation_rad, const Dynamics* dynamics); /** * @fn ~TorqueGenerator @@ -49,15 +49,15 @@ class TorqueGenerator : public Component, public ILoggable { */ void PowerOffRoutine(); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; @@ -102,7 +102,7 @@ class TorqueGenerator : public Component, public ILoggable { * @param [in] file_name: Path to initialize file * @param [in] dynamics: Dynamics information */ -TorqueGenerator InitializeTorqueGenerator(ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics); +TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics); } // namespace s2e::components diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 0d899b9e8..9b6a80cc8 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -13,7 +13,7 @@ namespace s2e::components { -GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, +GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, @@ -33,7 +33,7 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, } } -GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, +GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, @@ -291,7 +291,7 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat return gnss_receiver_param; } -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const Dynamics* dynamics, +GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); @@ -301,7 +301,7 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t comp return gnss_r; } -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, +GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 1ea260d0f..3714cc772 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -42,7 +42,7 @@ typedef struct _gnss_info { * @class GnssReceiver * @brief Class to emulate GNSS receiver */ -class GnssReceiver : public Component, public ILoggable { +class GnssReceiver : public Component, public logger::ILoggable { public: /** * @fn GnssReceiver @@ -60,7 +60,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] gnss_satellites: GNSS Satellites information * @param [in] simulation_time: Simulation time information */ - GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, + GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); @@ -80,7 +80,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] gnss_satellites: GNSS Satellites information * @param [in] simulation_time: Simulation time information */ - GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, + GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, @@ -116,15 +116,15 @@ class GnssReceiver : public Component, public ILoggable { */ inline const s2e::math::Vector<3> GetMeasuredVelocity_ecef_m_s(void) const { return velocity_ecef_m_s_; } - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; @@ -226,7 +226,7 @@ AntennaModel SetAntennaModel(const std::string antenna_model); * @param [in] gnss_satellites: GNSS satellites information * @param [in] simulation_time: Simulation time information */ -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const Dynamics* dynamics, +GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn InitGnssReceiver @@ -239,7 +239,7 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t comp * @param [in] gnss_satellites: GNSS satellites information * @param [in] simulation_time: Simulation time information */ -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, +GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); } // namespace s2e::components diff --git a/src/components/real/aocs/gyro_sensor.cpp b/src/components/real/aocs/gyro_sensor.cpp index 7af914f24..f967195fa 100644 --- a/src/components/real/aocs/gyro_sensor.cpp +++ b/src/components/real/aocs/gyro_sensor.cpp @@ -9,11 +9,11 @@ namespace s2e::components { -GyroSensor::GyroSensor(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, +GyroSensor::GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics) : Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), dynamics_(dynamics) {} -GyroSensor::GyroSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, +GyroSensor::GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics) : Component(prescaler, clock_generator, power_port), Sensor(sensor_base), @@ -47,7 +47,7 @@ std::string GyroSensor::GetLogValue() const { return str_tmp; } -GyroSensor InitGyroSensor(ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, +GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, const Dynamics* dynamics) { IniAccess gyro_conf(file_name); const char* sensor_name = "GYRO_SENSOR_"; @@ -68,7 +68,7 @@ GyroSensor InitGyroSensor(ClockGenerator* clock_generator, int sensor_id, const return gyro; } -GyroSensor InitGyroSensor(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, +GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const Dynamics* dynamics) { IniAccess gyro_conf(file_name); const char* sensor_name = "GYRO_SENSOR_"; diff --git a/src/components/real/aocs/gyro_sensor.hpp b/src/components/real/aocs/gyro_sensor.hpp index fd6ede443..c99759722 100644 --- a/src/components/real/aocs/gyro_sensor.hpp +++ b/src/components/real/aocs/gyro_sensor.hpp @@ -21,7 +21,7 @@ const size_t kGyroDimension = 3; //!< Dimension of gyro sensor * @class GyroSensor * @brief Class to emulate gyro sensor */ -class GyroSensor : public Component, public Sensor, public ILoggable { +class GyroSensor : public Component, public Sensor, public logger::ILoggable { public: /** * @fn GyroSensor @@ -33,7 +33,7 @@ class GyroSensor : public Component, public Sensor, public ILogg * @param [in] quaternion_b2c: Quaternion from body frame to component frame * @param [in] dynamics: Dynamics information */ - GyroSensor(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, + GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics); /** * @fn GyroSensor @@ -46,7 +46,7 @@ class GyroSensor : public Component, public Sensor, public ILogg * @param [in] quaternion_b2c: Quaternion from body frame to component frame * @param [in] dynamics: Dynamics information */ - GyroSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, + GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics); /** * @fn ~GyroSensor @@ -61,15 +61,15 @@ class GyroSensor : public Component, public Sensor, public ILogg */ void MainRoutine(const int time_count) override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const override; @@ -97,7 +97,7 @@ class GyroSensor : public Component, public Sensor, public ILogg * @param [in] file_name: Path to the initialize file * @param [in] dynamics: Dynamics information */ -GyroSensor InitGyroSensor(ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, +GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, const Dynamics* dynamics); /** * @fn InitGyroSensor @@ -109,7 +109,7 @@ GyroSensor InitGyroSensor(ClockGenerator* clock_generator, int sensor_id, const * @param [in] file_name: Path to the initialize file * @param [in] dynamics: Dynamics information */ -GyroSensor InitGyroSensor(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, +GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const Dynamics* dynamics); } //namespace s2e::components diff --git a/src/components/real/aocs/magnetometer.cpp b/src/components/real/aocs/magnetometer.cpp index 1326a7ba4..c12e7b37f 100644 --- a/src/components/real/aocs/magnetometer.cpp +++ b/src/components/real/aocs/magnetometer.cpp @@ -9,14 +9,14 @@ namespace s2e::components { -Magnetometer::Magnetometer(int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, +Magnetometer::Magnetometer(int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), geomagnetic_field_(geomagnetic_field) {} -Magnetometer::Magnetometer(int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, +Magnetometer::Magnetometer(int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator, power_port), Sensor(sensor_base), @@ -49,7 +49,7 @@ std::string Magnetometer::GetLogValue() const { return str_tmp; } -Magnetometer InitMagnetometer(ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, +Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field) { IniAccess magsensor_conf(file_name); const char* sensor_name = "MAGNETOMETER_"; @@ -70,7 +70,7 @@ Magnetometer InitMagnetometer(ClockGenerator* clock_generator, int sensor_id, co return magsensor; } -Magnetometer InitMagnetometer(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, +Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field) { IniAccess magsensor_conf(file_name); const char* sensor_name = "MAGNETOMETER_"; diff --git a/src/components/real/aocs/magnetometer.hpp b/src/components/real/aocs/magnetometer.hpp index 34dce8647..ab3c5cf85 100644 --- a/src/components/real/aocs/magnetometer.hpp +++ b/src/components/real/aocs/magnetometer.hpp @@ -21,7 +21,7 @@ const size_t kMagnetometerDimension = 3; //!< Dimension of magnetometer * @class Magnetometer * @brief Class to emulate magnetometer */ -class Magnetometer : public Component, public Sensor, public ILoggable { +class Magnetometer : public Component, public Sensor, public logger::ILoggable { public: /** * @fn Magnetometer @@ -33,7 +33,7 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] quaternion_b2c: Quaternion from body frame to component frame * @param [in] geomagnetic_field: Geomagnetic environment */ - Magnetometer(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, + Magnetometer(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); /** * @fn Magnetometer @@ -46,7 +46,7 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] quaternion_b2c: Quaternion from body frame to component frame * @param [in] geomagnetic_field: Geomagnetic environment */ - Magnetometer(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, + Magnetometer(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); /** * @fn ~Magnetometer @@ -61,15 +61,15 @@ class Magnetometer : public Component, public Sensor, pu */ void MainRoutine(const int time_count) override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const override; @@ -116,7 +116,7 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] component_step_time_s: Component step time [sec] * @param [in] geomagnetic_field: Geomegnetic environment */ -Magnetometer InitMagnetometer(ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, +Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field); /** * @fn InitMagnetometer @@ -128,7 +128,7 @@ Magnetometer InitMagnetometer(ClockGenerator* clock_generator, int sensor_id, co * @param [in] component_step_time_s: Component step time [sec] * @param [in] geomagnetic_field: Geomegnetic environment */ -Magnetometer InitMagnetometer(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, +Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field); } // namespace s2e::components diff --git a/src/components/real/aocs/magnetorquer.cpp b/src/components/real/aocs/magnetorquer.cpp index a213539a7..cab226ac7 100644 --- a/src/components/real/aocs/magnetorquer.cpp +++ b/src/components/real/aocs/magnetorquer.cpp @@ -13,7 +13,7 @@ namespace s2e::components { -Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, +Magnetorquer::Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const s2e::math::Matrix& scale_factor, const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, const s2e::math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, @@ -35,7 +35,7 @@ Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, } } -Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, +Magnetorquer::Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const s2e::math::Matrix& scale_factor, const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, const s2e::math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, @@ -113,7 +113,7 @@ std::string Magnetorquer::GetLogValue() const { return str_tmp; } -Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, int actuator_id, const std::string file_name, double component_step_time_s, +Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int actuator_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field) { IniAccess magtorquer_conf(file_name); const char* sensor_name = "MAGNETORQUER_"; @@ -158,7 +158,7 @@ Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, int actuator_id, return magtorquer; } -Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, const std::string file_name, +Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field) { IniAccess magtorquer_conf(file_name); const char* sensor_name = "MAGNETORQUER_"; diff --git a/src/components/real/aocs/magnetorquer.hpp b/src/components/real/aocs/magnetorquer.hpp index fb7ad153e..c5c1688bf 100644 --- a/src/components/real/aocs/magnetorquer.hpp +++ b/src/components/real/aocs/magnetorquer.hpp @@ -24,7 +24,7 @@ const size_t kMtqDimension = 3; //!< Dimension of magnetorquer * @class Magnetorquer * @brief Class to emulate magnetorquer */ -class Magnetorquer : public Component, public ILoggable { +class Magnetorquer : public Component, public logger::ILoggable { public: /** * @fn Magnetorquer @@ -43,7 +43,7 @@ class Magnetorquer : public Component, public ILoggable { * @param [in] normal_random_standard_deviation_c_Am2: Standard deviation for the normal random noise in the component frame [Am2] * @param [in] geomagnetic_field: Geomagnetic environment */ - Magnetorquer(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, + Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const s2e::math::Matrix& scale_factor, const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, const s2e::math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, const s2e::math::Vector& random_walk_standard_deviation_c_Am2, @@ -67,7 +67,7 @@ class Magnetorquer : public Component, public ILoggable { * @param [in] normal_random_standard_deviation_c_Am2: Standard deviation for the normal random noise in the component frame [Am2] * @param [in] geomagnetic_field: Geomagnetic environment */ - Magnetorquer(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, + Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const s2e::math::Matrix& scale_factor, const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, const s2e::math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, @@ -86,15 +86,15 @@ class Magnetorquer : public Component, public ILoggable { */ void PowerOffRoutine() override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const override; @@ -157,7 +157,7 @@ class Magnetorquer : public Component, public ILoggable { * @param [in] component_step_time_s: Component step time [sec] * @param [in] geomagnetic_field: Geomegnetic environment */ -Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, int actuator_id, const std::string file_name, double component_step_time_s, +Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int actuator_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field); /** * @fn InitMagnetorquer @@ -169,7 +169,7 @@ Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, int actuator_id, * @param [in] component_step_time_s: Component step time [sec] * @param [in] geomagnetic_field: Geomegnetic environment */ -Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, const std::string file_name, +Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field); } // namespace s2e::components diff --git a/src/components/real/aocs/reaction_wheel.cpp b/src/components/real/aocs/reaction_wheel.cpp index 918ed9578..9fe963c84 100644 --- a/src/components/real/aocs/reaction_wheel.cpp +++ b/src/components/real/aocs/reaction_wheel.cpp @@ -13,7 +13,7 @@ namespace s2e::components { -ReactionWheel::ReactionWheel(const int prescaler, ClockGenerator* clock_generator, const int component_id, const double step_width_s, +ReactionWheel::ReactionWheel(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const double step_width_s, const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const s2e::math::Quaternion quaternion_b2c, const s2e::math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, @@ -40,7 +40,7 @@ ReactionWheel::ReactionWheel(const int prescaler, ClockGenerator* clock_generato Initialize(); } -ReactionWheel::ReactionWheel(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, +ReactionWheel::ReactionWheel(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const double step_width_s, const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const s2e::math::Quaternion quaternion_b2c, const s2e::math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, @@ -333,7 +333,7 @@ void InitParams(int actuator_id, std::string file_name, double compo_update_step } } // namespace -ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, int actuator_id, std::string file_name, double compo_update_step_s) { +ReactionWheel InitReactionWheel(environment::ClockGenerator* clock_generator, int actuator_id, std::string file_name, double compo_update_step_s) { InitParams(actuator_id, file_name, compo_update_step_s); ReactionWheel rw(prescaler, clock_generator, actuator_id, step_width_s, rotor_inertia_kgm2, max_torque_Nm, max_velocity_rpm, quaternion_b2c, @@ -343,7 +343,7 @@ ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, int actuator_id return rw; } -ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, std::string file_name, +ReactionWheel InitReactionWheel(environment::ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, std::string file_name, double compo_update_step_s) { InitParams(actuator_id, file_name, compo_update_step_s); diff --git a/src/components/real/aocs/reaction_wheel.hpp b/src/components/real/aocs/reaction_wheel.hpp index 4735f6193..fc013dc11 100644 --- a/src/components/real/aocs/reaction_wheel.hpp +++ b/src/components/real/aocs/reaction_wheel.hpp @@ -24,7 +24,7 @@ namespace s2e::components { * @brief Class to emulate Reaction Wheel * @note For one reaction wheel */ -class ReactionWheel : public Component, public ILoggable { +class ReactionWheel : public Component, public logger::ILoggable { public: /** * @fn ReactionWheel @@ -50,7 +50,7 @@ class ReactionWheel : public Component, public ILoggable { * @param [in] drive_flag: RW drive flag * @param [in] init_velocity_rad_s: Initial value of angular velocity of RW */ - ReactionWheel(const int prescaler, ClockGenerator* clock_generator, const int component_id, const double step_width_s, + ReactionWheel(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const double step_width_s, const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const s2e::math::Quaternion quaternion_b2c, const s2e::math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, @@ -80,7 +80,7 @@ class ReactionWheel : public Component, public ILoggable { * @param [in] drive_flag: RW drive flag * @param [in] init_velocity_rad_s: Initial value of angular velocity of RW [rad/s] */ - ReactionWheel(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const double step_width_s, + ReactionWheel(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const double step_width_s, const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const s2e::math::Quaternion quaternion_b2c, const s2e::math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, @@ -104,15 +104,15 @@ class ReactionWheel : public Component, public ILoggable { */ void FastUpdate() override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const override; @@ -234,7 +234,7 @@ class ReactionWheel : public Component, public ILoggable { * @param [in] file_name: Path to the initialize file * @param [in] compo_update_step_s: Component step time [sec] */ -ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, int actuator_id, std::string file_name, double compo_update_step_s); +ReactionWheel InitReactionWheel(environment::ClockGenerator* clock_generator, int actuator_id, std::string file_name, double compo_update_step_s); /** * @fn InitReactionWheel * @brief Initialize functions for reaction wheel with power port @@ -245,7 +245,7 @@ ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, int actuator_id * @param [in] prop_step: Propagation step for RW dynamics [sec] * @param [in] compo_update_step_s: Component step time [sec] */ -ReactionWheel InitReactionWheel(ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, std::string file_name, +ReactionWheel InitReactionWheel(environment::ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, std::string file_name, double compo_update_step_s); } // namespace s2e::components diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index 83b105096..6466a60e8 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -18,7 +18,7 @@ using namespace s2e::math; namespace s2e::components { -StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, +StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, @@ -42,7 +42,7 @@ StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, con local_environment_(local_environment) { Initialize(); } -StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, +StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, @@ -215,7 +215,7 @@ void StarSensor::MainRoutine(const int time_count) { Measure(&(local_environment_->GetCelestialInformation()), &(dynamics_->GetAttitude())); } -StarSensor InitStarSensor(ClockGenerator* clock_generator, int sensor_id, const string file_name, double component_step_time_s, +StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, double component_step_time_s, const Dynamics* dynamics, const LocalEnvironment* local_environment) { IniAccess STT_conf(file_name); const char* sensor_name = "STAR_SENSOR_"; @@ -248,7 +248,7 @@ StarSensor InitStarSensor(ClockGenerator* clock_generator, int sensor_id, const return stt; } -StarSensor InitStarSensor(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const string file_name, double component_step_time_s, +StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const string file_name, double component_step_time_s, const Dynamics* dynamics, const LocalEnvironment* local_environment) { IniAccess STT_conf(file_name); const char* sensor_name = "STAR_SENSOR_"; diff --git a/src/components/real/aocs/star_sensor.hpp b/src/components/real/aocs/star_sensor.hpp index 888f91b18..9a7ebf842 100644 --- a/src/components/real/aocs/star_sensor.hpp +++ b/src/components/real/aocs/star_sensor.hpp @@ -24,7 +24,7 @@ namespace s2e::components { * @class StarSensor * @brief Class to emulate star tracker */ -class StarSensor : public Component, public ILoggable { +class StarSensor : public Component, public logger::ILoggable { public: /** * @fn StarSensor @@ -45,7 +45,7 @@ class StarSensor : public Component, public ILoggable { * @param [in] dynamics: Dynamics information * @param [in] local_environment: Local environment information */ - StarSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, + StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, @@ -70,7 +70,7 @@ class StarSensor : public Component, public ILoggable { * @param [in] dynamics: Dynamics information * @param [in] local_environment: Local environment information */ - StarSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, + StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, @@ -84,15 +84,15 @@ class StarSensor : public Component, public ILoggable { */ void MainRoutine(const int time_count) override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const override; @@ -218,7 +218,7 @@ class StarSensor : public Component, public ILoggable { * @param [in] dynamics: Dynamics information * @param [in] local_environment: Local environment information */ -StarSensor InitStarSensor(ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, +StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, const Dynamics* dynamics, const LocalEnvironment* local_environment); /** * @fn InitStarSensor @@ -231,7 +231,7 @@ StarSensor InitStarSensor(ClockGenerator* clock_generator, int sensor_id, const * @param [in] dynamics: Dynamics information * @param [in] local_environment: Local environment information */ -StarSensor InitStarSensor(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, +StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const Dynamics* dynamics, const LocalEnvironment* local_environment); } // namespace s2e::components diff --git a/src/components/real/aocs/sun_sensor.cpp b/src/components/real/aocs/sun_sensor.cpp index 773aba273..1e0b8142a 100644 --- a/src/components/real/aocs/sun_sensor.cpp +++ b/src/components/real/aocs/sun_sensor.cpp @@ -16,7 +16,7 @@ using namespace std; namespace s2e::components { -SunSensor::SunSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, +SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) @@ -30,7 +30,7 @@ SunSensor::SunSensor(const int prescaler, ClockGenerator* clock_generator, const Initialize(random_noise_standard_deviation_rad, bias_noise_standard_deviation_rad); } -SunSensor::SunSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, +SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) @@ -153,7 +153,7 @@ string SunSensor::GetLogValue() const { return str_tmp; } -SunSensor InitSunSensor(ClockGenerator* clock_generator, int ss_id, std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, +SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int ss_id, std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) { IniAccess ss_conf(file_name); const char* sensor_name = "SUN_SENSOR_"; @@ -186,7 +186,7 @@ SunSensor InitSunSensor(ClockGenerator* clock_generator, int ss_id, std::string return ss; } -SunSensor InitSunSensor(ClockGenerator* clock_generator, PowerPort* power_port, int ss_id, std::string file_name, +SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int ss_id, std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) { IniAccess ss_conf(file_name); const char* sensor_name = "SUN_SENSOR_"; diff --git a/src/components/real/aocs/sun_sensor.hpp b/src/components/real/aocs/sun_sensor.hpp index 08c8982d2..f8aba0d41 100644 --- a/src/components/real/aocs/sun_sensor.hpp +++ b/src/components/real/aocs/sun_sensor.hpp @@ -21,7 +21,7 @@ namespace s2e::components { * @class SunSensor * @brief Class to emulate sun sensor */ -class SunSensor : public Component, public ILoggable { +class SunSensor : public Component, public logger::ILoggable { public: /** * @fn SunSensor @@ -37,7 +37,7 @@ class SunSensor : public Component, public ILoggable { * @param [in] srp_environment: Solar Radiation Pressure environment * @param [in] local_celestial_information: Local celestial information */ - SunSensor(const int prescaler, ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, + SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); @@ -56,7 +56,7 @@ class SunSensor : public Component, public ILoggable { * @param [in] srp_environment: Solar Radiation Pressure environment * @param [in] local_celestial_information: Local celestial information */ - SunSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, + SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); @@ -68,15 +68,15 @@ class SunSensor : public Component, public ILoggable { */ void MainRoutine(const int time_count) override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const override; @@ -152,7 +152,7 @@ class SunSensor : public Component, public ILoggable { * @param [in] srp_environment: Solar radiation pressure environment * @param [in] local_environment: Local environment information */ -SunSensor InitSunSensor(ClockGenerator* clock_generator, int sensor_id, const std::string file_name, +SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); /** * @fn InitSunSensor @@ -164,7 +164,7 @@ SunSensor InitSunSensor(ClockGenerator* clock_generator, int sensor_id, const st * @param [in] srp_environment: Solar radiation pressure environment * @param [in] local_environment: Local environment information */ -SunSensor InitSunSensor(ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, +SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); } // namespace s2e::components diff --git a/src/components/real/cdh/on_board_computer.cpp b/src/components/real/cdh/on_board_computer.cpp index 33a75efd9..ce38a4196 100644 --- a/src/components/real/cdh/on_board_computer.cpp +++ b/src/components/real/cdh/on_board_computer.cpp @@ -6,14 +6,14 @@ namespace s2e::components { -OnBoardComputer::OnBoardComputer(ClockGenerator* clock_generator) : Component(1, clock_generator) { Initialize(); } +OnBoardComputer::OnBoardComputer(environment::ClockGenerator* clock_generator) : Component(1, clock_generator) { Initialize(); } -OnBoardComputer::OnBoardComputer(int prescaler, ClockGenerator* clock_generator, PowerPort* power_port) +OnBoardComputer::OnBoardComputer(int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port) : Component(prescaler, clock_generator, power_port) { Initialize(); } -OnBoardComputer::OnBoardComputer(int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const double minimum_voltage_V, +OnBoardComputer::OnBoardComputer(int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const double minimum_voltage_V, const double assumed_power_consumption_W) : Component(prescaler, clock_generator, power_port) { power_port_->SetMinimumVoltage_V(minimum_voltage_V); diff --git a/src/components/real/cdh/on_board_computer.hpp b/src/components/real/cdh/on_board_computer.hpp index 1943de045..91aeff65b 100644 --- a/src/components/real/cdh/on_board_computer.hpp +++ b/src/components/real/cdh/on_board_computer.hpp @@ -27,7 +27,7 @@ class OnBoardComputer : public Component { * @brief Constructor * @param [in] clock_generator: Clock generator */ - OnBoardComputer(ClockGenerator* clock_generator); + OnBoardComputer(environment::ClockGenerator* clock_generator); /** * @fn OnBoardComputer * @brief Constructor @@ -35,7 +35,7 @@ class OnBoardComputer : public Component { * @param [in] clock_generator: Clock generator * @param [in] power_port: Power port */ - OnBoardComputer(int prescaler, ClockGenerator* clock_generator, PowerPort* power_port); + OnBoardComputer(int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port); /** * @fn OnBoardComputer * @brief Constructor @@ -45,7 +45,7 @@ class OnBoardComputer : public Component { * @param [in] minimum_voltage_V: Minimum voltage [V] * @param [in] assumed_power_consumption_W: Assumed power consumption [W] */ - OnBoardComputer(int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const double minimum_voltage_V, + OnBoardComputer(int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const double minimum_voltage_V, const double assumed_power_consumption_W); /** * @fn ~OnBoardComputer diff --git a/src/components/real/cdh/on_board_computer_with_c2a.cpp b/src/components/real/cdh/on_board_computer_with_c2a.cpp index c0514d03f..3c6f58d1d 100644 --- a/src/components/real/cdh/on_board_computer_with_c2a.cpp +++ b/src/components/real/cdh/on_board_computer_with_c2a.cpp @@ -32,16 +32,16 @@ std::map ObcWithC2a::com_ports_c2a_; std::map ObcWithC2a::i2c_com_ports_c2a_; std::map ObcWithC2a::gpio_ports_c2a_; -ObcWithC2a::ObcWithC2a(ClockGenerator* clock_generator) : OnBoardComputer(clock_generator), timing_regulator_(1) { +ObcWithC2a::ObcWithC2a(environment::ClockGenerator* clock_generator) : OnBoardComputer(clock_generator), timing_regulator_(1) { // Initialize(); } -ObcWithC2a::ObcWithC2a(ClockGenerator* clock_generator, int timing_regulator) +ObcWithC2a::ObcWithC2a(environment::ClockGenerator* clock_generator, int timing_regulator) : OnBoardComputer(clock_generator), timing_regulator_(timing_regulator) { // Initialize(); } -ObcWithC2a::ObcWithC2a(int prescaler, ClockGenerator* clock_generator, int timing_regulator, PowerPort* power_port) +ObcWithC2a::ObcWithC2a(int prescaler, environment::ClockGenerator* clock_generator, int timing_regulator, PowerPort* power_port) : OnBoardComputer(prescaler, clock_generator, power_port), timing_regulator_(timing_regulator) { // Initialize(); } diff --git a/src/components/real/cdh/on_board_computer_with_c2a.hpp b/src/components/real/cdh/on_board_computer_with_c2a.hpp index c7b5fe968..05685df17 100644 --- a/src/components/real/cdh/on_board_computer_with_c2a.hpp +++ b/src/components/real/cdh/on_board_computer_with_c2a.hpp @@ -24,14 +24,14 @@ class ObcWithC2a : public OnBoardComputer { * @brief Constructor * @param [in] clock_generator: Clock generator */ - ObcWithC2a(ClockGenerator* clock_generator); + ObcWithC2a(environment::ClockGenerator* clock_generator); /** * @fn ObcWithC2a * @brief Constructor * @param [in] clock_generator: Clock generator * @param [in] timing_regulator: Timing regulator to update flight software faster than the component update */ - ObcWithC2a(ClockGenerator* clock_generator, int timing_regulator); + ObcWithC2a(environment::ClockGenerator* clock_generator, int timing_regulator); /** * @fn ObcWithC2a * @brief Constructor @@ -40,7 +40,7 @@ class ObcWithC2a : public OnBoardComputer { * @param [in] timing_regulator: Timing regulator to update flight software faster than the component update * @param [in] power_port: Power port */ - ObcWithC2a(int prescaler, ClockGenerator* clock_generator, int timing_regulator, PowerPort* power_port); + ObcWithC2a(int prescaler, environment::ClockGenerator* clock_generator, int timing_regulator, PowerPort* power_port); /** * @fn ~ObcWithC2a * @brief Destructor diff --git a/src/components/real/communication/ground_station_calculator.hpp b/src/components/real/communication/ground_station_calculator.hpp index 3d20f5a0b..c78cc41a4 100644 --- a/src/components/real/communication/ground_station_calculator.hpp +++ b/src/components/real/communication/ground_station_calculator.hpp @@ -19,7 +19,7 @@ namespace s2e::components { * @class GroundStationCalculator * @brief Emulation of analysis and calculation for Ground Stations */ -class GroundStationCalculator : public ILoggable { +class GroundStationCalculator : public logger::ILoggable { public: /** * @fn GroundStationCalculator @@ -54,15 +54,15 @@ class GroundStationCalculator : public ILoggable { void Update(const Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, const Antenna& ground_station_rx_antenna); - // Override ILoggable TODO: Maybe we don't need logabble, and this class should be used as library. + // Override logger::ILoggable TODO: Maybe we don't need logabble, and this class should be used as library. /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/components/real/communication/wings_command_sender_to_c2a.cpp b/src/components/real/communication/wings_command_sender_to_c2a.cpp index f2ce172e0..899241372 100644 --- a/src/components/real/communication/wings_command_sender_to_c2a.cpp +++ b/src/components/real/communication/wings_command_sender_to_c2a.cpp @@ -133,7 +133,7 @@ void WingsCommandSenderToC2a::AnalyzeC2aCommand(const std::vector t #endif } -WingsCommandSenderToC2a InitWingsCommandSenderToC2a(ClockGenerator* clock_generator, const double compo_update_step_s, +WingsCommandSenderToC2a InitWingsCommandSenderToC2a(environment::ClockGenerator* clock_generator, const double compo_update_step_s, const std::string initialize_file) { IniAccess ini_access(initialize_file); std::string section = "WINGS_COMMAND_SENDER_TO_C2A"; diff --git a/src/components/real/communication/wings_command_sender_to_c2a.hpp b/src/components/real/communication/wings_command_sender_to_c2a.hpp index 14e1eda9f..accc49546 100644 --- a/src/components/real/communication/wings_command_sender_to_c2a.hpp +++ b/src/components/real/communication/wings_command_sender_to_c2a.hpp @@ -24,7 +24,7 @@ class WingsCommandSenderToC2a : public Component { * @brief Constructor * @param [in] */ - WingsCommandSenderToC2a(int prescaler, ClockGenerator* clock_generator, const double step_width_s, const std::string command_database_file, + WingsCommandSenderToC2a(int prescaler, environment::ClockGenerator* clock_generator, const double step_width_s, const std::string command_database_file, const std::string operation_file, const bool is_enabled) : Component(prescaler, clock_generator), c2a_command_database_(command_database_file), @@ -75,7 +75,7 @@ class WingsCommandSenderToC2a : public Component { * @param[in] compo_update_step_s: Component update step time [s] * @param[in] initialize_file: Initialize file name */ -WingsCommandSenderToC2a InitWingsCommandSenderToC2a(ClockGenerator* clock_generator, const double compo_update_step_s, +WingsCommandSenderToC2a InitWingsCommandSenderToC2a(environment::ClockGenerator* clock_generator, const double compo_update_step_s, const std::string initialize_file); } // namespace s2e::components diff --git a/src/components/real/mission/telescope.cpp b/src/components/real/mission/telescope.cpp index 7d76de5c1..5e6e3c8da 100644 --- a/src/components/real/mission/telescope.cpp +++ b/src/components/real/mission/telescope.cpp @@ -15,7 +15,7 @@ using namespace s2e::math; namespace s2e::components { -Telescope::Telescope(ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, +Telescope::Telescope(environment::ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, const Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, @@ -247,7 +247,7 @@ string Telescope::GetLogValue() const { return str_tmp; } -Telescope InitTelescope(ClockGenerator* clock_generator, int sensor_id, const string file_name, const Attitude* attitude, +Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, const Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) { using s2e::math::pi; diff --git a/src/components/real/mission/telescope.hpp b/src/components/real/mission/telescope.hpp index e2d75c397..452639e56 100644 --- a/src/components/real/mission/telescope.hpp +++ b/src/components/real/mission/telescope.hpp @@ -32,7 +32,7 @@ struct Star { * @class Telescope * @brief Component emulation: Telescope */ -class Telescope : public Component, public ILoggable { +class Telescope : public Component, public logger::ILoggable { public: /** * @fn Telescope @@ -52,7 +52,7 @@ class Telescope : public Component, public ILoggable { * @param [in] local_celestial_information: Local celestial information * @param [in] orbit: Orbit information */ - Telescope(ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, + Telescope(environment::ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, const Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr); @@ -136,15 +136,15 @@ class Telescope : public Component, public ILoggable { void ObserveGroundPositionDeviation(); const Orbit* orbit_; //!< Orbit information - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; @@ -168,7 +168,7 @@ class Telescope : public Component, public ILoggable { * @param [in] hipparcos: Star information by Hipparcos catalogue * @param [in] local_celestial_information: Local celestial information */ -Telescope InitTelescope(ClockGenerator* clock_generator, int sensor_id, const std::string file_name, const Attitude* attitude, +Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, const Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr); diff --git a/src/components/real/power/battery.cpp b/src/components/real/power/battery.cpp index a8f1bc4d0..a050e35b8 100644 --- a/src/components/real/power/battery.cpp +++ b/src/components/real/power/battery.cpp @@ -10,7 +10,7 @@ namespace s2e::components { -Battery::Battery(const int prescaler, ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, double cell_capacity_Ah, +Battery::Battery(const int prescaler, environment::ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, double cell_capacity_Ah, const std::vector cell_discharge_curve_coefficients, double initial_dod, double cc_charge_c_rate, double cv_charge_voltage_V, double battery_resistance_Ohm, double component_step_time_s) : Component(prescaler, clock_generator), @@ -24,7 +24,7 @@ Battery::Battery(const int prescaler, ClockGenerator* clock_generator, int numbe battery_resistance_Ohm_(battery_resistance_Ohm), compo_step_time_s_(component_step_time_s) {} -Battery::Battery(ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, double cell_capacity_Ah, +Battery::Battery(environment::ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, double cell_capacity_Ah, const std::vector cell_discharge_curve_coefficients, double initial_dod, double cc_charge_c_rate, double cv_charge_voltage_V, double battery_resistance_Ohm) : Component(10, clock_generator), @@ -89,7 +89,7 @@ void Battery::UpdateBatVoltage() { battery_voltage_V_ = temp * number_of_series_; } -Battery InitBAT(ClockGenerator* clock_generator, int bat_id, const std::string file_name, double component_step_time_s) { +Battery InitBAT(environment::ClockGenerator* clock_generator, int bat_id, const std::string file_name, double component_step_time_s) { IniAccess bat_conf(file_name); const std::string section_name = "BATTERY_" + std::to_string(static_cast(bat_id)); diff --git a/src/components/real/power/battery.hpp b/src/components/real/power/battery.hpp index 24f43c464..c1adf5d49 100644 --- a/src/components/real/power/battery.hpp +++ b/src/components/real/power/battery.hpp @@ -17,7 +17,7 @@ namespace s2e::components { * @class Battery * @brief Component emulation of battery */ -class Battery : public Component, public ILoggable { +class Battery : public Component, public logger::ILoggable { public: /** * @fn Battery @@ -34,7 +34,7 @@ class Battery : public Component, public ILoggable { * @param [in] battery_resistance_Ohm: Battery internal resistance [Ohm] * @param [in] component_step_time_s: Component step time [sec] */ - Battery(const int prescaler, ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, double cell_capacity_Ah, + Battery(const int prescaler, environment::ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, double cell_capacity_Ah, const std::vector cell_discharge_curve_coefficients, double initial_dod, double cc_charge_c_rate, double cv_charge_voltage_V, double battery_resistance_Ohm, double component_step_time_s); /** @@ -51,7 +51,7 @@ class Battery : public Component, public ILoggable { * @param [in] cv_charge_voltage_V: Constant charge voltage [V] * @param [in] battery_resistance_Ohm: Battery internal resistance [Ohm] */ - Battery(ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, double cell_capacity_Ah, + Battery(environment::ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, double cell_capacity_Ah, const std::vector cell_discharge_curve_coefficients, double initial_dod, double cc_charge_c_rate, double cv_charge_voltage_V, double battery_resistance_Ohm); /** @@ -97,15 +97,15 @@ class Battery : public Component, public ILoggable { */ inline double GetCvChargeVoltage_V() const { return cv_charge_voltage_V_; } - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ std::string GetLogValue() const override; @@ -144,7 +144,7 @@ class Battery : public Component, public ILoggable { * @param [in] file_name: Path to initialize file * @param [in] component_step_time_s: Component step time [sec] */ -Battery InitBAT(ClockGenerator* clock_generator, int bat_id, const std::string file_name, double component_step_time_s); +Battery InitBAT(environment::ClockGenerator* clock_generator, int bat_id, const std::string file_name, double component_step_time_s); } // namespace s2e::components diff --git a/src/components/real/power/pcu_initial_study.cpp b/src/components/real/power/pcu_initial_study.cpp index df8527352..c79873dea 100644 --- a/src/components/real/power/pcu_initial_study.cpp +++ b/src/components/real/power/pcu_initial_study.cpp @@ -12,7 +12,7 @@ namespace s2e::components { -PcuInitialStudy::PcuInitialStudy(const int prescaler, ClockGenerator* clock_generator, const std::vector saps, Battery* battery, +PcuInitialStudy::PcuInitialStudy(const int prescaler, environment::ClockGenerator* clock_generator, const std::vector saps, Battery* battery, double component_step_time_s) : Component(prescaler, clock_generator), saps_(saps), @@ -24,7 +24,7 @@ PcuInitialStudy::PcuInitialStudy(const int prescaler, ClockGenerator* clock_gene power_consumption_W_ = 0.0; } -PcuInitialStudy::PcuInitialStudy(ClockGenerator* clock_generator, const std::vector saps, Battery* battery) +PcuInitialStudy::PcuInitialStudy(environment::ClockGenerator* clock_generator, const std::vector saps, Battery* battery) : Component(10, clock_generator), saps_(saps), battery_(battery), @@ -113,7 +113,7 @@ void PcuInitialStudy::UpdateChargeCurrentAndBusVoltage() { } } -PcuInitialStudy InitPCU_InitialStudy(ClockGenerator* clock_generator, int pcu_id, const std::string file_name, +PcuInitialStudy InitPCU_InitialStudy(environment::ClockGenerator* clock_generator, int pcu_id, const std::string file_name, const std::vector saps, Battery* battery, double component_step_time_s) { IniAccess pcu_conf(file_name); diff --git a/src/components/real/power/pcu_initial_study.hpp b/src/components/real/power/pcu_initial_study.hpp index f9dd8ccf8..5a8bd074b 100644 --- a/src/components/real/power/pcu_initial_study.hpp +++ b/src/components/real/power/pcu_initial_study.hpp @@ -15,7 +15,7 @@ namespace s2e::components { -class PcuInitialStudy : public Component, public ILoggable { +class PcuInitialStudy : public Component, public logger::ILoggable { public: /** * @fn PcuInitialStudy @@ -26,7 +26,7 @@ class PcuInitialStudy : public Component, public ILoggable { * @param [in] battery: Battery * @param [in] component_step_time_s: Component step time [sec] */ - PcuInitialStudy(const int prescaler, ClockGenerator* clock_generator, const std::vector saps, Battery* battery, + PcuInitialStudy(const int prescaler, environment::ClockGenerator* clock_generator, const std::vector saps, Battery* battery, double component_step_time_s); /** * @fn PcuInitialStudy @@ -35,22 +35,22 @@ class PcuInitialStudy : public Component, public ILoggable { * @param [in] saps: Solar Array Panels * @param [in] battery: Battery */ - PcuInitialStudy(ClockGenerator* clock_generator, const std::vector saps, Battery* battery); + PcuInitialStudy(environment::ClockGenerator* clock_generator, const std::vector saps, Battery* battery); /** * @fn ~PcuInitialStudy * @brief Destructor */ ~PcuInitialStudy(); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ std::string GetLogValue() const override; @@ -94,7 +94,7 @@ class PcuInitialStudy : public Component, public ILoggable { * @param [in] battery: Battery information * @param [in] component_step_time_s: Component step time [sec] */ -PcuInitialStudy InitPCU_InitialStudy(ClockGenerator* clock_generator, int pcu_id, const std::string file_name, +PcuInitialStudy InitPCU_InitialStudy(environment::ClockGenerator* clock_generator, int pcu_id, const std::string file_name, const std::vector saps, Battery* battery, double component_step_time_s); } // namespace s2e::components diff --git a/src/components/real/power/power_control_unit.cpp b/src/components/real/power/power_control_unit.cpp index 8e908c2af..ce820b1c3 100644 --- a/src/components/real/power/power_control_unit.cpp +++ b/src/components/real/power/power_control_unit.cpp @@ -6,9 +6,9 @@ namespace s2e::components { -PowerControlUnit::PowerControlUnit(ClockGenerator* clock_generator) : Component(1, clock_generator) {} +PowerControlUnit::PowerControlUnit(environment::ClockGenerator* clock_generator) : Component(1, clock_generator) {} -PowerControlUnit::PowerControlUnit(int prescaler, ClockGenerator* clock_generator) : Component(prescaler, clock_generator) {} +PowerControlUnit::PowerControlUnit(int prescaler, environment::ClockGenerator* clock_generator) : Component(prescaler, clock_generator) {} PowerControlUnit::~PowerControlUnit() {} diff --git a/src/components/real/power/power_control_unit.hpp b/src/components/real/power/power_control_unit.hpp index f4765e74f..d61bc397b 100644 --- a/src/components/real/power/power_control_unit.hpp +++ b/src/components/real/power/power_control_unit.hpp @@ -18,21 +18,21 @@ namespace s2e::components { * @class PowerControlUnit * @brief Component emulation of Power Control Unit */ -class PowerControlUnit : public Component, public ILoggable { +class PowerControlUnit : public Component, public logger::ILoggable { public: /** * @fn PowerControlUnit * @brief Constructor * @param [in] clock_generator: Clock generator */ - PowerControlUnit(ClockGenerator* clock_generator); + PowerControlUnit(environment::ClockGenerator* clock_generator); /** * @fn PowerControlUnit * @brief Constructor * @param [in] prescaler: Frequency scale factor for update * @param [in] clock_generator: Clock generator */ - PowerControlUnit(int prescaler, ClockGenerator* clock_generator); + PowerControlUnit(int prescaler, environment::ClockGenerator* clock_generator); /** * @fn ~PowerControlUnit * @brief Destructor @@ -46,15 +46,15 @@ class PowerControlUnit : public Component, public ILoggable { */ void MainRoutine(const int time_count) override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ std::string GetLogValue() const override; diff --git a/src/components/real/power/solar_array_panel.cpp b/src/components/real/power/solar_array_panel.cpp index 67e459f6e..fdde4d90a 100644 --- a/src/components/real/power/solar_array_panel.cpp +++ b/src/components/real/power/solar_array_panel.cpp @@ -11,7 +11,7 @@ namespace s2e::components { -SolarArrayPanel::SolarArrayPanel(const int prescaler, ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, +SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, double component_step_time_s) @@ -30,7 +30,7 @@ SolarArrayPanel::SolarArrayPanel(const int prescaler, ClockGenerator* clock_gene power_generation_W_ = 0.0; } -SolarArrayPanel::SolarArrayPanel(const int prescaler, ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, +SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s) : Component(prescaler, clock_generator), @@ -47,7 +47,7 @@ SolarArrayPanel::SolarArrayPanel(const int prescaler, ClockGenerator* clock_gene power_generation_W_ = 0.0; } -SolarArrayPanel::SolarArrayPanel(ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, +SolarArrayPanel::SolarArrayPanel(environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) @@ -116,7 +116,7 @@ void SolarArrayPanel::MainRoutine(const int time_count) { if (power_generation_W_ < 0) power_generation_W_ = 0.0; } -SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std::string file_name, +SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, double component_step_time_s) { IniAccess sap_conf(file_name); @@ -150,7 +150,7 @@ SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std:: return sap; } -SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std::string file_name, +SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s) { IniAccess sap_conf(file_name); diff --git a/src/components/real/power/solar_array_panel.hpp b/src/components/real/power/solar_array_panel.hpp index 6fb7aacac..f057f5066 100644 --- a/src/components/real/power/solar_array_panel.hpp +++ b/src/components/real/power/solar_array_panel.hpp @@ -15,7 +15,7 @@ namespace s2e::components { -class SolarArrayPanel : public Component, public ILoggable { +class SolarArrayPanel : public Component, public logger::ILoggable { public: /** * @fn SolarArrayPanel @@ -33,7 +33,7 @@ class SolarArrayPanel : public Component, public ILoggable { * @param [in] local_celestial_information: Local celestial information * @param [in] component_step_time_s: Component step time [sec] */ - SolarArrayPanel(const int prescaler, ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, + SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, double component_step_time_s); @@ -52,7 +52,7 @@ class SolarArrayPanel : public Component, public ILoggable { * @param [in] srp_environment: Solar Radiation Pressure environment * @param [in] component_step_time_s: Component step time [sec] */ - SolarArrayPanel(const int prescaler, ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, + SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s); /** @@ -70,7 +70,7 @@ class SolarArrayPanel : public Component, public ILoggable { * @param [in] srp_environment: Solar Radiation Pressure environment * @param [in] local_celestial_information: Local celestial information */ - SolarArrayPanel(ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, + SolarArrayPanel(environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); /** @@ -96,15 +96,15 @@ class SolarArrayPanel : public Component, public ILoggable { */ void SetVoltage_V(const double voltage_V) { voltage_V_ = voltage_V; } - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ std::string GetLogValue() const override; @@ -143,7 +143,7 @@ class SolarArrayPanel : public Component, public ILoggable { * @param [in] local_celestial_information: Local celestial information * @param [in] component_step_time_s: Component step time [sec] */ -SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std::string file_name, +SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, double component_step_time_s); @@ -156,7 +156,7 @@ SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std:: * @param [in] srp_environment: Solar Radiation Pressure environment * @param [in] component_step_time_s: Component step time [sec] */ -SolarArrayPanel InitSAP(ClockGenerator* clock_generator, int sap_id, const std::string file_name, +SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s); } // namespace s2e::components diff --git a/src/components/real/propulsion/simple_thruster.cpp b/src/components/real/propulsion/simple_thruster.cpp index 3cfdc6da7..9501f82bb 100644 --- a/src/components/real/propulsion/simple_thruster.cpp +++ b/src/components/real/propulsion/simple_thruster.cpp @@ -12,7 +12,7 @@ namespace s2e::components { // Constructor -SimpleThruster::SimpleThruster(const int prescaler, ClockGenerator* clock_generator, const int component_id, +SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Vector<3> thruster_position_b_m, const s2e::math::Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, const Dynamics* dynamics) @@ -27,7 +27,7 @@ SimpleThruster::SimpleThruster(const int prescaler, ClockGenerator* clock_genera Initialize(magnitude_standard_deviation_N, direction_standard_deviation_rad); } -SimpleThruster::SimpleThruster(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, +SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const s2e::math::Vector<3> thruster_position_b_m, const s2e::math::Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, const Dynamics* dynamics) @@ -122,7 +122,7 @@ s2e::math::Vector<3> SimpleThruster::CalcThrustDirection() { return thrust_dir_b_true; } -SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, int thruster_id, const std::string file_name, const Structure* structure, +SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, const Structure* structure, const Dynamics* dynamics) { IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); @@ -150,7 +150,7 @@ SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, int thruster_ return thruster; } -SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, +SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, const Structure* structure, const Dynamics* dynamics) { IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); diff --git a/src/components/real/propulsion/simple_thruster.hpp b/src/components/real/propulsion/simple_thruster.hpp index ab1e0734a..ba1eb0c07 100644 --- a/src/components/real/propulsion/simple_thruster.hpp +++ b/src/components/real/propulsion/simple_thruster.hpp @@ -21,7 +21,7 @@ namespace s2e::components { * @class SimpleThruster * @brief Component emulation of simple thruster */ -class SimpleThruster : public Component, public ILoggable { +class SimpleThruster : public Component, public logger::ILoggable { public: /** * @fn SimpleThruster @@ -37,7 +37,7 @@ class SimpleThruster : public Component, public ILoggable { * @param [in] structure: Spacecraft structure information * @param [in] dynamics: Spacecraft dynamics information */ - SimpleThruster(const int prescaler, ClockGenerator* clock_generator, const int component_id, const Vector<3> thruster_position_b_m, + SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const Vector<3> thruster_position_b_m, const Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, const Dynamics* dynamics); /** @@ -55,7 +55,7 @@ class SimpleThruster : public Component, public ILoggable { * @param [in] structure: Spacecraft structure information * @param [in] dynamics: Spacecraft dynamics information */ - SimpleThruster(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, + SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const Vector<3> thruster_position_b_m, const Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, const Dynamics* dynamics); @@ -77,15 +77,15 @@ class SimpleThruster : public Component, public ILoggable { */ void PowerOffRoutine() override; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const override; @@ -166,7 +166,7 @@ class SimpleThruster : public Component, public ILoggable { * @param [in] structure: Spacecraft structure information * @param [in] dynamics: Spacecraft dynamics information */ -SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, int thruster_id, const std::string file_name, const Structure* structure, +SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, const Structure* structure, const Dynamics* dynamics); /** * @fn InitSimpleThruster @@ -178,7 +178,7 @@ SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, int thruster_ * @param [in] structure: Spacecraft structure information * @param [in] dynamics: Spacecraft dynamics information */ -SimpleThruster InitSimpleThruster(ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, +SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, const Structure* structure, const Dynamics* dynamics); } // namespace s2e::components diff --git a/src/disturbances/air_drag.hpp b/src/disturbances/air_drag.hpp index 48ab76eb3..be27dfc02 100644 --- a/src/disturbances/air_drag.hpp +++ b/src/disturbances/air_drag.hpp @@ -43,15 +43,15 @@ class AirDrag : public SurfaceForce { */ virtual void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/disturbances/disturbance.hpp b/src/disturbances/disturbance.hpp index 58966c35a..1d153810c 100644 --- a/src/disturbances/disturbance.hpp +++ b/src/disturbances/disturbance.hpp @@ -15,7 +15,7 @@ namespace s2e::disturbances { * @class Disturbance * @brief Base class for a disturbance */ -class Disturbance : public ILoggable { +class Disturbance : public logger::ILoggable { public: /** * @fn Disturbance diff --git a/src/disturbances/geopotential.hpp b/src/disturbances/geopotential.hpp index 95ec16968..fae80dedb 100644 --- a/src/disturbances/geopotential.hpp +++ b/src/disturbances/geopotential.hpp @@ -50,15 +50,15 @@ class Geopotential : public Disturbance { */ virtual void Update(const LocalEnvironment &local_environment, const Dynamics &dynamics); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/disturbances/gravity_gradient.hpp b/src/disturbances/gravity_gradient.hpp index c54e502dd..785b9e0fa 100644 --- a/src/disturbances/gravity_gradient.hpp +++ b/src/disturbances/gravity_gradient.hpp @@ -45,15 +45,15 @@ class GravityGradient : public Disturbance { */ virtual void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/disturbances/lunar_gravity_field.hpp b/src/disturbances/lunar_gravity_field.hpp index c809bc4fb..03841dd15 100644 --- a/src/disturbances/lunar_gravity_field.hpp +++ b/src/disturbances/lunar_gravity_field.hpp @@ -52,15 +52,15 @@ class LunarGravityField : public Disturbance { */ virtual void Update(const LocalEnvironment &local_environment, const Dynamics &dynamics); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/disturbances/magnetic_disturbance.hpp b/src/disturbances/magnetic_disturbance.hpp index ab3bb7375..ecba81538 100644 --- a/src/disturbances/magnetic_disturbance.hpp +++ b/src/disturbances/magnetic_disturbance.hpp @@ -37,15 +37,15 @@ class MagneticDisturbance : public Disturbance { */ virtual void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/disturbances/solar_radiation_pressure_disturbance.hpp b/src/disturbances/solar_radiation_pressure_disturbance.hpp index e427e6dc3..d33233a1b 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.hpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.hpp @@ -38,15 +38,15 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { */ virtual void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/disturbances/third_body_gravity.hpp b/src/disturbances/third_body_gravity.hpp index cf45a8593..b1d6d5e48 100644 --- a/src/disturbances/third_body_gravity.hpp +++ b/src/disturbances/third_body_gravity.hpp @@ -47,7 +47,7 @@ class ThirdBodyGravity : public Disturbance { std::set third_body_list_; //!< List of celestial bodies to calculate the third body disturbances s2e::math::Vector<3> third_body_acceleration_i_m_s2_{0.0}; //!< Calculated third body disturbance acceleration in the inertial frame [m/s2] - // Override classes for ILoggable + // Override classes for logger::ILoggable /** * @fn GetLogHeader * @brief Override function of GetLogHeader diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index 1dbcfa809..533d534f3 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -18,7 +18,7 @@ namespace s2e::dynamics::attitude { * @class Attitude * @brief Base class for attitude of spacecraft */ -class Attitude : public ILoggable, public SimulationObject { +class Attitude : public logger::ILoggable, public SimulationObject { public: /** * @fn Attitude @@ -100,15 +100,15 @@ class Attitude : public ILoggable, public SimulationObject { */ virtual void Propagate(const double end_time_s) = 0; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp index dbc96391d..682759f8f 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp @@ -51,15 +51,15 @@ class AttitudeWithCantileverVibration : public Attitude { */ virtual void Propagate(const double end_time_s); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index dabd75f59..b83ba4716 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -44,7 +44,7 @@ enum class OrbitInitializeMode { * @class Orbit * @brief Base class of orbit propagation */ -class Orbit : public ILoggable { +class Orbit : public logger::ILoggable { public: /** * @fn Orbit @@ -174,15 +174,15 @@ class Orbit : public ILoggable { */ s2e::math::Quaternion CalcQuaternion_i2lvlh() const; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/dynamics/thermal/temperature.hpp b/src/dynamics/thermal/temperature.hpp index c0aaec67c..7f9596ef9 100644 --- a/src/dynamics/thermal/temperature.hpp +++ b/src/dynamics/thermal/temperature.hpp @@ -32,7 +32,7 @@ enum class SolarCalcSetting { * @class Temperature * @brief class to calculate temperature of all nodes */ -class Temperature : public ILoggable { +class Temperature : public logger::ILoggable { protected: std::vector> conductance_matrix_W_K_; //!< Coupling of node i and node j by heat conduction [W/K] std::vector> radiation_matrix_m2_; //!< Coupling of node i and node j by thermal radiation [m2] diff --git a/src/environment/global/celestial_information.hpp b/src/environment/global/celestial_information.hpp index eeb9ff5f2..162fe790a 100644 --- a/src/environment/global/celestial_information.hpp +++ b/src/environment/global/celestial_information.hpp @@ -24,7 +24,7 @@ namespace s2e::environment { * @brief Class to manage the information related with the celestial bodies * @details This class uses SPICE to get the information of celestial bodies */ -class CelestialInformation : public ILoggable { +class CelestialInformation : public logger::ILoggable { public: /** * @fn CelestialInformation @@ -49,15 +49,15 @@ class CelestialInformation : public ILoggable { */ virtual ~CelestialInformation(); - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/environment/global/gnss_satellites.hpp b/src/environment/global/gnss_satellites.hpp index 88994a500..563dbd3e7 100644 --- a/src/environment/global/gnss_satellites.hpp +++ b/src/environment/global/gnss_satellites.hpp @@ -27,7 +27,7 @@ namespace s2e::environment { * @class GnssSatellites * @brief Class to calculate GNSS satellite position and clock */ -class GnssSatellites : public ILoggable { +class GnssSatellites : public logger::ILoggable { public: /** * @fn GnssSatellites @@ -100,15 +100,15 @@ class GnssSatellites : public ILoggable { */ double GetClock_s(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time = s2e::time_system::EpochTime(0, 0.0)) const; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ std::string GetLogHeader() const override; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ std::string GetLogValue() const override; diff --git a/src/environment/global/hipparcos_catalogue.hpp b/src/environment/global/hipparcos_catalogue.hpp index dd20fee9d..bc7a70f0f 100644 --- a/src/environment/global/hipparcos_catalogue.hpp +++ b/src/environment/global/hipparcos_catalogue.hpp @@ -28,7 +28,7 @@ struct HipparcosData { *@class HipparcosCatalogue *@brief Class to calculate star direction with Hipparcos catalogue */ -class HipparcosCatalogue : public ILoggable { +class HipparcosCatalogue : public logger::ILoggable { public: /** *@fn HipparcosCatalogue @@ -93,15 +93,15 @@ class HipparcosCatalogue : public ILoggable { */ s2e::math::Vector<3> GetStarDirection_b(size_t rank, s2e::math::Quaternion quaternion_i2b) const; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/environment/global/simulation_time.hpp b/src/environment/global/simulation_time.hpp index 27892afdf..eacefcc71 100644 --- a/src/environment/global/simulation_time.hpp +++ b/src/environment/global/simulation_time.hpp @@ -49,7 +49,7 @@ struct UTC { *@class SimulationTime *@brief Class to manage simulation time related information */ -class SimulationTime : public ILoggable { +class SimulationTime : public logger::ILoggable { public: /** *@fn SimulationTime @@ -241,15 +241,15 @@ class SimulationTime : public ILoggable { */ inline double GetStartSecond(void) const { return start_sec_; }; - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/environment/local/atmosphere.hpp b/src/environment/local/atmosphere.hpp index 2c383b78c..6bc47e246 100644 --- a/src/environment/local/atmosphere.hpp +++ b/src/environment/local/atmosphere.hpp @@ -21,7 +21,7 @@ namespace s2e::environment { * @class Atmosphere * @brief Class to calculate earth's atmospheric density */ -class Atmosphere : public ILoggable { +class Atmosphere : public logger::ILoggable { public: /** * @fn Atmosphere @@ -63,15 +63,15 @@ class Atmosphere : public ILoggable { */ inline void SetCalcFlag(const bool is_calc_enabled) { is_calc_enabled_ = is_calc_enabled; } - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/environment/local/earth_albedo.hpp b/src/environment/local/earth_albedo.hpp index 724cba2a7..0635afd7a 100644 --- a/src/environment/local/earth_albedo.hpp +++ b/src/environment/local/earth_albedo.hpp @@ -16,7 +16,7 @@ namespace s2e::environment { * @class EarthAlbedo * @brief Class to calculate Solar Radiation Pressure */ -class EarthAlbedo : public ILoggable { +class EarthAlbedo : public logger::ILoggable { public: /** * @fn EarthAlbedo @@ -76,15 +76,15 @@ class EarthAlbedo : public ILoggable { LocalCelestialInformation* local_celestial_information_; //!< Local celestial information SolarRadiationPressureEnvironment* srp_environment_; //!< Solar radiation pressure environment - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/environment/local/geomagnetic_field.hpp b/src/environment/local/geomagnetic_field.hpp index 100baf480..ba3f05329 100644 --- a/src/environment/local/geomagnetic_field.hpp +++ b/src/environment/local/geomagnetic_field.hpp @@ -17,7 +17,7 @@ namespace s2e::environment { * @class GeomagneticField * @brief Class to calculate magnetic field of the earth */ -class GeomagneticField : public ILoggable { +class GeomagneticField : public logger::ILoggable { public: bool IsCalcEnabled = true; //!< Calculation flag @@ -59,15 +59,15 @@ class GeomagneticField : public ILoggable { */ inline s2e::math::Vector<3> GetGeomagneticField_b_nT() const { return magnetic_field_b_nT_; } - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/environment/local/local_celestial_information.hpp b/src/environment/local/local_celestial_information.hpp index 26e1c354b..e81a7652f 100644 --- a/src/environment/local/local_celestial_information.hpp +++ b/src/environment/local/local_celestial_information.hpp @@ -14,7 +14,7 @@ namespace s2e::environment { * @class LocalCelestialInformation * @brief Class to manage celestial body information in the spacecraft body frame */ -class LocalCelestialInformation : public ILoggable { +class LocalCelestialInformation : public logger::ILoggable { public: /** * @fn LocalCelestialInformation @@ -70,15 +70,15 @@ class LocalCelestialInformation : public ILoggable { */ inline const CelestialInformation& GetGlobalInformation() const { return *global_celestial_information_; } - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; diff --git a/src/environment/local/solar_radiation_pressure_environment.hpp b/src/environment/local/solar_radiation_pressure_environment.hpp index a768ce136..f49bc8106 100644 --- a/src/environment/local/solar_radiation_pressure_environment.hpp +++ b/src/environment/local/solar_radiation_pressure_environment.hpp @@ -15,7 +15,7 @@ namespace s2e::environment { * @class SolarRadiationPressureEnvironment * @brief Class to calculate Solar Radiation Pressure */ -class SolarRadiationPressureEnvironment : public ILoggable { +class SolarRadiationPressureEnvironment : public logger::ILoggable { public: bool IsCalcEnabled = true; //!< Calculation flag @@ -80,15 +80,15 @@ class SolarRadiationPressureEnvironment : public ILoggable { */ inline bool GetIsEclipsed() const { return (shadow_coefficient_ >= 1.0 ? false : true); } - // Override ILoggable + // Override logger::ILoggable /** * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable + * @brief Override GetLogHeader function of logger::ILoggable */ virtual std::string GetLogHeader() const; /** * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable + * @brief Override GetLogValue function of logger::ILoggable */ virtual std::string GetLogValue() const; From 5a4f12c3c9279011f19808d7d613a3f7bc978c9d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 28 Sep 2024 20:45:10 +0900 Subject: [PATCH 36/49] Add logger and setting_file_reader namespace --- .../base/sensor_template_functions.hpp | 2 +- .../ideal/angular_velocity_observer.cpp | 10 ++++----- .../ideal/angular_velocity_observer.hpp | 6 ++--- src/components/ideal/attitude_observer.cpp | 6 ++--- src/components/ideal/attitude_observer.hpp | 10 ++++----- src/components/ideal/force_generator.cpp | 18 +++++++-------- src/components/ideal/orbit_observer.cpp | 10 ++++----- src/components/ideal/torque_generator.cpp | 10 ++++----- src/components/ports/power_port.cpp | 2 +- src/components/real/aocs/gnss_receiver.cpp | 10 ++++----- src/components/real/aocs/gyro_sensor.cpp | 8 +++---- src/components/real/aocs/magnetometer.cpp | 8 +++---- src/components/real/aocs/magnetorquer.cpp | 12 +++++----- .../aocs/mtq_magnetometer_interference.cpp | 2 +- src/components/real/aocs/reaction_wheel.cpp | 14 ++++++------ src/components/real/aocs/star_sensor.cpp | 10 ++++----- src/components/real/aocs/star_sensor.hpp | 12 +++++----- src/components/real/aocs/sun_sensor.cpp | 8 +++---- src/components/real/communication/antenna.cpp | 2 +- .../antenna_radiation_pattern.cpp | 2 +- .../ground_station_calculator.cpp | 2 +- .../wings_command_sender_to_c2a.cpp | 2 +- src/components/real/mission/telescope.cpp | 22 +++++++++---------- src/components/real/mission/telescope.hpp | 10 ++++----- src/components/real/power/battery.cpp | 2 +- .../real/power/csv_scenario_interface.cpp | 2 +- .../real/power/pcu_initial_study.cpp | 2 +- .../real/power/solar_array_panel.cpp | 4 ++-- .../real/propulsion/simple_thruster.cpp | 12 +++++----- src/disturbances/air_drag.cpp | 10 ++++----- src/disturbances/disturbance.hpp | 2 +- src/disturbances/disturbances.cpp | 2 +- src/disturbances/geopotential.cpp | 10 ++++----- src/disturbances/gravity_gradient.cpp | 8 +++---- src/disturbances/lunar_gravity_field.cpp | 10 ++++----- src/disturbances/magnetic_disturbance.cpp | 10 ++++----- .../solar_radiation_pressure_disturbance.cpp | 10 ++++----- src/disturbances/third_body_gravity.cpp | 8 +++---- src/dynamics/attitude/attitude.cpp | 8 +++---- .../attitude_with_cantilever_vibration.cpp | 8 +++---- src/dynamics/attitude/initialize_attitude.cpp | 8 +++---- src/dynamics/orbit/initialize_orbit.cpp | 4 ++-- src/dynamics/orbit/orbit.cpp | 20 ++++++++--------- src/dynamics/thermal/temperature.cpp | 12 +++++----- .../global/celestial_information.cpp | 6 ++--- src/environment/global/global_environment.cpp | 2 +- src/environment/global/gnss_satellites.cpp | 6 ++--- .../global/hipparcos_catalogue.cpp | 2 +- src/environment/global/simulation_time.cpp | 2 +- src/environment/local/atmosphere.cpp | 2 +- src/environment/local/earth_albedo.cpp | 2 +- src/environment/local/geomagnetic_field.cpp | 10 ++++----- .../local/local_celestial_information.cpp | 4 ++-- src/environment/local/local_environment.cpp | 4 ++-- .../solar_radiation_pressure_environment.cpp | 2 +- src/logger/initialize_log.cpp | 4 ++-- 56 files changed, 198 insertions(+), 198 deletions(-) diff --git a/src/components/base/sensor_template_functions.hpp b/src/components/base/sensor_template_functions.hpp index 31c761393..b1236901a 100644 --- a/src/components/base/sensor_template_functions.hpp +++ b/src/components/base/sensor_template_functions.hpp @@ -79,7 +79,7 @@ void Sensor::RangeCheck(void) { template Sensor ReadSensorInformation(const std::string file_name, const double step_width_s, const std::string component_name, const std::string unit) { - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); std::string section = "SENSOR_BASE_" + component_name; s2e::math::Vector scale_factor_vector; diff --git a/src/components/ideal/angular_velocity_observer.cpp b/src/components/ideal/angular_velocity_observer.cpp index 7e3bea5d9..94a12c467 100644 --- a/src/components/ideal/angular_velocity_observer.cpp +++ b/src/components/ideal/angular_velocity_observer.cpp @@ -9,7 +9,7 @@ namespace s2e::components { -AngularVelocityObserver::AngularVelocityObserver(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const Attitude& attitude) +AngularVelocityObserver::AngularVelocityObserver(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const dynamics::attitude::Attitude& attitude) : Component(prescaler, clock_generator), Sensor(sensor_base), attitude_(attitude) {} void AngularVelocityObserver::MainRoutine(const int time_count) { @@ -21,7 +21,7 @@ std::string AngularVelocityObserver::GetLogHeader() const { std::string str_tmp = ""; std::string sensor_name = "angular_velocity_observer_"; - str_tmp += WriteVector(sensor_name + "measured_value", "b", "rad/s", 3); + str_tmp += logger::WriteVector(sensor_name + "measured_value", "b", "rad/s", 3); return str_tmp; } @@ -29,14 +29,14 @@ std::string AngularVelocityObserver::GetLogHeader() const { std::string AngularVelocityObserver::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(angular_velocity_b_rad_s_); + str_tmp += logger::WriteVector(angular_velocity_b_rad_s_); return str_tmp; } AngularVelocityObserver InitializeAngularVelocityObserver(environment::ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s, - const Attitude& attitude) { - IniAccess ini_file(file_name); + const dynamics::attitude::Attitude& attitude) { + setting_file_reader::IniAccess ini_file(file_name); int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); if (prescaler <= 1) prescaler = 1; diff --git a/src/components/ideal/angular_velocity_observer.hpp b/src/components/ideal/angular_velocity_observer.hpp index 67d421f9b..404785058 100644 --- a/src/components/ideal/angular_velocity_observer.hpp +++ b/src/components/ideal/angular_velocity_observer.hpp @@ -28,7 +28,7 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public logge * @param [in] sensor_base: Sensor base information * @param [in] dynamics: Dynamics information */ - AngularVelocityObserver(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const Attitude& attitude); + AngularVelocityObserver(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const dynamics::attitude::Attitude& attitude); /** * @fn ~AngularVelocityObserver * @brief Destructor @@ -63,7 +63,7 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public logge protected: s2e::math::Vector<3> angular_velocity_b_rad_s_{0.0}; //!< Observed angular velocity [rad/s] - const Attitude& attitude_; //!< Dynamics information + const dynamics::attitude::Attitude& attitude_; //!< Dynamics information }; /** @@ -75,7 +75,7 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public logge * @param [in] dynamics: Dynamics information */ AngularVelocityObserver InitializeAngularVelocityObserver(environment::ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s, - const Attitude& attitude); + const dynamics::attitude::Attitude& attitude); } // namespace s2e::components diff --git a/src/components/ideal/attitude_observer.cpp b/src/components/ideal/attitude_observer.cpp index 1cb6e6126..9d2199056 100644 --- a/src/components/ideal/attitude_observer.cpp +++ b/src/components/ideal/attitude_observer.cpp @@ -11,7 +11,7 @@ namespace s2e::components { AttitudeObserver::AttitudeObserver(const int prescaler, environment::ClockGenerator* clock_generator, const double standard_deviation_rad, - const Attitude& attitude) + const dynamics::attitude::Attitude& attitude) : Component(prescaler, clock_generator), angle_noise_(0.0, standard_deviation_rad), attitude_(attitude) { direction_noise_.SetParameters(0.0, 1.0); } @@ -49,9 +49,9 @@ std::string AttitudeObserver::GetLogValue() const { return str_tmp; } -AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const Attitude& attitude) { +AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::attitude::Attitude& attitude) { // General - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); // CompoBase int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); diff --git a/src/components/ideal/attitude_observer.hpp b/src/components/ideal/attitude_observer.hpp index 8ce9697e5..59c00dd29 100644 --- a/src/components/ideal/attitude_observer.hpp +++ b/src/components/ideal/attitude_observer.hpp @@ -26,9 +26,9 @@ class AttitudeObserver : public Component, public logger::ILoggable { * @brief Constructor without power port * @param [in] prescaler: Frequency scale factor for update * @param [in] clock_generator: Clock generator - * @param [in] attitude: Attitude information + * @param [in] attitude: dynamics::attitude::Attitude information */ - AttitudeObserver(const int prescaler, environment::ClockGenerator* clock_generator, const double standard_deviation_rad, const Attitude& attitude); + AttitudeObserver(const int prescaler, environment::ClockGenerator* clock_generator, const double standard_deviation_rad, const dynamics::attitude::Attitude& attitude); /** * @fn ~AttitudeObserver @@ -67,7 +67,7 @@ class AttitudeObserver : public Component, public logger::ILoggable { s2e::randomization::NormalRand angle_noise_; //!< Normal random for magnitude noise s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise - const Attitude& attitude_; //!< Attitude information + const dynamics::attitude::Attitude& attitude_; //!< dynamics::attitude::Attitude information }; /** @@ -75,9 +75,9 @@ class AttitudeObserver : public Component, public logger::ILoggable { * @brief Initialize functions for AttitudeObserver * @param [in] clock_generator: Clock generator * @param [in] file_name: Path to the initialize file - * @param [in] attitude: Attitude information + * @param [in] attitude: dynamics::attitude::Attitude information */ -AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const Attitude& attitude); +AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::attitude::Attitude& attitude); } // namespace s2e::components diff --git a/src/components/ideal/force_generator.cpp b/src/components/ideal/force_generator.cpp index f458160d7..86b17da21 100644 --- a/src/components/ideal/force_generator.cpp +++ b/src/components/ideal/force_generator.cpp @@ -68,10 +68,10 @@ std::string ForceGenerator::GetLogHeader() const { std::string str_tmp = ""; std::string head = "ideal_force_generator_"; - str_tmp += WriteVector(head + "ordered_force", "b", "N", 3); - str_tmp += WriteVector(head + "generated_force", "b", "N", 3); - str_tmp += WriteVector(head + "generated_force", "i", "N", 3); - str_tmp += WriteVector(head + "generated_force", "rtn", "N", 3); + str_tmp += logger::WriteVector(head + "ordered_force", "b", "N", 3); + str_tmp += logger::WriteVector(head + "generated_force", "b", "N", 3); + str_tmp += logger::WriteVector(head + "generated_force", "i", "N", 3); + str_tmp += logger::WriteVector(head + "generated_force", "rtn", "N", 3); return str_tmp; } @@ -79,10 +79,10 @@ std::string ForceGenerator::GetLogHeader() const { std::string ForceGenerator::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(ordered_force_b_N_); - str_tmp += WriteVector(generated_force_b_N_); - str_tmp += WriteVector(generated_force_i_N_); - str_tmp += WriteVector(generated_force_rtn_N_); + str_tmp += logger::WriteVector(ordered_force_b_N_); + str_tmp += logger::WriteVector(generated_force_b_N_); + str_tmp += logger::WriteVector(generated_force_i_N_); + str_tmp += logger::WriteVector(generated_force_rtn_N_); return str_tmp; } @@ -109,7 +109,7 @@ s2e::math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(s2e::math ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics) { // General - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); // CompoBase int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 0851de4d8..58ede8f6b 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -58,8 +58,8 @@ std::string OrbitObserver::GetLogHeader() const { std::string str_tmp = ""; std::string head = "orbit_observer_"; - str_tmp += WriteVector(head + "position", "i", "m", 3); - str_tmp += WriteVector(head + "velocity", "i", "m/s", 3); + str_tmp += logger::WriteVector(head + "position", "i", "m", 3); + str_tmp += logger::WriteVector(head + "velocity", "i", "m/s", 3); return str_tmp; } @@ -67,8 +67,8 @@ std::string OrbitObserver::GetLogHeader() const { std::string OrbitObserver::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(observed_position_i_m_, 16); - str_tmp += WriteVector(observed_velocity_i_m_s_, 16); + str_tmp += logger::WriteVector(observed_position_i_m_, 16); + str_tmp += logger::WriteVector(observed_velocity_i_m_s_, 16); return str_tmp; } @@ -87,7 +87,7 @@ NoiseFrame SetNoiseFrame(const std::string noise_frame) { OrbitObserver InitializeOrbitObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { // General - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); // CompoBase int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); diff --git a/src/components/ideal/torque_generator.cpp b/src/components/ideal/torque_generator.cpp index a47ede7c9..74a478a29 100644 --- a/src/components/ideal/torque_generator.cpp +++ b/src/components/ideal/torque_generator.cpp @@ -45,8 +45,8 @@ std::string TorqueGenerator::GetLogHeader() const { std::string str_tmp = ""; std::string head = "ideal_torque_generator_"; - str_tmp += WriteVector(head + "ordered_torque", "b", "Nm", 3); - str_tmp += WriteVector(head + "generated_torque", "b", "Nm", 3); + str_tmp += logger::WriteVector(head + "ordered_torque", "b", "Nm", 3); + str_tmp += logger::WriteVector(head + "generated_torque", "b", "Nm", 3); return str_tmp; } @@ -54,8 +54,8 @@ std::string TorqueGenerator::GetLogHeader() const { std::string TorqueGenerator::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(ordered_torque_b_Nm_); - str_tmp += WriteVector(generated_torque_b_Nm_); + str_tmp += logger::WriteVector(ordered_torque_b_Nm_); + str_tmp += logger::WriteVector(generated_torque_b_Nm_); return str_tmp; } @@ -82,7 +82,7 @@ s2e::math::Quaternion TorqueGenerator::GenerateDirectionNoiseQuaternion(s2e::mat TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics) { // General - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); // CompoBase int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); diff --git a/src/components/ports/power_port.cpp b/src/components/ports/power_port.cpp index ac763115e..3964bb674 100644 --- a/src/components/ports/power_port.cpp +++ b/src/components/ports/power_port.cpp @@ -68,7 +68,7 @@ void PowerPort::SubtractAssumedPowerConsumption_W(const double power_W) { } void PowerPort::InitializeWithInitializeFile(const std::string file_name) { - IniAccess initialize_file(file_name); + setting_file_reader::IniAccess initialize_file(file_name); const std::string section_name = "POWER_PORT"; double minimum_voltage_V = initialize_file.ReadDouble(section_name.c_str(), "minimum_voltage_V"); diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 9b6a80cc8..f68873453 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -208,8 +208,8 @@ std::string GnssReceiver::GetLogHeader() const // For logs str_tmp += WriteScalar(sensor_name + "measured_utc_time_hour"); str_tmp += WriteScalar(sensor_name + "measured_utc_time_min"); str_tmp += WriteScalar(sensor_name + "measured_utc_time_sec"); - str_tmp += WriteVector(sensor_name + "measured_position", "ecef", "m", 3); - str_tmp += WriteVector(sensor_name + "measured_velocity", "ecef", "m/s", 3); + str_tmp += logger::WriteVector(sensor_name + "measured_position", "ecef", "m", 3); + str_tmp += logger::WriteVector(sensor_name + "measured_velocity", "ecef", "m/s", 3); str_tmp += WriteScalar(sensor_name + "measured_latitude", "rad"); str_tmp += WriteScalar(sensor_name + "measured_longitude", "rad"); str_tmp += WriteScalar(sensor_name + "measured_altitude", "m"); @@ -228,8 +228,8 @@ std::string GnssReceiver::GetLogValue() const // For logs str_tmp += WriteScalar(utc_.hour); str_tmp += WriteScalar(utc_.minute); str_tmp += WriteScalar(utc_.second); - str_tmp += WriteVector(position_ecef_m_, 10); - str_tmp += WriteVector(velocity_ecef_m_s_, 10); + str_tmp += logger::WriteVector(position_ecef_m_, 10); + str_tmp += logger::WriteVector(velocity_ecef_m_s_, 10); str_tmp += WriteScalar(geodetic_position_.GetLatitude_rad(), 10); str_tmp += WriteScalar(geodetic_position_.GetLongitude_rad(), 10); str_tmp += WriteScalar(geodetic_position_.GetAltitude_m(), 10); @@ -264,7 +264,7 @@ typedef struct _gnss_receiver_param { GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const size_t component_id) { GnssReceiverParam gnss_receiver_param; - IniAccess gnssr_conf(file_name); + setting_file_reader::IniAccess gnssr_conf(file_name); const char* sensor_name = "GNSS_RECEIVER_"; const std::string section_name = sensor_name + std::to_string(static_cast(component_id)); const char* GSection = section_name.c_str(); diff --git a/src/components/real/aocs/gyro_sensor.cpp b/src/components/real/aocs/gyro_sensor.cpp index f967195fa..fe16a6465 100644 --- a/src/components/real/aocs/gyro_sensor.cpp +++ b/src/components/real/aocs/gyro_sensor.cpp @@ -34,7 +34,7 @@ std::string GyroSensor::GetLogHeader() const { std::string str_tmp = ""; const std::string sensor_id = std::to_string(static_cast(sensor_id_)); std::string sensor_name = "gyro_sensor" + sensor_id + "_"; - str_tmp += WriteVector(sensor_name + "measured_angular_velocity", "c", "rad/s", kGyroDimension); + str_tmp += logger::WriteVector(sensor_name + "measured_angular_velocity", "c", "rad/s", kGyroDimension); return str_tmp; } @@ -42,14 +42,14 @@ std::string GyroSensor::GetLogHeader() const { std::string GyroSensor::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(angular_velocity_c_rad_s_); + str_tmp += logger::WriteVector(angular_velocity_c_rad_s_); return str_tmp; } GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, const Dynamics* dynamics) { - IniAccess gyro_conf(file_name); + setting_file_reader::IniAccess gyro_conf(file_name); const char* sensor_name = "GYRO_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); const char* GSection = section_name.c_str(); @@ -70,7 +70,7 @@ GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, int sens GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const Dynamics* dynamics) { - IniAccess gyro_conf(file_name); + setting_file_reader::IniAccess gyro_conf(file_name); const char* sensor_name = "GYRO_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); const char* GSection = section_name.c_str(); diff --git a/src/components/real/aocs/magnetometer.cpp b/src/components/real/aocs/magnetometer.cpp index c12e7b37f..dba5c8f45 100644 --- a/src/components/real/aocs/magnetometer.cpp +++ b/src/components/real/aocs/magnetometer.cpp @@ -36,7 +36,7 @@ std::string Magnetometer::GetLogHeader() const { std::string str_tmp = ""; const std::string sensor_id = std::to_string(static_cast(sensor_id_)); std::string sensor_name = "magnetometer" + sensor_id + "_"; - str_tmp += WriteVector(sensor_name + "measured_magnetic_field", "c", "nT", kMagnetometerDimension); + str_tmp += logger::WriteVector(sensor_name + "measured_magnetic_field", "c", "nT", kMagnetometerDimension); return str_tmp; } @@ -44,14 +44,14 @@ std::string Magnetometer::GetLogHeader() const { std::string Magnetometer::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(magnetic_field_c_nT_); + str_tmp += logger::WriteVector(magnetic_field_c_nT_); return str_tmp; } Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field) { - IniAccess magsensor_conf(file_name); + setting_file_reader::IniAccess magsensor_conf(file_name); const char* sensor_name = "MAGNETOMETER_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); const char* MSSection = section_name.c_str(); @@ -72,7 +72,7 @@ Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, int Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field) { - IniAccess magsensor_conf(file_name); + setting_file_reader::IniAccess magsensor_conf(file_name); const char* sensor_name = "MAGNETOMETER_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); const char* MSSection = section_name.c_str(); diff --git a/src/components/real/aocs/magnetorquer.cpp b/src/components/real/aocs/magnetorquer.cpp index cab226ac7..863d1ce5b 100644 --- a/src/components/real/aocs/magnetorquer.cpp +++ b/src/components/real/aocs/magnetorquer.cpp @@ -99,23 +99,23 @@ std::string Magnetorquer::GetLogHeader() const { const std::string actuator_id = std::to_string(static_cast(component_id_)); std::string actuator_name = "magnetorquer" + actuator_id + "_"; - str_tmp += WriteVector(actuator_name + "output_magnetic_moment", "b", "Am2", kMtqDimension); - str_tmp += WriteVector(actuator_name + "output_torque", "b", "Nm", kMtqDimension); + str_tmp += logger::WriteVector(actuator_name + "output_magnetic_moment", "b", "Am2", kMtqDimension); + str_tmp += logger::WriteVector(actuator_name + "output_torque", "b", "Nm", kMtqDimension); return str_tmp; } std::string Magnetorquer::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(output_magnetic_moment_b_Am2_); - str_tmp += WriteVector(torque_b_Nm_); + str_tmp += logger::WriteVector(output_magnetic_moment_b_Am2_); + str_tmp += logger::WriteVector(torque_b_Nm_); return str_tmp; } Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int actuator_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field) { - IniAccess magtorquer_conf(file_name); + setting_file_reader::IniAccess magtorquer_conf(file_name); const char* sensor_name = "MAGNETORQUER_"; const std::string section_name = sensor_name + std::to_string(static_cast(actuator_id)); const char* MTSection = section_name.c_str(); @@ -160,7 +160,7 @@ Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field) { - IniAccess magtorquer_conf(file_name); + setting_file_reader::IniAccess magtorquer_conf(file_name); const char* sensor_name = "MAGNETORQUER_"; const std::string section_name = sensor_name + std::to_string(static_cast(actuator_id)); const char* MTSection = section_name.c_str(); diff --git a/src/components/real/aocs/mtq_magnetometer_interference.cpp b/src/components/real/aocs/mtq_magnetometer_interference.cpp index b2f23f474..7113ee869 100644 --- a/src/components/real/aocs/mtq_magnetometer_interference.cpp +++ b/src/components/real/aocs/mtq_magnetometer_interference.cpp @@ -13,7 +13,7 @@ MtqMagnetometerInterference::MtqMagnetometerInterference(const std::string file_ const size_t initialize_id) : magnetometer_(magnetometer), magnetorquer_(magnetorquer) { // Read ini file - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); std::string section = "MTQ_MAGNETOMETER_INTERFERENCE_" + std::to_string(static_cast(initialize_id)); polynomial_degree_ = (size_t)ini_file.ReadInt(section.c_str(), "polynomial_degree"); diff --git a/src/components/real/aocs/reaction_wheel.cpp b/src/components/real/aocs/reaction_wheel.cpp index 9fe963c84..4124f0af1 100644 --- a/src/components/real/aocs/reaction_wheel.cpp +++ b/src/components/real/aocs/reaction_wheel.cpp @@ -205,8 +205,8 @@ std::string ReactionWheel::GetLogHeader() const { str_tmp += WriteScalar(component_name + "angular_acceleration", "rad/s2"); if (is_logged_jitter_ && is_calculated_jitter_) { - str_tmp += WriteVector(component_name + "jitter_force", "c", "N", 3); - str_tmp += WriteVector(component_name + "jitter_torque", "c", "Nm", 3); + str_tmp += logger::WriteVector(component_name + "jitter_force", "c", "N", 3); + str_tmp += logger::WriteVector(component_name + "jitter_torque", "c", "Nm", 3); } return str_tmp; @@ -222,8 +222,8 @@ std::string ReactionWheel::GetLogValue() const { str_tmp += WriteScalar(generated_angular_acceleration_rad_s2_); if (is_logged_jitter_ && is_calculated_jitter_) { - str_tmp += WriteVector(rw_jitter_.GetJitterForce_c_N()); - str_tmp += WriteVector(rw_jitter_.GetJitterTorque_c_Nm()); + str_tmp += logger::WriteVector(rw_jitter_.GetJitterForce_c_N()); + str_tmp += logger::WriteVector(rw_jitter_.GetJitterTorque_c_Nm()); } return str_tmp; @@ -260,7 +260,7 @@ ReactionWheelJitter rw_jitter; void InitParams(int actuator_id, std::string file_name, double compo_update_step_s) { // Access Parameters - IniAccess rw_ini_file(file_name); + setting_file_reader::IniAccess rw_ini_file(file_name); std::string section_tmp = "REACTION_WHEEL_" + std::to_string(static_cast(actuator_id)); const char* rw_section = section_tmp.c_str(); @@ -316,8 +316,8 @@ void InitParams(int actuator_id, std::string file_name, double compo_update_step std::string radial_force_harmonics_coefficient_path = rw_ini_file.ReadString(jitter_section, "radial_force_harmonics_coefficient_file"); std::string radial_torque_harmonics_coefficient_path = rw_ini_file.ReadString(jitter_section, "radial_torque_harmonics_coefficient_file"); int harmonics_degree = rw_ini_file.ReadInt(jitter_section, "harmonics_degree"); - IniAccess conf_radial_force_harmonics(radial_force_harmonics_coefficient_path); - IniAccess conf_radial_torque_harmonics(radial_torque_harmonics_coefficient_path); + setting_file_reader::IniAccess conf_radial_force_harmonics(radial_force_harmonics_coefficient_path); + setting_file_reader::IniAccess conf_radial_torque_harmonics(radial_torque_harmonics_coefficient_path); std::vector> radial_force_harmonics_coefficients; std::vector> radial_torque_harmonics_coefficients; conf_radial_force_harmonics.ReadCsvDouble(radial_force_harmonics_coefficients, harmonics_degree); diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index 6466a60e8..324288777 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -90,7 +90,7 @@ void StarSensor::Initialize() { error_flag_ = true; } -Quaternion StarSensor::Measure(const LocalCelestialInformation* local_celestial_information, const Attitude* attitude) { +Quaternion StarSensor::Measure(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude) { update(local_celestial_information, attitude); // update delay buffer if (update_count_ == 0) { int hist = buffer_position_ - output_delay_ - 1; @@ -106,7 +106,7 @@ Quaternion StarSensor::Measure(const LocalCelestialInformation* local_celestial_ return measured_quaternion_i2c_; } -void StarSensor::update(const LocalCelestialInformation* local_celestial_information, const Attitude* attitude) { +void StarSensor::update(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude) { Quaternion quaternion_i2b = attitude->GetQuaternion_i2b(); // Read true value Quaternion q_stt_temp = quaternion_i2b * quaternion_b2c_; // Convert to component frame // Add noise on sight direction @@ -127,7 +127,7 @@ void StarSensor::update(const LocalCelestialInformation* local_celestial_informa buffer_position_ %= max_delay_; } -void StarSensor::AllJudgement(const LocalCelestialInformation* local_celestial_information, const Attitude* attitude) { +void StarSensor::AllJudgement(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude) { int judgement = 0; judgement = SunJudgement(local_celestial_information->GetPositionFromSpacecraft_b_m("SUN")); judgement += EarthJudgement(local_celestial_information->GetPositionFromSpacecraft_b_m("EARTH")); @@ -217,7 +217,7 @@ void StarSensor::MainRoutine(const int time_count) { StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, double component_step_time_s, const Dynamics* dynamics, const LocalEnvironment* local_environment) { - IniAccess STT_conf(file_name); + setting_file_reader::IniAccess STT_conf(file_name); const char* sensor_name = "STAR_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); const char* STTSection = section_name.c_str(); @@ -250,7 +250,7 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sens StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const string file_name, double component_step_time_s, const Dynamics* dynamics, const LocalEnvironment* local_environment) { - IniAccess STT_conf(file_name); + setting_file_reader::IniAccess STT_conf(file_name); const char* sensor_name = "STAR_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); const char* STTSection = section_name.c_str(); diff --git a/src/components/real/aocs/star_sensor.hpp b/src/components/real/aocs/star_sensor.hpp index 9a7ebf842..5e3ab99b2 100644 --- a/src/components/real/aocs/star_sensor.hpp +++ b/src/components/real/aocs/star_sensor.hpp @@ -146,24 +146,24 @@ class StarSensor : public Component, public logger::ILoggable { * @fn update * @brief Update delay buffer * @param [in] local_celestial_information: Local celestial information - * @param [in] attitude: Attitude information + * @param [in] attitude: dynamics::attitude::Attitude information */ - void update(const LocalCelestialInformation* local_celestial_information, const Attitude* attitude); + void update(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); /** * @fn Measure * @brief Calculate measured quaternion * @param [in] local_celestial_information: Local celestial information - * @param [in] attitude: Attitude information + * @param [in] attitude: dynamics::attitude::Attitude information */ - s2e::math::Quaternion Measure(const LocalCelestialInformation* local_celestial_information, const Attitude* attitude); + s2e::math::Quaternion Measure(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); /** * @fn AllJudgement * @brief Calculate all error judgement * @param [in] local_celestial_information: Local celestial information - * @param [in] attitude: Attitude information + * @param [in] attitude: dynamics::attitude::Attitude information */ - void AllJudgement(const LocalCelestialInformation* local_celestial_information, const Attitude* attitude); + void AllJudgement(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); /** * @fn SunJudgement * @brief Judge violation of sun forbidden angle diff --git a/src/components/real/aocs/sun_sensor.cpp b/src/components/real/aocs/sun_sensor.cpp index 1e0b8142a..82e74e40a 100644 --- a/src/components/real/aocs/sun_sensor.cpp +++ b/src/components/real/aocs/sun_sensor.cpp @@ -138,7 +138,7 @@ string SunSensor::GetLogHeader() const { const string sensor_id = std::to_string(static_cast(component_id_)); std::string sensor_name = "sun_sensor" + sensor_id + "_"; - str_tmp += WriteVector(sensor_name + "measured_sun_direction", "c", "-", 3); + str_tmp += logger::WriteVector(sensor_name + "measured_sun_direction", "c", "-", 3); str_tmp += WriteScalar(sensor_name + "sun_detected_flag", "-"); return str_tmp; @@ -147,7 +147,7 @@ string SunSensor::GetLogHeader() const { string SunSensor::GetLogValue() const { string str_tmp = ""; - str_tmp += WriteVector(measured_sun_direction_c_); + str_tmp += logger::WriteVector(measured_sun_direction_c_); str_tmp += WriteScalar(double(sun_detected_flag_)); return str_tmp; @@ -155,7 +155,7 @@ string SunSensor::GetLogValue() const { SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int ss_id, std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) { - IniAccess ss_conf(file_name); + setting_file_reader::IniAccess ss_conf(file_name); const char* sensor_name = "SUN_SENSOR_"; const std::string section_tmp = sensor_name + std::to_string(static_cast(ss_id)); const char* Section = section_tmp.c_str(); @@ -188,7 +188,7 @@ SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int ss_id, SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int ss_id, std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) { - IniAccess ss_conf(file_name); + setting_file_reader::IniAccess ss_conf(file_name); const char* sensor_name = "SUN_SENSOR_"; const std::string section_tmp = sensor_name + std::to_string(static_cast(ss_id)); const char* Section = section_tmp.c_str(); diff --git a/src/components/real/communication/antenna.cpp b/src/components/real/communication/antenna.cpp index fb22a00b8..3eb527fd4 100644 --- a/src/components/real/communication/antenna.cpp +++ b/src/components/real/communication/antenna.cpp @@ -108,7 +108,7 @@ AntennaGainModel SetAntennaGainModel(const std::string gain_model_name) { } Antenna InitAntenna(const int antenna_id, const std::string file_name) { - IniAccess antenna_conf(file_name); + setting_file_reader::IniAccess antenna_conf(file_name); const std::string section_name = "ANTENNA_" + std::to_string(static_cast(antenna_id)); diff --git a/src/components/real/communication/antenna_radiation_pattern.cpp b/src/components/real/communication/antenna_radiation_pattern.cpp index 71566f080..59e1ac935 100644 --- a/src/components/real/communication/antenna_radiation_pattern.cpp +++ b/src/components/real/communication/antenna_radiation_pattern.cpp @@ -16,7 +16,7 @@ AntennaRadiationPattern::AntennaRadiationPattern() { gain_dBi_.assign(length_the AntennaRadiationPattern::AntennaRadiationPattern(const std::string file_path, const size_t length_theta, const size_t length_phi, const double theta_max_rad, const double phi_max_rad) : length_theta_(length_theta), length_phi_(length_phi), theta_max_rad_(theta_max_rad), phi_max_rad_(phi_max_rad) { - IniAccess gain_file(file_path); + setting_file_reader::IniAccess gain_file(file_path); gain_file.ReadCsvDouble(gain_dBi_, (std::max)(length_theta_, length_phi_)); } diff --git a/src/components/real/communication/ground_station_calculator.cpp b/src/components/real/communication/ground_station_calculator.cpp index f53afc30a..554919b92 100644 --- a/src/components/real/communication/ground_station_calculator.cpp +++ b/src/components/real/communication/ground_station_calculator.cpp @@ -120,7 +120,7 @@ std::string GroundStationCalculator::GetLogValue() const { } GroundStationCalculator InitGsCalculator(const std::string file_name) { - IniAccess gs_conf(file_name); + setting_file_reader::IniAccess gs_conf(file_name); char Section[30] = "GROUND_STATION_CALCULATOR"; diff --git a/src/components/real/communication/wings_command_sender_to_c2a.cpp b/src/components/real/communication/wings_command_sender_to_c2a.cpp index 899241372..e075b82ae 100644 --- a/src/components/real/communication/wings_command_sender_to_c2a.cpp +++ b/src/components/real/communication/wings_command_sender_to_c2a.cpp @@ -135,7 +135,7 @@ void WingsCommandSenderToC2a::AnalyzeC2aCommand(const std::vector t WingsCommandSenderToC2a InitWingsCommandSenderToC2a(environment::ClockGenerator* clock_generator, const double compo_update_step_s, const std::string initialize_file) { - IniAccess ini_access(initialize_file); + setting_file_reader::IniAccess ini_access(initialize_file); std::string section = "WINGS_COMMAND_SENDER_TO_C2A"; bool is_enabled = ini_access.ReadEnable(section.c_str(), "command_send_enable"); diff --git a/src/components/real/mission/telescope.cpp b/src/components/real/mission/telescope.cpp index 5e6e3c8da..b04e5b0da 100644 --- a/src/components/real/mission/telescope.cpp +++ b/src/components/real/mission/telescope.cpp @@ -18,7 +18,7 @@ namespace s2e::components { Telescope::Telescope(environment::ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, - const Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, + const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) : Component(1, clock_generator), quaternion_b2c_(quaternion_b2c), @@ -198,9 +198,9 @@ string Telescope::GetLogHeader() const { str_tmp += WriteScalar(component_name + "sun_in_exclusion_angle", ""); str_tmp += WriteScalar(component_name + "earth_in_exclusion_angle", ""); str_tmp += WriteScalar(component_name + "moon_in_exclusion_angle", ""); - str_tmp += WriteVector(component_name + "sun_position", "img", "pix", 2); - str_tmp += WriteVector(component_name + "earth_position", "img", "pix", 2); - str_tmp += WriteVector(component_name + "moon_position", "img", "pix", 2); + str_tmp += logger::WriteVector(component_name + "sun_position", "img", "pix", 2); + str_tmp += logger::WriteVector(component_name + "earth_position", "img", "pix", 2); + str_tmp += logger::WriteVector(component_name + "moon_position", "img", "pix", 2); str_tmp += WriteScalar(component_name + "ground_position_x", "pix"); str_tmp += WriteScalar(component_name + "ground_position_y", "pix"); // When Hipparcos Catalogue was not read, no output of ObserveStars @@ -208,7 +208,7 @@ string Telescope::GetLogHeader() const { for (size_t i = 0; i < number_of_logged_stars_; i++) { str_tmp += WriteScalar(component_name + "hipparcos_id (" + to_string(i) + ")", " "); str_tmp += WriteScalar(component_name + "visible_magnitude (" + to_string(i) + ")", " "); - str_tmp += WriteVector(component_name + "star_position (" + to_string(i) + ")", "img", "pix", 2); + str_tmp += logger::WriteVector(component_name + "star_position (" + to_string(i) + ")", "img", "pix", 2); } } @@ -225,9 +225,9 @@ string Telescope::GetLogValue() const { str_tmp += WriteScalar(is_sun_in_forbidden_angle); str_tmp += WriteScalar(is_earth_in_forbidden_angle); str_tmp += WriteScalar(is_moon_in_forbidden_angle); - str_tmp += WriteVector(sun_position_image_sensor); - str_tmp += WriteVector(earth_position_image_sensor); - str_tmp += WriteVector(moon_position_image_sensor); + str_tmp += logger::WriteVector(sun_position_image_sensor); + str_tmp += logger::WriteVector(earth_position_image_sensor); + str_tmp += logger::WriteVector(moon_position_image_sensor); str_tmp += WriteScalar(ground_position_x_image_sensor_); str_tmp += WriteScalar(ground_position_y_image_sensor_); // When Hipparcos Catalogue was not read, no output of ObserveStars @@ -235,7 +235,7 @@ string Telescope::GetLogValue() const { for (size_t i = 0; i < number_of_logged_stars_; i++) { str_tmp += WriteScalar(star_list_in_sight[i].hipparcos_data.hipparcos_id); str_tmp += WriteScalar(star_list_in_sight[i].hipparcos_data.visible_magnitude); - str_tmp += WriteVector(star_list_in_sight[i].position_image_sensor); + str_tmp += logger::WriteVector(star_list_in_sight[i].position_image_sensor); } } @@ -247,11 +247,11 @@ string Telescope::GetLogValue() const { return str_tmp; } -Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, const Attitude* attitude, +Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) { using s2e::math::pi; - IniAccess Telescope_conf(file_name); + setting_file_reader::IniAccess Telescope_conf(file_name); const string st_sensor_id = std::to_string(static_cast(sensor_id)); const char* cs = st_sensor_id.data(); diff --git a/src/components/real/mission/telescope.hpp b/src/components/real/mission/telescope.hpp index 452639e56..bad6d0fbf 100644 --- a/src/components/real/mission/telescope.hpp +++ b/src/components/real/mission/telescope.hpp @@ -47,14 +47,14 @@ class Telescope : public Component, public logger::ILoggable { * @param [in] x_fov_per_pix: Field of view per pixel of X-axis in the image plane [rad/pix] * @param [in] y_fov_per_pix: Field of view per pixel of Y-axis in the image plane [rad/pix] * @param [in] number_of_logged_stars: Number of logged stars - * @param [in] attitude: Attitude Information + * @param [in] attitude: dynamics::attitude::Attitude Information * @param [in] hipparcos: Hipparcos catalogue information * @param [in] local_celestial_information: Local celestial information * @param [in] orbit: Orbit information */ Telescope(environment::ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, - const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, const Attitude* attitude, + const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr); /** * @fn ~Telescope @@ -126,7 +126,7 @@ class Telescope : public Component, public logger::ILoggable { */ void ObserveStars(); - const Attitude* attitude_; //!< Attitude information + const dynamics::attitude::Attitude* attitude_; //!< dynamics::attitude::Attitude information const HipparcosCatalogue* hipparcos_; //!< Star information const LocalCelestialInformation* local_celestial_information_; //!< Local celestial information /** @@ -164,11 +164,11 @@ class Telescope : public Component, public logger::ILoggable { * @param [in] clock_generator: Clock generator * @param [in] sensor_id: Sensor ID * @param [in] file_name: Path to initialize file - * @param [in] attitude: Attitude information + * @param [in] attitude: dynamics::attitude::Attitude information * @param [in] hipparcos: Star information by Hipparcos catalogue * @param [in] local_celestial_information: Local celestial information */ -Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, const Attitude* attitude, +Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr); diff --git a/src/components/real/power/battery.cpp b/src/components/real/power/battery.cpp index a050e35b8..a6442ad3f 100644 --- a/src/components/real/power/battery.cpp +++ b/src/components/real/power/battery.cpp @@ -90,7 +90,7 @@ void Battery::UpdateBatVoltage() { } Battery InitBAT(environment::ClockGenerator* clock_generator, int bat_id, const std::string file_name, double component_step_time_s) { - IniAccess bat_conf(file_name); + setting_file_reader::IniAccess bat_conf(file_name); const std::string section_name = "BATTERY_" + std::to_string(static_cast(bat_id)); diff --git a/src/components/real/power/csv_scenario_interface.cpp b/src/components/real/power/csv_scenario_interface.cpp index f0efc14fb..51f2c2f4b 100644 --- a/src/components/real/power/csv_scenario_interface.cpp +++ b/src/components/real/power/csv_scenario_interface.cpp @@ -14,7 +14,7 @@ std::map CsvScenarioInterface::buffer_line_id_; std::map CsvScenarioInterface::buffers_; void CsvScenarioInterface::Initialize(const std::string file_name) { - IniAccess scenario_conf(file_name); + setting_file_reader::IniAccess scenario_conf(file_name); char Section[30] = "SCENARIO"; CsvScenarioInterface::is_csv_scenario_enabled_ = scenario_conf.ReadBoolean(Section, "is_csv_scenario_enabled"); diff --git a/src/components/real/power/pcu_initial_study.cpp b/src/components/real/power/pcu_initial_study.cpp index c79873dea..e89729490 100644 --- a/src/components/real/power/pcu_initial_study.cpp +++ b/src/components/real/power/pcu_initial_study.cpp @@ -115,7 +115,7 @@ void PcuInitialStudy::UpdateChargeCurrentAndBusVoltage() { PcuInitialStudy InitPCU_InitialStudy(environment::ClockGenerator* clock_generator, int pcu_id, const std::string file_name, const std::vector saps, Battery* battery, double component_step_time_s) { - IniAccess pcu_conf(file_name); + setting_file_reader::IniAccess pcu_conf(file_name); const std::string section_name = "PCU_INITIAL_STUDY_" + std::to_string(static_cast(pcu_id)); diff --git a/src/components/real/power/solar_array_panel.cpp b/src/components/real/power/solar_array_panel.cpp index fdde4d90a..df4bdad96 100644 --- a/src/components/real/power/solar_array_panel.cpp +++ b/src/components/real/power/solar_array_panel.cpp @@ -119,7 +119,7 @@ void SolarArrayPanel::MainRoutine(const int time_count) { SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, double component_step_time_s) { - IniAccess sap_conf(file_name); + setting_file_reader::IniAccess sap_conf(file_name); const std::string section_name = "SOLAR_ARRAY_PANEL_" + std::to_string(static_cast(sap_id)); @@ -152,7 +152,7 @@ SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s) { - IniAccess sap_conf(file_name); + setting_file_reader::IniAccess sap_conf(file_name); const std::string section_name = "SOLAR_ARRAY_PANEL_" + std::to_string(static_cast(sap_id)); diff --git a/src/components/real/propulsion/simple_thruster.cpp b/src/components/real/propulsion/simple_thruster.cpp index 9501f82bb..735ec87f0 100644 --- a/src/components/real/propulsion/simple_thruster.cpp +++ b/src/components/real/propulsion/simple_thruster.cpp @@ -79,8 +79,8 @@ std::string SimpleThruster::GetLogHeader() const { std::string str_tmp = ""; std::string head = "simple_thruster" + std::to_string(component_id_) + "_"; - str_tmp += WriteVector(head + "output_thrust", "b", "N", 3); - str_tmp += WriteVector(head + "output_torque", "b", "Nm", 3); + str_tmp += logger::WriteVector(head + "output_thrust", "b", "N", 3); + str_tmp += logger::WriteVector(head + "output_torque", "b", "Nm", 3); str_tmp += WriteScalar(head + "output_thrust_norm", "N"); return str_tmp; } @@ -88,8 +88,8 @@ std::string SimpleThruster::GetLogHeader() const { std::string SimpleThruster::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(output_thrust_b_N_); - str_tmp += WriteVector(output_torque_b_Nm_); + str_tmp += logger::WriteVector(output_thrust_b_N_); + str_tmp += logger::WriteVector(output_torque_b_Nm_); str_tmp += WriteScalar(output_thrust_b_N_.CalcNorm()); return str_tmp; @@ -124,7 +124,7 @@ s2e::math::Vector<3> SimpleThruster::CalcThrustDirection() { SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, const Structure* structure, const Dynamics* dynamics) { - IniAccess thruster_conf(file_name); + setting_file_reader::IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); auto* Section = section_str.c_str(); @@ -152,7 +152,7 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, const Structure* structure, const Dynamics* dynamics) { - IniAccess thruster_conf(file_name); + setting_file_reader::IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); auto* Section = section_str.c_str(); diff --git a/src/disturbances/air_drag.cpp b/src/disturbances/air_drag.cpp index 87884ba9b..a15817e36 100644 --- a/src/disturbances/air_drag.cpp +++ b/src/disturbances/air_drag.cpp @@ -80,8 +80,8 @@ void AirDrag::CalcCnCt(const Vector<3>& velocity_b_m_s) { std::string AirDrag::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteVector("air_drag_torque", "b", "Nm", 3); - str_tmp += WriteVector("air_drag_force", "b", "N", 3); + str_tmp += logger::WriteVector("air_drag_torque", "b", "Nm", 3); + str_tmp += logger::WriteVector("air_drag_force", "b", "N", 3); return str_tmp; } @@ -89,14 +89,14 @@ std::string AirDrag::GetLogHeader() const { std::string AirDrag::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(torque_b_Nm_); - str_tmp += WriteVector(force_b_N_); + str_tmp += logger::WriteVector(torque_b_Nm_); + str_tmp += logger::WriteVector(force_b_N_); return str_tmp; } AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, const Vector<3>& center_of_gravity_b_m) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "AIR_DRAG"; const double wall_temperature_K = conf.ReadDouble(section, "wall_temperature_degC") + 273.0; diff --git a/src/disturbances/disturbance.hpp b/src/disturbances/disturbance.hpp index 1d153810c..1c93d18a9 100644 --- a/src/disturbances/disturbance.hpp +++ b/src/disturbances/disturbance.hpp @@ -21,7 +21,7 @@ class Disturbance : public logger::ILoggable { * @fn Disturbance * @brief Constructor * @param [in] is_calculation_enabled: Calculation flag - * @param [in] is_attitude_dependent: Attitude dependent flag + * @param [in] is_attitude_dependent: dynamics::attitude::Attitude dependent flag */ Disturbance(const bool is_calculation_enabled = true, const bool is_attitude_dependent = true) : is_calculation_enabled_(is_calculation_enabled), is_attitude_dependent_(is_attitude_dependent) { diff --git a/src/disturbances/disturbances.cpp b/src/disturbances/disturbances.cpp index 84380248b..e274cbe96 100644 --- a/src/disturbances/disturbances.cpp +++ b/src/disturbances/disturbances.cpp @@ -59,7 +59,7 @@ void Disturbances::LogSetup(Logger& logger) { void Disturbances::InitializeInstances(const SimulationConfiguration* simulation_configuration, const int spacecraft_id, const Structure* structure, const GlobalEnvironment* global_environment) { - IniAccess ini_access = IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); + setting_file_reader::IniAccess ini_access = setting_file_reader::IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); initialize_file_name_ = ini_access.ReadString("SETTING_FILES", "disturbance_file"); GravityGradient* gg_dist = new GravityGradient( diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index 16123cf5d..f28f6a591 100644 --- a/src/disturbances/geopotential.cpp +++ b/src/disturbances/geopotential.cpp @@ -94,10 +94,10 @@ std::string Geopotential::GetLogHeader() const { std::string str_tmp = ""; #ifdef DEBUG_GEOPOTENTIAL - str_tmp += WriteVector("geopotential_calculation_position_", "ecef", "m", 3); + str_tmp += logger::WriteVector("geopotential_calculation_position_", "ecef", "m", 3); str_tmp += WriteScalar("geopotential_calculation_time", "ms"); #endif - str_tmp += WriteVector("geopotential_acceleration", "ecef", "m/s2", 3); + str_tmp += logger::WriteVector("geopotential_acceleration", "ecef", "m/s2", 3); return str_tmp; } @@ -106,17 +106,17 @@ std::string Geopotential::GetLogValue() const { std::string str_tmp = ""; #ifdef DEBUG_GEOPOTENTIAL - str_tmp += WriteVector(debug_pos_ecef_m_, 15); + str_tmp += logger::WriteVector(debug_pos_ecef_m_, 15); str_tmp += WriteScalar(time_ms_); #endif - str_tmp += WriteVector(acceleration_ecef_m_s2_, 15); + str_tmp += logger::WriteVector(acceleration_ecef_m_s2_, 15); return str_tmp; } Geopotential InitGeopotential(const std::string initialize_file_path) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char *section = "GEOPOTENTIAL"; const int degree = conf.ReadInt(section, "degree"); diff --git a/src/disturbances/gravity_gradient.cpp b/src/disturbances/gravity_gradient.cpp index b04b2c8d7..8ac14cdfa 100644 --- a/src/disturbances/gravity_gradient.cpp +++ b/src/disturbances/gravity_gradient.cpp @@ -38,7 +38,7 @@ s2e::math::Vector<3> GravityGradient::CalcTorque_b_Nm(const s2e::math::Vector<3> std::string GravityGradient::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteVector("gravity_gradient_torque", "b", "Nm", 3); + str_tmp += logger::WriteVector("gravity_gradient_torque", "b", "Nm", 3); return str_tmp; } @@ -46,13 +46,13 @@ std::string GravityGradient::GetLogHeader() const { std::string GravityGradient::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(torque_b_Nm_); + str_tmp += logger::WriteVector(torque_b_Nm_); return str_tmp; } GravityGradient InitGravityGradient(const std::string initialize_file_path) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "GRAVITY_GRADIENT"; const bool is_calc_enable = conf.ReadEnable(section, INI_CALC_LABEL); @@ -63,7 +63,7 @@ GravityGradient InitGravityGradient(const std::string initialize_file_path) { } GravityGradient InitGravityGradient(const std::string initialize_file_path, const double gravity_constant_m3_s2) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "GRAVITY_GRADIENT"; const bool is_calc_enable = conf.ReadEnable(section, INI_CALC_LABEL); diff --git a/src/disturbances/lunar_gravity_field.cpp b/src/disturbances/lunar_gravity_field.cpp index 747a0e065..7f7e63191 100644 --- a/src/disturbances/lunar_gravity_field.cpp +++ b/src/disturbances/lunar_gravity_field.cpp @@ -117,10 +117,10 @@ std::string LunarGravityField::GetLogHeader() const { std::string str_tmp = ""; #ifdef DEBUG_LUNAR_GRAVITY_FIELD - str_tmp += WriteVector("lunar_gravity_calculation_position", "mcmf", "m", 3); + str_tmp += logger::WriteVector("lunar_gravity_calculation_position", "mcmf", "m", 3); str_tmp += WriteScalar("lunar_gravity_calculation_time", "ms"); #endif - str_tmp += WriteVector("lunar_gravity_acceleration", "mcmf", "m/s2", 3); + str_tmp += logger::WriteVector("lunar_gravity_acceleration", "mcmf", "m/s2", 3); return str_tmp; } @@ -129,17 +129,17 @@ std::string LunarGravityField::GetLogValue() const { std::string str_tmp = ""; #ifdef DEBUG_LUNAR_GRAVITY_FIELD - str_tmp += WriteVector(debug_pos_mcmf_m_, 15); + str_tmp += logger::WriteVector(debug_pos_mcmf_m_, 15); str_tmp += WriteScalar(time_ms_); #endif - str_tmp += WriteVector(acceleration_mcmf_m_s2_, 15); + str_tmp += logger::WriteVector(acceleration_mcmf_m_s2_, 15); return str_tmp; } LunarGravityField InitLunarGravityField(const std::string initialize_file_path) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char *section = "LUNAR_GRAVITY_FIELD"; const int degree = conf.ReadInt(section, "degree"); diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index fec275450..3368dc8df 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -49,8 +49,8 @@ void MagneticDisturbance::CalcRMM() { std::string MagneticDisturbance::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteVector("spacecraft_magnetic_moment", "b", "Am2", 3); - str_tmp += WriteVector("magnetic_disturbance_torque", "b", "Nm", 3); + str_tmp += logger::WriteVector("spacecraft_magnetic_moment", "b", "Am2", 3); + str_tmp += logger::WriteVector("magnetic_disturbance_torque", "b", "Nm", 3); return str_tmp; } @@ -58,14 +58,14 @@ std::string MagneticDisturbance::GetLogHeader() const { std::string MagneticDisturbance::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(rmm_b_Am2_); - str_tmp += WriteVector(torque_b_Nm_); + str_tmp += logger::WriteVector(rmm_b_Am2_); + str_tmp += logger::WriteVector(torque_b_Nm_); return str_tmp; } MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const ResidualMagneticMoment& rmm_params) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "MAGNETIC_DISTURBANCE"; const bool is_calc_enable = conf.ReadEnable(section, INI_CALC_LABEL); diff --git a/src/disturbances/solar_radiation_pressure_disturbance.cpp b/src/disturbances/solar_radiation_pressure_disturbance.cpp index 490938542..b28784672 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.cpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.cpp @@ -39,8 +39,8 @@ void SolarRadiationPressureDisturbance::CalcCoefficients(const s2e::math::Vector std::string SolarRadiationPressureDisturbance::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteVector("srp_torque", "b", "Nm", 3); - str_tmp += WriteVector("srp_force", "b", "N", 3); + str_tmp += logger::WriteVector("srp_torque", "b", "Nm", 3); + str_tmp += logger::WriteVector("srp_force", "b", "N", 3); return str_tmp; } @@ -48,15 +48,15 @@ std::string SolarRadiationPressureDisturbance::GetLogHeader() const { std::string SolarRadiationPressureDisturbance::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(torque_b_Nm_); - str_tmp += WriteVector(force_b_N_); + str_tmp += logger::WriteVector(torque_b_Nm_); + str_tmp += logger::WriteVector(force_b_N_); return str_tmp; } SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const std::string initialize_file_path, const std::vector& surfaces, const Vector<3>& center_of_gravity_b_m) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "SOLAR_RADIATION_PRESSURE_DISTURBANCE"; const bool is_calc_enable = conf.ReadEnable(section, INI_CALC_LABEL); diff --git a/src/disturbances/third_body_gravity.cpp b/src/disturbances/third_body_gravity.cpp index eebb2ec05..476fe673e 100644 --- a/src/disturbances/third_body_gravity.cpp +++ b/src/disturbances/third_body_gravity.cpp @@ -46,21 +46,21 @@ s2e::math::Vector<3> ThirdBodyGravity::CalcAcceleration_i_m_s2(const s2e::math:: std::string ThirdBodyGravity::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteVector("third_body_acceleration", "i", "m/s2", 3); + str_tmp += logger::WriteVector("third_body_acceleration", "i", "m/s2", 3); return str_tmp; } std::string ThirdBodyGravity::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(acceleration_i_m_s2_); + str_tmp += logger::WriteVector(acceleration_i_m_s2_); return str_tmp; } ThirdBodyGravity InitThirdBodyGravity(const std::string initialize_file_path, const std::string ini_path_celes) { // Generate a list of bodies to be calculated in "CelesInfo" - auto conf_celes = IniAccess(ini_path_celes); + auto conf_celes = setting_file_reader::IniAccess(ini_path_celes); const char* section_celes = "CELESTIAL_INFORMATION"; const int num_of_selected_body = conf_celes.ReadInt(section_celes, "number_of_selected_body"); const std::string center_object = conf_celes.ReadString(section_celes, "center_object"); @@ -72,7 +72,7 @@ ThirdBodyGravity InitThirdBodyGravity(const std::string initialize_file_path, co } // Generate a list of bodies to be calculated in "ThirdBodyGravity" from the list of bodies of "CelesInfo" - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "THIRD_BODY_GRAVITY"; const int num_of_third_body = conf.ReadInt(section, "number_of_third_body"); diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index 4777184de..2ad9c0f20 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -24,9 +24,9 @@ Attitude::Attitude(const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const std std::string Attitude::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteVector("spacecraft_angular_velocity", "b", "rad/s", 3); + str_tmp += logger::WriteVector("spacecraft_angular_velocity", "b", "rad/s", 3); str_tmp += WriteQuaternion("spacecraft_quaternion", "i2b"); - str_tmp += WriteVector("spacecraft_torque", "b", "Nm", 3); + str_tmp += logger::WriteVector("spacecraft_torque", "b", "Nm", 3); str_tmp += WriteScalar("spacecraft_total_angular_momentum", "Nms"); str_tmp += WriteScalar("spacecraft_kinematic_energy", "J"); @@ -36,9 +36,9 @@ std::string Attitude::GetLogHeader() const { std::string Attitude::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(angular_velocity_b_rad_s_); + str_tmp += logger::WriteVector(angular_velocity_b_rad_s_); str_tmp += WriteQuaternion(quaternion_i2b_); - str_tmp += WriteVector(torque_b_Nm_); + str_tmp += logger::WriteVector(torque_b_Nm_); str_tmp += WriteScalar(angular_momentum_total_Nms_); str_tmp += WriteScalar(kinetic_energy_J_); diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp index 2b2ff73e3..711c623f4 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp @@ -46,8 +46,8 @@ AttitudeWithCantileverVibration::~AttitudeWithCantileverVibration() {} std::string AttitudeWithCantileverVibration::GetLogHeader() const { std::string str_tmp = Attitude::GetLogHeader(); - str_tmp += WriteVector("euler_angular_cantilever", "c", "rad", 3); - str_tmp += WriteVector("angular_velocity_cantilever", "c", "rad/s", 3); + str_tmp += logger::WriteVector("euler_angular_cantilever", "c", "rad", 3); + str_tmp += logger::WriteVector("angular_velocity_cantilever", "c", "rad/s", 3); return str_tmp; } @@ -55,8 +55,8 @@ std::string AttitudeWithCantileverVibration::GetLogHeader() const { std::string AttitudeWithCantileverVibration::GetLogValue() const { std::string str_tmp = Attitude::GetLogValue(); - str_tmp += WriteVector(euler_angular_cantilever_rad_); - str_tmp += WriteVector(angular_velocity_cantilever_rad_s_); + str_tmp += logger::WriteVector(euler_angular_cantilever_rad_); + str_tmp += logger::WriteVector(angular_velocity_cantilever_rad_s_); return str_tmp; } diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index 02e19ad09..875bb92f2 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -10,7 +10,7 @@ namespace s2e::dynamics::attitude { Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* local_celestial_information, const double step_width_s, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id) { - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); const char* section_ = "ATTITUDE"; std::string mc_name = "attitude" + std::to_string(spacecraft_id); Attitude* attitude; @@ -23,7 +23,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel s2e::math::Vector<3> torque_b; if (initialize_mode == "CONTROLLED") { // Initialize with Controlled attitude (attitude_tmp temporary used) - IniAccess ini_file_ca(file_name); + setting_file_reader::IniAccess ini_file_ca(file_name); const char* section_ca_ = "CONTROLLED_ATTITUDE"; const std::string main_mode_in = ini_file.ReadString(section_ca_, "main_mode"); const std::string sub_mode_in = ini_file.ReadString(section_ca_, "sub_mode"); @@ -53,7 +53,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel attitude = new AttitudeRk4(omega_b, quaternion_i2b, inertia_tensor_kgm2, torque_b, step_width_s, mc_name); } else if (propagate_mode == "CANTILEVER_VIBRATION") { std::string ini_structure_name = ini_file.ReadString("SETTING_FILES", "structure_file"); - IniAccess ini_structure(ini_structure_name); + setting_file_reader::IniAccess ini_structure(ini_structure_name); const char* section_cantilever = "CANTILEVER_PARAMETERS"; s2e::math::Matrix<3, 3> inertia_tensor_cantilever_kgm2; @@ -72,7 +72,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel intrinsic_angular_velocity_cantilever_rad_s, torque_b, step_width_s, mc_name); } else if (propagate_mode == "CONTROLLED") { // Controlled attitude - IniAccess ini_file_ca(file_name); + setting_file_reader::IniAccess ini_file_ca(file_name); const char* section_ca_ = "CONTROLLED_ATTITUDE"; const std::string main_mode_in = ini_file.ReadString(section_ca_, "main_mode"); const std::string sub_mode_in = ini_file.ReadString(section_ca_, "sub_mode"); diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index 5b8329708..8aad74327 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -16,7 +16,7 @@ namespace s2e::dynamics::orbit { Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, double current_time_jd, double gravity_constant_m3_s2, std::string section, RelativeInformation* relative_information) { - auto conf = IniAccess(initialize_file); + auto conf = setting_file_reader::IniAccess(initialize_file); const char* section_ = section.c_str(); Orbit* orbit; @@ -118,7 +118,7 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string } s2e::math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section) { - auto conf = IniAccess(initialize_file); + auto conf = setting_file_reader::IniAccess(initialize_file); const char* section_ = section.c_str(); s2e::math::Vector<3> position_i_m; s2e::math::Vector<3> velocity_i_m_s; diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index c2b94493d..f838b7c76 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -69,11 +69,11 @@ OrbitInitializeMode SetOrbitInitializeMode(const std::string initialize_mode) { std::string Orbit::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteVector("spacecraft_position", "i", "m", 3); - str_tmp += WriteVector("spacecraft_position", "ecef", "m", 3); - str_tmp += WriteVector("spacecraft_velocity", "i", "m/s", 3); - str_tmp += WriteVector("spacecraft_velocity", "b", "m/s", 3); - str_tmp += WriteVector("spacecraft_acceleration", "i", "m/s2", 3); + str_tmp += logger::WriteVector("spacecraft_position", "i", "m", 3); + str_tmp += logger::WriteVector("spacecraft_position", "ecef", "m", 3); + str_tmp += logger::WriteVector("spacecraft_velocity", "i", "m/s", 3); + str_tmp += logger::WriteVector("spacecraft_velocity", "b", "m/s", 3); + str_tmp += logger::WriteVector("spacecraft_acceleration", "i", "m/s2", 3); str_tmp += WriteScalar("spacecraft_latitude", "rad"); str_tmp += WriteScalar("spacecraft_longitude", "rad"); str_tmp += WriteScalar("spacecraft_altitude", "m"); @@ -84,11 +84,11 @@ std::string Orbit::GetLogHeader() const { std::string Orbit::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(spacecraft_position_i_m_, 16); - str_tmp += WriteVector(spacecraft_position_ecef_m_, 16); - str_tmp += WriteVector(spacecraft_velocity_i_m_s_, 10); - str_tmp += WriteVector(spacecraft_velocity_b_m_s_, 10); - str_tmp += WriteVector(spacecraft_acceleration_i_m_s2_, 10); + str_tmp += logger::WriteVector(spacecraft_position_i_m_, 16); + str_tmp += logger::WriteVector(spacecraft_position_ecef_m_, 16); + str_tmp += logger::WriteVector(spacecraft_velocity_i_m_s_, 10); + str_tmp += logger::WriteVector(spacecraft_velocity_b_m_s_, 10); + str_tmp += logger::WriteVector(spacecraft_acceleration_i_m_s2_, 10); str_tmp += WriteScalar(spacecraft_geodetic_position_.GetLatitude_rad()); str_tmp += WriteScalar(spacecraft_geodetic_position_.GetLongitude_rad()); str_tmp += WriteScalar(spacecraft_geodetic_position_.GetAltitude_m()); diff --git a/src/dynamics/thermal/temperature.cpp b/src/dynamics/thermal/temperature.cpp index aec2acc7a..1843b28cc 100644 --- a/src/dynamics/thermal/temperature.cpp +++ b/src/dynamics/thermal/temperature.cpp @@ -293,7 +293,7 @@ using std::vector; Temperature* InitTemperature(const std::string file_name, const double rk_prop_step_s, const SolarRadiationPressureEnvironment* srp_environment, const EarthAlbedo* earth_albedo) { - auto mainIni = IniAccess(file_name); + auto mainIni = setting_file_reader::IniAccess(file_name); vector node_list; vector heater_list; @@ -331,7 +331,7 @@ Temperature* InitTemperature(const std::string file_name, const double rk_prop_s // Read Heatloads from CSV File string filepath_heatload = file_path + "heatload.csv"; - IniAccess conf_heatload(filepath_heatload); + setting_file_reader::IniAccess conf_heatload(filepath_heatload); conf_heatload.ReadCsvString(heatload_str_list, 100); /*since we don't know the number of node_list yet, set node_num=100 temporary. Recall that Nodes_num are given to this function only to reserve memory*/ @@ -344,7 +344,7 @@ Temperature* InitTemperature(const std::string file_name, const double rk_prop_s // Read Node Properties from CSV File string filepath_node = file_path + "node.csv"; - IniAccess conf_node(filepath_node); + setting_file_reader::IniAccess conf_node(filepath_node); conf_node.ReadCsvString(node_str_list, 100); /*since we don't know the number of node_list yet, set node_num=100 temporary. Recall that Nodes_num are given to this function only to reserve memory*/ @@ -359,7 +359,7 @@ Temperature* InitTemperature(const std::string file_name, const double rk_prop_s // Read Heater Properties from CSV File string filepath_heater = file_path + "heaters.csv"; - IniAccess conf_heater(filepath_heater); + setting_file_reader::IniAccess conf_heater(filepath_heater); conf_heater.ReadCsvString(heater_str_list, 100); /*since we don't know the number of heater_list yet, set heater_num=100 temporary. Recall that heater_num are given to this function only to reserve memory*/ @@ -375,8 +375,8 @@ Temperature* InitTemperature(const std::string file_name, const double rk_prop_s // Read Cij,Rij data from CSV File string filepath_cij = file_path + "cij.csv"; string filepath_rij = file_path + "rij.csv"; - IniAccess conf_cij(filepath_cij); - IniAccess conf_rij(filepath_rij); + setting_file_reader::IniAccess conf_cij(filepath_cij); + setting_file_reader::IniAccess conf_rij(filepath_rij); conf_cij.ReadCsvDoubleWithHeader(conductance_matrix, node_num, 1, 1); conf_rij.ReadCsvDoubleWithHeader(radiation_matrix, node_num, 1, 1); diff --git a/src/environment/global/celestial_information.cpp b/src/environment/global/celestial_information.cpp index 4eb09cf05..bcf9bf365 100644 --- a/src/environment/global/celestial_information.cpp +++ b/src/environment/global/celestial_information.cpp @@ -165,8 +165,8 @@ std::string CelestialInformation::GetLogHeader() const { std::string body_pos = name + "_position"; std::string body_vel = name + "_velocity"; - str_tmp += WriteVector(body_pos, "i", "m", 3); - str_tmp += WriteVector(body_vel, "i", "m/s", 3); + str_tmp += logger::WriteVector(body_pos, "i", "m", 3); + str_tmp += logger::WriteVector(body_vel, "i", "m/s", 3); } return str_tmp; } @@ -201,7 +201,7 @@ void CelestialInformation::GetPlanetOrbit(const char* planet_name, const double } CelestialInformation* InitCelestialInformation(std::string file_name) { - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); const char* section = "CELESTIAL_INFORMATION"; const char* furnsh_section = "CSPICE_KERNELS"; diff --git a/src/environment/global/global_environment.cpp b/src/environment/global/global_environment.cpp index b97b78da7..334c1d4ae 100644 --- a/src/environment/global/global_environment.cpp +++ b/src/environment/global/global_environment.cpp @@ -20,7 +20,7 @@ GlobalEnvironment::~GlobalEnvironment() { void GlobalEnvironment::Initialize(const SimulationConfiguration* simulation_configuration) { // Get ini file path - IniAccess iniAccess = IniAccess(simulation_configuration->initialize_base_file_name_); + setting_file_reader::IniAccess iniAccess = setting_file_reader::IniAccess(simulation_configuration->initialize_base_file_name_); std::string simulation_time_ini_path = simulation_configuration->initialize_base_file_name_; // Initialize diff --git a/src/environment/global/gnss_satellites.cpp b/src/environment/global/gnss_satellites.cpp index d16fefc26..ee801c9b6 100644 --- a/src/environment/global/gnss_satellites.cpp +++ b/src/environment/global/gnss_satellites.cpp @@ -158,7 +158,7 @@ std::string GnssSatellites::GetLogHeader() const { // TODO: Add log output for other navigation systems for (size_t gps_index = 0; gps_index < kNumberOfGpsSatellite; gps_index++) { - str_tmp += WriteVector("GPS" + std::to_string(gps_index) + "_position", "ecef", "m", 3); + str_tmp += logger::WriteVector("GPS" + std::to_string(gps_index) + "_position", "ecef", "m", 3); str_tmp += WriteScalar("GPS" + std::to_string(gps_index) + "_clock_offset", "s"); } @@ -169,7 +169,7 @@ std::string GnssSatellites::GetLogValue() const { std::string str_tmp = ""; for (size_t gps_index = 0; gps_index < kNumberOfGpsSatellite; gps_index++) { - str_tmp += WriteVector(GetPosition_ecef_m(gps_index), 16); + str_tmp += logger::WriteVector(GetPosition_ecef_m(gps_index), 16); str_tmp += WriteScalar(GetClock_s(gps_index)); } @@ -177,7 +177,7 @@ std::string GnssSatellites::GetLogValue() const { } GnssSatellites* InitGnssSatellites(const std::string file_name, const EarthRotation& earth_rotation, const SimulationTime& simulation_time) { - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); char section[] = "GNSS_SATELLITES"; const bool is_calc_enable = ini_file.ReadEnable(section, INI_CALC_LABEL); diff --git a/src/environment/global/hipparcos_catalogue.cpp b/src/environment/global/hipparcos_catalogue.cpp index 72ad7dfdf..a4fb9d5f4 100644 --- a/src/environment/global/hipparcos_catalogue.cpp +++ b/src/environment/global/hipparcos_catalogue.cpp @@ -87,7 +87,7 @@ std::string HipparcosCatalogue::GetLogValue() const { } HipparcosCatalogue* InitHipparcosCatalogue(std::string file_name) { - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); const char* section = "HIPPARCOS_CATALOGUE"; std::string catalogue_path = ini_file.ReadString(section, "catalogue_file_path"); diff --git a/src/environment/global/simulation_time.cpp b/src/environment/global/simulation_time.cpp index e44d5a416..d1c74d495 100644 --- a/src/environment/global/simulation_time.cpp +++ b/src/environment/global/simulation_time.cpp @@ -247,7 +247,7 @@ void SimulationTime::ConvJDtoCalendarDay(const double JD) { } SimulationTime* InitSimulationTime(std::string file_name) { - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); const char* section = "TIME"; // Parameters about entire simulation diff --git a/src/environment/local/atmosphere.cpp b/src/environment/local/atmosphere.cpp index cfada183c..0b0c9cccc 100644 --- a/src/environment/local/atmosphere.cpp +++ b/src/environment/local/atmosphere.cpp @@ -103,7 +103,7 @@ std::string Atmosphere::GetLogHeader() const { Atmosphere InitAtmosphere(const std::string initialize_file_path, const LocalCelestialInformation* local_celestial_information, const SimulationTime* simulation_time) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "ATMOSPHERE"; double f107_threshold = 50.0; double f107_default = 150.0; diff --git a/src/environment/local/earth_albedo.cpp b/src/environment/local/earth_albedo.cpp index fb3672a8e..46831b8e5 100644 --- a/src/environment/local/earth_albedo.cpp +++ b/src/environment/local/earth_albedo.cpp @@ -50,7 +50,7 @@ void EarthAlbedo::CalcEarthAlbedo(const LocalCelestialInformation* local_celesti EarthAlbedo InitEarthAlbedo(std::string initialize_file_path, LocalCelestialInformation* local_celestial_information, SolarRadiationPressureEnvironment* srp_environment) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "EARTH_ALBEDO"; EarthAlbedo earth_albedo(local_celestial_information, srp_environment); diff --git a/src/environment/local/geomagnetic_field.cpp b/src/environment/local/geomagnetic_field.cpp index 008f3277c..0b5eab1b9 100644 --- a/src/environment/local/geomagnetic_field.cpp +++ b/src/environment/local/geomagnetic_field.cpp @@ -57,8 +57,8 @@ void GeomagneticField::AddNoise(double* magnetic_field_array_i_nT) { std::string GeomagneticField::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteVector("geomagnetic_field_at_spacecraft_position", "i", "nT", 3); - str_tmp += WriteVector("geomagnetic_field_at_spacecraft_position", "b", "nT", 3); + str_tmp += logger::WriteVector("geomagnetic_field_at_spacecraft_position", "i", "nT", 3); + str_tmp += logger::WriteVector("geomagnetic_field_at_spacecraft_position", "b", "nT", 3); return str_tmp; } @@ -66,14 +66,14 @@ std::string GeomagneticField::GetLogHeader() const { std::string GeomagneticField::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(magnetic_field_i_nT_); - str_tmp += WriteVector(magnetic_field_b_nT_); + str_tmp += logger::WriteVector(magnetic_field_i_nT_); + str_tmp += logger::WriteVector(magnetic_field_b_nT_); return str_tmp; } GeomagneticField InitGeomagneticField(std::string initialize_file_path) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "MAGNETIC_FIELD_ENVIRONMENT"; std::string fname = conf.ReadString(section, "coefficient_file"); diff --git a/src/environment/local/local_celestial_information.cpp b/src/environment/local/local_celestial_information.cpp index fde483b84..8699e21f2 100644 --- a/src/environment/local/local_celestial_information.cpp +++ b/src/environment/local/local_celestial_information.cpp @@ -181,8 +181,8 @@ std::string LocalCelestialInformation::GetLogHeader() const { std::string body_pos = name + "_position_from_spacecraft"; std::string body_vel = name + "_velocity_from_spacecraft"; - str_tmp += WriteVector(body_pos, "b", "m", 3); - str_tmp += WriteVector(body_vel, "b", "m/s", 3); + str_tmp += logger::WriteVector(body_pos, "b", "m", 3); + str_tmp += logger::WriteVector(body_vel, "b", "m/s", 3); } return str_tmp; } diff --git a/src/environment/local/local_environment.cpp b/src/environment/local/local_environment.cpp index d12f5e882..b8738b87a 100644 --- a/src/environment/local/local_environment.cpp +++ b/src/environment/local/local_environment.cpp @@ -26,7 +26,7 @@ LocalEnvironment::~LocalEnvironment() { void LocalEnvironment::Initialize(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id) { // Read file name - IniAccess iniAccess = IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); + setting_file_reader::IniAccess iniAccess = setting_file_reader::IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); std::string ini_fname = iniAccess.ReadString("SETTING_FILES", "local_environment_file"); // Save ini file @@ -47,7 +47,7 @@ void LocalEnvironment::Initialize(const SimulationConfiguration* simulation_conf } // Log setting for Local celestial information - IniAccess conf = IniAccess(ini_fname); + setting_file_reader::IniAccess conf = setting_file_reader::IniAccess(ini_fname); celestial_information_->is_log_enabled_ = conf.ReadEnable("LOCAL_CELESTIAL_INFORMATION", "logging"); } diff --git a/src/environment/local/solar_radiation_pressure_environment.cpp b/src/environment/local/solar_radiation_pressure_environment.cpp index 1ab74ebdc..ff60f94be 100644 --- a/src/environment/local/solar_radiation_pressure_environment.cpp +++ b/src/environment/local/solar_radiation_pressure_environment.cpp @@ -106,7 +106,7 @@ void SolarRadiationPressureEnvironment::CalcShadowCoefficient(std::string shadow SolarRadiationPressureEnvironment InitSolarRadiationPressureEnvironment(std::string initialize_file_path, LocalCelestialInformation* local_celestial_information) { - auto conf = IniAccess(initialize_file_path); + auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "SOLAR_RADIATION_PRESSURE_ENVIRONMENT"; SolarRadiationPressureEnvironment srp_env(local_celestial_information); diff --git a/src/logger/initialize_log.cpp b/src/logger/initialize_log.cpp index 3dec812af..30c1036bb 100644 --- a/src/logger/initialize_log.cpp +++ b/src/logger/initialize_log.cpp @@ -10,7 +10,7 @@ namespace s2e::logger { Logger* InitLog(std::string file_name) { - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); std::string log_file_path = ini_file.ReadString("SIMULATION_SETTINGS", "log_file_save_directory"); bool log_ini = ini_file.ReadEnable("SIMULATION_SETTINGS", "save_initialize_files"); @@ -21,7 +21,7 @@ Logger* InitLog(std::string file_name) { } Logger* InitMonteCarloLog(std::string file_name, bool enable) { - IniAccess ini_file(file_name); + setting_file_reader::IniAccess ini_file(file_name); std::string log_file_path = ini_file.ReadString("SIMULATION_SETTINGS", "log_file_save_directory"); bool log_ini = ini_file.ReadEnable("SIMULATION_SETTINGS", "save_initialize_files"); From 679e084041063d09eb794375044585f8e277005b Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 28 Sep 2024 20:46:53 +0900 Subject: [PATCH 37/49] Add logger namespace --- src/components/ideal/attitude_observer.cpp | 4 ++-- src/components/real/aocs/star_sensor.cpp | 4 ++-- src/dynamics/attitude/attitude.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/components/ideal/attitude_observer.cpp b/src/components/ideal/attitude_observer.cpp index 9d2199056..dad39b3e3 100644 --- a/src/components/ideal/attitude_observer.cpp +++ b/src/components/ideal/attitude_observer.cpp @@ -36,7 +36,7 @@ std::string AttitudeObserver::GetLogHeader() const { std::string str_tmp = ""; std::string head = "attitude_observer_"; - str_tmp += WriteQuaternion(head + "quaternion", "i2b"); + str_tmp += logger::WriteQuaternion(head + "quaternion", "i2b"); return str_tmp; } @@ -44,7 +44,7 @@ std::string AttitudeObserver::GetLogHeader() const { std::string AttitudeObserver::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteQuaternion(observed_quaternion_i2b_); + str_tmp += logger::WriteQuaternion(observed_quaternion_i2b_); return str_tmp; } diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index 324288777..e84f8edc5 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -185,7 +185,7 @@ std::string StarSensor::GetLogHeader() const { const std::string sensor_id = std::to_string(static_cast(component_id_)); std::string sensor_name = "stt" + sensor_id + "_"; - str_tmp += WriteQuaternion(sensor_name + "measured_quaternion", "i2c"); + str_tmp += logger::WriteQuaternion(sensor_name + "measured_quaternion", "i2c"); str_tmp += WriteScalar(sensor_name + "error_flag"); return str_tmp; @@ -194,7 +194,7 @@ std::string StarSensor::GetLogHeader() const { std::string StarSensor::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteQuaternion(measured_quaternion_i2c_); + str_tmp += logger::WriteQuaternion(measured_quaternion_i2c_); str_tmp += WriteScalar(double(error_flag_)); return str_tmp; diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index 2ad9c0f20..55e172240 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -25,7 +25,7 @@ std::string Attitude::GetLogHeader() const { std::string str_tmp = ""; str_tmp += logger::WriteVector("spacecraft_angular_velocity", "b", "rad/s", 3); - str_tmp += WriteQuaternion("spacecraft_quaternion", "i2b"); + str_tmp += logger::WriteQuaternion("spacecraft_quaternion", "i2b"); str_tmp += logger::WriteVector("spacecraft_torque", "b", "Nm", 3); str_tmp += WriteScalar("spacecraft_total_angular_momentum", "Nms"); str_tmp += WriteScalar("spacecraft_kinematic_energy", "J"); @@ -37,7 +37,7 @@ std::string Attitude::GetLogValue() const { std::string str_tmp = ""; str_tmp += logger::WriteVector(angular_velocity_b_rad_s_); - str_tmp += WriteQuaternion(quaternion_i2b_); + str_tmp += logger::WriteQuaternion(quaternion_i2b_); str_tmp += logger::WriteVector(torque_b_Nm_); str_tmp += WriteScalar(angular_momentum_total_Nms_); str_tmp += WriteScalar(kinetic_energy_J_); From ebd0f75ee9b84a1421b8aafb62587534fbf23d76 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 28 Sep 2024 21:20:38 +0900 Subject: [PATCH 38/49] Add dynamics namespace --- src/components/ideal/force_generator.cpp | 4 ++-- src/components/ideal/force_generator.hpp | 4 ++-- src/components/ideal/torque_generator.cpp | 4 ++-- src/components/ideal/torque_generator.hpp | 6 +++--- src/components/real/aocs/gnss_receiver.cpp | 8 ++++---- src/components/real/aocs/gnss_receiver.hpp | 10 +++++----- src/components/real/aocs/gyro_sensor.cpp | 8 ++++---- src/components/real/aocs/gyro_sensor.hpp | 12 ++++++------ src/components/real/aocs/star_sensor.cpp | 8 ++++---- src/components/real/aocs/star_sensor.hpp | 10 +++++----- .../real/communication/ground_station_calculator.cpp | 6 +++--- .../real/communication/ground_station_calculator.hpp | 6 +++--- src/components/real/propulsion/simple_thruster.cpp | 8 ++++---- src/components/real/propulsion/simple_thruster.hpp | 10 +++++----- src/disturbances/air_drag.cpp | 2 +- src/disturbances/air_drag.hpp | 4 ++-- src/disturbances/disturbance.hpp | 4 ++-- src/disturbances/disturbances.cpp | 2 +- src/disturbances/disturbances.hpp | 4 ++-- src/disturbances/geopotential.cpp | 2 +- src/disturbances/geopotential.hpp | 4 ++-- src/disturbances/gravity_gradient.cpp | 2 +- src/disturbances/gravity_gradient.hpp | 4 ++-- src/disturbances/lunar_gravity_field.cpp | 2 +- src/disturbances/lunar_gravity_field.hpp | 4 ++-- src/disturbances/magnetic_disturbance.cpp | 2 +- src/disturbances/magnetic_disturbance.hpp | 4 ++-- .../solar_radiation_pressure_disturbance.cpp | 2 +- .../solar_radiation_pressure_disturbance.hpp | 4 ++-- src/disturbances/third_body_gravity.cpp | 2 +- src/disturbances/third_body_gravity.hpp | 4 ++-- src/dynamics/dynamics.cpp | 12 ++++++------ src/environment/local/local_environment.cpp | 2 +- src/environment/local/local_environment.hpp | 4 ++-- .../multiple_spacecraft/relative_information.cpp | 2 +- .../multiple_spacecraft/relative_information.hpp | 6 +++--- src/simulation/spacecraft/spacecraft.cpp | 2 +- src/simulation/spacecraft/spacecraft.hpp | 4 ++-- .../spacecraft/sample_components.cpp | 2 +- .../spacecraft/sample_components.hpp | 4 ++-- 40 files changed, 97 insertions(+), 97 deletions(-) diff --git a/src/components/ideal/force_generator.cpp b/src/components/ideal/force_generator.cpp index 86b17da21..d1770ac79 100644 --- a/src/components/ideal/force_generator.cpp +++ b/src/components/ideal/force_generator.cpp @@ -12,7 +12,7 @@ namespace s2e::components { // Constructor ForceGenerator::ForceGenerator(const int prescaler, environment::ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_N, - const double direction_error_standard_deviation_rad, const Dynamics* dynamics) + const double direction_error_standard_deviation_rad, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator), magnitude_noise_(0.0, magnitude_error_standard_deviation_N), direction_error_standard_deviation_rad_(direction_error_standard_deviation_rad), @@ -107,7 +107,7 @@ s2e::math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(s2e::math return error_quaternion; } -ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics) { +ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::Dynamics* dynamics) { // General setting_file_reader::IniAccess ini_file(file_name); diff --git a/src/components/ideal/force_generator.hpp b/src/components/ideal/force_generator.hpp index 03c4bd09b..0881e7773 100644 --- a/src/components/ideal/force_generator.hpp +++ b/src/components/ideal/force_generator.hpp @@ -114,7 +114,7 @@ class ForceGenerator : public Component, public logger::ILoggable { */ s2e::math::Quaternion GenerateDirectionNoiseQuaternion(s2e::math::Vector<3> true_direction, const double error_standard_deviation_rad); - const Dynamics* dynamics_; //!< Spacecraft dynamics information + const dynamics::Dynamics* dynamics_; //!< Spacecraft dynamics information }; /** @@ -124,7 +124,7 @@ class ForceGenerator : public Component, public logger::ILoggable { * @param [in] file_name: Path to initialize file * @param [in] dynamics: Dynamics information */ -ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics); +ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::Dynamics* dynamics); } // namespace s2e::components diff --git a/src/components/ideal/torque_generator.cpp b/src/components/ideal/torque_generator.cpp index 74a478a29..93b312270 100644 --- a/src/components/ideal/torque_generator.cpp +++ b/src/components/ideal/torque_generator.cpp @@ -12,7 +12,7 @@ namespace s2e::components { // Constructor TorqueGenerator::TorqueGenerator(const int prescaler, environment::ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_Nm, - const double direction_error_standard_deviation_rad, const Dynamics* dynamics) + const double direction_error_standard_deviation_rad, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator), magnitude_noise_(0.0, magnitude_error_standard_deviation_Nm), direction_error_standard_deviation_rad_(direction_error_standard_deviation_rad), @@ -80,7 +80,7 @@ s2e::math::Quaternion TorqueGenerator::GenerateDirectionNoiseQuaternion(s2e::mat return error_quaternion; } -TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics) { +TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::Dynamics* dynamics) { // General setting_file_reader::IniAccess ini_file(file_name); diff --git a/src/components/ideal/torque_generator.hpp b/src/components/ideal/torque_generator.hpp index 357f7b22c..7b5144729 100644 --- a/src/components/ideal/torque_generator.hpp +++ b/src/components/ideal/torque_generator.hpp @@ -30,7 +30,7 @@ class TorqueGenerator : public Component, public logger::ILoggable { * @param [in] dynamics: Dynamics information */ TorqueGenerator(const int prescaler, environment::ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_Nm, - const double direction_error_standard_deviation_rad, const Dynamics* dynamics); + const double direction_error_standard_deviation_rad, const dynamics::Dynamics* dynamics); /** * @fn ~TorqueGenerator * @brief Destructor @@ -92,7 +92,7 @@ class TorqueGenerator : public Component, public logger::ILoggable { */ s2e::math::Quaternion GenerateDirectionNoiseQuaternion(s2e::math::Vector<3> true_direction, const double error_standard_deviation_rad); - const Dynamics* dynamics_; //!< Spacecraft dynamics information + const dynamics::Dynamics* dynamics_; //!< Spacecraft dynamics information }; /** @@ -102,7 +102,7 @@ class TorqueGenerator : public Component, public logger::ILoggable { * @param [in] file_name: Path to initialize file * @param [in] dynamics: Dynamics information */ -TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const Dynamics* dynamics); +TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::Dynamics* dynamics); } // namespace s2e::components diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index f68873453..00ba3fd58 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -16,7 +16,7 @@ namespace s2e::components { GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, - const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, + const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator), component_id_(component_id), @@ -36,7 +36,7 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, - const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, + const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator, power_port), component_id_(component_id), @@ -291,7 +291,7 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat return gnss_receiver_param; } -GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const Dynamics* dynamics, +GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); @@ -302,7 +302,7 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, cons } GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, - const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { + const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); // PowerPort diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 3714cc772..e96f05f0f 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -63,7 +63,7 @@ class GnssReceiver : public Component, public logger::ILoggable { GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, - const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); + const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn GnssReceiver * @brief Constructor with power port @@ -83,7 +83,7 @@ class GnssReceiver : public Component, public logger::ILoggable { GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, - const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, + const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); // Override functions for Component @@ -156,7 +156,7 @@ class GnssReceiver : public Component, public logger::ILoggable { std::vector gnss_information_list_; //!< Information List of visible GNSS satellites // References - const Dynamics* dynamics_; //!< Dynamics of spacecraft + const dynamics::Dynamics* dynamics_; //!< Dynamics of spacecraft const GnssSatellites* gnss_satellites_; //!< Information of GNSS satellites const SimulationTime* simulation_time_; //!< Simulation time @@ -226,7 +226,7 @@ AntennaModel SetAntennaModel(const std::string antenna_model); * @param [in] gnss_satellites: GNSS satellites information * @param [in] simulation_time: Simulation time information */ -GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const Dynamics* dynamics, +GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn InitGnssReceiver @@ -240,7 +240,7 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, cons * @param [in] simulation_time: Simulation time information */ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, - const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); + const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); } // namespace s2e::components diff --git a/src/components/real/aocs/gyro_sensor.cpp b/src/components/real/aocs/gyro_sensor.cpp index fe16a6465..4776b6701 100644 --- a/src/components/real/aocs/gyro_sensor.cpp +++ b/src/components/real/aocs/gyro_sensor.cpp @@ -10,11 +10,11 @@ namespace s2e::components { GyroSensor::GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics) + const s2e::math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), dynamics_(dynamics) {} GyroSensor::GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics) + const s2e::math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator, power_port), Sensor(sensor_base), sensor_id_(sensor_id), @@ -48,7 +48,7 @@ std::string GyroSensor::GetLogValue() const { } GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, - const Dynamics* dynamics) { + const dynamics::Dynamics* dynamics) { setting_file_reader::IniAccess gyro_conf(file_name); const char* sensor_name = "GYRO_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); @@ -69,7 +69,7 @@ GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, int sens } GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, - double component_step_time_s, const Dynamics* dynamics) { + double component_step_time_s, const dynamics::Dynamics* dynamics) { setting_file_reader::IniAccess gyro_conf(file_name); const char* sensor_name = "GYRO_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); diff --git a/src/components/real/aocs/gyro_sensor.hpp b/src/components/real/aocs/gyro_sensor.hpp index c99759722..f4baaad1e 100644 --- a/src/components/real/aocs/gyro_sensor.hpp +++ b/src/components/real/aocs/gyro_sensor.hpp @@ -34,7 +34,7 @@ class GyroSensor : public Component, public Sensor, public logge * @param [in] dynamics: Dynamics information */ GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics); + const s2e::math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics); /** * @fn GyroSensor * @brief Constructor with power port @@ -47,7 +47,7 @@ class GyroSensor : public Component, public Sensor, public logge * @param [in] dynamics: Dynamics information */ GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const Dynamics* dynamics); + const s2e::math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics); /** * @fn ~GyroSensor * @brief Destructor @@ -85,7 +85,7 @@ class GyroSensor : public Component, public Sensor, public logge unsigned int sensor_id_ = 0; //!< Sensor ID s2e::math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame - const Dynamics* dynamics_; //!< Dynamics information + const dynamics::Dynamics* dynamics_; //!< Dynamics information }; /** @@ -98,7 +98,7 @@ class GyroSensor : public Component, public Sensor, public logge * @param [in] dynamics: Dynamics information */ GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, - const Dynamics* dynamics); + const dynamics::Dynamics* dynamics); /** * @fn InitGyroSensor * @brief Initialize functions for gyro sensor with power port @@ -107,10 +107,10 @@ GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, int sens * @param [in] sensor_id: Sensor ID * @param [in] component_step_time_s: Component step time [sec] * @param [in] file_name: Path to the initialize file - * @param [in] dynamics: Dynamics information + * @param [in] dynamics: dynamics::Dynamics information */ GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, - double component_step_time_s, const Dynamics* dynamics); + double component_step_time_s, const dynamics::Dynamics* dynamics); } //namespace s2e::components diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index e84f8edc5..c17302608 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -22,7 +22,7 @@ StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_g const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, - const double capture_rate_limit_rad_s, const Dynamics* dynamics, const LocalEnvironment* local_environment) + const double capture_rate_limit_rad_s, const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment) : Component(prescaler, clock_generator), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -46,7 +46,7 @@ StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_g const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, - const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const Dynamics* dynamics, + const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment) : Component(prescaler, clock_generator, power_port), component_id_(component_id), @@ -216,7 +216,7 @@ void StarSensor::MainRoutine(const int time_count) { } StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, double component_step_time_s, - const Dynamics* dynamics, const LocalEnvironment* local_environment) { + const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment) { setting_file_reader::IniAccess STT_conf(file_name); const char* sensor_name = "STAR_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); @@ -249,7 +249,7 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sens } StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const string file_name, double component_step_time_s, - const Dynamics* dynamics, const LocalEnvironment* local_environment) { + const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment) { setting_file_reader::IniAccess STT_conf(file_name); const char* sensor_name = "STAR_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); diff --git a/src/components/real/aocs/star_sensor.hpp b/src/components/real/aocs/star_sensor.hpp index 5e3ab99b2..772e3bd53 100644 --- a/src/components/real/aocs/star_sensor.hpp +++ b/src/components/real/aocs/star_sensor.hpp @@ -49,7 +49,7 @@ class StarSensor : public Component, public logger::ILoggable { const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, - const Dynamics* dynamics, const LocalEnvironment* local_environment); + const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment); /** * @fn StarSensor * @brief Constructor with power port @@ -74,7 +74,7 @@ class StarSensor : public Component, public logger::ILoggable { const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, - const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const Dynamics* dynamics, + const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment); // Override functions for Component @@ -138,7 +138,7 @@ class StarSensor : public Component, public logger::ILoggable { double capture_rate_limit_rad_s_; //!< Angular rate limit to get correct attitude [rad/s] // Observed variables - const Dynamics* dynamics_; //!< Dynamics information + const dynamics::Dynamics* dynamics_; //!< Dynamics information const LocalEnvironment* local_environment_; //!< Local environment information // Internal functions @@ -219,7 +219,7 @@ class StarSensor : public Component, public logger::ILoggable { * @param [in] local_environment: Local environment information */ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, - const Dynamics* dynamics, const LocalEnvironment* local_environment); + const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment); /** * @fn InitStarSensor * @brief Initialize functions for StarSensor with power port @@ -232,7 +232,7 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sens * @param [in] local_environment: Local environment information */ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, - double component_step_time_s, const Dynamics* dynamics, const LocalEnvironment* local_environment); + double component_step_time_s, const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment); } // namespace s2e::components diff --git a/src/components/real/communication/ground_station_calculator.cpp b/src/components/real/communication/ground_station_calculator.cpp index 554919b92..a5deff17d 100644 --- a/src/components/real/communication/ground_station_calculator.cpp +++ b/src/components/real/communication/ground_station_calculator.cpp @@ -42,7 +42,7 @@ void GroundStationCalculator::Update(const Spacecraft& spacecraft, const Antenna } // Private functions -double GroundStationCalculator::CalcMaxBitrate(const Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, +double GroundStationCalculator::CalcMaxBitrate(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { double cn0_dBHz = CalcCn0OnGs(dynamics, spacecraft_tx_antenna, ground_station, ground_station_rx_antenna); @@ -55,14 +55,14 @@ double GroundStationCalculator::CalcMaxBitrate(const Dynamics& dynamics, const A } } -double GroundStationCalculator::CalcReceiveMarginOnGs(const Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, +double GroundStationCalculator::CalcReceiveMarginOnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { double cn0_dB = CalcCn0OnGs(dynamics, spacecraft_tx_antenna, ground_station, ground_station_rx_antenna); double cn0_requirement_dB = ebn0_dB_ + hardware_deterioration_dB_ + coding_gain_dB_ + 10.0 * log10(spacecraft_tx_antenna.GetBitrate_bps()); return cn0_dB - cn0_requirement_dB; } -double GroundStationCalculator::CalcCn0OnGs(const Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, +double GroundStationCalculator::CalcCn0OnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { if (!spacecraft_tx_antenna.IsTransmitter() || !ground_station_rx_antenna.IsReceiver()) { // Check compatibility of transmitter and receiver diff --git a/src/components/real/communication/ground_station_calculator.hpp b/src/components/real/communication/ground_station_calculator.hpp index c78cc41a4..9d7551b4f 100644 --- a/src/components/real/communication/ground_station_calculator.hpp +++ b/src/components/real/communication/ground_station_calculator.hpp @@ -111,7 +111,7 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station_rx_antenna: Rx Antenna mounted on ground station * @return Max bitrate [Mbps] */ - double CalcMaxBitrate(const Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, + double CalcMaxBitrate(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, const Antenna& ground_station_rx_antenna); /** * @fn CalcReceiveMarginOnGs @@ -122,7 +122,7 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station_rx_antenna: Rx Antenna mounted on ground station * @return Receive margin [dB] */ - double CalcReceiveMarginOnGs(const Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, + double CalcReceiveMarginOnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, const Antenna& ground_station_rx_antenna); /** @@ -134,7 +134,7 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station_rx_antenna: Rx Antenna mounted on ground station * @return CN0 [dB] */ - double CalcCn0OnGs(const Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, + double CalcCn0OnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, const Antenna& ground_station_rx_antenna); }; diff --git a/src/components/real/propulsion/simple_thruster.cpp b/src/components/real/propulsion/simple_thruster.cpp index 735ec87f0..9b5b7c4b7 100644 --- a/src/components/real/propulsion/simple_thruster.cpp +++ b/src/components/real/propulsion/simple_thruster.cpp @@ -15,7 +15,7 @@ namespace s2e::components { SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Vector<3> thruster_position_b_m, const s2e::math::Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, - const Dynamics* dynamics) + const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator), component_id_(component_id), thruster_position_b_m_(thruster_position_b_m), @@ -30,7 +30,7 @@ SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const s2e::math::Vector<3> thruster_position_b_m, const s2e::math::Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, - const Dynamics* dynamics) + const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator, power_port), component_id_(component_id), thruster_position_b_m_(thruster_position_b_m), @@ -123,7 +123,7 @@ s2e::math::Vector<3> SimpleThruster::CalcThrustDirection() { } SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, const Structure* structure, - const Dynamics* dynamics) { + const dynamics::Dynamics* dynamics) { setting_file_reader::IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); auto* Section = section_str.c_str(); @@ -151,7 +151,7 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, } SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, - const Structure* structure, const Dynamics* dynamics) { + const Structure* structure, const dynamics::Dynamics* dynamics) { setting_file_reader::IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); auto* Section = section_str.c_str(); diff --git a/src/components/real/propulsion/simple_thruster.hpp b/src/components/real/propulsion/simple_thruster.hpp index ba1eb0c07..6cb9ab066 100644 --- a/src/components/real/propulsion/simple_thruster.hpp +++ b/src/components/real/propulsion/simple_thruster.hpp @@ -39,7 +39,7 @@ class SimpleThruster : public Component, public logger::ILoggable { */ SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const Vector<3> thruster_position_b_m, const Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, - const double direction_standard_deviation_rad, const Structure* structure, const Dynamics* dynamics); + const double direction_standard_deviation_rad, const Structure* structure, const dynamics::Dynamics* dynamics); /** * @fn SimpleThruster * @brief Constructor with power port @@ -58,7 +58,7 @@ class SimpleThruster : public Component, public logger::ILoggable { SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const Vector<3> thruster_position_b_m, const Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, - const Dynamics* dynamics); + const dynamics::Dynamics* dynamics); /** * @fn ~SimpleThruster * @brief Destructor @@ -154,7 +154,7 @@ class SimpleThruster : public Component, public logger::ILoggable { void Initialize(const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad); const Structure* structure_; //!< Spacecraft structure information - const Dynamics* dynamics_; //!< Spacecraft dynamics information + const dynamics::Dynamics* dynamics_; //!< Spacecraft dynamics information }; /** @@ -167,7 +167,7 @@ class SimpleThruster : public Component, public logger::ILoggable { * @param [in] dynamics: Spacecraft dynamics information */ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, const Structure* structure, - const Dynamics* dynamics); + const dynamics::Dynamics* dynamics); /** * @fn InitSimpleThruster * @brief Initialize function os SimpleThruster @@ -179,7 +179,7 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, * @param [in] dynamics: Spacecraft dynamics information */ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, - const Structure* structure, const Dynamics* dynamics); + const Structure* structure, const dynamics::Dynamics* dynamics); } // namespace s2e::components diff --git a/src/disturbances/air_drag.cpp b/src/disturbances/air_drag.cpp index a15817e36..163a80cbd 100644 --- a/src/disturbances/air_drag.cpp +++ b/src/disturbances/air_drag.cpp @@ -25,7 +25,7 @@ AirDrag::AirDrag(const std::vector& surfaces, const s2e::math::Vector<3 cn_.assign(num, 0.0); } -void AirDrag::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { +void AirDrag::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { double air_density_kg_m3 = local_environment.GetAtmosphere().GetAirDensity_kg_m3(); s2e::math::Matrix<3, 3> dcm_ecef2eci = diff --git a/src/disturbances/air_drag.hpp b/src/disturbances/air_drag.hpp index be27dfc02..7f87e9bb7 100644 --- a/src/disturbances/air_drag.hpp +++ b/src/disturbances/air_drag.hpp @@ -39,9 +39,9 @@ class AirDrag : public SurfaceForce { * @fn Update * @brief Override Updates function of SimpleDisturbance * @param [in] local_environment: Local environment information - * @param [in] dynamics: Dynamics information + * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics); + virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); // Override logger::ILoggable /** diff --git a/src/disturbances/disturbance.hpp b/src/disturbances/disturbance.hpp index 1c93d18a9..de7d476b8 100644 --- a/src/disturbances/disturbance.hpp +++ b/src/disturbances/disturbance.hpp @@ -41,7 +41,7 @@ class Disturbance : public logger::ILoggable { * @fn UpdateIfEnabled * @brief Update calculated disturbance when the calculation flag is true */ - virtual inline void UpdateIfEnabled(const LocalEnvironment& local_environment, const Dynamics& dynamics) { + virtual inline void UpdateIfEnabled(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { if (is_calculation_enabled_) { Update(local_environment, dynamics); } else { @@ -56,7 +56,7 @@ class Disturbance : public logger::ILoggable { * @fn Update * @brief Pure virtual function to define the disturbance calculation */ - virtual void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) = 0; + virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) = 0; /** * @fn GetTorque_b_Nm diff --git a/src/disturbances/disturbances.cpp b/src/disturbances/disturbances.cpp index e274cbe96..8d0c94037 100644 --- a/src/disturbances/disturbances.cpp +++ b/src/disturbances/disturbances.cpp @@ -30,7 +30,7 @@ Disturbances::~Disturbances() { } } -void Disturbances::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics, const SimulationTime* simulation_time) { +void Disturbances::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics, const SimulationTime* simulation_time) { InitializeForceAndTorque(); InitializeAcceleration(); diff --git a/src/disturbances/disturbances.hpp b/src/disturbances/disturbances.hpp index ab1d488fe..590dbc703 100644 --- a/src/disturbances/disturbances.hpp +++ b/src/disturbances/disturbances.hpp @@ -42,10 +42,10 @@ class Disturbances { * @fn Update * @brief Update all disturbance calculation * @param [in] local_environment: Local environment information - * @param [in] dynamics: Dynamics information + * @param [in] dynamics: dynamics::Dynamics information * @param [in] simulation_time: Simulation time */ - void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics, const SimulationTime* simulation_time); + void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics, const SimulationTime* simulation_time); /** * @fn LogSetup * @brief log setup for all disturbances diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index f28f6a591..40cbb3515 100644 --- a/src/disturbances/geopotential.cpp +++ b/src/disturbances/geopotential.cpp @@ -70,7 +70,7 @@ bool Geopotential::ReadCoefficientsEgm96(std::string file_name) { return true; } -void Geopotential::Update(const LocalEnvironment &local_environment, const Dynamics &dynamics) { +void Geopotential::Update(const LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics) { #ifdef DEBUG_GEOPOTENTIAL chrono::system_clock::time_point start, end; start = chrono::system_clock::now(); diff --git a/src/disturbances/geopotential.hpp b/src/disturbances/geopotential.hpp index fae80dedb..3bfcb83fa 100644 --- a/src/disturbances/geopotential.hpp +++ b/src/disturbances/geopotential.hpp @@ -46,9 +46,9 @@ class Geopotential : public Disturbance { * @fn Update * @brief Override Updates function of SimpleDisturbance * @param [in] local_environment: Local environment information - * @param [in] dynamics: Dynamics information + * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment &local_environment, const Dynamics &dynamics); + virtual void Update(const LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics); // Override logger::ILoggable /** diff --git a/src/disturbances/gravity_gradient.cpp b/src/disturbances/gravity_gradient.cpp index 8ac14cdfa..366c77778 100644 --- a/src/disturbances/gravity_gradient.cpp +++ b/src/disturbances/gravity_gradient.cpp @@ -19,7 +19,7 @@ GravityGradient::GravityGradient(const bool is_calculation_enabled) GravityGradient::GravityGradient(const double gravity_constant_m3_s2, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, true), gravity_constant_m3_s2_(gravity_constant_m3_s2) {} -void GravityGradient::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { +void GravityGradient::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { // TODO: use structure information to get inertia tensor CalcTorque_b_Nm(local_environment.GetCelestialInformation().GetCenterBodyPositionFromSpacecraft_b_m(), dynamics.GetAttitude().GetInertiaTensor_b_kgm2()); diff --git a/src/disturbances/gravity_gradient.hpp b/src/disturbances/gravity_gradient.hpp index 785b9e0fa..e4a578aff 100644 --- a/src/disturbances/gravity_gradient.hpp +++ b/src/disturbances/gravity_gradient.hpp @@ -41,9 +41,9 @@ class GravityGradient : public Disturbance { * @fn Update * @brief Override Updates function of SimpleDisturbance * @param [in] local_environment: Local environment information - * @param [in] dynamics: Dynamics information + * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics); + virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); // Override logger::ILoggable /** diff --git a/src/disturbances/lunar_gravity_field.cpp b/src/disturbances/lunar_gravity_field.cpp index 7f7e63191..d28ccfe76 100644 --- a/src/disturbances/lunar_gravity_field.cpp +++ b/src/disturbances/lunar_gravity_field.cpp @@ -88,7 +88,7 @@ bool LunarGravityField::ReadCoefficientsGrgm1200a(std::string file_name) { return true; } -void LunarGravityField::Update(const LocalEnvironment &local_environment, const Dynamics &dynamics) { +void LunarGravityField::Update(const LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics) { const CelestialInformation global_celestial_information = local_environment.GetCelestialInformation().GetGlobalInformation(); s2e::math::Matrix<3, 3> dcm_mci2mcmf_ = global_celestial_information.GetMoonRotation().GetDcmJ2000ToMcmf(); diff --git a/src/disturbances/lunar_gravity_field.hpp b/src/disturbances/lunar_gravity_field.hpp index 03841dd15..552e0425b 100644 --- a/src/disturbances/lunar_gravity_field.hpp +++ b/src/disturbances/lunar_gravity_field.hpp @@ -48,9 +48,9 @@ class LunarGravityField : public Disturbance { * @fn Update * @brief Override Updates function of SimpleDisturbance * @param [in] local_environment: Local environment information - * @param [in] dynamics: Dynamics information + * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment &local_environment, const Dynamics &dynamics); + virtual void Update(const LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics); // Override logger::ILoggable /** diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index 3368dc8df..98c352c32 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -26,7 +26,7 @@ Vector<3> MagneticDisturbance::CalcTorque_b_Nm(const Vector<3>& magnetic_field_b return torque_b_Nm_; } -void MagneticDisturbance::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { +void MagneticDisturbance::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { UNUSED(dynamics); CalcTorque_b_Nm(local_environment.GetGeomagneticField().GetGeomagneticField_b_nT()); diff --git a/src/disturbances/magnetic_disturbance.hpp b/src/disturbances/magnetic_disturbance.hpp index ecba81538..79f795ecf 100644 --- a/src/disturbances/magnetic_disturbance.hpp +++ b/src/disturbances/magnetic_disturbance.hpp @@ -33,9 +33,9 @@ class MagneticDisturbance : public Disturbance { * @fn Update * @brief Override Updates function of SimpleDisturbance * @param [in] local_environment: Local environment information - * @param [in] dynamics: Dynamics information + * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics); + virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); // Override logger::ILoggable /** diff --git a/src/disturbances/solar_radiation_pressure_disturbance.cpp b/src/disturbances/solar_radiation_pressure_disturbance.cpp index b28784672..18d66fd44 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.cpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.cpp @@ -16,7 +16,7 @@ SolarRadiationPressureDisturbance::SolarRadiationPressureDisturbance(const std:: const s2e::math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled) {} -void SolarRadiationPressureDisturbance::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { +void SolarRadiationPressureDisturbance::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { UNUSED(dynamics); s2e::math::Vector<3> sun_position_from_sc_b_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_b_m("SUN"); diff --git a/src/disturbances/solar_radiation_pressure_disturbance.hpp b/src/disturbances/solar_radiation_pressure_disturbance.hpp index d33233a1b..871538ba2 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.hpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.hpp @@ -34,9 +34,9 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { * @fn Update * @brief Override Updates function of SimpleDisturbance * @param [in] local_environment: Local environment information - * @param [in] dynamics: Dynamics information + * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics); + virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); // Override logger::ILoggable /** diff --git a/src/disturbances/third_body_gravity.cpp b/src/disturbances/third_body_gravity.cpp index 476fe673e..2598332a0 100644 --- a/src/disturbances/third_body_gravity.cpp +++ b/src/disturbances/third_body_gravity.cpp @@ -16,7 +16,7 @@ ThirdBodyGravity::ThirdBodyGravity(std::set third_body_list, const ThirdBodyGravity::~ThirdBodyGravity() {} -void ThirdBodyGravity::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { +void ThirdBodyGravity::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { acceleration_i_m_s2_ = s2e::math::Vector<3>(0.0); // initialize s2e::math::Vector<3> sc_position_i_m = dynamics.GetOrbit().GetPosition_i_m(); diff --git a/src/disturbances/third_body_gravity.hpp b/src/disturbances/third_body_gravity.hpp index b1d6d5e48..24a74ccc8 100644 --- a/src/disturbances/third_body_gravity.hpp +++ b/src/disturbances/third_body_gravity.hpp @@ -39,9 +39,9 @@ class ThirdBodyGravity : public Disturbance { * @fn Update * @brief Update third body disturbance * @param [in] local_environment: Local environment information - * @param [in] dynamics: Dynamics information + * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment& local_environment, const Dynamics& dynamics); + virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); private: std::set third_body_list_; //!< List of celestial bodies to calculate the third body disturbances diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index 98276524c..ea722d28d 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -9,20 +9,20 @@ namespace s2e::dynamics { -Dynamics::Dynamics(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, +dynamics::Dynamics::dynamics::Dynamics(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, const LocalEnvironment* local_environment, const int spacecraft_id, Structure* structure, RelativeInformation* relative_information) : structure_(structure), local_environment_(local_environment) { Initialize(simulation_configuration, simulation_time, spacecraft_id, structure, relative_information); } -Dynamics::~Dynamics() { +dynamics::Dynamics::~dynamics::Dynamics() { delete attitude_; delete orbit_; delete temperature_; } -void Dynamics::Initialize(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, const int spacecraft_id, +void dynamics::Dynamics::Initialize(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, const int spacecraft_id, Structure* structure, RelativeInformation* relative_information) { const LocalCelestialInformation& local_celestial_information = local_environment_->GetCelestialInformation(); // Initialize @@ -38,7 +38,7 @@ void Dynamics::Initialize(const SimulationConfiguration* simulation_configuratio orbit_->UpdateByAttitude(attitude_->GetQuaternion_i2b()); } -void Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestialInformation* local_celestial_information) { +void dynamics::Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestialInformation* local_celestial_information) { // Attitude propagation if (simulation_time->GetAttitudePropagateFlag()) { attitude_->Propagate(simulation_time->GetElapsedTime_s()); @@ -56,13 +56,13 @@ void Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestia } } -void Dynamics::ClearForceTorque(void) { +void dynamics::Dynamics::ClearForceTorque(void) { s2e::math::Vector<3> zero(0.0); attitude_->SetTorque_b_Nm(zero); orbit_->SetAcceleration_i_m_s2(zero); } -void Dynamics::LogSetup(Logger& logger) { +void dynamics::Dynamics::LogSetup(Logger& logger) { logger.AddLogList(attitude_); logger.AddLogList(orbit_); logger.AddLogList(temperature_); diff --git a/src/environment/local/local_environment.cpp b/src/environment/local/local_environment.cpp index b8738b87a..85d23926c 100644 --- a/src/environment/local/local_environment.cpp +++ b/src/environment/local/local_environment.cpp @@ -51,7 +51,7 @@ void LocalEnvironment::Initialize(const SimulationConfiguration* simulation_conf celestial_information_->is_log_enabled_ = conf.ReadEnable("LOCAL_CELESTIAL_INFORMATION", "logging"); } -void LocalEnvironment::Update(const Dynamics* dynamics, const SimulationTime* simulation_time) { +void LocalEnvironment::Update(const dynamics::Dynamics* dynamics, const SimulationTime* simulation_time) { auto& orbit = dynamics->GetOrbit(); auto& attitude = dynamics->GetAttitude(); diff --git a/src/environment/local/local_environment.hpp b/src/environment/local/local_environment.hpp index 807ffa100..f251135f4 100644 --- a/src/environment/local/local_environment.hpp +++ b/src/environment/local/local_environment.hpp @@ -15,7 +15,7 @@ #include "simulation/simulation_configuration.hpp" #include "solar_radiation_pressure_environment.hpp" -class Dynamics; +class dynamics::Dynamics; namespace s2e::environment { @@ -45,7 +45,7 @@ class LocalEnvironment { * @param [in] dynamics: Dynamics information of the satellite * @param [in] simulation_time: Simulation time */ - void Update(const Dynamics* dynamics, const SimulationTime* simulation_time); + void Update(const dynamics::Dynamics* dynamics, const SimulationTime* simulation_time); /** * @fn LogSetup diff --git a/src/simulation/multiple_spacecraft/relative_information.cpp b/src/simulation/multiple_spacecraft/relative_information.cpp index 782b50295..93cd3007b 100644 --- a/src/simulation/multiple_spacecraft/relative_information.cpp +++ b/src/simulation/multiple_spacecraft/relative_information.cpp @@ -39,7 +39,7 @@ void RelativeInformation::Update() { } } -void RelativeInformation::RegisterDynamicsInfo(const size_t spacecraft_id, const Dynamics* dynamics) { +void RelativeInformation::RegisterDynamicsInfo(const size_t spacecraft_id, const dynamics::Dynamics* dynamics) { dynamics_database_.emplace(spacecraft_id, dynamics); ResizeLists(); } diff --git a/src/simulation/multiple_spacecraft/relative_information.hpp b/src/simulation/multiple_spacecraft/relative_information.hpp index b68cab097..04423a2ed 100644 --- a/src/simulation/multiple_spacecraft/relative_information.hpp +++ b/src/simulation/multiple_spacecraft/relative_information.hpp @@ -42,7 +42,7 @@ class RelativeInformation : public logger::ILoggable { * @param [in] spacecraft_id: ID of target spacecraft * @param [in] dynamics: Dynamics information of the target spacecraft */ - void RegisterDynamicsInfo(const size_t spacecraft_id, const dynamics::Dynamics* dynamics); + void RegisterDynamicsInfo(const size_t spacecraft_id, const dynamics::dynamics::Dynamics* dynamics); /** * @fn RegisterDynamicsInfo * @brief Remove dynamics information of target spacecraft @@ -129,12 +129,12 @@ class RelativeInformation : public logger::ILoggable { * @brief Return the dynamics information of a spacecraft * @param [in] target_spacecraft_id: ID of the spacecraft */ - inline const dynamics::Dynamics* GetReferenceSatDynamics(const size_t reference_spacecraft_id) const { + inline const dynamics::dynamics::Dynamics* GetReferenceSatDynamics(const size_t reference_spacecraft_id) const { return dynamics_database_.at(reference_spacecraft_id); }; private: - std::map dynamics_database_; //!< Dynamics database of all spacecraft + std::map dynamics_database_; //!< Dynamics database of all spacecraft std::vector>> relative_position_list_i_m_; //!< Relative position list in the inertial frame in unit [m] std::vector>> relative_velocity_list_i_m_s_; //!< Relative velocity list in the inertial frame in unit [m/s] diff --git a/src/simulation/spacecraft/spacecraft.cpp b/src/simulation/spacecraft/spacecraft.cpp index d99b1a1a5..b0efbad8b 100644 --- a/src/simulation/spacecraft/spacecraft.cpp +++ b/src/simulation/spacecraft/spacecraft.cpp @@ -32,7 +32,7 @@ void Spacecraft::Initialize(const SimulationConfiguration* simulation_configurat clock_generator_.ClearTimerCount(); structure_ = new Structure(simulation_configuration, spacecraft_id); local_environment_ = new environment::LocalEnvironment(simulation_configuration, global_environment, spacecraft_id); - dynamics_ = new dynamics::Dynamics(simulation_configuration, &(global_environment->GetSimulationTime()), local_environment_, spacecraft_id, structure_, + dynamics_ = new dynamics::dynamics::Dynamics(simulation_configuration, &(global_environment->GetSimulationTime()), local_environment_, spacecraft_id, structure_, relative_information); disturbances_ = new disturbances::Disturbances(simulation_configuration, spacecraft_id, structure_, global_environment); diff --git a/src/simulation/spacecraft/spacecraft.hpp b/src/simulation/spacecraft/spacecraft.hpp index 88f612246..5b490ec1e 100644 --- a/src/simulation/spacecraft/spacecraft.hpp +++ b/src/simulation/spacecraft/spacecraft.hpp @@ -71,7 +71,7 @@ class Spacecraft { * @fn GetDynamics * @brief Get dynamics of the spacecraft */ - inline const dynamics::Dynamics& GetDynamics() const { return *dynamics_; } + inline const dynamics::dynamics::Dynamics& GetDynamics() const { return *dynamics_; } /** * @fn GetLocalEnvironment * @brief Get local environment around the spacecraft @@ -100,7 +100,7 @@ class Spacecraft { protected: environment::ClockGenerator clock_generator_; //!< Origin of clock for the spacecraft - dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft + dynamics::dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft RelativeInformation* relative_information_; //!< Relative information with respect to the other spacecraft environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft disturbances::Disturbances* disturbances_; //!< Disturbance information acting on the spacecraft diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index 38debc042..0a61fea77 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -13,7 +13,7 @@ namespace s2e::sample { using namespace components; -SampleComponents::SampleComponents(const dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, +SampleComponents::SampleComponents(const dynamics::dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, const environment::GlobalEnvironment* global_environment, const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, const unsigned int spacecraft_id) : configuration_(configuration), diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index 756e3fc9b..27c1118fc 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -62,7 +62,7 @@ class SampleComponents : public simulation::InstalledComponents { * @fn SampleComponents * @brief Constructor */ - SampleComponents(const dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, + SampleComponents(const dynamics::dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, const environment::GlobalEnvironment* global_environment, const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, const unsigned int spacecraft_id); /** @@ -139,7 +139,7 @@ class SampleComponents : public simulation::InstalledComponents { // States const simulation::SimulationConfiguration* configuration_; //!< Simulation settings - const dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft + const dynamics::dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft simulation::Structure* structure_; //!< Structure information of the spacecraft const environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft const environment::GlobalEnvironment* global_environment_; //!< Global environment information From 0a6a3d2fb983b64febe98437e6418b6f09aa539f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 28 Sep 2024 22:03:29 +0900 Subject: [PATCH 39/49] Fix namespace --- src/components/base/sensor.hpp | 18 +- .../base/sensor_template_functions.hpp | 34 ++-- .../examples/example_change_structure.cpp | 2 +- .../ideal/angular_velocity_observer.hpp | 4 +- src/components/ideal/attitude_observer.cpp | 6 +- src/components/ideal/attitude_observer.hpp | 4 +- src/components/ideal/force_generator.cpp | 32 ++-- src/components/ideal/force_generator.hpp | 16 +- src/components/ideal/orbit_observer.cpp | 14 +- src/components/ideal/orbit_observer.hpp | 10 +- src/components/ideal/torque_generator.cpp | 16 +- src/components/ideal/torque_generator.hpp | 8 +- src/components/real/aocs/gnss_receiver.cpp | 104 ++++++------ src/components/real/aocs/gnss_receiver.hpp | 32 ++-- src/components/real/aocs/gyro_sensor.cpp | 8 +- src/components/real/aocs/gyro_sensor.hpp | 10 +- src/components/real/aocs/magnetometer.cpp | 8 +- src/components/real/aocs/magnetometer.hpp | 16 +- src/components/real/aocs/magnetorquer.cpp | 64 +++---- src/components/real/aocs/magnetorquer.hpp | 50 +++--- .../aocs/mtq_magnetometer_interference.cpp | 10 +- .../aocs/mtq_magnetometer_interference.hpp | 4 +- src/components/real/aocs/reaction_wheel.cpp | 58 +++---- src/components/real/aocs/reaction_wheel.hpp | 28 +-- .../real/aocs/reaction_wheel_jitter.cpp | 6 +- .../real/aocs/reaction_wheel_jitter.hpp | 40 ++--- .../real/aocs/reaction_wheel_ode.cpp | 4 +- .../real/aocs/reaction_wheel_ode.hpp | 4 +- src/components/real/aocs/star_sensor.cpp | 68 ++++---- src/components/real/aocs/star_sensor.hpp | 30 ++-- src/components/real/aocs/sun_sensor.cpp | 40 ++--- src/components/real/aocs/sun_sensor.hpp | 14 +- src/components/real/communication/antenna.cpp | 4 +- src/components/real/communication/antenna.hpp | 8 +- .../antenna_radiation_pattern.hpp | 6 +- .../ground_station_calculator.cpp | 10 +- src/components/real/mission/telescope.cpp | 86 +++++----- src/components/real/mission/telescope.hpp | 26 +-- src/components/real/power/battery.cpp | 8 +- .../real/power/csv_scenario_interface.cpp | 4 +- .../real/power/csv_scenario_interface.hpp | 2 +- .../real/power/pcu_initial_study.cpp | 8 +- .../real/power/solar_array_panel.cpp | 22 +-- .../real/power/solar_array_panel.hpp | 8 +- .../real/propulsion/simple_thruster.cpp | 34 ++-- src/disturbances/air_drag.cpp | 20 +-- src/disturbances/air_drag.hpp | 6 +- src/disturbances/disturbance.hpp | 24 +-- src/disturbances/disturbances.hpp | 6 +- src/disturbances/geopotential.cpp | 12 +- src/disturbances/geopotential.hpp | 2 +- src/disturbances/gravity_gradient.cpp | 4 +- src/disturbances/gravity_gradient.hpp | 2 +- src/disturbances/lunar_gravity_field.cpp | 16 +- src/disturbances/lunar_gravity_field.hpp | 2 +- src/disturbances/magnetic_disturbance.cpp | 4 +- src/disturbances/magnetic_disturbance.hpp | 4 +- .../solar_radiation_pressure_disturbance.cpp | 6 +- .../solar_radiation_pressure_disturbance.hpp | 4 +- src/disturbances/surface_force.cpp | 24 +-- src/disturbances/surface_force.hpp | 10 +- src/disturbances/third_body_gravity.cpp | 14 +- src/disturbances/third_body_gravity.hpp | 4 +- src/dynamics/attitude/attitude.cpp | 32 ++-- src/dynamics/attitude/attitude.hpp | 40 ++--- src/dynamics/attitude/attitude_rk4.cpp | 34 ++-- src/dynamics/attitude/attitude_rk4.hpp | 14 +- .../attitude_with_cantilever_vibration.cpp | 20 +-- .../attitude_with_cantilever_vibration.hpp | 14 +- src/dynamics/attitude/controlled_attitude.cpp | 58 +++---- src/dynamics/attitude/controlled_attitude.hpp | 32 ++-- src/dynamics/attitude/initialize_attitude.cpp | 20 +-- src/dynamics/attitude/initialize_attitude.hpp | 4 +- ...ode_attitude_with_cantilever_vibration.hpp | 80 +++++---- src/dynamics/dynamics.cpp | 17 +- src/dynamics/dynamics.hpp | 36 ++-- .../orbit/encke_orbit_propagation.cpp | 18 +- .../orbit/encke_orbit_propagation.hpp | 18 +- src/dynamics/orbit/initialize_orbit.cpp | 34 ++-- src/dynamics/orbit/initialize_orbit.hpp | 4 +- src/dynamics/orbit/orbit.cpp | 38 ++--- src/dynamics/orbit/orbit.hpp | 44 ++--- src/dynamics/orbit/relative_orbit.cpp | 28 +-- src/dynamics/orbit/relative_orbit.hpp | 16 +- src/dynamics/orbit/rk4_orbit_propagation.cpp | 8 +- src/dynamics/orbit/rk4_orbit_propagation.hpp | 8 +- src/dynamics/thermal/heater_controller.hpp | 4 +- src/dynamics/thermal/node.cpp | 10 +- src/dynamics/thermal/node.hpp | 10 +- src/dynamics/thermal/temperature.cpp | 16 +- src/dynamics/thermal/temperature.hpp | 18 +- .../global/celestial_information.cpp | 4 +- .../global/celestial_information.hpp | 30 ++-- src/environment/global/clock_generator.cpp | 4 +- src/environment/global/clock_generator.hpp | 6 +- src/environment/global/earth_rotation.cpp | 160 +++++++++--------- src/environment/global/earth_rotation.hpp | 16 +- src/environment/global/global_environment.cpp | 10 +- src/environment/global/global_environment.hpp | 6 +- src/environment/global/gnss_satellites.cpp | 16 +- src/environment/global/gnss_satellites.hpp | 6 +- .../global/hipparcos_catalogue.cpp | 14 +- .../global/hipparcos_catalogue.hpp | 4 +- src/environment/global/moon_rotation.cpp | 8 +- src/environment/global/moon_rotation.hpp | 8 +- src/environment/global/physical_constants.hpp | 6 +- src/environment/global/simulation_time.cpp | 6 +- src/environment/local/atmosphere.cpp | 8 +- src/environment/local/atmosphere.hpp | 2 +- src/environment/local/earth_albedo.cpp | 10 +- src/environment/local/geomagnetic_field.cpp | 6 +- src/environment/local/geomagnetic_field.hpp | 10 +- .../local/local_celestial_information.cpp | 52 +++--- .../local/local_celestial_information.hpp | 22 +-- src/environment/local/local_environment.cpp | 8 +- src/environment/local/local_environment.hpp | 6 +- .../solar_radiation_pressure_environment.cpp | 18 +- src/logger/log_utility.hpp | 12 +- src/logger/logger.cpp | 4 +- .../atmosphere/harris_priester_model.cpp | 10 +- .../atmosphere/harris_priester_model.hpp | 2 +- .../atmosphere/wrapper_nrlmsise00.cpp | 6 +- .../geodesy/geodetic_position.cpp | 10 +- .../geodesy/geodetic_position.hpp | 8 +- src/math_physics/gnss/antex_file_reader.cpp | 2 +- src/math_physics/gnss/antex_file_reader.hpp | 6 +- src/math_physics/gnss/sp3_file_reader.cpp | 10 +- src/math_physics/gnss/sp3_file_reader.hpp | 10 +- .../gravity/gravity_potential.cpp | 8 +- .../gravity/gravity_potential.hpp | 4 +- .../gravity/test_gravity_potential.cpp | 40 ++--- .../math/ordinary_differential_equation.hpp | 2 +- src/math_physics/math/s2e_math.cpp | 8 +- src/math_physics/math/test_interpolation.cpp | 30 ++-- src/math_physics/math/test_matrix.cpp | 60 +++---- src/math_physics/math/test_matrix_vector.cpp | 10 +- src/math_physics/math/test_quaternion.cpp | 134 +++++++-------- src/math_physics/math/test_s2e_math.cpp | 14 +- src/math_physics/math/test_vector.cpp | 94 +++++----- .../dormand_prince_5.hpp | 4 +- .../dormand_prince_5_implementation.hpp | 6 +- .../embedded_runge_kutta_implementation.hpp | 6 +- .../numerical_integration/interface_ode.hpp | 2 +- .../numerical_integrator.hpp | 10 +- .../numerical_integration/ode_examples.hpp | 16 +- .../numerical_integration/runge_kutta.hpp | 2 +- .../numerical_integration/runge_kutta_4.hpp | 2 +- .../runge_kutta_fehlberg.hpp | 2 +- .../runge_kutta_fehlberg_implementation.hpp | 8 +- .../runge_kutta_template.hpp | 4 +- .../test_runge_kutta.cpp | 86 +++++----- .../optics/gaussian_beam_base.cpp | 8 +- .../optics/gaussian_beam_base.hpp | 12 +- .../orbit/interpolation_orbit.cpp | 8 +- .../orbit/interpolation_orbit.hpp | 6 +- src/math_physics/orbit/kepler_orbit.cpp | 16 +- src/math_physics/orbit/kepler_orbit.hpp | 10 +- src/math_physics/orbit/orbital_elements.cpp | 16 +- src/math_physics/orbit/orbital_elements.hpp | 8 +- .../orbit/relative_orbit_models.cpp | 8 +- .../orbit/relative_orbit_models.hpp | 4 +- .../orbit/test_interpolation_orbit.cpp | 4 +- .../moon_rotation_utilities.cpp | 30 ++-- .../moon_rotation_utilities.hpp | 6 +- .../randomization/random_walk.hpp | 8 +- .../random_walk_template_functions.hpp | 6 +- .../c2a_command_database.cpp | 20 +-- .../initialize_file_access.cpp | 4 +- .../initialize_file_access.hpp | 6 +- .../ground_station/ground_station.cpp | 14 +- .../initialize_monte_carlo_parameters.cpp | 70 ++++---- .../initialize_monte_carlo_simulation.cpp | 4 +- .../monte_carlo_simulation_executor.cpp | 2 +- .../monte_carlo_simulation_executor.hpp | 12 +- .../simulation_object.cpp | 2 +- .../simulation_object.hpp | 6 +- .../relative_information.cpp | 58 +++---- .../relative_information.hpp | 26 +-- .../spacecraft/installed_components.cpp | 8 +- .../spacecraft/installed_components.hpp | 4 +- .../structure/initialize_structure.cpp | 8 +- .../structure/kinematics_parameters.cpp | 2 +- .../structure/kinematics_parameters.hpp | 14 +- .../structure/residual_magnetic_moment.cpp | 2 +- .../structure/residual_magnetic_moment.hpp | 4 +- .../spacecraft/structure/surface.cpp | 2 +- .../spacecraft/structure/surface.hpp | 14 +- .../spacecraft/sample_components.cpp | 6 +- 188 files changed, 1670 insertions(+), 1673 deletions(-) diff --git a/src/components/base/sensor.hpp b/src/components/base/sensor.hpp index 38f2a9075..1091be06b 100644 --- a/src/components/base/sensor.hpp +++ b/src/components/base/sensor.hpp @@ -33,9 +33,9 @@ class Sensor { * @param [in] random_walk_standard_deviation_c: Standard deviation of random wark at the component frame * @param [in] random_walk_limit_c: Limit of random walk at the component frame */ - Sensor(const s2e::math::Matrix& scale_factor, const s2e::math::Vector& range_to_const_c, const s2e::math::Vector& range_to_zero_c, - const s2e::math::Vector& bias_noise_c, const s2e::math::Vector& normal_random_standard_deviation_c, const double random_walk_step_width_s, - const s2e::math::Vector& random_walk_standard_deviation_c, const s2e::math::Vector& random_walk_limit_c); + Sensor(const math::Matrix& scale_factor, const math::Vector& range_to_const_c, const math::Vector& range_to_zero_c, + const math::Vector& bias_noise_c, const math::Vector& normal_random_standard_deviation_c, const double random_walk_step_width_s, + const math::Vector& random_walk_standard_deviation_c, const math::Vector& random_walk_limit_c); /** * @fn ~Sensor * @brief Destructor @@ -43,7 +43,7 @@ class Sensor { ~Sensor(); protected: - s2e::math::Vector bias_noise_c_; //!< Constant bias noise at the component frame + math::Vector bias_noise_c_; //!< Constant bias noise at the component frame /** * @fn Measure @@ -51,12 +51,12 @@ class Sensor { * @param [in] true_value_c: True value at the component frame * @return Observed value with noise at the component frame */ - s2e::math::Vector Measure(const s2e::math::Vector true_value_c); + math::Vector Measure(const math::Vector true_value_c); private: - s2e::math::Matrix scale_factor_; //!< Scale factor matrix - s2e::math::Vector range_to_const_c_; //!< Output range limit to be constant output value at the component frame - s2e::math::Vector range_to_zero_c_; //!< Output range limit to be zero output value at the component frame + math::Matrix scale_factor_; //!< Scale factor matrix + math::Vector range_to_const_c_; //!< Output range limit to be constant output value at the component frame + math::Vector range_to_zero_c_; //!< Output range limit to be zero output value at the component frame s2e::randomization::NormalRand normal_random_noise_c_[N]; //!< Normal random RandomWalk random_walk_noise_c_; //!< Random Walk @@ -66,7 +66,7 @@ class Sensor { * @param [in] input_c: Input value at the component frame * @return Clipped value */ - s2e::math::Vector Clip(const s2e::math::Vector input_c); + math::Vector Clip(const math::Vector input_c); /** * @fn RangeCheck * @brief Check the range_to_const_c_ and range_to_zero_c_ is correct and fixed the values diff --git a/src/components/base/sensor_template_functions.hpp b/src/components/base/sensor_template_functions.hpp index b1236901a..89bcd73ab 100644 --- a/src/components/base/sensor_template_functions.hpp +++ b/src/components/base/sensor_template_functions.hpp @@ -12,10 +12,10 @@ namespace s2e::components { template -Sensor::Sensor(const s2e::math::Matrix& scale_factor, const s2e::math::Vector& range_to_const_c, const s2e::math::Vector& range_to_zero_c, - const s2e::math::Vector& bias_noise_c, const s2e::math::Vector& normal_random_standard_deviation_c, - const double random_walk_step_width_s, const s2e::math::Vector& random_walk_standard_deviation_c, - const s2e::math::Vector& random_walk_limit_c) +Sensor::Sensor(const math::Matrix& scale_factor, const math::Vector& range_to_const_c, const math::Vector& range_to_zero_c, + const math::Vector& bias_noise_c, const math::Vector& normal_random_standard_deviation_c, + const double random_walk_step_width_s, const math::Vector& random_walk_standard_deviation_c, + const math::Vector& random_walk_limit_c) : bias_noise_c_(bias_noise_c), scale_factor_(scale_factor), range_to_const_c_(range_to_const_c), @@ -31,8 +31,8 @@ template Sensor::~Sensor() {} template -s2e::math::Vector Sensor::Measure(const s2e::math::Vector true_value_c) { - s2e::math::Vector calc_value_c; +math::Vector Sensor::Measure(const math::Vector true_value_c) { + math::Vector calc_value_c; calc_value_c = scale_factor_ * true_value_c; calc_value_c += bias_noise_c_; for (size_t i = 0; i < N; ++i) { @@ -44,8 +44,8 @@ s2e::math::Vector Sensor::Measure(const s2e::math::Vector true_value_c) } template -s2e::math::Vector Sensor::Clip(const s2e::math::Vector input_c) { - s2e::math::Vector output_c; +math::Vector Sensor::Clip(const math::Vector input_c) { + math::Vector output_c; for (size_t i = 0; i < N; ++i) { if (input_c[i] >= range_to_const_c_[i] && input_c[i] < range_to_zero_c_[i]) { output_c[i] = range_to_const_c_[i]; @@ -82,11 +82,11 @@ Sensor ReadSensorInformation(const std::string file_name, const double step_w setting_file_reader::IniAccess ini_file(file_name); std::string section = "SENSOR_BASE_" + component_name; - s2e::math::Vector scale_factor_vector; + math::Vector scale_factor_vector; ini_file.ReadVector(section.c_str(), "scale_factor_c", scale_factor_vector); - s2e::math::Matrix scale_factor_c; + math::Matrix scale_factor_c; if (scale_factor_vector.CalcNorm() == 0.0) { - scale_factor_c = s2e::math::MakeIdentityMatrix(); + scale_factor_c = math::MakeIdentityMatrix(); } else { for (size_t i = 0; i < N; i++) { for (size_t j = 0; j < N; j++) { @@ -96,26 +96,26 @@ Sensor ReadSensorInformation(const std::string file_name, const double step_w } std::string key_name; - s2e::math::Vector constant_bias_c; + math::Vector constant_bias_c; key_name = "constant_bias_c_" + unit; ini_file.ReadVector(section.c_str(), key_name.c_str(), constant_bias_c); - s2e::math::Vector normal_random_standard_deviation_c; + math::Vector normal_random_standard_deviation_c; key_name = "normal_random_standard_deviation_c_" + unit; ini_file.ReadVector(section.c_str(), key_name.c_str(), normal_random_standard_deviation_c); - s2e::math::Vector random_walk_standard_deviation_c; + math::Vector random_walk_standard_deviation_c; key_name = "random_walk_standard_deviation_c_" + unit; ini_file.ReadVector(section.c_str(), key_name.c_str(), random_walk_standard_deviation_c); - s2e::math::Vector random_walk_limit_c; + math::Vector random_walk_limit_c; key_name = "random_walk_limit_c_" + unit; ini_file.ReadVector(section.c_str(), key_name.c_str(), random_walk_limit_c); key_name = "range_to_constant_" + unit; double range_to_const = ini_file.ReadDouble(section.c_str(), key_name.c_str()); - s2e::math::Vector range_to_const_c{range_to_const}; + math::Vector range_to_const_c{range_to_const}; key_name = "range_to_zero_" + unit; double range_to_zero = ini_file.ReadDouble(section.c_str(), key_name.c_str()); - s2e::math::Vector range_to_zero_c{range_to_zero}; + math::Vector range_to_zero_c{range_to_zero}; Sensor sensor_base(scale_factor_c, range_to_const_c, range_to_zero_c, constant_bias_c, normal_random_standard_deviation_c, step_width_s, random_walk_standard_deviation_c, random_walk_limit_c); diff --git a/src/components/examples/example_change_structure.cpp b/src/components/examples/example_change_structure.cpp index 0ee982771..62e2a0cc6 100644 --- a/src/components/examples/example_change_structure.cpp +++ b/src/components/examples/example_change_structure.cpp @@ -37,7 +37,7 @@ void ExampleChangeStructure::MainRoutine(const int time_count) { structure_->GetToSetSurfaces()[0].SetArea_m2(0.5); // Inertia Tensor - s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2(0.0); + math::Matrix<3, 3> inertia_tensor_b_kgm2(0.0); inertia_tensor_b_kgm2[0][0] = 0.2; inertia_tensor_b_kgm2[1][1] = 0.2; inertia_tensor_b_kgm2[2][2] = 0.2; diff --git a/src/components/ideal/angular_velocity_observer.hpp b/src/components/ideal/angular_velocity_observer.hpp index 404785058..a99c0cb13 100644 --- a/src/components/ideal/angular_velocity_observer.hpp +++ b/src/components/ideal/angular_velocity_observer.hpp @@ -59,10 +59,10 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public logge * @fn GetAngularVelocity_b_rad_s * @brief Return observed angular velocity */ - inline s2e::math::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; } + inline math::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; } protected: - s2e::math::Vector<3> angular_velocity_b_rad_s_{0.0}; //!< Observed angular velocity [rad/s] + math::Vector<3> angular_velocity_b_rad_s_{0.0}; //!< Observed angular velocity [rad/s] const dynamics::attitude::Attitude& attitude_; //!< Dynamics information }; diff --git a/src/components/ideal/attitude_observer.cpp b/src/components/ideal/attitude_observer.cpp index dad39b3e3..9d3a84165 100644 --- a/src/components/ideal/attitude_observer.cpp +++ b/src/components/ideal/attitude_observer.cpp @@ -20,14 +20,14 @@ void AttitudeObserver::MainRoutine(const int time_count) { UNUSED(time_count); // Error calculation - s2e::math::Vector<3> random_direction; + math::Vector<3> random_direction; random_direction[0] = direction_noise_; random_direction[1] = direction_noise_; random_direction[2] = direction_noise_; random_direction = random_direction.CalcNormalizedVector(); double error_angle_rad = angle_noise_; - s2e::math::Quaternion error_quaternion(random_direction, error_angle_rad); + math::Quaternion error_quaternion(random_direction, error_angle_rad); observed_quaternion_i2b_ = error_quaternion * attitude_.GetQuaternion_i2b(); } @@ -59,7 +59,7 @@ AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_g // AttitudeObserver double error_angle_standard_deviation_deg = ini_file.ReadDouble("ATTITUDE_OBSERVER", "error_angle_standard_deviation_deg"); - double error_angle_standard_deviation_rad = s2e::math::deg_to_rad * error_angle_standard_deviation_deg; + double error_angle_standard_deviation_rad = math::deg_to_rad * error_angle_standard_deviation_deg; AttitudeObserver attitude_observer(prescaler, clock_generator, error_angle_standard_deviation_rad, attitude); return attitude_observer; diff --git a/src/components/ideal/attitude_observer.hpp b/src/components/ideal/attitude_observer.hpp index 59c00dd29..148b3aa4f 100644 --- a/src/components/ideal/attitude_observer.hpp +++ b/src/components/ideal/attitude_observer.hpp @@ -59,10 +59,10 @@ class AttitudeObserver : public Component, public logger::ILoggable { * @fn GetQuaternion_i2c * @brief Return observed quaternion from the inertial frame to the body-fixed frame */ - inline const s2e::math::Quaternion GetQuaternion_i2b() const { return observed_quaternion_i2b_; }; + inline const math::Quaternion GetQuaternion_i2b() const { return observed_quaternion_i2b_; }; protected: - s2e::math::Quaternion observed_quaternion_i2b_ = {0.0, 0.0, 0.0, 1.0}; //!< Observed quaternion + math::Quaternion observed_quaternion_i2b_ = {0.0, 0.0, 0.0, 1.0}; //!< Observed quaternion s2e::randomization::NormalRand angle_noise_; //!< Normal random for magnitude noise s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise diff --git a/src/components/ideal/force_generator.cpp b/src/components/ideal/force_generator.cpp index d1770ac79..2bac42d3f 100644 --- a/src/components/ideal/force_generator.cpp +++ b/src/components/ideal/force_generator.cpp @@ -31,16 +31,16 @@ void ForceGenerator::MainRoutine(const int time_count) { double norm_ordered_force = ordered_force_b_N_.CalcNorm(); if (norm_ordered_force > 0.0 + DBL_EPSILON) { // Add noise only when the force is generated - s2e::math::Vector<3> true_direction = generated_force_b_N_.CalcNormalizedVector(); - s2e::math::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_); - s2e::math::Vector<3> converted_direction = error_quaternion.FrameConversion(true_direction); + math::Vector<3> true_direction = generated_force_b_N_.CalcNormalizedVector(); + math::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_); + math::Vector<3> converted_direction = error_quaternion.FrameConversion(true_direction); double force_norm_with_error = norm_ordered_force + magnitude_noise_; generated_force_b_N_ = force_norm_with_error * converted_direction; } // Convert frame - s2e::math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); - s2e::math::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh(); + math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + math::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh(); generated_force_i_N_ = q_i2b.InverseFrameConversion(generated_force_b_N_); generated_force_rtn_N_ = q_i2rtn.FrameConversion(generated_force_i_N_); } @@ -51,16 +51,16 @@ void ForceGenerator::PowerOffRoutine() { generated_force_rtn_N_ *= 0.0; } -void ForceGenerator::SetForce_i_N(const s2e::math::Vector<3> force_i_N) { - s2e::math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); +void ForceGenerator::SetForce_i_N(const math::Vector<3> force_i_N) { + math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); ordered_force_b_N_ = q_i2b.FrameConversion(force_i_N); } -void ForceGenerator::SetForce_rtn_N(const s2e::math::Vector<3> force_rtn_N) { - s2e::math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); - s2e::math::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh(); +void ForceGenerator::SetForce_rtn_N(const math::Vector<3> force_rtn_N) { + math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + math::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh(); - s2e::math::Vector<3> force_i_N = q_i2rtn.InverseFrameConversion(force_rtn_N); + math::Vector<3> force_i_N = q_i2rtn.InverseFrameConversion(force_rtn_N); ordered_force_b_N_ = q_i2b.FrameConversion(force_i_N); } @@ -87,14 +87,14 @@ std::string ForceGenerator::GetLogValue() const { return str_tmp; } -s2e::math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(s2e::math::Vector<3> true_direction, const double error_standard_deviation_rad) { - s2e::math::Vector<3> random_direction; +math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(math::Vector<3> true_direction, const double error_standard_deviation_rad) { + math::Vector<3> random_direction; random_direction[0] = direction_noise_; random_direction[1] = direction_noise_; random_direction[2] = direction_noise_; random_direction = random_direction.CalcNormalizedVector(); - s2e::math::Vector<3> rotation_axis; + math::Vector<3> rotation_axis; rotation_axis = OuterProduct(true_direction, random_direction); double norm_rotation_axis = rotation_axis.CalcNorm(); if (norm_rotation_axis < 0.0 + DBL_EPSILON) { @@ -103,7 +103,7 @@ s2e::math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(s2e::math } double error_angle_rad = direction_noise_ * error_standard_deviation_rad; - s2e::math::Quaternion error_quaternion(rotation_axis, error_angle_rad); + math::Quaternion error_quaternion(rotation_axis, error_angle_rad); return error_quaternion; } @@ -119,7 +119,7 @@ ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_gener char section[30] = "FORCE_GENERATOR"; double force_magnitude_standard_deviation_N = ini_file.ReadDouble(section, "force_magnitude_standard_deviation_N"); double force_direction_standard_deviation_deg = ini_file.ReadDouble(section, "force_direction_standard_deviation_deg"); - double force_direction_standard_deviation_rad = s2e::math::deg_to_rad * force_direction_standard_deviation_deg; + double force_direction_standard_deviation_rad = math::deg_to_rad * force_direction_standard_deviation_deg; ForceGenerator force_generator(prescaler, clock_generator, force_magnitude_standard_deviation_N, force_direction_standard_deviation_rad, dynamics); return force_generator; diff --git a/src/components/ideal/force_generator.hpp b/src/components/ideal/force_generator.hpp index 0881e7773..61f94d4d9 100644 --- a/src/components/ideal/force_generator.hpp +++ b/src/components/ideal/force_generator.hpp @@ -83,23 +83,23 @@ class ForceGenerator : public Component, public logger::ILoggable { * @fn SetForce_b_N * @brief Set ordered force in the body fixed frame [N] */ - inline void SetForce_b_N(const s2e::math::Vector<3> force_b_N) { ordered_force_b_N_ = force_b_N; }; + inline void SetForce_b_N(const math::Vector<3> force_b_N) { ordered_force_b_N_ = force_b_N; }; /** * @fn SetForce_i_N * @brief Set ordered force in the inertial frame [N] */ - void SetForce_i_N(const s2e::math::Vector<3> force_i_N); + void SetForce_i_N(const math::Vector<3> force_i_N); /** * @fn SetForce_rtn_N * @brief Set ordered force in the RTN frame [N] */ - void SetForce_rtn_N(const s2e::math::Vector<3> force_rtn_N); + void SetForce_rtn_N(const math::Vector<3> force_rtn_N); protected: - s2e::math::Vector<3> ordered_force_b_N_{0.0}; //!< Ordered force in the body fixed frame [N] - s2e::math::Vector<3> generated_force_b_N_{0.0}; //!< Generated force in the body fixed frame [N] - s2e::math::Vector<3> generated_force_i_N_{0.0}; //!< Generated force in the inertial frame [N] - s2e::math::Vector<3> generated_force_rtn_N_{0.0}; //!< Generated force in the RTN frame [N] + math::Vector<3> ordered_force_b_N_{0.0}; //!< Ordered force in the body fixed frame [N] + math::Vector<3> generated_force_b_N_{0.0}; //!< Generated force in the body fixed frame [N] + math::Vector<3> generated_force_i_N_{0.0}; //!< Generated force in the inertial frame [N] + math::Vector<3> generated_force_rtn_N_{0.0}; //!< Generated force in the RTN frame [N] // Noise s2e::randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise @@ -112,7 +112,7 @@ class ForceGenerator : public Component, public logger::ILoggable { * @param [in] true_direction: True direction * @param [in] error_standard_deviation_rad: Standard deviation of direction error [rad] */ - s2e::math::Quaternion GenerateDirectionNoiseQuaternion(s2e::math::Vector<3> true_direction, const double error_standard_deviation_rad); + math::Quaternion GenerateDirectionNoiseQuaternion(math::Vector<3> true_direction, const double error_standard_deviation_rad); const dynamics::Dynamics* dynamics_; //!< Spacecraft dynamics information }; diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 58ede8f6b..37e5c4dc3 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -11,7 +11,7 @@ namespace s2e::components { OrbitObserver::OrbitObserver(const int prescaler, environment::ClockGenerator* clock_generator, const NoiseFrame noise_frame, - const s2e::math::Vector<6> error_standard_deviation, const Orbit& orbit) + const math::Vector<6> error_standard_deviation, const Orbit& orbit) : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], s2e::randomization::global_randomization.MakeSeed()); @@ -22,11 +22,11 @@ void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); // Calc noise - s2e::math::Vector<3> position_error_i_m{0.0}; - s2e::math::Vector<3> position_error_rtn_m{0.0}; - s2e::math::Vector<3> velocity_error_i_m_s{0.0}; - s2e::math::Vector<3> velocity_error_rtn_m_s{0.0}; - s2e::math::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); + math::Vector<3> position_error_i_m{0.0}; + math::Vector<3> position_error_rtn_m{0.0}; + math::Vector<3> velocity_error_i_m_s{0.0}; + math::Vector<3> velocity_error_rtn_m_s{0.0}; + math::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); switch (noise_frame_) { case NoiseFrame::kInertial: for (size_t axis = 0; axis < 3; axis++) { @@ -95,7 +95,7 @@ OrbitObserver InitializeOrbitObserver(environment::ClockGenerator* clock_generat // Noise const NoiseFrame noise_frame = SetNoiseFrame(ini_file.ReadString("ORBIT_OBSERVER", "noise_frame")); - s2e::math::Vector<6> noise_standard_deviation; + math::Vector<6> noise_standard_deviation; ini_file.ReadVector("ORBIT_OBSERVER", "noise_standard_deviation", noise_standard_deviation); OrbitObserver orbit_observer(prescaler, clock_generator, noise_frame, noise_standard_deviation, orbit); diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 58a2cf3c9..953184fe4 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -39,7 +39,7 @@ class OrbitObserver : public Component, public logger::ILoggable { * @param [in] error_standard_deviation: Position and Velocity standard deviation noise [m, m/s] * @param [in] orbit: Orbit information */ - OrbitObserver(const int prescaler, environment::ClockGenerator* clock_generator, const NoiseFrame noise_frame, const s2e::math::Vector<6> error_standard_deviation, + OrbitObserver(const int prescaler, environment::ClockGenerator* clock_generator, const NoiseFrame noise_frame, const math::Vector<6> error_standard_deviation, const Orbit& orbit); /** @@ -71,17 +71,17 @@ class OrbitObserver : public Component, public logger::ILoggable { * @fn GetPosition_i_m * @brief Return observed position */ - inline const s2e::math::Vector<3> GetPosition_i_m() const { return observed_position_i_m_; }; + inline const math::Vector<3> GetPosition_i_m() const { return observed_position_i_m_; }; /** * @fn GetVelocity_i_m_s * @brief Return observed velocity */ - inline const s2e::math::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; }; + inline const math::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; }; protected: - s2e::math::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] - s2e::math::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] + math::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] + math::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] NoiseFrame noise_frame_; //!< Noise definition frame s2e::randomization::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] diff --git a/src/components/ideal/torque_generator.cpp b/src/components/ideal/torque_generator.cpp index 93b312270..aef542808 100644 --- a/src/components/ideal/torque_generator.cpp +++ b/src/components/ideal/torque_generator.cpp @@ -31,9 +31,9 @@ void TorqueGenerator::MainRoutine(const int time_count) { double norm_ordered_torque = ordered_torque_b_Nm_.CalcNorm(); if (norm_ordered_torque > 0.0 + DBL_EPSILON) { // Add noise only when the torque is generated - s2e::math::Vector<3> true_direction = generated_torque_b_Nm_.CalcNormalizedVector(); - s2e::math::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_); - s2e::math::Vector<3> converted_direction = error_quaternion.FrameConversion(true_direction); + math::Vector<3> true_direction = generated_torque_b_Nm_.CalcNormalizedVector(); + math::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_); + math::Vector<3> converted_direction = error_quaternion.FrameConversion(true_direction); double torque_norm_with_error = norm_ordered_torque + magnitude_noise_; generated_torque_b_Nm_ = torque_norm_with_error * converted_direction; } @@ -60,14 +60,14 @@ std::string TorqueGenerator::GetLogValue() const { return str_tmp; } -s2e::math::Quaternion TorqueGenerator::GenerateDirectionNoiseQuaternion(s2e::math::Vector<3> true_direction, const double error_standard_deviation_rad) { - s2e::math::Vector<3> random_direction; +math::Quaternion TorqueGenerator::GenerateDirectionNoiseQuaternion(math::Vector<3> true_direction, const double error_standard_deviation_rad) { + math::Vector<3> random_direction; random_direction[0] = direction_noise_; random_direction[1] = direction_noise_; random_direction[2] = direction_noise_; random_direction = random_direction.CalcNormalizedVector(); - s2e::math::Vector<3> rotation_axis; + math::Vector<3> rotation_axis; rotation_axis = OuterProduct(true_direction, random_direction); double norm_rotation_axis = rotation_axis.CalcNorm(); if (norm_rotation_axis < 0.0 + DBL_EPSILON) { @@ -76,7 +76,7 @@ s2e::math::Quaternion TorqueGenerator::GenerateDirectionNoiseQuaternion(s2e::mat } double error_angle_rad = direction_noise_ * error_standard_deviation_rad; - s2e::math::Quaternion error_quaternion(rotation_axis, error_angle_rad); + math::Quaternion error_quaternion(rotation_axis, error_angle_rad); return error_quaternion; } @@ -92,7 +92,7 @@ TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_gen char section[30] = "TORQUE_GENERATOR"; double torque_magnitude_standard_deviation_Nm = ini_file.ReadDouble(section, "torque_magnitude_standard_deviation_Nm"); double torque_direction_standard_deviation_deg = ini_file.ReadDouble(section, "torque_direction_standard_deviation_deg"); - double torque_direction_standard_deviation_rad = s2e::math::deg_to_rad * torque_direction_standard_deviation_deg; + double torque_direction_standard_deviation_rad = math::deg_to_rad * torque_direction_standard_deviation_deg; TorqueGenerator torque_generator(prescaler, clock_generator, torque_magnitude_standard_deviation_Nm, torque_direction_standard_deviation_rad, dynamics); diff --git a/src/components/ideal/torque_generator.hpp b/src/components/ideal/torque_generator.hpp index 7b5144729..1c4965b11 100644 --- a/src/components/ideal/torque_generator.hpp +++ b/src/components/ideal/torque_generator.hpp @@ -73,11 +73,11 @@ class TorqueGenerator : public Component, public logger::ILoggable { * @fn SetTorque_b_Nm * @brief Set ordered torque in the body fixed frame [Nm] */ - inline void SetTorque_b_Nm(const s2e::math::Vector<3> torque_b_Nm) { ordered_torque_b_Nm_ = torque_b_Nm; }; + inline void SetTorque_b_Nm(const math::Vector<3> torque_b_Nm) { ordered_torque_b_Nm_ = torque_b_Nm; }; protected: - s2e::math::Vector<3> ordered_torque_b_Nm_{0.0}; //!< Ordered torque in the body fixed frame [Nm] - s2e::math::Vector<3> generated_torque_b_Nm_{0.0}; //!< Generated torque in the body fixed frame [Nm] + math::Vector<3> ordered_torque_b_Nm_{0.0}; //!< Ordered torque in the body fixed frame [Nm] + math::Vector<3> generated_torque_b_Nm_{0.0}; //!< Generated torque in the body fixed frame [Nm] // Noise s2e::randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise @@ -90,7 +90,7 @@ class TorqueGenerator : public Component, public logger::ILoggable { * @param [in] true_direction: True direction * @param [in] error_standard_deviation_rad: Standard deviation of direction error [rad] */ - s2e::math::Quaternion GenerateDirectionNoiseQuaternion(s2e::math::Vector<3> true_direction, const double error_standard_deviation_rad); + math::Quaternion GenerateDirectionNoiseQuaternion(math::Vector<3> true_direction, const double error_standard_deviation_rad); const dynamics::Dynamics* dynamics_; //!< Spacecraft dynamics information }; diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 00ba3fd58..033a8db5b 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -14,9 +14,9 @@ namespace s2e::components { GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, - const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, - const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, - const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, + const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg, + const math::Vector<3> position_noise_standard_deviation_ecef_m, + const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator), component_id_(component_id), @@ -34,9 +34,9 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo } GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, - const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, - const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, + const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, + const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m, + const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator, power_port), component_id_(component_id), @@ -58,8 +58,8 @@ void GnssReceiver::MainRoutine(const int time_count) { // Antenna checking // TODO: Use ECEF position only - s2e::math::Vector<3> position_true_eci = dynamics_->GetOrbit().GetPosition_i_m(); - s2e::math::Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + math::Vector<3> position_true_eci = dynamics_->GetOrbit().GetPosition_i_m(); + math::Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); CheckAntenna(position_true_eci, quaternion_i2b); if (is_gnss_visible_) { @@ -76,7 +76,7 @@ void GnssReceiver::MainRoutine(const int time_count) { ConvertJulianDayToGpsTime(simulation_time_->GetCurrentTime_jd()); } -void GnssReceiver::CheckAntenna(const s2e::math::Vector<3> position_true_eci_m, const s2e::math::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntenna(const math::Vector<3> position_true_eci_m, const math::Quaternion quaternion_i2b) { if (antenna_model_ == AntennaModel::kSimple) { CheckAntennaSimple(position_true_eci_m, quaternion_i2b); } else if (antenna_model_ == AntennaModel::kCone) { @@ -86,15 +86,15 @@ void GnssReceiver::CheckAntenna(const s2e::math::Vector<3> position_true_eci_m, } } -void GnssReceiver::CheckAntennaSimple(const s2e::math::Vector<3> position_true_eci_m, const s2e::math::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntennaSimple(const math::Vector<3> position_true_eci_m, const math::Quaternion quaternion_i2b) { // Simplest model // GNSS satellites are visible when antenna directs anti-earth direction // Antenna normal vector at inertial frame - s2e::math::Vector<3> antenna_direction_c(0.0); + math::Vector<3> antenna_direction_c(0.0); antenna_direction_c[2] = 1.0; - s2e::math::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); - s2e::math::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); + math::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); + math::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); double inner = InnerProduct(position_true_eci_m, antenna_direction_i); if (inner <= 0.0) { @@ -104,18 +104,18 @@ void GnssReceiver::CheckAntennaSimple(const s2e::math::Vector<3> position_true_e } } -void GnssReceiver::CheckAntennaCone(const s2e::math::Vector<3> position_true_eci_m, const s2e::math::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntennaCone(const math::Vector<3> position_true_eci_m, const math::Quaternion quaternion_i2b) { // Cone model gnss_information_list_.clear(); // Antenna pointing direction vector at inertial frame - s2e::math::Vector<3> antenna_pointing_direction_c(0.0); + math::Vector<3> antenna_pointing_direction_c(0.0); antenna_pointing_direction_c[2] = 1.0; - s2e::math::Vector<3> antenna_pointing_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_pointing_direction_c); - s2e::math::Vector<3> antenna_pointing_direction_i = quaternion_i2b.InverseFrameConversion(antenna_pointing_direction_b); + math::Vector<3> antenna_pointing_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_pointing_direction_c); + math::Vector<3> antenna_pointing_direction_i = quaternion_i2b.InverseFrameConversion(antenna_pointing_direction_b); // Antenna position vector at inertial frame - s2e::math::Vector<3> antenna_position_i_m = position_true_eci_m + quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); + math::Vector<3> antenna_position_i_m = position_true_eci_m + quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); // initialize visible_satellite_number_ = 0; @@ -124,9 +124,9 @@ void GnssReceiver::CheckAntennaCone(const s2e::math::Vector<3> position_true_eci for (size_t i = 0; i < number_of_calculated_gnss_satellites; i++) { // compute direction from sat to gnss in body-fixed frame - s2e::math::Vector<3> gnss_satellite_position_i_m = gnss_satellites_->GetPosition_eci_m(i); - s2e::math::Vector<3> antenna_to_gnss_satellite_i_m = gnss_satellite_position_i_m - antenna_position_i_m; - s2e::math::Vector<3> antenna_to_gnss_satellite_direction_i = antenna_to_gnss_satellite_i_m.CalcNormalizedVector(); + math::Vector<3> gnss_satellite_position_i_m = gnss_satellites_->GetPosition_eci_m(i); + math::Vector<3> antenna_to_gnss_satellite_i_m = gnss_satellite_position_i_m - antenna_position_i_m; + math::Vector<3> antenna_to_gnss_satellite_direction_i = antenna_to_gnss_satellite_i_m.CalcNormalizedVector(); // Check GNSS satellites are visible from the receiver(not care antenna direction) bool is_gnss_satellite_visible_from_receiver = false; @@ -149,7 +149,7 @@ void GnssReceiver::CheckAntennaCone(const s2e::math::Vector<3> position_true_eci // Check GNSS satellites are in the antenna half width angle double inner2 = InnerProduct(antenna_pointing_direction_i, antenna_to_gnss_satellite_direction_i); - if (inner2 > cos(half_width_deg_ * s2e::math::deg_to_rad) && is_gnss_satellite_visible_from_receiver) { + if (inner2 > cos(half_width_deg_ * math::deg_to_rad) && is_gnss_satellite_visible_from_receiver) { // is visible visible_satellite_number_++; SetGnssInfo(antenna_to_gnss_satellite_i_m, quaternion_i2b, i); @@ -163,10 +163,10 @@ void GnssReceiver::CheckAntennaCone(const s2e::math::Vector<3> position_true_eci } } -void GnssReceiver::SetGnssInfo(const s2e::math::Vector<3> antenna_to_satellite_i_m, const s2e::math::Quaternion quaternion_i2b, +void GnssReceiver::SetGnssInfo(const math::Vector<3> antenna_to_satellite_i_m, const math::Quaternion quaternion_i2b, const std::size_t gnss_system_id) { - s2e::math::Vector<3> antenna_to_satellite_direction_b = quaternion_i2b.FrameConversion(antenna_to_satellite_i_m); - s2e::math::Vector<3> antenna_to_satellite_direction_c = quaternion_b2c_.FrameConversion(antenna_to_satellite_direction_b); + math::Vector<3> antenna_to_satellite_direction_b = quaternion_i2b.FrameConversion(antenna_to_satellite_i_m); + math::Vector<3> antenna_to_satellite_direction_c = quaternion_b2c_.FrameConversion(antenna_to_satellite_direction_b); double distance_m = antenna_to_satellite_i_m.CalcNorm(); double longitude_rad = AcTan(antenna_to_satellite_direction_c[1], antenna_to_satellite_direction_c[0]); @@ -177,7 +177,7 @@ void GnssReceiver::SetGnssInfo(const s2e::math::Vector<3> antenna_to_satellite_i gnss_information_list_.push_back(gnss_info_new); } -void GnssReceiver::AddNoise(const s2e::math::Vector<3> position_true_ecef_m, const s2e::math::Vector<3> velocity_true_ecef_m_s) { +void GnssReceiver::AddNoise(const math::Vector<3> position_true_ecef_m, const math::Vector<3> velocity_true_ecef_m_s) { for (size_t i = 0; i < 3; i++) { position_ecef_m_[i] = position_true_ecef_m[i] + position_random_noise_ecef_m_[i]; velocity_ecef_m_s_[i] = velocity_true_ecef_m_s[i] + velocity_random_noise_ecef_m_s_[i]; @@ -202,19 +202,19 @@ std::string GnssReceiver::GetLogHeader() const // For logs const std::string sensor_id = std::to_string(static_cast(component_id_)); std::string sensor_name = "gnss_receiver" + sensor_id + "_"; - str_tmp += WriteScalar(sensor_name + "measured_utc_time_year"); - str_tmp += WriteScalar(sensor_name + "measured_utc_time_month"); - str_tmp += WriteScalar(sensor_name + "measured_utc_time_day"); - str_tmp += WriteScalar(sensor_name + "measured_utc_time_hour"); - str_tmp += WriteScalar(sensor_name + "measured_utc_time_min"); - str_tmp += WriteScalar(sensor_name + "measured_utc_time_sec"); + str_tmp += logger::WriteScalar(sensor_name + "measured_utc_time_year"); + str_tmp += logger::WriteScalar(sensor_name + "measured_utc_time_month"); + str_tmp += logger::WriteScalar(sensor_name + "measured_utc_time_day"); + str_tmp += logger::WriteScalar(sensor_name + "measured_utc_time_hour"); + str_tmp += logger::WriteScalar(sensor_name + "measured_utc_time_min"); + str_tmp += logger::WriteScalar(sensor_name + "measured_utc_time_sec"); str_tmp += logger::WriteVector(sensor_name + "measured_position", "ecef", "m", 3); str_tmp += logger::WriteVector(sensor_name + "measured_velocity", "ecef", "m/s", 3); - str_tmp += WriteScalar(sensor_name + "measured_latitude", "rad"); - str_tmp += WriteScalar(sensor_name + "measured_longitude", "rad"); - str_tmp += WriteScalar(sensor_name + "measured_altitude", "m"); - str_tmp += WriteScalar(sensor_name + "satellite_visible_flag"); - str_tmp += WriteScalar(sensor_name + "number_of_visible_satellites"); + str_tmp += logger::WriteScalar(sensor_name + "measured_latitude", "rad"); + str_tmp += logger::WriteScalar(sensor_name + "measured_longitude", "rad"); + str_tmp += logger::WriteScalar(sensor_name + "measured_altitude", "m"); + str_tmp += logger::WriteScalar(sensor_name + "satellite_visible_flag"); + str_tmp += logger::WriteScalar(sensor_name + "number_of_visible_satellites"); return str_tmp; } @@ -222,19 +222,19 @@ std::string GnssReceiver::GetLogHeader() const // For logs std::string GnssReceiver::GetLogValue() const // For logs { std::string str_tmp = ""; - str_tmp += WriteScalar(utc_.year); - str_tmp += WriteScalar(utc_.month); - str_tmp += WriteScalar(utc_.day); - str_tmp += WriteScalar(utc_.hour); - str_tmp += WriteScalar(utc_.minute); - str_tmp += WriteScalar(utc_.second); + str_tmp += logger::WriteScalar(utc_.year); + str_tmp += logger::WriteScalar(utc_.month); + str_tmp += logger::WriteScalar(utc_.day); + str_tmp += logger::WriteScalar(utc_.hour); + str_tmp += logger::WriteScalar(utc_.minute); + str_tmp += logger::WriteScalar(utc_.second); str_tmp += logger::WriteVector(position_ecef_m_, 10); str_tmp += logger::WriteVector(velocity_ecef_m_s_, 10); - str_tmp += WriteScalar(geodetic_position_.GetLatitude_rad(), 10); - str_tmp += WriteScalar(geodetic_position_.GetLongitude_rad(), 10); - str_tmp += WriteScalar(geodetic_position_.GetAltitude_m(), 10); - str_tmp += WriteScalar(is_gnss_visible_); - str_tmp += WriteScalar(visible_satellite_number_); + str_tmp += logger::WriteScalar(geodetic_position_.GetLatitude_rad(), 10); + str_tmp += logger::WriteScalar(geodetic_position_.GetLongitude_rad(), 10); + str_tmp += logger::WriteScalar(geodetic_position_.GetAltitude_m(), 10); + str_tmp += logger::WriteScalar(is_gnss_visible_); + str_tmp += logger::WriteScalar(visible_satellite_number_); return str_tmp; } @@ -254,11 +254,11 @@ AntennaModel SetAntennaModel(const std::string antenna_model) { typedef struct _gnss_receiver_param { int prescaler; AntennaModel antenna_model; - s2e::math::Vector<3> antenna_pos_b; - s2e::math::Quaternion quaternion_b2c; + math::Vector<3> antenna_pos_b; + math::Quaternion quaternion_b2c; double half_width_deg; - s2e::math::Vector<3> position_noise_standard_deviation_ecef_m; - s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s; + math::Vector<3> position_noise_standard_deviation_ecef_m; + math::Vector<3> velocity_noise_standard_deviation_ecef_m_s; } GnssReceiverParam; GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const size_t component_id) { diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index e96f05f0f..aa2e54f09 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -61,8 +61,8 @@ class GnssReceiver : public Component, public logger::ILoggable { * @param [in] simulation_time: Simulation time information */ GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, - const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, const double half_width_deg, - const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, + const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg, + const math::Vector<3> position_noise_standard_deviation_ecef_m, const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn GnssReceiver @@ -81,9 +81,9 @@ class GnssReceiver : public Component, public logger::ILoggable { * @param [in] simulation_time: Simulation time information */ GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const AntennaModel antenna_model, const s2e::math::Vector<3> antenna_position_b_m, const s2e::math::Quaternion quaternion_b2c, - const double half_width_deg, const s2e::math::Vector<3> position_noise_standard_deviation_ecef_m, - const s2e::math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, + const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, + const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m, + const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); // Override functions for Component @@ -104,7 +104,7 @@ class GnssReceiver : public Component, public logger::ILoggable { * @fn GetMeasuredPosition_ecef_m * @brief Return Observed position in the ECEF frame [m] */ - inline const s2e::math::Vector<3> GetMeasuredPosition_ecef_m(void) const { return position_ecef_m_; } + inline const math::Vector<3> GetMeasuredPosition_ecef_m(void) const { return position_ecef_m_; } /** * @fn GetMeasuredGeodeticPosition * @brief Return Observed position in the LLH frame [m] @@ -114,7 +114,7 @@ class GnssReceiver : public Component, public logger::ILoggable { * @fn GetMeasuredVelocity_ecef_m_s * @brief Return Observed velocity in the ECEF frame [m/s] */ - inline const s2e::math::Vector<3> GetMeasuredVelocity_ecef_m_s(void) const { return velocity_ecef_m_s_; } + inline const math::Vector<3> GetMeasuredVelocity_ecef_m_s(void) const { return velocity_ecef_m_s_; } // Override logger::ILoggable /** @@ -133,16 +133,16 @@ class GnssReceiver : public Component, public logger::ILoggable { const size_t component_id_; //!< Receiver ID // Antenna - s2e::math::Vector<3> antenna_position_b_m_; //!< GNSS antenna position at the body-fixed frame [m] - s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (antenna frame) + math::Vector<3> antenna_position_b_m_; //!< GNSS antenna position at the body-fixed frame [m] + math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (antenna frame) double half_width_deg_ = 0.0; //!< Half width of the antenna cone model [deg] AntennaModel antenna_model_; //!< Antenna model // Simple position observation s2e::randomization::NormalRand position_random_noise_ecef_m_[3]; //!< Random noise for position at the ECEF frame [m] s2e::randomization::NormalRand velocity_random_noise_ecef_m_s_[3]; //!< Random noise for velocity at the ECEF frame [m] - s2e::math::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] - s2e::math::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] + math::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] + math::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] s2e::geodesy::GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame // Time observation @@ -168,7 +168,7 @@ class GnssReceiver : public Component, public logger::ILoggable { * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame */ - void CheckAntenna(const s2e::math::Vector<3> position_true_i_m, const s2e::math::Quaternion quaternion_i2b); + void CheckAntenna(const math::Vector<3> position_true_i_m, const math::Quaternion quaternion_i2b); /** * @fn CheckAntennaSimple * @brief Check the antenna can detect GNSS signal with Simple mode @@ -176,7 +176,7 @@ class GnssReceiver : public Component, public logger::ILoggable { * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame */ - void CheckAntennaSimple(const s2e::math::Vector<3> position_true_i_m, const s2e::math::Quaternion quaternion_i2b); + void CheckAntennaSimple(const math::Vector<3> position_true_i_m, const math::Quaternion quaternion_i2b); /** * @fn CheckAntennaCone * @brief Check the antenna can detect GNSS signal with Cone mode @@ -184,7 +184,7 @@ class GnssReceiver : public Component, public logger::ILoggable { * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame */ - void CheckAntennaCone(const s2e::math::Vector<3> position_true_i_m, const s2e::math::Quaternion quaternion_i2b); + void CheckAntennaCone(const math::Vector<3> position_true_i_m, const math::Quaternion quaternion_i2b); /** * @fn SetGnssInfo * @brief Calculate and set the GnssInfo values of target GNSS satellite @@ -192,14 +192,14 @@ class GnssReceiver : public Component, public logger::ILoggable { * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame * @param [in] gnss_system_id: ID of target GNSS satellite */ - void SetGnssInfo(const s2e::math::Vector<3> antenna_to_satellite_i_m, const s2e::math::Quaternion quaternion_i2b, const size_t gnss_system_id); + void SetGnssInfo(const math::Vector<3> antenna_to_satellite_i_m, const math::Quaternion quaternion_i2b, const size_t gnss_system_id); /** * @fn AddNoise * @brief Substitutional method for "Measure" in other sensor models inherited Sensor class * @param [in] position_true_ecef_m: True position of the spacecraft in the ECEF frame [m] * @param [in] velocity_true_ecef_m_s: True velocity of the spacecraft in the ECEF frame [m/s] */ - void AddNoise(const s2e::math::Vector<3> position_true_ecef_m, const s2e::math::Vector<3> velocity_true_ecef_m_s); + void AddNoise(const math::Vector<3> position_true_ecef_m, const math::Vector<3> velocity_true_ecef_m_s); /** * @fn ConvertJulianDayToGpsTime * @brief Convert Julian day to GPS time diff --git a/src/components/real/aocs/gyro_sensor.cpp b/src/components/real/aocs/gyro_sensor.cpp index 4776b6701..ac63221d7 100644 --- a/src/components/real/aocs/gyro_sensor.cpp +++ b/src/components/real/aocs/gyro_sensor.cpp @@ -10,11 +10,11 @@ namespace s2e::components { GyroSensor::GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics) + const math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), dynamics_(dynamics) {} GyroSensor::GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics) + const math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator, power_port), Sensor(sensor_base), sensor_id_(sensor_id), @@ -54,7 +54,7 @@ GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, int sens const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); const char* GSection = section_name.c_str(); - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; gyro_conf.ReadQuaternion(GSection, "quaternion_b2c", quaternion_b2c); int prescaler = gyro_conf.ReadInt(GSection, "prescaler"); if (prescaler <= 1) prescaler = 1; @@ -75,7 +75,7 @@ GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, PowerPor const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); const char* GSection = section_name.c_str(); - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; gyro_conf.ReadQuaternion(GSection, "quaternion_b2c", quaternion_b2c); int prescaler = gyro_conf.ReadInt(GSection, "prescaler"); if (prescaler <= 1) prescaler = 1; diff --git a/src/components/real/aocs/gyro_sensor.hpp b/src/components/real/aocs/gyro_sensor.hpp index f4baaad1e..d9f262655 100644 --- a/src/components/real/aocs/gyro_sensor.hpp +++ b/src/components/real/aocs/gyro_sensor.hpp @@ -34,7 +34,7 @@ class GyroSensor : public Component, public Sensor, public logge * @param [in] dynamics: Dynamics information */ GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics); + const math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics); /** * @fn GyroSensor * @brief Constructor with power port @@ -47,7 +47,7 @@ class GyroSensor : public Component, public Sensor, public logge * @param [in] dynamics: Dynamics information */ GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics); + const math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics); /** * @fn ~GyroSensor * @brief Destructor @@ -77,13 +77,13 @@ class GyroSensor : public Component, public Sensor, public logge * @fn GetMeasuredAngularVelocity_c_rad_s * @brief Return observed angular velocity of the component frame with respect to the inertial frame */ - inline const s2e::math::Vector& GetMeasuredAngularVelocity_c_rad_s(void) const { return angular_velocity_c_rad_s_; } + inline const math::Vector& GetMeasuredAngularVelocity_c_rad_s(void) const { return angular_velocity_c_rad_s_; } protected: - s2e::math::Vector angular_velocity_c_rad_s_{ + math::Vector angular_velocity_c_rad_s_{ 0.0}; //!< Observed angular velocity of the component frame with respect to the inertial frame [rad/s] unsigned int sensor_id_ = 0; //!< Sensor ID - s2e::math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame + math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame const dynamics::Dynamics* dynamics_; //!< Dynamics information }; diff --git a/src/components/real/aocs/magnetometer.cpp b/src/components/real/aocs/magnetometer.cpp index dba5c8f45..275d7fb1f 100644 --- a/src/components/real/aocs/magnetometer.cpp +++ b/src/components/real/aocs/magnetometer.cpp @@ -10,14 +10,14 @@ namespace s2e::components { Magnetometer::Magnetometer(int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) + const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), geomagnetic_field_(geomagnetic_field) {} Magnetometer::Magnetometer(int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) + const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator, power_port), Sensor(sensor_base), sensor_id_(sensor_id), @@ -59,7 +59,7 @@ Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, int int prescaler = magsensor_conf.ReadInt(MSSection, "prescaler"); if (prescaler <= 1) prescaler = 1; - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; magsensor_conf.ReadQuaternion(MSSection, "quaternion_b2c", quaternion_b2c); // Sensor @@ -80,7 +80,7 @@ Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, Powe int prescaler = magsensor_conf.ReadInt(MSSection, "prescaler"); if (prescaler <= 1) prescaler = 1; - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; magsensor_conf.ReadQuaternion(MSSection, "quaternion_b2c", quaternion_b2c); // Sensor diff --git a/src/components/real/aocs/magnetometer.hpp b/src/components/real/aocs/magnetometer.hpp index ab3c5cf85..896f24a3b 100644 --- a/src/components/real/aocs/magnetometer.hpp +++ b/src/components/real/aocs/magnetometer.hpp @@ -34,7 +34,7 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] geomagnetic_field: Geomagnetic environment */ Magnetometer(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); + const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); /** * @fn Magnetometer * @brief Constructor with power port @@ -47,7 +47,7 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] geomagnetic_field: Geomagnetic environment */ Magnetometer(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const s2e::math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); + const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); /** * @fn ~Magnetometer * @brief Destructor @@ -77,32 +77,32 @@ class Magnetometer : public Component, public Sensor, pu * @fn GetMeasuredMagneticField_c_nT * @brief Return observed magnetic field on the component frame */ - inline const s2e::math::Vector& GetMeasuredMagneticField_c_nT(void) const { return magnetic_field_c_nT_; } + inline const math::Vector& GetMeasuredMagneticField_c_nT(void) const { return magnetic_field_c_nT_; } /** * @fn SetConstantBiasNoise_c_nT * @brief Set constant bias noise at component frame [nT] * @param [in] constant_noise_c_nT: Constant bias noise at component frame [nT] */ - inline void SetConstantBiasNoise_c_nT(s2e::math::Vector constant_noise_c_nT) { bias_noise_c_ = constant_noise_c_nT; } + inline void SetConstantBiasNoise_c_nT(math::Vector constant_noise_c_nT) { bias_noise_c_ = constant_noise_c_nT; } /** * @fn AddConstantBiasNoise_c_nT * @brief Add constant bias noise at component frame [nT] * @param [in] constant_noise_c_nT: Additional constant bias noise at component frame [nT] */ - inline void AddConstantBiasNoise_c_nT(s2e::math::Vector constant_noise_c_nT) { bias_noise_c_ += constant_noise_c_nT; } + inline void AddConstantBiasNoise_c_nT(math::Vector constant_noise_c_nT) { bias_noise_c_ += constant_noise_c_nT; } /** * @fn GetConstantBiasNoise_c_nT * @brief Get constant bias noise at component frame [nT] */ - inline s2e::math::Vector GetConstantBiasNoise_c_nT() const { return bias_noise_c_; } + inline math::Vector GetConstantBiasNoise_c_nT() const { return bias_noise_c_; } protected: - s2e::math::Vector magnetic_field_c_nT_{0.0}; //!< Observed magnetic field on the component frame [nT] + math::Vector magnetic_field_c_nT_{0.0}; //!< Observed magnetic field on the component frame [nT] unsigned int sensor_id_ = 0; //!< Sensor ID - s2e::math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame + math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame const GeomagneticField* geomagnetic_field_; //!< Geomagnetic environment }; diff --git a/src/components/real/aocs/magnetorquer.cpp b/src/components/real/aocs/magnetorquer.cpp index 863d1ce5b..7d1352986 100644 --- a/src/components/real/aocs/magnetorquer.cpp +++ b/src/components/real/aocs/magnetorquer.cpp @@ -13,13 +13,13 @@ namespace s2e::components { -Magnetorquer::Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, - const s2e::math::Matrix& scale_factor, - const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, - const s2e::math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, - const s2e::math::Vector& random_walk_standard_deviation_c_Am2, - const s2e::math::Vector& random_walk_limit_c_Am2, - const s2e::math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field) +Magnetorquer::Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, + const math::Matrix& scale_factor, + const math::Vector& max_magnetic_moment_c_Am2, const math::Vector& min_magnetic_moment_c_Am2, + const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, + const math::Vector& random_walk_standard_deviation_c_Am2, + const math::Vector& random_walk_limit_c_Am2, + const math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -36,12 +36,12 @@ Magnetorquer::Magnetorquer(const int prescaler, environment::ClockGenerator* clo } Magnetorquer::Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const s2e::math::Quaternion& quaternion_b2c, const s2e::math::Matrix& scale_factor, - const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, - const s2e::math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, - const s2e::math::Vector& random_walk_standard_deviation_c_Am2, - const s2e::math::Vector& random_walk_limit_c_Am2, - const s2e::math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field) + const math::Quaternion& quaternion_b2c, const math::Matrix& scale_factor, + const math::Vector& max_magnetic_moment_c_Am2, const math::Vector& min_magnetic_moment_c_Am2, + const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, + const math::Vector& random_walk_standard_deviation_c_Am2, + const math::Vector& random_walk_limit_c_Am2, + const math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator, power_port), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -69,7 +69,7 @@ void Magnetorquer::PowerOffRoutine() { output_magnetic_moment_b_Am2_ *= 0.0; } -s2e::math::Vector Magnetorquer::CalcOutputTorque(void) { +math::Vector Magnetorquer::CalcOutputTorque(void) { for (size_t i = 0; i < kMtqDimension; ++i) { // Limit Check if (output_magnetic_moment_c_Am2_[i] > max_magnetic_moment_c_Am2_[i]) { @@ -123,33 +123,33 @@ Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int int prescaler = magtorquer_conf.ReadInt(MTSection, "prescaler"); if (prescaler <= 1) prescaler = 1; - s2e::math::Vector sf_vec; + math::Vector sf_vec; magtorquer_conf.ReadVector(MTSection, "scale_factor_c", sf_vec); - s2e::math::Matrix scale_factor; + math::Matrix scale_factor; for (size_t i = 0; i < kMtqDimension; i++) { for (size_t j = 0; j < kMtqDimension; j++) { scale_factor[i][j] = sf_vec[i * kMtqDimension + j]; } } - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; magtorquer_conf.ReadQuaternion(MTSection, "quaternion_b2c", quaternion_b2c); - s2e::math::Vector max_magnetic_moment_c_Am2; + math::Vector max_magnetic_moment_c_Am2; magtorquer_conf.ReadVector(MTSection, "max_output_magnetic_moment_c_Am2", max_magnetic_moment_c_Am2); - s2e::math::Vector min_magnetic_moment_c_Am2; + math::Vector min_magnetic_moment_c_Am2; magtorquer_conf.ReadVector(MTSection, "min_output_magnetic_moment_c_Am2", min_magnetic_moment_c_Am2); - s2e::math::Vector bias_noise_c_Am2; + math::Vector bias_noise_c_Am2; magtorquer_conf.ReadVector(MTSection, "constant_bias_noise_c_Am2", bias_noise_c_Am2); double random_walk_step_width_s = component_step_time_s * (double)prescaler; - s2e::math::Vector random_walk_standard_deviation_c_Am2; + math::Vector random_walk_standard_deviation_c_Am2; magtorquer_conf.ReadVector(MTSection, "random_walk_standard_deviation_c_Am2", random_walk_standard_deviation_c_Am2); - s2e::math::Vector random_walk_limit_c_Am2; + math::Vector random_walk_limit_c_Am2; magtorquer_conf.ReadVector(MTSection, "random_walk_limit_c_Am2", random_walk_limit_c_Am2); - s2e::math::Vector normal_random_standard_deviation_c_Am2; + math::Vector normal_random_standard_deviation_c_Am2; magtorquer_conf.ReadVector(MTSection, "white_noise_standard_deviation_c_Am2", normal_random_standard_deviation_c_Am2); Magnetorquer magtorquer(prescaler, clock_generator, actuator_id, quaternion_b2c, scale_factor, max_magnetic_moment_c_Am2, min_magnetic_moment_c_Am2, @@ -168,33 +168,33 @@ Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, Powe int prescaler = magtorquer_conf.ReadInt(MTSection, "prescaler"); if (prescaler <= 1) prescaler = 1; - s2e::math::Vector sf_vec; + math::Vector sf_vec; magtorquer_conf.ReadVector(MTSection, "scale_factor_c", sf_vec); - s2e::math::Matrix scale_factor; + math::Matrix scale_factor; for (size_t i = 0; i < kMtqDimension; i++) { for (size_t j = 0; j < kMtqDimension; j++) { scale_factor[i][j] = sf_vec[i * kMtqDimension + j]; } } - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; magtorquer_conf.ReadQuaternion(MTSection, "quaternion_b2c", quaternion_b2c); - s2e::math::Vector max_magnetic_moment_c_Am2; + math::Vector max_magnetic_moment_c_Am2; magtorquer_conf.ReadVector(MTSection, "max_output_magnetic_moment_c_Am2", max_magnetic_moment_c_Am2); - s2e::math::Vector min_magnetic_moment_c_Am2; + math::Vector min_magnetic_moment_c_Am2; magtorquer_conf.ReadVector(MTSection, "min_output_magnetic_moment_c_Am2", min_magnetic_moment_c_Am2); - s2e::math::Vector bias_noise_c_Am2; + math::Vector bias_noise_c_Am2; magtorquer_conf.ReadVector(MTSection, "constant_bias_noise_c_Am2", bias_noise_c_Am2); double random_walk_step_width_s = component_step_time_s * (double)prescaler; - s2e::math::Vector random_walk_standard_deviation_c_Am2; + math::Vector random_walk_standard_deviation_c_Am2; magtorquer_conf.ReadVector(MTSection, "random_walk_standard_deviation_c_Am2", random_walk_standard_deviation_c_Am2); - s2e::math::Vector random_walk_limit_c_Am2; + math::Vector random_walk_limit_c_Am2; magtorquer_conf.ReadVector(MTSection, "random_walk_limit_c_Am2", random_walk_limit_c_Am2); - s2e::math::Vector normal_random_standard_deviation_c_Am2; + math::Vector normal_random_standard_deviation_c_Am2; magtorquer_conf.ReadVector(MTSection, "white_noise_standard_deviation_c_Am2", normal_random_standard_deviation_c_Am2); // PowerPort diff --git a/src/components/real/aocs/magnetorquer.hpp b/src/components/real/aocs/magnetorquer.hpp index c5c1688bf..cec1294dd 100644 --- a/src/components/real/aocs/magnetorquer.hpp +++ b/src/components/real/aocs/magnetorquer.hpp @@ -43,11 +43,11 @@ class Magnetorquer : public Component, public logger::ILoggable { * @param [in] normal_random_standard_deviation_c_Am2: Standard deviation for the normal random noise in the component frame [Am2] * @param [in] geomagnetic_field: Geomagnetic environment */ - Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, - const s2e::math::Matrix& scale_factor, const s2e::math::Vector& max_magnetic_moment_c_Am2, - const s2e::math::Vector& min_magnetic_moment_c_Am2, const s2e::math::Vector& bias_noise_c_Am2_, - double random_walk_step_width_s, const s2e::math::Vector& random_walk_standard_deviation_c_Am2, - const s2e::math::Vector& random_walk_limit_c_Am2, const s2e::math::Vector& normal_random_standard_deviation_c_Am2, + Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, + const math::Matrix& scale_factor, const math::Vector& max_magnetic_moment_c_Am2, + const math::Vector& min_magnetic_moment_c_Am2, const math::Vector& bias_noise_c_Am2_, + double random_walk_step_width_s, const math::Vector& random_walk_standard_deviation_c_Am2, + const math::Vector& random_walk_limit_c_Am2, const math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field); /** * @fn Magnetorquer @@ -68,11 +68,11 @@ class Magnetorquer : public Component, public logger::ILoggable { * @param [in] geomagnetic_field: Geomagnetic environment */ Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const s2e::math::Quaternion& quaternion_b2c, const s2e::math::Matrix& scale_factor, - const s2e::math::Vector& max_magnetic_moment_c_Am2, const s2e::math::Vector& min_magnetic_moment_c_Am2, - const s2e::math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, - const s2e::math::Vector& random_walk_standard_deviation_c_Am2, const s2e::math::Vector& random_walk_limit_c_Am2, - const s2e::math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field); + const math::Quaternion& quaternion_b2c, const math::Matrix& scale_factor, + const math::Vector& max_magnetic_moment_c_Am2, const math::Vector& min_magnetic_moment_c_Am2, + const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, + const math::Vector& random_walk_standard_deviation_c_Am2, const math::Vector& random_walk_limit_c_Am2, + const math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field); // Override functions for Component /** @@ -102,39 +102,39 @@ class Magnetorquer : public Component, public logger::ILoggable { * @fn GetOutputTorque_b_Nm * @brief Return output torque in the body fixed frame [Nm] */ - inline const s2e::math::Vector& GetOutputTorque_b_Nm(void) const { return torque_b_Nm_; }; + inline const math::Vector& GetOutputTorque_b_Nm(void) const { return torque_b_Nm_; }; /** * @fn SetOutputMagneticMoment_c_Am2 * @brief Set output magnetic moment in the component frame [Am2] */ - inline void SetOutputMagneticMoment_c_Am2(const s2e::math::Vector mag_moment_c) { output_magnetic_moment_c_Am2_ = mag_moment_c; }; + inline void SetOutputMagneticMoment_c_Am2(const math::Vector mag_moment_c) { output_magnetic_moment_c_Am2_ = mag_moment_c; }; /** * @fn SetOutputMagneticMoment_b_Am2 * @brief Return output magnetic moment in the body fixed frame [Am2] */ - inline const s2e::math::Vector& SetOutputMagneticMoment_b_Am2(void) const { return output_magnetic_moment_b_Am2_; }; + inline const math::Vector& SetOutputMagneticMoment_b_Am2(void) const { return output_magnetic_moment_b_Am2_; }; /** * @fn GetOutputMagneticMoment_b_Am2 * @brief Return output magnetic moment in the body fixed frame [Am2] */ - inline const s2e::math::Vector& GetOutputMagneticMoment_c_Am2(void) const { return output_magnetic_moment_c_Am2_; }; + inline const math::Vector& GetOutputMagneticMoment_c_Am2(void) const { return output_magnetic_moment_c_Am2_; }; protected: const int component_id_ = 0; //!< Actuator ID const double kConvertNanoT2T = 1.0e-9; //!< Constant to convert nT to T - s2e::math::Vector torque_b_Nm_{0.0}; //!< Output torque in the body fixed frame [Nm] - s2e::math::Vector output_magnetic_moment_c_Am2_{0.0}; //!< Output output magnetic moment in the component frame [Am2] - s2e::math::Vector output_magnetic_moment_b_Am2_{0.0}; //!< Output output magnetic moment in the body fixed frame [Am2] - s2e::math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame - s2e::math::Quaternion quaternion_c2b_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from component frame to body frame - s2e::math::Matrix scale_factor_; //!< Scale factor matrix - s2e::math::Vector max_magnetic_moment_c_Am2_{100.0}; //!< Maximum magnetic moment in the component frame [Am2] - s2e::math::Vector min_magnetic_moment_c_Am2_{-100.0}; //!< Minimum magnetic moment in the component frame [Am2] - - s2e::math::Vector bias_noise_c_Am2_{0.0}; //!< Constant bias noise in the component frame [Am2] + math::Vector torque_b_Nm_{0.0}; //!< Output torque in the body fixed frame [Nm] + math::Vector output_magnetic_moment_c_Am2_{0.0}; //!< Output output magnetic moment in the component frame [Am2] + math::Vector output_magnetic_moment_b_Am2_{0.0}; //!< Output output magnetic moment in the body fixed frame [Am2] + math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame + math::Quaternion quaternion_c2b_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from component frame to body frame + math::Matrix scale_factor_; //!< Scale factor matrix + math::Vector max_magnetic_moment_c_Am2_{100.0}; //!< Maximum magnetic moment in the component frame [Am2] + math::Vector min_magnetic_moment_c_Am2_{-100.0}; //!< Minimum magnetic moment in the component frame [Am2] + + math::Vector bias_noise_c_Am2_{0.0}; //!< Constant bias noise in the component frame [Am2] RandomWalk random_walk_c_Am2_; //!< Random walk noise s2e::randomization::NormalRand random_noise_c_Am2_[kMtqDimension]; //!< Normal random noise @@ -145,7 +145,7 @@ class Magnetorquer : public Component, public logger::ILoggable { * @brief Calculate output torque * @return Output torque in the body fixed frame [Nm] */ - s2e::math::Vector CalcOutputTorque(void); + math::Vector CalcOutputTorque(void); }; /** diff --git a/src/components/real/aocs/mtq_magnetometer_interference.cpp b/src/components/real/aocs/mtq_magnetometer_interference.cpp index 7113ee869..249c6f299 100644 --- a/src/components/real/aocs/mtq_magnetometer_interference.cpp +++ b/src/components/real/aocs/mtq_magnetometer_interference.cpp @@ -20,8 +20,8 @@ MtqMagnetometerInterference::MtqMagnetometerInterference(const std::string file_ for (size_t degree = 1; degree <= polynomial_degree_; degree++) { const std::string key_name = "additional_bias_by_mtq_coefficients_" + std::to_string(static_cast(degree)); - s2e::math::Vector<9> additional_bias_by_mtq_coefficients_vec; - s2e::math::Matrix<3, 3> additional_bias_by_mtq_coefficients; + math::Vector<9> additional_bias_by_mtq_coefficients_vec; + math::Matrix<3, 3> additional_bias_by_mtq_coefficients; ini_file.ReadVector(section.c_str(), key_name.c_str(), additional_bias_by_mtq_coefficients_vec); for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { @@ -37,10 +37,10 @@ void MtqMagnetometerInterference::UpdateInterference(void) { magnetometer_.AddConstantBiasNoise_c_nT(-1.0 * previous_added_bias_c_nT_); // Calculate bias - s2e::math::Vector<3> additional_bias_c_nT{0.0}; - s2e::math::Vector<3> mtq_output_c_Am2 = magnetorquer_.GetOutputMagneticMoment_c_Am2(); + math::Vector<3> additional_bias_c_nT{0.0}; + math::Vector<3> mtq_output_c_Am2 = magnetorquer_.GetOutputMagneticMoment_c_Am2(); for (size_t degree = 1; degree <= polynomial_degree_; degree++) { - s2e::math::Vector<3> hadamard_mtq; + math::Vector<3> hadamard_mtq; for (size_t axis = 0; axis < 3; axis++) { hadamard_mtq[axis] = pow(mtq_output_c_Am2[axis], degree); } diff --git a/src/components/real/aocs/mtq_magnetometer_interference.hpp b/src/components/real/aocs/mtq_magnetometer_interference.hpp index 715cbce47..f468fb497 100644 --- a/src/components/real/aocs/mtq_magnetometer_interference.hpp +++ b/src/components/real/aocs/mtq_magnetometer_interference.hpp @@ -33,8 +33,8 @@ class MtqMagnetometerInterference { protected: size_t polynomial_degree_; //!< Polynomial degree - std::vector> additional_bias_by_mtq_coefficients_; //!< Polynomial coefficients of additional bias noise - s2e::math::Vector<3> previous_added_bias_c_nT_{0.0}; + std::vector> additional_bias_by_mtq_coefficients_; //!< Polynomial coefficients of additional bias noise + math::Vector<3> previous_added_bias_c_nT_{0.0}; Magnetometer& magnetometer_; //!< Magnetometer const Magnetorquer& magnetorquer_; //!< Magnetorquer diff --git a/src/components/real/aocs/reaction_wheel.cpp b/src/components/real/aocs/reaction_wheel.cpp index 4124f0af1..5494ca4ee 100644 --- a/src/components/real/aocs/reaction_wheel.cpp +++ b/src/components/real/aocs/reaction_wheel.cpp @@ -15,7 +15,7 @@ namespace s2e::components { ReactionWheel::ReactionWheel(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const double step_width_s, const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, - const s2e::math::Quaternion quaternion_b2c, const s2e::math::Vector<3> position_b_m, const double dead_time_s, + const math::Quaternion quaternion_b2c, const math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, bool drive_flag, const double init_velocity_rad_s) @@ -33,7 +33,7 @@ ReactionWheel::ReactionWheel(const int prescaler, environment::ClockGenerator* c stop_limit_angular_velocity_rad_s_(stop_limit_angular_velocity_rad_s), drive_flag_(drive_flag), velocity_limit_rpm_(max_velocity_rpm_), - ode_angular_velocity_(step_width_s_, velocity_limit_rpm_ * s2e::math::rpm_to_rad_s, init_velocity_rad_s), + ode_angular_velocity_(step_width_s_, velocity_limit_rpm_ * math::rpm_to_rad_s, init_velocity_rad_s), rw_jitter_(rw_jitter), is_calculated_jitter_(is_calc_jitter_enabled), is_logged_jitter_(is_log_jitter_enabled) { @@ -42,7 +42,7 @@ ReactionWheel::ReactionWheel(const int prescaler, environment::ClockGenerator* c ReactionWheel::ReactionWheel(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const double step_width_s, const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, - const s2e::math::Quaternion quaternion_b2c, const s2e::math::Vector<3> position_b_m, const double dead_time_s, + const math::Quaternion quaternion_b2c, const math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, const bool drive_flag, const double init_velocity_rad_s) @@ -60,7 +60,7 @@ ReactionWheel::ReactionWheel(const int prescaler, environment::ClockGenerator* c stop_limit_angular_velocity_rad_s_(stop_limit_angular_velocity_rad_s), drive_flag_(drive_flag), velocity_limit_rpm_(max_velocity_rpm_), - ode_angular_velocity_(step_width_s, velocity_limit_rpm_ * s2e::math::rpm_to_rad_s, init_velocity_rad_s), + ode_angular_velocity_(step_width_s, velocity_limit_rpm_ * math::rpm_to_rad_s, init_velocity_rad_s), rw_jitter_(rw_jitter), is_calculated_jitter_(is_calc_jitter_enabled), is_logged_jitter_(is_log_jitter_enabled) { @@ -68,7 +68,7 @@ ReactionWheel::ReactionWheel(const int prescaler, environment::ClockGenerator* c } void ReactionWheel::Initialize() { - rotation_axis_c_ = s2e::math::Vector<3>(0.0); + rotation_axis_c_ = math::Vector<3>(0.0); rotation_axis_c_[2] = 1.0; rotation_axis_b_ = quaternion_b2c_.InverseFrameConversion(rotation_axis_c_); @@ -79,7 +79,7 @@ void ReactionWheel::Initialize() { generated_angular_acceleration_rad_s2_ = 0.0; angular_velocity_rad_s_ = ode_angular_velocity_.GetAngularVelocity_rad_s(); - angular_velocity_rpm_ = angular_velocity_rad_s_ * s2e::math::rad_s_to_rpm; + angular_velocity_rpm_ = angular_velocity_rad_s_ * math::rad_s_to_rpm; // Turn on RW jitter calculation if (is_calculated_jitter_) { @@ -101,7 +101,7 @@ void ReactionWheel::FastUpdate() { } } -s2e::math::Vector<3> ReactionWheel::CalcTorque() { +math::Vector<3> ReactionWheel::CalcTorque() { if (!drive_flag_) // RW idle mode -> coasting mode { // Clear delay buffer @@ -118,7 +118,7 @@ s2e::math::Vector<3> ReactionWheel::CalcTorque() { if (abs_angular_velocity_rad_s < stop_limit_angular_velocity_rad_s_) { // Stop rotation rotation_direction = 0.0; - s2e::math::Vector<1> zero_rad_s{0.0}; + math::Vector<1> zero_rad_s{0.0}; ode_angular_velocity_.Setup(0.0, zero_rad_s); } else if (angular_velocity_rad_s_ > 0.0) { rotation_direction = -1.0; @@ -143,7 +143,7 @@ s2e::math::Vector<3> ReactionWheel::CalcTorque() { // Substitution double pre_angular_velocity_rad = angular_velocity_rad_s_; angular_velocity_rad_s_ = ode_angular_velocity_.GetAngularVelocity_rad_s(); - angular_velocity_rpm_ = angular_velocity_rad_s_ * s2e::math::rad_s_to_rpm; + angular_velocity_rpm_ = angular_velocity_rad_s_ * math::rad_s_to_rpm; generated_angular_acceleration_rad_s2_ = (angular_velocity_rad_s_ - pre_angular_velocity_rad) / step_width_s_; // Calc output torque by RW @@ -152,20 +152,20 @@ s2e::math::Vector<3> ReactionWheel::CalcTorque() { return output_torque_b_Nm_; } -const s2e::math::Vector<3> ReactionWheel::GetOutputTorque_b_Nm() const { +const math::Vector<3> ReactionWheel::GetOutputTorque_b_Nm() const { if (is_calculated_jitter_) { // Add jitter_force_b_N_-derived torque and jitter_torque_b_Nm_ to output_torque_b - return output_torque_b_Nm_ - s2e::math::OuterProduct(position_b_m_, rw_jitter_.GetJitterForce_b_N()) - rw_jitter_.GetJitterTorque_b_Nm(); + return output_torque_b_Nm_ - math::OuterProduct(position_b_m_, rw_jitter_.GetJitterForce_b_N()) - rw_jitter_.GetJitterTorque_b_Nm(); } else { return output_torque_b_Nm_; } } -const s2e::math::Vector<3> ReactionWheel::GetJitterForce_b_N() const { +const math::Vector<3> ReactionWheel::GetJitterForce_b_N() const { if (is_calculated_jitter_) { return rw_jitter_.GetJitterForce_b_N(); } else { - s2e::math::Vector<3> zero{0.0}; + math::Vector<3> zero{0.0}; return zero; } } @@ -190,7 +190,7 @@ void ReactionWheel::SetVelocityLimit_rpm(const double velocity_limit_rpm) { } else { velocity_limit_rpm_ = velocity_limit_rpm; } - ode_angular_velocity_.SetAngularVelocityLimit_rad_s(velocity_limit_rpm_ * s2e::math::rpm_to_rad_s); + ode_angular_velocity_.SetAngularVelocityLimit_rad_s(velocity_limit_rpm_ * math::rpm_to_rad_s); return; } @@ -198,11 +198,11 @@ std::string ReactionWheel::GetLogHeader() const { std::string str_tmp = ""; std::string component_name = "rw" + std::to_string(static_cast(component_id_)) + "_"; - str_tmp += WriteScalar(component_name + "angular_velocity", "rad/s"); - str_tmp += WriteScalar(component_name + "angular_velocity", "rpm"); - str_tmp += WriteScalar(component_name + "angular_velocity_upper_limit", "rpm"); - str_tmp += WriteScalar(component_name + "target_angular_acceleration", "rad/s2"); - str_tmp += WriteScalar(component_name + "angular_acceleration", "rad/s2"); + str_tmp += logger::WriteScalar(component_name + "angular_velocity", "rad/s"); + str_tmp += logger::WriteScalar(component_name + "angular_velocity", "rpm"); + str_tmp += logger::WriteScalar(component_name + "angular_velocity_upper_limit", "rpm"); + str_tmp += logger::WriteScalar(component_name + "target_angular_acceleration", "rad/s2"); + str_tmp += logger::WriteScalar(component_name + "angular_acceleration", "rad/s2"); if (is_logged_jitter_ && is_calculated_jitter_) { str_tmp += logger::WriteVector(component_name + "jitter_force", "c", "N", 3); @@ -215,11 +215,11 @@ std::string ReactionWheel::GetLogHeader() const { std::string ReactionWheel::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteScalar(angular_velocity_rad_s_); - str_tmp += WriteScalar(angular_velocity_rpm_); - str_tmp += WriteScalar(velocity_limit_rpm_); - str_tmp += WriteScalar(target_acceleration_rad_s2_); - str_tmp += WriteScalar(generated_angular_acceleration_rad_s2_); + str_tmp += logger::WriteScalar(angular_velocity_rad_s_); + str_tmp += logger::WriteScalar(angular_velocity_rpm_); + str_tmp += logger::WriteScalar(velocity_limit_rpm_); + str_tmp += logger::WriteScalar(target_acceleration_rad_s2_); + str_tmp += logger::WriteScalar(generated_angular_acceleration_rad_s2_); if (is_logged_jitter_ && is_calculated_jitter_) { str_tmp += logger::WriteVector(rw_jitter_.GetJitterForce_c_N()); @@ -240,8 +240,8 @@ double rotor_inertia_kgm2; double max_torque_Nm; double max_velocity_rpm; // Mounting -s2e::math::Quaternion quaternion_b2c; -s2e::math::Vector<3> position_b_m; +math::Quaternion quaternion_b2c; +math::Vector<3> position_b_m; // Time delay double dead_time_s; double time_constant_s; @@ -281,11 +281,11 @@ void InitParams(int actuator_id, std::string file_name, double compo_update_step rw_ini_file.ReadQuaternion(rw_section, "quaternion_b2c", quaternion_b2c); } else // direction_determination_mode == "DIRECTION" { - s2e::math::Vector<3> direction_b; + math::Vector<3> direction_b; rw_ini_file.ReadVector(rw_section, "direction_b", direction_b); - s2e::math::Vector<3> direction_c(0.0); + math::Vector<3> direction_c(0.0); direction_c[2] = 1.0; - s2e::math::Quaternion q(direction_b, direction_c); + math::Quaternion q(direction_b, direction_c); quaternion_b2c = q.Conjugate(); } rw_ini_file.ReadVector(rw_section, "position_b_m", position_b_m); diff --git a/src/components/real/aocs/reaction_wheel.hpp b/src/components/real/aocs/reaction_wheel.hpp index fc013dc11..c20166028 100644 --- a/src/components/real/aocs/reaction_wheel.hpp +++ b/src/components/real/aocs/reaction_wheel.hpp @@ -51,8 +51,8 @@ class ReactionWheel : public Component, public logger::ILoggable { * @param [in] init_velocity_rad_s: Initial value of angular velocity of RW */ ReactionWheel(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const double step_width_s, - const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const s2e::math::Quaternion quaternion_b2c, - const s2e::math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, + const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const math::Quaternion quaternion_b2c, + const math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, const bool drive_flag = false, const double init_velocity_rad_s = 0.0); @@ -81,8 +81,8 @@ class ReactionWheel : public Component, public logger::ILoggable { * @param [in] init_velocity_rad_s: Initial value of angular velocity of RW [rad/s] */ ReactionWheel(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const double step_width_s, - const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const s2e::math::Quaternion quaternion_b2c, - const s2e::math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, + const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const math::Quaternion quaternion_b2c, + const math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, const bool drive_flag = false, const double init_velocity_rad_s = 0.0); @@ -121,12 +121,12 @@ class ReactionWheel : public Component, public logger::ILoggable { * @fn GetOutputTorque_b_Nm * @brief Return output torque in the body fixed frame [Nm] */ - const s2e::math::Vector<3> GetOutputTorque_b_Nm() const; + const math::Vector<3> GetOutputTorque_b_Nm() const; /** * @fn GetJitterForce_b_N * @brief Return output force by jitter in the body fixed frame [N] */ - inline const s2e::math::Vector<3> GetJitterForce_b_N() const; + inline const math::Vector<3> GetJitterForce_b_N() const; /** * @fn GetDriveFlag * @brief Return drive flag @@ -146,7 +146,7 @@ class ReactionWheel : public Component, public logger::ILoggable { * @fn GetAngularMomentum_b_Nms * @brief Return angular momentum of RW [Nms] */ - inline const s2e::math::Vector<3> GetAngularMomentum_b_Nms() const { return angular_momentum_b_Nms_; }; + inline const math::Vector<3> GetAngularMomentum_b_Nms() const { return angular_momentum_b_Nms_; }; // Setter /** @@ -176,10 +176,10 @@ class ReactionWheel : public Component, public logger::ILoggable { const double rotor_inertia_kgm2_; //!< Inertia of RW rotor [kgm2] const double max_torque_Nm_; //!< Maximum output torque [Nm] const double max_velocity_rpm_; //!< Maximum angular velocity of rotor [rpm] - const s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame - const s2e::math::Vector<3> position_b_m_; //!< Position of RW in the body fixed frame [m] - s2e::math::Vector<3> rotation_axis_c_; //!< Wheel rotation axis on the component frame. Constant as (0 0 1). (Output torque is minus direction) - s2e::math::Vector<3> rotation_axis_b_; //!< Wheel rotation vector in the body fixed frame. + const math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame + const math::Vector<3> position_b_m_; //!< Position of RW in the body fixed frame [m] + math::Vector<3> rotation_axis_c_; //!< Wheel rotation axis on the component frame. Constant as (0 0 1). (Output torque is minus direction) + math::Vector<3> rotation_axis_b_; //!< Wheel rotation vector in the body fixed frame. // Parameters for control delay const double step_width_s_; //!< step width for ReactionWheelOde [sec] @@ -201,8 +201,8 @@ class ReactionWheel : public Component, public logger::ILoggable { double angular_velocity_rpm_ = 0.0; //!< Current angular velocity [rpm] double angular_velocity_rad_s_ = 0.0; //!< Current angular velocity [rad/s] // Output at body frame - s2e::math::Vector<3> output_torque_b_Nm_{0.0}; //!< Output torque in the body fixed frame [Nm] - s2e::math::Vector<3> angular_momentum_b_Nms_{0.0}; //!< Angular momentum of RW [Nms] + math::Vector<3> output_torque_b_Nm_{0.0}; //!< Output torque in the body fixed frame [Nm] + math::Vector<3> angular_momentum_b_Nms_{0.0}; //!< Angular momentum of RW [Nms] // ODE double velocity_limit_rpm_; //!< Velocity limit defined by users [RPM] @@ -218,7 +218,7 @@ class ReactionWheel : public Component, public logger::ILoggable { * @fn CalcTorque * @brief Calculation of generated torque */ - s2e::math::Vector<3> CalcTorque(); + math::Vector<3> CalcTorque(); /** * @fn Initialize * @brief Initialize function diff --git a/src/components/real/aocs/reaction_wheel_jitter.cpp b/src/components/real/aocs/reaction_wheel_jitter.cpp index d0b1408fe..e9ba604df 100644 --- a/src/components/real/aocs/reaction_wheel_jitter.cpp +++ b/src/components/real/aocs/reaction_wheel_jitter.cpp @@ -13,14 +13,14 @@ namespace s2e::components { ReactionWheelJitter::ReactionWheelJitter(std::vector> radial_force_harmonics_coefficients, std::vector> radial_torque_harmonics_coefficients, const double update_interval_s, - const s2e::math::Quaternion quaternion_b2c, const double structural_resonance_frequency_Hz, + const math::Quaternion quaternion_b2c, const double structural_resonance_frequency_Hz, const double damping_factor, const double bandwidth, const bool considers_structural_resonance) : radial_force_harmonics_coefficients_(radial_force_harmonics_coefficients), radial_torque_harmonics_coefficients_(radial_torque_harmonics_coefficients), update_interval_s_(update_interval_s), quaternion_b2c_(quaternion_b2c), structural_resonance_frequency_Hz_(structural_resonance_frequency_Hz), - structural_resonance_angular_frequency_Hz_(s2e::math::tau * structural_resonance_frequency_Hz), + structural_resonance_angular_frequency_Hz_(math::tau * structural_resonance_frequency_Hz), damping_factor_(damping_factor), bandwidth_(bandwidth), considers_structural_resonance_(considers_structural_resonance) { @@ -28,7 +28,7 @@ ReactionWheelJitter::ReactionWheelJitter(std::vector> radial // Generate random number for initial rotation phase std::random_device seed_gen; std::default_random_engine engine(seed_gen()); - std::uniform_real_distribution dist(0.0, s2e::math::tau); + std::uniform_real_distribution dist(0.0, math::tau); // Initialize RW rotation phase for (size_t i = 0; i < radial_force_harmonics_coefficients_.size(); i++) { jitter_force_rotation_phase_.push_back(dist(engine)); diff --git a/src/components/real/aocs/reaction_wheel_jitter.hpp b/src/components/real/aocs/reaction_wheel_jitter.hpp index ea2fcec93..88c228b6f 100644 --- a/src/components/real/aocs/reaction_wheel_jitter.hpp +++ b/src/components/real/aocs/reaction_wheel_jitter.hpp @@ -38,7 +38,7 @@ class ReactionWheelJitter { */ ReactionWheelJitter(std::vector> radial_force_harmonics_coefficients, std::vector> radial_torque_harmonics_coefficients, const double update_interval_s, - const s2e::math::Quaternion quaternion_b2c, const double structural_resonance_frequency_Hz, const double damping_factor, + const math::Quaternion quaternion_b2c, const double structural_resonance_frequency_Hz, const double damping_factor, const double bandwidth, const bool considers_structural_resonance); /** * @fn ~ReactionWheelJitter @@ -57,24 +57,24 @@ class ReactionWheelJitter { * @fn GetJitterForce_b_N * @brief Return generated jitter force in the body fixed frame [N] */ - const s2e::math::Vector<3> GetJitterForce_b_N() const { return jitter_force_b_N_; } + const math::Vector<3> GetJitterForce_b_N() const { return jitter_force_b_N_; } /** * @fn GetJitterTorque_b_Nm * @brief Return generated jitter torque in the body fixed frame [Nm] */ - const s2e::math::Vector<3> GetJitterTorque_b_Nm() const { return jitter_torque_b_Nm_; } + const math::Vector<3> GetJitterTorque_b_Nm() const { return jitter_torque_b_Nm_; } /** * @fn GetJitterForce_c_N * @brief Return generated jitter force in the components frame [N] */ - const s2e::math::Vector<3> GetJitterForce_c_N() const { + const math::Vector<3> GetJitterForce_c_N() const { return considers_structural_resonance_ ? filtered_jitter_force_n_c_ : unfiltered_jitter_force_n_c_; } /** * @fn GetJitterTorque_c_Nm * @brief Return generated jitter torque in the component frame [Nm] */ - const s2e::math::Vector<3> GetJitterTorque_c_Nm() const { + const math::Vector<3> GetJitterTorque_c_Nm() const { return considers_structural_resonance_ ? filtered_jitter_torque_n_c_ : unfiltered_jitter_torque_n_c_; } @@ -83,7 +83,7 @@ class ReactionWheelJitter { std::vector> radial_torque_harmonics_coefficients_; //!< Coefficients for radial torque harmonics double update_interval_s_; //!< Jitter update interval [sec] - s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame + math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame double structural_resonance_frequency_Hz_; //!< Frequency of structural resonance [Hz] double structural_resonance_angular_frequency_Hz_; //!< Angular Frequency of structural resonance @@ -96,22 +96,22 @@ class ReactionWheelJitter { std::vector jitter_torque_rotation_phase_; //!< 2 * pi * h_i * Omega * t [rad] // Variables for solving difference equations in component frame - s2e::math::Vector<3> unfiltered_jitter_force_n_c_{0.0}; - s2e::math::Vector<3> unfiltered_jitter_force_n_1_c_{0.0}; - s2e::math::Vector<3> unfiltered_jitter_force_n_2_c_{0.0}; - s2e::math::Vector<3> unfiltered_jitter_torque_n_c_{0.0}; - s2e::math::Vector<3> unfiltered_jitter_torque_n_1_c_{0.0}; - s2e::math::Vector<3> unfiltered_jitter_torque_n_2_c_{0.0}; - s2e::math::Vector<3> filtered_jitter_force_n_c_{0.0}; - s2e::math::Vector<3> filtered_jitter_force_n_1_c_{0.0}; - s2e::math::Vector<3> filtered_jitter_force_n_2_c_{0.0}; - s2e::math::Vector<3> filtered_jitter_torque_n_c_{0.0}; - s2e::math::Vector<3> filtered_jitter_torque_n_1_c_{0.0}; - s2e::math::Vector<3> filtered_jitter_torque_n_2_c_{0.0}; + math::Vector<3> unfiltered_jitter_force_n_c_{0.0}; + math::Vector<3> unfiltered_jitter_force_n_1_c_{0.0}; + math::Vector<3> unfiltered_jitter_force_n_2_c_{0.0}; + math::Vector<3> unfiltered_jitter_torque_n_c_{0.0}; + math::Vector<3> unfiltered_jitter_torque_n_1_c_{0.0}; + math::Vector<3> unfiltered_jitter_torque_n_2_c_{0.0}; + math::Vector<3> filtered_jitter_force_n_c_{0.0}; + math::Vector<3> filtered_jitter_force_n_1_c_{0.0}; + math::Vector<3> filtered_jitter_force_n_2_c_{0.0}; + math::Vector<3> filtered_jitter_torque_n_c_{0.0}; + math::Vector<3> filtered_jitter_torque_n_1_c_{0.0}; + math::Vector<3> filtered_jitter_torque_n_2_c_{0.0}; double coefficients_[6]; //!< Coefficients of difference equation - s2e::math::Vector<3> jitter_force_b_N_{0.0}; //!< Generated jitter force in the body frame [N] - s2e::math::Vector<3> jitter_torque_b_Nm_{0.0}; //!< Generated jitter torque in the body frame [Nm] + math::Vector<3> jitter_force_b_N_{0.0}; //!< Generated jitter force in the body frame [N] + math::Vector<3> jitter_torque_b_Nm_{0.0}; //!< Generated jitter torque in the body frame [Nm] /** * @fn AddStructuralResonance diff --git a/src/components/real/aocs/reaction_wheel_ode.cpp b/src/components/real/aocs/reaction_wheel_ode.cpp index d8b34ee86..621b4e598 100644 --- a/src/components/real/aocs/reaction_wheel_ode.cpp +++ b/src/components/real/aocs/reaction_wheel_ode.cpp @@ -10,10 +10,10 @@ namespace s2e::components { ReactionWheelOde::ReactionWheelOde(const double step_width_s, const double velocity_limit_rad_s, const double initial_angular_velocity_rad_s) : OrdinaryDifferentialEquation<1>(step_width_s), velocity_limit_rad_s_(velocity_limit_rad_s) { - this->Setup(0.0, s2e::math::Vector<1>(initial_angular_velocity_rad_s)); + this->Setup(0.0, math::Vector<1>(initial_angular_velocity_rad_s)); } -void ReactionWheelOde::DerivativeFunction(double x, const s2e::math::Vector<1> &state, s2e::math::Vector<1> &rhs) { +void ReactionWheelOde::DerivativeFunction(double x, const math::Vector<1> &state, math::Vector<1> &rhs) { UNUSED(x); double angular_velocity_rad_s = state[0]; diff --git a/src/components/real/aocs/reaction_wheel_ode.hpp b/src/components/real/aocs/reaction_wheel_ode.hpp index 552c2cc04..82d3888e0 100644 --- a/src/components/real/aocs/reaction_wheel_ode.hpp +++ b/src/components/real/aocs/reaction_wheel_ode.hpp @@ -14,7 +14,7 @@ namespace s2e::components { * @file ReactionWheelOde * @brief Ordinary differential equation of angular velocity of reaction wheel with first-order lag */ -class ReactionWheelOde : public s2e::math::OrdinaryDifferentialEquation<1> { +class ReactionWheelOde : public math::OrdinaryDifferentialEquation<1> { public: /** * @fn ReactionWheelOde @@ -55,7 +55,7 @@ class ReactionWheelOde : public s2e::math::OrdinaryDifferentialEquation<1> { * @param [in] state: State vector * @param [out] rhs: Differentiated value of state vector */ - void DerivativeFunction(double x, const s2e::math::Vector<1>& state, s2e::math::Vector<1>& rhs) override; + void DerivativeFunction(double x, const math::Vector<1>& state, math::Vector<1>& rhs) override; double velocity_limit_rad_s_; double angular_acceleration_rad_s2_ = 0.0; //!< Angular acceleration [rad/s2] diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index c17302608..36b47095a 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -18,7 +18,7 @@ using namespace s2e::math; namespace s2e::components { -StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, +StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, @@ -43,7 +43,7 @@ StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_g Initialize(); } StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, + const math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const dynamics::Dynamics* dynamics, @@ -69,21 +69,21 @@ StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_g } void StarSensor::Initialize() { - measured_quaternion_i2c_ = s2e::math::Quaternion(0.0, 0.0, 0.0, 1.0); + measured_quaternion_i2c_ = math::Quaternion(0.0, 0.0, 0.0, 1.0); // Decide delay buffer size max_delay_ = int(output_delay_ * 2 / step_time_s_); if (max_delay_ <= 0) max_delay_ = 1; - vector temp(max_delay_); + vector temp(max_delay_); delay_buffer_ = temp; // Initialize delay buffer for (int i = 0; i < max_delay_; ++i) { delay_buffer_[i] = measured_quaternion_i2c_; } - sight_direction_c_ = s2e::math::Vector<3>(0.0); - first_orthogonal_direction_c = s2e::math::Vector<3>(0.0); - second_orthogonal_direction_c = s2e::math::Vector<3>(0.0); + sight_direction_c_ = math::Vector<3>(0.0); + first_orthogonal_direction_c = math::Vector<3>(0.0); + second_orthogonal_direction_c = math::Vector<3>(0.0); sight_direction_c_[0] = 1.0; //(1,0,0)@Component coordinates, viewing direction first_orthogonal_direction_c[1] = 1.0; //(0,1,0)@Component coordinates, line-of-sight orthogonal direction second_orthogonal_direction_c[2] = 1.0; //(0,0,1)@Component coordinates, line-of-sight orthogonal direction @@ -112,10 +112,10 @@ void StarSensor::update(const LocalCelestialInformation* local_celestial_informa // Add noise on sight direction Quaternion q_sight(sight_direction_c_, sight_direction_noise_); // Random noise on orthogonal direction of sight. Range [0:2pi] - double rot = s2e::math::tau * double(rotation_noise_); + double rot = math::tau * double(rotation_noise_); // Calc observation error on orthogonal direction of sight - s2e::math::Vector<3> rot_axis = cos(rot) * first_orthogonal_direction_c + sin(rot) * second_orthogonal_direction_c; - s2e::math::Quaternion q_ortho(rot_axis, orthogonal_direction_noise_); + math::Vector<3> rot_axis = cos(rot) * first_orthogonal_direction_c + sin(rot) * second_orthogonal_direction_c; + math::Quaternion q_ortho(rot_axis, orthogonal_direction_noise_); // Judge errors AllJudgement(local_celestial_information, attitude); @@ -139,9 +139,9 @@ void StarSensor::AllJudgement(const LocalCelestialInformation* local_celestial_i error_flag_ = false; } -int StarSensor::SunJudgement(const s2e::math::Vector<3>& sun_b) { - s2e::math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); - s2e::math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); +int StarSensor::SunJudgement(const math::Vector<3>& sun_b) { + math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); + math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); double sun_angle_rad = CalAngleVector_rad(sun_b, sight_b); if (sun_angle_rad < sun_forbidden_angle_rad_) return 1; @@ -149,9 +149,9 @@ int StarSensor::SunJudgement(const s2e::math::Vector<3>& sun_b) { return 0; } -int StarSensor::EarthJudgement(const s2e::math::Vector<3>& earth_b) { - s2e::math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); - s2e::math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); +int StarSensor::EarthJudgement(const math::Vector<3>& earth_b) { + math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); + math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); double earth_size_rad = atan2(environment::earth_equatorial_radius_m, earth_b.CalcNorm()); // angles between sat<->earth_center & sat<->earth_edge double earth_center_angle_rad = CalAngleVector_rad(earth_b, sight_b); // angles between sat<->earth_center & sat_sight @@ -162,9 +162,9 @@ int StarSensor::EarthJudgement(const s2e::math::Vector<3>& earth_b) { return 0; } -int StarSensor::MoonJudgement(const s2e::math::Vector<3>& moon_b) { - s2e::math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); - s2e::math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); +int StarSensor::MoonJudgement(const math::Vector<3>& moon_b) { + math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); + math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); double moon_angle_rad = CalAngleVector_rad(moon_b, sight_b); if (moon_angle_rad < moon_forbidden_angle_rad_) return 1; @@ -172,7 +172,7 @@ int StarSensor::MoonJudgement(const s2e::math::Vector<3>& moon_b) { return 0; } -int StarSensor::CaptureRateJudgement(const s2e::math::Vector<3>& omega_b_rad_s) { +int StarSensor::CaptureRateJudgement(const math::Vector<3>& omega_b_rad_s) { double omega_norm = omega_b_rad_s.CalcNorm(); if (omega_norm > capture_rate_limit_rad_s_) return 1; @@ -186,7 +186,7 @@ std::string StarSensor::GetLogHeader() const { std::string sensor_name = "stt" + sensor_id + "_"; str_tmp += logger::WriteQuaternion(sensor_name + "measured_quaternion", "i2c"); - str_tmp += WriteScalar(sensor_name + "error_flag"); + str_tmp += logger::WriteScalar(sensor_name + "error_flag"); return str_tmp; } @@ -195,14 +195,14 @@ std::string StarSensor::GetLogValue() const { std::string str_tmp = ""; str_tmp += logger::WriteQuaternion(measured_quaternion_i2c_); - str_tmp += WriteScalar(double(error_flag_)); + str_tmp += logger::WriteScalar(double(error_flag_)); return str_tmp; } double StarSensor::CalAngleVector_rad(const Vector<3>& vector1, const Vector<3>& vector2) { - s2e::math::Vector<3> vect1_normal = vector1.CalcNormalizedVector(); - s2e::math::Vector<3> vect2_normal = vector2.CalcNormalizedVector(); + math::Vector<3> vect1_normal = vector1.CalcNormalizedVector(); + math::Vector<3> vect2_normal = vector2.CalcNormalizedVector(); double cosTheta = InnerProduct(vect1_normal, vect2_normal); // Calc cos value double theta_rad = acos(cosTheta); @@ -225,7 +225,7 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sens int prescaler = STT_conf.ReadInt(STTSection, "prescaler"); if (prescaler <= 1) prescaler = 1; double step_time_s = component_step_time_s * prescaler; - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; STT_conf.ReadQuaternion(STTSection, "quaternion_b2c", quaternion_b2c); double standard_deviation_orthogonal_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_orthogonal_direction_rad"); double standard_deviation_sight_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_sight_direction_rad"); @@ -234,13 +234,13 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sens double output_interval_sec = STT_conf.ReadDouble(STTSection, "output_interval_s"); int output_interval = max(int(output_interval_sec / step_time_s), 1); double sun_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "sun_exclusion_angle_deg"); - double sun_forbidden_angle_rad = sun_forbidden_angle_deg * s2e::math::pi / 180.0; + double sun_forbidden_angle_rad = sun_forbidden_angle_deg * math::pi / 180.0; double earth_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "earth_exclusion_angle_deg"); - double earth_forbidden_angle_rad = earth_forbidden_angle_deg * s2e::math::pi / 180.0; + double earth_forbidden_angle_rad = earth_forbidden_angle_deg * math::pi / 180.0; double moon_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "moon_exclusion_angle_deg"); - double moon_forbidden_angle_rad = moon_forbidden_angle_deg * s2e::math::pi / 180.0; + double moon_forbidden_angle_rad = moon_forbidden_angle_deg * math::pi / 180.0; double capture_rate_deg_s = STT_conf.ReadDouble(STTSection, "angular_rate_limit_deg_s"); - double capture_rate_rad_s = capture_rate_deg_s * s2e::math::pi / 180.0; + double capture_rate_rad_s = capture_rate_deg_s * math::pi / 180.0; StarSensor stt(prescaler, clock_generator, sensor_id, quaternion_b2c, standard_deviation_orthogonal_direction, standard_deviation_sight_direction, step_time_s, output_delay, output_interval, sun_forbidden_angle_rad, earth_forbidden_angle_rad, moon_forbidden_angle_rad, @@ -259,7 +259,7 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPor if (prescaler <= 1) prescaler = 1; double step_time_s = component_step_time_s * prescaler; - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; STT_conf.ReadQuaternion(STTSection, "quaternion_b2c", quaternion_b2c); double standard_deviation_orthogonal_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_orthogonal_direction_rad"); double standard_deviation_sight_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_sight_direction_rad"); @@ -268,13 +268,13 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPor double output_interval_sec = STT_conf.ReadDouble(STTSection, "output_interval_s"); int output_interval = max(int(output_interval_sec / step_time_s), 1); double sun_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "sun_exclusion_angle_deg"); - double sun_forbidden_angle_rad = sun_forbidden_angle_deg * s2e::math::pi / 180.0; + double sun_forbidden_angle_rad = sun_forbidden_angle_deg * math::pi / 180.0; double earth_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "earth_exclusion_angle_deg"); - double earth_forbidden_angle_rad = earth_forbidden_angle_deg * s2e::math::pi / 180.0; + double earth_forbidden_angle_rad = earth_forbidden_angle_deg * math::pi / 180.0; double moon_forbidden_angle_deg = STT_conf.ReadDouble(STTSection, "moon_exclusion_angle_deg"); - double moon_forbidden_angle_rad = moon_forbidden_angle_deg * s2e::math::pi / 180.0; + double moon_forbidden_angle_rad = moon_forbidden_angle_deg * math::pi / 180.0; double capture_rate_deg_s = STT_conf.ReadDouble(STTSection, "angular_rate_limit_deg_s"); - double capture_rate_rad_s = capture_rate_deg_s * s2e::math::pi / 180.0; + double capture_rate_rad_s = capture_rate_deg_s * math::pi / 180.0; power_port->InitializeWithInitializeFile(file_name); diff --git a/src/components/real/aocs/star_sensor.hpp b/src/components/real/aocs/star_sensor.hpp index 772e3bd53..e28e0add4 100644 --- a/src/components/real/aocs/star_sensor.hpp +++ b/src/components/real/aocs/star_sensor.hpp @@ -45,7 +45,7 @@ class StarSensor : public Component, public logger::ILoggable { * @param [in] dynamics: Dynamics information * @param [in] local_environment: Local environment information */ - StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, + StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, @@ -71,7 +71,7 @@ class StarSensor : public Component, public logger::ILoggable { * @param [in] local_environment: Local environment information */ StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const s2e::math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, + const math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const dynamics::Dynamics* dynamics, @@ -100,7 +100,7 @@ class StarSensor : public Component, public logger::ILoggable { * @fn GetMeasuredQuaternion_i2c * @brief Return observed quaternion from the inertial frame to the component frame */ - inline const s2e::math::Quaternion GetMeasuredQuaternion_i2c() const { return measured_quaternion_i2c_; }; + inline const math::Quaternion GetMeasuredQuaternion_i2c() const { return measured_quaternion_i2c_; }; /** * @fn GetErrorFlag * @brief Return error flag @@ -110,11 +110,11 @@ class StarSensor : public Component, public logger::ILoggable { protected: // StarSensor general parameters const int component_id_; //!< Sensor ID - s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame - s2e::math::Quaternion measured_quaternion_i2c_ = {0.0, 0.0, 0.0, 1.0}; //!< StarSensor observed quaternion - s2e::math::Vector<3> sight_direction_c_; //!< Sight direction vector at component frame - s2e::math::Vector<3> first_orthogonal_direction_c; //!< The first orthogonal direction of sight at component frame - s2e::math::Vector<3> second_orthogonal_direction_c; //!< The second orthogonal direction of sight at component frame + math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame + math::Quaternion measured_quaternion_i2c_ = {0.0, 0.0, 0.0, 1.0}; //!< StarSensor observed quaternion + math::Vector<3> sight_direction_c_; //!< Sight direction vector at component frame + math::Vector<3> first_orthogonal_direction_c; //!< The first orthogonal direction of sight at component frame + math::Vector<3> second_orthogonal_direction_c; //!< The second orthogonal direction of sight at component frame // Noise parameters s2e::randomization::MinimalStandardLcgWithShuffle rotation_noise_; //!< Randomize object for orthogonal direction @@ -123,7 +123,7 @@ class StarSensor : public Component, public logger::ILoggable { // Delay emulation parameters int max_delay_; //!< Max delay - std::vector delay_buffer_; //!< Buffer of quaternion for delay emulation + std::vector delay_buffer_; //!< Buffer of quaternion for delay emulation int buffer_position_; //!< Buffer position double step_time_s_; //!< Step time for delay calculation [sec] unsigned int output_delay_; //!< Output delay [0, max_delay_] [step_sec] @@ -155,7 +155,7 @@ class StarSensor : public Component, public logger::ILoggable { * @param [in] local_celestial_information: Local celestial information * @param [in] attitude: dynamics::attitude::Attitude information */ - s2e::math::Quaternion Measure(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); + math::Quaternion Measure(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); /** * @fn AllJudgement @@ -170,28 +170,28 @@ class StarSensor : public Component, public logger::ILoggable { * @param [in] sun_b: Sun direction vector in the body fixed frame * @return 1: violated, 0: not violated */ - int SunJudgement(const s2e::math::Vector<3>& sun_b); + int SunJudgement(const math::Vector<3>& sun_b); /** * @fn EarthJudgement * @brief Judge violation of earth forbidden angle * @param [in] earth_b: Earth direction vector in the body fixed frame * @return 1: violated, 0: not violated */ - int EarthJudgement(const s2e::math::Vector<3>& earth_b); + int EarthJudgement(const math::Vector<3>& earth_b); /** * @fn MoonJudgement * @brief Judge violation of moon forbidden angle * @param [in] moon_b: Moon direction vector in the body fixed frame * @return 1: violated, 0: not violated */ - int MoonJudgement(const s2e::math::Vector<3>& moon_b); + int MoonJudgement(const math::Vector<3>& moon_b); /** * @fn CaptureRateJudgement * @brief Judge violation of angular velocity limit * @param [in] omega_b_rad_s: Angular velocity of spacecraft in the body fixed frame * @return 1: violated, 0: not violated */ - int CaptureRateJudgement(const s2e::math::Vector<3>& omega_b_rad_s); + int CaptureRateJudgement(const math::Vector<3>& omega_b_rad_s); /** * @fn CalAngleVector_rad * @brief Calculate angle between two vectors @@ -199,7 +199,7 @@ class StarSensor : public Component, public logger::ILoggable { * @param [in] vector2: Second vector * @return Angle between two vectors [rad] */ - double CalAngleVector_rad(const s2e::math::Vector<3>& vector1, const s2e::math::Vector<3>& vector2); + double CalAngleVector_rad(const math::Vector<3>& vector1, const math::Vector<3>& vector2); /** * @fn Initialize diff --git a/src/components/real/aocs/sun_sensor.cpp b/src/components/real/aocs/sun_sensor.cpp index 82e74e40a..15fe6210a 100644 --- a/src/components/real/aocs/sun_sensor.cpp +++ b/src/components/real/aocs/sun_sensor.cpp @@ -16,7 +16,7 @@ using namespace std; namespace s2e::components { -SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, +SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) @@ -31,7 +31,7 @@ SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_gen } SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, + const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) : Component(prescaler, clock_generator, power_port), @@ -61,8 +61,8 @@ void SunSensor::MainRoutine(const int time_count) { } void SunSensor::Measure() { - s2e::math::Vector<3> sun_pos_b = local_celestial_information_->GetPositionFromSpacecraft_b_m("SUN"); - s2e::math::Vector<3> sun_dir_b = sun_pos_b.CalcNormalizedVector(); + math::Vector<3> sun_pos_b = local_celestial_information_->GetPositionFromSpacecraft_b_m("SUN"); + math::Vector<3> sun_dir_b = sun_pos_b.CalcNormalizedVector(); sun_direction_true_c_ = quaternion_b2c_.FrameConversion(sun_dir_b); // Frame conversion from body to component @@ -89,7 +89,7 @@ void SunSensor::Measure() { measured_sun_direction_c_ = measured_sun_direction_c_.CalcNormalizedVector(); } else { - measured_sun_direction_c_ = s2e::math::Vector<3>(0); + measured_sun_direction_c_ = math::Vector<3>(0); alpha_rad_ = 0.0; beta_rad_ = 0.0; } @@ -98,7 +98,7 @@ void SunSensor::Measure() { } void SunSensor::SunDetectionJudgement() { - s2e::math::Vector<3> sun_direction_c = sun_direction_true_c_.CalcNormalizedVector(); + math::Vector<3> sun_direction_c = sun_direction_true_c_.CalcNormalizedVector(); double sun_angle_ = acos(sun_direction_c[2]); @@ -114,10 +114,10 @@ void SunSensor::SunDetectionJudgement() { } void SunSensor::CalcSolarIlluminance() { - s2e::math::Vector<3> sun_direction_c = sun_direction_true_c_.CalcNormalizedVector(); + math::Vector<3> sun_direction_c = sun_direction_true_c_.CalcNormalizedVector(); double sun_angle_ = acos(sun_direction_c[2]); - if (sun_angle_ > s2e::math::pi_2) { + if (sun_angle_ > math::pi_2) { solar_illuminance_W_m2_ = 0.0; return; } @@ -128,8 +128,8 @@ void SunSensor::CalcSolarIlluminance() { } double SunSensor::TanRange(double x) { - if (x > s2e::math::pi_2) x = s2e::math::pi - x; - if (x < -s2e::math::pi_2) x = -s2e::math::pi - x; + if (x > math::pi_2) x = math::pi - x; + if (x < -math::pi_2) x = -math::pi - x; return x; } @@ -139,7 +139,7 @@ string SunSensor::GetLogHeader() const { std::string sensor_name = "sun_sensor" + sensor_id + "_"; str_tmp += logger::WriteVector(sensor_name + "measured_sun_direction", "c", "-", 3); - str_tmp += WriteScalar(sensor_name + "sun_detected_flag", "-"); + str_tmp += logger::WriteScalar(sensor_name + "sun_detected_flag", "-"); return str_tmp; } @@ -148,7 +148,7 @@ string SunSensor::GetLogValue() const { string str_tmp = ""; str_tmp += logger::WriteVector(measured_sun_direction_c_); - str_tmp += WriteScalar(double(sun_detected_flag_)); + str_tmp += logger::WriteScalar(double(sun_detected_flag_)); return str_tmp; } @@ -163,20 +163,20 @@ SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int ss_id, int prescaler = ss_conf.ReadInt(Section, "prescaler"); if (prescaler <= 1) prescaler = 1; - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; ss_conf.ReadQuaternion(Section, "quaternion_b2c", quaternion_b2c); double detectable_angle_deg = 0.0, detectable_angle_rad = 0.0; detectable_angle_deg = ss_conf.ReadDouble(Section, "field_of_view_deg"); - detectable_angle_rad = s2e::math::pi / 180.0 * detectable_angle_deg; + detectable_angle_rad = math::pi / 180.0 * detectable_angle_deg; double nr_stddev = 0.0; nr_stddev = ss_conf.ReadDouble(Section, "white_noise_standard_deviation_deg"); - nr_stddev *= s2e::math::pi / 180.0; + nr_stddev *= math::pi / 180.0; double nr_bias_stddev = 0.0; nr_bias_stddev = ss_conf.ReadDouble(Section, "bias_standard_deviation_deg"); - nr_bias_stddev *= s2e::math::pi / 180.0; + nr_bias_stddev *= math::pi / 180.0; double intensity_lower_threshold_percent; intensity_lower_threshold_percent = ss_conf.ReadDouble(Section, "intensity_lower_threshold_percent"); @@ -196,20 +196,20 @@ SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, PowerPort* int prescaler = ss_conf.ReadInt(Section, "prescaler"); if (prescaler <= 1) prescaler = 1; - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; ss_conf.ReadQuaternion(Section, "quaternion_b2c", quaternion_b2c); double detectable_angle_deg = 0.0, detectable_angle_rad = 0.0; detectable_angle_deg = ss_conf.ReadDouble(Section, "field_of_view_deg"); - detectable_angle_rad = s2e::math::pi / 180.0 * detectable_angle_deg; + detectable_angle_rad = math::pi / 180.0 * detectable_angle_deg; double nr_stddev = 0.0; nr_stddev = ss_conf.ReadDouble(Section, "white_noise_standard_deviation_deg"); - nr_stddev *= s2e::math::pi / 180.0; + nr_stddev *= math::pi / 180.0; double nr_bias_stddev = 0.0; nr_bias_stddev = ss_conf.ReadDouble(Section, "bias_standard_deviation_deg"); - nr_bias_stddev *= s2e::math::pi / 180.0; + nr_bias_stddev *= math::pi / 180.0; double intensity_lower_threshold_percent; intensity_lower_threshold_percent = ss_conf.ReadDouble(Section, "intensity_lower_threshold_percent"); diff --git a/src/components/real/aocs/sun_sensor.hpp b/src/components/real/aocs/sun_sensor.hpp index f8aba0d41..cd23d2d38 100644 --- a/src/components/real/aocs/sun_sensor.hpp +++ b/src/components/real/aocs/sun_sensor.hpp @@ -37,7 +37,7 @@ class SunSensor : public Component, public logger::ILoggable { * @param [in] srp_environment: Solar Radiation Pressure environment * @param [in] local_celestial_information: Local celestial information */ - SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const s2e::math::Quaternion& quaternion_b2c, + SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); @@ -57,7 +57,7 @@ class SunSensor : public Component, public logger::ILoggable { * @param [in] local_celestial_information: Local celestial information */ SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const s2e::math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, + const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); @@ -82,19 +82,19 @@ class SunSensor : public Component, public logger::ILoggable { // Getter inline bool GetSunDetectedFlag() const { return sun_detected_flag_; }; - inline const s2e::math::Vector<3> GetMeasuredSunDirection_c() const { return measured_sun_direction_c_; }; - inline const s2e::math::Vector<3> GetMeasuredSunDirection_b() const { return quaternion_b2c_.Conjugate().FrameConversion(measured_sun_direction_c_); }; + inline const math::Vector<3> GetMeasuredSunDirection_c() const { return measured_sun_direction_c_; }; + inline const math::Vector<3> GetMeasuredSunDirection_b() const { return quaternion_b2c_.Conjugate().FrameConversion(measured_sun_direction_c_); }; inline double GetSunAngleAlpha_rad() const { return alpha_rad_; }; inline double GetSunAngleBeta_rad() const { return beta_rad_; }; inline double GetSolarIlluminance_W_m2() const { return solar_illuminance_W_m2_; }; protected: const int component_id_; //!< Sensor ID - s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (Z-axis of the component is sight direction) + math::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (Z-axis of the component is sight direction) double intensity_lower_threshold_percent_; //!< If the light intensity becomes smaller than this, it becomes impossible to get the sun direction - s2e::math::Vector<3> sun_direction_true_c_{0.0}; //!< True value of sun vector in the component frame - s2e::math::Vector<3> measured_sun_direction_c_{0.0}; //!< Measured sun vector in the component frame + math::Vector<3> sun_direction_true_c_{0.0}; //!< True value of sun vector in the component frame + math::Vector<3> measured_sun_direction_c_{0.0}; //!< Measured sun vector in the component frame double alpha_rad_ = 0.0; //!< Angle between Z-axis and the sun direction projected on XZ plane [rad] double beta_rad_ = 0.0; //!< Angle between Z-axis and the sun direction projected on YZ plane [rad] diff --git a/src/components/real/communication/antenna.cpp b/src/components/real/communication/antenna.cpp index 3eb527fd4..12f24c46b 100644 --- a/src/components/real/communication/antenna.cpp +++ b/src/components/real/communication/antenna.cpp @@ -13,7 +13,7 @@ namespace s2e::components { -Antenna::Antenna(const int component_id, const s2e::math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, +Antenna::Antenna(const int component_id, const math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, const double frequency_MHz, const Vector<5> tx_parameters, const Vector<4> rx_parameters) : component_id_(component_id), is_transmitter_(is_transmitter), is_receiver_(is_receiver), frequency_MHz_(frequency_MHz) { quaternion_b2c_ = quaternion_b2c; @@ -47,7 +47,7 @@ Antenna::Antenna(const int component_id, const s2e::math::Quaternion& quaternion } } -Antenna::Antenna(const int component_id, const s2e::math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, +Antenna::Antenna(const int component_id, const math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, const double frequency_MHz, const double tx_bitrate_bps, const double tx_output_power_W, const AntennaParameters tx_parameters, const double rx_system_noise_temperature_K, const AntennaParameters rx_parameters) : component_id_(component_id), diff --git a/src/components/real/communication/antenna.hpp b/src/components/real/communication/antenna.hpp index 28023cff6..7419e3219 100644 --- a/src/components/real/communication/antenna.hpp +++ b/src/components/real/communication/antenna.hpp @@ -8,8 +8,8 @@ #include #include -using s2e::math::Quaternion; -using s2e::math::Vector; +using math::Quaternion; +using math::Vector; #include #include "./antenna_radiation_pattern.hpp" @@ -56,7 +56,7 @@ class Antenna { * @param [in] tx_parameters: output, gain, loss_feeder, loss_pointing for TX * @param [in] rx_parameters: gain, loss_feeder, loss_pointing, system_temperature for RX */ - Antenna(const int component_id, const s2e::math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, + Antenna(const int component_id, const math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, const double frequency_MHz, const Vector<5> tx_parameters, const Vector<4> rx_parameters); /** @@ -73,7 +73,7 @@ class Antenna { * @param [in] rx_system_noise_temperature_K: Receive system noise temperature [K] * @param [in] rx_parameters: RX antenna parameters */ - Antenna(const int component_id, const s2e::math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, + Antenna(const int component_id, const math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, const double frequency_MHz, const double tx_bitrate_bps, const double tx_output_power_W, const AntennaParameters tx_parameters, const double rx_system_noise_temperature_K, const AntennaParameters rx_parameters); /** diff --git a/src/components/real/communication/antenna_radiation_pattern.hpp b/src/components/real/communication/antenna_radiation_pattern.hpp index 3d2382862..f924fbe41 100644 --- a/src/components/real/communication/antenna_radiation_pattern.hpp +++ b/src/components/real/communication/antenna_radiation_pattern.hpp @@ -38,7 +38,7 @@ class AntennaRadiationPattern { * @param[in] phi_max_rad_: Maximum value of phi */ AntennaRadiationPattern(const std::string file_path, const size_t length_theta = 360, const size_t length_phi = 181, - const double theta_max_rad = s2e::math::tau, const double phi_max_rad = s2e::math::pi); + const double theta_max_rad = math::tau, const double phi_max_rad = math::pi); /** * @fn ~AntennaRadiationPattern @@ -58,8 +58,8 @@ class AntennaRadiationPattern { private: size_t length_theta_ = 360; //!< Length of grid for theta direction size_t length_phi_ = 181; //!< Length of grid for phi direction - double theta_max_rad_ = s2e::math::tau; //!< Maximum value of theta - double phi_max_rad_ = s2e::math::pi; //!< Maximum value of phi + double theta_max_rad_ = math::tau; //!< Maximum value of theta + double phi_max_rad_ = math::pi; //!< Maximum value of phi std::vector> gain_dBi_; //!< Antenna gain table [dBi] }; diff --git a/src/components/real/communication/ground_station_calculator.cpp b/src/components/real/communication/ground_station_calculator.cpp index a5deff17d..c3b3f9edd 100644 --- a/src/components/real/communication/ground_station_calculator.cpp +++ b/src/components/real/communication/ground_station_calculator.cpp @@ -74,7 +74,7 @@ double GroundStationCalculator::CalcCn0OnGs(const dynamics::Dynamics& dynamics, Vector<3> pos_gs2sc_i = sc_pos_i - gs_pos_i; double dist_sc_gs_km = pos_gs2sc_i.CalcNorm() / 1000.0; - double loss_space_dB = -20.0 * log10(4.0 * s2e::math::pi * dist_sc_gs_km / (300.0 / spacecraft_tx_antenna.GetFrequency_MHz() / 1000.0)); + double loss_space_dB = -20.0 * log10(4.0 * math::pi * dist_sc_gs_km / (300.0 / spacecraft_tx_antenna.GetFrequency_MHz() / 1000.0)); // GS direction on SC TX antenna frame Vector<3> sc_to_gs_i = gs_pos_i - sc_pos_i; @@ -104,8 +104,8 @@ std::string GroundStationCalculator::GetLogHeader() const { std::string str_tmp = ""; std::string component_name = "gs_calculator_"; - str_tmp += WriteScalar(component_name + "max_bitrate", "Mbps"); - str_tmp += WriteScalar(component_name + "receive_margin", "dB"); + str_tmp += logger::WriteScalar(component_name + "max_bitrate", "Mbps"); + str_tmp += logger::WriteScalar(component_name + "receive_margin", "dB"); return str_tmp; } @@ -113,8 +113,8 @@ std::string GroundStationCalculator::GetLogHeader() const { std::string GroundStationCalculator::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteScalar(max_bitrate_Mbps_); - str_tmp += WriteScalar(receive_margin_dB_); + str_tmp += logger::WriteScalar(max_bitrate_Mbps_); + str_tmp += logger::WriteScalar(receive_margin_dB_); return str_tmp; } diff --git a/src/components/real/mission/telescope.cpp b/src/components/real/mission/telescope.cpp index b04e5b0da..a05730683 100644 --- a/src/components/real/mission/telescope.cpp +++ b/src/components/real/mission/telescope.cpp @@ -15,7 +15,7 @@ using namespace s2e::math; namespace s2e::components { -Telescope::Telescope(environment::ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, +Telescope::Telescope(environment::ClockGenerator* clock_generator, const math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, @@ -40,8 +40,8 @@ Telescope::Telescope(environment::ClockGenerator* clock_generator, const s2e::ma x_field_of_view_rad = x_number_of_pix_ * x_fov_per_pix_; y_field_of_view_rad = y_number_of_pix_ * y_fov_per_pix_; - assert(x_field_of_view_rad < s2e::math::pi_2); // Avoid the case that the field of view is over 90 degrees - assert(y_field_of_view_rad < s2e::math::pi_2); + assert(x_field_of_view_rad < math::pi_2); // Avoid the case that the field of view is over 90 degrees + assert(y_field_of_view_rad < math::pi_2); sight_direction_c_ = Vector<3>(0); sight_direction_c_[0] = 1; // (1,0,0) at component frame, Sight direction vector @@ -60,7 +60,7 @@ Telescope::Telescope(environment::ClockGenerator* clock_generator, const s2e::ma } // Get initial spacecraft position in ECEF if (orbit_ != nullptr) { - s2e::math::Vector<3> initial_spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m(); + math::Vector<3> initial_spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m(); initial_ground_position_ecef_m_ = environment::earth_equatorial_radius_m * initial_spacecraft_position_ecef_m; initial_ground_position_ecef_m_ /= (orbit_->GetGeodeticPosition().GetAltitude_m() + environment::earth_equatorial_radius_m); } @@ -85,26 +85,26 @@ void Telescope::MainRoutine(const int time_count) { // sun_pos_c = quaternion_b2c_.FrameConversion(dynamics_->celestial_->GetPositionFromSpacecraft_b_m("SUN")); // earth_pos_c = quaternion_b2c_.FrameConversion(dynamics_->celestial_->GetPositionFromSpacecraft_b_m("EARTH")); // moon_pos_c = quaternion_b2c_.FrameConversion(dynamics_->celestial_->GetPositionFromSpacecraft_b_m("MOON")); - // angle_sun = CalcAngleTwoVectors_rad(sight_direction_c_, sun_pos_c) * 180/s2e::math::pi; - // angle_earth = CalcAngleTwoVectors_rad(sight_direction_c_, earth_pos_c) * 180 / s2e::math::pi; angle_moon = - // CalcAngleTwoVectors_rad(sight_direction_c_, moon_pos_c) * 180 / s2e::math::pi; + // angle_sun = CalcAngleTwoVectors_rad(sight_direction_c_, sun_pos_c) * 180/math::pi; + // angle_earth = CalcAngleTwoVectors_rad(sight_direction_c_, earth_pos_c) * 180 / math::pi; angle_moon = + // CalcAngleTwoVectors_rad(sight_direction_c_, moon_pos_c) * 180 / math::pi; //****************************************************************************** // Direction calculation of ground point ObserveGroundPositionDeviation(); } -bool Telescope::JudgeForbiddenAngle(const s2e::math::Vector<3>& target_b, const double forbidden_angle) { - s2e::math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); - s2e::math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); - double angle_rad = s2e::math::CalcAngleTwoVectors_rad(target_b, sight_b); +bool Telescope::JudgeForbiddenAngle(const math::Vector<3>& target_b, const double forbidden_angle) { + math::Quaternion q_c2b = quaternion_b2c_.Conjugate(); + math::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_); + double angle_rad = math::CalcAngleTwoVectors_rad(target_b, sight_b); if (angle_rad < forbidden_angle) { return true; } else return false; } -void Telescope::Observe(s2e::math::Vector<2>& position_image_sensor, const s2e::math::Vector<3, double> target_b) { - s2e::math::Vector<3, double> target_c = quaternion_b2c_.FrameConversion(target_b); +void Telescope::Observe(math::Vector<2>& position_image_sensor, const math::Vector<3, double> target_b) { + math::Vector<3, double> target_c = quaternion_b2c_.FrameConversion(target_b); double arg_x = atan2(target_c[2], target_c[0]); // Angle from X-axis on XZ plane in the component frame double arg_y = atan2(target_c[1], target_c[0]); // Angle from X-axis on XY plane in the component frame @@ -124,8 +124,8 @@ void Telescope::ObserveStars() { size_t count = 0; // Counter for while loop while (star_list_in_sight.size() < number_of_logged_stars_) { - s2e::math::Vector<3> target_b = hipparcos_->GetStarDirection_b(count, quaternion_i2b); - s2e::math::Vector<3> target_c = quaternion_b2c_.FrameConversion(target_b); + math::Vector<3> target_b = hipparcos_->GetStarDirection_b(count, quaternion_i2b); + math::Vector<3> target_c = quaternion_b2c_.FrameConversion(target_b); double arg_x = atan2(target_c[2], target_c[0]); // Angle from X-axis on XZ plane in the component frame double arg_y = atan2(target_c[1], target_c[0]); // Angle from X-axis on XY plane in the component frame @@ -176,12 +176,12 @@ void Telescope::ObserveGroundPositionDeviation() { } Quaternion quaternion_i2b = attitude_->GetQuaternion_i2b(); - s2e::math::Vector<3> spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m(); - s2e::math::Vector<3> direction_ecef = (initial_ground_position_ecef_m_ - spacecraft_position_ecef_m).CalcNormalizedVector(); - s2e::math::Matrix<3, 3> dcm_ecef_to_i = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); - s2e::math::Vector<3> direction_i = (dcm_ecef_to_i * direction_ecef).CalcNormalizedVector(); - s2e::math::Vector<3> direction_b = quaternion_i2b.FrameConversion(direction_i); - s2e::math::Vector<3> target_c = quaternion_b2c_.FrameConversion(direction_b); + math::Vector<3> spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m(); + math::Vector<3> direction_ecef = (initial_ground_position_ecef_m_ - spacecraft_position_ecef_m).CalcNormalizedVector(); + math::Matrix<3, 3> dcm_ecef_to_i = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); + math::Vector<3> direction_i = (dcm_ecef_to_i * direction_ecef).CalcNormalizedVector(); + math::Vector<3> direction_b = quaternion_i2b.FrameConversion(direction_i); + math::Vector<3> target_c = quaternion_b2c_.FrameConversion(direction_b); // Ground position in the image sensor in the satellite frame double ground_angle_z_rad = atan2(target_c[2], target_c[0]); @@ -195,61 +195,61 @@ string Telescope::GetLogHeader() const { std::string component_name = "telescope_"; - str_tmp += WriteScalar(component_name + "sun_in_exclusion_angle", ""); - str_tmp += WriteScalar(component_name + "earth_in_exclusion_angle", ""); - str_tmp += WriteScalar(component_name + "moon_in_exclusion_angle", ""); + str_tmp += logger::WriteScalar(component_name + "sun_in_exclusion_angle", ""); + str_tmp += logger::WriteScalar(component_name + "earth_in_exclusion_angle", ""); + str_tmp += logger::WriteScalar(component_name + "moon_in_exclusion_angle", ""); str_tmp += logger::WriteVector(component_name + "sun_position", "img", "pix", 2); str_tmp += logger::WriteVector(component_name + "earth_position", "img", "pix", 2); str_tmp += logger::WriteVector(component_name + "moon_position", "img", "pix", 2); - str_tmp += WriteScalar(component_name + "ground_position_x", "pix"); - str_tmp += WriteScalar(component_name + "ground_position_y", "pix"); + str_tmp += logger::WriteScalar(component_name + "ground_position_x", "pix"); + str_tmp += logger::WriteScalar(component_name + "ground_position_y", "pix"); // When Hipparcos Catalogue was not read, no output of ObserveStars if (hipparcos_->IsCalcEnabled) { for (size_t i = 0; i < number_of_logged_stars_; i++) { - str_tmp += WriteScalar(component_name + "hipparcos_id (" + to_string(i) + ")", " "); - str_tmp += WriteScalar(component_name + "visible_magnitude (" + to_string(i) + ")", " "); + str_tmp += logger::WriteScalar(component_name + "hipparcos_id (" + to_string(i) + ")", " "); + str_tmp += logger::WriteScalar(component_name + "visible_magnitude (" + to_string(i) + ")", " "); str_tmp += logger::WriteVector(component_name + "star_position (" + to_string(i) + ")", "img", "pix", 2); } } // Debug output ********************************************** - // str_tmp += WriteScalar("angle_sun", ""); - // str_tmp += WriteScalar("angle_earth", ""); - // str_tmp += WriteScalar("angle_moon", ""); + // str_tmp += logger::WriteScalar("angle_sun", ""); + // str_tmp += logger::WriteScalar("angle_earth", ""); + // str_tmp += logger::WriteScalar("angle_moon", ""); //********************************************************** return str_tmp; } string Telescope::GetLogValue() const { string str_tmp = ""; - str_tmp += WriteScalar(is_sun_in_forbidden_angle); - str_tmp += WriteScalar(is_earth_in_forbidden_angle); - str_tmp += WriteScalar(is_moon_in_forbidden_angle); + str_tmp += logger::WriteScalar(is_sun_in_forbidden_angle); + str_tmp += logger::WriteScalar(is_earth_in_forbidden_angle); + str_tmp += logger::WriteScalar(is_moon_in_forbidden_angle); str_tmp += logger::WriteVector(sun_position_image_sensor); str_tmp += logger::WriteVector(earth_position_image_sensor); str_tmp += logger::WriteVector(moon_position_image_sensor); - str_tmp += WriteScalar(ground_position_x_image_sensor_); - str_tmp += WriteScalar(ground_position_y_image_sensor_); + str_tmp += logger::WriteScalar(ground_position_x_image_sensor_); + str_tmp += logger::WriteScalar(ground_position_y_image_sensor_); // When Hipparcos Catalogue was not read, no output of ObserveStars if (hipparcos_->IsCalcEnabled) { for (size_t i = 0; i < number_of_logged_stars_; i++) { - str_tmp += WriteScalar(star_list_in_sight[i].hipparcos_data.hipparcos_id); - str_tmp += WriteScalar(star_list_in_sight[i].hipparcos_data.visible_magnitude); + str_tmp += logger::WriteScalar(star_list_in_sight[i].hipparcos_data.hipparcos_id); + str_tmp += logger::WriteScalar(star_list_in_sight[i].hipparcos_data.visible_magnitude); str_tmp += logger::WriteVector(star_list_in_sight[i].position_image_sensor); } } // Debug output ********************************************** - // str_tmp += WriteScalar(angle_sun); - // str_tmp += WriteScalar(angle_earth); - // str_tmp += WriteScalar(angle_moon); + // str_tmp += logger::WriteScalar(angle_sun); + // str_tmp += logger::WriteScalar(angle_earth); + // str_tmp += logger::WriteScalar(angle_moon); //********************************************************** return str_tmp; } Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) { - using s2e::math::pi; + using math::pi; setting_file_reader::IniAccess Telescope_conf(file_name); const string st_sensor_id = std::to_string(static_cast(sensor_id)); @@ -262,7 +262,7 @@ Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor strcat(TelescopeSection, cs); #endif - s2e::math::Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; Telescope_conf.ReadQuaternion(TelescopeSection, "quaternion_b2c", quaternion_b2c); double sun_forbidden_angle_deg = Telescope_conf.ReadDouble(TelescopeSection, "sun_exclusion_angle_deg"); diff --git a/src/components/real/mission/telescope.hpp b/src/components/real/mission/telescope.hpp index bad6d0fbf..d6c6310b5 100644 --- a/src/components/real/mission/telescope.hpp +++ b/src/components/real/mission/telescope.hpp @@ -25,7 +25,7 @@ namespace s2e::components { */ struct Star { HipparcosData hipparcos_data; //!< Hipparcos data - s2e::math::Vector<2> position_image_sensor; //!< Position of image sensor + math::Vector<2> position_image_sensor; //!< Position of image sensor }; /* @@ -52,7 +52,7 @@ class Telescope : public Component, public logger::ILoggable { * @param [in] local_celestial_information: Local celestial information * @param [in] orbit: Orbit information */ - Telescope(environment::ClockGenerator* clock_generator, const s2e::math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, + Telescope(environment::ClockGenerator* clock_generator, const math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr); @@ -69,8 +69,8 @@ class Telescope : public Component, public logger::ILoggable { protected: private: - s2e::math::Quaternion quaternion_b2c_; //!< Quaternion from the body frame to component frame - s2e::math::Vector<3> sight_direction_c_; //!< Sight direction vector in the component frame + math::Quaternion quaternion_b2c_; //!< Quaternion from the body frame to component frame + math::Vector<3> sight_direction_c_; //!< Sight direction vector in the component frame double sun_forbidden_angle_rad_; //!< Sun forbidden angle [rad] double earth_forbidden_angle_rad_; //!< Earth forbidden angle [rad] @@ -91,10 +91,10 @@ class Telescope : public Component, public logger::ILoggable { size_t number_of_logged_stars_; //!< Number of logged stars - s2e::math::Vector<2> sun_position_image_sensor{-1}; //!< Position of the sun on the image plane - s2e::math::Vector<2> earth_position_image_sensor{-1}; //!< Position of the earth on the image plane - s2e::math::Vector<2> moon_position_image_sensor{-1}; //!< Position of the moon on the image plane - s2e::math::Vector<3> initial_ground_position_ecef_m_; //!< Initial spacecraft position + math::Vector<2> sun_position_image_sensor{-1}; //!< Position of the sun on the image plane + math::Vector<2> earth_position_image_sensor{-1}; //!< Position of the earth on the image plane + math::Vector<2> moon_position_image_sensor{-1}; //!< Position of the moon on the image plane + math::Vector<3> initial_ground_position_ecef_m_; //!< Initial spacecraft position std::vector star_list_in_sight; //!< Star information in the field of view @@ -104,7 +104,7 @@ class Telescope : public Component, public logger::ILoggable { * @param [in] target_b: Direction vector of target on the body fixed frame * @param [in] forbidden_angle: Forbidden angle [rad] */ - bool JudgeForbiddenAngle(const s2e::math::Vector<3>& target_b, const double forbidden_angle); + bool JudgeForbiddenAngle(const math::Vector<3>& target_b, const double forbidden_angle); // Override functions for Component /** @@ -119,7 +119,7 @@ class Telescope : public Component, public logger::ILoggable { * @param [out] position_image_sensor: Position on image sensor plane * @param [in] target_b: Direction vector of target on the body fixed frame */ - void Observe(s2e::math::Vector<2>& position_image_sensor, const s2e::math::Vector<3, double> target_b); + void Observe(math::Vector<2>& position_image_sensor, const math::Vector<3, double> target_b); /** * @fn ObserveStars * @brief Observe stars from Hipparcos catalogue @@ -149,9 +149,9 @@ class Telescope : public Component, public logger::ILoggable { virtual std::string GetLogValue() const; // For debug ********************************************** - // s2e::math::Vector<3> sun_pos_c; - // s2e::math::Vector<3> earth_pos_c; - // s2e::math::Vector<3> moon_pos_c; + // math::Vector<3> sun_pos_c; + // math::Vector<3> earth_pos_c; + // math::Vector<3> moon_pos_c; // double angle_sun; // double angle_earth; // double angle_moon; diff --git a/src/components/real/power/battery.cpp b/src/components/real/power/battery.cpp index a6442ad3f..9637e5a23 100644 --- a/src/components/real/power/battery.cpp +++ b/src/components/real/power/battery.cpp @@ -58,15 +58,15 @@ Battery::~Battery() {} std::string Battery::GetLogHeader() const { std::string str_tmp = ""; std::string component_name = "battery_"; - str_tmp += WriteScalar(component_name + "voltage", "V"); - str_tmp += WriteScalar(component_name + "dod", "%"); + str_tmp += logger::WriteScalar(component_name + "voltage", "V"); + str_tmp += logger::WriteScalar(component_name + "dod", "%"); return str_tmp; } std::string Battery::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteScalar(battery_voltage_V_); - str_tmp += WriteScalar(depth_of_discharge_percent_); + str_tmp += logger::WriteScalar(battery_voltage_V_); + str_tmp += logger::WriteScalar(depth_of_discharge_percent_); return str_tmp; } diff --git a/src/components/real/power/csv_scenario_interface.cpp b/src/components/real/power/csv_scenario_interface.cpp index 51f2c2f4b..999369a02 100644 --- a/src/components/real/power/csv_scenario_interface.cpp +++ b/src/components/real/power/csv_scenario_interface.cpp @@ -38,8 +38,8 @@ void CsvScenarioInterface::Initialize(const std::string file_name) { bool CsvScenarioInterface::IsCsvScenarioEnabled() { return CsvScenarioInterface::is_csv_scenario_enabled_; } -s2e::math::Vector<3> CsvScenarioInterface::GetSunDirectionBody(const double time_query) { - s2e::math::Vector<3> sun_dir_b; +math::Vector<3> CsvScenarioInterface::GetSunDirectionBody(const double time_query) { + math::Vector<3> sun_dir_b; sun_dir_b[0] = GetValueFromBuffer("sun_dir_b_x", time_query); sun_dir_b[1] = GetValueFromBuffer("sun_dir_b_y", time_query); sun_dir_b[2] = GetValueFromBuffer("sun_dir_b_z", time_query); diff --git a/src/components/real/power/csv_scenario_interface.hpp b/src/components/real/power/csv_scenario_interface.hpp index f105dd773..98dc2180d 100644 --- a/src/components/real/power/csv_scenario_interface.hpp +++ b/src/components/real/power/csv_scenario_interface.hpp @@ -38,7 +38,7 @@ class CsvScenarioInterface { * @brief Return sun direction vector in the body fixed frame * @param [in] time_query: Time query */ - static s2e::math::Vector<3> GetSunDirectionBody(const double time_query); + static math::Vector<3> GetSunDirectionBody(const double time_query); /** * @fn GetSunFlag * @brief Return sun flag diff --git a/src/components/real/power/pcu_initial_study.cpp b/src/components/real/power/pcu_initial_study.cpp index e89729490..5a20fb79e 100644 --- a/src/components/real/power/pcu_initial_study.cpp +++ b/src/components/real/power/pcu_initial_study.cpp @@ -40,15 +40,15 @@ PcuInitialStudy::~PcuInitialStudy() {} std::string PcuInitialStudy::GetLogHeader() const { std::string str_tmp = ""; std::string component_name = "pcu_initial_study_"; - str_tmp += WriteScalar(component_name + "power_consumption", "W"); - str_tmp += WriteScalar(component_name + "bus_voltage", "V"); + str_tmp += logger::WriteScalar(component_name + "power_consumption", "W"); + str_tmp += logger::WriteScalar(component_name + "bus_voltage", "V"); return str_tmp; } std::string PcuInitialStudy::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteScalar(power_consumption_W_); - str_tmp += WriteScalar(bus_voltage_V_); + str_tmp += logger::WriteScalar(power_consumption_W_); + str_tmp += logger::WriteScalar(bus_voltage_V_); return str_tmp; } diff --git a/src/components/real/power/solar_array_panel.cpp b/src/components/real/power/solar_array_panel.cpp index df4bdad96..651e49eb2 100644 --- a/src/components/real/power/solar_array_panel.cpp +++ b/src/components/real/power/solar_array_panel.cpp @@ -12,7 +12,7 @@ namespace s2e::components { SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, - double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, double component_step_time_s) : Component(prescaler, clock_generator), @@ -31,7 +31,7 @@ SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerato } SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, - double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s) : Component(prescaler, clock_generator), component_id_(component_id), @@ -48,7 +48,7 @@ SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerato } SolarArrayPanel::SolarArrayPanel(environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, - s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) : Component(10, clock_generator), @@ -87,13 +87,13 @@ SolarArrayPanel::~SolarArrayPanel() {} std::string SolarArrayPanel::GetLogHeader() const { std::string str_tmp = ""; std::string component_name = "sap" + std::to_string(component_id_) + "_"; - str_tmp += WriteScalar(component_name + "generated_power", "W"); + str_tmp += logger::WriteScalar(component_name + "generated_power", "W"); return str_tmp; } std::string SolarArrayPanel::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteScalar(power_generation_W_); + str_tmp += logger::WriteScalar(power_generation_W_); return str_tmp; } @@ -101,14 +101,14 @@ void SolarArrayPanel::MainRoutine(const int time_count) { if (CsvScenarioInterface::IsCsvScenarioEnabled()) { double time_query = compo_step_time_s_ * time_count; const auto solar_constant = srp_environment_->GetSolarConstant_W_m2(); - s2e::math::Vector<3> sun_direction_body = CsvScenarioInterface::GetSunDirectionBody(time_query); - s2e::math::Vector<3> normalized_sun_direction_body = sun_direction_body.CalcNormalizedVector(); + math::Vector<3> sun_direction_body = CsvScenarioInterface::GetSunDirectionBody(time_query); + math::Vector<3> normalized_sun_direction_body = sun_direction_body.CalcNormalizedVector(); power_generation_W_ = cell_efficiency_ * transmission_efficiency_ * solar_constant * (int)CsvScenarioInterface::GetSunFlag(time_query) * cell_area_m2_ * number_of_parallel_ * number_of_series_ * InnerProduct(normal_vector_, normalized_sun_direction_body); } else { const auto power_density = srp_environment_->GetPowerDensity_W_m2(); - s2e::math::Vector<3> sun_pos_b = local_celestial_information_->GetPositionFromSpacecraft_b_m("SUN"); - s2e::math::Vector<3> sun_dir_b = sun_pos_b.CalcNormalizedVector(); + math::Vector<3> sun_pos_b = local_celestial_information_->GetPositionFromSpacecraft_b_m("SUN"); + math::Vector<3> sun_dir_b = sun_pos_b.CalcNormalizedVector(); power_generation_W_ = cell_efficiency_ * transmission_efficiency_ * power_density * cell_area_m2_ * number_of_parallel_ * number_of_series_ * InnerProduct(normal_vector_, sun_dir_b); // TODO: Improve implementation. For example, update IV curve with sun direction and calculate generated power @@ -135,7 +135,7 @@ SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id double cell_area_m2; cell_area_m2 = sap_conf.ReadDouble(section_name.c_str(), "cell_area_m2"); - s2e::math::Vector<3> normal_vector; + math::Vector<3> normal_vector; sap_conf.ReadVector(section_name.c_str(), "normal_vector_b", normal_vector); double cell_efficiency; @@ -168,7 +168,7 @@ SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id double cell_area_m2; cell_area_m2 = sap_conf.ReadDouble(section_name.c_str(), "cell_area_m2"); - s2e::math::Vector<3> normal_vector; + math::Vector<3> normal_vector; sap_conf.ReadVector(section_name.c_str(), "normal_vector_b", normal_vector); double cell_efficiency; diff --git a/src/components/real/power/solar_array_panel.hpp b/src/components/real/power/solar_array_panel.hpp index f057f5066..e77eecd3a 100644 --- a/src/components/real/power/solar_array_panel.hpp +++ b/src/components/real/power/solar_array_panel.hpp @@ -34,7 +34,7 @@ class SolarArrayPanel : public Component, public logger::ILoggable { * @param [in] component_step_time_s: Component step time [sec] */ SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, - double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, double component_step_time_s); /** @@ -53,7 +53,7 @@ class SolarArrayPanel : public Component, public logger::ILoggable { * @param [in] component_step_time_s: Component step time [sec] */ SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, - double cell_area_m2, s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s); /** * @fn SolarArrayPanel @@ -71,7 +71,7 @@ class SolarArrayPanel : public Component, public logger::ILoggable { * @param [in] local_celestial_information: Local celestial information */ SolarArrayPanel(environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, - s2e::math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, + math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); /** * @fn SolarArrayPanel @@ -113,7 +113,7 @@ class SolarArrayPanel : public Component, public logger::ILoggable { const int number_of_series_; //!< Number of series connected solar cells const int number_of_parallel_; //!< Number of parallel connected solar cells const double cell_area_m2_; //!< Solar cell area [m^2] - const s2e::math::Vector<3> normal_vector_; //!< Normal vector of SolarArrayPanel on the body fixed frame + const math::Vector<3> normal_vector_; //!< Normal vector of SolarArrayPanel on the body fixed frame const double cell_efficiency_; //!< Power generation efficiency of solar cell const double transmission_efficiency_; //!< Efficiency of transmission to PCU diff --git a/src/components/real/propulsion/simple_thruster.cpp b/src/components/real/propulsion/simple_thruster.cpp index 9b5b7c4b7..61b1ec766 100644 --- a/src/components/real/propulsion/simple_thruster.cpp +++ b/src/components/real/propulsion/simple_thruster.cpp @@ -13,7 +13,7 @@ namespace s2e::components { // Constructor SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, - const s2e::math::Vector<3> thruster_position_b_m, const s2e::math::Vector<3> thrust_direction_b, const double max_magnitude_N, + const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator), @@ -28,7 +28,7 @@ SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* } SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const s2e::math::Vector<3> thruster_position_b_m, const s2e::math::Vector<3> thrust_direction_b, const double max_magnitude_N, + const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator, power_port), @@ -68,9 +68,9 @@ void SimpleThruster::CalcThrust() { output_thrust_b_N_ = mag * CalcThrustDirection(); } -void SimpleThruster::CalcTorque(const s2e::math::Vector<3> center_of_mass_b_m) { - s2e::math::Vector<3> vector_center2thruster = thruster_position_b_m_ - center_of_mass_b_m; - s2e::math::Vector<3> torque = OuterProduct(vector_center2thruster, output_thrust_b_N_); +void SimpleThruster::CalcTorque(const math::Vector<3> center_of_mass_b_m) { + math::Vector<3> vector_center2thruster = thruster_position_b_m_ - center_of_mass_b_m; + math::Vector<3> torque = OuterProduct(vector_center2thruster, output_thrust_b_N_); output_torque_b_Nm_ = torque; } @@ -81,7 +81,7 @@ std::string SimpleThruster::GetLogHeader() const { std::string head = "simple_thruster" + std::to_string(component_id_) + "_"; str_tmp += logger::WriteVector(head + "output_thrust", "b", "N", 3); str_tmp += logger::WriteVector(head + "output_torque", "b", "Nm", 3); - str_tmp += WriteScalar(head + "output_thrust_norm", "N"); + str_tmp += logger::WriteScalar(head + "output_thrust_norm", "N"); return str_tmp; } @@ -90,32 +90,32 @@ std::string SimpleThruster::GetLogValue() const { str_tmp += logger::WriteVector(output_thrust_b_N_); str_tmp += logger::WriteVector(output_torque_b_Nm_); - str_tmp += WriteScalar(output_thrust_b_N_.CalcNorm()); + str_tmp += logger::WriteScalar(output_thrust_b_N_.CalcNorm()); return str_tmp; } double SimpleThruster::CalcThrustMagnitude() { return duty_ * thrust_magnitude_max_N_; } -s2e::math::Vector<3> SimpleThruster::CalcThrustDirection() { - s2e::math::Vector<3> thrust_dir_b_true = thrust_direction_b_; +math::Vector<3> SimpleThruster::CalcThrustDirection() { + math::Vector<3> thrust_dir_b_true = thrust_direction_b_; if (direction_noise_standard_deviation_rad_ > 0.0 + DBL_EPSILON) { - s2e::math::Vector<3> ex; // Fixme: to use outer product to generate orthogonal vector + math::Vector<3> ex; // Fixme: to use outer product to generate orthogonal vector ex[0] = 1.0; ex[1] = 0.0; ex[2] = 0.0; int flag = rand() % 2; double make_axis_rot_rad; if (flag == 0) { - make_axis_rot_rad = s2e::math::pi * (double)rand() / RAND_MAX; + make_axis_rot_rad = math::pi * (double)rand() / RAND_MAX; } else { - make_axis_rot_rad = -s2e::math::pi * (double)rand() / RAND_MAX; + make_axis_rot_rad = -math::pi * (double)rand() / RAND_MAX; } - s2e::math::Quaternion make_axis_rot(thrust_dir_b_true, make_axis_rot_rad); - s2e::math::Vector<3> axis_rot = make_axis_rot.FrameConversion(ex); + math::Quaternion make_axis_rot(thrust_dir_b_true, make_axis_rot_rad); + math::Vector<3> axis_rot = make_axis_rot.FrameConversion(ex); - s2e::math::Quaternion err_rot(axis_rot, direction_random_noise_); // Generate error quaternion + math::Quaternion err_rot(axis_rot, direction_random_noise_); // Generate error quaternion thrust_dir_b_true = err_rot.FrameConversion(thrust_dir_b_true); // Add error } @@ -143,7 +143,7 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, magnitude_standard_deviation_N = thruster_conf.ReadDouble(Section, "thrust_error_standard_deviation_N"); double deg_err; - deg_err = thruster_conf.ReadDouble(Section, "direction_error_standard_deviation_deg") * s2e::math::pi / 180.0; + deg_err = thruster_conf.ReadDouble(Section, "direction_error_standard_deviation_deg") * math::pi / 180.0; SimpleThruster thruster(prescaler, clock_generator, thruster_id, thruster_pos, thruster_dir, max_magnitude_N, magnitude_standard_deviation_N, deg_err, structure, dynamics); @@ -171,7 +171,7 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, magnitude_standard_deviation_N = thruster_conf.ReadDouble(Section, "thrust_error_standard_deviation_N"); double deg_err; - deg_err = thruster_conf.ReadDouble(Section, "direction_error_standard_deviation_deg") * s2e::math::pi / 180.0; + deg_err = thruster_conf.ReadDouble(Section, "direction_error_standard_deviation_deg") * math::pi / 180.0; power_port->InitializeWithInitializeFile(file_name); diff --git a/src/disturbances/air_drag.cpp b/src/disturbances/air_drag.cpp index 163a80cbd..c4f306523 100644 --- a/src/disturbances/air_drag.cpp +++ b/src/disturbances/air_drag.cpp @@ -14,7 +14,7 @@ namespace s2e::disturbances { -AirDrag::AirDrag(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, +AirDrag::AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, const double molecular_temperature_K, const double molecular_weight_g_mol, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled), wall_temperature_K_(wall_temperature_K), @@ -28,15 +28,15 @@ AirDrag::AirDrag(const std::vector& surfaces, const s2e::math::Vector<3 void AirDrag::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { double air_density_kg_m3 = local_environment.GetAtmosphere().GetAirDensity_kg_m3(); - s2e::math::Matrix<3, 3> dcm_ecef2eci = + math::Matrix<3, 3> dcm_ecef2eci = local_environment.GetCelestialInformation().GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); - s2e::math::Vector<3> relative_velocity_wrt_atmosphere_i_m_s = dcm_ecef2eci * dynamics.GetOrbit().GetVelocity_ecef_m_s(); - s2e::math::Quaternion quaternion_i2b = dynamics.GetAttitude().GetQuaternion_i2b(); - s2e::math::Vector<3> velocity_b_m_s = quaternion_i2b.FrameConversion(relative_velocity_wrt_atmosphere_i_m_s); + math::Vector<3> relative_velocity_wrt_atmosphere_i_m_s = dcm_ecef2eci * dynamics.GetOrbit().GetVelocity_ecef_m_s(); + math::Quaternion quaternion_i2b = dynamics.GetAttitude().GetQuaternion_i2b(); + math::Vector<3> velocity_b_m_s = quaternion_i2b.FrameConversion(relative_velocity_wrt_atmosphere_i_m_s); CalcTorqueForce(velocity_b_m_s, air_density_kg_m3); } -void AirDrag::CalcCoefficients(const s2e::math::Vector<3>& velocity_b_m_s, const double air_density_kg_m3) { +void AirDrag::CalcCoefficients(const math::Vector<3>& velocity_b_m_s, const double air_density_kg_m3) { double velocity_norm_m_s = velocity_b_m_s.CalcNorm(); CalcCnCt(velocity_b_m_s); for (size_t i = 0; i < surfaces_.size(); i++) { @@ -49,14 +49,14 @@ void AirDrag::CalcCoefficients(const s2e::math::Vector<3>& velocity_b_m_s, const double AirDrag::CalcFunctionPi(const double s) { double x; double erfs = erf(s); // ERF function is defined in math standard library - x = s * exp(-s * s) + sqrt(s2e::math::pi) * (s * s + 0.5) * (1.0 + erfs); + x = s * exp(-s * s) + sqrt(math::pi) * (s * s + 0.5) * (1.0 + erfs); return x; } double AirDrag::CalcFunctionChi(const double s) { double x; double erfs = erf(s); - x = exp(-s * s) + sqrt(s2e::math::pi) * s * (1.0 + erfs); + x = exp(-s * s) + sqrt(math::pi) * s * (1.0 + erfs); return x; } @@ -71,9 +71,9 @@ void AirDrag::CalcCnCt(const Vector<3>& velocity_b_m_s) { double speed_n = speed * cos_theta_[i]; double speed_t = speed * sin_theta_[i]; double diffuse = 1.0 - surfaces_[i].GetAirSpecularity(); - cn_[i] = (2.0 - diffuse) / sqrt(s2e::math::pi) * CalcFunctionPi(speed_n) / (speed * speed) + + cn_[i] = (2.0 - diffuse) / sqrt(math::pi) * CalcFunctionPi(speed_n) / (speed * speed) + diffuse / 2.0 * CalcFunctionChi(speed_n) / (speed * speed) * sqrt(wall_temperature_K_ / molecular_temperature_K_); - ct_[i] = diffuse * speed_t * CalcFunctionChi(speed_n) / (sqrt(s2e::math::pi) * speed * speed); + ct_[i] = diffuse * speed_t * CalcFunctionChi(speed_n) / (sqrt(math::pi) * speed * speed); } } diff --git a/src/disturbances/air_drag.hpp b/src/disturbances/air_drag.hpp index 7f87e9bb7..6c031cf0b 100644 --- a/src/disturbances/air_drag.hpp +++ b/src/disturbances/air_drag.hpp @@ -32,7 +32,7 @@ class AirDrag : public SurfaceForce { * @param [in] molecular_weight_g_mol: Molecular weight [g/mol] * @param [in] is_calculation_enabled: Calculation flag */ - AirDrag(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, + AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, const double molecular_temperature_K, const double molecular_weight_g_mol, const bool is_calculation_enabled = true); /** @@ -68,7 +68,7 @@ class AirDrag : public SurfaceForce { * @param [in] velocity_b_m_s: Spacecraft's velocity vector in the body frame [m/s] * @param [in] air_density_kg_m3: Air density around the spacecraft [kg/m^3] */ - void CalcCoefficients(const s2e::math::Vector<3>& velocity_b_m_s, const double air_density_kg_m3); + void CalcCoefficients(const math::Vector<3>& velocity_b_m_s, const double air_density_kg_m3); // internal function for calculation /** @@ -76,7 +76,7 @@ class AirDrag : public SurfaceForce { * @brief Calculate the Cn and Ct * @param [in] velocity_b_m_s: Spacecraft's velocity vector in the body frame [m/s] */ - void CalcCnCt(const s2e::math::Vector<3>& velocity_b_m_s); + void CalcCnCt(const math::Vector<3>& velocity_b_m_s); /** * @fn CalcFunctionPi * @brief Calculate The Pi function in the algorithm diff --git a/src/disturbances/disturbance.hpp b/src/disturbances/disturbance.hpp index de7d476b8..6321f06d2 100644 --- a/src/disturbances/disturbance.hpp +++ b/src/disturbances/disturbance.hpp @@ -25,10 +25,10 @@ class Disturbance : public logger::ILoggable { */ Disturbance(const bool is_calculation_enabled = true, const bool is_attitude_dependent = true) : is_calculation_enabled_(is_calculation_enabled), is_attitude_dependent_(is_attitude_dependent) { - force_b_N_ = s2e::math::Vector<3>(0.0); - torque_b_Nm_ = s2e::math::Vector<3>(0.0); - acceleration_i_m_s2_ = s2e::math::Vector<3>(0.0); - acceleration_b_m_s2_ = s2e::math::Vector<3>(0.0); + force_b_N_ = math::Vector<3>(0.0); + torque_b_Nm_ = math::Vector<3>(0.0); + acceleration_i_m_s2_ = math::Vector<3>(0.0); + acceleration_b_m_s2_ = math::Vector<3>(0.0); } /** @@ -62,22 +62,22 @@ class Disturbance : public logger::ILoggable { * @fn GetTorque_b_Nm * @brief Return the disturbance torque in the body frame [Nm] */ - virtual inline s2e::math::Vector<3> GetTorque_b_Nm() { return torque_b_Nm_; } + virtual inline math::Vector<3> GetTorque_b_Nm() { return torque_b_Nm_; } /** * @fn GetForce_b_N * @brief Return the disturbance force in the body frame [N] */ - virtual inline s2e::math::Vector<3> GetForce_b_N() { return force_b_N_; } + virtual inline math::Vector<3> GetForce_b_N() { return force_b_N_; } /** * @fn GetAcceleration_b_m_s2 * @brief Return the disturbance acceleration in the body frame [m/s2] */ - virtual inline s2e::math::Vector<3> GetAcceleration_b_m_s2() { return acceleration_b_m_s2_; } + virtual inline math::Vector<3> GetAcceleration_b_m_s2() { return acceleration_b_m_s2_; } /** * @fn GetAcceleration_i_m_s2 * @brief Return the disturbance acceleration in the inertial frame [m/s2] */ - virtual inline s2e::math::Vector<3> GetAcceleration_i_m_s2() { return acceleration_i_m_s2_; } + virtual inline math::Vector<3> GetAcceleration_i_m_s2() { return acceleration_i_m_s2_; } /** * @fn IsAttitudeDependent * @brief Return the attitude dependent flag @@ -87,10 +87,10 @@ class Disturbance : public logger::ILoggable { protected: bool is_calculation_enabled_; //!< Flag to calculate the disturbance bool is_attitude_dependent_; //!< Flag to show the disturbance depends on attitude information - s2e::math::Vector<3> force_b_N_; //!< Disturbance force in the body frame [N] - s2e::math::Vector<3> torque_b_Nm_; //!< Disturbance torque in the body frame [Nm] - s2e::math::Vector<3> acceleration_b_m_s2_; //!< Disturbance acceleration in the body frame [m/s2] - s2e::math::Vector<3> acceleration_i_m_s2_; //!< Disturbance acceleration in the inertial frame [m/s2] + math::Vector<3> force_b_N_; //!< Disturbance force in the body frame [N] + math::Vector<3> torque_b_Nm_; //!< Disturbance torque in the body frame [Nm] + math::Vector<3> acceleration_b_m_s2_; //!< Disturbance acceleration in the body frame [m/s2] + math::Vector<3> acceleration_i_m_s2_; //!< Disturbance acceleration in the inertial frame [m/s2] }; } // namespace s2e::disturbances diff --git a/src/disturbances/disturbances.hpp b/src/disturbances/disturbances.hpp index 590dbc703..a3f57b39a 100644 --- a/src/disturbances/disturbances.hpp +++ b/src/disturbances/disturbances.hpp @@ -57,19 +57,19 @@ class Disturbances { * @fn GetTorque * @brief Return total disturbance torque in the body frame [Nm] */ - inline s2e::math::Vector<3> GetTorque_b_Nm() { return total_torque_b_Nm_; } + inline math::Vector<3> GetTorque_b_Nm() { return total_torque_b_Nm_; } /** * @fn GetTorque * @brief Return total disturbance force in the body frame [N] */ - inline s2e::math::Vector<3> GetForce_b_N() { return total_force_b_N_; } + inline math::Vector<3> GetForce_b_N() { return total_force_b_N_; } /** * @fn GetTorque * @brief Return total disturbance acceleration in the inertial frame [m/s2] */ - inline s2e::math::Vector<3> GetAcceleration_i_m_s2() { return total_acceleration_i_m_s2_; } + inline math::Vector<3> GetAcceleration_i_m_s2() { return total_acceleration_i_m_s2_; } private: std::string initialize_file_name_; //!< Initialization file name diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index 40cbb3515..dffba8b93 100644 --- a/src/disturbances/geopotential.cpp +++ b/src/disturbances/geopotential.cpp @@ -22,8 +22,8 @@ namespace s2e::disturbances { Geopotential::Geopotential(const int degree, const std::string file_path, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, false), degree_(degree) { // Initialize - acceleration_ecef_m_s2_ = s2e::math::Vector<3>(0.0); - debug_pos_ecef_m_ = s2e::math::Vector<3>(0.0); + acceleration_ecef_m_s2_ = math::Vector<3>(0.0); + debug_pos_ecef_m_ = math::Vector<3>(0.0); // degree if (degree_ > 360) { degree_ = 360; @@ -85,8 +85,8 @@ void Geopotential::Update(const LocalEnvironment &local_environment, const dynam UNUSED(time_ms_); #endif - s2e::math::Matrix<3, 3> trans_eci2ecef_ = local_environment.GetCelestialInformation().GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef(); - s2e::math::Matrix<3, 3> trans_ecef2eci = trans_eci2ecef_.Transpose(); + math::Matrix<3, 3> trans_eci2ecef_ = local_environment.GetCelestialInformation().GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef(); + math::Matrix<3, 3> trans_ecef2eci = trans_eci2ecef_.Transpose(); acceleration_i_m_s2_ = trans_ecef2eci * acceleration_ecef_m_s2_; } @@ -95,7 +95,7 @@ std::string Geopotential::GetLogHeader() const { #ifdef DEBUG_GEOPOTENTIAL str_tmp += logger::WriteVector("geopotential_calculation_position_", "ecef", "m", 3); - str_tmp += WriteScalar("geopotential_calculation_time", "ms"); + str_tmp += logger::WriteScalar("geopotential_calculation_time", "ms"); #endif str_tmp += logger::WriteVector("geopotential_acceleration", "ecef", "m/s2", 3); @@ -107,7 +107,7 @@ std::string Geopotential::GetLogValue() const { #ifdef DEBUG_GEOPOTENTIAL str_tmp += logger::WriteVector(debug_pos_ecef_m_, 15); - str_tmp += WriteScalar(time_ms_); + str_tmp += logger::WriteScalar(time_ms_); #endif str_tmp += logger::WriteVector(acceleration_ecef_m_s2_, 15); diff --git a/src/disturbances/geopotential.hpp b/src/disturbances/geopotential.hpp index 3bfcb83fa..c82f792b1 100644 --- a/src/disturbances/geopotential.hpp +++ b/src/disturbances/geopotential.hpp @@ -70,7 +70,7 @@ class Geopotential : public Disturbance { Vector<3> acceleration_ecef_m_s2_; //!< Calculated acceleration in the ECEF frame [m/s2] // debug - s2e::math::Vector<3> debug_pos_ecef_m_; //!< Spacecraft position in ECEF frame [m] + math::Vector<3> debug_pos_ecef_m_; //!< Spacecraft position in ECEF frame [m] double time_ms_ = 0.0; //!< Calculation time [ms] /** diff --git a/src/disturbances/gravity_gradient.cpp b/src/disturbances/gravity_gradient.cpp index 366c77778..5129ec5be 100644 --- a/src/disturbances/gravity_gradient.cpp +++ b/src/disturbances/gravity_gradient.cpp @@ -25,9 +25,9 @@ void GravityGradient::Update(const LocalEnvironment& local_environment, const dy dynamics.GetAttitude().GetInertiaTensor_b_kgm2()); } -s2e::math::Vector<3> GravityGradient::CalcTorque_b_Nm(const s2e::math::Vector<3> earth_position_from_sc_b_m, const s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2) { +math::Vector<3> GravityGradient::CalcTorque_b_Nm(const math::Vector<3> earth_position_from_sc_b_m, const math::Matrix<3, 3> inertia_tensor_b_kgm2) { double r_norm_m = earth_position_from_sc_b_m.CalcNorm(); - s2e::math::Vector<3> u_b = earth_position_from_sc_b_m; // TODO: make undestructive normalize function for Vector + math::Vector<3> u_b = earth_position_from_sc_b_m; // TODO: make undestructive normalize function for Vector u_b /= r_norm_m; double coeff = 3.0 * gravity_constant_m3_s2_ / pow(r_norm_m, 3.0); diff --git a/src/disturbances/gravity_gradient.hpp b/src/disturbances/gravity_gradient.hpp index e4a578aff..b58b8df33 100644 --- a/src/disturbances/gravity_gradient.hpp +++ b/src/disturbances/gravity_gradient.hpp @@ -67,7 +67,7 @@ class GravityGradient : public Disturbance { * @param [in] inertia_tensor_b_kgm2: Inertia Tensor at body frame [kg*m^2] * @return Calculated torque at body frame [Nm] */ - s2e::math::Vector<3> CalcTorque_b_Nm(const s2e::math::Vector<3> earth_position_from_sc_b_m, const s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2); + math::Vector<3> CalcTorque_b_Nm(const math::Vector<3> earth_position_from_sc_b_m, const math::Matrix<3, 3> inertia_tensor_b_kgm2); }; /** diff --git a/src/disturbances/lunar_gravity_field.cpp b/src/disturbances/lunar_gravity_field.cpp index d28ccfe76..3511537e6 100644 --- a/src/disturbances/lunar_gravity_field.cpp +++ b/src/disturbances/lunar_gravity_field.cpp @@ -22,8 +22,8 @@ namespace s2e::disturbances { LunarGravityField::LunarGravityField(const int degree, const std::string file_path, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, false), degree_(degree) { // Initialize - acceleration_mcmf_m_s2_ = s2e::math::Vector<3>(0.0); - debug_pos_mcmf_m_ = s2e::math::Vector<3>(0.0); + acceleration_mcmf_m_s2_ = math::Vector<3>(0.0); + debug_pos_mcmf_m_ = math::Vector<3>(0.0); debug_pos_mcmf_m_[0] = 2000000; debug_pos_mcmf_m_[1] = 2000000; debug_pos_mcmf_m_[2] = 2000000; @@ -90,10 +90,10 @@ bool LunarGravityField::ReadCoefficientsGrgm1200a(std::string file_name) { void LunarGravityField::Update(const LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics) { const CelestialInformation global_celestial_information = local_environment.GetCelestialInformation().GetGlobalInformation(); - s2e::math::Matrix<3, 3> dcm_mci2mcmf_ = global_celestial_information.GetMoonRotation().GetDcmJ2000ToMcmf(); + math::Matrix<3, 3> dcm_mci2mcmf_ = global_celestial_information.GetMoonRotation().GetDcmJ2000ToMcmf(); - s2e::math::Vector<3> spacecraft_position_mci_m = dynamics.GetOrbit().GetPosition_i_m(); - s2e::math::Vector<3> spacecraft_position_mcmf_m = dcm_mci2mcmf_ * spacecraft_position_mci_m; + math::Vector<3> spacecraft_position_mci_m = dynamics.GetOrbit().GetPosition_i_m(); + math::Vector<3> spacecraft_position_mcmf_m = dcm_mci2mcmf_ * spacecraft_position_mci_m; #ifdef DEBUG_LUNAR_GRAVITY_FIELD std::chrono::system_clock::time_point start, end; @@ -109,7 +109,7 @@ void LunarGravityField::Update(const LocalEnvironment &local_environment, const UNUSED(time_ms_); #endif - s2e::math::Matrix<3, 3> dcm_mcmf2i = dcm_mci2mcmf_.Transpose(); + math::Matrix<3, 3> dcm_mcmf2i = dcm_mci2mcmf_.Transpose(); acceleration_i_m_s2_ = dcm_mcmf2i * acceleration_mcmf_m_s2_; } @@ -118,7 +118,7 @@ std::string LunarGravityField::GetLogHeader() const { #ifdef DEBUG_LUNAR_GRAVITY_FIELD str_tmp += logger::WriteVector("lunar_gravity_calculation_position", "mcmf", "m", 3); - str_tmp += WriteScalar("lunar_gravity_calculation_time", "ms"); + str_tmp += logger::WriteScalar("lunar_gravity_calculation_time", "ms"); #endif str_tmp += logger::WriteVector("lunar_gravity_acceleration", "mcmf", "m/s2", 3); @@ -130,7 +130,7 @@ std::string LunarGravityField::GetLogValue() const { #ifdef DEBUG_LUNAR_GRAVITY_FIELD str_tmp += logger::WriteVector(debug_pos_mcmf_m_, 15); - str_tmp += WriteScalar(time_ms_); + str_tmp += logger::WriteScalar(time_ms_); #endif str_tmp += logger::WriteVector(acceleration_mcmf_m_s2_, 15); diff --git a/src/disturbances/lunar_gravity_field.hpp b/src/disturbances/lunar_gravity_field.hpp index 552e0425b..40b5d0e7d 100644 --- a/src/disturbances/lunar_gravity_field.hpp +++ b/src/disturbances/lunar_gravity_field.hpp @@ -74,7 +74,7 @@ class LunarGravityField : public Disturbance { Vector<3> acceleration_mcmf_m_s2_; //!< Calculated acceleration in the MCMF(Moon Centered Moon Fixed) frame [m/s2] // debug - s2e::math::Vector<3> debug_pos_mcmf_m_; //!< Spacecraft position in MCMF frame [m] + math::Vector<3> debug_pos_mcmf_m_; //!< Spacecraft position in MCMF frame [m] double time_ms_ = 0.0; //!< Calculation time [ms] /** diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index 98c352c32..11fc64530 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -33,8 +33,8 @@ void MagneticDisturbance::Update(const LocalEnvironment& local_environment, cons } void MagneticDisturbance::CalcRMM() { - static s2e::math::Vector<3> random_walk_std_dev(residual_magnetic_moment_.GetRandomWalkStandardDeviation_Am2()); - static s2e::math::Vector<3> random_walk_limit(residual_magnetic_moment_.GetRandomWalkLimit_Am2()); + static math::Vector<3> random_walk_std_dev(residual_magnetic_moment_.GetRandomWalkStandardDeviation_Am2()); + static math::Vector<3> random_walk_limit(residual_magnetic_moment_.GetRandomWalkLimit_Am2()); static RandomWalk<3> random_walk(0.1, random_walk_std_dev, random_walk_limit); // [FIXME] step width is constant static s2e::randomization::NormalRand normal_random(0.0, residual_magnetic_moment_.GetRandomNoiseStandardDeviation_Am2(), s2e::randomization::global_randomization.MakeSeed()); diff --git a/src/disturbances/magnetic_disturbance.hpp b/src/disturbances/magnetic_disturbance.hpp index 79f795ecf..7b5187598 100644 --- a/src/disturbances/magnetic_disturbance.hpp +++ b/src/disturbances/magnetic_disturbance.hpp @@ -52,7 +52,7 @@ class MagneticDisturbance : public Disturbance { private: const double kMagUnit_ = 1.0e-9; //!< Constant value to change the unit [nT] -> [T] - s2e::math::Vector<3> rmm_b_Am2_; //!< True RMM of the spacecraft in the body frame [Am2] + math::Vector<3> rmm_b_Am2_; //!< True RMM of the spacecraft in the body frame [Am2] const ResidualMagneticMoment& residual_magnetic_moment_; //!< RMM parameters /** @@ -66,7 +66,7 @@ class MagneticDisturbance : public Disturbance { * @param [in] magnetic_field_b_nT: Magnetic field vector at the body frame [nT] * @return Calculated disturbance torque in body frame [Nm] */ - s2e::math::Vector<3> CalcTorque_b_Nm(const s2e::math::Vector<3>& magnetic_field_b_nT); + math::Vector<3> CalcTorque_b_Nm(const math::Vector<3>& magnetic_field_b_nT); }; /** diff --git a/src/disturbances/solar_radiation_pressure_disturbance.cpp b/src/disturbances/solar_radiation_pressure_disturbance.cpp index 18d66fd44..638a19833 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.cpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.cpp @@ -13,17 +13,17 @@ namespace s2e::disturbances { SolarRadiationPressureDisturbance::SolarRadiationPressureDisturbance(const std::vector& surfaces, - const s2e::math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) + const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled) {} void SolarRadiationPressureDisturbance::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { UNUSED(dynamics); - s2e::math::Vector<3> sun_position_from_sc_b_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_b_m("SUN"); + math::Vector<3> sun_position_from_sc_b_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_b_m("SUN"); CalcTorqueForce(sun_position_from_sc_b_m, local_environment.GetSolarRadiationPressure().GetPressure_N_m2()); } -void SolarRadiationPressureDisturbance::CalcCoefficients(const s2e::math::Vector<3>& input_direction_b, const double item) { +void SolarRadiationPressureDisturbance::CalcCoefficients(const math::Vector<3>& input_direction_b, const double item) { UNUSED(input_direction_b); for (size_t i = 0; i < surfaces_.size(); i++) { // Calculate for each surface diff --git a/src/disturbances/solar_radiation_pressure_disturbance.hpp b/src/disturbances/solar_radiation_pressure_disturbance.hpp index 871538ba2..63f20e4e0 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.hpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.hpp @@ -27,7 +27,7 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { * @param [in] center_of_gravity_b_m: Center of gravity position at the body frame [m] * @param [in] is_calculation_enabled: Calculation flag */ - SolarRadiationPressureDisturbance(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, + SolarRadiationPressureDisturbance(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled = true); /** @@ -57,7 +57,7 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { * @param [in] input_direction_b: Direction vector of the sun at the body frame * @param [in] item: Solar pressure [N/m^2] */ - void CalcCoefficients(const s2e::math::Vector<3>& input_direction_b, const double item); + void CalcCoefficients(const math::Vector<3>& input_direction_b, const double item); }; /** diff --git a/src/disturbances/surface_force.cpp b/src/disturbances/surface_force.cpp index 1bf0b1dfe..0cef5b365 100644 --- a/src/disturbances/surface_force.cpp +++ b/src/disturbances/surface_force.cpp @@ -9,7 +9,7 @@ namespace s2e::disturbances { -SurfaceForce::SurfaceForce(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) +SurfaceForce::SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, true), surfaces_(surfaces), center_of_gravity_b_m_(center_of_gravity_b_m) { // Initialize vectors size_t num = surfaces_.size(); @@ -19,23 +19,23 @@ SurfaceForce::SurfaceForce(const std::vector& surfaces, const s2e::math sin_theta_.assign(num, 0.0); } -s2e::math::Vector<3> SurfaceForce::CalcTorqueForce(s2e::math::Vector<3>& input_direction_b, double item) { +math::Vector<3> SurfaceForce::CalcTorqueForce(math::Vector<3>& input_direction_b, double item) { CalcTheta(input_direction_b); CalcCoefficients(input_direction_b, item); - s2e::math::Vector<3> force_b_N(0.0); - s2e::math::Vector<3> torque_b_Nm(0.0); - s2e::math::Vector<3> input_b_normal = input_direction_b.CalcNormalizedVector(); + math::Vector<3> force_b_N(0.0); + math::Vector<3> torque_b_Nm(0.0); + math::Vector<3> input_b_normal = input_direction_b.CalcNormalizedVector(); for (size_t i = 0; i < surfaces_.size(); i++) { if (cos_theta_[i] > 0.0) { // if the surface faces to the disturbance source (sun or air) // calc direction of in-plane force - s2e::math::Vector<3> normal = surfaces_[i].GetNormal_b(); - s2e::math::Vector<3> ncu = OuterProduct(input_b_normal, normal); - s2e::math::Vector<3> ncu_normalized = ncu.CalcNormalizedVector(); - s2e::math::Vector<3> in_plane_force_direction = OuterProduct(ncu_normalized, normal); + math::Vector<3> normal = surfaces_[i].GetNormal_b(); + math::Vector<3> ncu = OuterProduct(input_b_normal, normal); + math::Vector<3> ncu_normalized = ncu.CalcNormalizedVector(); + math::Vector<3> in_plane_force_direction = OuterProduct(ncu_normalized, normal); // calc force - s2e::math::Vector<3> force_per_surface_b_N = -1.0 * normal_coefficients_[i] * normal + tangential_coefficients_[i] * in_plane_force_direction; + math::Vector<3> force_per_surface_b_N = -1.0 * normal_coefficients_[i] * normal + tangential_coefficients_[i] * in_plane_force_direction; force_b_N += force_per_surface_b_N; // calc torque torque_b_Nm += OuterProduct(surfaces_[i].GetPosition_b_m() - center_of_gravity_b_m_, force_per_surface_b_N); @@ -46,8 +46,8 @@ s2e::math::Vector<3> SurfaceForce::CalcTorqueForce(s2e::math::Vector<3>& input_d return torque_b_Nm_; } -void SurfaceForce::CalcTheta(s2e::math::Vector<3>& input_direction_b) { - s2e::math::Vector<3> input_b_normal = input_direction_b.CalcNormalizedVector(); +void SurfaceForce::CalcTheta(math::Vector<3>& input_direction_b) { + math::Vector<3> input_b_normal = input_direction_b.CalcNormalizedVector(); for (size_t i = 0; i < surfaces_.size(); i++) { cos_theta_[i] = InnerProduct(surfaces_[i].GetNormal_b(), input_b_normal); diff --git a/src/disturbances/surface_force.hpp b/src/disturbances/surface_force.hpp index 943daa9e6..c6c0c3d06 100644 --- a/src/disturbances/surface_force.hpp +++ b/src/disturbances/surface_force.hpp @@ -30,7 +30,7 @@ class SurfaceForce : public Disturbance { * @param [in] center_of_gravity_b_m: Center of gravity position at the body frame [m] * @param [in] is_calculation_enabled: Calculation flag */ - SurfaceForce(const std::vector& surfaces, const s2e::math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled = true); + SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled = true); /** * @fn ~SurfaceForce * @brief Destructor @@ -40,7 +40,7 @@ class SurfaceForce : public Disturbance { protected: // Spacecraft Structure parameters const std::vector& surfaces_; //!< List of surfaces - const s2e::math::Vector<3>& center_of_gravity_b_m_; //!< Position vector of the center of mass_kg at body frame [m] + const math::Vector<3>& center_of_gravity_b_m_; //!< Position vector of the center of mass_kg at body frame [m] // Internal calculated variables std::vector normal_coefficients_; //!< coefficients for out-plane force for each surface @@ -56,13 +56,13 @@ class SurfaceForce : public Disturbance { * @param [in] item: Parameter which decide the magnitude of the disturbances (e.g., Solar flux, air density) * @return Calculated disturbance torque in body frame [Nm] */ - s2e::math::Vector<3> CalcTorqueForce(s2e::math::Vector<3>& input_direction_b, double item); + math::Vector<3> CalcTorqueForce(math::Vector<3>& input_direction_b, double item); /** * @fn CalcTheta * @brief Calculate cosX and sinX * @param [in] input_direction_b: Direction of disturbance source at the body frame */ - void CalcTheta(s2e::math::Vector<3>& input_direction_b); + void CalcTheta(math::Vector<3>& input_direction_b); /** * @fn CalcCoefficients @@ -70,7 +70,7 @@ class SurfaceForce : public Disturbance { * @param [in] input_direction_b: Direction of disturbance source at the body frame * @param [in] item: Parameter which decide the magnitude of the disturbances (e.g., Solar flux, air density) */ - virtual void CalcCoefficients(const s2e::math::Vector<3>& input_direction_b, const double item) = 0; + virtual void CalcCoefficients(const math::Vector<3>& input_direction_b, const double item) = 0; }; } // namespace s2e::disturbances diff --git a/src/disturbances/third_body_gravity.cpp b/src/disturbances/third_body_gravity.cpp index 2598332a0..7d99ab2fb 100644 --- a/src/disturbances/third_body_gravity.cpp +++ b/src/disturbances/third_body_gravity.cpp @@ -11,18 +11,18 @@ namespace s2e::disturbances { ThirdBodyGravity::ThirdBodyGravity(std::set third_body_list, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, false), third_body_list_(third_body_list) { - acceleration_i_m_s2_ = s2e::math::Vector<3>(0.0); + acceleration_i_m_s2_ = math::Vector<3>(0.0); } ThirdBodyGravity::~ThirdBodyGravity() {} void ThirdBodyGravity::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { - acceleration_i_m_s2_ = s2e::math::Vector<3>(0.0); // initialize + acceleration_i_m_s2_ = math::Vector<3>(0.0); // initialize - s2e::math::Vector<3> sc_position_i_m = dynamics.GetOrbit().GetPosition_i_m(); + math::Vector<3> sc_position_i_m = dynamics.GetOrbit().GetPosition_i_m(); for (auto third_body : third_body_list_) { - s2e::math::Vector<3> third_body_position_from_sc_i_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_i_m(third_body.c_str()); - s2e::math::Vector<3> third_body_pos_i_m = sc_position_i_m + third_body_position_from_sc_i_m; + math::Vector<3> third_body_position_from_sc_i_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_i_m(third_body.c_str()); + math::Vector<3> third_body_pos_i_m = sc_position_i_m + third_body_position_from_sc_i_m; double gravity_constant = local_environment.GetCelestialInformation().GetGlobalInformation().GetGravityConstant_m3_s2(third_body.c_str()); third_body_acceleration_i_m_s2_ = CalcAcceleration_i_m_s2(third_body_pos_i_m, third_body_position_from_sc_i_m, gravity_constant); @@ -30,8 +30,8 @@ void ThirdBodyGravity::Update(const LocalEnvironment& local_environment, const d } } -s2e::math::Vector<3> ThirdBodyGravity::CalcAcceleration_i_m_s2(const s2e::math::Vector<3> s, const s2e::math::Vector<3> sr, const double gravity_constant_m_s2) { - s2e::math::Vector<3> acceleration_i_m_s2; +math::Vector<3> ThirdBodyGravity::CalcAcceleration_i_m_s2(const math::Vector<3> s, const math::Vector<3> sr, const double gravity_constant_m_s2) { + math::Vector<3> acceleration_i_m_s2; double s_norm = s.CalcNorm(); double s_norm3 = s_norm * s_norm * s_norm; diff --git a/src/disturbances/third_body_gravity.hpp b/src/disturbances/third_body_gravity.hpp index 24a74ccc8..7207f7e78 100644 --- a/src/disturbances/third_body_gravity.hpp +++ b/src/disturbances/third_body_gravity.hpp @@ -45,7 +45,7 @@ class ThirdBodyGravity : public Disturbance { private: std::set third_body_list_; //!< List of celestial bodies to calculate the third body disturbances - s2e::math::Vector<3> third_body_acceleration_i_m_s2_{0.0}; //!< Calculated third body disturbance acceleration in the inertial frame [m/s2] + math::Vector<3> third_body_acceleration_i_m_s2_{0.0}; //!< Calculated third body disturbance acceleration in the inertial frame [m/s2] // Override classes for logger::ILoggable /** @@ -67,7 +67,7 @@ class ThirdBodyGravity : public Disturbance { * @param [in] GM: The gravitational constants of the third celestial body [m3/s2] * @return Third body disturbance acceleration in the inertial frame in unit [m/s2] */ - s2e::math::Vector<3> CalcAcceleration_i_m_s2(const s2e::math::Vector<3> s, const s2e::math::Vector<3> sr, const double gravity_constant_m_s2); + math::Vector<3> CalcAcceleration_i_m_s2(const math::Vector<3> s, const math::Vector<3> sr, const double gravity_constant_m_s2); }; /** diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index 55e172240..84a9040e2 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -8,15 +8,15 @@ namespace s2e::dynamics::attitude { -Attitude::Attitude(const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const std::string& simulation_object_name) +Attitude::Attitude(const math::Matrix<3, 3>& inertia_tensor_kgm2, const std::string& simulation_object_name) : SimulationObject(simulation_object_name), inertia_tensor_kgm2_(inertia_tensor_kgm2) { - angular_velocity_b_rad_s_ = s2e::math::Vector<3>(0.0); - quaternion_i2b_ = s2e::math::Quaternion(0.0, 0.0, 0.0, 1.0); - torque_b_Nm_ = s2e::math::Vector<3>(0.0); - angular_momentum_spacecraft_b_Nms_ = s2e::math::Vector<3>(0.0); - angular_momentum_reaction_wheel_b_Nms_ = s2e::math::Vector<3>(0.0); - angular_momentum_total_b_Nms_ = s2e::math::Vector<3>(0.0); - angular_momentum_total_i_Nms_ = s2e::math::Vector<3>(0.0); + angular_velocity_b_rad_s_ = math::Vector<3>(0.0); + quaternion_i2b_ = math::Quaternion(0.0, 0.0, 0.0, 1.0); + torque_b_Nm_ = math::Vector<3>(0.0); + angular_momentum_spacecraft_b_Nms_ = math::Vector<3>(0.0); + angular_momentum_reaction_wheel_b_Nms_ = math::Vector<3>(0.0); + angular_momentum_total_b_Nms_ = math::Vector<3>(0.0); + angular_momentum_total_i_Nms_ = math::Vector<3>(0.0); angular_momentum_total_Nms_ = 0.0; kinetic_energy_J_ = 0.0; } @@ -27,8 +27,8 @@ std::string Attitude::GetLogHeader() const { str_tmp += logger::WriteVector("spacecraft_angular_velocity", "b", "rad/s", 3); str_tmp += logger::WriteQuaternion("spacecraft_quaternion", "i2b"); str_tmp += logger::WriteVector("spacecraft_torque", "b", "Nm", 3); - str_tmp += WriteScalar("spacecraft_total_angular_momentum", "Nms"); - str_tmp += WriteScalar("spacecraft_kinematic_energy", "J"); + str_tmp += logger::WriteScalar("spacecraft_total_angular_momentum", "Nms"); + str_tmp += logger::WriteScalar("spacecraft_kinematic_energy", "J"); return str_tmp; } @@ -39,8 +39,8 @@ std::string Attitude::GetLogValue() const { str_tmp += logger::WriteVector(angular_velocity_b_rad_s_); str_tmp += logger::WriteQuaternion(quaternion_i2b_); str_tmp += logger::WriteVector(torque_b_Nm_); - str_tmp += WriteScalar(angular_momentum_total_Nms_); - str_tmp += WriteScalar(kinetic_energy_J_); + str_tmp += logger::WriteScalar(angular_momentum_total_Nms_); + str_tmp += logger::WriteScalar(kinetic_energy_J_); return str_tmp; } @@ -52,15 +52,15 @@ void Attitude::SetParameters(const MonteCarloSimulationExecutor& mc_simulator) { void Attitude::CalcAngularMomentum(void) { angular_momentum_spacecraft_b_Nms_ = inertia_tensor_kgm2_ * angular_velocity_b_rad_s_; angular_momentum_total_b_Nms_ = angular_momentum_reaction_wheel_b_Nms_ + angular_momentum_spacecraft_b_Nms_; - s2e::math::Quaternion q_b2i = quaternion_i2b_.Conjugate(); + math::Quaternion q_b2i = quaternion_i2b_.Conjugate(); angular_momentum_total_i_Nms_ = q_b2i.FrameConversion(angular_momentum_total_b_Nms_); angular_momentum_total_Nms_ = angular_momentum_total_i_Nms_.CalcNorm(); - kinetic_energy_J_ = 0.5 * s2e::math::InnerProduct(angular_momentum_spacecraft_b_Nms_, angular_velocity_b_rad_s_); + kinetic_energy_J_ = 0.5 * math::InnerProduct(angular_momentum_spacecraft_b_Nms_, angular_velocity_b_rad_s_); } -s2e::math::Matrix<4, 4> CalcAngularVelocityMatrix(s2e::math::Vector<3> angular_velocity_b_rad_s) { - s2e::math::Matrix<4, 4> angular_velocity_matrix; +math::Matrix<4, 4> CalcAngularVelocityMatrix(math::Vector<3> angular_velocity_b_rad_s) { + math::Matrix<4, 4> angular_velocity_matrix; angular_velocity_matrix[0][0] = 0.0f; angular_velocity_matrix[0][1] = angular_velocity_b_rad_s[2]; diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index 533d534f3..6c3c80fef 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -18,14 +18,14 @@ namespace s2e::dynamics::attitude { * @class Attitude * @brief Base class for attitude of spacecraft */ -class Attitude : public logger::ILoggable, public SimulationObject { +class Attitude : public logger::ILoggable, public simulation::SimulationObject { public: /** * @fn Attitude * @brief Constructor * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - Attitude(const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const std::string& simulation_object_name = "attitude"); + Attitude(const math::Matrix<3, 3>& inertia_tensor_kgm2, const std::string& simulation_object_name = "attitude"); /** * @fn ~Attitude * @brief Destructor @@ -42,12 +42,12 @@ class Attitude : public logger::ILoggable, public SimulationObject { * @fn GetAngularVelocity_b_rad_s * @brief Return angular velocity of spacecraft body-fixed frame with respect to the inertial frame [rad/s] */ - inline s2e::math::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; } + inline math::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; } /** * @fn GetQuaternion_i2b * @brief Return attitude quaternion from the inertial frame to the body fixed frame */ - inline s2e::math::Quaternion GetQuaternion_i2b() const { return quaternion_i2b_; } + inline math::Quaternion GetQuaternion_i2b() const { return quaternion_i2b_; } /** * @fn GetTotalAngularMomentNorm_Nms * @brief Return norm of total angular momentum of the spacecraft [Nms] @@ -62,34 +62,34 @@ class Attitude : public logger::ILoggable, public SimulationObject { * @fn GetInertiaTensor_b_kgm2 * @brief Return inertia tensor [kg m^2] */ - inline s2e::math::Matrix<3, 3> GetInertiaTensor_b_kgm2() const { return inertia_tensor_kgm2_; } + inline math::Matrix<3, 3> GetInertiaTensor_b_kgm2() const { return inertia_tensor_kgm2_; } // Setter /** * @fn SetAngularVelocity_b_rad_s * @brief Set angular velocity of the body fixed frame with respect to the inertial frame [rad/s] */ - inline void SetAngularVelocity_b_rad_s(const s2e::math::Vector<3> angular_velocity_b_rad_s) { angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; } + inline void SetAngularVelocity_b_rad_s(const math::Vector<3> angular_velocity_b_rad_s) { angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; } /** * @fn SetQuaternion_i2b * @brief Set attitude quaternion from the inertial frame to the body frame */ - inline void SetQuaternion_i2b(const s2e::math::Quaternion quaternion_i2b) { quaternion_i2b_ = quaternion_i2b; } + inline void SetQuaternion_i2b(const math::Quaternion quaternion_i2b) { quaternion_i2b_ = quaternion_i2b; } /** * @fn SetTorque_b_Nm * @brief Set torque acting on the spacecraft on the body fixed frame [Nm] */ - inline void SetTorque_b_Nm(const s2e::math::Vector<3> torque_b_Nm) { torque_b_Nm_ = torque_b_Nm; } + inline void SetTorque_b_Nm(const math::Vector<3> torque_b_Nm) { torque_b_Nm_ = torque_b_Nm; } /** * @fn AddTorque_b_Nm * @brief Add torque acting on the spacecraft on the body fixed frame [Nm] */ - inline void AddTorque_b_Nm(const s2e::math::Vector<3> torque_b_Nm) { torque_b_Nm_ += torque_b_Nm; } + inline void AddTorque_b_Nm(const math::Vector<3> torque_b_Nm) { torque_b_Nm_ += torque_b_Nm; } /** * @fn SetRwAngularMomentum_b_Nms * @brief Set angular momentum of reaction wheel in the body fixed frame [Nms] */ - inline void SetRwAngularMomentum_b_Nms(const s2e::math::Vector<3> angular_momentum_rw_b_Nms) { + inline void SetRwAngularMomentum_b_Nms(const math::Vector<3> angular_momentum_rw_b_Nms) { angular_momentum_reaction_wheel_b_Nms_ = angular_momentum_rw_b_Nms; } @@ -113,20 +113,20 @@ class Attitude : public logger::ILoggable, public SimulationObject { virtual std::string GetLogValue() const; // SimulationObject for McSim - virtual void SetParameters(const MonteCarloSimulationExecutor& mc_simulator); + virtual void SetParameters(const simulation::MonteCarloSimulationExecutor& mc_simulator); protected: bool is_calc_enabled_ = true; //!< Calculation flag double propagation_step_s_; //!< Propagation step [sec] - s2e::math::Vector<3> angular_velocity_b_rad_s_; //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s] - s2e::math::Quaternion quaternion_i2b_; //!< Attitude quaternion from the inertial frame to the body fixed frame - s2e::math::Vector<3> torque_b_Nm_; //!< Torque in the body fixed frame [Nm] - const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2_; //!< Inertia tensor of the spacecraft [kg m^2] + math::Vector<3> angular_velocity_b_rad_s_; //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s] + math::Quaternion quaternion_i2b_; //!< Attitude quaternion from the inertial frame to the body fixed frame + math::Vector<3> torque_b_Nm_; //!< Torque in the body fixed frame [Nm] + const math::Matrix<3, 3>& inertia_tensor_kgm2_; //!< Inertia tensor of the spacecraft [kg m^2] - s2e::math::Vector<3> angular_momentum_spacecraft_b_Nms_; //!< Angular momentum of spacecraft in the body fixed frame [Nms] - s2e::math::Vector<3> angular_momentum_reaction_wheel_b_Nms_; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] - s2e::math::Vector<3> angular_momentum_total_b_Nms_; //!< Total angular momentum of spacecraft in the body fixed frame [Nms] - s2e::math::Vector<3> angular_momentum_total_i_Nms_; //!< Total angular momentum of spacecraft in the inertial frame [Nms] + math::Vector<3> angular_momentum_spacecraft_b_Nms_; //!< Angular momentum of spacecraft in the body fixed frame [Nms] + math::Vector<3> angular_momentum_reaction_wheel_b_Nms_; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] + math::Vector<3> angular_momentum_total_b_Nms_; //!< Total angular momentum of spacecraft in the body fixed frame [Nms] + math::Vector<3> angular_momentum_total_i_Nms_; //!< Total angular momentum of spacecraft in the inertial frame [Nms] double angular_momentum_total_Nms_; //!< Norm of total angular momentum [Nms] double kinetic_energy_J_; //!< Rotational Kinetic Energy of Spacecraft [J] @@ -142,7 +142,7 @@ class Attitude : public logger::ILoggable, public SimulationObject { * @brief Generate angular velocity matrix for kinematics calculation * @param [in] angular_velocity_b_rad_s: Angular velocity [rad/s] */ -s2e::math::Matrix<4, 4> CalcAngularVelocityMatrix(s2e::math::Vector<3> angular_velocity_b_rad_s); +math::Matrix<4, 4> CalcAngularVelocityMatrix(math::Vector<3> angular_velocity_b_rad_s); } // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index bac023c33..52af18344 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -11,8 +11,8 @@ namespace s2e::dynamics::attitude { -AttitudeRk4::AttitudeRk4(const s2e::math::Vector<3>& angular_velocity_b_rad_s, const s2e::math::Quaternion& quaternion_i2b, - const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const s2e::math::Vector<3>& torque_b_Nm, const double propagation_step_s, +AttitudeRk4::AttitudeRk4(const math::Vector<3>& angular_velocity_b_rad_s, const math::Quaternion& quaternion_i2b, + const math::Matrix<3, 3>& inertia_tensor_kgm2, const math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name) : Attitude(inertia_tensor_kgm2, simulation_object_name) { angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; @@ -20,7 +20,7 @@ AttitudeRk4::AttitudeRk4(const s2e::math::Vector<3>& angular_velocity_b_rad_s, c torque_b_Nm_ = torque_b_Nm; propagation_step_s_ = propagation_step_s; current_propagation_time_s_ = 0.0; - angular_momentum_reaction_wheel_b_Nms_ = s2e::math::Vector<3>(0.0); + angular_momentum_reaction_wheel_b_Nms_ = math::Vector<3>(0.0); previous_inertia_tensor_kgm2_ = inertia_tensor_kgm2_; inverse_inertia_tensor_ = CalcInverseMatrix(inertia_tensor_kgm2_); CalcAngularMomentum(); @@ -34,14 +34,14 @@ void AttitudeRk4::SetParameters(const MonteCarloSimulationExecutor& mc_simulator // TODO: Consider the following calculation is needed here? current_propagation_time_s_ = 0.0; - angular_momentum_reaction_wheel_b_Nms_ = s2e::math::Vector<3>(0.0); //!< Consider how to handle this variable + angular_momentum_reaction_wheel_b_Nms_ = math::Vector<3>(0.0); //!< Consider how to handle this variable CalcAngularMomentum(); } void AttitudeRk4::Propagate(const double end_time_s) { if (!is_calc_enabled_) return; - s2e::math::Matrix<3, 3> dot_inertia_tensor = (1.0 / (end_time_s - current_propagation_time_s_)) * (inertia_tensor_kgm2_ - previous_inertia_tensor_kgm2_); + math::Matrix<3, 3> dot_inertia_tensor = (1.0 / (end_time_s - current_propagation_time_s_)) * (inertia_tensor_kgm2_ - previous_inertia_tensor_kgm2_); torque_inertia_tensor_change_b_Nm_ = dot_inertia_tensor * angular_velocity_b_rad_s_; inverse_inertia_tensor_ = CalcInverseMatrix(inertia_tensor_kgm2_); @@ -57,29 +57,29 @@ void AttitudeRk4::Propagate(const double end_time_s) { CalcAngularMomentum(); } -s2e::math::Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(s2e::math::Vector<7> x, double t) { +math::Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(math::Vector<7> x, double t) { UNUSED(t); - s2e::math::Vector<7> dxdt; + math::Vector<7> dxdt; - s2e::math::Vector<3> omega_b; + math::Vector<3> omega_b; for (int i = 0; i < 3; i++) { omega_b[i] = x[i]; } - s2e::math::Vector<3> angular_momentum_total_b_Nms = (previous_inertia_tensor_kgm2_ * omega_b) + angular_momentum_reaction_wheel_b_Nms_; - s2e::math::Vector<3> rhs = - inverse_inertia_tensor_ * (torque_b_Nm_ - s2e::math::OuterProduct(omega_b, angular_momentum_total_b_Nms) - torque_inertia_tensor_change_b_Nm_); + math::Vector<3> angular_momentum_total_b_Nms = (previous_inertia_tensor_kgm2_ * omega_b) + angular_momentum_reaction_wheel_b_Nms_; + math::Vector<3> rhs = + inverse_inertia_tensor_ * (torque_b_Nm_ - math::OuterProduct(omega_b, angular_momentum_total_b_Nms) - torque_inertia_tensor_change_b_Nm_); for (int i = 0; i < 3; ++i) { dxdt[i] = rhs[i]; } - s2e::math::Vector<4> quaternion_i2b; + math::Vector<4> quaternion_i2b; for (int i = 0; i < 4; i++) { quaternion_i2b[i] = x[i + 3]; } - s2e::math::Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b) * quaternion_i2b; + math::Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b) * quaternion_i2b; for (int i = 0; i < 4; i++) { dxdt[i + 3] = d_quaternion[i]; @@ -89,7 +89,7 @@ s2e::math::Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(s2e::math::Vecto } void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { - s2e::math::Vector<7> x; + math::Vector<7> x; for (int i = 0; i < 3; i++) { x[i] = angular_velocity_b_rad_s_[i]; } @@ -97,8 +97,8 @@ void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { x[i + 3] = quaternion_i2b_[i]; } - s2e::math::Vector<7> k1, k2, k3, k4; - s2e::math::Vector<7> xk2, xk3, xk4; + math::Vector<7> k1, k2, k3, k4; + math::Vector<7> xk2, xk3, xk4; k1 = AttitudeDynamicsAndKinematics(x, t); xk2 = x + (dt / 2.0) * k1; @@ -111,7 +111,7 @@ void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { k4 = AttitudeDynamicsAndKinematics(xk4, (t + dt)); - s2e::math::Vector<7> next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4); + math::Vector<7> next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4); for (int i = 0; i < 3; i++) { angular_velocity_b_rad_s_[i] = next_x[i]; diff --git a/src/dynamics/attitude/attitude_rk4.hpp b/src/dynamics/attitude/attitude_rk4.hpp index 1e71fca19..2b3b4f06c 100644 --- a/src/dynamics/attitude/attitude_rk4.hpp +++ b/src/dynamics/attitude/attitude_rk4.hpp @@ -26,8 +26,8 @@ class AttitudeRk4 : public Attitude { * @param [in] propagation_step_s: Initial value of propagation step width [sec] * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - AttitudeRk4(const s2e::math::Vector<3>& angular_velocity_b_rad_s, const s2e::math::Quaternion& quaternion_i2b, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, - const s2e::math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name = "attitude"); + AttitudeRk4(const math::Vector<3>& angular_velocity_b_rad_s, const math::Quaternion& quaternion_i2b, const math::Matrix<3, 3>& inertia_tensor_kgm2, + const math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name = "attitude"); /** * @fn ~AttitudeRk4 * @brief Destructor @@ -46,13 +46,13 @@ class AttitudeRk4 : public Attitude { * @brief Set parameters for Monte-Carlo simulation * @param [in] mc_simulator: Monte-Carlo simulation executor */ - virtual void SetParameters(const MonteCarloSimulationExecutor& mc_simulator); + virtual void SetParameters(const simulation::MonteCarloSimulationExecutor& mc_simulator); private: double current_propagation_time_s_; //!< current time [sec] - s2e::math::Matrix<3, 3> inverse_inertia_tensor_; //!< Inverse of inertia tensor - s2e::math::Matrix<3, 3> previous_inertia_tensor_kgm2_; //!< Previous inertia tensor [kgm2] - s2e::math::Vector<3> torque_inertia_tensor_change_b_Nm_; //!< Torque generated by inertia tensor change [Nm] + math::Matrix<3, 3> inverse_inertia_tensor_; //!< Inverse of inertia tensor + math::Matrix<3, 3> previous_inertia_tensor_kgm2_; //!< Previous inertia tensor [kgm2] + math::Vector<3> torque_inertia_tensor_change_b_Nm_; //!< Torque generated by inertia tensor change [Nm] /** * @fn AttitudeDynamicsAndKinematics @@ -60,7 +60,7 @@ class AttitudeRk4 : public Attitude { * @param [in] x: State vector (angular velocity and quaternion) * @param [in] t: Unused TODO: remove? */ - s2e::math::Vector<7> AttitudeDynamicsAndKinematics(s2e::math::Vector<7> x, double t); + math::Vector<7> AttitudeDynamicsAndKinematics(math::Vector<7> x, double t); /** * @fn RungeKuttaOneStep * @brief Equation for one step of Runge-Kutta method diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp index 711c623f4..78292eb5c 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp @@ -11,9 +11,9 @@ namespace s2e::dynamics::attitude { AttitudeWithCantileverVibration::AttitudeWithCantileverVibration( - const s2e::math::Vector<3>& angular_velocity_b_rad_s, const s2e::math::Quaternion& quaternion_i2b, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, - const s2e::math::Matrix<3, 3>& inertia_tensor_cantilever_kgm2, const double damping_ratio_cantilever, - const double intrinsic_angular_velocity_cantilever_rad_s, const s2e::math::Vector<3>& torque_b_Nm, const double propagation_step_s, + const math::Vector<3>& angular_velocity_b_rad_s, const math::Quaternion& quaternion_i2b, const math::Matrix<3, 3>& inertia_tensor_kgm2, + const math::Matrix<3, 3>& inertia_tensor_cantilever_kgm2, const double damping_ratio_cantilever, + const double intrinsic_angular_velocity_cantilever_rad_s, const math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name) : Attitude(inertia_tensor_kgm2, simulation_object_name), numerical_integrator_(propagation_step_s, attitude_ode_, @@ -23,7 +23,7 @@ AttitudeWithCantileverVibration::AttitudeWithCantileverVibration( torque_b_Nm_ = torque_b_Nm; propagation_step_s_ = propagation_step_s; current_propagation_time_s_ = 0.0; - angular_momentum_reaction_wheel_b_Nms_ = s2e::math::Vector<3>(0.0); + angular_momentum_reaction_wheel_b_Nms_ = math::Vector<3>(0.0); attitude_ode_.SetInertiaTensorCantilever_kgm2(inertia_tensor_cantilever_kgm2); attitude_ode_.SetPreviousInertiaTensor_kgm2(inertia_tensor_kgm2_); @@ -32,7 +32,7 @@ AttitudeWithCantileverVibration::AttitudeWithCantileverVibration( double spring_coefficient = pow(intrinsic_angular_velocity_cantilever_rad_s, 2.0); attitude_ode_.SetSpringCoefficient(spring_coefficient); attitude_ode_.SetInverseInertiaTensor(CalcInverseMatrix(inertia_tensor_kgm2_)); - s2e::math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever = + math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever = CalcInverseMatrix(inertia_tensor_kgm2_ - inertia_tensor_cantilever_kgm2) * inertia_tensor_kgm2_; attitude_ode_.SetInverseEquivalentInertiaTensorCantilever(inverse_equivalent_inertia_tensor_cantilever); attitude_ode_.SetTorque_b_Nm(torque_b_Nm_); @@ -67,23 +67,23 @@ void AttitudeWithCantileverVibration::SetParameters(const MonteCarloSimulationEx // TODO: Consider the following calculation is needed here? current_propagation_time_s_ = 0.0; - angular_momentum_reaction_wheel_b_Nms_ = s2e::math::Vector<3>(0.0); //!< TODO: Consider how to handle this variable + angular_momentum_reaction_wheel_b_Nms_ = math::Vector<3>(0.0); //!< TODO: Consider how to handle this variable CalcAngularMomentum(); } void AttitudeWithCantileverVibration::Propagate(const double end_time_s) { if (!is_calc_enabled_) return; - s2e::math::Matrix<3, 3> previous_inertia_tensor_kgm2 = attitude_ode_.GetPreviousInertiaTensor_kgm2(); + math::Matrix<3, 3> previous_inertia_tensor_kgm2 = attitude_ode_.GetPreviousInertiaTensor_kgm2(); assert(end_time_s - current_propagation_time_s_ > 1e-6); - s2e::math::Matrix<3, 3> dot_inertia_tensor = (1.0 / (end_time_s - current_propagation_time_s_)) * (inertia_tensor_kgm2_ - previous_inertia_tensor_kgm2); - s2e::math::Vector<3> torque_inertia_tensor_b_Nm = dot_inertia_tensor * angular_velocity_b_rad_s_; + math::Matrix<3, 3> dot_inertia_tensor = (1.0 / (end_time_s - current_propagation_time_s_)) * (inertia_tensor_kgm2_ - previous_inertia_tensor_kgm2); + math::Vector<3> torque_inertia_tensor_b_Nm = dot_inertia_tensor * angular_velocity_b_rad_s_; attitude_ode_.SetTorqueInertiaTensor_b_Nm(torque_inertia_tensor_b_Nm); attitude_ode_.SetInverseInertiaTensor(CalcInverseMatrix(inertia_tensor_kgm2_)); attitude_ode_.SetTorque_b_Nm(torque_b_Nm_); attitude_ode_.SetAngularMomentumReactionWheel_b_Nms(angular_momentum_reaction_wheel_b_Nms_); - s2e::math::Vector<13> state = attitude_ode_.SetStateFromPhysicalQuantities(angular_velocity_b_rad_s_, angular_velocity_cantilever_rad_s_, + math::Vector<13> state = attitude_ode_.SetStateFromPhysicalQuantities(angular_velocity_b_rad_s_, angular_velocity_cantilever_rad_s_, quaternion_i2b_, euler_angular_cantilever_rad_); numerical_integrator_.GetIntegrator()->SetState(propagation_step_s_, state); while (end_time_s - current_propagation_time_s_ - propagation_step_s_ > 1.0e-6) { diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp index 682759f8f..30469c2fd 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp @@ -33,10 +33,10 @@ class AttitudeWithCantileverVibration : public Attitude { * @param [in] propagation_step_s: Initial value of propagation step width [sec] * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - AttitudeWithCantileverVibration(const s2e::math::Vector<3>& angular_velocity_b_rad_s, const s2e::math::Quaternion& quaternion_i2b, - const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const s2e::math::Matrix<3, 3>& inertia_tensor_cantilever_kgm2, + AttitudeWithCantileverVibration(const math::Vector<3>& angular_velocity_b_rad_s, const math::Quaternion& quaternion_i2b, + const math::Matrix<3, 3>& inertia_tensor_kgm2, const math::Matrix<3, 3>& inertia_tensor_cantilever_kgm2, const double damping_ratio_cantilever, const double intrinsic_angular_velocity_cantilever_rad_s, - const s2e::math::Vector<3>& torque_b_Nm, const double propagation_step_s, + const math::Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name = "attitude"); /** * @fn ~AttitudeWithCantileverVibration @@ -68,14 +68,14 @@ class AttitudeWithCantileverVibration : public Attitude { * @brief Set parameters for Monte-Carlo simulation * @param [in] mc_simulator: Monte-Carlo simulation executor */ - virtual void SetParameters(const MonteCarloSimulationExecutor& mc_simulator); + virtual void SetParameters(const simulation::MonteCarloSimulationExecutor& mc_simulator); private: double current_propagation_time_s_; //!< current time [sec] - s2e::math::Vector<3> angular_velocity_cantilever_rad_s_{0.0}; //!< Angular velocity of the cantilever with respect to the body frame [rad/s] - s2e::math::Vector<3> euler_angular_cantilever_rad_{0.0}; //!< Euler angle of the cantilever with respect to the body frame [rad/s] + math::Vector<3> angular_velocity_cantilever_rad_s_{0.0}; //!< Angular velocity of the cantilever with respect to the body frame [rad/s] + math::Vector<3> euler_angular_cantilever_rad_{0.0}; //!< Euler angle of the cantilever with respect to the body frame [rad/s] - s2e::numerical_integration::AttitudeWithCantileverVibrationOde attitude_ode_; + AttitudeWithCantileverVibrationOde attitude_ode_; s2e::numerical_integration::NumericalIntegratorManager<13> numerical_integrator_; }; diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index 04937978c..a1850b8fb 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -9,9 +9,9 @@ namespace s2e::dynamics::attitude { -ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const s2e::math::Quaternion quaternion_i2b, - const s2e::math::Vector<3> main_target_direction_b, const s2e::math::Vector<3> sub_target_direction_b, - const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, +ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const math::Quaternion quaternion_i2b, + const math::Vector<3> main_target_direction_b, const math::Vector<3> sub_target_direction_b, + const math::Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, const std::string& simulation_object_name) : Attitude(inertia_tensor_kgm2, simulation_object_name), main_mode_(main_mode), @@ -55,7 +55,7 @@ void ControlledAttitude::Initialize(void) { } void ControlledAttitude::Propagate(const double end_time_s) { - s2e::math::Vector<3> main_direction_i, sub_direction_i; + math::Vector<3> main_direction_i, sub_direction_i; if (!is_calc_enabled_) return; if (main_mode_ == AttitudeControlMode::kInertialStabilize) { @@ -74,28 +74,28 @@ void ControlledAttitude::Propagate(const double end_time_s) { return; } -s2e::math::Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMode mode) { - s2e::math::Vector<3> direction; +math::Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMode mode) { + math::Vector<3> direction; if (mode == AttitudeControlMode::kSunPointing) { direction = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); // When the local_celestial_information is not initialized. FIXME: This is temporary codes for attitude initialize. if (direction.CalcNorm() == 0.0) { - s2e::math::Vector<3> sun_position_i_m = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("SUN"); - s2e::math::Vector<3> spacecraft_position_i_m = orbit_->GetPosition_i_m(); + math::Vector<3> sun_position_i_m = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("SUN"); + math::Vector<3> spacecraft_position_i_m = orbit_->GetPosition_i_m(); direction = sun_position_i_m - spacecraft_position_i_m; } } else if (mode == AttitudeControlMode::kEarthCenterPointing) { direction = local_celestial_information_->GetPositionFromSpacecraft_i_m("EARTH"); // When the local_celestial_information is not initialized. FIXME: This is temporary codes for attitude initialize. if (direction.CalcNorm() == 0.0) { - s2e::math::Vector<3> earth_position_i_m = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("EARTH"); - s2e::math::Vector<3> spacecraft_position_i_m = orbit_->GetPosition_i_m(); + math::Vector<3> earth_position_i_m = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("EARTH"); + math::Vector<3> spacecraft_position_i_m = orbit_->GetPosition_i_m(); direction = earth_position_i_m - spacecraft_position_i_m; } } else if (mode == AttitudeControlMode::kVelocityDirectionPointing) { direction = orbit_->GetVelocity_i_m_s(); } else if (mode == AttitudeControlMode::kGroundSpeedDirectionPointing) { - s2e::math::Matrix<3, 3> dcm_ecef2eci = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); + math::Matrix<3, 3> dcm_ecef2eci = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); direction = dcm_ecef2eci * orbit_->GetVelocity_ecef_m_s(); } else if (mode == AttitudeControlMode::kOrbitNormalPointing) { direction = OuterProduct(orbit_->GetPosition_i_m(), orbit_->GetVelocity_i_m_s()); @@ -104,29 +104,29 @@ s2e::math::Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMo return direction; } -void ControlledAttitude::PointingControl(const s2e::math::Vector<3> main_direction_i, const s2e::math::Vector<3> sub_direction_i) { +void ControlledAttitude::PointingControl(const math::Vector<3> main_direction_i, const math::Vector<3> sub_direction_i) { // Calc DCM ECI->Target - s2e::math::Matrix<3, 3> dcm_t2i = CalcDcm(main_direction_i, sub_direction_i); + math::Matrix<3, 3> dcm_t2i = CalcDcm(main_direction_i, sub_direction_i); // Calc DCM Target->body - s2e::math::Matrix<3, 3> dcm_t2b = CalcDcm(main_target_direction_b_, sub_target_direction_b_); + math::Matrix<3, 3> dcm_t2b = CalcDcm(main_target_direction_b_, sub_target_direction_b_); // Calc DCM ECI->body - s2e::math::Matrix<3, 3> dcm_i2b = dcm_t2b * dcm_t2i.Transpose(); + math::Matrix<3, 3> dcm_i2b = dcm_t2b * dcm_t2i.Transpose(); // Convert to Quaternion - quaternion_i2b_ = s2e::math::Quaternion::ConvertFromDcm(dcm_i2b); + quaternion_i2b_ = math::Quaternion::ConvertFromDcm(dcm_i2b); } -s2e::math::Matrix<3, 3> ControlledAttitude::CalcDcm(const s2e::math::Vector<3> main_direction, const s2e::math::Vector<3> sub_direction) { +math::Matrix<3, 3> ControlledAttitude::CalcDcm(const math::Vector<3> main_direction, const math::Vector<3> sub_direction) { // Calc basis vectors - s2e::math::Vector<3> ex, ey, ez; + math::Vector<3> ex, ey, ez; ex = main_direction; - s2e::math::Vector<3> tmp1 = OuterProduct(ex, sub_direction); - s2e::math::Vector<3> tmp2 = OuterProduct(tmp1, ex); + math::Vector<3> tmp1 = OuterProduct(ex, sub_direction); + math::Vector<3> tmp2 = OuterProduct(tmp1, ex); ey = tmp2.CalcNormalizedVector(); - s2e::math::Vector<3> tmp3 = OuterProduct(ex, ey); + math::Vector<3> tmp3 = OuterProduct(ex, ey); ez = tmp3.CalcNormalizedVector(); // Generate DCM - s2e::math::Matrix<3, 3> dcm; + math::Matrix<3, 3> dcm; for (int i = 0; i < 3; i++) { dcm[i][0] = ex[i]; dcm[i][1] = ey[i]; @@ -154,24 +154,24 @@ AttitudeControlMode ConvertStringToCtrlMode(const std::string mode) { } void ControlledAttitude::CalcAngularVelocity(const double current_time_s) { - s2e::math::Vector<3> controlled_torque_b_Nm(0.0); + math::Vector<3> controlled_torque_b_Nm(0.0); if (previous_calc_time_s_ > 0.0) { double time_diff_sec = current_time_s - previous_calc_time_s_; - s2e::math::Quaternion prev_q_b2i = previous_quaternion_i2b_.Conjugate(); - s2e::math::Quaternion q_diff = prev_q_b2i * quaternion_i2b_; + math::Quaternion prev_q_b2i = previous_quaternion_i2b_.Conjugate(); + math::Quaternion q_diff = prev_q_b2i * quaternion_i2b_; q_diff = (2.0 / time_diff_sec) * q_diff; - s2e::math::Vector<3> angular_acc_b_rad_s2_; + math::Vector<3> angular_acc_b_rad_s2_; for (int i = 0; i < 3; i++) { angular_velocity_b_rad_s_[i] = q_diff[i]; angular_acc_b_rad_s2_[i] = (previous_omega_b_rad_s_[i] - angular_velocity_b_rad_s_[i]) / time_diff_sec; } - s2e::math::Matrix<3, 3> inv_inertia_tensor = CalcInverseMatrix(inertia_tensor_kgm2_); + math::Matrix<3, 3> inv_inertia_tensor = CalcInverseMatrix(inertia_tensor_kgm2_); controlled_torque_b_Nm = inv_inertia_tensor * angular_acc_b_rad_s2_; } else { - angular_velocity_b_rad_s_ = s2e::math::Vector<3>(0.0); - controlled_torque_b_Nm = s2e::math::Vector<3>(0.0); + angular_velocity_b_rad_s_ = math::Vector<3>(0.0); + controlled_torque_b_Nm = math::Vector<3>(0.0); } // Add torque with disturbances AddTorque_b_Nm(controlled_torque_b_Nm); diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index 97154f376..0b1b65ac7 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -56,9 +56,9 @@ class ControlledAttitude : public Attitude { * @param [in] orbit: Orbit * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const s2e::math::Quaternion quaternion_i2b, - const s2e::math::Vector<3> main_target_direction_b, const s2e::math::Vector<3> sub_target_direction_b, - const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, + ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const math::Quaternion quaternion_i2b, + const math::Vector<3> main_target_direction_b, const math::Vector<3> sub_target_direction_b, + const math::Matrix<3, 3>& inertia_tensor_kgm2, const environment::LocalCelestialInformation* local_celestial_information, const orbit::Orbit* orbit, const std::string& simulation_object_name = "attitude"); /** * @fn ~ControlledAttitude @@ -81,17 +81,17 @@ class ControlledAttitude : public Attitude { * @fn SetQuaternion_i2t * @brief Set quaternion for INERTIAL_STABILIZE mode */ - inline void SetQuaternion_i2t(const s2e::math::Quaternion quaternion_i2t) { quaternion_i2b_ = quaternion_i2t; } + inline void SetQuaternion_i2t(const math::Quaternion quaternion_i2t) { quaternion_i2b_ = quaternion_i2t; } /** * @fn SetMainTargetDirection_b * @brief Set main target direction on the body fixed frame */ - inline void SetMainTargetDirection_b(s2e::math::Vector<3> main_target_direction_b) { main_target_direction_b_ = main_target_direction_b; } + inline void SetMainTargetDirection_b(math::Vector<3> main_target_direction_b) { main_target_direction_b_ = main_target_direction_b; } /** * @fn SetSubTargetDirection_b * @brief Set sub target direction on the body fixed frame */ - inline void SetSubTargetDirection_b(s2e::math::Vector<3> sub_target_direction_b) { sub_target_direction_b_ = sub_target_direction_b; } + inline void SetSubTargetDirection_b(math::Vector<3> sub_target_direction_b) { sub_target_direction_b_ = sub_target_direction_b; } /** * @fn Propagate @@ -103,18 +103,18 @@ class ControlledAttitude : public Attitude { private: AttitudeControlMode main_mode_; //!< Main control mode AttitudeControlMode sub_mode_; //!< Sub control mode - s2e::math::Vector<3> main_target_direction_b_; //!< Main target direction on the body fixed frame - s2e::math::Vector<3> sub_target_direction_b_; //!< Sub target direction on tge body fixed frame + math::Vector<3> main_target_direction_b_; //!< Main target direction on the body fixed frame + math::Vector<3> sub_target_direction_b_; //!< Sub target direction on tge body fixed frame double previous_calc_time_s_ = -1.0; //!< Previous time of velocity calculation [sec] - s2e::math::Quaternion previous_quaternion_i2b_; //!< Previous quaternion - s2e::math::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s] + math::Quaternion previous_quaternion_i2b_; //!< Previous quaternion + math::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s] - const double kMinDirectionAngle_rad = 30.0 * s2e::math::deg_to_rad; //!< Minimum angle b/w main and sub direction + const double kMinDirectionAngle_rad = 30.0 * math::deg_to_rad; //!< Minimum angle b/w main and sub direction // TODO Change with ini file // Inputs - const LocalCelestialInformation* local_celestial_information_; //!< Local celestial information - const Orbit* orbit_; //!< Orbit information + const environment::LocalCelestialInformation* local_celestial_information_; //!< Local celestial information + const orbit::Orbit* orbit_; //!< Orbit information // Local functions /** @@ -128,14 +128,14 @@ class ControlledAttitude : public Attitude { * @param [in] mode: Attitude control mode * @return Target direction at the inertia frame */ - s2e::math::Vector<3> CalcTargetDirection_i(AttitudeControlMode mode); + math::Vector<3> CalcTargetDirection_i(AttitudeControlMode mode); /** * @fn PointingControl * @brief Calculate attitude quaternion * @param [in] main_direction_i: Main target direction in the inertial frame * @param [in] sub_direction_i: Sub target direction in the inertial frame */ - void PointingControl(const s2e::math::Vector<3> main_direction_i, const s2e::math::Vector<3> sub_direction_i); + void PointingControl(const math::Vector<3> main_direction_i, const math::Vector<3> sub_direction_i); /** * @fn CalcAngularVelocity * @brief Calculate angular velocity @@ -148,7 +148,7 @@ class ControlledAttitude : public Attitude { * @param [in] main_direction: Main target direction * @param [in] sub_direction: Sub target direction */ - s2e::math::Matrix<3, 3> CalcDcm(const s2e::math::Vector<3> main_direction, const s2e::math::Vector<3> sub_direction); + math::Matrix<3, 3> CalcDcm(const math::Vector<3> main_direction, const math::Vector<3> sub_direction); }; } // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index 875bb92f2..8460ea988 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -9,7 +9,7 @@ namespace s2e::dynamics::attitude { Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* local_celestial_information, - const double step_width_s, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id) { + const double step_width_s, const math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id) { setting_file_reader::IniAccess ini_file(file_name); const char* section_ = "ATTITUDE"; std::string mc_name = "attitude" + std::to_string(spacecraft_id); @@ -18,9 +18,9 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel const std::string propagate_mode = ini_file.ReadString(section_, "propagate_mode"); const std::string initialize_mode = ini_file.ReadString(section_, "initialize_mode"); - s2e::math::Vector<3> omega_b; - s2e::math::Quaternion quaternion_i2b; - s2e::math::Vector<3> torque_b; + math::Vector<3> omega_b; + math::Quaternion quaternion_i2b; + math::Vector<3> torque_b; if (initialize_mode == "CONTROLLED") { // Initialize with Controlled attitude (attitude_tmp temporary used) setting_file_reader::IniAccess ini_file_ca(file_name); @@ -32,7 +32,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel AttitudeControlMode sub_mode = ConvertStringToCtrlMode(sub_mode_in); ini_file_ca.ReadQuaternion(section_, "initial_quaternion_i2b", quaternion_i2b); - s2e::math::Vector<3> main_target_direction_b, sub_target_direction_b; + math::Vector<3> main_target_direction_b, sub_target_direction_b; ini_file_ca.ReadVector(section_ca_, "main_pointing_direction_b", main_target_direction_b); ini_file_ca.ReadVector(section_ca_, "sub_pointing_direction_b", sub_target_direction_b); std::string mc_name_temp = section_ + std::to_string(spacecraft_id) + "_TEMP"; @@ -40,8 +40,8 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel inertia_tensor_kgm2, local_celestial_information, orbit, mc_name_temp); attitude_temp->Propagate(step_width_s); quaternion_i2b = attitude_temp->GetQuaternion_i2b(); - omega_b = s2e::math::Vector<3>(0.0); - torque_b = s2e::math::Vector<3>(0.0); + omega_b = math::Vector<3>(0.0); + torque_b = math::Vector<3>(0.0); } else { // Including the case: initialize_mode == "MANUAL" ini_file.ReadVector(section_, "initial_angular_velocity_b_rad_s", omega_b); @@ -56,8 +56,8 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel setting_file_reader::IniAccess ini_structure(ini_structure_name); const char* section_cantilever = "CANTILEVER_PARAMETERS"; - s2e::math::Matrix<3, 3> inertia_tensor_cantilever_kgm2; - s2e::math::Vector<9> inertia_vec; + math::Matrix<3, 3> inertia_tensor_cantilever_kgm2; + math::Vector<9> inertia_vec; ini_structure.ReadVector(section_cantilever, "inertia_tensor_cantilever_kgm2", inertia_vec); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { @@ -80,7 +80,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel AttitudeControlMode main_mode = ConvertStringToCtrlMode(main_mode_in); AttitudeControlMode sub_mode = ConvertStringToCtrlMode(sub_mode_in); ini_file_ca.ReadQuaternion(section_, "initial_quaternion_i2b", quaternion_i2b); - s2e::math::Vector<3> main_target_direction_b, sub_target_direction_b; + math::Vector<3> main_target_direction_b, sub_target_direction_b; ini_file_ca.ReadVector(section_ca_, "main_pointing_direction_b", main_target_direction_b); ini_file_ca.ReadVector(section_ca_, "sub_pointing_direction_b", sub_target_direction_b); diff --git a/src/dynamics/attitude/initialize_attitude.hpp b/src/dynamics/attitude/initialize_attitude.hpp index fae3b99ab..a03141105 100644 --- a/src/dynamics/attitude/initialize_attitude.hpp +++ b/src/dynamics/attitude/initialize_attitude.hpp @@ -23,8 +23,8 @@ namespace s2e::dynamics::attitude { * @param [in] inertia_tensor_kgm2: Inertia tensor [kg m^2] * @param [in] spacecraft_id: Satellite ID */ -Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* local_celestial_information, - const double step_width_s, const s2e::math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id); +Attitude* InitAttitude(std::string file_name, const orbit::Orbit* orbit, const environment::LocalCelestialInformation* local_celestial_information, + const double step_width_s, const math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id); } // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp index b0cc7711c..84591e9b3 100644 --- a/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp @@ -13,15 +13,13 @@ namespace s2e::dynamics::attitude { -using s2e::numerical_integration - /** * @class AttitudeWithCantileverVibrationOde * @brief Class to implement Ordinary Differential Equations for Attitude with Cantilever Vibration * @note State variables in this ODE compose the following elenents (in order): angular_velocity_b_rad_s_ (3-dimension), * angular_velocity_cantilever_rad_s_ (3-dimension), quaternion_i2b_ (4-dmension), and euler_angular_cantilever_rad_ (3-dimension) */ -class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { +class AttitudeWithCantileverVibrationOde : public numerical_integration::InterfaceOde<13> { public: /** * @fn SetStateFromPhysicalQuantities @@ -31,10 +29,10 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame * @param [in] euler_angule_cantilever_rad: Euler angle of the cantilever@ body-fixed frame [rad] */ - s2e::math::Vector<13> SetStateFromPhysicalQuantities(const s2e::math::Vector<3> angular_velocity_b_rad_s, - const s2e::math::Vector<3> angular_velocity_cantilever_rad_s, const s2e::math::Quaternion quaternion_i2b, - const s2e::math::Vector<3> euler_angule_cantilever_rad) const { - s2e::math::Vector<13> state; + math::Vector<13> SetStateFromPhysicalQuantities(const math::Vector<3> angular_velocity_b_rad_s, + const math::Vector<3> angular_velocity_cantilever_rad_s, const math::Quaternion quaternion_i2b, + const math::Vector<3> euler_angule_cantilever_rad) const { + math::Vector<13> state; for (size_t i = 0; i < 3; i++) { state[i] = angular_velocity_b_rad_s[i]; } @@ -55,9 +53,9 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { * @brief Set physical quantities from state acquired by calculation of the ordinary differential equation * @param [in] state: state variables used to calculate the ordinary differential equation */ - void SetPhysicalQuantitiesFromState(const s2e::math::Vector<13> state, s2e::math::Vector<3>& angular_velocity_b_rad_s, - s2e::math::Vector<3>& angular_velocity_cantilever_rad_s, s2e::math::Quaternion& quaternion_i2b, - s2e::math::Vector<3>& euler_angular_cantilever_rad) const { + void SetPhysicalQuantitiesFromState(const math::Vector<13> state, math::Vector<3>& angular_velocity_b_rad_s, + math::Vector<3>& angular_velocity_cantilever_rad_s, math::Quaternion& quaternion_i2b, + math::Vector<3>& euler_angular_cantilever_rad) const { for (size_t i = 0; i < 3; i++) { angular_velocity_b_rad_s[i] = state[i]; } @@ -72,27 +70,27 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { } } - s2e::math::Vector<13> DerivativeFunction(const double time_s, const s2e::math::Vector<13>& state) const override { + math::Vector<13> DerivativeFunction(const double time_s, const math::Vector<13>& state) const override { UNUSED(time_s); - s2e::math::Vector<13> output; + math::Vector<13> output; - s2e::math::Vector<3> omega_b_rad_s; - s2e::math::Vector<3> omega_cantilever_rad_s; - s2e::math::Quaternion quaternion_i2b; - s2e::math::Vector<3> euler_angle_cantilever_rad; + math::Vector<3> omega_b_rad_s; + math::Vector<3> omega_cantilever_rad_s; + math::Quaternion quaternion_i2b; + math::Vector<3> euler_angle_cantilever_rad; SetPhysicalQuantitiesFromState(state, omega_b_rad_s, omega_cantilever_rad_s, quaternion_i2b, euler_angle_cantilever_rad); - s2e::math::Vector<3> angular_momentum_total_b_Nms = (previous_inertia_tensor_kgm2_ * omega_b_rad_s) + angular_momentum_reaction_wheel_b_Nms_; - s2e::math::Vector<3> net_torque_b_Nm = torque_b_Nm_ - s2e::math::OuterProduct(omega_b_rad_s, angular_momentum_total_b_Nms) - torque_inertia_tensor_b_Nm_; + math::Vector<3> angular_momentum_total_b_Nms = (previous_inertia_tensor_kgm2_ * omega_b_rad_s) + angular_momentum_reaction_wheel_b_Nms_; + math::Vector<3> net_torque_b_Nm = torque_b_Nm_ - math::OuterProduct(omega_b_rad_s, angular_momentum_total_b_Nms) - torque_inertia_tensor_b_Nm_; - s2e::math::Vector<3> angular_accelaration_cantilever_rad_s2 = + math::Vector<3> angular_accelaration_cantilever_rad_s2 = -(inverse_equivalent_inertia_tensor_cantilever_ * (attenuation_coefficient_ * omega_cantilever_rad_s + spring_coefficient_ * euler_angle_cantilever_rad)) - inverse_inertia_tensor_ * net_torque_b_Nm; - s2e::math::Vector<3> rhs = inverse_inertia_tensor_ * (net_torque_b_Nm - inertia_tensor_cantilever_kgm2_ * angular_accelaration_cantilever_rad_s2); + math::Vector<3> rhs = inverse_inertia_tensor_ * (net_torque_b_Nm - inertia_tensor_cantilever_kgm2_ * angular_accelaration_cantilever_rad_s2); for (size_t i = 0; i < 3; ++i) { output[i] = rhs[i]; @@ -102,7 +100,7 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { output[i + 3] = angular_accelaration_cantilever_rad_s2[i]; } - s2e::math::Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b_rad_s) * (s2e::math::Vector<4>(quaternion_i2b)); + math::Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b_rad_s) * (math::Vector<4>(quaternion_i2b)); for (size_t i = 0; i < 4; i++) { output[i + 6] = d_quaternion[i]; @@ -113,7 +111,7 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { } // The function is used to output the derivative of each corresponding physical quantity. - output = SetStateFromPhysicalQuantities(rhs, angular_accelaration_cantilever_rad_s2, s2e::math::Quaternion(d_quaternion), omega_cantilever_rad_s); + output = SetStateFromPhysicalQuantities(rhs, angular_accelaration_cantilever_rad_s2, math::Quaternion(d_quaternion), omega_cantilever_rad_s); return output; } @@ -133,27 +131,27 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { * @fn GetTorqueInertiaTensor_b_Nm * @brief Get torque generated by inertia tensor [Nm] */ - inline s2e::math::Vector<3> GetTorqueInertiaTensor_b_Nm() { return torque_inertia_tensor_b_Nm_; } + inline math::Vector<3> GetTorqueInertiaTensor_b_Nm() { return torque_inertia_tensor_b_Nm_; } /** * @fn GetInverseInertiaTensor * @brief Get inverse of inertia tensor */ - inline s2e::math::Matrix<3, 3> GetInverseInertiaTensor() { return inverse_inertia_tensor_; } + inline math::Matrix<3, 3> GetInverseInertiaTensor() { return inverse_inertia_tensor_; } /** * @fn GetPreviousInertiaTensor_kgm2 * @brief Get previous inertia tensor [kgm2] */ - inline s2e::math::Matrix<3, 3> GetPreviousInertiaTensor_kgm2() { return previous_inertia_tensor_kgm2_; } + inline math::Matrix<3, 3> GetPreviousInertiaTensor_kgm2() { return previous_inertia_tensor_kgm2_; } /** * @fn GetInertiaTensorCantilever_kgm2 * @brief Get inertia tensor of the cantilever [kgm2] */ - inline s2e::math::Matrix<3, 3> GetInertiaTensorCantilever_kgm2() { return inertia_tensor_cantilever_kgm2_; } + inline math::Matrix<3, 3> GetInertiaTensorCantilever_kgm2() { return inertia_tensor_cantilever_kgm2_; } /** * @fn GetInverseEquivalentInertiaTensorCantilever * @brief Get inverse of inertia tensor of the cantilever */ - inline s2e::math::Matrix<3, 3> GetInverseEquivalentInertiaTensorCantilever() { return inverse_equivalent_inertia_tensor_cantilever_; } + inline math::Matrix<3, 3> GetInverseEquivalentInertiaTensorCantilever() { return inverse_equivalent_inertia_tensor_cantilever_; } // Setter /** @@ -170,58 +168,58 @@ class AttitudeWithCantileverVibrationOde : public InterfaceOde<13> { * @fn SetTorque_b_Nm * @brief Set torque acting on the spacecraft on the body fixed frame [Nm] */ - inline void SetTorque_b_Nm(const s2e::math::Vector<3> torque_b_Nm) { torque_b_Nm_ = torque_b_Nm; } + inline void SetTorque_b_Nm(const math::Vector<3> torque_b_Nm) { torque_b_Nm_ = torque_b_Nm; } /** * @fn SetTorqueInertiaTensor_b_Nm * @brief Set torque generated by inertia tensor [Nm] */ - inline void SetTorqueInertiaTensor_b_Nm(const s2e::math::Vector<3> torque_inertia_tensor_b_Nm) { + inline void SetTorqueInertiaTensor_b_Nm(const math::Vector<3> torque_inertia_tensor_b_Nm) { torque_inertia_tensor_b_Nm_ = torque_inertia_tensor_b_Nm; } /** * @fn SetAngularMomentumReactionWheel_b_Nms * @brief Set angular momentum of reaction wheel in the body fixed frame [Nms] */ - inline void SetAngularMomentumReactionWheel_b_Nms(const s2e::math::Vector<3> angular_momentum_reaction_wheel_b_Nms) { + inline void SetAngularMomentumReactionWheel_b_Nms(const math::Vector<3> angular_momentum_reaction_wheel_b_Nms) { angular_momentum_reaction_wheel_b_Nms_ = angular_momentum_reaction_wheel_b_Nms; } /** * @fn SetInverseInertiaTensor * @brief Set inverse of inertia tensor */ - inline void SetInverseInertiaTensor(const s2e::math::Matrix<3, 3> inverse_inertia_tensor) { inverse_inertia_tensor_ = inverse_inertia_tensor; } + inline void SetInverseInertiaTensor(const math::Matrix<3, 3> inverse_inertia_tensor) { inverse_inertia_tensor_ = inverse_inertia_tensor; } /** * @fn SetPreviousInertiaTensor_kgm2 * @brief Set previous inertia tensor [kgm2] */ - inline void SetPreviousInertiaTensor_kgm2(const s2e::math::Matrix<3, 3> previous_inertia_tensor_kgm2) { + inline void SetPreviousInertiaTensor_kgm2(const math::Matrix<3, 3> previous_inertia_tensor_kgm2) { previous_inertia_tensor_kgm2_ = previous_inertia_tensor_kgm2; } /** * @fn SetInertiaTensorCantilever_kgm2 * @brief Set inertia tensor of the cantilever [kgm2] */ - inline void SetInertiaTensorCantilever_kgm2(const s2e::math::Matrix<3, 3> inertia_tensor_cantilever_kgm2) { + inline void SetInertiaTensorCantilever_kgm2(const math::Matrix<3, 3> inertia_tensor_cantilever_kgm2) { inertia_tensor_cantilever_kgm2_ = inertia_tensor_cantilever_kgm2; } /** * @fn SetInverseEquivalentInertiaTensorCantilever * @brief Set inverse of inertia tensor of the cantilever */ - inline void SetInverseEquivalentInertiaTensorCantilever(const s2e::math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever) { + inline void SetInverseEquivalentInertiaTensorCantilever(const math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever) { inverse_equivalent_inertia_tensor_cantilever_ = inverse_equivalent_inertia_tensor_cantilever; } protected: double attenuation_coefficient_ = 0.0; //!< Attenuation coefficient double spring_coefficient_ = 0.0; //!< Spring coefficient - s2e::math::Vector<3> torque_b_Nm_{0.0}; //!< Torque in the body fixed frame [Nm] - s2e::math::Vector<3> torque_inertia_tensor_b_Nm_{0.0}; //!< Torque generated by inertia tensor [Nm] - s2e::math::Vector<3> angular_momentum_reaction_wheel_b_Nms_{0.0}; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] - s2e::math::Matrix<3, 3> inverse_inertia_tensor_{0.0}; //!< Inverse of inertia tensor - s2e::math::Matrix<3, 3> previous_inertia_tensor_kgm2_{0.0}; //!< Previous inertia tensor [kgm2] - s2e::math::Matrix<3, 3> inertia_tensor_cantilever_kgm2_{0.0}; //!< Inertia tensor of the cantilever [kgm2] - s2e::math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever_{0.0}; //!< Inverse of inertia tensor of the cantilever + math::Vector<3> torque_b_Nm_{0.0}; //!< Torque in the body fixed frame [Nm] + math::Vector<3> torque_inertia_tensor_b_Nm_{0.0}; //!< Torque generated by inertia tensor [Nm] + math::Vector<3> angular_momentum_reaction_wheel_b_Nms_{0.0}; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] + math::Matrix<3, 3> inverse_inertia_tensor_{0.0}; //!< Inverse of inertia tensor + math::Matrix<3, 3> previous_inertia_tensor_kgm2_{0.0}; //!< Previous inertia tensor [kgm2] + math::Matrix<3, 3> inertia_tensor_cantilever_kgm2_{0.0}; //!< Inertia tensor of the cantilever [kgm2] + math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever_{0.0}; //!< Inverse of inertia tensor of the cantilever }; } // namespace s2e::dynamics::attitude diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index ea722d28d..e87f57dd0 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -9,20 +9,19 @@ namespace s2e::dynamics { -dynamics::Dynamics::dynamics::Dynamics(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, - const LocalEnvironment* local_environment, const int spacecraft_id, Structure* structure, - RelativeInformation* relative_information) +Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, const environment::LocalEnvironment* local_environment, + const int spacecraft_id, simulation::Structure* structure, simulation::RelativeInformation* relative_information) : structure_(structure), local_environment_(local_environment) { Initialize(simulation_configuration, simulation_time, spacecraft_id, structure, relative_information); } -dynamics::Dynamics::~dynamics::Dynamics() { +Dynamics::~dynamics::Dynamics() { delete attitude_; delete orbit_; delete temperature_; } -void dynamics::Dynamics::Initialize(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, const int spacecraft_id, +void Dynamics::Initialize(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, const int spacecraft_id, Structure* structure, RelativeInformation* relative_information) { const LocalCelestialInformation& local_celestial_information = local_environment_->GetCelestialInformation(); // Initialize @@ -38,7 +37,7 @@ void dynamics::Dynamics::Initialize(const SimulationConfiguration* simulation_co orbit_->UpdateByAttitude(attitude_->GetQuaternion_i2b()); } -void dynamics::Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestialInformation* local_celestial_information) { +void Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestialInformation* local_celestial_information) { // Attitude propagation if (simulation_time->GetAttitudePropagateFlag()) { attitude_->Propagate(simulation_time->GetElapsedTime_s()); @@ -56,13 +55,13 @@ void dynamics::Dynamics::Update(const SimulationTime* simulation_time, const Loc } } -void dynamics::Dynamics::ClearForceTorque(void) { - s2e::math::Vector<3> zero(0.0); +void Dynamics::ClearForceTorque(void) { + math::Vector<3> zero(0.0); attitude_->SetTorque_b_Nm(zero); orbit_->SetAcceleration_i_m_s2(zero); } -void dynamics::Dynamics::LogSetup(Logger& logger) { +void Dynamics::LogSetup(Logger& logger) { logger.AddLogList(attitude_); logger.AddLogList(orbit_); logger.AddLogList(temperature_); diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index 27797a506..0681d6d4c 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -38,8 +38,8 @@ class Dynamics { * @param [in] structure: Structure of the spacecraft * @param [in] relative_information: Relative information */ - Dynamics(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, const LocalEnvironment* local_environment, - const int spacecraft_id, Structure* structure, RelativeInformation* relative_information = (RelativeInformation*)nullptr); + Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, const environment::LocalEnvironment* local_environment, + const int spacecraft_id, simulation::Structure* structure, simulation::RelativeInformation* relative_information = (simulation::RelativeInformation*)nullptr); /** * @fn ~Dynamics * @brief Destructor @@ -52,26 +52,26 @@ class Dynamics { * @param [in] simulation_time: Simulation time * @param [in] local_celestial_information: Local celestial information */ - void Update(const SimulationTime* simulation_time, const LocalCelestialInformation* local_celestial_information); + void Update(const simulation::SimulationTime* simulation_time, const environment::LocalCelestialInformation* local_celestial_information); /** * @fn LogSetup * @brief Log setup for dynamics calculation */ - void LogSetup(Logger& logger); + void LogSetup(logger::Logger& logger); /** * @fn AddTorque_b_Nm * @brief Add input torque for the attitude dynamics propagation * @param [in] torque_b_Nm: Torque in the body fixed frame [Nm] */ - inline void AddTorque_b_Nm(s2e::math::Vector<3> torque_b_Nm) { attitude_->AddTorque_b_Nm(torque_b_Nm); } + inline void AddTorque_b_Nm(math::Vector<3> torque_b_Nm) { attitude_->AddTorque_b_Nm(torque_b_Nm); } /** * @fn AddForce_b_N * @brief Add input force for the orbit dynamics propagation * @param [in] force_b_N: Force in the body fixed frame [N] */ - inline void AddForce_b_N(s2e::math::Vector<3> force_b_N) { + inline void AddForce_b_N(math::Vector<3> force_b_N) { orbit_->AddForce_b_N(force_b_N, attitude_->GetQuaternion_i2b(), structure_->GetKinematicsParameters().GetMass_kg()); } /** @@ -79,7 +79,7 @@ class Dynamics { * @brief Add input acceleration for the orbit dynamics propagation * @param [in] acceleration_i_m_s2: Acceleration in the inertial fixed frame [N] */ - inline void AddAcceleration_i_m_s2(s2e::math::Vector<3> acceleration_i_m_s2) { orbit_->AddAcceleration_i_m_s2(acceleration_i_m_s2); } + inline void AddAcceleration_i_m_s2(math::Vector<3> acceleration_i_m_s2) { orbit_->AddAcceleration_i_m_s2(acceleration_i_m_s2); } /** * @fn ClearForceTorque @@ -91,29 +91,29 @@ class Dynamics { * @fn GetAttitude * @brief Return Attitude class */ - inline const Attitude& GetAttitude() const { return *attitude_; } + inline const attitude::Attitude& GetAttitude() const { return *attitude_; } /** * @fn GetOrbit * @brief Return Orbit class */ - inline const Orbit& GetOrbit() const { return *orbit_; } + inline const orbit::Orbit& GetOrbit() const { return *orbit_; } /** * @fn GetTemperature * @brief Return Temperature class */ - inline const Temperature& GetTemperature() const { return *temperature_; } + inline const thermal::Temperature& GetTemperature() const { return *temperature_; } /** * @fn SetAttitude * @brief Return Attitude class to change the Attitude */ - inline Attitude& SetAttitude() const { return *attitude_; } + inline attitude::Attitude& SetAttitude() const { return *attitude_; } private: - Attitude* attitude_; //!< Attitude dynamics - Orbit* orbit_; //!< Orbit dynamics - Temperature* temperature_; //!< Thermal dynamics - const Structure* structure_; //!< Structure information - const LocalEnvironment* local_environment_; //!< Local environment + attitude::Attitude* attitude_; //!< Attitude dynamics + orbit::Orbit* orbit_; //!< Orbit dynamics + thermal::Temperature* temperature_; //!< Thermal dynamics + const simulation::Structure* structure_; //!< Structure information + const environment::LocalEnvironment* local_environment_; //!< Local environment /** * @fn Initialize @@ -125,8 +125,8 @@ class Dynamics { * @param [in] structure: Structure of the spacecraft * @param [in] relative_information: Relative information */ - void Initialize(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, const int spacecraft_id, - Structure* structure, RelativeInformation* relative_information = (RelativeInformation*)nullptr); + void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, const int spacecraft_id, + simulation::Structure* structure, simulation::RelativeInformation* relative_information = (simulation::RelativeInformation*)nullptr); }; } // namespace s2e::dynamics diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index b846fa37c..ac661f705 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -12,10 +12,10 @@ namespace s2e::dynamics::orbit { EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double gravity_constant_m3_s2, - const double propagation_step_s, const double current_time_jd, const s2e::math::Vector<3> position_i_m, - const s2e::math::Vector<3> velocity_i_m_s, const double error_tolerance) + const double propagation_step_s, const double current_time_jd, const math::Vector<3> position_i_m, + const math::Vector<3> velocity_i_m_s, const double error_tolerance) : Orbit(celestial_information), - s2e::math::OrdinaryDifferentialEquation<6>(propagation_step_s), + math::OrdinaryDifferentialEquation<6>(propagation_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2), error_tolerance_(error_tolerance), propagation_step_s_(propagation_step_s) { @@ -62,9 +62,9 @@ void EnckeOrbitPropagation::Propagate(const double end_time_s, const double curr } // Functions for OrdinaryDifferentialEquation -void EnckeOrbitPropagation::DerivativeFunction(double t, const s2e::math::Vector<6>& state, s2e::math::Vector<6>& rhs) { +void EnckeOrbitPropagation::DerivativeFunction(double t, const math::Vector<6>& state, math::Vector<6>& rhs) { UNUSED(t); - s2e::math::Vector<3> difference_position_i_m_m, difference_acc_i_m_s2; + math::Vector<3> difference_position_i_m_m, difference_acc_i_m_s2; for (int i = 0; i < 3; i++) { difference_position_i_m_m[i] = state[i]; } @@ -85,7 +85,7 @@ void EnckeOrbitPropagation::DerivativeFunction(double t, const s2e::math::Vector } // Private Functions -void EnckeOrbitPropagation::Initialize(double current_time_jd, s2e::math::Vector<3> reference_position_i_m, s2e::math::Vector<3> reference_velocity_i_m_s) { +void EnckeOrbitPropagation::Initialize(double current_time_jd, math::Vector<3> reference_position_i_m, math::Vector<3> reference_velocity_i_m_s) { // General spacecraft_acceleration_i_m_s2_.FillUp(0.0); @@ -99,7 +99,7 @@ void EnckeOrbitPropagation::Initialize(double current_time_jd, s2e::math::Vector difference_position_i_m_.FillUp(0.0); difference_velocity_i_m_s_.FillUp(0.0); - s2e::math::Vector<6> zero(0.0f); + math::Vector<6> zero(0.0f); Setup(0.0, zero); UpdateSatOrbit(); @@ -113,11 +113,11 @@ void EnckeOrbitPropagation::UpdateSatOrbit() { TransformEcefToGeodetic(); } -double EnckeOrbitPropagation::CalcQFunction(s2e::math::Vector<3> difference_position_i_m) { +double EnckeOrbitPropagation::CalcQFunction(math::Vector<3> difference_position_i_m) { double r2; r2 = InnerProduct(spacecraft_position_i_m_, spacecraft_position_i_m_); - s2e::math::Vector<3> dr_2r; + math::Vector<3> dr_2r; dr_2r = difference_position_i_m - 2.0 * spacecraft_position_i_m_; double q = InnerProduct(difference_position_i_m, dr_2r) / r2; diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index a06a207f1..7b2ba00a8 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -16,7 +16,7 @@ namespace s2e::dynamics::orbit { * @class EnckeOrbitPropagation * @brief Class to propagate spacecraft orbit with Encke's method */ -class EnckeOrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferentialEquation<6> { +class EnckeOrbitPropagation : public Orbit, public math::OrdinaryDifferentialEquation<6> { public: /** * @fn EnckeOrbitPropagation @@ -30,7 +30,7 @@ class EnckeOrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferenti * @param [in] error_tolerance: Error tolerance threshold */ EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double gravity_constant_m3_s2, const double propagation_step_s, - const double current_time_jd, const s2e::math::Vector<3> position_i_m, const s2e::math::Vector<3> velocity_i_m_s, + const double current_time_jd, const math::Vector<3> position_i_m, const math::Vector<3> velocity_i_m_s, const double error_tolerance); /** * @fn ~EnckeOrbitPropagation @@ -55,7 +55,7 @@ class EnckeOrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferenti * @param [in] state: Position and velocity as state vector * @param [out] rhs: Output of the function */ - virtual void DerivativeFunction(double t, const s2e::math::Vector<6>& state, s2e::math::Vector<6>& rhs); + virtual void DerivativeFunction(double t, const math::Vector<6>& state, math::Vector<6>& rhs); private: // General @@ -65,13 +65,13 @@ class EnckeOrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferenti double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 // reference orbit - s2e::math::Vector<3> reference_position_i_m_; //!< Reference orbit position in the inertial frame [m] - s2e::math::Vector<3> reference_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] + math::Vector<3> reference_position_i_m_; //!< Reference orbit position in the inertial frame [m] + math::Vector<3> reference_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] s2e::orbit::KeplerOrbit reference_kepler_orbit; //!< Reference Kepler orbital element // difference orbit - s2e::math::Vector<3> difference_position_i_m_; //!< Difference orbit position in the inertial frame [m] - s2e::math::Vector<3> difference_velocity_i_m_s_; //!< Difference orbit velocity in the inertial frame [m/s] + math::Vector<3> difference_position_i_m_; //!< Difference orbit position in the inertial frame [m] + math::Vector<3> difference_velocity_i_m_s_; //!< Difference orbit velocity in the inertial frame [m/s] // functions /** @@ -81,7 +81,7 @@ class EnckeOrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferenti * @param [in] reference_position_i_m: Initial value of reference orbit position in the inertial frame [m] * @param [in] reference_velocity_i_m_s: Initial value of reference orbit position in the inertial frame [m] */ - void Initialize(const double current_time_jd, const s2e::math::Vector<3> reference_position_i_m, const s2e::math::Vector<3> reference_velocity_i_m_s); + void Initialize(const double current_time_jd, const math::Vector<3> reference_position_i_m, const math::Vector<3> reference_velocity_i_m_s); /** * @fn UpdateSatOrbit * @brief Update satellite orbit @@ -92,7 +92,7 @@ class EnckeOrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferenti * @brief Calculate Q function * @param [in] difference_position_i_m: Difference of position in the inertial frame [m] */ - double CalcQFunction(const s2e::math::Vector<3> difference_position_i_m); + double CalcQFunction(const math::Vector<3> difference_position_i_m); }; } // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index 8aad74327..a564b2521 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -28,9 +28,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string if (propagate_mode == "RK4") { // initialize RK4 orbit propagator - s2e::math::Vector<3> position_i_m; - s2e::math::Vector<3> velocity_i_m_s; - s2e::math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); + math::Vector<3> position_i_m; + math::Vector<3> velocity_i_m_s; + math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); for (size_t i = 0; i < 3; i++) { position_i_m[i] = pos_vel[i]; velocity_i_m_s[i] = pos_vel[i + 3]; @@ -51,9 +51,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string s2e::orbit::RelativeOrbitModel relative_dynamics_model_type = (s2e::orbit::RelativeOrbitModel)(conf.ReadInt(section_, "relative_dynamics_model_type")); s2e::orbit::StmModel stm_model_type = (s2e::orbit::StmModel)(conf.ReadInt(section_, "stm_model_type")); - s2e::math::Vector<3> init_relative_position_lvlh; + math::Vector<3> init_relative_position_lvlh; conf.ReadVector<3>(section_, "initial_relative_position_lvlh_m", init_relative_position_lvlh); - s2e::math::Vector<3> init_relative_velocity_lvlh; + math::Vector<3> init_relative_velocity_lvlh; conf.ReadVector<3>(section_, "initial_relative_velocity_lvlh_m_s", init_relative_velocity_lvlh); // There is a possibility that the orbit of the reference sat is not initialized when RelativeOrbit initialization is called To ensure that @@ -68,9 +68,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // TODO: init_mode_kepler should be removed in the next major update if (initialize_mode == OrbitInitializeMode::kInertialPositionAndVelocity) { // initialize with position and velocity - s2e::math::Vector<3> init_pos_m; + math::Vector<3> init_pos_m; conf.ReadVector<3>(section_, "initial_position_i_m", init_pos_m); - s2e::math::Vector<3> init_vel_m_s; + math::Vector<3> init_vel_m_s; conf.ReadVector<3>(section_, "initial_velocity_i_m_s", init_vel_m_s); oe = s2e::orbit::OrbitalElements(gravity_constant_m3_s2, current_time_jd, init_pos_m, init_vel_m_s); } else { @@ -87,9 +87,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string orbit = new KeplerOrbitPropagation(celestial_information, current_time_jd, kepler_orbit); } else if (propagate_mode == "ENCKE") { // initialize orbit for Encke's method - s2e::math::Vector<3> position_i_m; - s2e::math::Vector<3> velocity_i_m_s; - s2e::math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); + math::Vector<3> position_i_m; + math::Vector<3> velocity_i_m_s; + math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); for (size_t i = 0; i < 3; i++) { position_i_m[i] = pos_vel[i]; velocity_i_m_s[i] = pos_vel[i + 3]; @@ -102,9 +102,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string std::cerr << "ERROR: orbit propagation mode: " << propagate_mode << " is not defined!" << std::endl; std::cerr << "The orbit mode is automatically set as RK4" << std::endl; - s2e::math::Vector<3> position_i_m; - s2e::math::Vector<3> velocity_i_m_s; - s2e::math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); + math::Vector<3> position_i_m; + math::Vector<3> velocity_i_m_s; + math::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, gravity_constant_m3_s2); for (size_t i = 0; i < 3; i++) { position_i_m[i] = pos_vel[i]; velocity_i_m_s[i] = pos_vel[i + 3]; @@ -117,12 +117,12 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string return orbit; } -s2e::math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section) { +math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section) { auto conf = setting_file_reader::IniAccess(initialize_file); const char* section_ = section.c_str(); - s2e::math::Vector<3> position_i_m; - s2e::math::Vector<3> velocity_i_m_s; - s2e::math::Vector<6> pos_vel; + math::Vector<3> position_i_m; + math::Vector<3> velocity_i_m_s; + math::Vector<6> pos_vel; OrbitInitializeMode initialize_mode = SetOrbitInitializeMode(conf.ReadString(section_, "initialize_mode")); if (initialize_mode == OrbitInitializeMode::kOrbitalElements) { diff --git a/src/dynamics/orbit/initialize_orbit.hpp b/src/dynamics/orbit/initialize_orbit.hpp index 59a03d8a0..ece960dd2 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -25,7 +25,7 @@ namespace s2e::dynamics::orbit { * @param [in] section: Section name * @param [in] relative_information: Relative information */ -Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, double current_time_jd, +Orbit* InitOrbit(const environment::CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, double current_time_jd, double gravity_constant_m3_s2, std::string section = "ORBIT", RelativeInformation* relative_information = (RelativeInformation*)nullptr); @@ -37,7 +37,7 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string * @param [in] gravity_constant_m3_s2: Gravity constant [m3/s2] * @param [in] section: Section name */ -s2e::math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section = "ORBIT"); +math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section = "ORBIT"); } // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index f838b7c76..d08e642ad 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -6,16 +6,16 @@ namespace s2e::dynamics::orbit { -s2e::math::Quaternion Orbit::CalcQuaternion_i2lvlh() const { - s2e::math::Vector<3> lvlh_x = spacecraft_position_i_m_; // x-axis in LVLH frame is position vector direction from geocenter to satellite - s2e::math::Vector<3> lvlh_ex = lvlh_x.CalcNormalizedVector(); - s2e::math::Vector<3> lvlh_z = +math::Quaternion Orbit::CalcQuaternion_i2lvlh() const { + math::Vector<3> lvlh_x = spacecraft_position_i_m_; // x-axis in LVLH frame is position vector direction from geocenter to satellite + math::Vector<3> lvlh_ex = lvlh_x.CalcNormalizedVector(); + math::Vector<3> lvlh_z = OuterProduct(spacecraft_position_i_m_, spacecraft_velocity_i_m_s_); // z-axis in LVLH frame is angular momentum vector direction of orbit - s2e::math::Vector<3> lvlh_ez = lvlh_z.CalcNormalizedVector(); - s2e::math::Vector<3> lvlh_y = OuterProduct(lvlh_z, lvlh_x); - s2e::math::Vector<3> lvlh_ey = lvlh_y.CalcNormalizedVector(); + math::Vector<3> lvlh_ez = lvlh_z.CalcNormalizedVector(); + math::Vector<3> lvlh_y = OuterProduct(lvlh_z, lvlh_x); + math::Vector<3> lvlh_ey = lvlh_y.CalcNormalizedVector(); - s2e::math::Matrix<3, 3> dcm_i2lvlh; + math::Matrix<3, 3> dcm_i2lvlh; dcm_i2lvlh[0][0] = lvlh_ex[0]; dcm_i2lvlh[0][1] = lvlh_ex[1]; dcm_i2lvlh[0][2] = lvlh_ex[2]; @@ -26,19 +26,19 @@ s2e::math::Quaternion Orbit::CalcQuaternion_i2lvlh() const { dcm_i2lvlh[2][1] = lvlh_ez[1]; dcm_i2lvlh[2][2] = lvlh_ez[2]; - s2e::math::Quaternion q_i2lvlh = s2e::math::Quaternion::ConvertFromDcm(dcm_i2lvlh); + math::Quaternion q_i2lvlh = math::Quaternion::ConvertFromDcm(dcm_i2lvlh); return q_i2lvlh.Normalize(); } void Orbit::TransformEciToEcef(void) { - s2e::math::Matrix<3, 3> dcm_i_to_xcxf = celestial_information_->GetEarthRotation().GetDcmJ2000ToEcef(); + math::Matrix<3, 3> dcm_i_to_xcxf = celestial_information_->GetEarthRotation().GetDcmJ2000ToEcef(); spacecraft_position_ecef_m_ = dcm_i_to_xcxf * spacecraft_position_i_m_; // convert velocity vector in ECI to the vector in ECEF - s2e::math::Vector<3> earth_angular_velocity_i_rad_s{0.0}; + math::Vector<3> earth_angular_velocity_i_rad_s{0.0}; earth_angular_velocity_i_rad_s[2] = environment::earth_mean_angular_velocity_rad_s; - s2e::math::Vector<3> we_cross_r = OuterProduct(earth_angular_velocity_i_rad_s, spacecraft_position_i_m_); - s2e::math::Vector<3> velocity_we_cross_r = spacecraft_velocity_i_m_s_ - we_cross_r; + math::Vector<3> we_cross_r = OuterProduct(earth_angular_velocity_i_rad_s, spacecraft_position_i_m_); + math::Vector<3> velocity_we_cross_r = spacecraft_velocity_i_m_s_ - we_cross_r; spacecraft_velocity_ecef_m_s_ = dcm_i_to_xcxf * velocity_we_cross_r; } @@ -74,9 +74,9 @@ std::string Orbit::GetLogHeader() const { str_tmp += logger::WriteVector("spacecraft_velocity", "i", "m/s", 3); str_tmp += logger::WriteVector("spacecraft_velocity", "b", "m/s", 3); str_tmp += logger::WriteVector("spacecraft_acceleration", "i", "m/s2", 3); - str_tmp += WriteScalar("spacecraft_latitude", "rad"); - str_tmp += WriteScalar("spacecraft_longitude", "rad"); - str_tmp += WriteScalar("spacecraft_altitude", "m"); + str_tmp += logger::WriteScalar("spacecraft_latitude", "rad"); + str_tmp += logger::WriteScalar("spacecraft_longitude", "rad"); + str_tmp += logger::WriteScalar("spacecraft_altitude", "m"); return str_tmp; } @@ -89,9 +89,9 @@ std::string Orbit::GetLogValue() const { str_tmp += logger::WriteVector(spacecraft_velocity_i_m_s_, 10); str_tmp += logger::WriteVector(spacecraft_velocity_b_m_s_, 10); str_tmp += logger::WriteVector(spacecraft_acceleration_i_m_s2_, 10); - str_tmp += WriteScalar(spacecraft_geodetic_position_.GetLatitude_rad()); - str_tmp += WriteScalar(spacecraft_geodetic_position_.GetLongitude_rad()); - str_tmp += WriteScalar(spacecraft_geodetic_position_.GetAltitude_m()); + str_tmp += logger::WriteScalar(spacecraft_geodetic_position_.GetLatitude_rad()); + str_tmp += logger::WriteScalar(spacecraft_geodetic_position_.GetLongitude_rad()); + str_tmp += logger::WriteScalar(spacecraft_geodetic_position_.GetAltitude_m()); return str_tmp; } diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index b83ba4716..f6d80c7e7 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -51,7 +51,7 @@ class Orbit : public logger::ILoggable { * @brief Constructor * @param [in] celestial_information: Celestial information */ - Orbit(const CelestialInformation* celestial_information) : celestial_information_(celestial_information) {} + Orbit(const environment::CelestialInformation* celestial_information) : celestial_information_(celestial_information) {} /** * @fn ~Orbit * @brief Destructor @@ -71,7 +71,7 @@ class Orbit : public logger::ILoggable { * @brief Update attitude information * @param [in] quaternion_i2b: End time of simulation [sec] */ - inline void UpdateByAttitude(const s2e::math::Quaternion quaternion_i2b) { + inline void UpdateByAttitude(const math::Quaternion quaternion_i2b) { spacecraft_velocity_b_m_s_ = quaternion_i2b.FrameConversion(spacecraft_velocity_i_m_s_); } @@ -90,27 +90,27 @@ class Orbit : public logger::ILoggable { * @fn GetPosition_i_m * @brief Return spacecraft position in the inertial frame [m] */ - inline s2e::math::Vector<3> GetPosition_i_m() const { return spacecraft_position_i_m_; } + inline math::Vector<3> GetPosition_i_m() const { return spacecraft_position_i_m_; } /** * @fn GetPosition_ecef_m * @brief Return spacecraft position in the ECEF frame [m] */ - inline s2e::math::Vector<3> GetPosition_ecef_m() const { return spacecraft_position_ecef_m_; } + inline math::Vector<3> GetPosition_ecef_m() const { return spacecraft_position_ecef_m_; } /** * @fn GetVelocity_i_m_s * @brief Return spacecraft velocity in the inertial frame [m/s] */ - inline s2e::math::Vector<3> GetVelocity_i_m_s() const { return spacecraft_velocity_i_m_s_; } + inline math::Vector<3> GetVelocity_i_m_s() const { return spacecraft_velocity_i_m_s_; } /** * @fn GetVelocity_b_m_s * @brief Return spacecraft velocity in the body fixed frame [m/s] */ - inline s2e::math::Vector<3> GetVelocity_b_m_s() const { return spacecraft_velocity_b_m_s_; } + inline math::Vector<3> GetVelocity_b_m_s() const { return spacecraft_velocity_b_m_s_; } /** * @fn GetVelocity_ecef_m_s * @brief Return spacecraft velocity in the ECEF frame [m/s] */ - inline s2e::math::Vector<3> GetVelocity_ecef_m_s() const { return spacecraft_velocity_ecef_m_s_; } + inline math::Vector<3> GetVelocity_ecef_m_s() const { return spacecraft_velocity_ecef_m_s_; } /** * @fn GetGeodeticPosition * @brief Return spacecraft position in the geodetic frame [m] @@ -121,8 +121,8 @@ class Orbit : public logger::ILoggable { inline double GetLatitude_rad() const { return spacecraft_geodetic_position_.GetLatitude_rad(); } inline double GetLongitude_rad() const { return spacecraft_geodetic_position_.GetLongitude_rad(); } inline double GetAltitude_m() const { return spacecraft_geodetic_position_.GetAltitude_m(); } - inline s2e::math::Vector<3> GetLatLonAlt() const { - s2e::math::Vector<3> vec; + inline math::Vector<3> GetLatLonAlt() const { + math::Vector<3> vec; vec(0) = spacecraft_geodetic_position_.GetLatitude_rad(); vec(1) = spacecraft_geodetic_position_.GetLongitude_rad(); vec(2) = spacecraft_geodetic_position_.GetAltitude_m(); @@ -139,15 +139,15 @@ class Orbit : public logger::ILoggable { * @fn SetAcceleration_i_m_s2 * @brief Set acceleration in the inertial frame [m/s2] */ - inline void SetAcceleration_i_m_s2(const s2e::math::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ = acceleration_i_m_s2; } + inline void SetAcceleration_i_m_s2(const math::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ = acceleration_i_m_s2; } /** * @fn AddForce_i_N * @brief Add force * @param [in] force_i: Force in the inertial frame [N] * @param [in] spacecraft_mass_kg: Mass of spacecraft [kg] */ - inline void AddForce_i_N(const s2e::math::Vector<3> force_i_N, double spacecraft_mass_kg) { - s2e::math::Vector<3> acceleration_i_m_s2 = force_i_N; + inline void AddForce_i_N(const math::Vector<3> force_i_N, double spacecraft_mass_kg) { + math::Vector<3> acceleration_i_m_s2 = force_i_N; acceleration_i_m_s2 /= spacecraft_mass_kg; // FIXME spacecraft_acceleration_i_m_s2_ += acceleration_i_m_s2; } @@ -155,7 +155,7 @@ class Orbit : public logger::ILoggable { * @fn AddAcceleration_i_m_s2 * @brief Add acceleration in the inertial frame [m/s2] */ - inline void AddAcceleration_i_m_s2(const s2e::math::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ += acceleration_i_m_s2; } + inline void AddAcceleration_i_m_s2(const math::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ += acceleration_i_m_s2; } /** * @fn AddForce_i_N * @brief Add force @@ -163,7 +163,7 @@ class Orbit : public logger::ILoggable { * @param [in] quaternion_i2b: Quaternion from the inertial frame to the body fixed frame * @param [in] spacecraft_mass_kg: Mass of spacecraft [kg] */ - inline void AddForce_b_N(const s2e::math::Vector<3> force_b_N, const s2e::math::Quaternion quaternion_i2b, const double spacecraft_mass_kg) { + inline void AddForce_b_N(const math::Vector<3> force_b_N, const math::Quaternion quaternion_i2b, const double spacecraft_mass_kg) { auto force_i = quaternion_i2b.InverseFrameConversion(force_b_N); AddForce_i_N(force_i, spacecraft_mass_kg); } @@ -172,7 +172,7 @@ class Orbit : public logger::ILoggable { * @fn CalcQuaternion_i2lvlh * @brief Calculate and return quaternion from the inertial frame to the LVLH frame */ - s2e::math::Quaternion CalcQuaternion_i2lvlh() const; + math::Quaternion CalcQuaternion_i2lvlh() const; // Override logger::ILoggable /** @@ -187,21 +187,21 @@ class Orbit : public logger::ILoggable { virtual std::string GetLogValue() const; protected: - const CelestialInformation* celestial_information_; //!< Celestial information + const environment::CelestialInformation* celestial_information_; //!< Celestial information // Settings bool is_calc_enabled_ = false; //!< Calculate flag OrbitPropagateMode propagate_mode_; //!< Propagation mode - s2e::math::Vector<3> spacecraft_position_i_m_; //!< Spacecraft position in the inertial frame [m] - s2e::math::Vector<3> spacecraft_position_ecef_m_; //!< Spacecraft position in the ECEF frame [m] + math::Vector<3> spacecraft_position_i_m_; //!< Spacecraft position in the inertial frame [m] + math::Vector<3> spacecraft_position_ecef_m_; //!< Spacecraft position in the ECEF frame [m] s2e::geodesy::GeodeticPosition spacecraft_geodetic_position_; //!< Spacecraft position in the Geodetic frame - s2e::math::Vector<3> spacecraft_velocity_i_m_s_; //!< Spacecraft velocity in the inertial frame [m/s] - s2e::math::Vector<3> spacecraft_velocity_b_m_s_; //!< Spacecraft velocity in the body frame [m/s] - s2e::math::Vector<3> spacecraft_velocity_ecef_m_s_; //!< Spacecraft velocity in the ECEF frame [m/s] + math::Vector<3> spacecraft_velocity_i_m_s_; //!< Spacecraft velocity in the inertial frame [m/s] + math::Vector<3> spacecraft_velocity_b_m_s_; //!< Spacecraft velocity in the body frame [m/s] + math::Vector<3> spacecraft_velocity_ecef_m_s_; //!< Spacecraft velocity in the ECEF frame [m/s] - s2e::math::Vector<3> spacecraft_acceleration_i_m_s2_; //!< Spacecraft acceleration in the inertial frame [m/s2] + math::Vector<3> spacecraft_acceleration_i_m_s2_; //!< Spacecraft acceleration in the inertial frame [m/s2] //!< NOTE: Clear to zero at the end of the Propagate function // Frame Conversion TODO: consider other planet diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 4dbb9838a..7620b3866 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -11,11 +11,11 @@ namespace s2e::dynamics::orbit { RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, - int reference_spacecraft_id, s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, + int reference_spacecraft_id, math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, s2e::orbit::StmModel stm_model_type, RelativeInformation* relative_information) : Orbit(celestial_information), - s2e::math::OrdinaryDifferentialEquation<6>(time_step_s), + math::OrdinaryDifferentialEquation<6>(time_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2), reference_spacecraft_id_(reference_spacecraft_id), update_method_(update_method), @@ -32,7 +32,7 @@ RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, RelativeOrbit::~RelativeOrbit() {} -void RelativeOrbit::InitializeState(s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, +void RelativeOrbit::InitializeState(math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, double gravity_constant_m3_s2, double initial_time_s) { relative_position_lvlh_m_ = relative_position_lvlh_m; relative_velocity_lvlh_m_s_ = relative_velocity_lvlh_m_s; @@ -40,10 +40,10 @@ void RelativeOrbit::InitializeState(s2e::math::Vector<3> relative_position_lvlh_ // Disturbance acceleration are not considered in relative orbit propagation spacecraft_acceleration_i_m_s2_ *= 0.0; - s2e::math::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); - s2e::math::Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); - s2e::math::Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); - s2e::math::Quaternion q_lvlh2i = q_i2lvlh.Conjugate(); + math::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); + math::Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); + math::Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); + math::Quaternion q_lvlh2i = q_i2lvlh.Conjugate(); spacecraft_position_i_m_ = q_lvlh2i.FrameConversion(relative_position_lvlh_m_) + reference_sat_position_i; spacecraft_velocity_i_m_s_ = q_lvlh2i.FrameConversion(relative_velocity_lvlh_m_s_) + reference_sat_velocity_i; @@ -110,10 +110,10 @@ void RelativeOrbit::Propagate(const double end_time_s, const double current_time PropagateStm(end_time_s); } - s2e::math::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); - s2e::math::Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); - s2e::math::Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); - s2e::math::Quaternion q_lvlh2i = q_i2lvlh.Conjugate(); + math::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); + math::Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); + math::Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); + math::Quaternion q_lvlh2i = q_i2lvlh.Conjugate(); spacecraft_position_i_m_ = q_lvlh2i.FrameConversion(relative_position_lvlh_m_) + reference_sat_position_i; spacecraft_velocity_i_m_s_ = q_lvlh2i.FrameConversion(relative_velocity_lvlh_m_s_) + reference_sat_velocity_i; @@ -140,7 +140,7 @@ void RelativeOrbit::PropagateRk4(double elapsed_sec) { } void RelativeOrbit::PropagateStm(double elapsed_sec) { - s2e::math::Vector<6> current_state; + math::Vector<6> current_state; CalculateStm(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), gravity_constant_m3_s2_, elapsed_sec); current_state = stm_ * initial_state_; @@ -153,8 +153,8 @@ void RelativeOrbit::PropagateStm(double elapsed_sec) { relative_velocity_lvlh_m_s_[2] = current_state[5]; } -void RelativeOrbit::DerivativeFunction(double t, const s2e::math::Vector<6>& state, - s2e::math::Vector<6>& rhs) // only for RK4 relative dynamics propagation +void RelativeOrbit::DerivativeFunction(double t, const math::Vector<6>& state, + math::Vector<6>& rhs) // only for RK4 relative dynamics propagation { rhs = system_matrix_ * state; (void)t; diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 609994168..7aa505d88 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -19,7 +19,7 @@ namespace s2e::dynamics::orbit { * @class RelativeOrbit * @brief Class to propagate relative orbit */ -class RelativeOrbit : public Orbit, public s2e::math::OrdinaryDifferentialEquation<6> { +class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> { public: /** * @enum RelativeOrbitUpdateMethod @@ -42,7 +42,7 @@ class RelativeOrbit : public Orbit, public s2e::math::OrdinaryDifferentialEquati * @param [in] relative_information: Relative information */ RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, int reference_spacecraft_id, - s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, + math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, s2e::orbit::StmModel stm_model_type, RelativeInformation* relative_information); /** * @fn ~RelativeOrbit @@ -75,12 +75,12 @@ class RelativeOrbit : public Orbit, public s2e::math::OrdinaryDifferentialEquati double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] double propagation_step_s_; //!< Step width for RK4 [sec] - s2e::math::Matrix<6, 6> system_matrix_; //!< System matrix - s2e::math::Matrix<6, 6> stm_; //!< State transition matrix + math::Matrix<6, 6> system_matrix_; //!< System matrix + math::Matrix<6, 6> stm_; //!< State transition matrix - s2e::math::Vector<6> initial_state_; //!< Initial state (Position and Velocity) - s2e::math::Vector<3> relative_position_lvlh_m_; //!< Relative position in the LVLH frame - s2e::math::Vector<3> relative_velocity_lvlh_m_s_; //!< Relative velocity in the LVLH frame + math::Vector<6> initial_state_; //!< Initial state (Position and Velocity) + math::Vector<3> relative_position_lvlh_m_; //!< Relative position in the LVLH frame + math::Vector<3> relative_velocity_lvlh_m_s_; //!< Relative velocity in the LVLH frame RelativeOrbitUpdateMethod update_method_; //!< Update method s2e::orbit::RelativeOrbitModel relative_dynamics_model_type_; //!< Relative dynamics model type @@ -95,7 +95,7 @@ class RelativeOrbit : public Orbit, public s2e::math::OrdinaryDifferentialEquati * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] initial_time_s: Initialize time [sec] */ - void InitializeState(s2e::math::Vector<3> relative_position_lvlh_m, s2e::math::Vector<3> relative_velocity_lvlh_m_s, double gravity_constant_m3_s2, + void InitializeState(math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, double gravity_constant_m3_s2, double initial_time_s = 0); /** * @fn CalculateSystemMatrix diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index b73971d46..824637259 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -11,7 +11,7 @@ namespace s2e::dynamics::orbit { Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, - s2e::math::Vector<3> position_i_m, s2e::math::Vector<3> velocity_i_m_s, double initial_time_s) + math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s) : Orbit(celestial_information), OrdinaryDifferentialEquation<6>(time_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2) { propagate_mode_ = OrbitPropagateMode::kRk4; @@ -24,7 +24,7 @@ Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_i Rk4OrbitPropagation::~Rk4OrbitPropagation() {} -void Rk4OrbitPropagation::DerivativeFunction(double t, const s2e::math::Vector<6>& state, s2e::math::Vector<6>& rhs) { +void Rk4OrbitPropagation::DerivativeFunction(double t, const math::Vector<6>& state, math::Vector<6>& rhs) { double x = state[0], y = state[1], z = state[2]; double vx = state[3], vy = state[4], vz = state[5]; @@ -40,9 +40,9 @@ void Rk4OrbitPropagation::DerivativeFunction(double t, const s2e::math::Vector<6 (void)t; } -void Rk4OrbitPropagation::Initialize(s2e::math::Vector<3> position_i_m, s2e::math::Vector<3> velocity_i_m_s, double initial_time_s) { +void Rk4OrbitPropagation::Initialize(math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s) { // state vector [x,y,z,vx,vy,vz] - s2e::math::Vector<6> init_state; + math::Vector<6> init_state; init_state[0] = position_i_m[0]; init_state[1] = position_i_m[1]; init_state[2] = position_i_m[2]; diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index dbdc33b0d..b46b35646 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -17,7 +17,7 @@ namespace s2e::dynamics::orbit { * @class Rk4OrbitPropagation * @brief Class to propagate spacecraft orbit with Runge-Kutta-4 method */ -class Rk4OrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferentialEquation<6> { +class Rk4OrbitPropagation : public Orbit, public math::OrdinaryDifferentialEquation<6> { public: /** * @fn Rk4OrbitPropagation @@ -30,7 +30,7 @@ class Rk4OrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferential * @param [in] initial_time_s: Initial time [sec] */ Rk4OrbitPropagation(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, - s2e::math::Vector<3> position_i_m, s2e::math::Vector<3> velocity_i_m_s, double initial_time_s = 0); + math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s = 0); /** * @fn ~Rk4OrbitPropagation * @brief Destructor @@ -45,7 +45,7 @@ class Rk4OrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferential * @param [in] state: Position and velocity as state vector * @param [out] rhs: Output of the function */ - virtual void DerivativeFunction(double t, const s2e::math::Vector<6>& state, s2e::math::Vector<6>& rhs); + virtual void DerivativeFunction(double t, const math::Vector<6>& state, math::Vector<6>& rhs); // Override Orbit /** @@ -68,7 +68,7 @@ class Rk4OrbitPropagation : public Orbit, public s2e::math::OrdinaryDifferential * @param [in] velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] * @param [in] initial_time_s: Initial time [sec] */ - void Initialize(s2e::math::Vector<3> position_i_m, s2e::math::Vector<3> velocity_i_m_s, double initial_time_s = 0); + void Initialize(math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s = 0); }; } // namespace s2e::dynamics::orbit diff --git a/src/dynamics/thermal/heater_controller.hpp b/src/dynamics/thermal/heater_controller.hpp index 638e8d9e2..70ff3fb6c 100644 --- a/src/dynamics/thermal/heater_controller.hpp +++ b/src/dynamics/thermal/heater_controller.hpp @@ -45,12 +45,12 @@ class HeaterController { * @fn GetLowerThreshold_K * @brief Return Lower Threshold [K] */ - inline double GetLowerThreshold_K(void) const { return degC2K(lower_threshold_degC_); } + inline double GetLowerThreshold_K(void) const { return environment::degC2K(lower_threshold_degC_); } /** * @fn GetUpperThreshold_K * @brief Return Upper Threshold [K] */ - inline double GetUpperThreshold_K(void) const { return degC2K(upper_threshold_degC_); } + inline double GetUpperThreshold_K(void) const { return environment::degC2K(upper_threshold_degC_); } // Setter /** diff --git a/src/dynamics/thermal/node.cpp b/src/dynamics/thermal/node.cpp index 992fe489d..5c586f634 100644 --- a/src/dynamics/thermal/node.cpp +++ b/src/dynamics/thermal/node.cpp @@ -15,7 +15,7 @@ using namespace s2e::math; namespace s2e::dynamics::thermal { Node::Node(const size_t node_id, const string node_name, const NodeType node_type, const size_t heater_id, const double temperature_ini_K, - const double capacity_J_K, const double alpha, const double area_m2, s2e::math::Vector<3> normal_vector_b) + const double capacity_J_K, const double alpha, const double area_m2, math::Vector<3> normal_vector_b) : node_id_(node_id), node_name_(node_name), heater_id_(heater_id), @@ -32,7 +32,7 @@ Node::Node(const size_t node_id, const string node_name, const NodeType node_typ Node::~Node() {} -double Node::CalcSolarRadiation_W(s2e::math::Vector<3> sun_direction_b, double solar_flux_W_m2) { +double Node::CalcSolarRadiation_W(math::Vector<3> sun_direction_b, double solar_flux_W_m2) { double cos_theta = InnerProduct(sun_direction_b, normal_vector_b_); // calculate sun_power @@ -43,8 +43,8 @@ double Node::CalcSolarRadiation_W(s2e::math::Vector<3> sun_direction_b, double s return solar_radiation_W_; } -double Node::CalcAlbedoRadiation_W(s2e::math::Vector<3> earth_position_b_m, double earth_albedo_W_m2) { - s2e::math::Vector<3> earth_direction_b = earth_position_b_m.CalcNormalizedVector(); +double Node::CalcAlbedoRadiation_W(math::Vector<3> earth_position_b_m, double earth_albedo_W_m2) { + math::Vector<3> earth_direction_b = earth_position_b_m.CalcNormalizedVector(); double cos_theta_albedo = InnerProduct(earth_direction_b, normal_vector_b_); @@ -164,7 +164,7 @@ Node InitNode(const std::vector& node_str) { capacity_J_K = stod(node_str[index_capacity]); alpha = stod(node_str[index_alpha]); area_m2 = stod(node_str[index_area]); - s2e::math::Vector<3> normal_v_b; + math::Vector<3> normal_v_b; for (size_t i = 0; i < 3; i++) { normal_v_b[i] = stod(node_str[index_normal_v_b_head + i]); } diff --git a/src/dynamics/thermal/node.hpp b/src/dynamics/thermal/node.hpp index b75e6ad72..fc42b4239 100644 --- a/src/dynamics/thermal/node.hpp +++ b/src/dynamics/thermal/node.hpp @@ -47,7 +47,7 @@ class Node { * @param[in] normal_vector_b: Normal vector of face with possibility of solar incidence (Body frame) */ Node(const size_t node_id, const std::string node_name, const NodeType node_type, const size_t heater_id, const double temperature_ini_K, - const double capacity_J_K, const double alpha, const double area_m2, s2e::math::Vector<3> normal_vector_b); + const double capacity_J_K, const double alpha, const double area_m2, math::Vector<3> normal_vector_b); /** * @fn ~Node * @brief Destroy the Node object @@ -61,7 +61,7 @@ class Node { * @param solar_flux_W_m2: Solar flux [W/m^2] * @return double: Solar Radiation [W] */ - double CalcSolarRadiation_W(s2e::math::Vector<3> sun_direction_b, double solar_flux_W_m2); + double CalcSolarRadiation_W(math::Vector<3> sun_direction_b, double solar_flux_W_m2); /** * @fn CalcAlbedoRadiation_W * @brief Calculate albedo radiation [W] from earth direction, albedo factor, area, and normal vector @@ -70,7 +70,7 @@ class Node { * @param earth_albedo_W_m2: Earth albedo [W/m^2] * @return double: Albedo Radiation [W] */ - double CalcAlbedoRadiation_W(s2e::math::Vector<3> earth_position_b_m, double earth_albedo_W_m2); + double CalcAlbedoRadiation_W(math::Vector<3> earth_position_b_m, double earth_albedo_W_m2); // Getter /** @@ -102,7 +102,7 @@ class Node { * @brief Get temperature of node in degC * @return double: temperature [degC] */ - inline double GetTemperature_degC(void) const { return K2degC(temperature_K_); } + inline double GetTemperature_degC(void) const { return environment::K2degC(temperature_K_); } /** * @fn GetCapacity_J_K * @brief Return heat capacity of node [J/K] @@ -155,7 +155,7 @@ class Node { double solar_radiation_W_; double albedo_radiation_W_; NodeType node_type_; - s2e::math::Vector<3> normal_vector_b_; + math::Vector<3> normal_vector_b_; /** * @fn AssertNodeParams diff --git a/src/dynamics/thermal/temperature.cpp b/src/dynamics/thermal/temperature.cpp index 1843b28cc..0583ac6c0 100644 --- a/src/dynamics/thermal/temperature.cpp +++ b/src/dynamics/thermal/temperature.cpp @@ -70,7 +70,7 @@ void Temperature::Propagate(const LocalCelestialInformation* local_celestial_inf for (auto itr = nodes_.begin(); itr != nodes_.end(); ++itr) { cout << setprecision(4) << itr->GetSolarRadiation_W() << " "; } - s2e::math::Vector<3> sun_direction_b = local_celestial_information->GetPositionFromSpacecraft_b_m("SUN").CalcNormalizedVector(); + math::Vector<3> sun_direction_b = local_celestial_information->GetPositionFromSpacecraft_b_m("SUN").CalcNormalizedVector(); cout << "SunDir: "; for (size_t i = 0; i < 3; i++) { cout << setprecision(3) << sun_direction_b[i] << " "; @@ -80,7 +80,7 @@ void Temperature::Propagate(const LocalCelestialInformation* local_celestial_inf for (auto itr = nodes_.begin(); itr != nodes_.end(); ++itr) { cout << setprecision(4) << itr->GetAlbedoRadiation_W() << " "; } - s2e::math::Vector<3> earth_direction_b = local_celestial_information->GetPositionFromSpacecraft_b_m("EARTH").CalcNormalizedVector(); + math::Vector<3> earth_direction_b = local_celestial_information->GetPositionFromSpacecraft_b_m("EARTH").CalcNormalizedVector(); cout << "EarthDir: "; for (size_t i = 0; i < 3; i++) { cout << setprecision(3) << earth_direction_b[i] << " "; @@ -135,7 +135,7 @@ vector Temperature::CalcTemperatureDifferentials(vector temperat // TODO: consider the following unused arguments are really needed UNUSED(temperatures_K); - s2e::math::Vector<3> sun_direction_b = local_celestial_information->GetPositionFromSpacecraft_b_m("SUN").CalcNormalizedVector(); + math::Vector<3> sun_direction_b = local_celestial_information->GetPositionFromSpacecraft_b_m("SUN").CalcNormalizedVector(); vector differentials_K_s(node_num); for (size_t i = 0; i < node_num; i++) { heatloads_[i].SetElapsedTime_s(t); @@ -143,7 +143,7 @@ vector Temperature::CalcTemperatureDifferentials(vector temperat double solar_flux_W_m2 = srp_environment_->GetPowerDensity_W_m2(); if (solar_calc_setting_ == SolarCalcSetting::kEnable) { double solar_radiation_W = nodes_[i].CalcSolarRadiation_W(sun_direction_b, solar_flux_W_m2); - s2e::math::Vector<3> earth_position_b_m = local_celestial_information->GetPositionFromSpacecraft_b_m("EARTH"); + math::Vector<3> earth_position_b_m = local_celestial_information->GetPositionFromSpacecraft_b_m("EARTH"); double albedo_radiation_W = nodes_[i].CalcAlbedoRadiation_W(earth_position_b_m, earth_albedo_->GetEarthAlbedoRadiationPower_W_m2()); heatloads_[i].SetAlbedoHeatload_W(albedo_radiation_W); heatloads_[i].SetSolarHeatload_W(solar_radiation_W); @@ -196,14 +196,14 @@ string Temperature::GetLogHeader() const { // Do not retrieve boundary node values if (nodes_[i].GetNodeType() != NodeType::kBoundary) { string str_node = "temp_" + to_string(nodes_[i].GetNodeId()) + " (" + nodes_[i].GetNodeName() + ")"; - str_tmp += WriteScalar(str_node, "deg"); + str_tmp += logger::WriteScalar(str_node, "deg"); } } for (size_t i = 0; i < node_num_; i++) { // Do not retrieve boundary node values if (nodes_[i].GetNodeType() != NodeType::kBoundary) { string str_node = "heat_" + to_string(nodes_[i].GetNodeId()) + " (" + nodes_[i].GetNodeName() + ")"; - str_tmp += WriteScalar(str_node, "W"); + str_tmp += logger::WriteScalar(str_node, "W"); } } return str_tmp; @@ -214,13 +214,13 @@ string Temperature::GetLogValue() const { for (size_t i = 0; i < node_num_; i++) { // Do not retrieve boundary node values if (nodes_[i].GetNodeType() != NodeType::kBoundary) { - str_tmp += WriteScalar(nodes_[i].GetTemperature_degC()); + str_tmp += logger::WriteScalar(nodes_[i].GetTemperature_degC()); } } for (size_t i = 0; i < node_num_; i++) { // Do not retrieve boundary node values if (nodes_[i].GetNodeType() != NodeType::kBoundary) { - str_tmp += WriteScalar(heatloads_[i].GetTotalHeatload_W()); + str_tmp += logger::WriteScalar(heatloads_[i].GetTotalHeatload_W()); } } return str_tmp; diff --git a/src/dynamics/thermal/temperature.hpp b/src/dynamics/thermal/temperature.hpp index 7f9596ef9..8c99db6e1 100644 --- a/src/dynamics/thermal/temperature.hpp +++ b/src/dynamics/thermal/temperature.hpp @@ -43,8 +43,8 @@ class Temperature : public logger::ILoggable { size_t node_num_; //!< number of nodes double propagation_step_s_; //!< propagation step [s] double propagation_time_s_; //!< Incremented time inside class Temperature [s], finish propagation when reaching end_time - const SolarRadiationPressureEnvironment* srp_environment_; //!< SolarRadiationPressureEnvironment for calculating solar flux - const EarthAlbedo* earth_albedo_; //!< EarthAlbedo object for calculating earth albedo + const environment::SolarRadiationPressureEnvironment* srp_environment_; //!< SolarRadiationPressureEnvironment for calculating solar flux + const environment::EarthAlbedo* earth_albedo_; //!< EarthAlbedo object for calculating earth albedo bool is_calc_enabled_; //!< Whether temperature calculation is enabled SolarCalcSetting solar_calc_setting_; //!< setting for solar calculation bool debug_; //!< Activate debug output or not @@ -58,7 +58,7 @@ class Temperature : public logger::ILoggable { * @param[in] local_celestial_information: LocalCelestialInformation object for calculating radiation * @param[in] node_num: Number of nodes */ - void CalcRungeOneStep(double time_now_s, double time_step_s, const LocalCelestialInformation* local_celestial_information, size_t node_num); + void CalcRungeOneStep(double time_now_s, double time_step_s, const environment::LocalCelestialInformation* local_celestial_information, size_t node_num); /** * @fn CalcTemperatureDifferentials * @brief Calculate differential of thermal equilibrium equation @@ -70,7 +70,7 @@ class Temperature : public logger::ILoggable { * @return std::vector: Differential of thermal equilibrium equation at time now */ std::vector CalcTemperatureDifferentials(std::vector temperatures_K, double time_now_s, - const LocalCelestialInformation* local_celestial_information, size_t node_num); + const environment::LocalCelestialInformation* local_celestial_information, size_t node_num); public: /** @@ -93,8 +93,8 @@ class Temperature : public logger::ILoggable { */ Temperature(const std::vector> conductance_matrix_W_K, const std::vector> radiation_matrix_m2, std::vector nodes, std::vector heatloads, std::vector heaters, std::vector heater_controllers, - const size_t node_num, const double propagation_step_s, const SolarRadiationPressureEnvironment* srp_environment, - const EarthAlbedo* earth_albedo, const bool is_calc_enabled, const SolarCalcSetting solar_calc_setting, const bool debug); + const size_t node_num, const double propagation_step_s, const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::EarthAlbedo* earth_albedo, const bool is_calc_enabled, const SolarCalcSetting solar_calc_setting, const bool debug); /** * @fn Temperature * @brief Construct a new Temperature object, used when thermal calculation is disabled. @@ -114,7 +114,7 @@ class Temperature : public logger::ILoggable { * @param[in] local_celestial_information: LocalCelestialInformation object for calculating radiation * @param time_end_s: Time to finish propagation [s] */ - void Propagate(const LocalCelestialInformation* local_celestial_information, const double time_end_s); + void Propagate(const environment::LocalCelestialInformation* local_celestial_information, const double time_end_s); // Getter /** @@ -165,8 +165,8 @@ class Temperature : public logger::ILoggable { * @param[in] srp_environment: SolarRadiationPressureEnvironment object for calculating solar flux * @return Temperature* */ -Temperature* InitTemperature(const std::string file_name, const double rk_prop_step_s, const SolarRadiationPressureEnvironment* srp_environment, - const EarthAlbedo* earth_albedo); +Temperature* InitTemperature(const std::string file_name, const double rk_prop_step_s, const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::EarthAlbedo* earth_albedo); } // namespace s2e::dynamics::thermal diff --git a/src/environment/global/celestial_information.cpp b/src/environment/global/celestial_information.cpp index bcf9bf365..af94cfcb6 100644 --- a/src/environment/global/celestial_information.cpp +++ b/src/environment/global/celestial_information.cpp @@ -175,10 +175,10 @@ std::string CelestialInformation::GetLogValue() const { std::string str_tmp = ""; for (unsigned int i = 0; i < number_of_selected_bodies_; i++) { for (int j = 0; j < 3; j++) { - str_tmp += WriteScalar(celestial_body_position_from_center_i_m_[i * 3 + j]); + str_tmp += logger::WriteScalar(celestial_body_position_from_center_i_m_[i * 3 + j]); } for (int j = 0; j < 3; j++) { - str_tmp += WriteScalar(celestial_body_velocity_from_center_i_m_s_[i * 3 + j]); + str_tmp += logger::WriteScalar(celestial_body_velocity_from_center_i_m_s_[i * 3 + j]); } } return str_tmp; diff --git a/src/environment/global/celestial_information.hpp b/src/environment/global/celestial_information.hpp index 162fe790a..a00ceac53 100644 --- a/src/environment/global/celestial_information.hpp +++ b/src/environment/global/celestial_information.hpp @@ -75,8 +75,8 @@ class CelestialInformation : public logger::ILoggable { * @brief Return position from the center body in the inertial frame [m] * @param [in] id: ID of CelestialInformation list */ - inline s2e::math::Vector<3> GetPositionFromCenter_i_m(const unsigned int id) const { - s2e::math::Vector<3> pos(0.0); + inline math::Vector<3> GetPositionFromCenter_i_m(const unsigned int id) const { + math::Vector<3> pos(0.0); if (id > number_of_selected_bodies_) return pos; for (int i = 0; i < 3; i++) pos[i] = celestial_body_position_from_center_i_m_[id * 3 + i]; return pos; @@ -86,7 +86,7 @@ class CelestialInformation : public logger::ILoggable { * @brief Return position from the center body in the inertial frame [m] * @param [in] body_name: Name of the body defined in the SPICE */ - inline s2e::math::Vector<3> GetPositionFromCenter_i_m(const char* body_name) const { + inline math::Vector<3> GetPositionFromCenter_i_m(const char* body_name) const { int id = CalcBodyIdFromName(body_name); return GetPositionFromCenter_i_m(id); } @@ -96,11 +96,11 @@ class CelestialInformation : public logger::ILoggable { * @param [in] target_body_name: Name of the target body defined in the SPICE * @param [in] reference_body_name: Name of the reference body defined in the SPICE */ - inline s2e::math::Vector<3> GetPositionFromSelectedBody_i_m(const char* target_body_name, const char* reference_body_name) const { + inline math::Vector<3> GetPositionFromSelectedBody_i_m(const char* target_body_name, const char* reference_body_name) const { int target_id = CalcBodyIdFromName(target_body_name); - s2e::math::Vector<3> target_body_position_i_m = GetPositionFromCenter_i_m(target_id); + math::Vector<3> target_body_position_i_m = GetPositionFromCenter_i_m(target_id); int reference_id = CalcBodyIdFromName(reference_body_name); - s2e::math::Vector<3> reference_body_position_i_m = GetPositionFromCenter_i_m(reference_id); + math::Vector<3> reference_body_position_i_m = GetPositionFromCenter_i_m(reference_id); return target_body_position_i_m - reference_body_position_i_m; } @@ -110,8 +110,8 @@ class CelestialInformation : public logger::ILoggable { * @brief Return velocity from the center body in the inertial frame [m/s] * @param [in] id: ID of CelestialInformation list */ - inline s2e::math::Vector<3> GetVelocityFromCenter_i_m_s(const unsigned int id) const { - s2e::math::Vector<3> vel(0.0); + inline math::Vector<3> GetVelocityFromCenter_i_m_s(const unsigned int id) const { + math::Vector<3> vel(0.0); if (id > number_of_selected_bodies_) return vel; for (int i = 0; i < 3; i++) vel[i] = celestial_body_velocity_from_center_i_m_s_[id * 3 + i]; return vel; @@ -121,7 +121,7 @@ class CelestialInformation : public logger::ILoggable { * @brief Return velocity from the center body in the inertial frame [m/s] * @param [in] body_name: Name of the body defined in the SPICE */ - inline s2e::math::Vector<3> GetVelocityFromCenter_i_m_s(const char* body_name) const { + inline math::Vector<3> GetVelocityFromCenter_i_m_s(const char* body_name) const { int id = CalcBodyIdFromName(body_name); return GetVelocityFromCenter_i_m_s(id); } @@ -131,11 +131,11 @@ class CelestialInformation : public logger::ILoggable { * @param [in] target_body_name: Name of the target body defined in the SPICE * @param [in] reference_body_name: Name of the reference body defined in the SPICE */ - inline s2e::math::Vector<3> GetVelocityFromSelectedBody_i_m_s(const char* target_body_name, const char* reference_body_name) const { + inline math::Vector<3> GetVelocityFromSelectedBody_i_m_s(const char* target_body_name, const char* reference_body_name) const { int target_id = CalcBodyIdFromName(target_body_name); - s2e::math::Vector<3> target_body_velocity_i_m_s = GetVelocityFromCenter_i_m_s(target_id); + math::Vector<3> target_body_velocity_i_m_s = GetVelocityFromCenter_i_m_s(target_id); int reference_id = CalcBodyIdFromName(reference_body_name); - s2e::math::Vector<3> reference_body_velocity_i_m_s = GetVelocityFromCenter_i_m_s(reference_id); + math::Vector<3> reference_body_velocity_i_m_s = GetVelocityFromCenter_i_m_s(reference_id); return target_body_velocity_i_m_s - reference_body_velocity_i_m_s; } @@ -162,8 +162,8 @@ class CelestialInformation : public logger::ILoggable { * @brief Return 3 axis planetographic radii of a celestial body [m] * @param [in] id: ID of CelestialInformation list */ - inline s2e::math::Vector<3> GetRadii_m(const unsigned int id) const { - s2e::math::Vector<3> radii(0.0); + inline math::Vector<3> GetRadii_m(const unsigned int id) const { + math::Vector<3> radii(0.0); if (id > number_of_selected_bodies_) return radii; for (int i = 0; i < 3; i++) radii[i] = celestial_body_planetographic_radii_m_[id * 3 + i]; return radii; @@ -173,7 +173,7 @@ class CelestialInformation : public logger::ILoggable { * @brief Return 3 axis planetographic radii of a celestial body [m] * @param [in] body_name: Name of the body defined in the SPICE */ - inline s2e::math::Vector<3> GetRadiiFromName_m(const char* body_name) const { + inline math::Vector<3> GetRadiiFromName_m(const char* body_name) const { int id = CalcBodyIdFromName(body_name); return GetRadii_m(id); } diff --git a/src/environment/global/clock_generator.cpp b/src/environment/global/clock_generator.cpp index f2a1b7c8f..db573baf3 100644 --- a/src/environment/global/clock_generator.cpp +++ b/src/environment/global/clock_generator.cpp @@ -9,9 +9,9 @@ namespace s2e::environment { ClockGenerator::~ClockGenerator() {} -void ClockGenerator::RegisterComponent(ITickable* tickable) { components_.push_back(tickable); } +void ClockGenerator::RegisterComponent(components::ITickable* tickable) { components_.push_back(tickable); } -void ClockGenerator::RemoveComponent(ITickable* tickable) { +void ClockGenerator::RemoveComponent(components::ITickable* tickable) { for (auto itr = components_.begin(); itr != components_.end();) { if (*itr == tickable) { components_.erase(itr++); diff --git a/src/environment/global/clock_generator.hpp b/src/environment/global/clock_generator.hpp index 91ef4c383..d11e837f7 100644 --- a/src/environment/global/clock_generator.hpp +++ b/src/environment/global/clock_generator.hpp @@ -30,13 +30,13 @@ class ClockGenerator { * @brief Register component which has ITickable * @param [in] tickable: Component class */ - void RegisterComponent(ITickable* tickable); + void RegisterComponent(components::ITickable* tickable); /** * @fn RemoveComponent * @brief Removed registered component * @param [in] tickable: Registered component class */ - void RemoveComponent(ITickable* tickable); + void RemoveComponent(components::ITickable* tickable); /** * @fn TickToComponents * @brief Execute tick function of all registered components @@ -55,7 +55,7 @@ class ClockGenerator { inline void ClearTimerCount(void) { timer_count_ = 0; } private: - std::vector components_; //!< Component list fot tick + std::vector components_; //!< Component list fot tick unsigned int timer_count_; //!< Timer count TODO: change to long? }; diff --git a/src/environment/global/earth_rotation.cpp b/src/environment/global/earth_rotation.cpp index fd62d7b10..73d5811c8 100644 --- a/src/environment/global/earth_rotation.cpp +++ b/src/environment/global/earth_rotation.cpp @@ -19,7 +19,7 @@ namespace s2e::environment { // Default constructor EarthRotation::EarthRotation(const EarthRotationMode rotation_mode) : rotation_mode_(rotation_mode) { - dcm_j2000_to_ecef_ = s2e::math::MakeIdentityMatrix<3>(); + dcm_j2000_to_ecef_ = math::MakeIdentityMatrix<3>(); dcm_teme_to_ecef_ = dcm_j2000_to_ecef_; InitializeParameters(); } @@ -34,80 +34,80 @@ void EarthRotation::InitializeParameters() { // Coefficients to compute mean obliquity of the ecliptic // The actual unit of the coefficients are [rad/century^i], where i is the index of the array - c_epsilon_rad_[0] = 23.4392911 * s2e::math::deg_to_rad; // [rad] - c_epsilon_rad_[1] = -46.8150000 * s2e::math::arcsec_to_rad; // [rad/century] - c_epsilon_rad_[2] = -5.9000e-4 * s2e::math::arcsec_to_rad; // [rad/century^2] - c_epsilon_rad_[3] = 1.8130e-3 * s2e::math::arcsec_to_rad; // [rad/century^3] + c_epsilon_rad_[0] = 23.4392911 * math::deg_to_rad; // [rad] + c_epsilon_rad_[1] = -46.8150000 * math::arcsec_to_rad; // [rad/century] + c_epsilon_rad_[2] = -5.9000e-4 * math::arcsec_to_rad; // [rad/century^2] + c_epsilon_rad_[3] = 1.8130e-3 * math::arcsec_to_rad; // [rad/century^3] // Coefficients to compute Delaunay angles // The actual unit of the coefficients are [rad/century^i], where i is the index of the array - c_lm_rad_[0] = 134.96340251 * s2e::math::deg_to_rad; // [rad] - c_lm_rad_[1] = 1717915923.21780000 * s2e::math::arcsec_to_rad; // [rad/century] - c_lm_rad_[2] = 31.87920000 * s2e::math::arcsec_to_rad; // [rad/century^2] - c_lm_rad_[3] = 0.05163500 * s2e::math::arcsec_to_rad; // [rad/century^3] - c_lm_rad_[4] = -0.00024470 * s2e::math::arcsec_to_rad; // [rad/century^4] - - c_ls_rad_[0] = 357.52910918 * s2e::math::deg_to_rad; // [rad] - c_ls_rad_[1] = 129596581.04810000 * s2e::math::arcsec_to_rad; // [rad/century] - c_ls_rad_[2] = -0.55320000 * s2e::math::arcsec_to_rad; // [rad/century^2] - c_ls_rad_[3] = 0.00013600 * s2e::math::arcsec_to_rad; // [rad/century^3] - c_ls_rad_[4] = -0.00001149 * s2e::math::arcsec_to_rad; // [rad/century^4] - - c_f_rad_[0] = 93.27209062 * s2e::math::deg_to_rad; // [rad] - c_f_rad_[1] = 1739527262.84780000 * s2e::math::arcsec_to_rad; // [rad/century] - c_f_rad_[2] = -12.75120000 * s2e::math::arcsec_to_rad; // [rad/century^2] - c_f_rad_[3] = -0.00103700 * s2e::math::arcsec_to_rad; // [rad/century^3] - c_f_rad_[4] = 0.00000417 * s2e::math::arcsec_to_rad; // [rad/century^4] - - c_d_rad_[0] = 297.85019547 * s2e::math::deg_to_rad; // [rad] - c_d_rad_[1] = 1602961601.20900000 * s2e::math::arcsec_to_rad; // [rad/century] - c_d_rad_[2] = -6.37060000 * s2e::math::arcsec_to_rad; // [rad/century^2] - c_d_rad_[3] = 0.00659300 * s2e::math::arcsec_to_rad; // [rad/century^3] - c_d_rad_[4] = -0.00003169 * s2e::math::arcsec_to_rad; // [rad/century^4] - - c_o_rad_[0] = 125.04455501 * s2e::math::deg_to_rad; // [rad] - c_o_rad_[1] = -6962890.54310000 * s2e::math::arcsec_to_rad; // [rad/century] - c_o_rad_[2] = 7.47220000 * s2e::math::arcsec_to_rad; // [rad/century^2] - c_o_rad_[3] = 0.00770200 * s2e::math::arcsec_to_rad; // [rad/century^3] - c_o_rad_[4] = -0.00005939 * s2e::math::arcsec_to_rad; // [rad/century^4] + c_lm_rad_[0] = 134.96340251 * math::deg_to_rad; // [rad] + c_lm_rad_[1] = 1717915923.21780000 * math::arcsec_to_rad; // [rad/century] + c_lm_rad_[2] = 31.87920000 * math::arcsec_to_rad; // [rad/century^2] + c_lm_rad_[3] = 0.05163500 * math::arcsec_to_rad; // [rad/century^3] + c_lm_rad_[4] = -0.00024470 * math::arcsec_to_rad; // [rad/century^4] + + c_ls_rad_[0] = 357.52910918 * math::deg_to_rad; // [rad] + c_ls_rad_[1] = 129596581.04810000 * math::arcsec_to_rad; // [rad/century] + c_ls_rad_[2] = -0.55320000 * math::arcsec_to_rad; // [rad/century^2] + c_ls_rad_[3] = 0.00013600 * math::arcsec_to_rad; // [rad/century^3] + c_ls_rad_[4] = -0.00001149 * math::arcsec_to_rad; // [rad/century^4] + + c_f_rad_[0] = 93.27209062 * math::deg_to_rad; // [rad] + c_f_rad_[1] = 1739527262.84780000 * math::arcsec_to_rad; // [rad/century] + c_f_rad_[2] = -12.75120000 * math::arcsec_to_rad; // [rad/century^2] + c_f_rad_[3] = -0.00103700 * math::arcsec_to_rad; // [rad/century^3] + c_f_rad_[4] = 0.00000417 * math::arcsec_to_rad; // [rad/century^4] + + c_d_rad_[0] = 297.85019547 * math::deg_to_rad; // [rad] + c_d_rad_[1] = 1602961601.20900000 * math::arcsec_to_rad; // [rad/century] + c_d_rad_[2] = -6.37060000 * math::arcsec_to_rad; // [rad/century^2] + c_d_rad_[3] = 0.00659300 * math::arcsec_to_rad; // [rad/century^3] + c_d_rad_[4] = -0.00003169 * math::arcsec_to_rad; // [rad/century^4] + + c_o_rad_[0] = 125.04455501 * math::deg_to_rad; // [rad] + c_o_rad_[1] = -6962890.54310000 * math::arcsec_to_rad; // [rad/century] + c_o_rad_[2] = 7.47220000 * math::arcsec_to_rad; // [rad/century^2] + c_o_rad_[3] = 0.00770200 * math::arcsec_to_rad; // [rad/century^3] + c_o_rad_[4] = -0.00005939 * math::arcsec_to_rad; // [rad/century^4] // Coefficients to compute nutation angles - c_d_epsilon_rad_[0] = 9.2050 * s2e::math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[1] = 0.5730 * s2e::math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[2] = -0.0900 * s2e::math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[3] = 0.0980 * s2e::math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[4] = 0.0070 * s2e::math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[5] = -0.0010 * s2e::math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[6] = 0.0220 * s2e::math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[7] = 0.0130 * s2e::math::arcsec_to_rad; // [rad] - c_d_epsilon_rad_[8] = -0.0100 * s2e::math::arcsec_to_rad; // [rad] - - c_d_psi_rad_[0] = -17.2060 * s2e::math::arcsec_to_rad; // [rad] - c_d_psi_rad_[1] = -1.3170 * s2e::math::arcsec_to_rad; // [rad] - c_d_psi_rad_[2] = 0.2070 * s2e::math::arcsec_to_rad; // [rad] - c_d_psi_rad_[3] = -0.2280 * s2e::math::arcsec_to_rad; // [rad] - c_d_psi_rad_[4] = 0.1480 * s2e::math::arcsec_to_rad; // [rad] - c_d_psi_rad_[5] = 0.0710 * s2e::math::arcsec_to_rad; // [rad] - c_d_psi_rad_[6] = -0.0520 * s2e::math::arcsec_to_rad; // [rad] - c_d_psi_rad_[7] = -0.0300 * s2e::math::arcsec_to_rad; // [rad] - c_d_psi_rad_[8] = 0.0220 * s2e::math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[0] = 9.2050 * math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[1] = 0.5730 * math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[2] = -0.0900 * math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[3] = 0.0980 * math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[4] = 0.0070 * math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[5] = -0.0010 * math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[6] = 0.0220 * math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[7] = 0.0130 * math::arcsec_to_rad; // [rad] + c_d_epsilon_rad_[8] = -0.0100 * math::arcsec_to_rad; // [rad] + + c_d_psi_rad_[0] = -17.2060 * math::arcsec_to_rad; // [rad] + c_d_psi_rad_[1] = -1.3170 * math::arcsec_to_rad; // [rad] + c_d_psi_rad_[2] = 0.2070 * math::arcsec_to_rad; // [rad] + c_d_psi_rad_[3] = -0.2280 * math::arcsec_to_rad; // [rad] + c_d_psi_rad_[4] = 0.1480 * math::arcsec_to_rad; // [rad] + c_d_psi_rad_[5] = 0.0710 * math::arcsec_to_rad; // [rad] + c_d_psi_rad_[6] = -0.0520 * math::arcsec_to_rad; // [rad] + c_d_psi_rad_[7] = -0.0300 * math::arcsec_to_rad; // [rad] + c_d_psi_rad_[8] = 0.0220 * math::arcsec_to_rad; // [rad] // Coefficients to compute precession angle // The actual unit of the coefficients are [rad/century^i], where i is the index of the array - c_zeta_rad_[0] = 2306.218100 * s2e::math::arcsec_to_rad; // [rad/century] - c_zeta_rad_[1] = 0.301880 * s2e::math::arcsec_to_rad; // [rad/century^2] - c_zeta_rad_[2] = 0.017998 * s2e::math::arcsec_to_rad; // [rad/century^3] + c_zeta_rad_[0] = 2306.218100 * math::arcsec_to_rad; // [rad/century] + c_zeta_rad_[1] = 0.301880 * math::arcsec_to_rad; // [rad/century^2] + c_zeta_rad_[2] = 0.017998 * math::arcsec_to_rad; // [rad/century^3] - c_theta_rad_[0] = 2004.310900 * s2e::math::arcsec_to_rad; // [rad/century] - c_theta_rad_[1] = -0.426650 * s2e::math::arcsec_to_rad; // [rad/century^2] - c_theta_rad_[2] = -0.041833 * s2e::math::arcsec_to_rad; // [rad/century^3] + c_theta_rad_[0] = 2004.310900 * math::arcsec_to_rad; // [rad/century] + c_theta_rad_[1] = -0.426650 * math::arcsec_to_rad; // [rad/century^2] + c_theta_rad_[2] = -0.041833 * math::arcsec_to_rad; // [rad/century^3] - c_z_rad_[0] = 2306.218100 * s2e::math::arcsec_to_rad; // [rad/century] - c_z_rad_[1] = 1.094680 * s2e::math::arcsec_to_rad; // [rad/century^2] - c_z_rad_[2] = 0.018203 * s2e::math::arcsec_to_rad; // [rad/century^3] + c_z_rad_[0] = 2306.218100 * math::arcsec_to_rad; // [rad/century] + c_z_rad_[1] = 1.094680 * math::arcsec_to_rad; // [rad/century^2] + c_z_rad_[2] = 0.018203 * math::arcsec_to_rad; // [rad/century^3] } else { // If the rotation mode is neither Simple nor Full, disable the rotation calculation and make the DCM a unit matrix - dcm_j2000_to_ecef_ = s2e::math::MakeIdentityMatrix<3>(); + dcm_j2000_to_ecef_ = math::MakeIdentityMatrix<3>(); } } @@ -127,10 +127,10 @@ void EarthRotation::Update(const double julian_date) { terrestrial_time_julian_century[i + 1] = terrestrial_time_julian_century[i] * terrestrial_time_julian_century[0]; } - s2e::math::Matrix<3, 3> dcm_precession; - s2e::math::Matrix<3, 3> dcm_nutation; - s2e::math::Matrix<3, 3> dcm_rotation; - s2e::math::Matrix<3, 3> dcm_polar_motion; + math::Matrix<3, 3> dcm_precession; + math::Matrix<3, 3> dcm_nutation; + math::Matrix<3, 3> dcm_rotation; + math::Matrix<3, 3> dcm_polar_motion; // Nutation + Precession dcm_precession = Precession(terrestrial_time_julian_century); dcm_nutation = Nutation(terrestrial_time_julian_century); // epsilon_rad_, d_epsilon_rad_, d_psi_rad_ are updated in this procedure @@ -156,9 +156,9 @@ void EarthRotation::Update(const double julian_date) { } } -s2e::math::Matrix<3, 3> EarthRotation::AxialRotation(const double gast_rad) { return s2e::math::MakeRotationMatrixZ(gast_rad); } +math::Matrix<3, 3> EarthRotation::AxialRotation(const double gast_rad) { return math::MakeRotationMatrixZ(gast_rad); } -s2e::math::Matrix<3, 3> EarthRotation::Nutation(const double (&t_tt_century)[4]) { +math::Matrix<3, 3> EarthRotation::Nutation(const double (&t_tt_century)[4]) { // Mean obliquity of the ecliptic epsilon_rad_ = c_epsilon_rad_[0]; for (int i = 0; i < 3; i++) { @@ -210,17 +210,17 @@ s2e::math::Matrix<3, 3> EarthRotation::Nutation(const double (&t_tt_century)[4]) c_d_epsilon_rad_[7] * cos(2 * l_rad + lm_rad) + c_d_epsilon_rad_[8] * cos(2 * ld_rad - ls_rad); double epsi_mod_rad = epsilon_rad_ + d_epsilon_rad_; - s2e::math::Matrix<3, 3> x_epsi_1st = s2e::math::MakeRotationMatrixX(epsilon_rad_); - s2e::math::Matrix<3, 3> z_d_psi = s2e::math::MakeRotationMatrixZ(-d_psi_rad_); - s2e::math::Matrix<3, 3> x_epsi_2nd = s2e::math::MakeRotationMatrixX(-epsi_mod_rad); + math::Matrix<3, 3> x_epsi_1st = math::MakeRotationMatrixX(epsilon_rad_); + math::Matrix<3, 3> z_d_psi = math::MakeRotationMatrixZ(-d_psi_rad_); + math::Matrix<3, 3> x_epsi_2nd = math::MakeRotationMatrixX(-epsi_mod_rad); - s2e::math::Matrix<3, 3> dcm_nutation; + math::Matrix<3, 3> dcm_nutation; dcm_nutation = x_epsi_2nd * z_d_psi * x_epsi_1st; return dcm_nutation; } -s2e::math::Matrix<3, 3> EarthRotation::Precession(const double (&t_tt_century)[4]) { +math::Matrix<3, 3> EarthRotation::Precession(const double (&t_tt_century)[4]) { // Compute precession angles(zeta, theta, z) double zeta_rad = 0.0; for (int i = 0; i < 3; i++) { @@ -236,18 +236,18 @@ s2e::math::Matrix<3, 3> EarthRotation::Precession(const double (&t_tt_century)[4 } // Develop transformation matrix - s2e::math::Matrix<3, 3> z_zeta = s2e::math::MakeRotationMatrixZ(-zeta_rad); - s2e::math::Matrix<3, 3> y_theta = s2e::math::MakeRotationMatrixY(theta_rad); - s2e::math::Matrix<3, 3> z_z = s2e::math::MakeRotationMatrixZ(-z_rad); + math::Matrix<3, 3> z_zeta = math::MakeRotationMatrixZ(-zeta_rad); + math::Matrix<3, 3> y_theta = math::MakeRotationMatrixY(theta_rad); + math::Matrix<3, 3> z_z = math::MakeRotationMatrixZ(-z_rad); - s2e::math::Matrix<3, 3> dcm_precession; + math::Matrix<3, 3> dcm_precession; dcm_precession = z_z * y_theta * z_zeta; return dcm_precession; } -s2e::math::Matrix<3, 3> EarthRotation::PolarMotion(const double x_p, const double y_p) { - s2e::math::Matrix<3, 3> dcm_polar_motion; +math::Matrix<3, 3> EarthRotation::PolarMotion(const double x_p, const double y_p) { + math::Matrix<3, 3> dcm_polar_motion; dcm_polar_motion[0][0] = 1.0; dcm_polar_motion[0][1] = 0.0; diff --git a/src/environment/global/earth_rotation.hpp b/src/environment/global/earth_rotation.hpp index b9680e478..25cfa1992 100644 --- a/src/environment/global/earth_rotation.hpp +++ b/src/environment/global/earth_rotation.hpp @@ -47,20 +47,20 @@ class EarthRotation { * @fn GetDcmJ2000ToEcef * @brief Return the DCM between J2000 inertial frame and the Earth Centered Earth Fixed frame */ - inline const s2e::math::Matrix<3, 3> GetDcmJ2000ToEcef() const { return dcm_j2000_to_ecef_; }; + inline const math::Matrix<3, 3> GetDcmJ2000ToEcef() const { return dcm_j2000_to_ecef_; }; /** * @fn GetDcmTemeToEcef * @brief Return the DCM between TEME (Inertial frame used in SGP4) and the Earth Centered Earth Fixed frame */ - inline const s2e::math::Matrix<3, 3> GetDcmTemeToEcef() const { return dcm_teme_to_ecef_; }; + inline const math::Matrix<3, 3> GetDcmTemeToEcef() const { return dcm_teme_to_ecef_; }; private: double d_psi_rad_; //!< Nutation in obliquity [rad] double d_epsilon_rad_; //!< Nutation in longitude [rad] double epsilon_rad_; //!< Mean obliquity of the ecliptic [rad] - s2e::math::Matrix<3, 3> dcm_j2000_to_ecef_; //!< Direction Cosine Matrix J2000 to ECEF - s2e::math::Matrix<3, 3> dcm_teme_to_ecef_; //!< Direction Cosine Matrix TEME to ECEF + math::Matrix<3, 3> dcm_j2000_to_ecef_; //!< Direction Cosine Matrix J2000 to ECEF + math::Matrix<3, 3> dcm_teme_to_ecef_; //!< Direction Cosine Matrix TEME to ECEF EarthRotationMode rotation_mode_; //!< Designation of dynamics model // Definitions of coefficients @@ -96,7 +96,7 @@ class EarthRotation { * @param [in] gast_rad: Greenwich 'Apparent' Sidereal Time [rad] * @return Rotation matrix */ - s2e::math::Matrix<3, 3> AxialRotation(const double gast_rad); + math::Matrix<3, 3> AxialRotation(const double gast_rad); /** * @fn Nutation @@ -104,7 +104,7 @@ class EarthRotation { * @param [in] t_tt_century: nth power of julian century for terrestrial time * @return Rotation matrix */ - s2e::math::Matrix<3, 3> Nutation(const double (&t_tt_century)[4]); + math::Matrix<3, 3> Nutation(const double (&t_tt_century)[4]); /** * @fn Precession @@ -112,14 +112,14 @@ class EarthRotation { * @param [in] t_tt_century: nth power of julian century for terrestrial time * @return Rotation matrix */ - s2e::math::Matrix<3, 3> Precession(const double (&t_tt_century)[4]); + math::Matrix<3, 3> Precession(const double (&t_tt_century)[4]); /** * @fn PolarMotion * @brief Calculate movement of the coordinate axes due to Polar Motion * @note Currently, this function is not used. */ - s2e::math::Matrix<3, 3> PolarMotion(const double x_p, const double y_p); + math::Matrix<3, 3> PolarMotion(const double x_p, const double y_p); }; EarthRotationMode ConvertEarthRotationMode(const std::string mode); diff --git a/src/environment/global/global_environment.cpp b/src/environment/global/global_environment.cpp index 334c1d4ae..3657594c5 100644 --- a/src/environment/global/global_environment.cpp +++ b/src/environment/global/global_environment.cpp @@ -9,7 +9,7 @@ namespace s2e::environment { -GlobalEnvironment::GlobalEnvironment(const SimulationConfiguration* simulation_configuration) { Initialize(simulation_configuration); } +GlobalEnvironment::GlobalEnvironment(const simulation::SimulationConfiguration* simulation_configuration) { Initialize(simulation_configuration); } GlobalEnvironment::~GlobalEnvironment() { delete simulation_time_; @@ -18,7 +18,7 @@ GlobalEnvironment::~GlobalEnvironment() { delete gnss_satellites_; } -void GlobalEnvironment::Initialize(const SimulationConfiguration* simulation_configuration) { +void GlobalEnvironment::Initialize(const simulation::SimulationConfiguration* simulation_configuration) { // Get ini file path setting_file_reader::IniAccess iniAccess = setting_file_reader::IniAccess(simulation_configuration->initialize_base_file_name_); std::string simulation_time_ini_path = simulation_configuration->initialize_base_file_name_; @@ -39,12 +39,12 @@ void GlobalEnvironment::Update() { gnss_satellites_->Update(*simulation_time_); } -void GlobalEnvironment::LogSetup(Logger& logger) { +void GlobalEnvironment::LogSetup(logger::Logger& logger) { logger.AddLogList(simulation_time_); logger.AddLogList(celestial_information_); logger.AddLogList(gnss_satellites_); } -} // namespace s2e::environment - void GlobalEnvironment::Reset(void) { simulation_time_->ResetClock(); } + +} // namespace s2e::environment diff --git a/src/environment/global/global_environment.hpp b/src/environment/global/global_environment.hpp index 37c51cbfb..f067c33b8 100644 --- a/src/environment/global/global_environment.hpp +++ b/src/environment/global/global_environment.hpp @@ -26,7 +26,7 @@ class GlobalEnvironment { * @brief Constructor * @param [in] simulation_configuration: Simulation configuration */ - GlobalEnvironment(const SimulationConfiguration* simulation_configuration); + GlobalEnvironment(const simulation::SimulationConfiguration* simulation_configuration); /** * @fn ~GlobalEnvironment * @brief Destructor @@ -38,7 +38,7 @@ class GlobalEnvironment { * @brief Initialize all global environment members * @param [in] simulation_configuration: Simulation configuration */ - void Initialize(const SimulationConfiguration* simulation_configuration); + void Initialize(const simulation::SimulationConfiguration* simulation_configuration); /** * @fn Update * @brief Update states of all global environment @@ -48,7 +48,7 @@ class GlobalEnvironment { * @fn LogSetup * @brief Log setup of global environment information */ - void LogSetup(Logger& logger); + void LogSetup(logger::Logger& logger); /** * @fn Reset * @brief Reset clock of SimulationTime diff --git a/src/environment/global/gnss_satellites.cpp b/src/environment/global/gnss_satellites.cpp index ee801c9b6..9dbf72393 100644 --- a/src/environment/global/gnss_satellites.cpp +++ b/src/environment/global/gnss_satellites.cpp @@ -47,7 +47,7 @@ void GnssSatellites::Initialize(const std::vector& sp3_files, con // Initialize clock std::vector temp; temp.assign(kNumberOfInterpolation, -1.0); - clock_.assign(number_of_calculated_gnss_satellites_, s2e::math::Interpolation(temp, temp)); + clock_.assign(number_of_calculated_gnss_satellites_, math::Interpolation(temp, temp)); // Initialize interpolation for (size_t i = 0; i < kNumberOfInterpolation; i++) { @@ -76,8 +76,8 @@ void GnssSatellites::Update(const SimulationTime& simulation_time) { return; } -s2e::math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time) const { - if (gnss_satellite_id > number_of_calculated_gnss_satellites_) return s2e::math::Vector<3>(0.0); +math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time) const { + if (gnss_satellite_id > number_of_calculated_gnss_satellites_) return math::Vector<3>(0.0); s2e::time_system::EpochTime target_time; @@ -88,10 +88,10 @@ s2e::math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satell } double diff_s = target_time.GetTimeWithFraction_s() - reference_time_.GetTimeWithFraction_s(); - if (diff_s < 0.0 || diff_s > 1e6) return s2e::math::Vector<3>(0.0); + if (diff_s < 0.0 || diff_s > 1e6) return math::Vector<3>(0.0); const double kOrbitalPeriodCorrection_s = 24 * 60 * 60 * 1.003; // See http://acc.igs.org/orbits/orbit-interp_gpssoln03.pdf - return orbit_[gnss_satellite_id].CalcPositionWithTrigonometric(diff_s, s2e::math::tau / kOrbitalPeriodCorrection_s); + return orbit_[gnss_satellite_id].CalcPositionWithTrigonometric(diff_s, math::tau / kOrbitalPeriodCorrection_s); } double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time) const { @@ -133,7 +133,7 @@ bool GnssSatellites::UpdateInterpolationInformation() { for (size_t gnss_id = 0; gnss_id < number_of_calculated_gnss_satellites_; gnss_id++) { s2e::time_system::EpochTime sp3_time = s2e::time_system::EpochTime(sp3_file.GetEpochData(reference_interpolation_id_)); double time_diff_s = sp3_time.GetTimeWithFraction_s() - reference_time_.GetTimeWithFraction_s(); - s2e::math::Vector<3> sp3_position_m = 1000.0 * sp3_file.GetSatellitePosition_km(reference_interpolation_id_, gnss_id); + math::Vector<3> sp3_position_m = 1000.0 * sp3_file.GetSatellitePosition_km(reference_interpolation_id_, gnss_id); orbit_[gnss_id].PushAndPopData(time_diff_s, sp3_position_m); clock_[gnss_id].PushAndPopData(time_diff_s, sp3_file.GetSatelliteClockOffset(reference_interpolation_id_, gnss_id)); @@ -159,7 +159,7 @@ std::string GnssSatellites::GetLogHeader() const { // TODO: Add log output for other navigation systems for (size_t gps_index = 0; gps_index < kNumberOfGpsSatellite; gps_index++) { str_tmp += logger::WriteVector("GPS" + std::to_string(gps_index) + "_position", "ecef", "m", 3); - str_tmp += WriteScalar("GPS" + std::to_string(gps_index) + "_clock_offset", "s"); + str_tmp += logger::WriteScalar("GPS" + std::to_string(gps_index) + "_clock_offset", "s"); } return str_tmp; @@ -170,7 +170,7 @@ std::string GnssSatellites::GetLogValue() const { for (size_t gps_index = 0; gps_index < kNumberOfGpsSatellite; gps_index++) { str_tmp += logger::WriteVector(GetPosition_ecef_m(gps_index), 16); - str_tmp += WriteScalar(GetClock_s(gps_index)); + str_tmp += logger::WriteScalar(GetClock_s(gps_index)); } return str_tmp; diff --git a/src/environment/global/gnss_satellites.hpp b/src/environment/global/gnss_satellites.hpp index 563dbd3e7..49e73f2a1 100644 --- a/src/environment/global/gnss_satellites.hpp +++ b/src/environment/global/gnss_satellites.hpp @@ -77,7 +77,7 @@ class GnssSatellites : public logger::ILoggable { */ void Update(const SimulationTime& simulation_time); - inline s2e::math::Vector<3> GetPosition_eci_m(const size_t gnss_satellite_id) const { + inline math::Vector<3> GetPosition_eci_m(const size_t gnss_satellite_id) const { // TODO: Add target time for earth rotation calculation return earth_rotation_.GetDcmJ2000ToEcef().Transpose() * GetPosition_ecef_m(gnss_satellite_id); } @@ -89,7 +89,7 @@ class GnssSatellites : public logger::ILoggable { * @param [in] time: Target time to get the GNSS satellite. When the argument is not set, the last updated time is used for the calculation. * @return GNSS satellite position at ECEF frame at the time. Or return zero vector when the arguments are out of range. */ - s2e::math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time = s2e::time_system::EpochTime(0, 0.0)) const; + math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time = s2e::time_system::EpochTime(0, 0.0)) const; /** * @fn GetGetClock_s @@ -123,7 +123,7 @@ class GnssSatellites : public logger::ILoggable { s2e::time_system::EpochTime current_epoch_time_; //!< The last updated time std::vector orbit_; //!< GNSS satellite orbit with interpolation - std::vector clock_; //!< GNSS satellite clock offset with interpolation + std::vector clock_; //!< GNSS satellite clock offset with interpolation // References const EarthRotation& earth_rotation_; //!< Earth rotation diff --git a/src/environment/global/hipparcos_catalogue.cpp b/src/environment/global/hipparcos_catalogue.cpp index a4fb9d5f4..83e378997 100644 --- a/src/environment/global/hipparcos_catalogue.cpp +++ b/src/environment/global/hipparcos_catalogue.cpp @@ -52,10 +52,10 @@ bool HipparcosCatalogue::ReadContents(const std::string& file_name, const char d return true; } -s2e::math::Vector<3> HipparcosCatalogue::GetStarDirection_i(size_t rank) const { - s2e::math::Vector<3> direction_i; - double ra_rad = GetRightAscension_deg(rank) * s2e::math::deg_to_rad; - double de_rad = GetDeclination_deg(rank) * s2e::math::deg_to_rad; +math::Vector<3> HipparcosCatalogue::GetStarDirection_i(size_t rank) const { + math::Vector<3> direction_i; + double ra_rad = GetRightAscension_deg(rank) * math::deg_to_rad; + double de_rad = GetDeclination_deg(rank) * math::deg_to_rad; direction_i[0] = cos(ra_rad) * cos(de_rad); direction_i[1] = sin(ra_rad) * cos(de_rad); @@ -64,9 +64,9 @@ s2e::math::Vector<3> HipparcosCatalogue::GetStarDirection_i(size_t rank) const { return direction_i; } -s2e::math::Vector<3> HipparcosCatalogue::GetStarDirection_b(size_t rank, s2e::math::Quaternion quaternion_i2b) const { - s2e::math::Vector<3> direction_i; - s2e::math::Vector<3> direction_b; +math::Vector<3> HipparcosCatalogue::GetStarDirection_b(size_t rank, math::Quaternion quaternion_i2b) const { + math::Vector<3> direction_i; + math::Vector<3> direction_b; direction_i = GetStarDirection_i(rank); direction_b = quaternion_i2b.FrameConversion(direction_i); diff --git a/src/environment/global/hipparcos_catalogue.hpp b/src/environment/global/hipparcos_catalogue.hpp index bc7a70f0f..8067bfcd5 100644 --- a/src/environment/global/hipparcos_catalogue.hpp +++ b/src/environment/global/hipparcos_catalogue.hpp @@ -84,14 +84,14 @@ class HipparcosCatalogue : public logger::ILoggable { *@brief Return direction vector of a star in the inertial frame *@param [in] rank: Rank of star magnitude in read catalogue */ - s2e::math::Vector<3> GetStarDirection_i(size_t rank) const; + math::Vector<3> GetStarDirection_i(size_t rank) const; /** *@fn GetStarDir_b *@brief Return direction vector of a star in the body-fixed frame *@param [in] rank: Rank of star magnitude in read catalogue *@param [in] quaternion_i2b: Quaternion from the inertial frame to the body-fixed frame */ - s2e::math::Vector<3> GetStarDirection_b(size_t rank, s2e::math::Quaternion quaternion_i2b) const; + math::Vector<3> GetStarDirection_b(size_t rank, math::Quaternion quaternion_i2b) const; // Override logger::ILoggable /** diff --git a/src/environment/global/moon_rotation.cpp b/src/environment/global/moon_rotation.cpp index c8843e3a0..b40692284 100644 --- a/src/environment/global/moon_rotation.cpp +++ b/src/environment/global/moon_rotation.cpp @@ -14,13 +14,13 @@ namespace s2e::environment { MoonRotation::MoonRotation(const CelestialInformation& celestial_information, MoonRotationMode mode) : mode_(mode), celestial_information_(celestial_information) { - dcm_j2000_to_mcmf_ = s2e::math::MakeIdentityMatrix<3>(); + dcm_j2000_to_mcmf_ = math::MakeIdentityMatrix<3>(); } void MoonRotation::Update(const SimulationTime& simulation_time) { if (mode_ == MoonRotationMode::kSimple) { - s2e::math::Vector<3> moon_position_eci_m = celestial_information_.GetPositionFromSelectedBody_i_m("MOON", "EARTH"); - s2e::math::Vector<3> moon_velocity_eci_m_s = celestial_information_.GetVelocityFromSelectedBody_i_m_s("MOON", "EARTH"); + math::Vector<3> moon_position_eci_m = celestial_information_.GetPositionFromSelectedBody_i_m("MOON", "EARTH"); + math::Vector<3> moon_velocity_eci_m_s = celestial_information_.GetVelocityFromSelectedBody_i_m_s("MOON", "EARTH"); dcm_j2000_to_mcmf_ = s2e::planet_rotation::CalcDcmEciToPrincipalAxis(moon_position_eci_m, moon_velocity_eci_m_s); } else if (mode_ == MoonRotationMode::kIauMoon) { ConstSpiceChar from[] = "J2000"; @@ -34,7 +34,7 @@ void MoonRotation::Update(const SimulationTime& simulation_time) { } } } else { - dcm_j2000_to_mcmf_ = s2e::math::MakeIdentityMatrix<3>(); + dcm_j2000_to_mcmf_ = math::MakeIdentityMatrix<3>(); } } diff --git a/src/environment/global/moon_rotation.hpp b/src/environment/global/moon_rotation.hpp index ebdc7c4a3..196e2d6a6 100644 --- a/src/environment/global/moon_rotation.hpp +++ b/src/environment/global/moon_rotation.hpp @@ -11,10 +11,10 @@ #include "math_physics/math/vector.hpp" #include "simulation_time.hpp" -class CelestialInformation; - namespace s2e::environment { +class CelestialInformation; + /** * @enum MoonRotationMode * @brief Definition of calculation mode of moon rotation @@ -55,11 +55,11 @@ class MoonRotation { * @brief Return the DCM between J2000 inertial frame and the Moon Centered Moon Fixed frame * @note Because this is just a DCM, users need to consider the origin of the vector, which you want to convert with this matrix. */ - inline const s2e::math::Matrix<3, 3> GetDcmJ2000ToMcmf() const { return dcm_j2000_to_mcmf_; }; + inline const math::Matrix<3, 3> GetDcmJ2000ToMcmf() const { return dcm_j2000_to_mcmf_; }; private: MoonRotationMode mode_; //!< Rotation mode - s2e::math::Matrix<3, 3> dcm_j2000_to_mcmf_; //!< Direction Cosine Matrix J2000 to MCMF (Moon Centered Moon Fixed) + math::Matrix<3, 3> dcm_j2000_to_mcmf_; //!< Direction Cosine Matrix J2000 to MCMF (Moon Centered Moon Fixed) const CelestialInformation &celestial_information_; //!< Celestial Information to get moon orbit }; diff --git a/src/environment/global/physical_constants.hpp b/src/environment/global/physical_constants.hpp index 2c279b389..c97259ee2 100644 --- a/src/environment/global/physical_constants.hpp +++ b/src/environment/global/physical_constants.hpp @@ -39,9 +39,9 @@ DEFINE_PHYSICAL_CONSTANT(earth_flattening, 3.352797e-3L) / #undef DEFINE_PHYSICAL_CONSTANT -} // namespace s2e::environment +inline double degC2K(double degC) { return (degC - absolute_zero_degC); } +inline double K2degC(double K) { return (K + absolute_zero_degC); } -inline double degC2K(double degC) { return (degC - environment::absolute_zero_degC); } -inline double K2degC(double K) { return (K + environment::absolute_zero_degC); } +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_GLOBAL_PHYSICAL_CONSTANT_HPP_ diff --git a/src/environment/global/simulation_time.cpp b/src/environment/global/simulation_time.cpp index d1c74d495..600e70440 100644 --- a/src/environment/global/simulation_time.cpp +++ b/src/environment/global/simulation_time.cpp @@ -204,8 +204,8 @@ void SimulationTime::PrintStartDateTime(void) const { string SimulationTime::GetLogHeader() const { string str_tmp = ""; - str_tmp += WriteScalar("elapsed_time", "s"); - str_tmp += WriteScalar("time", "UTC"); + str_tmp += logger::WriteScalar("elapsed_time", "s"); + str_tmp += logger::WriteScalar("time", "UTC"); return str_tmp; } @@ -213,7 +213,7 @@ string SimulationTime::GetLogHeader() const { string SimulationTime::GetLogValue() const { string str_tmp = ""; - str_tmp += WriteScalar(elapsed_time_sec_); + str_tmp += logger::WriteScalar(elapsed_time_sec_); const char kSize = 100; char ymdhms[kSize]; diff --git a/src/environment/local/atmosphere.cpp b/src/environment/local/atmosphere.cpp index 0b0c9cccc..4659f8c43 100644 --- a/src/environment/local/atmosphere.cpp +++ b/src/environment/local/atmosphere.cpp @@ -52,7 +52,7 @@ Atmosphere::Atmosphere(const std::string model, const std::string space_weather_ } } -double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const Orbit& orbit) { +double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const dynamics::orbit::Orbit& orbit) { if (!is_calc_enabled_) return 0; if (model_ == "STANDARD") { @@ -68,7 +68,7 @@ double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const Orbit& manual_average_f107_, manual_ap_); } else if (model_ == "HARRIS_PRIESTER") { // Harris-Priester - s2e::math::Vector<3> sun_direction_eci = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("SUN").CalcNormalizedVector(); + math::Vector<3> sun_direction_eci = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("SUN").CalcNormalizedVector(); air_density_kg_m3_ = s2e::atmosphere::CalcAirDensityWithHarrisPriester_kg_m3(orbit.GetGeodeticPosition(), sun_direction_eci); } else { // No suitable model @@ -88,7 +88,7 @@ double Atmosphere::AddNoise(const double rho_kg_m3) { std::string Atmosphere::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteScalar(air_density_kg_m3_); + str_tmp += logger::WriteScalar(air_density_kg_m3_); return str_tmp; } @@ -96,7 +96,7 @@ std::string Atmosphere::GetLogValue() const { std::string Atmosphere::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteScalar("air_density_at_spacecraft_position", "kg/m3"); + str_tmp += logger::WriteScalar("air_density_at_spacecraft_position", "kg/m3"); return str_tmp; } diff --git a/src/environment/local/atmosphere.hpp b/src/environment/local/atmosphere.hpp index 6bc47e246..197c0972a 100644 --- a/src/environment/local/atmosphere.hpp +++ b/src/environment/local/atmosphere.hpp @@ -51,7 +51,7 @@ class Atmosphere : public logger::ILoggable { * @param [in] position: Position of target point to calculate the air density * @return Atmospheric density [kg/m^3] */ - double CalcAirDensity_kg_m3(const double decimal_year, const Orbit& orbit); + double CalcAirDensity_kg_m3(const double decimal_year, const dynamics::orbit::Orbit& orbit); /** * @fn GetAirDensity * @brief Return Atmospheric density [kg/m^3] diff --git a/src/environment/local/earth_albedo.cpp b/src/environment/local/earth_albedo.cpp index 46831b8e5..1a38af8da 100644 --- a/src/environment/local/earth_albedo.cpp +++ b/src/environment/local/earth_albedo.cpp @@ -26,8 +26,8 @@ void EarthAlbedo::UpdateAllStates() { std::string EarthAlbedo::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteScalar("earth_albedo_factor"); - str_tmp += WriteScalar("earth_albedo_W_m2"); + str_tmp += logger::WriteScalar("earth_albedo_factor"); + str_tmp += logger::WriteScalar("earth_albedo_W_m2"); return str_tmp; } @@ -35,14 +35,14 @@ std::string EarthAlbedo::GetLogHeader() const { std::string EarthAlbedo::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteScalar(earth_albedo_factor_); - str_tmp += WriteScalar(earth_albedo_W_m2_); + str_tmp += logger::WriteScalar(earth_albedo_factor_); + str_tmp += logger::WriteScalar(earth_albedo_W_m2_); return str_tmp; } void EarthAlbedo::CalcEarthAlbedo(const LocalCelestialInformation* local_celestial_information) { - s2e::math::Vector<3> earth_position_b_m = local_celestial_information->GetPositionFromSpacecraft_b_m("EARTH"); + math::Vector<3> earth_position_b_m = local_celestial_information->GetPositionFromSpacecraft_b_m("EARTH"); double earth_distance_m = earth_position_b_m.CalcNorm(); earth_albedo_W_m2_ = srp_environment_->GetPowerDensity_W_m2() * GetEarthAlbedoFactor() * pow((environment::astronomy::earth_equatorial_radius_m / earth_distance_m), 2.0) / 4.0; diff --git a/src/environment/local/geomagnetic_field.cpp b/src/environment/local/geomagnetic_field.cpp index 0b5eab1b9..5b713a11d 100644 --- a/src/environment/local/geomagnetic_field.cpp +++ b/src/environment/local/geomagnetic_field.cpp @@ -25,7 +25,7 @@ GeomagneticField::GeomagneticField(const std::string igrf_file_name, const doubl } void GeomagneticField::CalcMagneticField(const double decimal_year, const double sidereal_day, const s2e::geodesy::GeodeticPosition position, - const s2e::math::Quaternion quaternion_i2b) { + const math::Quaternion quaternion_i2b) { if (!IsCalcEnabled) return; const double lat_rad = position.GetLatitude_rad(); @@ -42,8 +42,8 @@ void GeomagneticField::CalcMagneticField(const double decimal_year, const double } void GeomagneticField::AddNoise(double* magnetic_field_array_i_nT) { - static s2e::math::Vector<3> standard_deviation(random_walk_standard_deviation_nT_); - static s2e::math::Vector<3> limit(random_walk_limit_nT_); + static math::Vector<3> standard_deviation(random_walk_standard_deviation_nT_); + static math::Vector<3> limit(random_walk_limit_nT_); static RandomWalk<3> random_walk(0.1, standard_deviation, limit); static s2e::randomization::NormalRand white_noise(0.0, white_noise_standard_deviation_nT_, s2e::randomization::global_randomization.MakeSeed()); diff --git a/src/environment/local/geomagnetic_field.hpp b/src/environment/local/geomagnetic_field.hpp index ba3f05329..6b9857946 100644 --- a/src/environment/local/geomagnetic_field.hpp +++ b/src/environment/local/geomagnetic_field.hpp @@ -46,18 +46,18 @@ class GeomagneticField : public logger::ILoggable { * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame */ void CalcMagneticField(const double decimal_year, const double sidereal_day, const s2e::geodesy::GeodeticPosition position, - const s2e::math::Quaternion quaternion_i2b); + const math::Quaternion quaternion_i2b); /** * @fn GetGeomagneticField_i_nT * @brief Return magnetic field vector in the inertial frame [nT] */ - inline s2e::math::Vector<3> GetGeomagneticField_i_nT() const { return magnetic_field_i_nT_; } + inline math::Vector<3> GetGeomagneticField_i_nT() const { return magnetic_field_i_nT_; } /** * @fn GetGeomagneticField_b_nT * @brief Return magnetic field vector in the body fixed frame [nT] */ - inline s2e::math::Vector<3> GetGeomagneticField_b_nT() const { return magnetic_field_b_nT_; } + inline math::Vector<3> GetGeomagneticField_b_nT() const { return magnetic_field_b_nT_; } // Override logger::ILoggable /** @@ -72,8 +72,8 @@ class GeomagneticField : public logger::ILoggable { virtual std::string GetLogValue() const; private: - s2e::math::Vector<3> magnetic_field_i_nT_; //!< Magnetic field vector at the inertial frame [nT] - s2e::math::Vector<3> magnetic_field_b_nT_; //!< Magnetic field vector at the spacecraft body fixed frame [nT] + math::Vector<3> magnetic_field_i_nT_; //!< Magnetic field vector at the inertial frame [nT] + math::Vector<3> magnetic_field_b_nT_; //!< Magnetic field vector at the spacecraft body fixed frame [nT] double random_walk_standard_deviation_nT_; //!< Standard deviation of Random Walk [nT] double random_walk_limit_nT_; //!< Limit of Random Walk [nT] double white_noise_standard_deviation_nT_; //!< Standard deviation of white noise [nT] diff --git a/src/environment/local/local_celestial_information.cpp b/src/environment/local/local_celestial_information.cpp index 8699e21f2..40d78c536 100644 --- a/src/environment/local/local_celestial_information.cpp +++ b/src/environment/local/local_celestial_information.cpp @@ -45,11 +45,11 @@ LocalCelestialInformation::~LocalCelestialInformation() { delete[] celestial_body_velocity_from_spacecraft_b_m_s_; } -void LocalCelestialInformation::UpdateAllObjectsInformation(const s2e::math::Vector<3> spacecraft_position_from_center_i_m, - const s2e::math::Vector<3> spacecraft_velocity_from_center_i_m_s, - const s2e::math::Quaternion quaternion_i2b, - const s2e::math::Vector<3> spacecraft_angular_velocity_rad_s) { - s2e::math::Vector<3> celestial_body_position_i_m, celestial_body_velocity_i_m_s; +void LocalCelestialInformation::UpdateAllObjectsInformation(const math::Vector<3> spacecraft_position_from_center_i_m, + const math::Vector<3> spacecraft_velocity_from_center_i_m_s, + const math::Quaternion quaternion_i2b, + const math::Vector<3> spacecraft_angular_velocity_rad_s) { + math::Vector<3> celestial_body_position_i_m, celestial_body_velocity_i_m_s; for (int i = 0; i < global_celestial_information_->GetNumberOfSelectedBodies(); i++) { celestial_body_position_i_m = global_celestial_information_->GetPositionFromCenter_i_m(i); celestial_body_velocity_i_m_s = global_celestial_information_->GetVelocityFromCenter_i_m_s(i); @@ -64,8 +64,8 @@ void LocalCelestialInformation::UpdateAllObjectsInformation(const s2e::math::Vec return; } -void LocalCelestialInformation::CalcAllPosVel_b(const s2e::math::Quaternion quaternion_i2b, const s2e::math::Vector<3> spacecraft_angular_velocity_rad_s) { - s2e::math::Vector<3> celestial_body_position_i_m, celestial_body_velocity_i_m_s; +void LocalCelestialInformation::CalcAllPosVel_b(const math::Quaternion quaternion_i2b, const math::Vector<3> spacecraft_angular_velocity_rad_s) { + math::Vector<3> celestial_body_position_i_m, celestial_body_velocity_i_m_s; double r_buf1_i[3], velocity_buf1_i[3], r_buf1_b[3], velocity_buf1_b[3]; double r_buf2_i[3], velocity_buf2_i[3], r_buf2_b[3], velocity_buf2_b[3]; for (int i = 0; i < global_celestial_information_->GetNumberOfSelectedBodies(); i++) { @@ -91,50 +91,50 @@ void LocalCelestialInformation::CalcAllPosVel_b(const s2e::math::Quaternion quat } } -void LocalCelestialInformation::ConvertInertialToBody(const double* input_i, double* output_b, s2e::math::Quaternion quaternion_i2b) { - s2e::math::Vector<3> temp_i; +void LocalCelestialInformation::ConvertInertialToBody(const double* input_i, double* output_b, math::Quaternion quaternion_i2b) { + math::Vector<3> temp_i; for (int i = 0; i < 3; i++) { temp_i[i] = input_i[i]; } - s2e::math::Vector<3> temp_b = quaternion_i2b.FrameConversion(temp_i); + math::Vector<3> temp_b = quaternion_i2b.FrameConversion(temp_i); for (int i = 0; i < 3; i++) { output_b[i] = temp_b[i]; } } void LocalCelestialInformation::ConvertVelocityInertialToBody(const double* position_i, const double* velocity_i, double* velocity_b, - const s2e::math::Quaternion quaternion_i2b, const s2e::math::Vector<3> angular_velocity_b) { + const math::Quaternion quaternion_i2b, const math::Vector<3> angular_velocity_b) { // copy input vector - s2e::math::Vector<3> vi; + math::Vector<3> vi; for (int i = 0; i < 3; i++) { vi[i] = velocity_i[i]; } - s2e::math::Vector<3> ri; + math::Vector<3> ri; for (int i = 0; i < 3; i++) { ri[i] = position_i[i]; } // convert body rate vector into that in inertial coordinate - s2e::math::Vector<3> wb; + math::Vector<3> wb; for (int i = 0; i < 3; i++) { wb[i] = angular_velocity_b[i]; } // compute cross term wxr - s2e::math::Vector<3> wxposition_i = OuterProduct(wb, ri); + math::Vector<3> wxposition_i = OuterProduct(wb, ri); // compute dr/dt + wxr for (int i = 0; i < 3; i++) { vi[i] = vi[i] - wxposition_i[i]; } // convert vector in inertial coordinate into that in body coordinate - s2e::math::Vector<3> temp_b = quaternion_i2b.FrameConversion(vi); + math::Vector<3> temp_b = quaternion_i2b.FrameConversion(vi); for (int i = 0; i < 3; i++) { velocity_b[i] = temp_b[i]; } } -s2e::math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_i_m(const char* body_name) const { - s2e::math::Vector<3> position; +math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_i_m(const char* body_name) const { + math::Vector<3> position; int index = 0; index = global_celestial_information_->CalcBodyIdFromName(body_name); for (int i = 0; i < 3; i++) { @@ -143,14 +143,14 @@ s2e::math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_i_m(co return position; } -s2e::math::Vector<3> LocalCelestialInformation::GetCenterBodyPositionFromSpacecraft_i_m() const { +math::Vector<3> LocalCelestialInformation::GetCenterBodyPositionFromSpacecraft_i_m() const { std::string body_name = global_celestial_information_->GetCenterBodyName(); - s2e::math::Vector<3> position = GetPositionFromSpacecraft_i_m(body_name.c_str()); + math::Vector<3> position = GetPositionFromSpacecraft_i_m(body_name.c_str()); return position; } -s2e::math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_b_m(const char* body_name) const { - s2e::math::Vector<3> position; +math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_b_m(const char* body_name) const { + math::Vector<3> position; int index = 0; index = global_celestial_information_->CalcBodyIdFromName(body_name); for (int i = 0; i < 3; i++) { @@ -159,9 +159,9 @@ s2e::math::Vector<3> LocalCelestialInformation::GetPositionFromSpacecraft_b_m(co return position; } -s2e::math::Vector<3> LocalCelestialInformation::GetCenterBodyPositionFromSpacecraft_b_m(void) const { +math::Vector<3> LocalCelestialInformation::GetCenterBodyPositionFromSpacecraft_b_m(void) const { std::string body_name = global_celestial_information_->GetCenterBodyName(); - s2e::math::Vector<3> position = GetPositionFromSpacecraft_b_m(body_name.c_str()); + math::Vector<3> position = GetPositionFromSpacecraft_b_m(body_name.c_str()); return position; } @@ -191,10 +191,10 @@ std::string LocalCelestialInformation::GetLogValue() const { std::string str_tmp = ""; for (int i = 0; i < global_celestial_information_->GetNumberOfSelectedBodies(); i++) { for (int j = 0; j < 3; j++) { - str_tmp += WriteScalar(celestial_body_position_from_spacecraft_b_m_[i * 3 + j]); + str_tmp += logger::WriteScalar(celestial_body_position_from_spacecraft_b_m_[i * 3 + j]); } for (int j = 0; j < 3; j++) { - str_tmp += WriteScalar(celestial_body_velocity_from_spacecraft_b_m_s_[i * 3 + j]); + str_tmp += logger::WriteScalar(celestial_body_velocity_from_spacecraft_b_m_s_[i * 3 + j]); } } return str_tmp; diff --git a/src/environment/local/local_celestial_information.hpp b/src/environment/local/local_celestial_information.hpp index e81a7652f..93a2c13c8 100644 --- a/src/environment/local/local_celestial_information.hpp +++ b/src/environment/local/local_celestial_information.hpp @@ -36,33 +36,33 @@ class LocalCelestialInformation : public logger::ILoggable { * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame * @param [in] spacecraft_angular_velocity_rad_s: Spacecraft angular velocity with respect to the inertial frame [rad/s] */ - void UpdateAllObjectsInformation(const s2e::math::Vector<3> spacecraft_position_from_center_i_m, - const s2e::math::Vector<3> spacecraft_velocity_from_center_i_m_s, const s2e::math::Quaternion quaternion_i2b, - const s2e::math::Vector<3> spacecraft_angular_velocity_rad_s); + void UpdateAllObjectsInformation(const math::Vector<3> spacecraft_position_from_center_i_m, + const math::Vector<3> spacecraft_velocity_from_center_i_m_s, const math::Quaternion quaternion_i2b, + const math::Vector<3> spacecraft_angular_velocity_rad_s); /** * @fn GetPositionFromSpacecraft_i_m * @brief Return position of a selected body (Origin: Spacecraft, Frame: Inertial frame) * @param [in] body_name Celestial body name */ - s2e::math::Vector<3> GetPositionFromSpacecraft_i_m(const char* body_name) const; + math::Vector<3> GetPositionFromSpacecraft_i_m(const char* body_name) const; /** * @fn GetCenterBodyPositionFromSpacecraft_i_m * @brief Return position of the center body (Origin: Spacecraft, Frame: Inertial frame) */ - s2e::math::Vector<3> GetCenterBodyPositionFromSpacecraft_i_m(void) const; + math::Vector<3> GetCenterBodyPositionFromSpacecraft_i_m(void) const; /** * @fn GetPositionFromSpacecraft_b_m * @brief Return position of a selected body (Origin: Spacecraft, Frame: Body fixed frame) * @param [in] body_name Celestial body name */ - s2e::math::Vector<3> GetPositionFromSpacecraft_b_m(const char* body_name) const; + math::Vector<3> GetPositionFromSpacecraft_b_m(const char* body_name) const; /** * @fn GetCenterBodyPositionFromSpacecraft_b_m * @brief Return position of the center body (Origin: Spacecraft, Frame: Body fixed frame) */ - s2e::math::Vector<3> GetCenterBodyPositionFromSpacecraft_b_m(void) const; + math::Vector<3> GetCenterBodyPositionFromSpacecraft_b_m(void) const; /** * @fn GetGlobalInfo @@ -98,7 +98,7 @@ class LocalCelestialInformation : public logger::ILoggable { * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame * @param [in] spacecraft_angular_velocity_rad_s: Spacecraft angular velocity with respect to the inertial frame [rad/s] */ - void CalcAllPosVel_b(const s2e::math::Quaternion quaternion_i2b, const s2e::math::Vector<3> spacecraft_angular_velocity_rad_s); + void CalcAllPosVel_b(const math::Quaternion quaternion_i2b, const math::Vector<3> spacecraft_angular_velocity_rad_s); /** * @fn ConvertInertialToBody @@ -107,7 +107,7 @@ class LocalCelestialInformation : public logger::ILoggable { * @param [out] output_b: Output vector in the body fixed frame * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame */ - void ConvertInertialToBody(const double* input_i, double* output_b, const s2e::math::Quaternion quaternion_i2b); + void ConvertInertialToBody(const double* input_i, double* output_b, const math::Quaternion quaternion_i2b); /** * @fn ConvertVelocityInertialToBody @@ -118,8 +118,8 @@ class LocalCelestialInformation : public logger::ILoggable { * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame * @param [in] angular_velocity_b: Spacecraft angular velocity with respect to the inertial frame [rad/s] */ - void ConvertVelocityInertialToBody(const double* position_i, const double* velocity_i, double* velocity_b, const s2e::math::Quaternion quaternion_i2b, - const s2e::math::Vector<3> angular_velocity_b); + void ConvertVelocityInertialToBody(const double* position_i, const double* velocity_i, double* velocity_b, const math::Quaternion quaternion_i2b, + const math::Vector<3> angular_velocity_b); }; } // namespace s2e::environment diff --git a/src/environment/local/local_environment.cpp b/src/environment/local/local_environment.cpp index 85d23926c..3fbe777b1 100644 --- a/src/environment/local/local_environment.cpp +++ b/src/environment/local/local_environment.cpp @@ -8,9 +8,9 @@ #include "dynamics/orbit/orbit.hpp" #include "setting_file_reader/initialize_file_access.hpp" -namespace s2e::environment +namespace s2e::environment{ -LocalEnvironment::LocalEnvironment(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, +LocalEnvironment::LocalEnvironment(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id) { Initialize(simulation_configuration, global_environment, spacecraft_id); } @@ -23,7 +23,7 @@ LocalEnvironment::~LocalEnvironment() { delete celestial_information_; } -void LocalEnvironment::Initialize(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, +void LocalEnvironment::Initialize(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id) { // Read file name setting_file_reader::IniAccess iniAccess = setting_file_reader::IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); @@ -71,7 +71,7 @@ void LocalEnvironment::Update(const dynamics::Dynamics* dynamics, const Simulati } } -void LocalEnvironment::LogSetup(Logger& logger) { +void LocalEnvironment::LogSetup(logger::Logger& logger) { logger.AddLogList(geomagnetic_field_); logger.AddLogList(solar_radiation_pressure_environment_); logger.AddLogList(earth_albedo_); diff --git a/src/environment/local/local_environment.hpp b/src/environment/local/local_environment.hpp index f251135f4..f6d698f67 100644 --- a/src/environment/local/local_environment.hpp +++ b/src/environment/local/local_environment.hpp @@ -32,7 +32,7 @@ class LocalEnvironment { * @param [in] global_environment: Global environment * @param [in] spacecraft_id: Satellite ID */ - LocalEnvironment(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id); + LocalEnvironment(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id); /** * @fn ~LocalEnvironment * @brief Destructor @@ -51,7 +51,7 @@ class LocalEnvironment { * @fn LogSetup * @brief Log setup for local environments */ - void LogSetup(Logger& logger); + void LogSetup(logger::Logger& logger); /** * @fn GetAtmosphere @@ -93,7 +93,7 @@ class LocalEnvironment { * @param [in] global_environment: Global environment * @param [in] spacecraft_id: Satellite ID */ - void Initialize(const SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id); + void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id); }; } // namespace s2e::environment diff --git a/src/environment/local/solar_radiation_pressure_environment.cpp b/src/environment/local/solar_radiation_pressure_environment.cpp index ff60f94be..ba259567b 100644 --- a/src/environment/local/solar_radiation_pressure_environment.cpp +++ b/src/environment/local/solar_radiation_pressure_environment.cpp @@ -33,7 +33,7 @@ void SolarRadiationPressureEnvironment::UpdateAllStates() { } void SolarRadiationPressureEnvironment::UpdatePressure() { - const s2e::math::Vector<3> r_sc2sun_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); + const math::Vector<3> r_sc2sun_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); const double distance_sat_to_sun = r_sc2sun_eci.CalcNorm(); solar_radiation_pressure_N_m2_ = solar_constant_W_m2_ / environment::speed_of_light_m_s / pow(distance_sat_to_sun / environment::astronomical_unit_m, 2.0); @@ -42,8 +42,8 @@ void SolarRadiationPressureEnvironment::UpdatePressure() { std::string SolarRadiationPressureEnvironment::GetLogHeader() const { std::string str_tmp = ""; - str_tmp += WriteScalar("solar_radiation_pressure_at_spacecraft_position", "N/m2"); - str_tmp += WriteScalar("shadow_coefficient_at_spacecraft_position"); + str_tmp += logger::WriteScalar("solar_radiation_pressure_at_spacecraft_position", "N/m2"); + str_tmp += logger::WriteScalar("shadow_coefficient_at_spacecraft_position"); return str_tmp; } @@ -51,8 +51,8 @@ std::string SolarRadiationPressureEnvironment::GetLogHeader() const { std::string SolarRadiationPressureEnvironment::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteScalar(solar_radiation_pressure_N_m2_ * shadow_coefficient_); - str_tmp += WriteScalar(shadow_coefficient_); + str_tmp += logger::WriteScalar(solar_radiation_pressure_N_m2_ * shadow_coefficient_); + str_tmp += logger::WriteScalar(shadow_coefficient_); return str_tmp; } @@ -63,8 +63,8 @@ void SolarRadiationPressureEnvironment::CalcShadowCoefficient(std::string shadow return; } - const s2e::math::Vector<3> r_sc2sun_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); - const s2e::math::Vector<3> r_sc2source_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m(shadow_source_name.c_str()); + const math::Vector<3> r_sc2sun_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); + const math::Vector<3> r_sc2source_eci = local_celestial_information_->GetPositionFromSpacecraft_i_m(shadow_source_name.c_str()); const double shadow_source_radius_m = local_celestial_information_->GetGlobalInformation().GetMeanRadiusFromName_m(shadow_source_name.c_str()); @@ -73,7 +73,7 @@ void SolarRadiationPressureEnvironment::CalcShadowCoefficient(std::string shadow const double sd_source = asin(shadow_source_radius_m / r_sc2source_eci.CalcNorm()); // Apparent radius of the shadow source // Angle of deviation from shadow source center to sun center - s2e::math::Vector<3> r_source2sun_eci = r_sc2sun_eci - r_sc2source_eci; + math::Vector<3> r_source2sun_eci = r_sc2sun_eci - r_sc2source_eci; const double delta = acos(InnerProduct(r_sc2source_eci, r_sc2sun_eci - r_sc2source_eci) / r_sc2source_eci.CalcNorm() / r_source2sun_eci.CalcNorm()); // The angle between the center of the sun and the common chord const double x = (delta * delta + sd_sun * sd_sun - sd_source * sd_source) / (2.0 * delta); @@ -93,7 +93,7 @@ void SolarRadiationPressureEnvironment::CalcShadowCoefficient(std::string shadow } else if (fabs(a - b) <= c && c <= (a + b)) // spacecraft is in penumbra { double A = a * a * acos(x / a) + b * b * acos((c - x) / b) - c * y; // The area of the occulted segment of the apparent solar disk - shadow_coefficient_ *= 1.0 - A / (s2e::math::pi * a * a); + shadow_coefficient_ *= 1.0 - A / (math::pi * a * a); } else { // no occultation takes place if (c < (a + b)) { std::cout << "[Error SRP Environment]: The calculation error was occurred at the shadow calculation." << std::endl; diff --git a/src/logger/log_utility.hpp b/src/logger/log_utility.hpp index 54599ee07..40f47f8f1 100644 --- a/src/logger/log_utility.hpp +++ b/src/logger/log_utility.hpp @@ -37,7 +37,7 @@ inline std::string WriteScalar(const std::string name, const std::string unit); * @param [in] precision: precision for the value (number of digit) */ template -inline std::string WriteVector(const s2e::math::Vector vector, const int precision = 6); +inline std::string WriteVector(const math::Vector vector, const int precision = 6); /** * @fn WriteVector * @brief Write header for vector value @@ -54,7 +54,7 @@ inline std::string WriteVector(const std::string name, const std::string frame, * @param [in] matrix: matrix value */ template -inline std::string WriteMatrix(const s2e::math::Matrix matrix, const int precision = 6); +inline std::string WriteMatrix(const math::Matrix matrix, const int precision = 6); /** * @fn WriteMatrix * @brief Write header for matrix value @@ -72,7 +72,7 @@ inline std::string WriteMatrix(const std::string name, const std::string frame, * @brief Write quaternion value * @param [in] quaternion: Quaternion */ -inline std::string WriteQuaternion(const s2e::math::Quaternion quaternion, const int precision = 6); +inline std::string WriteQuaternion(const math::Quaternion quaternion, const int precision = 6); /** * @fn WriteQuaternion * @brief Write header for quaternion @@ -93,7 +93,7 @@ std::string WriteScalar(const T scalar, const int precision) { std::string WriteScalar(const std::string name, const std::string unit) { return name + "[" + unit + "],"; } template -std::string WriteVector(const s2e::math::Vector vector, const int precision) { +std::string WriteVector(const math::Vector vector, const int precision) { std::stringstream str_tmp; for (size_t n = 0; n < NUM; n++) { @@ -117,7 +117,7 @@ std::string WriteVector(const std::string name, const std::string frame, const s } template -std::string WriteMatrix(const s2e::math::Matrix matrix, const int precision) { +std::string WriteMatrix(const math::Matrix matrix, const int precision) { std::stringstream str_tmp; for (size_t n = 0; n < ROW; n++) { @@ -140,7 +140,7 @@ std::string WriteMatrix(const std::string name, const std::string frame, const s return str_tmp.str(); } -std::string WriteQuaternion(const s2e::math::Quaternion quaternion, const int precision) { +std::string WriteQuaternion(const math::Quaternion quaternion, const int precision) { std::stringstream str_tmp; for (size_t i = 0; i < 4; i++) { diff --git a/src/logger/logger.cpp b/src/logger/logger.cpp index 0469f38e9..4299127db 100644 --- a/src/logger/logger.cpp +++ b/src/logger/logger.cpp @@ -8,13 +8,13 @@ #include #include +namespace s2e::logger { + std::vector log_list_; bool Logger::is_directory_created_ = false; namespace fs = std::filesystem; -namespace s2e::logger { - Logger::Logger(const std::string &file_name, const fs::path &data_path, const fs::path &ini_file_name, const bool is_ini_save_enabled, const bool is_enabled) : is_enabled_(is_enabled), is_ini_save_enabled_(is_ini_save_enabled) { diff --git a/src/math_physics/atmosphere/harris_priester_model.cpp b/src/math_physics/atmosphere/harris_priester_model.cpp index e38476335..09d7a2143 100644 --- a/src/math_physics/atmosphere/harris_priester_model.cpp +++ b/src/math_physics/atmosphere/harris_priester_model.cpp @@ -28,27 +28,27 @@ double CalcScaleHeight_km(const std::map::const_iterator density */ double CalcApexDensity_g_km3(const std::map::const_iterator density_itr, const double altitude_km); -double CalcAirDensityWithHarrisPriester_kg_m3(const s2e::geodesy::GeodeticPosition geodetic_position, const s2e::math::Vector<3> sun_direction_eci, +double CalcAirDensityWithHarrisPriester_kg_m3(const s2e::geodesy::GeodeticPosition geodetic_position, const math::Vector<3> sun_direction_eci, const double f10_7, const double exponent_parameter) { // Altitude double altitude_km = geodetic_position.GetAltitude_m() / 1000.0; // Position - s2e::math::Vector<3> position_ecef_m = geodetic_position.CalcEcefPosition(); + math::Vector<3> position_ecef_m = geodetic_position.CalcEcefPosition(); // Phi: angle between the satellite position and apex of the diurnal bulge double sun_ra_rad; //!< Right ascension of the sun phi double sun_dec_rad; //!< Declination of the sun theta sun_ra_rad = atan2(sun_direction_eci[1], sun_direction_eci[0]); sun_dec_rad = atan2(sun_direction_eci[2], sqrt(sun_direction_eci[0] * sun_direction_eci[0] + sun_direction_eci[1] * sun_direction_eci[1])); - s2e::math::Vector<3> apex_direction; - const double lag_angle_rad = 30.0 * s2e::math::deg_to_rad; + math::Vector<3> apex_direction; + const double lag_angle_rad = 30.0 * math::deg_to_rad; double apex_ra_rad = sun_ra_rad + lag_angle_rad; apex_direction[0] = cos(sun_dec_rad) * cos(apex_ra_rad); apex_direction[1] = cos(sun_dec_rad) * sin(apex_ra_rad); apex_direction[2] = sin(sun_dec_rad); - double beta_rad = s2e::math::InnerProduct(position_ecef_m.CalcNormalizedVector(), apex_direction); + double beta_rad = math::InnerProduct(position_ecef_m.CalcNormalizedVector(), apex_direction); double cos_phi = pow(0.5 + beta_rad / 2.0, exponent_parameter / 2.0); // Find density coefficients from altitude diff --git a/src/math_physics/atmosphere/harris_priester_model.hpp b/src/math_physics/atmosphere/harris_priester_model.hpp index a5b05bc7e..266682c88 100644 --- a/src/math_physics/atmosphere/harris_priester_model.hpp +++ b/src/math_physics/atmosphere/harris_priester_model.hpp @@ -20,7 +20,7 @@ namespace s2e::atmosphere { * @param [in] exponent_parameter: n in the equation. n=2 for low inclination orbit and n=6 for polar orbit. * @return Atmospheric density [kg/m^3] */ -double CalcAirDensityWithHarrisPriester_kg_m3(const s2e::geodesy::GeodeticPosition geodetic_position, const s2e::math::Vector<3> sun_direction_eci, +double CalcAirDensityWithHarrisPriester_kg_m3(const s2e::geodesy::GeodeticPosition geodetic_position, const math::Vector<3> sun_direction_eci, const double f10_7 = 100.0, const double exponent_parameter = 4); } // namespace s2e::atmosphere diff --git a/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp b/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp index 8401d233b..c000ca49e 100644 --- a/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp +++ b/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp @@ -187,9 +187,9 @@ double CalcNRLMSISE00(double decyear, double latrad, double lonrad, double alt, input.year = 0; /* without effect */ input.sec = date[3] * 60.0 * 60.0 + date[4] * 60.0 + date[5]; input.alt = alt / 1000.0; - input.g_lat = latrad * s2e::math::rad_to_deg; - input.g_long = lonrad * s2e::math::rad_to_deg; - input.lst = input.sec / 3600.0 + lonrad * s2e::math::rad_to_deg / 15.0; + input.g_lat = latrad * math::rad_to_deg; + input.g_long = lonrad * math::rad_to_deg; + input.lst = input.sec / 3600.0 + lonrad * math::rad_to_deg / 15.0; if (is_manual_param) { input.f107 = manual_f107; diff --git a/src/math_physics/geodesy/geodetic_position.cpp b/src/math_physics/geodesy/geodetic_position.cpp index 50bac4071..b72aa1804 100644 --- a/src/math_physics/geodesy/geodetic_position.cpp +++ b/src/math_physics/geodesy/geodetic_position.cpp @@ -25,7 +25,7 @@ GeodeticPosition::GeodeticPosition(const double latitude_rad, const double longi CalcQuaternionXcxfToLtc(); } -void GeodeticPosition::UpdateFromEcef(const s2e::math::Vector<3> position_ecef_m) { +void GeodeticPosition::UpdateFromEcef(const math::Vector<3> position_ecef_m) { const double earth_radius_m = environment::earth_equatorial_radius_m; const double flattening = environment::earth_flattening; @@ -46,7 +46,7 @@ void GeodeticPosition::UpdateFromEcef(const s2e::math::Vector<3> position_ecef_m altitude_m_ = r_m / cos(lat_tmp_rad) - c * earth_radius_m; - if (lat_tmp_rad > s2e::math::pi_2) lat_tmp_rad -= s2e::math::tau; + if (lat_tmp_rad > math::pi_2) lat_tmp_rad -= math::tau; latitude_rad_ = lat_tmp_rad; @@ -54,7 +54,7 @@ void GeodeticPosition::UpdateFromEcef(const s2e::math::Vector<3> position_ecef_m return; } -s2e::math::Vector<3> GeodeticPosition::CalcEcefPosition() const { +math::Vector<3> GeodeticPosition::CalcEcefPosition() const { const double earth_radius_m = environment::earth_equatorial_radius_m; const double flattening = environment::earth_flattening; @@ -63,7 +63,7 @@ s2e::math::Vector<3> GeodeticPosition::CalcEcefPosition() const { double c = 1.0 / sqrt(1.0 - e2 * sin(latitude_rad_) * sin(latitude_rad_)); double n = c * earth_radius_m; - s2e::math::Vector<3> pos_ecef_m; + math::Vector<3> pos_ecef_m; pos_ecef_m(0) = (n + altitude_m_) * cos(latitude_rad_) * cos(theta); pos_ecef_m(1) = (n + altitude_m_) * cos(latitude_rad_) * sin(theta); pos_ecef_m(2) = (n * (1.0 - e2) + altitude_m_) * sin(latitude_rad_); @@ -72,7 +72,7 @@ s2e::math::Vector<3> GeodeticPosition::CalcEcefPosition() const { } void GeodeticPosition::CalcQuaternionXcxfToLtc() { - s2e::math::Matrix<3, 3> dcm_xcxf_to_ltc; + math::Matrix<3, 3> dcm_xcxf_to_ltc; dcm_xcxf_to_ltc[0][0] = -sin(longitude_rad_); dcm_xcxf_to_ltc[0][1] = cos(longitude_rad_); dcm_xcxf_to_ltc[0][2] = 0; diff --git a/src/math_physics/geodesy/geodetic_position.hpp b/src/math_physics/geodesy/geodetic_position.hpp index 9687e0ea9..36cb4e839 100644 --- a/src/math_physics/geodesy/geodetic_position.hpp +++ b/src/math_physics/geodesy/geodetic_position.hpp @@ -36,13 +36,13 @@ class GeodeticPosition { * @brief Update geodetic position with position vector in the ECEF frame * @param [in] position_ecef_m: Position vector in the ECEF frame [m] */ - void UpdateFromEcef(const s2e::math::Vector<3> position_ecef_m); + void UpdateFromEcef(const math::Vector<3> position_ecef_m); /** * @fn CalcEcefPosition * @brief Calculate and return the ECEF position from the geodetic position */ - s2e::math::Vector<3> CalcEcefPosition() const; + math::Vector<3> CalcEcefPosition() const; // Getter /** @@ -64,14 +64,14 @@ class GeodeticPosition { * @fn GetQuaternionXcxfToLtc * @brief Conversion quaternion from XCXF (e.g. ECEF) to LTC frame */ - inline s2e::math::Quaternion GetQuaternionXcxfToLtc() const { return quaternion_xcxf_to_ltc_; } + inline math::Quaternion GetQuaternionXcxfToLtc() const { return quaternion_xcxf_to_ltc_; } private: double latitude_rad_; //!< Latitude [rad] South: -π/2 to 0, North: 0 to π/2 double longitude_rad_; //!< Longitude [rad] East: 0 to π, West: 2π to π (i.e., defined as 0 to 2π [rad] east of the Greenwich meridian) double altitude_m_; //!< Altitude [m] - s2e::math::Quaternion quaternion_xcxf_to_ltc_; //!< Conversion quaternion from XCXF (e.g. ECEF) to LTC (Local Topographic Coordinate) + math::Quaternion quaternion_xcxf_to_ltc_; //!< Conversion quaternion from XCXF (e.g. ECEF) to LTC (Local Topographic Coordinate) /** * @fn CalcQuaternionXcxfToLtc diff --git a/src/math_physics/gnss/antex_file_reader.cpp b/src/math_physics/gnss/antex_file_reader.cpp index c27c34ff7..e3008d6b0 100644 --- a/src/math_physics/gnss/antex_file_reader.cpp +++ b/src/math_physics/gnss/antex_file_reader.cpp @@ -171,7 +171,7 @@ AntexPhaseCenterData AntexFileReader::ReadPhaseCenterData(std::ifstream& antex_f } // Phase center offset if (line.find("NORTH / EAST / UP") == ANTEX_LINE_TYPE_POSITION) { - s2e::math::Vector<3> offset{0.0}; + math::Vector<3> offset{0.0}; sscanf(line.c_str(), "%lf %lf %lf", &offset[0], &offset[1], &offset[2]); phase_center_data.SetPhaseCenterOffset_mm(offset); } diff --git a/src/math_physics/gnss/antex_file_reader.hpp b/src/math_physics/gnss/antex_file_reader.hpp index 179616bb9..9e30ebce6 100644 --- a/src/math_physics/gnss/antex_file_reader.hpp +++ b/src/math_physics/gnss/antex_file_reader.hpp @@ -128,7 +128,7 @@ class AntexPhaseCenterData { * @fn SetPhaseCenterOffset_mm * @param[in] phase_center_offset_mm: Phase center offset vector [mm] */ - inline void SetPhaseCenterOffset_mm(const s2e::math::Vector<3> phase_center_offset_mm) { phase_center_offset_mm_ = phase_center_offset_mm; } + inline void SetPhaseCenterOffset_mm(const math::Vector<3> phase_center_offset_mm) { phase_center_offset_mm_ = phase_center_offset_mm; } /** * @fn SetGridInformation * @param[in] grid_information: Grid information @@ -152,7 +152,7 @@ class AntexPhaseCenterData { * @fn GetPhaseCenterOffset_mm * @return Phase center offset vector [mm] */ - inline s2e::math::Vector<3> GetPhaseCenterOffset_mm() const { return phase_center_offset_mm_; } + inline math::Vector<3> GetPhaseCenterOffset_mm() const { return phase_center_offset_mm_; } /** * @fn GetGridInformation * @return Grid information @@ -166,7 +166,7 @@ class AntexPhaseCenterData { private: std::string frequency_name_ = ""; //!< Frequency name - s2e::math::Vector<3> phase_center_offset_mm_{0.0}; //!< Phase center offset [mm] + math::Vector<3> phase_center_offset_mm_{0.0}; //!< Phase center offset [mm] AntexGridDefinition grid_information_; //!< Grid information std::vector> phase_center_variation_matrix_mm_; //!< Phase center variation [mm] (column, row definition: [azimuth][zenith]) }; diff --git a/src/math_physics/gnss/sp3_file_reader.cpp b/src/math_physics/gnss/sp3_file_reader.cpp index 212661e73..aa857965a 100644 --- a/src/math_physics/gnss/sp3_file_reader.cpp +++ b/src/math_physics/gnss/sp3_file_reader.cpp @@ -40,7 +40,7 @@ double Sp3FileReader::GetSatelliteClockOffset(const size_t epoch_id, const size_ return position_clock.clock_us_; } -s2e::math::Vector<3> Sp3FileReader::GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id) { +math::Vector<3> Sp3FileReader::GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id) { Sp3PositionClock position_clock = GetPositionClock(epoch_id, satellite_id); return position_clock.position_km_; } @@ -314,7 +314,7 @@ Sp3PositionClock Sp3FileReader::DecodePositionClockData(std::string line) { position_clock.satellite_id_ = line.substr(1, 3); // Position and clock - s2e::math::Vector<3> position_km; + math::Vector<3> position_km; for (size_t axis = 0; axis < 3; axis++) { position_km[axis] = stod(line.substr(4 + axis * 14, 14)); } @@ -323,7 +323,7 @@ Sp3PositionClock Sp3FileReader::DecodePositionClockData(std::string line) { // Standard deviations if (line.size() > 61) { - s2e::math::Vector<3> position_standard_deviation; + math::Vector<3> position_standard_deviation; for (size_t axis = 0; axis < 3; axis++) { try { position_standard_deviation[axis] = stod(line.substr(61 + axis * 3, 2)); @@ -384,7 +384,7 @@ Sp3VelocityClockRate Sp3FileReader::DecodeVelocityClockRateData(std::string line velocity_clock_rate.satellite_id_ = line.substr(1, 3); // Velocity and clock rate - s2e::math::Vector<3> velocity_dm_s; + math::Vector<3> velocity_dm_s; for (size_t axis = 0; axis < 3; axis++) { velocity_dm_s[axis] = stod(line.substr(4 + axis * 14, 14)); } @@ -393,7 +393,7 @@ Sp3VelocityClockRate Sp3FileReader::DecodeVelocityClockRateData(std::string line // Standard deviations if (line.size() > 60) { - s2e::math::Vector<3> velocity_standard_deviation; + math::Vector<3> velocity_standard_deviation; for (size_t axis = 0; axis < 3; axis++) { velocity_standard_deviation[axis] = stod(line.substr(61 + axis * 2, 2)); } diff --git a/src/math_physics/gnss/sp3_file_reader.hpp b/src/math_physics/gnss/sp3_file_reader.hpp index 076ebd4d4..1b7164dbf 100644 --- a/src/math_physics/gnss/sp3_file_reader.hpp +++ b/src/math_physics/gnss/sp3_file_reader.hpp @@ -95,9 +95,9 @@ struct Sp3Header { */ struct Sp3PositionClock { std::string satellite_id_; //!< GNSS satellite ID - s2e::math::Vector<3> position_km_{SP3_BAD_POSITION_VALUE}; //!< Satellite position [km] + math::Vector<3> position_km_{SP3_BAD_POSITION_VALUE}; //!< Satellite position [km] double clock_us_ = SP3_BAD_CLOCK_VALUE; //!< Satellite clock offset [us] - s2e::math::Vector<3> position_standard_deviation_{0.0}; //!< Satellite position standard deviation [-] + math::Vector<3> position_standard_deviation_{0.0}; //!< Satellite position standard deviation [-] double clock_standard_deviation_ = 0.0; //!< Satellite clock offset standard deviation [-] bool clock_event_flag_ = false; //!< true when clock discontinuity is happened bool clock_prediction_flag_ = false; //!< true when clock data is predicted @@ -130,9 +130,9 @@ struct Sp3PositionClockCorrelation { */ struct Sp3VelocityClockRate { std::string satellite_id_; //!< GNSS satellite ID - s2e::math::Vector<3> velocity_dm_s_{0.0}; //!< Satellite velocity [dm/s] + math::Vector<3> velocity_dm_s_{0.0}; //!< Satellite velocity [dm/s] double clock_rate_ = 0.0; //!< Satellite clock offset change rate [-] - s2e::math::Vector<3> velocity_standard_deviation_{0.0}; //!< Satellite position standard deviation [-] + math::Vector<3> velocity_standard_deviation_{0.0}; //!< Satellite position standard deviation [-] double clock_rate_standard_deviation_ = 0.0; //!< Satellite clock offset standard deviation [-] }; @@ -178,7 +178,7 @@ class Sp3FileReader { s2e::time_system::DateTime GetEpochData(const size_t epoch_id) const; Sp3PositionClock GetPositionClock(const size_t epoch_id, const size_t satellite_id); double GetSatelliteClockOffset(const size_t epoch_id, const size_t satellite_id); - s2e::math::Vector<3> GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id); + math::Vector<3> GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id); size_t SearchNearestEpochId(const s2e::time_system::EpochTime time); diff --git a/src/math_physics/gravity/gravity_potential.cpp b/src/math_physics/gravity/gravity_potential.cpp index f2adcaa7e..05f502fa5 100644 --- a/src/math_physics/gravity/gravity_potential.cpp +++ b/src/math_physics/gravity/gravity_potential.cpp @@ -28,8 +28,8 @@ GravityPotential::GravityPotential(const size_t degree, const std::vector GravityPotential::CalcAcceleration_xcxf_m_s2(const s2e::math::Vector<3> &position_xcxf_m) { - s2e::math::Vector<3> acceleration_xcxf_m_s2(0.0); +math::Vector<3> GravityPotential::CalcAcceleration_xcxf_m_s2(const math::Vector<3> &position_xcxf_m) { + math::Vector<3> acceleration_xcxf_m_s2(0.0); if (degree_ <= 0) return acceleration_xcxf_m_s2; // TODO: Consider this assertion is needed xcxf_x_m_ = position_xcxf_m[0]; @@ -95,8 +95,8 @@ s2e::math::Vector<3> GravityPotential::CalcAcceleration_xcxf_m_s2(const s2e::mat return acceleration_xcxf_m_s2; } -s2e::math::Matrix<3, 3> GravityPotential::CalcPartialDerivative_xcxf_s2(const s2e::math::Vector<3> &position_xcxf_m) { - s2e::math::Matrix<3, 3> partial_derivative(0.0); +math::Matrix<3, 3> GravityPotential::CalcPartialDerivative_xcxf_s2(const math::Vector<3> &position_xcxf_m) { + math::Matrix<3, 3> partial_derivative(0.0); if (degree_ <= 0) return partial_derivative; xcxf_x_m_ = position_xcxf_m[0]; diff --git a/src/math_physics/gravity/gravity_potential.hpp b/src/math_physics/gravity/gravity_potential.hpp index 23d6df4c9..bf06ee4d2 100644 --- a/src/math_physics/gravity/gravity_potential.hpp +++ b/src/math_physics/gravity/gravity_potential.hpp @@ -48,7 +48,7 @@ class GravityPotential { * @param [in] position_xcxf_m: Position of the spacecraft in the XCXF frame [m] * @return Acceleration in XCXF frame [m/s2] */ - s2e::math::Vector<3> CalcAcceleration_xcxf_m_s2(const s2e::math::Vector<3> &position_xcxf_m); + math::Vector<3> CalcAcceleration_xcxf_m_s2(const math::Vector<3> &position_xcxf_m); /** * @fn CalcAcceleration_xcxf_m_s2 @@ -56,7 +56,7 @@ class GravityPotential { * @param [in] position_xcxf_m: Position of the spacecraft in the XCXF frame [m] * @return Partial derivative of acceleration in XCXF frame [-/s2] */ - s2e::math::Matrix<3, 3> CalcPartialDerivative_xcxf_s2(const s2e::math::Vector<3> &position_xcxf_m); + math::Matrix<3, 3> CalcPartialDerivative_xcxf_s2(const math::Vector<3> &position_xcxf_m); private: size_t degree_ = 0; //!< Maximum degree diff --git a/src/math_physics/gravity/test_gravity_potential.cpp b/src/math_physics/gravity/test_gravity_potential.cpp index 54717f1c1..0ac2fa038 100644 --- a/src/math_physics/gravity/test_gravity_potential.cpp +++ b/src/math_physics/gravity/test_gravity_potential.cpp @@ -25,8 +25,8 @@ TEST(GravityPotential, Acceleration) { GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); // Acceleration Calculation check - s2e::math::Vector<3> position_xcxf_m; - s2e::math::Vector<3> acceleration_xcxf_m_s2; + math::Vector<3> position_xcxf_m; + math::Vector<3> acceleration_xcxf_m_s2; const double accuracy = 1.0e-3; // Calc Acceleration @@ -67,8 +67,8 @@ TEST(GravityPotential, PartialDerivative1) { GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); // Calculation check - s2e::math::Vector<3> position_xcxf_m; - s2e::math::Matrix<3, 3> partial_derivative_xcxf_s2; + math::Vector<3> position_xcxf_m; + math::Matrix<3, 3> partial_derivative_xcxf_s2; const double accuracy = 1.0e-3; // Calc Partial Derivative @@ -79,22 +79,22 @@ TEST(GravityPotential, PartialDerivative1) { // Calc Acceleration and numerical partial derivatives double d_r = 1e-9; - s2e::math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; + math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; for (size_t i = 0; i < 3; i++) { - s2e::math::Vector<3> position_1_xcxf_m = position_xcxf_m; - s2e::math::Vector<3> position_2_xcxf_m = position_xcxf_m; + math::Vector<3> position_1_xcxf_m = position_xcxf_m; + math::Vector<3> position_2_xcxf_m = position_xcxf_m; position_1_xcxf_m[i] = position_xcxf_m[i] - d_r / 2.0; position_2_xcxf_m[i] = position_xcxf_m[i] + d_r / 2.0; - s2e::math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); - s2e::math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); - s2e::math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; + math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); + math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); + math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; for (size_t j = 0; j < 3; j++) { numerical_partial_derivative_xcxf_s2[i][j] = diff_acceleration_xcxf_m_s2[j] / d_r; } } // Compare numerical and analytical calculation - s2e::math::Matrix<3, 3> diff; + math::Matrix<3, 3> diff; for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { EXPECT_NEAR(numerical_partial_derivative_xcxf_s2[i][j], partial_derivative_xcxf_s2[i][j], accuracy); @@ -120,8 +120,8 @@ TEST(GravityPotential, PartialDerivative2) { GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); // Calculation check - s2e::math::Vector<3> position_xcxf_m; - s2e::math::Matrix<3, 3> partial_derivative_xcxf_s2; + math::Vector<3> position_xcxf_m; + math::Matrix<3, 3> partial_derivative_xcxf_s2; const double accuracy = 1.0e-3; // Calc Partial Derivative @@ -132,22 +132,22 @@ TEST(GravityPotential, PartialDerivative2) { // Calc Acceleration and numerical partial derivatives double d_r = 1e-9; - s2e::math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; + math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; for (size_t i = 0; i < 3; i++) { - s2e::math::Vector<3> position_1_xcxf_m = position_xcxf_m; - s2e::math::Vector<3> position_2_xcxf_m = position_xcxf_m; + math::Vector<3> position_1_xcxf_m = position_xcxf_m; + math::Vector<3> position_2_xcxf_m = position_xcxf_m; position_1_xcxf_m[i] = position_xcxf_m[i] - d_r / 2.0; position_2_xcxf_m[i] = position_xcxf_m[i] + d_r / 2.0; - s2e::math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); - s2e::math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); - s2e::math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; + math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); + math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); + math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; for (size_t j = 0; j < 3; j++) { numerical_partial_derivative_xcxf_s2[i][j] = diff_acceleration_xcxf_m_s2[j] / d_r; } } // Compare numerical and analytical calculation - s2e::math::Matrix<3, 3> diff; + math::Matrix<3, 3> diff; for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { EXPECT_NEAR(numerical_partial_derivative_xcxf_s2[i][j], partial_derivative_xcxf_s2[i][j], accuracy); diff --git a/src/math_physics/math/ordinary_differential_equation.hpp b/src/math_physics/math/ordinary_differential_equation.hpp index fb0d81b36..1c861a391 100644 --- a/src/math_physics/math/ordinary_differential_equation.hpp +++ b/src/math_physics/math/ordinary_differential_equation.hpp @@ -102,7 +102,7 @@ class OrdinaryDifferentialEquation { * @fn GetState * @brief Return current state vector for inheriting class */ - inline s2e::math::Vector& GetState() { return state_; } + inline math::Vector& GetState() { return state_; } private: double independent_variable_; //!< Latest value of independent variable diff --git a/src/math_physics/math/s2e_math.cpp b/src/math_physics/math/s2e_math.cpp index 26020cd77..fb4e53e1e 100644 --- a/src/math_physics/math/s2e_math.cpp +++ b/src/math_physics/math/s2e_math.cpp @@ -12,11 +12,11 @@ double WrapTo2Pi(const double angle_rad) { double angle_out = angle_rad; if (angle_out < 0.0) { while (angle_out < 0.0) { - angle_out += s2e::math::tau; + angle_out += math::tau; } - } else if (angle_out > s2e::math::tau) { - while (angle_out > s2e::math::tau) { - angle_out -= s2e::math::tau; + } else if (angle_out > math::tau) { + while (angle_out > math::tau) { + angle_out -= math::tau; } } else { // nothing to do diff --git a/src/math_physics/math/test_interpolation.cpp b/src/math_physics/math/test_interpolation.cpp index a29f3b330..674b12419 100644 --- a/src/math_physics/math/test_interpolation.cpp +++ b/src/math_physics/math/test_interpolation.cpp @@ -15,7 +15,7 @@ TEST(Interpolation, PolynomialLinearFunction) { std::vector x{0.0, 1.0, 2.0, 3.0, 4.0}; std::vector y{0.0, 2.0, 4.0, 6.0, 8.0}; - s2e::math::Interpolation interpolation(x, y); + math::Interpolation interpolation(x, y); double xx = 0.4; EXPECT_DOUBLE_EQ(2.0 * xx, interpolation.CalcPolynomial(xx)); @@ -31,7 +31,7 @@ TEST(Interpolation, PolynomialLinearFunction) { TEST(Interpolation, PolynomialQuadraticFunction) { std::vector x{0.0, 1.0, 2.0, 3.0, 4.0}; std::vector y{0.0, 1.0, 4.0, 9.0, 16.0}; - s2e::math::Interpolation interpolation(x, y); + math::Interpolation interpolation(x, y); double xx = 0.4; EXPECT_DOUBLE_EQ(pow(xx, 2.0), interpolation.CalcPolynomial(xx)); @@ -45,16 +45,16 @@ TEST(Interpolation, PolynomialQuadraticFunction) { * @brief Test for sin function with trigonometric interpolation */ TEST(Interpolation, TrigonometricSinFunction) { - std::vector x{0.0, s2e::math::pi_2, s2e::math::pi, s2e::math::pi * 3.0 / 2.0, s2e::math::tau}; + std::vector x{0.0, math::pi_2, math::pi, math::pi * 3.0 / 2.0, math::tau}; std::vector y; for (size_t i = 0; i < x.size(); i++) { y.push_back(sin(x[i])); } - s2e::math::Interpolation interpolation(x, y); + math::Interpolation interpolation(x, y); - double xx = 0.4 * s2e::math::pi; + double xx = 0.4 * math::pi; EXPECT_DOUBLE_EQ(sin(xx), interpolation.CalcTrigonometric(xx)); - xx = 1.4 * s2e::math::pi; + xx = 1.4 * math::pi; EXPECT_DOUBLE_EQ(sin(xx), interpolation.CalcTrigonometric(xx)); } @@ -62,16 +62,16 @@ TEST(Interpolation, TrigonometricSinFunction) { * @brief Test for cos function with trigonometric interpolation */ TEST(Interpolation, TrigonometricCosFunction) { - std::vector x{0.0, 0.3 * s2e::math::pi_2, 0.6 * s2e::math::pi_2, 0.9 * s2e::math::pi_2, 1.2 * s2e::math::pi_2}; + std::vector x{0.0, 0.3 * math::pi_2, 0.6 * math::pi_2, 0.9 * math::pi_2, 1.2 * math::pi_2}; std::vector y; for (size_t i = 0; i < x.size(); i++) { y.push_back(cos(x[i])); } - s2e::math::Interpolation interpolation(x, y); + math::Interpolation interpolation(x, y); - double xx = 0.1 * s2e::math::pi_2; + double xx = 0.1 * math::pi_2; EXPECT_NEAR(cos(xx), interpolation.CalcTrigonometric(xx), 1e-6); - xx = 0.8 * s2e::math::pi_2; + xx = 0.8 * math::pi_2; EXPECT_NEAR(cos(xx), interpolation.CalcTrigonometric(xx), 1e-6); } @@ -79,16 +79,16 @@ TEST(Interpolation, TrigonometricCosFunction) { * @brief Test for cos function with trigonometric interpolation */ TEST(Interpolation, TrigonometricCosSinFunctionOddDegree) { - std::vector x{0.0, 0.3 * s2e::math::pi_2, 0.6 * s2e::math::pi_2, 0.9 * s2e::math::pi_2, 1.2 * s2e::math::pi_2, 1.5 * s2e::math::pi_2}; + std::vector x{0.0, 0.3 * math::pi_2, 0.6 * math::pi_2, 0.9 * math::pi_2, 1.2 * math::pi_2, 1.5 * math::pi_2}; std::vector y; for (size_t i = 0; i < x.size(); i++) { y.push_back(cos(x[i]) + sin(x[i])); } - s2e::math::Interpolation interpolation(x, y); + math::Interpolation interpolation(x, y); - double xx = 0.1 * s2e::math::pi_2; + double xx = 0.1 * math::pi_2; EXPECT_NEAR(cos(xx) + sin(xx), interpolation.CalcTrigonometric(xx), 1e-6); - xx = 0.8 * s2e::math::pi_2; + xx = 0.8 * math::pi_2; EXPECT_NEAR(cos(xx) + sin(xx), interpolation.CalcTrigonometric(xx), 1e-6); } @@ -98,7 +98,7 @@ TEST(Interpolation, TrigonometricCosSinFunctionOddDegree) { TEST(Interpolation, PushAndPop) { std::vector x{0.0, 1.0, 2.0, 3.0, 4.0}; std::vector y{0.0, 2.0, 4.0, 6.0, 8.0}; - s2e::math::Interpolation interpolation(x, y); + math::Interpolation interpolation(x, y); EXPECT_EQ(x.size(), interpolation.GetDegree()); for (size_t i = 0; i < x.size(); i++) { diff --git a/src/math_physics/math/test_matrix.cpp b/src/math_physics/math/test_matrix.cpp index 9b23c15d5..53dee0b75 100644 --- a/src/math_physics/math/test_matrix.cpp +++ b/src/math_physics/math/test_matrix.cpp @@ -14,7 +14,7 @@ TEST(Matrix, ConstructorWithNumber) { const size_t R = 6; const size_t C = 3; double initialize_value = 2.0; - s2e::math::Matrix m(initialize_value); + math::Matrix m(initialize_value); for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -30,7 +30,7 @@ TEST(Matrix, ConstructorWithNumber) { TEST(Matrix, GetLength) { const size_t R = 6; const size_t C = 3; - s2e::math::Matrix m; + math::Matrix m; EXPECT_EQ(R, m.GetRowLength()); EXPECT_EQ(C, m.GetColumnLength()); @@ -43,8 +43,8 @@ TEST(Matrix, OperatorPlusEqual) { const size_t R = 6; const size_t C = 3; double initialize_value = 2.0; - s2e::math::Matrix m(initialize_value); - s2e::math::Matrix adding; + math::Matrix m(initialize_value); + math::Matrix adding; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -71,8 +71,8 @@ TEST(Matrix, OperatorMinusEqual) { const size_t R = 6; const size_t C = 3; double initialize_value = 2.0; - s2e::math::Matrix m(initialize_value); - s2e::math::Matrix subtracting; + math::Matrix m(initialize_value); + math::Matrix subtracting; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -99,7 +99,7 @@ TEST(Matrix, OperatorMultiplyEqual) { const size_t R = 6; const size_t C = 3; - s2e::math::Matrix m; + math::Matrix m; double multiplying = 2.0; for (size_t r = 0; r < R; r++) { @@ -125,7 +125,7 @@ TEST(Matrix, OperatorDivideEqual) { const size_t R = 6; const size_t C = 3; - s2e::math::Matrix m; + math::Matrix m; double dividing = 2.0; for (size_t r = 0; r < R; r++) { @@ -152,7 +152,7 @@ TEST(Matrix, FillUp) { const size_t C = 3; double value = 3.0; - s2e::math::Matrix m; + math::Matrix m; m.FillUp(value); @@ -169,7 +169,7 @@ TEST(Matrix, FillUp) { */ TEST(Matrix, CalcTrace) { const size_t N = 6; - s2e::math::Matrix m; + math::Matrix m; for (size_t r = 0; r < N; r++) { for (size_t c = 0; c < N; c++) { @@ -201,8 +201,8 @@ TEST(Matrix, OperatorPlus) { const size_t R = 6; const size_t C = 3; double initialize_value = -2.0; - s2e::math::Matrix m(initialize_value); - s2e::math::Matrix adding; + math::Matrix m(initialize_value); + math::Matrix adding; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -210,7 +210,7 @@ TEST(Matrix, OperatorPlus) { } } - s2e::math::Matrix added = m + adding; + math::Matrix added = m + adding; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -230,8 +230,8 @@ TEST(Matrix, OperatorMinus) { const size_t R = 6; const size_t C = 3; double initialize_value = 0.6; - s2e::math::Matrix m(initialize_value); - s2e::math::Matrix subtracting; + math::Matrix m(initialize_value); + math::Matrix subtracting; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -239,7 +239,7 @@ TEST(Matrix, OperatorMinus) { } } - s2e::math::Matrix subtracted = m - subtracting; + math::Matrix subtracted = m - subtracting; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -259,7 +259,7 @@ TEST(Matrix, OperatorMultiplyScalar) { const size_t R = 6; const size_t C = 3; - s2e::math::Matrix m; + math::Matrix m; double multiplying = 0.3; for (size_t r = 0; r < R; r++) { @@ -268,7 +268,7 @@ TEST(Matrix, OperatorMultiplyScalar) { } } - s2e::math::Matrix subtracted = multiplying * m; + math::Matrix subtracted = multiplying * m; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -287,8 +287,8 @@ TEST(Matrix, OperatorMultiplyMatrix) { const size_t R = 2; const size_t C = 3; - s2e::math::Matrix a; - s2e::math::Matrix b; + math::Matrix a; + math::Matrix b; a[0][0] = 1.0; a[0][1] = 2.0; @@ -304,7 +304,7 @@ TEST(Matrix, OperatorMultiplyMatrix) { b[2][0] = 5.0; b[2][1] = 6.0; - s2e::math::Matrix result = a * b; + math::Matrix result = a * b; EXPECT_DOUBLE_EQ(22.0, result[0][0]); EXPECT_DOUBLE_EQ(28.0, result[0][1]); @@ -319,14 +319,14 @@ TEST(Matrix, Transpose) { const size_t R = 6; const size_t C = 3; - s2e::math::Matrix m; + math::Matrix m; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { m[r][c] = r * c; } } - s2e::math::Matrix transposed = m.Transpose(); + math::Matrix transposed = m.Transpose(); for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -344,7 +344,7 @@ TEST(Matrix, Transpose) { TEST(Matrix, MakeIdentityMatrix) { const size_t N = 6; - s2e::math::Matrix m = s2e::math::MakeIdentityMatrix(); + math::Matrix m = math::MakeIdentityMatrix(); for (size_t r = 0; r < N; r++) { for (size_t c = 0; c < N; c++) { @@ -362,9 +362,9 @@ TEST(Matrix, MakeIdentityMatrix) { */ TEST(Matrix, MakeRotationMatrixX) { const size_t N = 3; - double theta_rad = -45.0 * s2e::math::deg_to_rad; + double theta_rad = -45.0 * math::deg_to_rad; - s2e::math::Matrix m = s2e::math::MakeRotationMatrixX(theta_rad); + math::Matrix m = math::MakeRotationMatrixX(theta_rad); EXPECT_DOUBLE_EQ(1.0, m[0][0]); EXPECT_DOUBLE_EQ(0.0, m[0][1]); @@ -382,9 +382,9 @@ TEST(Matrix, MakeRotationMatrixX) { */ TEST(Matrix, MakeRotationMatrixY) { const size_t N = 3; - double theta_rad = 120.0 * s2e::math::deg_to_rad; + double theta_rad = 120.0 * math::deg_to_rad; - s2e::math::Matrix m = s2e::math::MakeRotationMatrixY(theta_rad); + math::Matrix m = math::MakeRotationMatrixY(theta_rad); EXPECT_DOUBLE_EQ(cos(theta_rad), m[0][0]); EXPECT_DOUBLE_EQ(0.0, m[0][1]); @@ -402,9 +402,9 @@ TEST(Matrix, MakeRotationMatrixY) { */ TEST(Matrix, MakeRotationMatrixZ) { const size_t N = 3; - double theta_rad = 30.0 * s2e::math::deg_to_rad; + double theta_rad = 30.0 * math::deg_to_rad; - s2e::math::Matrix m = s2e::math::MakeRotationMatrixZ(theta_rad); + math::Matrix m = math::MakeRotationMatrixZ(theta_rad); EXPECT_DOUBLE_EQ(cos(theta_rad), m[0][0]); EXPECT_DOUBLE_EQ(sin(theta_rad), m[0][1]); diff --git a/src/math_physics/math/test_matrix_vector.cpp b/src/math_physics/math/test_matrix_vector.cpp index 958a409b6..78fc9babf 100644 --- a/src/math_physics/math/test_matrix_vector.cpp +++ b/src/math_physics/math/test_matrix_vector.cpp @@ -14,8 +14,8 @@ TEST(MatrixVector, MultiplyMatrixVector) { const size_t R = 3; const size_t C = 2; - s2e::math::Matrix m; - s2e::math::Vector v; + math::Matrix m; + math::Vector v; m[0][0] = 1.0; m[0][1] = 2.0; @@ -27,7 +27,7 @@ TEST(MatrixVector, MultiplyMatrixVector) { v[0] = 7.0; v[1] = 1.0; - s2e::math::Vector result = m * v; + math::Vector result = m * v; EXPECT_DOUBLE_EQ(9.0, result[0]); EXPECT_DOUBLE_EQ(-7.0, result[1]); @@ -40,7 +40,7 @@ TEST(MatrixVector, MultiplyMatrixVector) { TEST(MatrixVector, CalcInverseMatrix) { const size_t N = 3; - s2e::math::Matrix m; + math::Matrix m; m[0][0] = 1.0; m[0][1] = 1.0; @@ -52,7 +52,7 @@ TEST(MatrixVector, CalcInverseMatrix) { m[2][1] = -2.0; m[2][2] = 1.0; - s2e::math::Matrix inverse = s2e::math::CalcInverseMatrix(m); + math::Matrix inverse = math::CalcInverseMatrix(m); EXPECT_NEAR(-1.0, inverse[0][0], 1e-10); EXPECT_NEAR(-1.0, inverse[0][1], 1e-10); diff --git a/src/math_physics/math/test_quaternion.cpp b/src/math_physics/math/test_quaternion.cpp index 2821eccee..4995afccc 100644 --- a/src/math_physics/math/test_quaternion.cpp +++ b/src/math_physics/math/test_quaternion.cpp @@ -11,7 +11,7 @@ * @brief Test for constructor from four numbers */ TEST(Quaternion, ConstructorFourNumber) { - s2e::math::Quaternion q(0.5, 0.5, 0.5, 0.5); + math::Quaternion q(0.5, 0.5, 0.5, 0.5); EXPECT_DOUBLE_EQ(0.5, q[0]); EXPECT_DOUBLE_EQ(0.5, q[1]); @@ -23,8 +23,8 @@ TEST(Quaternion, ConstructorFourNumber) { * @brief Test for constructor from Vector */ TEST(Quaternion, ConstructorVector) { - s2e::math::Vector<4> v(0.5); - s2e::math::Quaternion q(v); + math::Vector<4> v(0.5); + math::Quaternion q(v); EXPECT_DOUBLE_EQ(0.5, q[0]); EXPECT_DOUBLE_EQ(0.5, q[1]); @@ -36,12 +36,12 @@ TEST(Quaternion, ConstructorVector) { * @brief Test for constructor from axis and rotation angle X rotation */ TEST(Quaternion, ConstructorAxisAndAngleX) { - s2e::math::Vector<3> axis; + math::Vector<3> axis; axis[0] = 1.0; axis[1] = 0.0; axis[2] = 0.0; - double theta_rad = 90 * s2e::math::deg_to_rad; - s2e::math::Quaternion q(axis, theta_rad); + double theta_rad = 90 * math::deg_to_rad; + math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -53,12 +53,12 @@ TEST(Quaternion, ConstructorAxisAndAngleX) { * @brief Test for constructor from axis and rotation angle Y rotation */ TEST(Quaternion, ConstructorAxisAndAngleY) { - s2e::math::Vector<3> axis; + math::Vector<3> axis; axis[0] = 0.0; axis[1] = 1.0; axis[2] = 0.0; - double theta_rad = 45 * s2e::math::deg_to_rad; - s2e::math::Quaternion q(axis, theta_rad); + double theta_rad = 45 * math::deg_to_rad; + math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -70,12 +70,12 @@ TEST(Quaternion, ConstructorAxisAndAngleY) { * @brief Test for constructor from axis and rotation angle Z rotation */ TEST(Quaternion, ConstructorAxisAndAngleZ) { - s2e::math::Vector<3> axis; + math::Vector<3> axis; axis[0] = 0.0; axis[1] = 0.0; axis[2] = 1.0; - double theta_rad = -60 * s2e::math::deg_to_rad; - s2e::math::Quaternion q(axis, theta_rad); + double theta_rad = -60 * math::deg_to_rad; + math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -87,12 +87,12 @@ TEST(Quaternion, ConstructorAxisAndAngleZ) { * @brief Test for constructor from axis and rotation angle All axes rotation */ TEST(Quaternion, ConstructorAxisAndAngleAll) { - s2e::math::Vector<3> axis; + math::Vector<3> axis; axis[0] = 1.0; axis[1] = 1.0; axis[2] = 1.0; - double theta_rad = 180 * s2e::math::deg_to_rad; - s2e::math::Quaternion q(axis, theta_rad); + double theta_rad = 180 * math::deg_to_rad; + math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -104,16 +104,16 @@ TEST(Quaternion, ConstructorAxisAndAngleAll) { * @brief Test for constructor from two vectors: No rotation */ TEST(Quaternion, ConstructorTwoVectorsNoRotation) { - s2e::math::Vector<3> before; + math::Vector<3> before; before[0] = 0.0; before[1] = 0.0; before[2] = 2.0; // To check normalization - s2e::math::Vector<3> after; + math::Vector<3> after; after[0] = 0.0; after[1] = 0.0; after[2] = 1.0; - s2e::math::Quaternion q(before, after); + math::Quaternion q(before, after); EXPECT_NEAR(0.0, q[0], 1e-5); EXPECT_NEAR(0.0, q[1], 1e-5); @@ -125,18 +125,18 @@ TEST(Quaternion, ConstructorTwoVectorsNoRotation) { * @brief Test for constructor from two vectors: X rotation */ TEST(Quaternion, ConstructorTwoVectorsX) { - s2e::math::Vector<3> before; + math::Vector<3> before; before[0] = 0.0; before[1] = 0.0; before[2] = 1.0; - s2e::math::Vector<3> after; + math::Vector<3> after; after[0] = 0.0; after[1] = 1.0; after[2] = 0.0; - s2e::math::Quaternion q(before, after); + math::Quaternion q(before, after); - double theta_rad = -90 * s2e::math::deg_to_rad; + double theta_rad = -90 * math::deg_to_rad; EXPECT_NEAR(sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(0.0, q[1], 1e-5); EXPECT_NEAR(0.0, q[2], 1e-5); @@ -147,18 +147,18 @@ TEST(Quaternion, ConstructorTwoVectorsX) { * @brief Test for constructor from two vectors: Y rotation */ TEST(Quaternion, ConstructorTwoVectorsY) { - s2e::math::Vector<3> before; + math::Vector<3> before; before[0] = 0.0; before[1] = 0.0; before[2] = 1.0; - s2e::math::Vector<3> after; + math::Vector<3> after; after[0] = 1.0; after[1] = 0.0; after[2] = 0.0; - s2e::math::Quaternion q(before, after); + math::Quaternion q(before, after); - double theta_rad = 90 * s2e::math::deg_to_rad; + double theta_rad = 90 * math::deg_to_rad; EXPECT_NEAR(0.0, q[0], 1e-5); EXPECT_NEAR(sin(theta_rad / 2.0), q[1], 1e-5); EXPECT_NEAR(0.0, q[2], 1e-5); @@ -169,18 +169,18 @@ TEST(Quaternion, ConstructorTwoVectorsY) { * @brief Test for constructor from two vectors: Z rotation */ TEST(Quaternion, ConstructorTwoVectorsZ) { - s2e::math::Vector<3> before; + math::Vector<3> before; before[0] = 1.0; before[1] = 0.0; before[2] = 0.0; - s2e::math::Vector<3> after; + math::Vector<3> after; after[0] = 0.0; after[1] = 1.0; after[2] = 0.0; - s2e::math::Quaternion q(before, after); + math::Quaternion q(before, after); - double theta_rad = 90 * s2e::math::deg_to_rad; + double theta_rad = 90 * math::deg_to_rad; EXPECT_NEAR(0.0, q[0], 1e-5); EXPECT_NEAR(0.0, q[1], 1e-5); EXPECT_NEAR(sin(theta_rad / 2.0), q[2], 1e-5); @@ -192,7 +192,7 @@ TEST(Quaternion, ConstructorTwoVectorsZ) { * @note TODO: Fix to nondestructive function */ TEST(Quaternion, Normalize) { - s2e::math::Quaternion q(1.0, 1.0, 1.0, 1.0); + math::Quaternion q(1.0, 1.0, 1.0, 1.0); EXPECT_DOUBLE_EQ(1.0, q[0]); EXPECT_DOUBLE_EQ(1.0, q[1]); EXPECT_DOUBLE_EQ(1.0, q[2]); @@ -209,9 +209,9 @@ TEST(Quaternion, Normalize) { * @brief Test for Conjugate */ TEST(Quaternion, Conjugate) { - s2e::math::Quaternion q(0.5, 0.5, 0.5, 0.5); + math::Quaternion q(0.5, 0.5, 0.5, 0.5); - s2e::math::Quaternion q_conjugate = q.Conjugate(); + math::Quaternion q_conjugate = q.Conjugate(); // Check nondestructive function EXPECT_DOUBLE_EQ(0.5, q[0]); @@ -229,10 +229,10 @@ TEST(Quaternion, Conjugate) { * @brief Test for ConvertToDcm Y rotation */ TEST(Quaternion, ConvertToDcmY) { - s2e::math::Quaternion q(0.0, 1.0, 0.0, 1.0); + math::Quaternion q(0.0, 1.0, 0.0, 1.0); q.Normalize(); - s2e::math::Matrix<3, 3> dcm = q.ConvertToDcm(); + math::Matrix<3, 3> dcm = q.ConvertToDcm(); // Check nondestructive function EXPECT_DOUBLE_EQ(0.0, q[0]); @@ -253,7 +253,7 @@ TEST(Quaternion, ConvertToDcmY) { EXPECT_NEAR(0.0, dcm[2][2], accuracy); // Inverse Conversion - s2e::math::Quaternion q_from_dcm = s2e::math::Quaternion::ConvertFromDcm(dcm); + math::Quaternion q_from_dcm = math::Quaternion::ConvertFromDcm(dcm); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_dcm[i], accuracy); } @@ -263,10 +263,10 @@ TEST(Quaternion, ConvertToDcmY) { * @brief Test for ConvertToDcm */ TEST(Quaternion, ConvertToDcm) { - s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); + math::Quaternion q(0.5, 0.3, 0.1, 1.0); q.Normalize(); - s2e::math::Matrix<3, 3> dcm = q.ConvertToDcm(); + math::Matrix<3, 3> dcm = q.ConvertToDcm(); // Check nondestructive function const double accuracy = 1.0e-5; @@ -281,7 +281,7 @@ TEST(Quaternion, ConvertToDcm) { EXPECT_NEAR(0.4962963, dcm[2][2], accuracy); // Inverse Conversion - s2e::math::Quaternion q_from_dcm = s2e::math::Quaternion::ConvertFromDcm(dcm); + math::Quaternion q_from_dcm = math::Quaternion::ConvertFromDcm(dcm); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_dcm[i], accuracy); } @@ -291,10 +291,10 @@ TEST(Quaternion, ConvertToDcm) { * @brief Test for ConvertToEuler X rotation */ TEST(Quaternion, ConvertToEulerX) { - s2e::math::Quaternion q(1.0, 0.0, 0.0, 1.0); + math::Quaternion q(1.0, 0.0, 0.0, 1.0); q.Normalize(); - s2e::math::Vector<3> euler = q.ConvertToEuler(); + math::Vector<3> euler = q.ConvertToEuler(); // Check nondestructive function EXPECT_DOUBLE_EQ(1.0 / sqrt(2.0), q[0]); @@ -304,12 +304,12 @@ TEST(Quaternion, ConvertToEulerX) { // Check nondestructive function const double accuracy = 1.0e-7; - EXPECT_NEAR(90 * s2e::math::deg_to_rad, euler[0], accuracy); + EXPECT_NEAR(90 * math::deg_to_rad, euler[0], accuracy); EXPECT_NEAR(0.0, euler[1], accuracy); EXPECT_NEAR(0.0, euler[2], accuracy); // Inverse Conversion - s2e::math::Quaternion q_from_euler = s2e::math::Quaternion::ConvertFromEuler(euler); + math::Quaternion q_from_euler = math::Quaternion::ConvertFromEuler(euler); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_euler[i], accuracy); } @@ -319,10 +319,10 @@ TEST(Quaternion, ConvertToEulerX) { * @brief Test for ConvertToEuler */ TEST(Quaternion, ConvertToEuler) { - s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); + math::Quaternion q(0.5, 0.3, 0.1, 1.0); q.Normalize(); - s2e::math::Vector<3> euler = q.ConvertToEuler(); + math::Vector<3> euler = q.ConvertToEuler(); // Check nondestructive function const double accuracy = 1.0e-7; @@ -331,7 +331,7 @@ TEST(Quaternion, ConvertToEuler) { EXPECT_NEAR(0.41012734, euler[2], accuracy); // Inverse Conversion - s2e::math::Quaternion q_from_euler = s2e::math::Quaternion::ConvertFromEuler(euler); + math::Quaternion q_from_euler = math::Quaternion::ConvertFromEuler(euler); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_euler[i], accuracy); } @@ -341,22 +341,22 @@ TEST(Quaternion, ConvertToEuler) { * @brief Test for FrameConversion Z rotation */ TEST(Quaternion, FrameConversionZ) { - s2e::math::Quaternion q(0.0, 0.0, 1.0, 1.0); + math::Quaternion q(0.0, 0.0, 1.0, 1.0); q.Normalize(); - s2e::math::Vector<3> v; + math::Vector<3> v; v[0] = 1.0; v[1] = 0.0; v[2] = 0.0; - s2e::math::Vector<3> v_frame_conv = q.FrameConversion(v); + math::Vector<3> v_frame_conv = q.FrameConversion(v); const double accuracy = 1.0e-7; EXPECT_NEAR(0.0, v_frame_conv[0], accuracy); EXPECT_NEAR(-1.0, v_frame_conv[1], accuracy); EXPECT_NEAR(0.0, v_frame_conv[2], accuracy); - s2e::math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); + math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); for (size_t i = 0; i < 3; i++) { EXPECT_NEAR(v[i], v_frame_conv_inv[i], accuracy); @@ -367,15 +367,15 @@ TEST(Quaternion, FrameConversionZ) { * @brief Test for FrameConversion */ TEST(Quaternion, FrameConversion) { - s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); + math::Quaternion q(0.5, 0.3, 0.1, 1.0); q.Normalize(); - s2e::math::Vector<3> v; + math::Vector<3> v; v[0] = 1.0; v[1] = 0.0; v[2] = 0.0; - s2e::math::Vector<3> v_frame_conv = q.FrameConversion(v); - s2e::math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); + math::Vector<3> v_frame_conv = q.FrameConversion(v); + math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); const double accuracy = 1.0e-7; for (size_t i = 0; i < 3; i++) { @@ -387,9 +387,9 @@ TEST(Quaternion, FrameConversion) { * @brief Test for ConvertToVector */ TEST(Quaternion, ConvertToVector) { - s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); + math::Quaternion q(0.5, 0.3, 0.1, 1.0); - s2e::math::Vector<4> v = q.ConvertToVector(); + math::Vector<4> v = q.ConvertToVector(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(q[i], v[i]); @@ -400,10 +400,10 @@ TEST(Quaternion, ConvertToVector) { * @brief Test for operator+ */ TEST(Quaternion, OperatorPlus) { - s2e::math::Quaternion q1(0.5, 0.3, 0.1, 1.0); - s2e::math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); + math::Quaternion q1(0.5, 0.3, 0.1, 1.0); + math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); - s2e::math::Quaternion result = q1 + q2; + math::Quaternion result = q1 + q2; EXPECT_DOUBLE_EQ(0.2, result[0]); EXPECT_DOUBLE_EQ(0.4, result[1]); @@ -415,10 +415,10 @@ TEST(Quaternion, OperatorPlus) { * @brief Test for operator- */ TEST(Quaternion, OperatorMinus) { - s2e::math::Quaternion q1(0.5, 0.3, 0.1, 1.0); - s2e::math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); + math::Quaternion q1(0.5, 0.3, 0.1, 1.0); + math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); - s2e::math::Quaternion result = q1 - q2; + math::Quaternion result = q1 - q2; EXPECT_DOUBLE_EQ(0.8, result[0]); EXPECT_DOUBLE_EQ(0.2, result[1]); @@ -430,10 +430,10 @@ TEST(Quaternion, OperatorMinus) { * @brief Test for operator* quaternion */ TEST(Quaternion, OperatorQuaternionMultiply) { - s2e::math::Quaternion q1(0.289271, -0.576012, -0.420972, 0.638212); - s2e::math::Quaternion q2(-0.0821846, 0.501761, 0.721995, -0.469259); + math::Quaternion q1(0.289271, -0.576012, -0.420972, 0.638212); + math::Quaternion q2(-0.0821846, 0.501761, 0.721995, -0.469259); - s2e::math::Quaternion result = q1 * q2; + math::Quaternion result = q1 * q2; const double accuracy = 1.0e-7; EXPECT_NEAR(-0.3928446703722, result[0], accuracy); @@ -446,10 +446,10 @@ TEST(Quaternion, OperatorQuaternionMultiply) { * @brief Test for operator* scalar */ TEST(Quaternion, OperatorScalarMultiply) { - s2e::math::Quaternion q(0.289271, -0.576012, -0.420972, 0.638212); + math::Quaternion q(0.289271, -0.576012, -0.420972, 0.638212); double scalar = 2.3; - s2e::math::Quaternion result = scalar * q; + math::Quaternion result = scalar * q; const double accuracy = 1.0e-7; EXPECT_NEAR(q[0] * 2.3, result[0], accuracy); diff --git a/src/math_physics/math/test_s2e_math.cpp b/src/math_physics/math/test_s2e_math.cpp index 7a293b967..949751a3b 100644 --- a/src/math_physics/math/test_s2e_math.cpp +++ b/src/math_physics/math/test_s2e_math.cpp @@ -14,18 +14,18 @@ TEST(S2eMath, WrapTo2Pi) { const double accuracy = 1.0e-7; double input_angle_rad = 0.0; - double wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); + double wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); EXPECT_NEAR(0.0, wrapped_angle_rad, accuracy); input_angle_rad = -1.0e-5; - wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); - EXPECT_NEAR(s2e::math::tau + input_angle_rad, wrapped_angle_rad, accuracy); + wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); + EXPECT_NEAR(math::tau + input_angle_rad, wrapped_angle_rad, accuracy); - input_angle_rad = s2e::math::tau + 1.0e-5; - wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); + input_angle_rad = math::tau + 1.0e-5; + wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); EXPECT_NEAR(1.0e-5, wrapped_angle_rad, accuracy); - input_angle_rad = 10 * s2e::math::tau + 1.0e-5; - wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); + input_angle_rad = 10 * math::tau + 1.0e-5; + wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); EXPECT_NEAR(1.0e-5, wrapped_angle_rad, accuracy); } diff --git a/src/math_physics/math/test_vector.cpp b/src/math_physics/math/test_vector.cpp index e920dc0b8..5f636960d 100644 --- a/src/math_physics/math/test_vector.cpp +++ b/src/math_physics/math/test_vector.cpp @@ -13,7 +13,7 @@ TEST(Vector, ConstructorWithNumber) { const size_t N = 6; double initialize_value = 2.0; - s2e::math::Vector v(initialize_value); + math::Vector v(initialize_value); for (size_t i = 0; i < N; i++) { EXPECT_DOUBLE_EQ(initialize_value, v[i]); @@ -26,7 +26,7 @@ TEST(Vector, ConstructorWithNumber) { */ TEST(Vector, GetLength) { const size_t N = 6; - s2e::math::Vector v; + math::Vector v; EXPECT_EQ(N, v.GetLength()); } @@ -37,8 +37,8 @@ TEST(Vector, GetLength) { TEST(Vector, OperatorPlusEqual) { const size_t N = 6; double initialize_value = 2.0; - s2e::math::Vector v(initialize_value); - s2e::math::Vector adding; + math::Vector v(initialize_value); + math::Vector adding; for (size_t i = 0; i < N; i++) { adding[i] = double(i); @@ -60,8 +60,8 @@ TEST(Vector, OperatorPlusEqual) { TEST(Vector, OperatorMinusEqual) { const size_t N = 6; double initialize_value = 2.0; - s2e::math::Vector v(initialize_value); - s2e::math::Vector subtracting; + math::Vector v(initialize_value); + math::Vector subtracting; for (size_t i = 0; i < N; i++) { subtracting[i] = double(i); @@ -82,7 +82,7 @@ TEST(Vector, OperatorMinusEqual) { */ TEST(Vector, OperatorMultiplyEqual) { const size_t N = 6; - s2e::math::Vector v; + math::Vector v; double multiplying = 2.0; for (size_t i = 0; i < N; i++) { @@ -104,7 +104,7 @@ TEST(Vector, OperatorMultiplyEqual) { */ TEST(Vector, OperatorDivideEqual) { const size_t N = 6; - s2e::math::Vector v; + math::Vector v; double dividing = 3.0; for (size_t i = 0; i < N; i++) { @@ -126,13 +126,13 @@ TEST(Vector, OperatorDivideEqual) { */ TEST(Vector, OperatorNegative) { const size_t N = 6; - s2e::math::Vector v; + math::Vector v; for (size_t i = 0; i < N; i++) { v[i] = double(i); } - s2e::math::Vector v_negative = -v; + math::Vector v_negative = -v; for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -147,7 +147,7 @@ TEST(Vector, OperatorNegative) { */ TEST(Vector, FillUp) { const size_t N = 6; - s2e::math::Vector v; + math::Vector v; for (size_t i = 0; i < N; i++) { v[i] = double(i); @@ -171,14 +171,14 @@ TEST(Vector, FillUp) { TEST(Vector, OperatorPlus) { const size_t N = 6; double initialize_value = 2.0; - s2e::math::Vector v(initialize_value); - s2e::math::Vector adding; + math::Vector v(initialize_value); + math::Vector adding; for (size_t i = 0; i < N; i++) { adding[i] = double(i); } - s2e::math::Vector added = v + adding; + math::Vector added = v + adding; for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -195,14 +195,14 @@ TEST(Vector, OperatorPlus) { TEST(Vector, OperatorMinus) { const size_t N = 6; double initialize_value = 2.0; - s2e::math::Vector v(initialize_value); - s2e::math::Vector subtracting; + math::Vector v(initialize_value); + math::Vector subtracting; for (size_t i = 0; i < N; i++) { subtracting[i] = double(i); } - s2e::math::Vector subtracted = v - subtracting; + math::Vector subtracted = v - subtracting; for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -218,8 +218,8 @@ TEST(Vector, OperatorMinus) { */ TEST(Vector, InnerProduct) { const size_t N = 3; - s2e::math::Vector a; - s2e::math::Vector b; + math::Vector a; + math::Vector b; for (size_t i = 0; i < N; i++) { a[i] = double(i + 1); @@ -235,8 +235,8 @@ TEST(Vector, InnerProduct) { */ TEST(Vector, InnerProductZero) { const size_t N = 3; - s2e::math::Vector a; - s2e::math::Vector b; + math::Vector a; + math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -255,8 +255,8 @@ TEST(Vector, InnerProductZero) { */ TEST(Vector, OuterProductZero) { const size_t N = 3; - s2e::math::Vector a; - s2e::math::Vector b; + math::Vector a; + math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -266,7 +266,7 @@ TEST(Vector, OuterProductZero) { b[1] = 0.0; b[2] = 0.0; - s2e::math::Vector<3> result = OuterProduct(a, b); + math::Vector<3> result = OuterProduct(a, b); for (size_t i = 0; i < N; i++) { EXPECT_DOUBLE_EQ(0.0, result[i]); @@ -278,8 +278,8 @@ TEST(Vector, OuterProductZero) { */ TEST(Vector, OuterProductX) { const size_t N = 3; - s2e::math::Vector a; - s2e::math::Vector b; + math::Vector a; + math::Vector b; a[0] = 0.0; a[1] = 0.0; @@ -289,7 +289,7 @@ TEST(Vector, OuterProductX) { b[1] = 1.0; b[2] = 0.0; - s2e::math::Vector<3> result = OuterProduct(a, b); + math::Vector<3> result = OuterProduct(a, b); EXPECT_DOUBLE_EQ(-1.0, result[0]); EXPECT_DOUBLE_EQ(0.0, result[1]); @@ -301,8 +301,8 @@ TEST(Vector, OuterProductX) { */ TEST(Vector, OuterProductY) { const size_t N = 3; - s2e::math::Vector a; - s2e::math::Vector b; + math::Vector a; + math::Vector b; a[0] = 0.0; a[1] = 0.0; @@ -312,7 +312,7 @@ TEST(Vector, OuterProductY) { b[1] = 0.0; b[2] = 0.0; - s2e::math::Vector<3> result = OuterProduct(a, b); + math::Vector<3> result = OuterProduct(a, b); EXPECT_DOUBLE_EQ(0.0, result[0]); EXPECT_DOUBLE_EQ(1.0, result[1]); @@ -324,8 +324,8 @@ TEST(Vector, OuterProductY) { */ TEST(Vector, OuterProductZ) { const size_t N = 3; - s2e::math::Vector a; - s2e::math::Vector b; + math::Vector a; + math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -335,7 +335,7 @@ TEST(Vector, OuterProductZ) { b[1] = 1.0; b[2] = 0.0; - s2e::math::Vector<3> result = OuterProduct(a, b); + math::Vector<3> result = OuterProduct(a, b); EXPECT_DOUBLE_EQ(0.0, result[0]); EXPECT_DOUBLE_EQ(0.0, result[1]); @@ -347,7 +347,7 @@ TEST(Vector, OuterProductZ) { */ TEST(Vector, CalcNorm) { const size_t N = 10; - s2e::math::Vector v(1.0); + math::Vector v(1.0); double norm = v.CalcNorm(); @@ -364,9 +364,9 @@ TEST(Vector, CalcNorm) { */ TEST(Vector, Normalize) { const size_t N = 5; - s2e::math::Vector v(1.0); + math::Vector v(1.0); - s2e::math::Vector normalized = v.CalcNormalizedVector(); + math::Vector normalized = v.CalcNormalizedVector(); for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -381,8 +381,8 @@ TEST(Vector, Normalize) { */ TEST(Vector, CalcAngleTwoVectors90deg) { const size_t N = 3; - s2e::math::Vector a; - s2e::math::Vector b; + math::Vector a; + math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -394,7 +394,7 @@ TEST(Vector, CalcAngleTwoVectors90deg) { double angle_rad = CalcAngleTwoVectors_rad(a, b); - EXPECT_DOUBLE_EQ(90.0 * s2e::math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(90.0 * math::deg_to_rad, angle_rad); } /** @@ -402,7 +402,7 @@ TEST(Vector, CalcAngleTwoVectors90deg) { */ TEST(Vector, CalcAngleTwoVectors0deg) { const size_t N = 3; - s2e::math::Vector a; + math::Vector a; a[0] = 1.0; a[1] = 0.0; @@ -410,7 +410,7 @@ TEST(Vector, CalcAngleTwoVectors0deg) { double angle_rad = CalcAngleTwoVectors_rad(a, a); - EXPECT_DOUBLE_EQ(0.0 * s2e::math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(0.0 * math::deg_to_rad, angle_rad); } /** @@ -418,8 +418,8 @@ TEST(Vector, CalcAngleTwoVectors0deg) { */ TEST(Vector, CalcAngleTwoVectors45deg) { const size_t N = 3; - s2e::math::Vector a; - s2e::math::Vector b; + math::Vector a; + math::Vector b; a[0] = 0.0; a[1] = 1.0; @@ -431,7 +431,7 @@ TEST(Vector, CalcAngleTwoVectors45deg) { double angle_rad = CalcAngleTwoVectors_rad(a, b); - EXPECT_DOUBLE_EQ(45.0 * s2e::math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(45.0 * math::deg_to_rad, angle_rad); } /** @@ -439,11 +439,11 @@ TEST(Vector, CalcAngleTwoVectors45deg) { */ TEST(Vector, GenerateOrthogonalUnitVector) { const size_t N = 3; - s2e::math::Vector a(1.0); + math::Vector a(1.0); - s2e::math::Vector b = GenerateOrthogonalUnitVector(a); + math::Vector b = GenerateOrthogonalUnitVector(a); double angle_rad = CalcAngleTwoVectors_rad(a, b); - EXPECT_DOUBLE_EQ(90.0 * s2e::math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(90.0 * math::deg_to_rad, angle_rad); } diff --git a/src/math_physics/numerical_integration/dormand_prince_5.hpp b/src/math_physics/numerical_integration/dormand_prince_5.hpp index 90e5fb413..0e5ff40e6 100644 --- a/src/math_physics/numerical_integration/dormand_prince_5.hpp +++ b/src/math_physics/numerical_integration/dormand_prince_5.hpp @@ -33,10 +33,10 @@ class DormandPrince5 : public EmbeddedRungeKutta { * @param [in] sigma: Sigma value (0 < sigma < 1) for interpolation * @return : interpolated state x(t0 + sigma * h) */ - s2e::math::Vector CalcInterpolationState(const double sigma) const override; + math::Vector CalcInterpolationState(const double sigma) const override; private: - std::vector> coefficients_; //!< Coefficients to calculate interpolation weights + std::vector> coefficients_; //!< Coefficients to calculate interpolation weights /** * @fn CalcInterpolationWeights * @brief Calculate weights for interpolation diff --git a/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp b/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp index 3dc53411c..8a5bbb55e 100644 --- a/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp +++ b/src/math_physics/numerical_integration/dormand_prince_5_implementation.hpp @@ -75,7 +75,7 @@ DormandPrince5::DormandPrince5(const double step_width, const InterfaceOde this->rk_matrix_[6][5] = 11.0 / 84.0; // Interpolation coefficients - s2e::math::Vector<5> coefficients_temp; + math::Vector<5> coefficients_temp; coefficients_temp[0] = 11282082432.0; coefficients_temp[1] = -32272833064.0; coefficients_temp[2] = 34969693132.0; @@ -123,10 +123,10 @@ DormandPrince5::DormandPrince5(const double step_width, const InterfaceOde } template -s2e::math::Vector DormandPrince5::CalcInterpolationState(const double sigma) const { +math::Vector DormandPrince5::CalcInterpolationState(const double sigma) const { std::vector interpolation_weights = CalcInterpolationWeights(sigma); - s2e::math::Vector interpolation_state = this->previous_state_; + math::Vector interpolation_state = this->previous_state_; for (size_t i = 0; i < this->number_of_stages_; i++) { interpolation_state = interpolation_state + (sigma * this->step_width_ * interpolation_weights[i]) * this->slope_[i]; } diff --git a/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp b/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp index 1a73d0775..f4e794a0f 100644 --- a/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp +++ b/src/math_physics/numerical_integration/embedded_runge_kutta_implementation.hpp @@ -14,15 +14,15 @@ void EmbeddedRungeKutta::Integrate() { this->CalcSlope(); this->previous_state_ = this->current_state_; - s2e::math::Vector lower_current_state = this->current_state_; //!< eta in the equation - s2e::math::Vector higher_current_state = this->current_state_; //!< eta_hat in the equation + math::Vector lower_current_state = this->current_state_; //!< eta in the equation + math::Vector higher_current_state = this->current_state_; //!< eta_hat in the equation for (size_t i = 0; i < this->number_of_stages_; i++) { lower_current_state = lower_current_state + this->weights_[i] * this->step_width_ * this->slope_[i]; higher_current_state = higher_current_state + higher_order_weights_[i] * this->step_width_ * this->slope_[i]; } // Error evaluation - s2e::math::Vector truncation_error = lower_current_state - higher_current_state; + math::Vector truncation_error = lower_current_state - higher_current_state; local_truncation_error_ = truncation_error.CalcNorm(); // State update diff --git a/src/math_physics/numerical_integration/interface_ode.hpp b/src/math_physics/numerical_integration/interface_ode.hpp index a050119b9..fada6e5b0 100644 --- a/src/math_physics/numerical_integration/interface_ode.hpp +++ b/src/math_physics/numerical_integration/interface_ode.hpp @@ -24,7 +24,7 @@ class InterfaceOde { * @param [in] state: State vector * @return Differentiated value of state vector */ - virtual s2e::math::Vector DerivativeFunction(const double independent_variable, const s2e::math::Vector& state) const = 0; + virtual math::Vector DerivativeFunction(const double independent_variable, const math::Vector& state) const = 0; }; } // namespace s2e::numerical_integration diff --git a/src/math_physics/numerical_integration/numerical_integrator.hpp b/src/math_physics/numerical_integration/numerical_integrator.hpp index e125ec810..461bfa225 100644 --- a/src/math_physics/numerical_integration/numerical_integrator.hpp +++ b/src/math_physics/numerical_integration/numerical_integrator.hpp @@ -44,7 +44,7 @@ class NumericalIntegrator { * @fn SetState * @brief Set state information */ - inline void SetState(const double independent_variable, const s2e::math::Vector& state) { + inline void SetState(const double independent_variable, const math::Vector& state) { current_independent_variable_ = independent_variable; current_state_ = state; previous_state_ = state; @@ -54,7 +54,7 @@ class NumericalIntegrator { * @fn GetState * @brief Return current state vector */ - inline const s2e::math::Vector& GetState() const { return current_state_; } + inline const math::Vector& GetState() const { return current_state_; } /** * @fn CalcInterpolationState @@ -62,7 +62,7 @@ class NumericalIntegrator { * @param [in] sigma: Sigma value (0 < sigma < 1) for interpolation * @return : interpolated state x(t0 + sigma * h) */ - virtual s2e::math::Vector CalcInterpolationState(const double sigma) const = 0; + virtual math::Vector CalcInterpolationState(const double sigma) const = 0; protected: // Settings @@ -71,8 +71,8 @@ class NumericalIntegrator { // States const InterfaceOde& ode_; //!< Ordinary differential equation double current_independent_variable_; //!< Latest value of independent variable - s2e::math::Vector current_state_; //!< Latest state vector - s2e::math::Vector previous_state_; //!< Previous state vector + math::Vector current_state_; //!< Latest state vector + math::Vector previous_state_; //!< Previous state vector }; } // namespace s2e::numerical_integration diff --git a/src/math_physics/numerical_integration/ode_examples.hpp b/src/math_physics/numerical_integration/ode_examples.hpp index f248f3d3b..98ee43d63 100644 --- a/src/math_physics/numerical_integration/ode_examples.hpp +++ b/src/math_physics/numerical_integration/ode_examples.hpp @@ -13,11 +13,11 @@ namespace s2e::numerical_integration { class ExampleLinearOde : public InterfaceOde<1> { public: - s2e::math::Vector<1> DerivativeFunction(const double time_s, const s2e::math::Vector<1>& state) const { + math::Vector<1> DerivativeFunction(const double time_s, const math::Vector<1>& state) const { UNUSED(time_s); UNUSED(state); - s2e::math::Vector<1> output(1.0); + math::Vector<1> output(1.0); return output; } }; @@ -35,11 +35,11 @@ class ExampleQuadraticOde : public InterfaceOde<1> { * @param [in] state: State vector * @return Differentiated value of state vector */ - virtual s2e::math::Vector<1> DerivativeFunction(const double time_s, const s2e::math::Vector<1>& state) const { + virtual math::Vector<1> DerivativeFunction(const double time_s, const math::Vector<1>& state) const { UNUSED(time_s); UNUSED(state); - s2e::math::Vector<1> output(0.0); + math::Vector<1> output(0.0); output[0] = 2.0 * time_s; return output; } @@ -51,10 +51,10 @@ class ExampleQuadraticOde : public InterfaceOde<1> { */ class Example1dPositionVelocityOde : public InterfaceOde<2> { public: - virtual s2e::math::Vector<2> DerivativeFunction(const double time_s, const s2e::math::Vector<2>& state) const { + virtual math::Vector<2> DerivativeFunction(const double time_s, const math::Vector<2>& state) const { UNUSED(time_s); - s2e::math::Vector<2> output(0.0); + math::Vector<2> output(0.0); output[0] = state[1]; output[1] = 0.0; return output; @@ -67,10 +67,10 @@ class Example1dPositionVelocityOde : public InterfaceOde<2> { */ class Example2dTwoBodyOrbitOde : public InterfaceOde<4> { public: - virtual s2e::math::Vector<4> DerivativeFunction(const double time_s, const s2e::math::Vector<4>& state) const { + virtual math::Vector<4> DerivativeFunction(const double time_s, const math::Vector<4>& state) const { UNUSED(time_s); - s2e::math::Vector<4> output(0.0); + math::Vector<4> output(0.0); output[0] = state[2]; output[1] = state[3]; double denominator = pow(state[0] * state[0] + state[1] * state[1], 3.0 / 2.0); diff --git a/src/math_physics/numerical_integration/runge_kutta.hpp b/src/math_physics/numerical_integration/runge_kutta.hpp index 2bb9a2505..d2072d4cb 100644 --- a/src/math_physics/numerical_integration/runge_kutta.hpp +++ b/src/math_physics/numerical_integration/runge_kutta.hpp @@ -46,7 +46,7 @@ class RungeKutta : public NumericalIntegrator { std::vector nodes_; //!< Nodes vector for general RK (c vector in the equation) std::vector weights_; //!< Weights vector for general RK (b vector in the equation) std::vector> rk_matrix_; //!< Runge-Kutta matrix for general RK (a matrix in the equation) - std::vector> slope_; //!< Slope vector for general RK (k vector in the equation) + std::vector> slope_; //!< Slope vector for general RK (k vector in the equation) /** * @fn CalcSlope diff --git a/src/math_physics/numerical_integration/runge_kutta_4.hpp b/src/math_physics/numerical_integration/runge_kutta_4.hpp index d52106039..d8925f51c 100644 --- a/src/math_physics/numerical_integration/runge_kutta_4.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_4.hpp @@ -45,7 +45,7 @@ class RungeKutta4 : public RungeKutta { } // We did not implement the interpolation for RK4 - s2e::math::Vector CalcInterpolationState(const double sigma) const override { + math::Vector CalcInterpolationState(const double sigma) const override { UNUSED(sigma); return this->current_state_; } diff --git a/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp b/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp index e22c969eb..11a4a51ff 100644 --- a/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_fehlberg.hpp @@ -29,7 +29,7 @@ class RungeKuttaFehlberg : public EmbeddedRungeKutta { * @param [in] sigma: Sigma value (0 < sigma < 1) for interpolation * @return : interpolated state x(t0 + sigma * h) */ - s2e::math::Vector CalcInterpolationState(const double sigma) const override; + math::Vector CalcInterpolationState(const double sigma) const override; private: /** diff --git a/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp b/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp index 5ccfe3941..c9887034c 100644 --- a/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_fehlberg_implementation.hpp @@ -64,15 +64,15 @@ RungeKuttaFehlberg::RungeKuttaFehlberg(const double step_width, const Interfa } template -s2e::math::Vector RungeKuttaFehlberg::CalcInterpolationState(const double sigma) const { +math::Vector RungeKuttaFehlberg::CalcInterpolationState(const double sigma) const { // Calc k7 (slope after state update) - s2e::math::Vector state_7 = + math::Vector state_7 = this->previous_state_ + this->step_width_ * (1.0 / 6.0 * this->slope_[0] + 1.0 / 6.0 * this->slope_[4] + 2.0 / 3.0 * this->slope_[5]); - s2e::math::Vector k7 = this->ode_.DerivativeFunction(this->current_independent_variable_, state_7); + math::Vector k7 = this->ode_.DerivativeFunction(this->current_independent_variable_, state_7); std::vector interpolation_weights = CalcInterpolationWeights(sigma); - s2e::math::Vector interpolation_state = this->previous_state_; + math::Vector interpolation_state = this->previous_state_; for (size_t i = 0; i < this->number_of_stages_; i++) { interpolation_state = interpolation_state + (sigma * this->step_width_ * interpolation_weights[i]) * this->slope_[i]; } diff --git a/src/math_physics/numerical_integration/runge_kutta_template.hpp b/src/math_physics/numerical_integration/runge_kutta_template.hpp index 296fe302f..0a34a7485 100644 --- a/src/math_physics/numerical_integration/runge_kutta_template.hpp +++ b/src/math_physics/numerical_integration/runge_kutta_template.hpp @@ -22,10 +22,10 @@ void RungeKutta::Integrate() { template void RungeKutta::CalcSlope() { - slope_.assign(number_of_stages_, s2e::math::Vector(0.0)); + slope_.assign(number_of_stages_, math::Vector(0.0)); for (size_t i = 0; i < number_of_stages_; i++) { - s2e::math::Vector state = this->current_state_; + math::Vector state = this->current_state_; for (size_t j = 0; j < i; j++) { state = state + rk_matrix_[i][j] * this->step_width_ * slope_[j]; } diff --git a/src/math_physics/numerical_integration/test_runge_kutta.cpp b/src/math_physics/numerical_integration/test_runge_kutta.cpp index b2571ba5c..2223a9a9b 100644 --- a/src/math_physics/numerical_integration/test_runge_kutta.cpp +++ b/src/math_physics/numerical_integration/test_runge_kutta.cpp @@ -18,7 +18,7 @@ TEST(NUMERICAL_INTEGRATION, Constructor) { s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::RungeKutta4<1> linear_ode(0.1, ode); - s2e::math::Vector<1> state = linear_ode.GetState(); + math::Vector<1> state = linear_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); } @@ -30,7 +30,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearRk4) { s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::RungeKutta4<1> rk4_ode(step_width_s, ode); - s2e::math::Vector<1> state = rk4_ode.GetState(); + math::Vector<1> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -51,7 +51,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearRkf) { s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); - s2e::math::Vector<1> state = rkf_ode.GetState(); + math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -72,7 +72,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearDp5) { s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); - s2e::math::Vector<1> state = dp5_ode.GetState(); + math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -93,7 +93,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRk4) { s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode); - s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); + math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -115,7 +115,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRkf) { s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, s2e::numerical_integration::NumericalIntegrationMethod::kRkf); - s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); + math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -141,7 +141,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerDp5) { s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, s2e::numerical_integration::NumericalIntegrationMethod::kDp5); - s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); + math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -166,7 +166,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticRk4) { s2e::numerical_integration::ExampleQuadraticOde ode; s2e::numerical_integration::RungeKutta4<1> rk4_ode(step_width_s, ode); - s2e::math::Vector<1> state = rk4_ode.GetState(); + math::Vector<1> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -187,7 +187,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticRkf) { s2e::numerical_integration::ExampleQuadraticOde ode; s2e::numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); - s2e::math::Vector<1> state = rkf_ode.GetState(); + math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -208,7 +208,7 @@ TEST(NUMERICAL_INTEGRATION, InterpolationQuadraticRkf) { s2e::numerical_integration::ExampleQuadraticOde ode; s2e::numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); - s2e::math::Vector<1> state = rkf_ode.GetState(); + math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); rkf_ode.Integrate(); @@ -237,7 +237,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticDp5) { s2e::numerical_integration::ExampleQuadraticOde ode; s2e::numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); - s2e::math::Vector<1> state = dp5_ode.GetState(); + math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -258,7 +258,7 @@ TEST(NUMERICAL_INTEGRATION, InterpolationQuadraticDp5) { s2e::numerical_integration::ExampleQuadraticOde ode; s2e::numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); - s2e::math::Vector<1> state = dp5_ode.GetState(); + math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); dp5_ode.Integrate(); @@ -287,12 +287,12 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRk4) { s2e::numerical_integration::Example1dPositionVelocityOde ode; s2e::numerical_integration::RungeKutta4<2> rk4_ode(step_width_s, ode); - s2e::math::Vector<2> initial_state(0.0); + math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; initial_state[1] = 0.1; rk4_ode.SetState(0.0, initial_state); - s2e::math::Vector<2> state = rk4_ode.GetState(); + math::Vector<2> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(initial_state[0], state[0]); EXPECT_DOUBLE_EQ(initial_state[1], state[1]); @@ -301,7 +301,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRk4) { rk4_ode.Integrate(); } state = rk4_ode.GetState(); - s2e::math::Vector<2> estimated_result(0.0); + math::Vector<2> estimated_result(0.0); estimated_result[0] = (step_width_s * step_num) * initial_state[1] + initial_state[0]; estimated_result[1] = initial_state[1]; @@ -317,12 +317,12 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRkf) { s2e::numerical_integration::Example1dPositionVelocityOde ode; s2e::numerical_integration::RungeKuttaFehlberg<2> rkf_ode(step_width_s, ode); - s2e::math::Vector<2> initial_state(0.0); + math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; initial_state[1] = 0.1; rkf_ode.SetState(0.0, initial_state); - s2e::math::Vector<2> state = rkf_ode.GetState(); + math::Vector<2> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(initial_state[0], state[0]); EXPECT_DOUBLE_EQ(initial_state[1], state[1]); @@ -331,7 +331,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRkf) { rkf_ode.Integrate(); } state = rkf_ode.GetState(); - s2e::math::Vector<2> estimated_result(0.0); + math::Vector<2> estimated_result(0.0); estimated_result[0] = (step_width_s * step_num) * initial_state[1] + initial_state[0]; estimated_result[1] = initial_state[1]; @@ -347,12 +347,12 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityDp5) { s2e::numerical_integration::Example1dPositionVelocityOde ode; s2e::numerical_integration::DormandPrince5<2> dp5_ode(step_width_s, ode); - s2e::math::Vector<2> initial_state(0.0); + math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; initial_state[1] = 0.1; dp5_ode.SetState(0.0, initial_state); - s2e::math::Vector<2> state = dp5_ode.GetState(); + math::Vector<2> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(initial_state[0], state[0]); EXPECT_DOUBLE_EQ(initial_state[1], state[1]); @@ -361,7 +361,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityDp5) { dp5_ode.Integrate(); } state = dp5_ode.GetState(); - s2e::math::Vector<2> estimated_result(0.0); + math::Vector<2> estimated_result(0.0); estimated_result[0] = (step_width_s * step_num) * initial_state[1] + initial_state[0]; estimated_result[1] = initial_state[1]; @@ -379,7 +379,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - s2e::math::Vector<4> initial_state(0.0); + math::Vector<4> initial_state(0.0); const double eccentricity = 0.1; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -389,9 +389,9 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - s2e::math::Vector<4> state_rk4 = rk4_ode.GetState(); - s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); - s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); + math::Vector<4> state_rk4 = rk4_ode.GetState(); + math::Vector<4> state_rkf = rkf_ode.GetState(); + math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rk4[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); @@ -409,8 +409,8 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - s2e::math::Vector<3> initial_position(0.0); - s2e::math::Vector<3> initial_velocity(0.0); + math::Vector<3> initial_position(0.0); + math::Vector<3> initial_velocity(0.0); initial_position[0] = initial_state[0]; initial_position[1] = initial_state[1]; @@ -449,7 +449,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - s2e::math::Vector<4> initial_state(0.0); + math::Vector<4> initial_state(0.0); const double eccentricity = 0.9; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -459,9 +459,9 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - s2e::math::Vector<4> state_rk4 = rk4_ode.GetState(); - s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); - s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); + math::Vector<4> state_rk4 = rk4_ode.GetState(); + math::Vector<4> state_rkf = rkf_ode.GetState(); + math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rk4[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); @@ -479,8 +479,8 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - s2e::math::Vector<3> initial_position(0.0); - s2e::math::Vector<3> initial_velocity(0.0); + math::Vector<3> initial_position(0.0); + math::Vector<3> initial_velocity(0.0); initial_position[0] = initial_state[0]; initial_position[1] = initial_state[1]; @@ -518,7 +518,7 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - s2e::math::Vector<4> initial_state(0.0); + math::Vector<4> initial_state(0.0); const double eccentricity = 0.1; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -527,8 +527,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); - s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); + math::Vector<4> state_rkf = rkf_ode.GetState(); + math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_dp5[i]); @@ -543,8 +543,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - s2e::math::Vector<3> initial_position(0.0); - s2e::math::Vector<3> initial_velocity(0.0); + math::Vector<3> initial_position(0.0); + math::Vector<3> initial_velocity(0.0); // Final value initial_position[0] = initial_state[0]; @@ -613,7 +613,7 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - s2e::math::Vector<4> initial_state(0.0); + math::Vector<4> initial_state(0.0); const double eccentricity = 0.8; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -622,8 +622,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); - s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); + math::Vector<4> state_rkf = rkf_ode.GetState(); + math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_dp5[i]); @@ -638,8 +638,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - s2e::math::Vector<3> initial_position(0.0); - s2e::math::Vector<3> initial_velocity(0.0); + math::Vector<3> initial_position(0.0); + math::Vector<3> initial_velocity(0.0); // Final value initial_position[0] = initial_state[0]; diff --git a/src/math_physics/optics/gaussian_beam_base.cpp b/src/math_physics/optics/gaussian_beam_base.cpp index 0cfc5e40b..10b9745c8 100644 --- a/src/math_physics/optics/gaussian_beam_base.cpp +++ b/src/math_physics/optics/gaussian_beam_base.cpp @@ -30,19 +30,19 @@ void GaussianBeamBase::SetTotalPower_W(const double total_power_W) { total_power_W_ = total_power_W; } -void GaussianBeamBase::SetPointingVector_i(const s2e::math::Vector<3> pointing_vector_i) { pointing_vector_i_ = pointing_vector_i; } +void GaussianBeamBase::SetPointingVector_i(const math::Vector<3> pointing_vector_i) { pointing_vector_i_ = pointing_vector_i; } -void GaussianBeamBase::SetBeamWaistPosition_i_m(const s2e::math::Vector<3> position_beam_waist_i_m) { position_beam_waist_i_m_ = position_beam_waist_i_m; } +void GaussianBeamBase::SetBeamWaistPosition_i_m(const math::Vector<3> position_beam_waist_i_m) { position_beam_waist_i_m_ = position_beam_waist_i_m; } double GaussianBeamBase::CalcBeamWidthRadius_m(double distance_from_beam_waist_m) { - double rayleigh_length_m = s2e::math::pi * radius_beam_waist_m_ * radius_beam_waist_m_ / wavelength_m_; + double rayleigh_length_m = math::pi * radius_beam_waist_m_ * radius_beam_waist_m_ / wavelength_m_; double beam_width_radius_m = radius_beam_waist_m_ * sqrt(1.0 + std::pow((distance_from_beam_waist_m / rayleigh_length_m), 2.0)); return beam_width_radius_m; } double GaussianBeamBase::CalcIntensity_W_m2(double distance_from_beam_waist_m, double deviation_from_optical_axis_m) { double beam_width_radius_m = CalcBeamWidthRadius_m(distance_from_beam_waist_m); - double peak_intensity_W_m2 = (2.0 * total_power_W_) / (s2e::math::pi * beam_width_radius_m * beam_width_radius_m); + double peak_intensity_W_m2 = (2.0 * total_power_W_) / (math::pi * beam_width_radius_m * beam_width_radius_m); double gaussian_dist = std::exp((-2.0 * deviation_from_optical_axis_m * deviation_from_optical_axis_m) / (beam_width_radius_m * beam_width_radius_m)); double intensity_W_m2 = peak_intensity_W_m2 * gaussian_dist; diff --git a/src/math_physics/optics/gaussian_beam_base.hpp b/src/math_physics/optics/gaussian_beam_base.hpp index f9fd71432..cf79b46e3 100644 --- a/src/math_physics/optics/gaussian_beam_base.hpp +++ b/src/math_physics/optics/gaussian_beam_base.hpp @@ -50,12 +50,12 @@ class GaussianBeamBase { * @fn SetPointingVector_i * @brief Set pointing direction vector in the inertial frame */ - void SetPointingVector_i(const s2e::math::Vector<3> pointing_vector_i); + void SetPointingVector_i(const math::Vector<3> pointing_vector_i); /** * @fn SetBeamWaistPosition_i_m * @brief Set position of beam waist in the inertial frame [m] (Not used?) */ - void SetBeamWaistPosition_i_m(const s2e::math::Vector<3> position_beam_waist_i_m); + void SetBeamWaistPosition_i_m(const math::Vector<3> position_beam_waist_i_m); // Getter /** @@ -77,12 +77,12 @@ class GaussianBeamBase { * @fn GetPointingVector_i * @brief Return pointing direction vector in the inertial frame */ - inline const s2e::math::Vector<3> GetPointingVector_i() const { return pointing_vector_i_; } + inline const math::Vector<3> GetPointingVector_i() const { return pointing_vector_i_; } /** * @fn GetBeamWaistPosition_i_m * @brief Return position of beam waist in the inertial frame [m] (Not used?) */ - inline const s2e::math::Vector<3> GetBeamWaistPosition_i_m() const { return position_beam_waist_i_m_; } + inline const math::Vector<3> GetBeamWaistPosition_i_m() const { return position_beam_waist_i_m_; } // Calculate functions /** @@ -105,8 +105,8 @@ class GaussianBeamBase { double wavelength_m_; //!< Wavelength [m] double radius_beam_waist_m_; //!< Radius of beam waist [m] double total_power_W_; //!< Total power [W] - s2e::math::Vector<3> pointing_vector_i_{0.0}; //!< Pointing direction vector in the inertial frame - s2e::math::Vector<3> position_beam_waist_i_m_{0.0}; //!< Position of beam waist in the inertial frame [m] (Not used?) + math::Vector<3> pointing_vector_i_{0.0}; //!< Pointing direction vector in the inertial frame + math::Vector<3> position_beam_waist_i_m_{0.0}; //!< Position of beam waist in the inertial frame [m] (Not used?) }; } // namespace s2e::optics diff --git a/src/math_physics/orbit/interpolation_orbit.cpp b/src/math_physics/orbit/interpolation_orbit.cpp index db373629a..8f35bcbae 100644 --- a/src/math_physics/orbit/interpolation_orbit.cpp +++ b/src/math_physics/orbit/interpolation_orbit.cpp @@ -12,13 +12,13 @@ InterpolationOrbit::InterpolationOrbit(const size_t degree) { time.assign(degree, -1.0); std::vector position; position.assign(degree, 0.0); - s2e::math::Interpolation temp(time, position); + math::Interpolation temp(time, position); for (size_t axis = 0; axis < 3; axis++) { interpolation_position_.push_back(temp); } } -bool InterpolationOrbit::PushAndPopData(const double time, const s2e::math::Vector<3> position) { +bool InterpolationOrbit::PushAndPopData(const double time, const math::Vector<3> position) { bool result; for (size_t axis = 0; axis < 3; axis++) { result = interpolation_position_[axis].PushAndPopData(time, position[axis]); @@ -29,8 +29,8 @@ bool InterpolationOrbit::PushAndPopData(const double time, const s2e::math::Vect return true; } -s2e::math::Vector<3> InterpolationOrbit::CalcPositionWithTrigonometric(const double time, const double period) const { - s2e::math::Vector<3> output_position; +math::Vector<3> InterpolationOrbit::CalcPositionWithTrigonometric(const double time, const double period) const { + math::Vector<3> output_position; for (size_t axis = 0; axis < 3; axis++) { output_position[axis] = interpolation_position_[axis].CalcTrigonometric(time, period); } diff --git a/src/math_physics/orbit/interpolation_orbit.hpp b/src/math_physics/orbit/interpolation_orbit.hpp index 1443ec959..77719eac7 100644 --- a/src/math_physics/orbit/interpolation_orbit.hpp +++ b/src/math_physics/orbit/interpolation_orbit.hpp @@ -32,7 +32,7 @@ class InterpolationOrbit { * @param [in] time: time of the new data * @param [in] position: Satellite position of the new data */ - bool PushAndPopData(const double time, const s2e::math::Vector<3> position); + bool PushAndPopData(const double time, const math::Vector<3> position); /** * @fn CalcPositionWithTrigonometric @@ -41,7 +41,7 @@ class InterpolationOrbit { * @param [in] period: Characteristic period * @return Calculated position */ - s2e::math::Vector<3> CalcPositionWithTrigonometric(const double time, const double period = 0.0) const; + math::Vector<3> CalcPositionWithTrigonometric(const double time, const double period = 0.0) const; // Getters /** @@ -68,7 +68,7 @@ class InterpolationOrbit { } private: - std::vector interpolation_position_; // 3D vector of interpolation + std::vector interpolation_position_; // 3D vector of interpolation }; } // namespace s2e::orbit diff --git a/src/math_physics/orbit/kepler_orbit.cpp b/src/math_physics/orbit/kepler_orbit.cpp index 1f7630ef2..2f49fd5db 100644 --- a/src/math_physics/orbit/kepler_orbit.cpp +++ b/src/math_physics/orbit/kepler_orbit.cpp @@ -24,10 +24,10 @@ void KeplerOrbit::CalcConstKeplerMotion() { mean_motion_rad_s_ = sqrt(gravity_constant_m3_s2_ / a_m3); // DCM - s2e::math::Matrix<3, 3> dcm_arg_perigee = s2e::math::MakeRotationMatrixZ(-1.0 * oe_.GetArgPerigee_rad()); - s2e::math::Matrix<3, 3> dcm_inclination = s2e::math::MakeRotationMatrixX(-1.0 * oe_.GetInclination_rad()); - s2e::math::Matrix<3, 3> dcm_raan = s2e::math::MakeRotationMatrixZ(-1.0 * oe_.GetRaan_rad()); - s2e::math::Matrix<3, 3> dcm_inc_arg = dcm_inclination * dcm_arg_perigee; + math::Matrix<3, 3> dcm_arg_perigee = math::MakeRotationMatrixZ(-1.0 * oe_.GetArgPerigee_rad()); + math::Matrix<3, 3> dcm_inclination = math::MakeRotationMatrixX(-1.0 * oe_.GetInclination_rad()); + math::Matrix<3, 3> dcm_raan = math::MakeRotationMatrixZ(-1.0 * oe_.GetRaan_rad()); + math::Matrix<3, 3> dcm_inc_arg = dcm_inclination * dcm_arg_perigee; dcm_inplane_to_i_ = dcm_raan * dcm_inc_arg; } @@ -39,12 +39,12 @@ void KeplerOrbit::CalcOrbit(double time_jday) { double dt_s = (time_jday - oe_.GetEpoch_jday()) * (24.0 * 60.0 * 60.0); double mean_anomaly_rad = mean_motion_rad_s_ * dt_s; - double l_rad = s2e::math::WrapTo2Pi(mean_anomaly_rad); + double l_rad = math::WrapTo2Pi(mean_anomaly_rad); // Solve Kepler Equation double eccentric_anomaly_rad; eccentric_anomaly_rad = SolveKeplerNewtonMethod(e, l_rad, 1.0e-5, 10); - double u_rad = s2e::math::WrapTo2Pi(eccentric_anomaly_rad); + double u_rad = math::WrapTo2Pi(eccentric_anomaly_rad); // Calc position and velocity in the plane double cos_u = cos(u_rad); @@ -52,12 +52,12 @@ void KeplerOrbit::CalcOrbit(double time_jday) { double a_sqrt_e_m = a_m * sqrt(1.0 - e * e); double e_cos_u = 1.0 - e * cos_u; - s2e::math::Vector<3> pos_inplane_m; + math::Vector<3> pos_inplane_m; pos_inplane_m[0] = a_m * (cos_u - e); pos_inplane_m[1] = a_sqrt_e_m * sin_u; pos_inplane_m[2] = 0.0; - s2e::math::Vector<3> vel_inplane_m_s; + math::Vector<3> vel_inplane_m_s; vel_inplane_m_s[0] = -1.0 * a_m * n_rad_s * sin_u / e_cos_u; vel_inplane_m_s[1] = n_rad_s * a_sqrt_e_m * cos_u / e_cos_u; vel_inplane_m_s[2] = 0.0; diff --git a/src/math_physics/orbit/kepler_orbit.hpp b/src/math_physics/orbit/kepler_orbit.hpp index 7a12ca760..efe4e7f5c 100644 --- a/src/math_physics/orbit/kepler_orbit.hpp +++ b/src/math_physics/orbit/kepler_orbit.hpp @@ -47,22 +47,22 @@ class KeplerOrbit { * @fn GetPosition_i_m * @brief Return position vector in the inertial frame [m] */ - inline const s2e::math::Vector<3> GetPosition_i_m() const { return position_i_m_; } + inline const math::Vector<3> GetPosition_i_m() const { return position_i_m_; } /** * @fn GetVelocity_i_m_s * @brief Return velocity vector in the inertial frame [m/s] */ - inline const s2e::math::Vector<3> GetVelocity_i_m_s() const { return velocity_i_m_s_; } + inline const math::Vector<3> GetVelocity_i_m_s() const { return velocity_i_m_s_; } protected: - s2e::math::Vector<3> position_i_m_; //!< Position vector in the inertial frame [m] - s2e::math::Vector<3> velocity_i_m_s_; //!< Velocity vector in the inertial frame [m/s] + math::Vector<3> position_i_m_; //!< Position vector in the inertial frame [m] + math::Vector<3> velocity_i_m_s_; //!< Velocity vector in the inertial frame [m/s] private: double gravity_constant_m3_s2_; //!< Gravity constant of the center body [m3/s2] OrbitalElements oe_; //!< Orbital elements double mean_motion_rad_s_; //!< Mean motion of the orbit [rad/s] - s2e::math::Matrix<3, 3> dcm_inplane_to_i_; //!< Direction cosine matrix from the in-plane frame to the inertial frame + math::Matrix<3, 3> dcm_inplane_to_i_; //!< Direction cosine matrix from the in-plane frame to the inertial frame /** * @fn CalcConstKeplerMotion diff --git a/src/math_physics/orbit/orbital_elements.cpp b/src/math_physics/orbit/orbital_elements.cpp index 69817f80f..d02a324e2 100644 --- a/src/math_physics/orbit/orbital_elements.cpp +++ b/src/math_physics/orbit/orbital_elements.cpp @@ -24,20 +24,20 @@ OrbitalElements::OrbitalElements(const double epoch_jday, const double semi_majo epoch_jday_(epoch_jday) {} // initialize with position and velocity -OrbitalElements::OrbitalElements(const double gravity_constant_m3_s2, const double time_jday, const s2e::math::Vector<3> position_i_m, - const s2e::math::Vector<3> velocity_i_m_s) { +OrbitalElements::OrbitalElements(const double gravity_constant_m3_s2, const double time_jday, const math::Vector<3> position_i_m, + const math::Vector<3> velocity_i_m_s) { CalcOeFromPosVel(gravity_constant_m3_s2, time_jday, position_i_m, velocity_i_m_s); } OrbitalElements::~OrbitalElements() {} // Private Function -void OrbitalElements::CalcOeFromPosVel(const double gravity_constant_m3_s2, const double time_jday, const s2e::math::Vector<3> position_i_m, - const s2e::math::Vector<3> velocity_i_m_s) { +void OrbitalElements::CalcOeFromPosVel(const double gravity_constant_m3_s2, const double time_jday, const math::Vector<3> position_i_m, + const math::Vector<3> velocity_i_m_s) { // common variables double r_m = position_i_m.CalcNorm(); double v2_m2_s2 = InnerProduct(velocity_i_m_s, velocity_i_m_s); - s2e::math::Vector<3> h; //!< angular momentum vector + math::Vector<3> h; //!< angular momentum vector h = OuterProduct(position_i_m, velocity_i_m_s); double h_norm = h.CalcNorm(); @@ -45,7 +45,7 @@ void OrbitalElements::CalcOeFromPosVel(const double gravity_constant_m3_s2, cons semi_major_axis_m_ = gravity_constant_m3_s2 / (2.0 * gravity_constant_m3_s2 / r_m - v2_m2_s2); // inclination - s2e::math::Vector<3> h_direction = h.CalcNormalizedVector(); + math::Vector<3> h_direction = h.CalcNormalizedVector(); inclination_rad_ = acos(h_direction[2]); // RAAN @@ -77,10 +77,10 @@ void OrbitalElements::CalcOeFromPosVel(const double gravity_constant_m3_s2, cons // true anomaly f_rad and eccentric anomaly u_rad double phi_rad = atan2(y_p_m, x_p_m); double f_rad = phi_rad - arg_perigee_rad_; - f_rad = s2e::math::WrapTo2Pi(f_rad); + f_rad = math::WrapTo2Pi(f_rad); double u_rad = atan2(r_m * sin(f_rad) / sqrt(1.0 - eccentricity_ * eccentricity_), r_m * cos(f_rad) + semi_major_axis_m_ * eccentricity_); - u_rad = s2e::math::WrapTo2Pi(u_rad); + u_rad = math::WrapTo2Pi(u_rad); // epoch t0 double n_rad_s = sqrt(gravity_constant_m3_s2 / pow(semi_major_axis_m_, 3.0)); diff --git a/src/math_physics/orbit/orbital_elements.hpp b/src/math_physics/orbit/orbital_elements.hpp index b177fda41..4b4e3f04f 100644 --- a/src/math_physics/orbit/orbital_elements.hpp +++ b/src/math_physics/orbit/orbital_elements.hpp @@ -41,8 +41,8 @@ class OrbitalElements { * @param[in] position_i_m: Position vector in the inertial frame [m] * @param[in] velocity_i_m_s: Velocity vector in the inertial frame [m/s] */ - OrbitalElements(const double gravity_constant_m3_s2, const double time_jday, const s2e::math::Vector<3> position_i_m, - const s2e::math::Vector<3> velocity_i_m_s); + OrbitalElements(const double gravity_constant_m3_s2, const double time_jday, const math::Vector<3> position_i_m, + const math::Vector<3> velocity_i_m_s); /** * @fn ~OrbitalElements * @brief Destructor @@ -102,8 +102,8 @@ class OrbitalElements { * @param[in] position_i_m: Position vector in the inertial frame [m] * @param[in] velocity_i_m_s: Velocity vector in the inertial frame [m/s] */ - void CalcOeFromPosVel(const double gravity_constant_m3_s2, const double time_jday, const s2e::math::Vector<3> position_i_m, - const s2e::math::Vector<3> velocity_i_m_s); + void CalcOeFromPosVel(const double gravity_constant_m3_s2, const double time_jday, const math::Vector<3> position_i_m, + const math::Vector<3> velocity_i_m_s); }; } // namespace s2e::orbit diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 0c6e358b6..0ec2ebfff 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -6,8 +6,8 @@ namespace s2e::orbit { -s2e::math::Matrix<6, 6> CalcHillSystemMatrix(double orbit_radius_m, double gravity_constant_m3_s2) { - s2e::math::Matrix<6, 6> system_matrix; +math::Matrix<6, 6> CalcHillSystemMatrix(double orbit_radius_m, double gravity_constant_m3_s2) { + math::Matrix<6, 6> system_matrix; double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); system_matrix[0][0] = 0.0; @@ -50,8 +50,8 @@ s2e::math::Matrix<6, 6> CalcHillSystemMatrix(double orbit_radius_m, double gravi return system_matrix; } -s2e::math::Matrix<6, 6> CalcHcwStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s) { - s2e::math::Matrix<6, 6> stm; +math::Matrix<6, 6> CalcHcwStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s) { + math::Matrix<6, 6> stm; double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double t = elapsed_time_s; diff --git a/src/math_physics/orbit/relative_orbit_models.hpp b/src/math_physics/orbit/relative_orbit_models.hpp index 1a12bf634..eb5c42a45 100644 --- a/src/math_physics/orbit/relative_orbit_models.hpp +++ b/src/math_physics/orbit/relative_orbit_models.hpp @@ -31,7 +31,7 @@ enum class StmModel { kHcw = 0 }; * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] * @return System matrix */ -s2e::math::Matrix<6, 6> CalcHillSystemMatrix(const double orbit_radius_m, const double gravity_constant_m3_s2); +math::Matrix<6, 6> CalcHillSystemMatrix(const double orbit_radius_m, const double gravity_constant_m3_s2); // STMs /** @@ -42,7 +42,7 @@ s2e::math::Matrix<6, 6> CalcHillSystemMatrix(const double orbit_radius_m, const * @param [in] elapsed_time_s: Elapsed time [s] * @return State Transition Matrix */ -s2e::math::Matrix<6, 6> CalcHcwStm(const double orbit_radius_m, const double gravity_constant_m3_s2, const double elapsed_time_s); +math::Matrix<6, 6> CalcHcwStm(const double orbit_radius_m, const double gravity_constant_m3_s2, const double elapsed_time_s); } // namespace s2e::orbit diff --git a/src/math_physics/orbit/test_interpolation_orbit.cpp b/src/math_physics/orbit/test_interpolation_orbit.cpp index f83a3f0cf..1e05a13fb 100644 --- a/src/math_physics/orbit/test_interpolation_orbit.cpp +++ b/src/math_physics/orbit/test_interpolation_orbit.cpp @@ -33,7 +33,7 @@ TEST(InterpolationOrbit, PushAndPop) { EXPECT_EQ(degree, interpolation_orbit.GetDegree()); for (size_t i = 0; i < degree; i++) { double time = (double)i; - s2e::math::Vector<3> position{i * 2.0}; + math::Vector<3> position{i * 2.0}; bool ret = interpolation_orbit.PushAndPopData(time, position); EXPECT_TRUE(ret); } @@ -46,7 +46,7 @@ TEST(InterpolationOrbit, PushAndPop) { // False test double time = 2.0; - s2e::math::Vector<3> position{-100.0}; + math::Vector<3> position{-100.0}; bool ret = interpolation_orbit.PushAndPopData(time, position); EXPECT_FALSE(ret); } diff --git a/src/math_physics/planet_rotation/moon_rotation_utilities.cpp b/src/math_physics/planet_rotation/moon_rotation_utilities.cpp index f2d30c96c..370e5ba63 100644 --- a/src/math_physics/planet_rotation/moon_rotation_utilities.cpp +++ b/src/math_physics/planet_rotation/moon_rotation_utilities.cpp @@ -12,22 +12,22 @@ namespace s2e::planet_rotation { -s2e::math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const s2e::math::Vector<3> moon_position_eci_m, const s2e::math::Vector<3> moon_velocity_eci_m_s) { - s2e::math::Matrix<3, 3> dcm_eci2me = CalcDcmEciToMeanEarth(moon_position_eci_m, moon_velocity_eci_m_s); - s2e::math::Matrix<3, 3> dcm_me2pa = CalcDcmMeanEarthToPrincipalAxis(); +math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const math::Vector<3> moon_position_eci_m, const math::Vector<3> moon_velocity_eci_m_s) { + math::Matrix<3, 3> dcm_eci2me = CalcDcmEciToMeanEarth(moon_position_eci_m, moon_velocity_eci_m_s); + math::Matrix<3, 3> dcm_me2pa = CalcDcmMeanEarthToPrincipalAxis(); return dcm_me2pa * dcm_eci2me; } -s2e::math::Matrix<3, 3> CalcDcmEciToMeanEarth(const s2e::math::Vector<3> moon_position_eci_m, const s2e::math::Vector<3> moon_velocity_eci_m_s) { - s2e::math::Vector<3> me_ex_eci = -1.0 * moon_position_eci_m.CalcNormalizedVector(); +math::Matrix<3, 3> CalcDcmEciToMeanEarth(const math::Vector<3> moon_position_eci_m, const math::Vector<3> moon_velocity_eci_m_s) { + math::Vector<3> me_ex_eci = -1.0 * moon_position_eci_m.CalcNormalizedVector(); - s2e::math::Vector<3> moon_orbit_norm = s2e::math::OuterProduct(moon_position_eci_m, moon_velocity_eci_m_s); - s2e::math::Vector<3> me_ez_eci = moon_orbit_norm.CalcNormalizedVector(); + math::Vector<3> moon_orbit_norm = math::OuterProduct(moon_position_eci_m, moon_velocity_eci_m_s); + math::Vector<3> me_ez_eci = moon_orbit_norm.CalcNormalizedVector(); - s2e::math::Vector<3> me_ey_eci = s2e::math::OuterProduct(me_ez_eci, me_ex_eci); + math::Vector<3> me_ey_eci = math::OuterProduct(me_ez_eci, me_ex_eci); - s2e::math::Matrix<3, 3> dcm_eci_to_me; + math::Matrix<3, 3> dcm_eci_to_me; for (size_t i = 0; i < 3; i++) { dcm_eci_to_me[0][i] = me_ex_eci[i]; dcm_eci_to_me[1][i] = me_ey_eci[i]; @@ -37,14 +37,14 @@ s2e::math::Matrix<3, 3> CalcDcmEciToMeanEarth(const s2e::math::Vector<3> moon_po return dcm_eci_to_me; } -s2e::math::Matrix<3, 3> CalcDcmMeanEarthToPrincipalAxis() { +math::Matrix<3, 3> CalcDcmMeanEarthToPrincipalAxis() { // The correction values between DE430 Principal Axis and Mean Earth frame - const double theta_x_rad = 0.285 * s2e::math::arcsec_to_rad; - const double theta_y_rad = 78.580 * s2e::math::arcsec_to_rad; - const double theta_z_rad = 67.573 * s2e::math::arcsec_to_rad; + const double theta_x_rad = 0.285 * math::arcsec_to_rad; + const double theta_y_rad = 78.580 * math::arcsec_to_rad; + const double theta_z_rad = 67.573 * math::arcsec_to_rad; - s2e::math::Matrix<3, 3> dcm_me_pa = - s2e::math::MakeRotationMatrixZ(theta_z_rad) * s2e::math::MakeRotationMatrixY(theta_y_rad) * s2e::math::MakeRotationMatrixX(theta_x_rad); + math::Matrix<3, 3> dcm_me_pa = + math::MakeRotationMatrixZ(theta_z_rad) * math::MakeRotationMatrixY(theta_y_rad) * math::MakeRotationMatrixX(theta_x_rad); return dcm_me_pa; } diff --git a/src/math_physics/planet_rotation/moon_rotation_utilities.hpp b/src/math_physics/planet_rotation/moon_rotation_utilities.hpp index d7e3eff30..b46dbd7e1 100644 --- a/src/math_physics/planet_rotation/moon_rotation_utilities.hpp +++ b/src/math_physics/planet_rotation/moon_rotation_utilities.hpp @@ -20,7 +20,7 @@ namespace s2e::planet_rotation { * @param[in] moon_position_eci_m: Moon position vector @ ECI frame [m] * @param[in] moon_velocity_eci_m_s: Moon velocity vector @ ECI frame [m/s] */ -s2e::math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const s2e::math::Vector<3> moon_position_eci_m, const s2e::math::Vector<3> moon_velocity_eci_m_s); +math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const math::Vector<3> moon_position_eci_m, const math::Vector<3> moon_velocity_eci_m_s); /** * @fn CalcDcmEciToMeanEarth @@ -28,13 +28,13 @@ s2e::math::Matrix<3, 3> CalcDcmEciToPrincipalAxis(const s2e::math::Vector<3> moo * @param[in] moon_position_eci_m: Moon position vector @ ECI frame [m] * @param[in] moon_velocity_eci_m_s: Moon velocity vector @ ECI frame [m/s] */ -s2e::math::Matrix<3, 3> CalcDcmEciToMeanEarth(const s2e::math::Vector<3> moon_position_eci_m, const s2e::math::Vector<3> moon_velocity_eci_m_s); +math::Matrix<3, 3> CalcDcmEciToMeanEarth(const math::Vector<3> moon_position_eci_m, const math::Vector<3> moon_velocity_eci_m_s); /** * @fn CalcDcmMeToPrincipalAxis * @brief Calculate DCM from ME (Mean Earth) moon fixed frame to PA (Principal Axis) moon fixed frame */ -s2e::math::Matrix<3, 3> CalcDcmMeanEarthToPrincipalAxis(); +math::Matrix<3, 3> CalcDcmMeanEarthToPrincipalAxis(); } // namespace s2e::planet_rotation diff --git a/src/math_physics/randomization/random_walk.hpp b/src/math_physics/randomization/random_walk.hpp index 9f05aff9f..ee7db5bb6 100644 --- a/src/math_physics/randomization/random_walk.hpp +++ b/src/math_physics/randomization/random_walk.hpp @@ -15,7 +15,7 @@ * @brief Class to calculate random wark value */ template -class RandomWalk : public s2e::math::OrdinaryDifferentialEquation { +class RandomWalk : public math::OrdinaryDifferentialEquation { public: /** * @fn RandomWalk @@ -24,7 +24,7 @@ class RandomWalk : public s2e::math::OrdinaryDifferentialEquation { * @param standard_deviation: Standard deviation of random walk excitation noise * @param limit: Limit of random walk */ - RandomWalk(double step_width_s, const s2e::math::Vector& standard_deviation, const s2e::math::Vector& limit); + RandomWalk(double step_width_s, const math::Vector& standard_deviation, const math::Vector& limit); /** * @fn DerivativeFunction @@ -33,10 +33,10 @@ class RandomWalk : public s2e::math::OrdinaryDifferentialEquation { * @param [in] state: State vector * @param [out] rhs: Differentiated value of state vector */ - virtual void DerivativeFunction(double x, const s2e::math::Vector& state, s2e::math::Vector& rhs); + virtual void DerivativeFunction(double x, const math::Vector& state, math::Vector& rhs); private: - s2e::math::Vector limit_; //!< Limit of random walk + math::Vector limit_; //!< Limit of random walk s2e::randomization::NormalRand normal_randomizer_[N]; //!< Random walk excitation noise }; diff --git a/src/math_physics/randomization/random_walk_template_functions.hpp b/src/math_physics/randomization/random_walk_template_functions.hpp index 4c4a77ca6..68fa37eea 100644 --- a/src/math_physics/randomization/random_walk_template_functions.hpp +++ b/src/math_physics/randomization/random_walk_template_functions.hpp @@ -10,8 +10,8 @@ #include template -RandomWalk::RandomWalk(double step_width_s, const s2e::math::Vector& standard_deviation, const s2e::math::Vector& limit) - : s2e::math::OrdinaryDifferentialEquation(step_width_s), limit_(limit) { +RandomWalk::RandomWalk(double step_width_s, const math::Vector& standard_deviation, const math::Vector& limit) + : math::OrdinaryDifferentialEquation(step_width_s), limit_(limit) { // Set standard deviation for (size_t i = 0; i < N; ++i) { normal_randomizer_[i].SetParameters(0.0, standard_deviation[i], s2e::randomization::global_randomization.MakeSeed()); @@ -19,7 +19,7 @@ RandomWalk::RandomWalk(double step_width_s, const s2e::math::Vector& stand } template -void RandomWalk::DerivativeFunction(double x, const s2e::math::Vector& state, s2e::math::Vector& rhs) { +void RandomWalk::DerivativeFunction(double x, const math::Vector& state, math::Vector& rhs) { UNUSED(x); // TODO: consider the x is really need for this function for (size_t i = 0; i < N; ++i) { diff --git a/src/setting_file_reader/c2a_command_database.cpp b/src/setting_file_reader/c2a_command_database.cpp index 72e8bdf18..76572c1ab 100644 --- a/src/setting_file_reader/c2a_command_database.cpp +++ b/src/setting_file_reader/c2a_command_database.cpp @@ -95,61 +95,61 @@ void DecodeC2aCommandArgument(const C2aArgumentType type, const std::string argu case C2aArgumentType::kUint8t: { size_param = 1; uint8_t argument = (uint8_t)std::stoul(argument_string); // TODO: 範囲外処理 - endian_memcpy(param, &argument, size_param); + utilities::endian_memcpy(param, &argument, size_param); break; } case C2aArgumentType::kUint16t: { size_param = 2; uint16_t argument = (uint16_t)std::stoul(argument_string); // TODO: 範囲外処理 - endian_memcpy(param, &argument, size_param); + utilities::endian_memcpy(param, &argument, size_param); break; } case C2aArgumentType::kUint32t: { size_param = 4; uint32_t argument = (uint32_t)std::stoul(argument_string); // TODO: 範囲外処理 - endian_memcpy(param, &argument, size_param); + utilities::endian_memcpy(param, &argument, size_param); break; } case C2aArgumentType::kUint64t: { size_param = 8; uint64_t argument = std::stoul(argument_string); - endian_memcpy(param, &argument, size_param); + utilities::endian_memcpy(param, &argument, size_param); break; } case C2aArgumentType::kInt8t: { size_param = 1; int8_t argument = (int8_t)std::stoi(argument_string); // TODO: 範囲外処理 - endian_memcpy(param, &argument, size_param); + utilities::endian_memcpy(param, &argument, size_param); break; } case C2aArgumentType::kInt16t: { size_param = 2; int16_t argument = (int16_t)std::stoi(argument_string); // TODO: 範囲外処理 - endian_memcpy(param, &argument, size_param); + utilities::endian_memcpy(param, &argument, size_param); break; } case C2aArgumentType::kInt32t: { size_param = 4; int32_t argument = std::stoi(argument_string); - endian_memcpy(param, &argument, size_param); + utilities::endian_memcpy(param, &argument, size_param); break; } case C2aArgumentType::kInt64t: { size_param = 8; int64_t argument = std::stol(argument_string); - endian_memcpy(param, &argument, size_param); + utilities::endian_memcpy(param, &argument, size_param); break; } case C2aArgumentType::kFloat: { size_param = 4; float argument = std::stof(argument_string); - endian_memcpy(param, &argument, size_param); + utilities::endian_memcpy(param, &argument, size_param); break; } case C2aArgumentType::kDouble: { size_param = 8; double argument = std::stod(argument_string); - endian_memcpy(param, &argument, size_param); + utilities::endian_memcpy(param, &argument, size_param); break; } case C2aArgumentType::kRaw: { diff --git a/src/setting_file_reader/initialize_file_access.cpp b/src/setting_file_reader/initialize_file_access.cpp index 9397057ad..4ca4c0769 100644 --- a/src/setting_file_reader/initialize_file_access.cpp +++ b/src/setting_file_reader/initialize_file_access.cpp @@ -120,8 +120,8 @@ std::vector IniAccess::ReadVectorDouble(const char* section_name, const return data; } -void IniAccess::ReadQuaternion(const char* section_name, const char* key_name, s2e::math::Quaternion& data) { - s2e::math::Quaternion temp; +void IniAccess::ReadQuaternion(const char* section_name, const char* key_name, math::Quaternion& data) { + math::Quaternion temp; double norm = 0.0; for (int i = 0; i < 4; i++) { // Read Quaternion as new format diff --git a/src/setting_file_reader/initialize_file_access.hpp b/src/setting_file_reader/initialize_file_access.hpp index 48ff1acd1..9af1a3a8a 100644 --- a/src/setting_file_reader/initialize_file_access.hpp +++ b/src/setting_file_reader/initialize_file_access.hpp @@ -112,7 +112,7 @@ class IniAccess { * @param[out] data: Read vector type data */ template - void ReadVector(const char* section_name, const char* key_name, s2e::math::Vector& data); + void ReadVector(const char* section_name, const char* key_name, math::Vector& data); /** * @fn ReadStrVector * @brief Read list of string type @@ -128,7 +128,7 @@ class IniAccess { * @param[in] key_name: Key name * @param[out] data: Read quaternion data */ - void ReadQuaternion(const char* section_name, const char* key_name, s2e::math::Quaternion& data); + void ReadQuaternion(const char* section_name, const char* key_name, math::Quaternion& data); /** * @fn ReadChar * @brief Read characters data @@ -210,7 +210,7 @@ class IniAccess { }; template -void IniAccess::ReadVector(const char* section_name, const char* key_name, s2e::math::Vector& data) { +void IniAccess::ReadVector(const char* section_name, const char* key_name, math::Vector& data) { for (size_t i = 0; i < NumElement; i++) { std::stringstream c_name; c_name << key_name << "(" << i << ")"; diff --git a/src/simulation/ground_station/ground_station.cpp b/src/simulation/ground_station/ground_station.cpp index 5340c30dc..d6f230149 100644 --- a/src/simulation/ground_station/ground_station.cpp +++ b/src/simulation/ground_station/ground_station.cpp @@ -37,7 +37,7 @@ void GroundStation::Initialize(const SimulationConfiguration* configuration, con double latitude_deg = conf.ReadDouble(Section, "latitude_deg"); double longitude_deg = conf.ReadDouble(Section, "longitude_deg"); double height_m = conf.ReadDouble(Section, "height_m"); - geodetic_position_ = s2e::geodesy::GeodeticPosition(latitude_deg * s2e::math::deg_to_rad, longitude_deg * s2e::math::deg_to_rad, height_m); + geodetic_position_ = s2e::geodesy::GeodeticPosition(latitude_deg * math::deg_to_rad, longitude_deg * math::deg_to_rad, height_m); position_ecef_m_ = geodetic_position_.CalcEcefPosition(); elevation_limit_angle_deg_ = conf.ReadDouble(Section, "elevation_limit_angle_deg"); @@ -48,23 +48,23 @@ void GroundStation::Initialize(const SimulationConfiguration* configuration, con void GroundStation::LogSetup(logger::Logger& logger) { logger.AddLogList(this); } void GroundStation::Update(const environment::EarthRotation& celestial_rotation, const Spacecraft& spacecraft) { - s2e::math::Matrix<3, 3> dcm_ecef2eci = celestial_rotation.GetDcmJ2000ToEcef().Transpose(); + math::Matrix<3, 3> dcm_ecef2eci = celestial_rotation.GetDcmJ2000ToEcef().Transpose(); position_i_m_ = dcm_ecef2eci * position_ecef_m_; is_visible_[spacecraft.GetSpacecraftId()] = CalcIsVisible(spacecraft.GetDynamics().GetOrbit().GetPosition_ecef_m()); } -bool GroundStation::CalcIsVisible(const s2e::math::Vector<3> spacecraft_position_ecef_m) { - s2e::math::Quaternion q_ecef_to_ltc = geodetic_position_.GetQuaternionXcxfToLtc(); +bool GroundStation::CalcIsVisible(const math::Vector<3> spacecraft_position_ecef_m) { + math::Quaternion q_ecef_to_ltc = geodetic_position_.GetQuaternionXcxfToLtc(); - s2e::math::Vector<3> sc_pos_ltc = q_ecef_to_ltc.FrameConversion(spacecraft_position_ecef_m - position_ecef_m_); // Satellite position in LTC frame [m] + math::Vector<3> sc_pos_ltc = q_ecef_to_ltc.FrameConversion(spacecraft_position_ecef_m - position_ecef_m_); // Satellite position in LTC frame [m] sc_pos_ltc = sc_pos_ltc.CalcNormalizedVector(); - s2e::math::Vector<3> dir_gs_to_zenith = s2e::math::Vector<3>(0); + math::Vector<3> dir_gs_to_zenith = math::Vector<3>(0); dir_gs_to_zenith[2] = 1; // Judge the satellite position angle is over the minimum elevation - if (dot(sc_pos_ltc, dir_gs_to_zenith) > sin(elevation_limit_angle_deg_ * s2e::math::deg_to_rad)) { + if (dot(sc_pos_ltc, dir_gs_to_zenith) > sin(elevation_limit_angle_deg_ * math::deg_to_rad)) { return true; } else { return false; diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp index 0020cffc5..0a8cfb0e5 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp @@ -48,7 +48,7 @@ void InitializedMonteCarloParameters::GetRandomizedScalar(double& destination) c } } -void InitializedMonteCarloParameters::GetRandomizedQuaternion(s2e::math::Quaternion& destination) const { +void InitializedMonteCarloParameters::GetRandomizedQuaternion(math::Quaternion& destination) const { if (randomization_type_ == kNoRandomization) { ; } else if (4 > randomized_value_.size()) { @@ -122,7 +122,7 @@ void InitializedMonteCarloParameters::GenerateCartesianNormal() { } } -void InitializedMonteCarloParameters::CalcCircularNormalUniform(s2e::math::Vector<2>& destination, double r_mean, double r_sigma, double theta_min, +void InitializedMonteCarloParameters::CalcCircularNormalUniform(math::Vector<2>& destination, double r_mean, double r_sigma, double theta_min, double theta_max) { // r follows normal distribution, and θ follows uniform distribution in Circular frame double r = InitializedMonteCarloParameters::Generate1dNormal(r_mean, r_sigma); @@ -136,7 +136,7 @@ void InitializedMonteCarloParameters::GenerateCircularNormalUniform() { if (mean_or_min_.size() < dim || sigma_or_max_.size() < dim) { throw "Config parameters dimension unmatched."; } - s2e::math::Vector temp_vec; + math::Vector temp_vec; CalcCircularNormalUniform(temp_vec, mean_or_min_[0], sigma_or_max_[0], mean_or_min_[1], sigma_or_max_[1]); randomized_value_.clear(); @@ -145,7 +145,7 @@ void InitializedMonteCarloParameters::GenerateCircularNormalUniform() { } } -void InitializedMonteCarloParameters::CalcCircularNormalNormal(s2e::math::Vector<2>& destination, double r_mean, double r_sigma, double theta_mean, +void InitializedMonteCarloParameters::CalcCircularNormalNormal(math::Vector<2>& destination, double r_mean, double r_sigma, double theta_mean, double theta_sigma) { // r and θ follow normal distribution in Circular frame double r = InitializedMonteCarloParameters::Generate1dNormal(r_mean, r_sigma); @@ -159,7 +159,7 @@ void InitializedMonteCarloParameters::GenerateCircularNormalNormal() { if (mean_or_min_.size() < dim || sigma_or_max_.size() < dim) { throw "Config parameters dimension unmatched."; } - s2e::math::Vector temp_vec; + math::Vector temp_vec; CalcCircularNormalNormal(temp_vec, mean_or_min_[0], sigma_or_max_[0], mean_or_min_[1], sigma_or_max_[1]); randomized_value_.clear(); @@ -168,7 +168,7 @@ void InitializedMonteCarloParameters::GenerateCircularNormalNormal() { } } -void InitializedMonteCarloParameters::CalcSphericalNormalUniformUniform(s2e::math::Vector<3>& destination, double r_mean, double r_sigma, double theta_min, +void InitializedMonteCarloParameters::CalcSphericalNormalUniformUniform(math::Vector<3>& destination, double r_mean, double r_sigma, double theta_min, double theta_max, double phi_min, double phi_max) { // r follows normal distribution, and θ and φ follow uniform distribution in Spherical frame double r = InitializedMonteCarloParameters::Generate1dNormal(r_mean, r_sigma); @@ -184,7 +184,7 @@ void InitializedMonteCarloParameters::GenerateSphericalNormalUniformUniform() { if (mean_or_min_.size() < dim || sigma_or_max_.size() < dim) { throw "Config parameters dimension unmatched."; } - s2e::math::Vector temp_vec; + math::Vector temp_vec; CalcSphericalNormalUniformUniform(temp_vec, mean_or_min_[0], sigma_or_max_[0], mean_or_min_[1], sigma_or_max_[1], mean_or_min_[2], sigma_or_max_[2]); @@ -194,29 +194,29 @@ void InitializedMonteCarloParameters::GenerateSphericalNormalUniformUniform() { } } -void InitializedMonteCarloParameters::CalcSphericalNormalNormal(s2e::math::Vector<3>& destination, const s2e::math::Vector<3>& mean_vec) { +void InitializedMonteCarloParameters::CalcSphericalNormalNormal(math::Vector<3>& destination, const math::Vector<3>& mean_vec) { // r and θ follow normal distribution, and mean vector angle φ follows uniform distribution [0,2*pi] - s2e::math::Vector<3> mean_vec_dir; + math::Vector<3> mean_vec_dir; mean_vec_dir = 1.0 / mean_vec.CalcNorm() * mean_vec; // Unit vector of mean vector direction - s2e::math::Vector<3> x_axis(0.0), y_axis(0.0); + math::Vector<3> x_axis(0.0), y_axis(0.0); x_axis[0] = 1.0; y_axis[1] = 1.0; - s2e::math::Vector<3> op_x = OuterProduct(mean_vec_dir, x_axis); - s2e::math::Vector<3> op_y = OuterProduct(mean_vec_dir, y_axis); + math::Vector<3> op_x = OuterProduct(mean_vec_dir, x_axis); + math::Vector<3> op_y = OuterProduct(mean_vec_dir, y_axis); // An unit vector perpendicular with the mean vector // In case of the mean vector is parallel with X or Y axis, selecting the axis depend on the norm of outer product - s2e::math::Vector<3> normal_unit_vec = op_x.CalcNorm() > op_y.CalcNorm() ? op_x = op_x.CalcNormalizedVector() : op_y = op_y.CalcNormalizedVector(); + math::Vector<3> normal_unit_vec = op_x.CalcNorm() > op_y.CalcNorm() ? op_x = op_x.CalcNormalizedVector() : op_y = op_y.CalcNormalizedVector(); - double rotation_angle_of_normal_unit_vec = InitializedMonteCarloParameters::Generate1dUniform(0.0, s2e::math::tau); - s2e::math::Quaternion rotation_of_normal_unit_vec(mean_vec_dir, + double rotation_angle_of_normal_unit_vec = InitializedMonteCarloParameters::Generate1dUniform(0.0, math::tau); + math::Quaternion rotation_of_normal_unit_vec(mean_vec_dir, -rotation_angle_of_normal_unit_vec); // Use opposite sign to rotate the vector (not the frame) - s2e::math::Vector<3> rotation_axis = rotation_of_normal_unit_vec.FrameConversion(normal_unit_vec); // Axis of mean vector rotation + math::Vector<3> rotation_axis = rotation_of_normal_unit_vec.FrameConversion(normal_unit_vec); // Axis of mean vector rotation double rotation_angle_of_mean_vec = InitializedMonteCarloParameters::Generate1dNormal(0.0, sigma_or_max_[1]); - s2e::math::Quaternion rotation_of_mean_vec(rotation_axis, -rotation_angle_of_mean_vec); // Use opposite sign to rotate the vector (not the frame) - s2e::math::Vector<3> ret_vec = rotation_of_mean_vec.FrameConversion(mean_vec_dir); // Complete calculation of the direction + math::Quaternion rotation_of_mean_vec(rotation_axis, -rotation_angle_of_mean_vec); // Use opposite sign to rotate the vector (not the frame) + math::Vector<3> ret_vec = rotation_of_mean_vec.FrameConversion(mean_vec_dir); // Complete calculation of the direction ret_vec = InitializedMonteCarloParameters::Generate1dNormal(mean_vec.CalcNorm(), sigma_or_max_[0]) * ret_vec; // multiply norm @@ -230,7 +230,7 @@ void InitializedMonteCarloParameters::GenerateSphericalNormalNormal() { if (mean_or_min_.size() < dim || sigma_or_max_.size() < 2) { throw "Config parameters dimension unmatched."; } - s2e::math::Vector temp_vec, temp_mean_vec; + math::Vector temp_vec, temp_mean_vec; for (int i = 0; i < dim; i++) { temp_mean_vec[i] = mean_or_min_[i]; } @@ -242,32 +242,32 @@ void InitializedMonteCarloParameters::GenerateSphericalNormalNormal() { } } -void InitializedMonteCarloParameters::CalcQuaternionUniform(s2e::math::Quaternion& destination) { - // Perfectly Randomized s2e::math::Quaternion - s2e::math::Vector<3> x_axis(0.0); +void InitializedMonteCarloParameters::CalcQuaternionUniform(math::Quaternion& destination) { + // Perfectly Randomized math::Quaternion + math::Vector<3> x_axis(0.0); x_axis[0] = 1.0; // A direction vector converted from the X-axis by a quaternion may follows the uniform distribution in full sphere. - s2e::math::Quaternion first_cnv; - s2e::math::Vector<3> x_axis_cnvd; + math::Quaternion first_cnv; + math::Vector<3> x_axis_cnvd; double theta = acos(1 - (1 - (-1)) * InitializedMonteCarloParameters::Generate1dUniform(0.0, 1.0)); - double phi = InitializedMonteCarloParameters::Generate1dUniform(0, s2e::math::tau); + double phi = InitializedMonteCarloParameters::Generate1dUniform(0, math::tau); x_axis_cnvd[0] = sin(theta) * cos(phi); x_axis_cnvd[1] = sin(theta) * sin(phi); x_axis_cnvd[2] = cos(theta); double cos_angle_between = InnerProduct(x_axis, x_axis_cnvd); - s2e::math::Vector<3> op = OuterProduct(x_axis, x_axis_cnvd); + math::Vector<3> op = OuterProduct(x_axis, x_axis_cnvd); for (int i = 0; i < 3; i++) { first_cnv[i] = op[i]; } first_cnv[3] = cos_angle_between; // Generate randomized rotation angle around the X-axis - double rotation_angle = InitializedMonteCarloParameters::Generate1dUniform(0.0, s2e::math::tau); - s2e::math::Quaternion second_cnv(x_axis, rotation_angle); + double rotation_angle = InitializedMonteCarloParameters::Generate1dUniform(0.0, math::tau); + math::Quaternion second_cnv(x_axis, rotation_angle); - s2e::math::Quaternion ret_q = first_cnv * second_cnv; + math::Quaternion ret_q = first_cnv * second_cnv; for (int i = 0; i < 4; i++) { destination[i] = ret_q[i]; @@ -276,7 +276,7 @@ void InitializedMonteCarloParameters::CalcQuaternionUniform(s2e::math::Quaternio void InitializedMonteCarloParameters::GenerateQuaternionUniform() { const static int dim = 4; - s2e::math::Quaternion temp_q; + math::Quaternion temp_q; CalcQuaternionUniform(temp_q); randomized_value_.clear(); @@ -285,19 +285,19 @@ void InitializedMonteCarloParameters::GenerateQuaternionUniform() { } } -void InitializedMonteCarloParameters::CalcQuaternionNormal(s2e::math::Quaternion& destination, double theta_sigma) { +void InitializedMonteCarloParameters::CalcQuaternionNormal(math::Quaternion& destination, double theta_sigma) { // Angle from the default quaternion θ follows normal distribution // The rotation axis follows uniform distribution on full sphere - s2e::math::Vector<3> rot_axis; + math::Vector<3> rot_axis; double theta = acos(1 - (1 - (-1)) * InitializedMonteCarloParameters::Generate1dUniform(0.0, 1.0)); - double phi = InitializedMonteCarloParameters::Generate1dUniform(0, s2e::math::tau); + double phi = InitializedMonteCarloParameters::Generate1dUniform(0, math::tau); rot_axis[0] = sin(theta) * cos(phi); rot_axis[1] = sin(theta) * sin(phi); rot_axis[2] = cos(theta); double rotation_angle = InitializedMonteCarloParameters::Generate1dNormal(0.0, theta_sigma); - s2e::math::Quaternion ret_q(rot_axis, rotation_angle); + math::Quaternion ret_q(rot_axis, rotation_angle); for (int i = 0; i < 4; i++) { destination[i] = ret_q[i]; } @@ -309,7 +309,7 @@ void InitializedMonteCarloParameters::GenerateQuaternionNormal() { if (sigma_or_max_.size() < 1) { throw "Config parameters dimension unmatched."; } - s2e::math::Quaternion temp_q; + math::Quaternion temp_q; CalcQuaternionNormal(temp_q, sigma_or_max_[0]); randomized_value_.clear(); diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp index 720d678eb..348a459de 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_simulation.cpp @@ -81,12 +81,12 @@ MonteCarloSimulationExecutor* InitMonteCarloSimulation(std::string file_name) { // Read mean_or_min vector key_name = so_dot_ip_str + MonteCarloSimulationExecutor::separator_ + "mean_or_min"; - s2e::math::Vector<3> mean_or_min; + math::Vector<3> mean_or_min; ini_file.ReadVector(section, key_name.c_str(), mean_or_min); // Read sigma_or_max vector key_name = so_dot_ip_str + MonteCarloSimulationExecutor::separator_ + "sigma_or_max"; - s2e::math::Vector<3> sigma_or_max; + math::Vector<3> sigma_or_max; ini_file.ReadVector(section, key_name.c_str(), sigma_or_max); // Write randomize setting diff --git a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp index dda7abfd0..6f422e5ce 100644 --- a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp +++ b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.cpp @@ -49,7 +49,7 @@ void MonteCarloSimulationExecutor::GetInitializedMonteCarloParameterDouble(strin } void MonteCarloSimulationExecutor::GetInitializedMonteCarloParameterQuaternion(string so_name, string init_monte_carlo_parameter_name, - s2e::math::Quaternion& destination) const { + math::Quaternion& destination) const { if (!enabled_) return; { string name = so_name + MonteCarloSimulationExecutor::separator_ + init_monte_carlo_parameter_name; diff --git a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp index 97626e47e..fdf2328dc 100644 --- a/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp +++ b/src/simulation/monte_carlo_simulation/monte_carlo_simulation_executor.hpp @@ -88,7 +88,7 @@ class MonteCarloSimulationExecutor { */ template void GetInitializedMonteCarloParameterVector(std::string so_name, std::string init_monte_carlo_parameter_name, - s2e::math::Vector& destination) const; + math::Vector& destination) const; /** * @fn GetInitializedMonteCarloParameterDouble * @brief Get randomized value and store it in dest @@ -99,7 +99,7 @@ class MonteCarloSimulationExecutor { * @brief Get randomized quaternion and store it in dest_quat */ void GetInitializedMonteCarloParameterQuaternion(std::string so_name, std::string init_monte_carlo_parameter_name, - s2e::math::Quaternion& destination) const; + math::Quaternion& destination) const; // Calculation /** @@ -128,7 +128,7 @@ class MonteCarloSimulationExecutor { * @brief Add initialized parameter */ void AddInitializedMonteCarloParameter(std::string so_name, std::string init_monte_carlo_parameter_name, - const s2e::math::Vector& mean_or_min, const s2e::math::Vector& sigma_or_max, + const math::Vector& mean_or_min, const math::Vector& sigma_or_max, InitializedMonteCarloParameters::RandomizationType random_type); /** @@ -140,7 +140,7 @@ class MonteCarloSimulationExecutor { template void MonteCarloSimulationExecutor::GetInitializedMonteCarloParameterVector(std::string so_name, std::string init_monte_carlo_parameter_name, - s2e::math::Vector& destination) const { + math::Vector& destination) const { if (!enabled_) return; std::string name = so_name + MonteCarloSimulationExecutor::separator_ + init_monte_carlo_parameter_name; if (init_parameter_list_.find(name) == init_parameter_list_.end()) { @@ -153,8 +153,8 @@ void MonteCarloSimulationExecutor::GetInitializedMonteCarloParameterVector(std:: template void MonteCarloSimulationExecutor::AddInitializedMonteCarloParameter(std::string so_name, std::string init_monte_carlo_parameter_name, - const s2e::math::Vector& mean_or_min, - const s2e::math::Vector& sigma_or_max, + const math::Vector& mean_or_min, + const math::Vector& sigma_or_max, InitializedMonteCarloParameters::RandomizationType random_type) { std::string name = so_name + MonteCarloSimulationExecutor::separator_ + init_monte_carlo_parameter_name; if (init_parameter_list_.find(name) == init_parameter_list_.end()) { diff --git a/src/simulation/monte_carlo_simulation/simulation_object.cpp b/src/simulation/monte_carlo_simulation/simulation_object.cpp index 0f41bf32d..cb5f015ec 100644 --- a/src/simulation/monte_carlo_simulation/simulation_object.cpp +++ b/src/simulation/monte_carlo_simulation/simulation_object.cpp @@ -40,7 +40,7 @@ void SimulationObject::GetInitializedMonteCarloParameterDouble(const MonteCarloS } void SimulationObject::GetInitializedMonteCarloParameterQuaternion(const MonteCarloSimulationExecutor& monte_carlo_simulator, - std::string init_monte_carlo_parameter_name, s2e::math::Quaternion& destination) const { + std::string init_monte_carlo_parameter_name, math::Quaternion& destination) const { monte_carlo_simulator.GetInitializedMonteCarloParameterQuaternion(name_, init_monte_carlo_parameter_name, destination); } diff --git a/src/simulation/monte_carlo_simulation/simulation_object.hpp b/src/simulation/monte_carlo_simulation/simulation_object.hpp index cb1c2df09..4273771ec 100644 --- a/src/simulation/monte_carlo_simulation/simulation_object.hpp +++ b/src/simulation/monte_carlo_simulation/simulation_object.hpp @@ -42,7 +42,7 @@ class SimulationObject { */ template void GetInitializedMonteCarloParameterVector(const MonteCarloSimulationExecutor& monte_carlo_simulator, std::string init_monte_carlo_parameter_name, - s2e::math::Vector& destination) const; + math::Vector& destination) const; /** * @fn GetInitializedMonteCarloParameterDouble @@ -56,7 +56,7 @@ class SimulationObject { * @brief Get randomized quaternion and store it in destination */ void GetInitializedMonteCarloParameterQuaternion(const MonteCarloSimulationExecutor& monte_carlo_simulator, - std::string init_monte_carlo_parameter_name, s2e::math::Quaternion& destination) const; + std::string init_monte_carlo_parameter_name, math::Quaternion& destination) const; /** * @fn SetParameters @@ -82,7 +82,7 @@ class SimulationObject { template void SimulationObject::GetInitializedMonteCarloParameterVector(const MonteCarloSimulationExecutor& monte_carlo_simulator, std::string init_monte_carlo_parameter_name, - s2e::math::Vector& destination) const { + math::Vector& destination) const { monte_carlo_simulator.GetInitializedMonteCarloParameterVector(name_, init_monte_carlo_parameter_name, destination); } diff --git a/src/simulation/multiple_spacecraft/relative_information.cpp b/src/simulation/multiple_spacecraft/relative_information.cpp index 93cd3007b..8c1465239 100644 --- a/src/simulation/multiple_spacecraft/relative_information.cpp +++ b/src/simulation/multiple_spacecraft/relative_information.cpp @@ -15,8 +15,8 @@ void RelativeInformation::Update() { for (size_t target_spacecraft_id = 0; target_spacecraft_id < dynamics_database_.size(); target_spacecraft_id++) { for (size_t reference_spacecraft_id = 0; reference_spacecraft_id < dynamics_database_.size(); reference_spacecraft_id++) { // Position - s2e::math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); - s2e::math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); + math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); + math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); relative_position_list_i_m_[target_spacecraft_id][reference_spacecraft_id] = target_sat_pos_i - reference_sat_pos_i; relative_position_list_rtn_m_[target_spacecraft_id][reference_spacecraft_id] = CalcRelativePosition_rtn_m(target_spacecraft_id, reference_spacecraft_id); @@ -26,8 +26,8 @@ void RelativeInformation::Update() { relative_position_list_i_m_[target_spacecraft_id][reference_spacecraft_id].CalcNorm(); // Velocity - s2e::math::Vector<3> target_sat_vel_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); - s2e::math::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); + math::Vector<3> target_sat_vel_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); + math::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); relative_velocity_list_i_m_s_[target_spacecraft_id][reference_spacecraft_id] = target_sat_vel_i - reference_sat_vel_i; relative_velocity_list_rtn_m_s_[target_spacecraft_id][reference_spacecraft_id] = CalcRelativeVelocity_rtn_m_s(target_spacecraft_id, reference_spacecraft_id); @@ -113,57 +113,57 @@ std::string RelativeInformation::GetLogValue() const { void RelativeInformation::LogSetup(logger::Logger& logger) { logger.AddLogList(this); } -s2e::math::Quaternion RelativeInformation::CalcRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { +math::Quaternion RelativeInformation::CalcRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { // Observer SC Body frame(obs_sat) -> ECI frame(i) - s2e::math::Quaternion q_reference_i2b = dynamics_database_.at(reference_spacecraft_id)->GetAttitude().GetQuaternion_i2b(); - s2e::math::Quaternion q_reference_b2i = q_reference_i2b.Conjugate(); + math::Quaternion q_reference_i2b = dynamics_database_.at(reference_spacecraft_id)->GetAttitude().GetQuaternion_i2b(); + math::Quaternion q_reference_b2i = q_reference_i2b.Conjugate(); // ECI frame(i) -> Target SC body frame(main_sat) - s2e::math::Quaternion q_target_i2b = dynamics_database_.at(target_spacecraft_id)->GetAttitude().GetQuaternion_i2b(); + math::Quaternion q_target_i2b = dynamics_database_.at(target_spacecraft_id)->GetAttitude().GetQuaternion_i2b(); return q_target_i2b * q_reference_b2i; } -s2e::math::Vector<3> RelativeInformation::CalcRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { - s2e::math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); - s2e::math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); - s2e::math::Vector<3> relative_pos_i = target_sat_pos_i - reference_sat_pos_i; +math::Vector<3> RelativeInformation::CalcRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { + math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); + math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); + math::Vector<3> relative_pos_i = target_sat_pos_i - reference_sat_pos_i; // RTN frame for the reference satellite - s2e::math::Quaternion q_i2rtn = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().CalcQuaternion_i2lvlh(); + math::Quaternion q_i2rtn = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().CalcQuaternion_i2lvlh(); - s2e::math::Vector<3> relative_pos_rtn = q_i2rtn.FrameConversion(relative_pos_i); + math::Vector<3> relative_pos_rtn = q_i2rtn.FrameConversion(relative_pos_i); return relative_pos_rtn; } -s2e::math::Vector<3> RelativeInformation::CalcRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { - s2e::math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); - s2e::math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); - s2e::math::Vector<3> relative_pos_i = target_sat_pos_i - reference_sat_pos_i; +math::Vector<3> RelativeInformation::CalcRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) { + math::Vector<3> target_sat_pos_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetPosition_i_m(); + math::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetPosition_i_m(); + math::Vector<3> relative_pos_i = target_sat_pos_i - reference_sat_pos_i; // RTN frame for the reference satellite - s2e::math::Quaternion q_i2rtn = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().CalcQuaternion_i2lvlh(); + math::Quaternion q_i2rtn = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().CalcQuaternion_i2lvlh(); // Rotation vector of RTN frame - s2e::math::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); - s2e::math::Vector<3> target_sat_vel_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); - s2e::math::Vector<3> rot_vec_rtn_i = cross(reference_sat_pos_i, reference_sat_vel_i); + math::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); + math::Vector<3> target_sat_vel_i = dynamics_database_.at(target_spacecraft_id)->GetOrbit().GetVelocity_i_m_s(); + math::Vector<3> rot_vec_rtn_i = cross(reference_sat_pos_i, reference_sat_vel_i); double r2_ref = reference_sat_pos_i.CalcNorm() * reference_sat_pos_i.CalcNorm(); rot_vec_rtn_i /= r2_ref; - s2e::math::Vector<3> relative_vel_i = target_sat_vel_i - reference_sat_vel_i - cross(rot_vec_rtn_i, relative_pos_i); + math::Vector<3> relative_vel_i = target_sat_vel_i - reference_sat_vel_i - cross(rot_vec_rtn_i, relative_pos_i); - s2e::math::Vector<3> relative_vel_rtn = q_i2rtn.FrameConversion(relative_vel_i); + math::Vector<3> relative_vel_rtn = q_i2rtn.FrameConversion(relative_vel_i); return relative_vel_rtn; } void RelativeInformation::ResizeLists() { size_t size = dynamics_database_.size(); - relative_position_list_i_m_.assign(size, std::vector>(size, s2e::math::Vector<3>(0))); - relative_velocity_list_i_m_s_.assign(size, std::vector>(size, s2e::math::Vector<3>(0))); + relative_position_list_i_m_.assign(size, std::vector>(size, math::Vector<3>(0))); + relative_velocity_list_i_m_s_.assign(size, std::vector>(size, math::Vector<3>(0))); relative_distance_list_m_.assign(size, std::vector(size, 0.0)); - relative_position_list_rtn_m_.assign(size, std::vector>(size, s2e::math::Vector<3>(0))); - relative_velocity_list_rtn_m_s_.assign(size, std::vector>(size, s2e::math::Vector<3>(0))); - relative_attitude_quaternion_list_.assign(size, std::vector(size, s2e::math::Quaternion(0, 0, 0, 1))); + relative_position_list_rtn_m_.assign(size, std::vector>(size, math::Vector<3>(0))); + relative_velocity_list_rtn_m_s_.assign(size, std::vector>(size, math::Vector<3>(0))); + relative_attitude_quaternion_list_.assign(size, std::vector(size, math::Quaternion(0, 0, 0, 1))); } } // namespace s2e::simulation diff --git a/src/simulation/multiple_spacecraft/relative_information.hpp b/src/simulation/multiple_spacecraft/relative_information.hpp index 04423a2ed..e0dbf1a78 100644 --- a/src/simulation/multiple_spacecraft/relative_information.hpp +++ b/src/simulation/multiple_spacecraft/relative_information.hpp @@ -75,7 +75,7 @@ class RelativeInformation : public logger::ILoggable { * @param [in] target_spacecraft_id: ID of target spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - inline s2e::math::Quaternion GetRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { + inline math::Quaternion GetRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { return relative_attitude_quaternion_list_[target_spacecraft_id][reference_spacecraft_id]; } /** @@ -84,7 +84,7 @@ class RelativeInformation : public logger::ILoggable { * @param [in] target_spacecraft_id: ID of target spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - inline s2e::math::Vector<3> GetRelativePosition_i_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { + inline math::Vector<3> GetRelativePosition_i_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { return relative_position_list_i_m_[target_spacecraft_id][reference_spacecraft_id]; } /** @@ -93,7 +93,7 @@ class RelativeInformation : public logger::ILoggable { * @param [in] target_spacecraft_id: ID of target spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - inline s2e::math::Vector<3> GetRelativeVelocity_i_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { + inline math::Vector<3> GetRelativeVelocity_i_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { return relative_velocity_list_i_m_s_[target_spacecraft_id][reference_spacecraft_id]; } /** @@ -111,7 +111,7 @@ class RelativeInformation : public logger::ILoggable { * @param [in] target_spacecraft_id: ID of target spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - inline s2e::math::Vector<3> GetRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { + inline math::Vector<3> GetRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { return relative_position_list_rtn_m_[target_spacecraft_id][reference_spacecraft_id]; } /** @@ -120,7 +120,7 @@ class RelativeInformation : public logger::ILoggable { * @param [in] target_spacecraft_id: ID of target spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - inline s2e::math::Vector<3> GetRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { + inline math::Vector<3> GetRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id) const { return relative_velocity_list_rtn_m_s_[target_spacecraft_id][reference_spacecraft_id]; } @@ -136,12 +136,12 @@ class RelativeInformation : public logger::ILoggable { private: std::map dynamics_database_; //!< Dynamics database of all spacecraft - std::vector>> relative_position_list_i_m_; //!< Relative position list in the inertial frame in unit [m] - std::vector>> relative_velocity_list_i_m_s_; //!< Relative velocity list in the inertial frame in unit [m/s] - std::vector>> relative_position_list_rtn_m_; //!< Relative position list in the RTN frame in unit [m] - std::vector>> relative_velocity_list_rtn_m_s_; //!< Relative velocity list in the RTN frame in unit [m/s] + std::vector>> relative_position_list_i_m_; //!< Relative position list in the inertial frame in unit [m] + std::vector>> relative_velocity_list_i_m_s_; //!< Relative velocity list in the inertial frame in unit [m/s] + std::vector>> relative_position_list_rtn_m_; //!< Relative position list in the RTN frame in unit [m] + std::vector>> relative_velocity_list_rtn_m_s_; //!< Relative velocity list in the RTN frame in unit [m/s] std::vector> relative_distance_list_m_; //!< Relative distance list in unit [m] - std::vector> relative_attitude_quaternion_list_; //!< Relative attitude quaternion list + std::vector> relative_attitude_quaternion_list_; //!< Relative attitude quaternion list /** * @fn CalcRelativeAttitudeQuaternion @@ -149,21 +149,21 @@ class RelativeInformation : public logger::ILoggable { * @param [in] target_spacecraft_id: ID of the spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - s2e::math::Quaternion CalcRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); + math::Quaternion CalcRelativeAttitudeQuaternion(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); /** * @fn CalcRelativePosition_rtn_m * @brief Calculate and return the relative position in RTN frame * @param [in] target_spacecraft_id: ID of the spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - s2e::math::Vector<3> CalcRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); + math::Vector<3> CalcRelativePosition_rtn_m(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); /** * @fn CalcRelativeVelocity_rtn_m_s * @brief Calculate and return the relative velocity in RTN frame * @param [in] target_spacecraft_id: ID of the spacecraft * @param [in] reference_spacecraft_id: ID of reference spacecraft */ - s2e::math::Vector<3> CalcRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); + math::Vector<3> CalcRelativeVelocity_rtn_m_s(const size_t target_spacecraft_id, const size_t reference_spacecraft_id); /** * @fn ResizeLists diff --git a/src/simulation/spacecraft/installed_components.cpp b/src/simulation/spacecraft/installed_components.cpp index ebee0e571..db5899965 100644 --- a/src/simulation/spacecraft/installed_components.cpp +++ b/src/simulation/spacecraft/installed_components.cpp @@ -9,13 +9,13 @@ namespace s2e::simulation { -s2e::math::Vector<3> InstalledComponents::GenerateForce_b_N() { - s2e::math::Vector<3> force_b_N_(0.0); +math::Vector<3> InstalledComponents::GenerateForce_b_N() { + math::Vector<3> force_b_N_(0.0); return force_b_N_; } -s2e::math::Vector<3> InstalledComponents::GenerateTorque_b_Nm() { - s2e::math::Vector<3> torque_b_Nm_(0.0); +math::Vector<3> InstalledComponents::GenerateTorque_b_Nm() { + math::Vector<3> torque_b_Nm_(0.0); return torque_b_Nm_; } diff --git a/src/simulation/spacecraft/installed_components.hpp b/src/simulation/spacecraft/installed_components.hpp index f357daca3..d0a731355 100644 --- a/src/simulation/spacecraft/installed_components.hpp +++ b/src/simulation/spacecraft/installed_components.hpp @@ -28,14 +28,14 @@ class InstalledComponents { * @brief Return force generated by components in unit Newton in body fixed frame * @details Users need to override this function to add force generated by components */ - virtual s2e::math::Vector<3> GenerateForce_b_N(); + virtual math::Vector<3> GenerateForce_b_N(); /** * @fn GenerateTorque_b_Nm * @brief Return torque generated by components in unit Newton-meter in body fixed frame * @details Users need to override this function to add torque generated by components */ - virtual s2e::math::Vector<3> GenerateTorque_b_Nm(); + virtual math::Vector<3> GenerateTorque_b_Nm(); /** * @fn ComponentInterference diff --git a/src/simulation/spacecraft/structure/initialize_structure.cpp b/src/simulation/spacecraft/structure/initialize_structure.cpp index 5eaa7c863..b52091e23 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.cpp +++ b/src/simulation/spacecraft/structure/initialize_structure.cpp @@ -15,11 +15,11 @@ KinematicsParameters InitKinematicsParameters(std::string file_name) { auto conf = setting_file_reader::IniAccess(file_name); const char* section = "KINEMATIC_PARAMETERS"; - s2e::math::Vector<3> center_of_gravity_b_m; + math::Vector<3> center_of_gravity_b_m; conf.ReadVector(section, "center_of_gravity_b_m", center_of_gravity_b_m); double mass_kg = conf.ReadDouble(section, "mass_kg"); - s2e::math::Vector<9> inertia_vec; - s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2; + math::Vector<9> inertia_vec; + math::Matrix<3, 3> inertia_tensor_b_kgm2; conf.ReadVector(section, "inertia_tensor_kgm2", inertia_vec); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { @@ -112,7 +112,7 @@ ResidualMagneticMoment InitResidualMagneticMoment(std::string file_name) { auto conf = setting_file_reader::IniAccess(file_name); const char* section = "RESIDUAL_MAGNETIC_MOMENT"; - s2e::math::Vector<3> rmm_const_b; + math::Vector<3> rmm_const_b; conf.ReadVector(section, "rmm_constant_b_Am2", rmm_const_b); double rmm_rwdev = conf.ReadDouble(section, "rmm_random_walk_speed_Am2"); double random_walk_limit_Am2 = conf.ReadDouble(section, "rmm_random_walk_limit_Am2"); diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.cpp b/src/simulation/spacecraft/structure/kinematics_parameters.cpp index 9318c92e7..403487a3f 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.cpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.cpp @@ -7,7 +7,7 @@ namespace s2e::simulation { -KinematicsParameters::KinematicsParameters(s2e::math::Vector<3> center_of_gravity_b_m, double mass_kg, s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2) +KinematicsParameters::KinematicsParameters(math::Vector<3> center_of_gravity_b_m, double mass_kg, math::Matrix<3, 3> inertia_tensor_b_kgm2) : center_of_gravity_b_m_(center_of_gravity_b_m), mass_kg_(mass_kg), inertia_tensor_b_kgm2_(inertia_tensor_b_kgm2) {} } // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.hpp b/src/simulation/spacecraft/structure/kinematics_parameters.hpp index c7d3db504..2a1805dc3 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.hpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.hpp @@ -21,7 +21,7 @@ class KinematicsParameters { * @fn KinematicsParameters * @brief Constructor */ - KinematicsParameters(s2e::math::Vector<3> center_of_gravity_b_m, double mass_kg, s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2); + KinematicsParameters(math::Vector<3> center_of_gravity_b_m, double mass_kg, math::Matrix<3, 3> inertia_tensor_b_kgm2); /** * @fn ~KinematicsParameters * @brief Destructor @@ -33,7 +33,7 @@ class KinematicsParameters { * @fn GetCenterOfGravity_b_m * @brief Return Position vector of center of gravity at body frame [m] */ - inline const s2e::math::Vector<3>& GetCenterOfGravity_b_m() const { return center_of_gravity_b_m_; } + inline const math::Vector<3>& GetCenterOfGravity_b_m() const { return center_of_gravity_b_m_; } /** * @fn GetMass_kg * @brief Return Mass of the satellite [kg] @@ -43,7 +43,7 @@ class KinematicsParameters { * @fn GetInertiaTensor_b_kgm2 * @brief Return Inertia tensor at body frame [kgm2] */ - inline const s2e::math::Matrix<3, 3>& GetInertiaTensor_b_kgm2() const { return inertia_tensor_b_kgm2_; } + inline const math::Matrix<3, 3>& GetInertiaTensor_b_kgm2() const { return inertia_tensor_b_kgm2_; } // Setter /** @@ -51,7 +51,7 @@ class KinematicsParameters { * @brief Set center of gravity vector at the body frame [m] * @param [in] center_of_gravity_vector_b_m: Center of gravity vector at the body frame [m] */ - inline void SetCenterOfGravityVector_b_m(const s2e::math::Vector<3> center_of_gravity_vector_b_m) { + inline void SetCenterOfGravityVector_b_m(const math::Vector<3> center_of_gravity_vector_b_m) { center_of_gravity_b_m_ = center_of_gravity_vector_b_m; } /** @@ -77,15 +77,15 @@ class KinematicsParameters { * @brief Inertia tensor at body frame * @param [in] inertia_tensor_b_kgm2: Inertia tensor at body frame [kgm2] */ - inline void SetInertiaTensor_b_kgm2(const s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2) { + inline void SetInertiaTensor_b_kgm2(const math::Matrix<3, 3> inertia_tensor_b_kgm2) { // TODO add assertion check inertia_tensor_b_kgm2_ = inertia_tensor_b_kgm2; } private: - s2e::math::Vector<3> center_of_gravity_b_m_; //!< Position vector of center of gravity at body frame [m] + math::Vector<3> center_of_gravity_b_m_; //!< Position vector of center of gravity at body frame [m] double mass_kg_; //!< Mass of the satellite [kg] - s2e::math::Matrix<3, 3> inertia_tensor_b_kgm2_; //!< Inertia tensor at body frame [kgm2] + math::Matrix<3, 3> inertia_tensor_b_kgm2_; //!< Inertia tensor at body frame [kgm2] }; } // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp index f88ef8b39..0f8fc1377 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp @@ -7,7 +7,7 @@ namespace s2e::simulation { -ResidualMagneticMoment::ResidualMagneticMoment(const Vector<3> constant_value_b_Am2_, const double random_walk_standard_deviation_Am2, +ResidualMagneticMoment::ResidualMagneticMoment(const math::Vector<3> constant_value_b_Am2_, const double random_walk_standard_deviation_Am2, const double random_walk_limit_Am2, const double random_noise_standard_deviation_Am2) : constant_value_b_Am2_(constant_value_b_Am2_), random_walk_standard_deviation_Am2_(random_walk_standard_deviation_Am2), diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp index b66d7c90d..138e8ddf0 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp @@ -20,7 +20,7 @@ class ResidualMagneticMoment { * @fn ResidualMagneticMoment * @brief Constructor */ - ResidualMagneticMoment(const Vector<3> constant_value_b_Am2_, const double random_walk_standard_deviation_Am2, const double random_walk_limit_Am2, + ResidualMagneticMoment(const math::Vector<3> constant_value_b_Am2_, const double random_walk_standard_deviation_Am2, const double random_walk_limit_Am2, const double random_noise_standard_deviation_Am2); /** * @fn ~ResidualMagneticMoment @@ -33,7 +33,7 @@ class ResidualMagneticMoment { * @fn GetConstantValue_b_Am2 * @brief Return Constant value of RMM at body frame [Am2] */ - inline const Vector<3>& GetConstantValue_b_Am2(void) const { return constant_value_b_Am2_; } + inline const math::Vector<3>& GetConstantValue_b_Am2(void) const { return constant_value_b_Am2_; } /** * @fn GetRandomWalkStandardDeviation_Am2 * @brief Return Random walk standard deviation of RMM [Am2] diff --git a/src/simulation/spacecraft/structure/surface.cpp b/src/simulation/spacecraft/structure/surface.cpp index 9c6c1326c..6ea2356b0 100644 --- a/src/simulation/spacecraft/structure/surface.cpp +++ b/src/simulation/spacecraft/structure/surface.cpp @@ -7,7 +7,7 @@ namespace s2e::simulation { -Surface::Surface(const s2e::math::Vector<3> position_b_m, const s2e::math::Vector<3> normal_b, const double area_m2, const double reflectivity, +Surface::Surface(const math::Vector<3> position_b_m, const math::Vector<3> normal_b, const double area_m2, const double reflectivity, const double specularity, const double air_specularity) : position_b_m_(position_b_m), normal_b_(normal_b), diff --git a/src/simulation/spacecraft/structure/surface.hpp b/src/simulation/spacecraft/structure/surface.hpp index 01367ee95..5f9007fe4 100644 --- a/src/simulation/spacecraft/structure/surface.hpp +++ b/src/simulation/spacecraft/structure/surface.hpp @@ -20,7 +20,7 @@ class Surface { * @fn Surface * @brief Constructor */ - Surface(const s2e::math::Vector<3> position_b_m, const s2e::math::Vector<3> normal_b, const double area_m2, const double reflectivity, + Surface(const math::Vector<3> position_b_m, const math::Vector<3> normal_b, const double area_m2, const double reflectivity, const double specularity, const double air_specularity); /** * @fn ~Surface @@ -33,12 +33,12 @@ class Surface { * @fn GetPosition_b_m * @brief Return position vector of geometric center of the surface in body frame and meter unit */ - inline const s2e::math::Vector<3>& GetPosition_b_m(void) const { return position_b_m_; } + inline const math::Vector<3>& GetPosition_b_m(void) const { return position_b_m_; } /** * @fn GetNormal_b * @brief Return normal vector of the surface in body frame */ - inline const s2e::math::Vector<3>& GetNormal_b(void) const { return normal_b_; } + inline const math::Vector<3>& GetNormal_b(void) const { return normal_b_; } /** * @fn GetArea_m2 * @brief Return area of the surface in meter^2 unit @@ -66,13 +66,13 @@ class Surface { * @brief Set position vector of geometric center of the surface in body frame [m] * @param[in] position_b_m: Position vector of geometric center of the surface in body frame [m] */ - inline void SetPosition_b_m(const s2e::math::Vector<3> position_b_m) { position_b_m_ = position_b_m; } + inline void SetPosition_b_m(const math::Vector<3> position_b_m) { position_b_m_ = position_b_m; } /** * @fn SetNormal * @brief Set normal vector of the surface in body frame * @param[in] normal_b: Normal vector of the surface in body frame */ - inline void SetNormal_b(const s2e::math::Vector<3> normal_b) { normal_b_ = normal_b.CalcNormalizedVector(); } + inline void SetNormal_b(const math::Vector<3> normal_b) { normal_b_ = normal_b.CalcNormalizedVector(); } /** * @fn SetArea_m2 * @brief Set area of the surface @@ -107,8 +107,8 @@ class Surface { } private: - s2e::math::Vector<3> position_b_m_; //!< Position vector of the surface @ Body Frame [m] - s2e::math::Vector<3> normal_b_; //!< Normal unit vector of the surface @ Body Frame [-] + math::Vector<3> position_b_m_; //!< Position vector of the surface @ Body Frame [m] + math::Vector<3> normal_b_; //!< Normal unit vector of the surface @ Body Frame [-] double area_m2_; //!< Area of the surface [m2] double reflectivity_; //!< Total reflectivity for solar wavelength (1.0 - solar absorption) double specularity_; //!< Ratio of specular reflection in the total reflected light diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index 0a61fea77..4a9d9e852 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -145,21 +145,21 @@ SampleComponents::SampleComponents(const dynamics::dynamics::Dynamics* dynamics, /**************/ // actuator debug output - // s2e::math::Vector mag_moment_c{0.01}; + // math::Vector mag_moment_c{0.01}; // magnetorquer_->SetOutputMagneticMoment_c_Am2(mag_moment_c); // reaction_wheel_->SetTargetTorque_rw_Nm(0.01); // reaction_wheel_->SetDriveFlag(true); // thruster_->SetDuty(0.9); // force generator debug output - // s2e::math::Vector<3> force_N; + // math::Vector<3> force_N; // force_N[0] = 1.0; // force_N[1] = 0.0; // force_N[2] = 0.0; // force_generator_->SetForce_b_N(force_N); // torque generator debug output - // s2e::math::Vector<3> torque_Nm; + // math::Vector<3> torque_Nm; // torque_Nm[0] = 0.1; // torque_Nm[1] = 0.0; // torque_Nm[2] = 0.0; From 3d87e0371b9ed642953d3e644da42f57248f1207 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 28 Sep 2024 22:30:26 +0900 Subject: [PATCH 40/49] Fix format --- src/components/base/component.cpp | 5 ++-- src/components/base/component.hpp | 8 ++--- .../base/gpio_connection_with_obc.cpp | 2 +- .../base/gpio_connection_with_obc.hpp | 2 +- src/components/base/i2c_controller.cpp | 3 +- .../i2c_target_communication_with_obc.hpp | 2 +- .../base/interface_gpio_component.hpp | 2 +- src/components/base/sensor.hpp | 8 ++--- .../base/uart_communication_with_obc.cpp | 3 +- .../base/uart_communication_with_obc.hpp | 2 +- .../example_i2c_controller_for_hils.cpp | 7 +++-- .../example_i2c_controller_for_hils.hpp | 5 ++-- .../example_serial_communication_for_hils.cpp | 7 +++-- .../example_serial_communication_for_hils.hpp | 5 ++-- .../ideal/angular_velocity_observer.cpp | 7 +++-- .../ideal/angular_velocity_observer.hpp | 9 +++--- src/components/ideal/attitude_observer.cpp | 3 +- src/components/ideal/attitude_observer.hpp | 6 ++-- src/components/ideal/force_generator.cpp | 3 +- src/components/ideal/force_generator.hpp | 9 +++--- src/components/ideal/orbit_observer.hpp | 8 ++--- src/components/ideal/torque_generator.cpp | 8 +++-- src/components/ideal/torque_generator.hpp | 9 +++--- src/components/ports/i2c_port.cpp | 2 +- src/components/ports/power_port.cpp | 2 +- src/components/ports/uart_port.cpp | 2 +- src/components/ports/uart_port.hpp | 2 +- src/components/real/aocs/gnss_receiver.cpp | 29 +++++++++++-------- src/components/real/aocs/gnss_receiver.hpp | 21 +++++++------- src/components/real/aocs/gyro_sensor.cpp | 6 ++-- src/components/real/aocs/gyro_sensor.hpp | 6 ++-- src/components/real/aocs/magnetometer.cpp | 6 ++-- src/components/real/aocs/magnetometer.hpp | 6 ++-- src/components/real/aocs/magnetorquer.cpp | 10 +++---- src/components/real/aocs/magnetorquer.hpp | 10 +++---- .../aocs/mtq_magnetometer_interference.cpp | 2 +- .../aocs/mtq_magnetometer_interference.hpp | 2 +- src/components/real/aocs/reaction_wheel.cpp | 2 +- src/components/real/aocs/reaction_wheel.hpp | 14 ++++----- .../real/aocs/reaction_wheel_jitter.cpp | 2 +- .../real/aocs/reaction_wheel_jitter.hpp | 2 +- .../real/aocs/reaction_wheel_ode.cpp | 2 +- .../real/aocs/reaction_wheel_ode.hpp | 2 +- src/components/real/aocs/star_sensor.cpp | 17 ++++++----- src/components/real/aocs/star_sensor.hpp | 4 +-- src/components/real/aocs/sun_sensor.cpp | 10 +++---- src/components/real/aocs/sun_sensor.hpp | 6 ++-- src/components/real/cdh/c2a_communication.hpp | 2 +- src/components/real/cdh/on_board_computer.cpp | 2 +- src/components/real/cdh/on_board_computer.hpp | 2 +- .../real/cdh/on_board_computer_with_c2a.cpp | 2 +- .../real/cdh/on_board_computer_with_c2a.hpp | 2 +- src/components/real/communication/antenna.cpp | 2 +- src/components/real/communication/antenna.hpp | 2 +- .../antenna_radiation_pattern.cpp | 2 +- .../antenna_radiation_pattern.hpp | 2 +- .../ground_station_calculator.cpp | 10 +++---- .../ground_station_calculator.hpp | 2 +- .../wings_command_sender_to_c2a.cpp | 2 +- .../wings_command_sender_to_c2a.hpp | 6 ++-- src/components/real/mission/telescope.cpp | 11 +++---- src/components/real/mission/telescope.hpp | 10 +++---- src/components/real/power/battery.cpp | 8 ++--- src/components/real/power/battery.hpp | 2 +- .../real/power/csv_scenario_interface.cpp | 2 +- .../real/power/csv_scenario_interface.hpp | 2 +- .../real/power/pcu_initial_study.cpp | 6 ++-- .../real/power/pcu_initial_study.hpp | 2 +- .../real/power/power_control_unit.cpp | 2 +- .../real/power/power_control_unit.hpp | 2 +- .../real/power/solar_array_panel.cpp | 19 ++++++------ .../real/power/solar_array_panel.hpp | 2 +- .../real/propulsion/simple_thruster.cpp | 6 ++-- .../real/propulsion/simple_thruster.hpp | 26 ++++++++--------- src/disturbances/air_drag.cpp | 2 +- src/disturbances/air_drag.hpp | 2 +- src/disturbances/disturbance.hpp | 2 +- src/disturbances/disturbances.cpp | 2 +- src/disturbances/disturbances.hpp | 2 +- src/disturbances/geopotential.cpp | 2 +- src/disturbances/geopotential.hpp | 2 +- src/disturbances/gravity_gradient.cpp | 2 +- src/disturbances/gravity_gradient.hpp | 2 +- src/disturbances/lunar_gravity_field.cpp | 2 +- src/disturbances/lunar_gravity_field.hpp | 2 +- src/disturbances/magnetic_disturbance.cpp | 4 +-- src/disturbances/magnetic_disturbance.hpp | 2 +- .../solar_radiation_pressure_disturbance.cpp | 2 +- .../solar_radiation_pressure_disturbance.hpp | 2 +- src/disturbances/surface_force.cpp | 2 +- src/disturbances/surface_force.hpp | 2 +- src/disturbances/third_body_gravity.cpp | 2 +- src/disturbances/third_body_gravity.hpp | 2 +- src/dynamics/attitude/attitude.cpp | 2 +- src/dynamics/attitude/attitude.hpp | 2 +- src/dynamics/attitude/attitude_rk4.cpp | 2 +- src/dynamics/attitude/attitude_rk4.hpp | 2 +- .../attitude_with_cantilever_vibration.cpp | 2 +- .../attitude_with_cantilever_vibration.hpp | 2 +- src/dynamics/attitude/controlled_attitude.cpp | 2 +- src/dynamics/attitude/controlled_attitude.hpp | 8 ++--- src/dynamics/attitude/initialize_attitude.cpp | 2 +- src/dynamics/attitude/initialize_attitude.hpp | 2 +- ...ode_attitude_with_cantilever_vibration.hpp | 2 +- src/dynamics/dynamics.cpp | 7 +++-- src/dynamics/dynamics.hpp | 24 ++++++++------- .../orbit/encke_orbit_propagation.cpp | 2 +- .../orbit/encke_orbit_propagation.hpp | 6 ++-- src/dynamics/orbit/initialize_orbit.cpp | 5 ++-- src/dynamics/orbit/initialize_orbit.hpp | 6 ++-- .../orbit/kepler_orbit_propagation.cpp | 2 +- .../orbit/kepler_orbit_propagation.hpp | 2 +- src/dynamics/orbit/orbit.cpp | 2 +- src/dynamics/orbit/orbit.hpp | 6 ++-- src/dynamics/orbit/relative_orbit.cpp | 2 +- src/dynamics/orbit/relative_orbit.hpp | 12 ++++---- src/dynamics/orbit/rk4_orbit_propagation.cpp | 2 +- src/dynamics/orbit/rk4_orbit_propagation.hpp | 2 +- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 2 +- src/dynamics/orbit/sgp4_orbit_propagation.hpp | 2 +- src/dynamics/thermal/heater.cpp | 2 +- src/dynamics/thermal/heater.hpp | 2 +- src/dynamics/thermal/heater_controller.cpp | 2 +- src/dynamics/thermal/heater_controller.hpp | 2 +- src/dynamics/thermal/heatload.cpp | 2 +- src/dynamics/thermal/heatload.hpp | 2 +- src/dynamics/thermal/node.cpp | 2 +- src/dynamics/thermal/node.hpp | 3 +- src/dynamics/thermal/temperature.cpp | 2 +- src/dynamics/thermal/temperature.hpp | 17 ++++++----- .../global/celestial_information.cpp | 2 +- .../global/celestial_information.hpp | 2 +- src/environment/global/clock_generator.cpp | 2 +- src/environment/global/clock_generator.hpp | 4 +-- src/environment/global/earth_rotation.cpp | 2 +- src/environment/global/earth_rotation.hpp | 2 +- src/environment/global/global_environment.cpp | 2 +- src/environment/global/global_environment.hpp | 2 +- src/environment/global/gnss_satellites.cpp | 8 ++--- src/environment/global/gnss_satellites.hpp | 19 ++++++------ .../global/hipparcos_catalogue.cpp | 2 +- .../global/hipparcos_catalogue.hpp | 2 +- src/environment/global/moon_rotation.cpp | 2 +- src/environment/global/moon_rotation.hpp | 2 +- src/environment/global/simulation_time.cpp | 2 +- src/environment/global/simulation_time.hpp | 2 +- src/environment/local/atmosphere.cpp | 2 +- src/environment/local/atmosphere.hpp | 6 ++-- src/environment/local/earth_albedo.cpp | 2 +- src/environment/local/earth_albedo.hpp | 2 +- src/environment/local/geomagnetic_field.cpp | 4 +-- src/environment/local/geomagnetic_field.hpp | 2 +- .../local/local_celestial_information.cpp | 2 +- .../local/local_celestial_information.hpp | 2 +- src/environment/local/local_environment.cpp | 4 +-- src/environment/local/local_environment.hpp | 10 +++---- .../solar_radiation_pressure_environment.cpp | 2 +- .../solar_radiation_pressure_environment.hpp | 2 +- src/logger/initialize_log.cpp | 2 +- src/logger/initialize_log.hpp | 2 +- src/logger/log_utility.hpp | 2 +- src/logger/loggable.hpp | 2 +- src/logger/logger.cpp | 2 +- src/logger/logger.hpp | 2 +- .../atmosphere/wrapper_nrlmsise00.cpp | 4 +++ .../atmosphere/wrapper_nrlmsise00.hpp | 4 +++ src/math_physics/gnss/antex_file_reader.hpp | 4 +-- src/math_physics/gnss/sp3_file_reader.hpp | 16 +++++----- .../numerical_integrator.hpp | 2 +- .../numerical_integration/runge_kutta.hpp | 2 +- .../test_runge_kutta.cpp | 4 +-- .../randomization/global_randomization.hpp | 4 +-- .../randomization/random_walk.hpp | 8 +++-- .../random_walk_template_functions.hpp | 4 +++ src/s2e.cpp | 2 +- .../c2a_command_database.cpp | 2 +- .../c2a_command_database.hpp | 2 +- .../initialize_file_access.cpp | 2 +- .../initialize_file_access.hpp | 2 +- .../wings_operation_file.cpp | 2 +- src/simulation/case/simulation_case.cpp | 2 +- src/simulation/case/simulation_case.hpp | 6 ++-- .../ground_station/ground_station.cpp | 2 +- .../ground_station/ground_station.hpp | 6 ++-- src/simulation/hils/hils_port_manager.cpp | 2 +- src/simulation/hils/hils_port_manager.hpp | 2 +- .../initialize_monte_carlo_parameters.cpp | 2 +- .../initialize_monte_carlo_parameters.hpp | 2 +- .../initialize_monte_carlo_simulation.cpp | 2 +- .../initialize_monte_carlo_simulation.hpp | 2 +- .../monte_carlo_simulation_executor.cpp | 2 +- .../monte_carlo_simulation_executor.hpp | 2 +- .../simulation_object.cpp | 2 +- .../simulation_object.hpp | 2 +- .../inter_spacecraft_communication.cpp | 2 +- .../inter_spacecraft_communication.hpp | 2 +- .../relative_information.cpp | 2 +- .../relative_information.hpp | 2 +- src/simulation/simulation_configuration.hpp | 4 +-- .../spacecraft/installed_components.cpp | 2 +- .../spacecraft/installed_components.hpp | 2 +- src/simulation/spacecraft/spacecraft.cpp | 10 +++---- src/simulation/spacecraft/spacecraft.hpp | 22 +++++++------- .../structure/initialize_structure.cpp | 2 +- .../structure/initialize_structure.hpp | 2 +- .../structure/kinematics_parameters.cpp | 2 +- .../structure/kinematics_parameters.hpp | 4 +-- .../structure/residual_magnetic_moment.cpp | 2 +- .../structure/residual_magnetic_moment.hpp | 10 +++---- .../spacecraft/structure/structure.cpp | 2 +- .../spacecraft/structure/structure.hpp | 2 +- .../spacecraft/structure/surface.cpp | 2 +- .../spacecraft/structure/surface.hpp | 4 +-- src/simulation_sample/case/sample_case.cpp | 2 +- src/simulation_sample/case/sample_case.hpp | 2 +- .../ground_station/sample_ground_station.cpp | 2 +- .../ground_station/sample_ground_station.hpp | 2 +- .../sample_ground_station_components.cpp | 2 +- .../sample_ground_station_components.hpp | 2 +- .../spacecraft/sample_components.cpp | 9 +++--- .../spacecraft/sample_components.hpp | 13 +++++---- .../spacecraft/sample_port_configuration.hpp | 2 +- .../spacecraft/sample_spacecraft.cpp | 6 ++-- .../spacecraft/sample_spacecraft.hpp | 2 +- src/utilities/com_port_interface.hpp | 2 +- src/utilities/endian.cpp | 2 +- src/utilities/endian.hpp | 2 +- src/utilities/macros.hpp | 2 +- src/utilities/quantization.hpp | 2 +- src/utilities/ring_buffer.cpp | 2 +- src/utilities/ring_buffer.hpp | 2 +- src/utilities/slip.cpp | 2 +- src/utilities/slip.hpp | 2 +- 233 files changed, 509 insertions(+), 458 deletions(-) diff --git a/src/components/base/component.cpp b/src/components/base/component.cpp index 65f9f88ae..469c4e981 100644 --- a/src/components/base/component.cpp +++ b/src/components/base/component.cpp @@ -15,7 +15,8 @@ Component::Component(const unsigned int prescaler, environment::ClockGenerator* fast_prescaler_ = (fast_prescaler > 0) ? fast_prescaler : 1; } -Component::Component(const unsigned int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const unsigned int fast_prescaler) +Component::Component(const unsigned int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, + const unsigned int fast_prescaler) : clock_generator_(clock_generator), power_port_(power_port) { clock_generator_->RegisterComponent(this); prescaler_ = (prescaler > 0) ? prescaler : 1; @@ -51,4 +52,4 @@ void Component::FastTick(const unsigned int fast_count) { } } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/base/component.hpp b/src/components/base/component.hpp index 25c035415..2cfc64db5 100644 --- a/src/components/base/component.hpp +++ b/src/components/base/component.hpp @@ -78,18 +78,18 @@ class Component : public ITickable { * @brief Pure virtual function used to calculate high-frequency disturbances(e.g. RW jitter) * @note Override only when high-frequency disturbances need to be calculated. */ - virtual void FastUpdate(){}; + virtual void FastUpdate() {}; /** * @fn PowerOffRoutine * @brief Pure virtual function executed when the power switch is off. */ - virtual void PowerOffRoutine(){}; + virtual void PowerOffRoutine() {}; environment::ClockGenerator* clock_generator_; //!< Clock generator - PowerPort* power_port_; //!< Power port + PowerPort* power_port_; //!< Power port }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_BASE_COMPONENT_HPP_ diff --git a/src/components/base/gpio_connection_with_obc.cpp b/src/components/base/gpio_connection_with_obc.cpp index 7b87659b0..ceae6170b 100644 --- a/src/components/base/gpio_connection_with_obc.cpp +++ b/src/components/base/gpio_connection_with_obc.cpp @@ -20,4 +20,4 @@ bool GpioConnectionWithObc::Read(const int index) { return obc_->GpioComponentRe void GpioConnectionWithObc::Write(const int index, const bool is_high) { obc_->GpioComponentWrite(port_id_[index], is_high); } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/base/gpio_connection_with_obc.hpp b/src/components/base/gpio_connection_with_obc.hpp index b066b1f38..752d36d5b 100644 --- a/src/components/base/gpio_connection_with_obc.hpp +++ b/src/components/base/gpio_connection_with_obc.hpp @@ -52,6 +52,6 @@ class GpioConnectionWithObc { OnBoardComputer* obc_; //!< The communication target OBC }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_BASE_GPIO_CONNECTION_WITH_OBC_HPP_ diff --git a/src/components/base/i2c_controller.cpp b/src/components/base/i2c_controller.cpp index 400c88ae3..48a6850d7 100644 --- a/src/components/base/i2c_controller.cpp +++ b/src/components/base/i2c_controller.cpp @@ -7,7 +7,6 @@ #include #include - namespace s2e::components { I2cController::I2cController(const unsigned int hils_port_id, const unsigned int baud_rate, const unsigned int tx_buffer_size, @@ -52,4 +51,4 @@ int I2cController::SendCommand(const unsigned char length) { return hils_port_manager_->I2cControllerSend(hils_port_id_, &tx_buffer_.front(), 0, length); } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/base/i2c_target_communication_with_obc.hpp b/src/components/base/i2c_target_communication_with_obc.hpp index 61a1b3e34..5fa5bf279 100644 --- a/src/components/base/i2c_target_communication_with_obc.hpp +++ b/src/components/base/i2c_target_communication_with_obc.hpp @@ -119,7 +119,7 @@ class I2cTargetCommunicationWithObc { SimulationMode simulation_mode_ = SimulationMode::kError; //!< Simulation mode - OnBoardComputer* obc_; //!< Communication target OBC + OnBoardComputer* obc_; //!< Communication target OBC simulation::HilsPortManager* hils_port_manager_; //!< HILS port manager }; diff --git a/src/components/base/interface_gpio_component.hpp b/src/components/base/interface_gpio_component.hpp index 9df3db260..6a5d3bb75 100644 --- a/src/components/base/interface_gpio_component.hpp +++ b/src/components/base/interface_gpio_component.hpp @@ -18,7 +18,7 @@ class IGPIOCompo { * @fn ~IGPIOCompo * @brief Destructor */ - virtual ~IGPIOCompo(){}; + virtual ~IGPIOCompo() {}; /** * @fn GpioStateChanged diff --git a/src/components/base/sensor.hpp b/src/components/base/sensor.hpp index 1091be06b..351a1275d 100644 --- a/src/components/base/sensor.hpp +++ b/src/components/base/sensor.hpp @@ -54,11 +54,11 @@ class Sensor { math::Vector Measure(const math::Vector true_value_c); private: - math::Matrix scale_factor_; //!< Scale factor matrix - math::Vector range_to_const_c_; //!< Output range limit to be constant output value at the component frame - math::Vector range_to_zero_c_; //!< Output range limit to be zero output value at the component frame + math::Matrix scale_factor_; //!< Scale factor matrix + math::Vector range_to_const_c_; //!< Output range limit to be constant output value at the component frame + math::Vector range_to_zero_c_; //!< Output range limit to be zero output value at the component frame s2e::randomization::NormalRand normal_random_noise_c_[N]; //!< Normal random - RandomWalk random_walk_noise_c_; //!< Random Walk + RandomWalk random_walk_noise_c_; //!< Random Walk /** * @fn Clip diff --git a/src/components/base/uart_communication_with_obc.cpp b/src/components/base/uart_communication_with_obc.cpp index 075b67944..74a7f92e6 100644 --- a/src/components/base/uart_communication_with_obc.cpp +++ b/src/components/base/uart_communication_with_obc.cpp @@ -35,7 +35,8 @@ UartCommunicationWithObc::UartCommunicationWithObc(const unsigned int sils_port_ InitializeObcComBase(); } -UartCommunicationWithObc::UartCommunicationWithObc(const unsigned int hils_port_id, const unsigned int baud_rate, simulation::HilsPortManager* hils_port_manager) +UartCommunicationWithObc::UartCommunicationWithObc(const unsigned int hils_port_id, const unsigned int baud_rate, + simulation::HilsPortManager* hils_port_manager) : hils_port_id_(hils_port_id), baud_rate_(baud_rate), hils_port_manager_(hils_port_manager) { #ifdef USE_HILS simulation_mode_ = SimulationMode::kHils; diff --git a/src/components/base/uart_communication_with_obc.hpp b/src/components/base/uart_communication_with_obc.hpp index caad9f1c9..ec887a685 100644 --- a/src/components/base/uart_communication_with_obc.hpp +++ b/src/components/base/uart_communication_with_obc.hpp @@ -109,7 +109,7 @@ class UartCommunicationWithObc { SimulationMode simulation_mode_ = SimulationMode::kError; //!< Simulation mode - OnBoardComputer* obc_; //!< Communication target OBC + OnBoardComputer* obc_; //!< Communication target OBC simulation::HilsPortManager* hils_port_manager_; //!< HILS port manager /** diff --git a/src/components/examples/example_i2c_controller_for_hils.cpp b/src/components/examples/example_i2c_controller_for_hils.cpp index c67f5fe8e..892268768 100644 --- a/src/components/examples/example_i2c_controller_for_hils.cpp +++ b/src/components/examples/example_i2c_controller_for_hils.cpp @@ -6,9 +6,10 @@ namespace s2e::components { -ExampleI2cControllerForHils::ExampleI2cControllerForHils(const int prescaler, environment::ClockGenerator* clock_generator, const unsigned int hils_port_id, - const unsigned int baud_rate, const unsigned int tx_buffer_size, - const unsigned int rx_buffer_size, simulation::HilsPortManager* hils_port_manager) +ExampleI2cControllerForHils::ExampleI2cControllerForHils(const int prescaler, environment::ClockGenerator* clock_generator, + const unsigned int hils_port_id, const unsigned int baud_rate, + const unsigned int tx_buffer_size, const unsigned int rx_buffer_size, + simulation::HilsPortManager* hils_port_manager) : Component(prescaler, clock_generator), I2cController(hils_port_id, baud_rate, tx_buffer_size, rx_buffer_size, hils_port_manager) {} ExampleI2cControllerForHils::~ExampleI2cControllerForHils() {} diff --git a/src/components/examples/example_i2c_controller_for_hils.hpp b/src/components/examples/example_i2c_controller_for_hils.hpp index 84537cf6a..84254633f 100644 --- a/src/components/examples/example_i2c_controller_for_hils.hpp +++ b/src/components/examples/example_i2c_controller_for_hils.hpp @@ -33,8 +33,9 @@ class ExampleI2cControllerForHils : public Component, public I2cController { * @param [in] rx_buffer_size: RX (Target to Controller) buffer size * @param [in] hils_port_manager: HILS port manager */ - ExampleI2cControllerForHils(const int prescaler, environment::ClockGenerator* clock_generator, const unsigned int hils_port_id, const unsigned int baud_rate, - const unsigned int tx_buffer_size, const unsigned int rx_buffer_size, simulation::HilsPortManager* hils_port_manager); + ExampleI2cControllerForHils(const int prescaler, environment::ClockGenerator* clock_generator, const unsigned int hils_port_id, + const unsigned int baud_rate, const unsigned int tx_buffer_size, const unsigned int rx_buffer_size, + simulation::HilsPortManager* hils_port_manager); /** * @fn ~ExampleI2cControllerForHils * @brief Destructor diff --git a/src/components/examples/example_serial_communication_for_hils.cpp b/src/components/examples/example_serial_communication_for_hils.cpp index 3c84e0b2b..f39bc91fd 100644 --- a/src/components/examples/example_serial_communication_for_hils.cpp +++ b/src/components/examples/example_serial_communication_for_hils.cpp @@ -8,9 +8,10 @@ namespace s2e::components { -ExampleSerialCommunicationForHils::ExampleSerialCommunicationForHils(environment::ClockGenerator* clock_generator, const int sils_port_id, OnBoardComputer* obc, - const unsigned int hils_port_id, const unsigned int baud_rate, - simulation::HilsPortManager* hils_port_manager, const int mode_id) +ExampleSerialCommunicationForHils::ExampleSerialCommunicationForHils(environment::ClockGenerator* clock_generator, const int sils_port_id, + OnBoardComputer* obc, const unsigned int hils_port_id, + const unsigned int baud_rate, simulation::HilsPortManager* hils_port_manager, + const int mode_id) : Component(300, clock_generator), UartCommunicationWithObc(sils_port_id, obc, hils_port_id, baud_rate, hils_port_manager), mode_id_(mode_id) {} ExampleSerialCommunicationForHils::~ExampleSerialCommunicationForHils() {} diff --git a/src/components/examples/example_serial_communication_for_hils.hpp b/src/components/examples/example_serial_communication_for_hils.hpp index aeac14f2e..077c9fc30 100644 --- a/src/components/examples/example_serial_communication_for_hils.hpp +++ b/src/components/examples/example_serial_communication_for_hils.hpp @@ -36,8 +36,9 @@ class ExampleSerialCommunicationForHils : public Component, public UartCommunica * @param [in] hils_port_manager: HILS port manager * @param [in] mode_id: Mode ID to select sender(0) or responder(1) */ - ExampleSerialCommunicationForHils(environment::ClockGenerator* clock_generator, const int sils_port_id, OnBoardComputer* obc, const unsigned int hils_port_id, - const unsigned int baud_rate, simulation::HilsPortManager* hils_port_manager, const int mode_id); + ExampleSerialCommunicationForHils(environment::ClockGenerator* clock_generator, const int sils_port_id, OnBoardComputer* obc, + const unsigned int hils_port_id, const unsigned int baud_rate, simulation::HilsPortManager* hils_port_manager, + const int mode_id); /** * @fn ~ExampleSerialCommunicationForHils * @brief Destructor diff --git a/src/components/ideal/angular_velocity_observer.cpp b/src/components/ideal/angular_velocity_observer.cpp index 94a12c467..3b72b890b 100644 --- a/src/components/ideal/angular_velocity_observer.cpp +++ b/src/components/ideal/angular_velocity_observer.cpp @@ -9,7 +9,8 @@ namespace s2e::components { -AngularVelocityObserver::AngularVelocityObserver(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const dynamics::attitude::Attitude& attitude) +AngularVelocityObserver::AngularVelocityObserver(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, + const dynamics::attitude::Attitude& attitude) : Component(prescaler, clock_generator), Sensor(sensor_base), attitude_(attitude) {} void AngularVelocityObserver::MainRoutine(const int time_count) { @@ -34,8 +35,8 @@ std::string AngularVelocityObserver::GetLogValue() const { return str_tmp; } -AngularVelocityObserver InitializeAngularVelocityObserver(environment::ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s, - const dynamics::attitude::Attitude& attitude) { +AngularVelocityObserver InitializeAngularVelocityObserver(environment::ClockGenerator* clock_generator, const std::string file_name, + double component_step_time_s, const dynamics::attitude::Attitude& attitude) { setting_file_reader::IniAccess ini_file(file_name); int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); diff --git a/src/components/ideal/angular_velocity_observer.hpp b/src/components/ideal/angular_velocity_observer.hpp index a99c0cb13..5f8f10e22 100644 --- a/src/components/ideal/angular_velocity_observer.hpp +++ b/src/components/ideal/angular_velocity_observer.hpp @@ -28,7 +28,8 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public logge * @param [in] sensor_base: Sensor base information * @param [in] dynamics: Dynamics information */ - AngularVelocityObserver(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const dynamics::attitude::Attitude& attitude); + AngularVelocityObserver(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, + const dynamics::attitude::Attitude& attitude); /** * @fn ~AngularVelocityObserver * @brief Destructor @@ -63,7 +64,7 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public logge protected: math::Vector<3> angular_velocity_b_rad_s_{0.0}; //!< Observed angular velocity [rad/s] - const dynamics::attitude::Attitude& attitude_; //!< Dynamics information + const dynamics::attitude::Attitude& attitude_; //!< Dynamics information }; /** @@ -74,8 +75,8 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public logge * @param [in] component_step_time_s: Component step time [sec] * @param [in] dynamics: Dynamics information */ -AngularVelocityObserver InitializeAngularVelocityObserver(environment::ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s, - const dynamics::attitude::Attitude& attitude); +AngularVelocityObserver InitializeAngularVelocityObserver(environment::ClockGenerator* clock_generator, const std::string file_name, + double component_step_time_s, const dynamics::attitude::Attitude& attitude); } // namespace s2e::components diff --git a/src/components/ideal/attitude_observer.cpp b/src/components/ideal/attitude_observer.cpp index 9d3a84165..a77c1b429 100644 --- a/src/components/ideal/attitude_observer.cpp +++ b/src/components/ideal/attitude_observer.cpp @@ -49,7 +49,8 @@ std::string AttitudeObserver::GetLogValue() const { return str_tmp; } -AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::attitude::Attitude& attitude) { +AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_generator, const std::string file_name, + const dynamics::attitude::Attitude& attitude) { // General setting_file_reader::IniAccess ini_file(file_name); diff --git a/src/components/ideal/attitude_observer.hpp b/src/components/ideal/attitude_observer.hpp index 148b3aa4f..0dfd29fe7 100644 --- a/src/components/ideal/attitude_observer.hpp +++ b/src/components/ideal/attitude_observer.hpp @@ -28,7 +28,8 @@ class AttitudeObserver : public Component, public logger::ILoggable { * @param [in] clock_generator: Clock generator * @param [in] attitude: dynamics::attitude::Attitude information */ - AttitudeObserver(const int prescaler, environment::ClockGenerator* clock_generator, const double standard_deviation_rad, const dynamics::attitude::Attitude& attitude); + AttitudeObserver(const int prescaler, environment::ClockGenerator* clock_generator, const double standard_deviation_rad, + const dynamics::attitude::Attitude& attitude); /** * @fn ~AttitudeObserver @@ -77,7 +78,8 @@ class AttitudeObserver : public Component, public logger::ILoggable { * @param [in] file_name: Path to the initialize file * @param [in] attitude: dynamics::attitude::Attitude information */ -AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::attitude::Attitude& attitude); +AttitudeObserver InitializeAttitudeObserver(environment::ClockGenerator* clock_generator, const std::string file_name, + const dynamics::attitude::Attitude& attitude); } // namespace s2e::components diff --git a/src/components/ideal/force_generator.cpp b/src/components/ideal/force_generator.cpp index 2bac42d3f..2104ad4b9 100644 --- a/src/components/ideal/force_generator.cpp +++ b/src/components/ideal/force_generator.cpp @@ -107,7 +107,8 @@ math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(math::Vector<3 return error_quaternion; } -ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::Dynamics* dynamics) { +ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, + const dynamics::Dynamics* dynamics) { // General setting_file_reader::IniAccess ini_file(file_name); diff --git a/src/components/ideal/force_generator.hpp b/src/components/ideal/force_generator.hpp index 61f94d4d9..b5d254414 100644 --- a/src/components/ideal/force_generator.hpp +++ b/src/components/ideal/force_generator.hpp @@ -102,9 +102,9 @@ class ForceGenerator : public Component, public logger::ILoggable { math::Vector<3> generated_force_rtn_N_{0.0}; //!< Generated force in the RTN frame [N] // Noise - s2e::randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise - s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise - double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad] + s2e::randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise + s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise + double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad] /** * @fn GenerateDirectionNoiseQuaternion @@ -124,7 +124,8 @@ class ForceGenerator : public Component, public logger::ILoggable { * @param [in] file_name: Path to initialize file * @param [in] dynamics: Dynamics information */ -ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::Dynamics* dynamics); +ForceGenerator InitializeForceGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, + const dynamics::Dynamics* dynamics); } // namespace s2e::components diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 953184fe4..6e56fdd59 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -39,8 +39,8 @@ class OrbitObserver : public Component, public logger::ILoggable { * @param [in] error_standard_deviation: Position and Velocity standard deviation noise [m, m/s] * @param [in] orbit: Orbit information */ - OrbitObserver(const int prescaler, environment::ClockGenerator* clock_generator, const NoiseFrame noise_frame, const math::Vector<6> error_standard_deviation, - const Orbit& orbit); + OrbitObserver(const int prescaler, environment::ClockGenerator* clock_generator, const NoiseFrame noise_frame, + const math::Vector<6> error_standard_deviation, const Orbit& orbit); /** * @fn ~AttitudeObserver @@ -83,7 +83,7 @@ class OrbitObserver : public Component, public logger::ILoggable { math::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] math::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] - NoiseFrame noise_frame_; //!< Noise definition frame + NoiseFrame noise_frame_; //!< Noise definition frame s2e::randomization::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] // Observed variables @@ -107,6 +107,6 @@ NoiseFrame SetNoiseFrame(const std::string noise_frame); */ OrbitObserver InitializeOrbitObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ diff --git a/src/components/ideal/torque_generator.cpp b/src/components/ideal/torque_generator.cpp index aef542808..c64478b6b 100644 --- a/src/components/ideal/torque_generator.cpp +++ b/src/components/ideal/torque_generator.cpp @@ -11,8 +11,9 @@ namespace s2e::components { // Constructor -TorqueGenerator::TorqueGenerator(const int prescaler, environment::ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_Nm, - const double direction_error_standard_deviation_rad, const dynamics::Dynamics* dynamics) +TorqueGenerator::TorqueGenerator(const int prescaler, environment::ClockGenerator* clock_generator, + const double magnitude_error_standard_deviation_Nm, const double direction_error_standard_deviation_rad, + const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator), magnitude_noise_(0.0, magnitude_error_standard_deviation_Nm), direction_error_standard_deviation_rad_(direction_error_standard_deviation_rad), @@ -80,7 +81,8 @@ math::Quaternion TorqueGenerator::GenerateDirectionNoiseQuaternion(math::Vector< return error_quaternion; } -TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::Dynamics* dynamics) { +TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, + const dynamics::Dynamics* dynamics) { // General setting_file_reader::IniAccess ini_file(file_name); diff --git a/src/components/ideal/torque_generator.hpp b/src/components/ideal/torque_generator.hpp index 1c4965b11..b94a54919 100644 --- a/src/components/ideal/torque_generator.hpp +++ b/src/components/ideal/torque_generator.hpp @@ -80,9 +80,9 @@ class TorqueGenerator : public Component, public logger::ILoggable { math::Vector<3> generated_torque_b_Nm_{0.0}; //!< Generated torque in the body fixed frame [Nm] // Noise - s2e::randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise - s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise - double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad] + s2e::randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise + s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise + double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad] /** * @fn GenerateDirectionNoiseQuaternion @@ -102,7 +102,8 @@ class TorqueGenerator : public Component, public logger::ILoggable { * @param [in] file_name: Path to initialize file * @param [in] dynamics: Dynamics information */ -TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::Dynamics* dynamics); +TorqueGenerator InitializeTorqueGenerator(environment::ClockGenerator* clock_generator, const std::string file_name, + const dynamics::Dynamics* dynamics); } // namespace s2e::components diff --git a/src/components/ports/i2c_port.cpp b/src/components/ports/i2c_port.cpp index 8b4e1cfa1..3e4578a28 100644 --- a/src/components/ports/i2c_port.cpp +++ b/src/components/ports/i2c_port.cpp @@ -96,4 +96,4 @@ unsigned char I2cPort::ReadCommand(const unsigned char i2c_address, unsigned cha return length; } -} // namespace s2e::components \ No newline at end of file +} // namespace s2e::components \ No newline at end of file diff --git a/src/components/ports/power_port.cpp b/src/components/ports/power_port.cpp index 3964bb674..5671b94e9 100644 --- a/src/components/ports/power_port.cpp +++ b/src/components/ports/power_port.cpp @@ -77,4 +77,4 @@ void PowerPort::InitializeWithInitializeFile(const std::string file_name) { this->SetAssumedPowerConsumption_W(assumed_power_consumption_W); } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/ports/uart_port.cpp b/src/components/ports/uart_port.cpp index a88f98699..6969b9361 100644 --- a/src/components/ports/uart_port.cpp +++ b/src/components/ports/uart_port.cpp @@ -39,4 +39,4 @@ int UartPort::ReadRx(unsigned char* buffer, const unsigned int offset, const uns return rx_buffer_->Read(buffer, offset, data_length); } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/ports/uart_port.hpp b/src/components/ports/uart_port.hpp index eb5e6683c..2d00a0101 100644 --- a/src/components/ports/uart_port.hpp +++ b/src/components/ports/uart_port.hpp @@ -80,6 +80,6 @@ class UartPort { RingBuffer* tx_buffer_; //!< Transmit buffer (OBC-> Component) }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_PORTS_UART_PORT_HPP_ diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 033a8db5b..e6b528f94 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -13,9 +13,9 @@ namespace s2e::components { -GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, - const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg, - const math::Vector<3> position_noise_standard_deviation_ecef_m, +GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id, + const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, + const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m, const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator), @@ -28,8 +28,10 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) { for (size_t i = 0; i < 3; i++) { - position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], s2e::randomization::global_randomization.MakeSeed()); - velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], s2e::randomization::global_randomization.MakeSeed()); + position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], + s2e::randomization::global_randomization.MakeSeed()); + velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], + s2e::randomization::global_randomization.MakeSeed()); } } @@ -48,8 +50,10 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) { for (size_t i = 0; i < 3; i++) { - position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], s2e::randomization::global_randomization.MakeSeed()); - velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], s2e::randomization::global_randomization.MakeSeed()); + position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], + s2e::randomization::global_randomization.MakeSeed()); + velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], + s2e::randomization::global_randomization.MakeSeed()); } } @@ -291,8 +295,8 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat return gnss_receiver_param; } -GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const dynamics::Dynamics* dynamics, - const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { +GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, + const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.antenna_model, gr_param.antenna_pos_b, gr_param.quaternion_b2c, @@ -301,8 +305,9 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, cons return gnss_r; } -GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, - const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { +GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, + const std::string file_name, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, + const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); // PowerPort @@ -314,4 +319,4 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, Powe return gnss_r; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index aa2e54f09..2661d856b 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -83,8 +83,8 @@ class GnssReceiver : public Component, public logger::ILoggable { GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m, - const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, - const SimulationTime* simulation_time); + const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, + const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); // Override functions for Component /** @@ -141,8 +141,8 @@ class GnssReceiver : public Component, public logger::ILoggable { // Simple position observation s2e::randomization::NormalRand position_random_noise_ecef_m_[3]; //!< Random noise for position at the ECEF frame [m] s2e::randomization::NormalRand velocity_random_noise_ecef_m_s_[3]; //!< Random noise for velocity at the ECEF frame [m] - math::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] - math::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] + math::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] + math::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] s2e::geodesy::GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame // Time observation @@ -156,7 +156,7 @@ class GnssReceiver : public Component, public logger::ILoggable { std::vector gnss_information_list_; //!< Information List of visible GNSS satellites // References - const dynamics::Dynamics* dynamics_; //!< Dynamics of spacecraft + const dynamics::Dynamics* dynamics_; //!< Dynamics of spacecraft const GnssSatellites* gnss_satellites_; //!< Information of GNSS satellites const SimulationTime* simulation_time_; //!< Simulation time @@ -226,8 +226,8 @@ AntennaModel SetAntennaModel(const std::string antenna_model); * @param [in] gnss_satellites: GNSS satellites information * @param [in] simulation_time: Simulation time information */ -GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const dynamics::Dynamics* dynamics, - const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); +GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, + const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn InitGnssReceiver * @brief Initialize functions for GNSS Receiver with power port @@ -239,9 +239,10 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, cons * @param [in] gnss_satellites: GNSS satellites information * @param [in] simulation_time: Simulation time information */ -GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, - const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); +GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, + const std::string file_name, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, + const SimulationTime* simulation_time); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_AOCS_GNSS_RECEIVER_HPP_ diff --git a/src/components/real/aocs/gyro_sensor.cpp b/src/components/real/aocs/gyro_sensor.cpp index ac63221d7..5a019cd5a 100644 --- a/src/components/real/aocs/gyro_sensor.cpp +++ b/src/components/real/aocs/gyro_sensor.cpp @@ -13,8 +13,8 @@ GyroSensor::GyroSensor(const int prescaler, environment::ClockGenerator* clock_g const math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), dynamics_(dynamics) {} -GyroSensor::GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics) +GyroSensor::GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, + const unsigned int sensor_id, const math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator, power_port), Sensor(sensor_base), sensor_id_(sensor_id), @@ -91,4 +91,4 @@ GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, PowerPor return gyro; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/aocs/gyro_sensor.hpp b/src/components/real/aocs/gyro_sensor.hpp index d9f262655..ab75c929e 100644 --- a/src/components/real/aocs/gyro_sensor.hpp +++ b/src/components/real/aocs/gyro_sensor.hpp @@ -46,8 +46,8 @@ class GyroSensor : public Component, public Sensor, public logge * @param [in] quaternion_b2c: Quaternion from body frame to component frame * @param [in] dynamics: Dynamics information */ - GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics); + GyroSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, + const unsigned int sensor_id, const math::Quaternion& quaternion_b2c, const dynamics::Dynamics* dynamics); /** * @fn ~GyroSensor * @brief Destructor @@ -112,6 +112,6 @@ GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, int sens GyroSensor InitGyroSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const dynamics::Dynamics* dynamics); -} //namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_AOCS_GYRO_SENSOR_HPP_ diff --git a/src/components/real/aocs/magnetometer.cpp b/src/components/real/aocs/magnetometer.cpp index 275d7fb1f..a709c91b1 100644 --- a/src/components/real/aocs/magnetometer.cpp +++ b/src/components/real/aocs/magnetometer.cpp @@ -16,8 +16,8 @@ Magnetometer::Magnetometer(int prescaler, environment::ClockGenerator* clock_gen sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), geomagnetic_field_(geomagnetic_field) {} -Magnetometer::Magnetometer(int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) +Magnetometer::Magnetometer(int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, + const unsigned int sensor_id, const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator, power_port), Sensor(sensor_base), sensor_id_(sensor_id), @@ -94,4 +94,4 @@ Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, Powe return magsensor; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/aocs/magnetometer.hpp b/src/components/real/aocs/magnetometer.hpp index 896f24a3b..7a52c30cd 100644 --- a/src/components/real/aocs/magnetometer.hpp +++ b/src/components/real/aocs/magnetometer.hpp @@ -46,8 +46,8 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] quaternion_b2c: Quaternion from body frame to component frame * @param [in] geomagnetic_field: Geomagnetic environment */ - Magnetometer(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); + Magnetometer(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, + const unsigned int sensor_id, const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); /** * @fn ~Magnetometer * @brief Destructor @@ -131,6 +131,6 @@ Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, int Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_AOCS_MAGNETOMETER_HPP_ diff --git a/src/components/real/aocs/magnetorquer.cpp b/src/components/real/aocs/magnetorquer.cpp index 7d1352986..2dcbe762a 100644 --- a/src/components/real/aocs/magnetorquer.cpp +++ b/src/components/real/aocs/magnetorquer.cpp @@ -13,8 +13,8 @@ namespace s2e::components { -Magnetorquer::Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, - const math::Matrix& scale_factor, +Magnetorquer::Magnetorquer(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, + const math::Quaternion& quaternion_b2c, const math::Matrix& scale_factor, const math::Vector& max_magnetic_moment_c_Am2, const math::Vector& min_magnetic_moment_c_Am2, const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, const math::Vector& random_walk_standard_deviation_c_Am2, @@ -113,8 +113,8 @@ std::string Magnetorquer::GetLogValue() const { return str_tmp; } -Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int actuator_id, const std::string file_name, double component_step_time_s, - const GeomagneticField* geomagnetic_field) { +Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int actuator_id, const std::string file_name, + double component_step_time_s, const GeomagneticField* geomagnetic_field) { setting_file_reader::IniAccess magtorquer_conf(file_name); const char* sensor_name = "MAGNETORQUER_"; const std::string section_name = sensor_name + std::to_string(static_cast(actuator_id)); @@ -206,4 +206,4 @@ Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, Powe return magtorquer; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/aocs/magnetorquer.hpp b/src/components/real/aocs/magnetorquer.hpp index cec1294dd..f18dec450 100644 --- a/src/components/real/aocs/magnetorquer.hpp +++ b/src/components/real/aocs/magnetorquer.hpp @@ -134,8 +134,8 @@ class Magnetorquer : public Component, public logger::ILoggable { math::Vector max_magnetic_moment_c_Am2_{100.0}; //!< Maximum magnetic moment in the component frame [Am2] math::Vector min_magnetic_moment_c_Am2_{-100.0}; //!< Minimum magnetic moment in the component frame [Am2] - math::Vector bias_noise_c_Am2_{0.0}; //!< Constant bias noise in the component frame [Am2] - RandomWalk random_walk_c_Am2_; //!< Random walk noise + math::Vector bias_noise_c_Am2_{0.0}; //!< Constant bias noise in the component frame [Am2] + RandomWalk random_walk_c_Am2_; //!< Random walk noise s2e::randomization::NormalRand random_noise_c_Am2_[kMtqDimension]; //!< Normal random noise const GeomagneticField* geomagnetic_field_; //!< Geomagnetic environment @@ -157,8 +157,8 @@ class Magnetorquer : public Component, public logger::ILoggable { * @param [in] component_step_time_s: Component step time [sec] * @param [in] geomagnetic_field: Geomegnetic environment */ -Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int actuator_id, const std::string file_name, double component_step_time_s, - const GeomagneticField* geomagnetic_field); +Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int actuator_id, const std::string file_name, + double component_step_time_s, const GeomagneticField* geomagnetic_field); /** * @fn InitMagnetorquer * @brief Initialize functions for magnetometer with power port @@ -172,6 +172,6 @@ Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, const std::string file_name, double component_step_time_s, const GeomagneticField* geomagnetic_field); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_AOCS_MAGNETORQUER_HPP_ diff --git a/src/components/real/aocs/mtq_magnetometer_interference.cpp b/src/components/real/aocs/mtq_magnetometer_interference.cpp index 249c6f299..c2428b15b 100644 --- a/src/components/real/aocs/mtq_magnetometer_interference.cpp +++ b/src/components/real/aocs/mtq_magnetometer_interference.cpp @@ -52,4 +52,4 @@ void MtqMagnetometerInterference::UpdateInterference(void) { previous_added_bias_c_nT_ = additional_bias_c_nT; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/aocs/mtq_magnetometer_interference.hpp b/src/components/real/aocs/mtq_magnetometer_interference.hpp index f468fb497..67c5f0863 100644 --- a/src/components/real/aocs/mtq_magnetometer_interference.hpp +++ b/src/components/real/aocs/mtq_magnetometer_interference.hpp @@ -40,6 +40,6 @@ class MtqMagnetometerInterference { const Magnetorquer& magnetorquer_; //!< Magnetorquer }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_AOCS_MTQ_MAGNETOMETER_INTERFERENCE_HPP_ diff --git a/src/components/real/aocs/reaction_wheel.cpp b/src/components/real/aocs/reaction_wheel.cpp index 5494ca4ee..9cdbac455 100644 --- a/src/components/real/aocs/reaction_wheel.cpp +++ b/src/components/real/aocs/reaction_wheel.cpp @@ -356,4 +356,4 @@ ReactionWheel InitReactionWheel(environment::ClockGenerator* clock_generator, Po return rw; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/aocs/reaction_wheel.hpp b/src/components/real/aocs/reaction_wheel.hpp index c20166028..e59f704b2 100644 --- a/src/components/real/aocs/reaction_wheel.hpp +++ b/src/components/real/aocs/reaction_wheel.hpp @@ -80,9 +80,9 @@ class ReactionWheel : public Component, public logger::ILoggable { * @param [in] drive_flag: RW drive flag * @param [in] init_velocity_rad_s: Initial value of angular velocity of RW [rad/s] */ - ReactionWheel(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const double step_width_s, - const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, const math::Quaternion quaternion_b2c, - const math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, + ReactionWheel(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, + const double step_width_s, const double rotor_inertia_kgm2, const double max_torque_Nm, const double max_velocity_rpm, + const math::Quaternion quaternion_b2c, const math::Vector<3> position_b_m, const double dead_time_s, const double time_constant_s, const std::vector friction_coefficients, const double stop_limit_angular_velocity_rad_s, const bool is_calc_jitter_enabled, const bool is_log_jitter_enabled, const int fast_prescaler, ReactionWheelJitter& rw_jitter, const bool drive_flag = false, const double init_velocity_rad_s = 0.0); @@ -182,9 +182,9 @@ class ReactionWheel : public Component, public logger::ILoggable { math::Vector<3> rotation_axis_b_; //!< Wheel rotation vector in the body fixed frame. // Parameters for control delay - const double step_width_s_; //!< step width for ReactionWheelOde [sec] - const double dead_time_s_; //!< dead time [sec] - std::vector acceleration_delay_buffer_; //!< Delay buffer for acceleration + const double step_width_s_; //!< step width for ReactionWheelOde [sec] + const double dead_time_s_; //!< dead time [sec] + std::vector acceleration_delay_buffer_; //!< Delay buffer for acceleration s2e::control_utilities::FirstOrderLag delayed_acceleration_rad_s2_; //!< Delayed acceleration [rad/s2] // Coasting friction @@ -248,6 +248,6 @@ ReactionWheel InitReactionWheel(environment::ClockGenerator* clock_generator, in ReactionWheel InitReactionWheel(environment::ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, std::string file_name, double compo_update_step_s); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_AOCS_REACTION_WHEEL_HPP_ diff --git a/src/components/real/aocs/reaction_wheel_jitter.cpp b/src/components/real/aocs/reaction_wheel_jitter.cpp index e9ba604df..c7edc497f 100644 --- a/src/components/real/aocs/reaction_wheel_jitter.cpp +++ b/src/components/real/aocs/reaction_wheel_jitter.cpp @@ -129,4 +129,4 @@ void ReactionWheelJitter::CalcCoefficients() { pow(update_interval_s_, 2.0) * pow(structural_resonance_angular_frequency_Hz_, 2.0); } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/aocs/reaction_wheel_jitter.hpp b/src/components/real/aocs/reaction_wheel_jitter.hpp index 88c228b6f..f926754dd 100644 --- a/src/components/real/aocs/reaction_wheel_jitter.hpp +++ b/src/components/real/aocs/reaction_wheel_jitter.hpp @@ -130,6 +130,6 @@ class ReactionWheelJitter { void CalcCoefficients(); }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_AOCS_REACTION_WHEEL_JITTER_HPP_ diff --git a/src/components/real/aocs/reaction_wheel_ode.cpp b/src/components/real/aocs/reaction_wheel_ode.cpp index 621b4e598..fa5b1ac69 100644 --- a/src/components/real/aocs/reaction_wheel_ode.cpp +++ b/src/components/real/aocs/reaction_wheel_ode.cpp @@ -31,4 +31,4 @@ void ReactionWheelOde::DerivativeFunction(double x, const math::Vector<1> &state rhs[0] = angular_acceleration_rad_s2_; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/aocs/reaction_wheel_ode.hpp b/src/components/real/aocs/reaction_wheel_ode.hpp index 82d3888e0..13c0b26f2 100644 --- a/src/components/real/aocs/reaction_wheel_ode.hpp +++ b/src/components/real/aocs/reaction_wheel_ode.hpp @@ -61,6 +61,6 @@ class ReactionWheelOde : public math::OrdinaryDifferentialEquation<1> { double angular_acceleration_rad_s2_ = 0.0; //!< Angular acceleration [rad/s2] }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_AOCS_REACTION_WHEEL_ODE_HPP_ diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index 36b47095a..be7ddd363 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -18,11 +18,12 @@ using namespace s2e::math; namespace s2e::components { -StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, - const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, - const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, - const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, - const double capture_rate_limit_rad_s, const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment) +StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, + const math::Quaternion& quaternion_b2c, const double standard_deviation_orthogonal_direction, + const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, + const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, + const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const dynamics::Dynamics* dynamics, + const LocalEnvironment* local_environment) : Component(prescaler, clock_generator), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -248,8 +249,8 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sens return stt; } -StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const string file_name, double component_step_time_s, - const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment) { +StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const string file_name, + double component_step_time_s, const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment) { setting_file_reader::IniAccess STT_conf(file_name); const char* sensor_name = "STAR_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); @@ -284,4 +285,4 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPor return stt; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/aocs/star_sensor.hpp b/src/components/real/aocs/star_sensor.hpp index e28e0add4..8ad44ae86 100644 --- a/src/components/real/aocs/star_sensor.hpp +++ b/src/components/real/aocs/star_sensor.hpp @@ -138,7 +138,7 @@ class StarSensor : public Component, public logger::ILoggable { double capture_rate_limit_rad_s_; //!< Angular rate limit to get correct attitude [rad/s] // Observed variables - const dynamics::Dynamics* dynamics_; //!< Dynamics information + const dynamics::Dynamics* dynamics_; //!< Dynamics information const LocalEnvironment* local_environment_; //!< Local environment information // Internal functions @@ -234,6 +234,6 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sens StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, double component_step_time_s, const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_AOCS_STAR_SENSOR_HPP_ diff --git a/src/components/real/aocs/sun_sensor.cpp b/src/components/real/aocs/sun_sensor.cpp index 15fe6210a..63a5ea09d 100644 --- a/src/components/real/aocs/sun_sensor.cpp +++ b/src/components/real/aocs/sun_sensor.cpp @@ -16,8 +16,8 @@ using namespace std; namespace s2e::components { -SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, - const double detectable_angle_rad, const double random_noise_standard_deviation_rad, +SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, + const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) : Component(prescaler, clock_generator), @@ -153,8 +153,8 @@ string SunSensor::GetLogValue() const { return str_tmp; } -SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int ss_id, std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, - const LocalCelestialInformation* local_celestial_information) { +SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int ss_id, std::string file_name, + const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) { setting_file_reader::IniAccess ss_conf(file_name); const char* sensor_name = "SUN_SENSOR_"; const std::string section_tmp = sensor_name + std::to_string(static_cast(ss_id)); @@ -221,4 +221,4 @@ SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, PowerPort* return ss; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/aocs/sun_sensor.hpp b/src/components/real/aocs/sun_sensor.hpp index cd23d2d38..4d988d7ac 100644 --- a/src/components/real/aocs/sun_sensor.hpp +++ b/src/components/real/aocs/sun_sensor.hpp @@ -104,8 +104,8 @@ class SunSensor : public Component, public logger::ILoggable { // Noise parameters s2e::randomization::NormalRand random_noise_alpha_; //!< Normal random for alpha angle s2e::randomization::NormalRand random_noise_beta_; //!< Normal random for beta angle - double bias_noise_alpha_rad_ = 0.0; //!< Constant bias for alpha angle (Value is calculated by random number generator) - double bias_noise_beta_rad_ = 0.0; //!< Constant bias for beta angle (Value is calculated by random number generator) + double bias_noise_alpha_rad_ = 0.0; //!< Constant bias for alpha angle (Value is calculated by random number generator) + double bias_noise_beta_rad_ = 0.0; //!< Constant bias for beta angle (Value is calculated by random number generator) // Measured variables const SolarRadiationPressureEnvironment* srp_environment_; //!< Solar Radiation Pressure environment @@ -167,6 +167,6 @@ SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int sensor SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_AOCS_SUN_SENSOR_HPP_ diff --git a/src/components/real/cdh/c2a_communication.hpp b/src/components/real/cdh/c2a_communication.hpp index 4517a3355..ea7afef13 100644 --- a/src/components/real/cdh/c2a_communication.hpp +++ b/src/components/real/cdh/c2a_communication.hpp @@ -25,6 +25,6 @@ int OBC_C2A_I2cReadRegister(int port_id, const unsigned char i2c_address, unsign int OBC_C2A_GpioWrite(int port_id, const bool is_high); bool OBC_C2A_GpioRead(int port_id); // return false when the port_id is not used -} // namespace s2e::components +} // namespace s2e::components #endif // C2A_COMMUNICATION_H_ diff --git a/src/components/real/cdh/on_board_computer.cpp b/src/components/real/cdh/on_board_computer.cpp index ce38a4196..c661fc6d6 100644 --- a/src/components/real/cdh/on_board_computer.cpp +++ b/src/components/real/cdh/on_board_computer.cpp @@ -135,4 +135,4 @@ bool OnBoardComputer::GpioComponentRead(int port_id) { return port->DigitalRead(); } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/cdh/on_board_computer.hpp b/src/components/real/cdh/on_board_computer.hpp index 91aeff65b..123066539 100644 --- a/src/components/real/cdh/on_board_computer.hpp +++ b/src/components/real/cdh/on_board_computer.hpp @@ -209,6 +209,6 @@ class OnBoardComputer : public Component { std::map gpio_ports_; //!< GPIO ports }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_CDH_OBC_HPP_ diff --git a/src/components/real/cdh/on_board_computer_with_c2a.cpp b/src/components/real/cdh/on_board_computer_with_c2a.cpp index 3c6f58d1d..639e87f5e 100644 --- a/src/components/real/cdh/on_board_computer_with_c2a.cpp +++ b/src/components/real/cdh/on_board_computer_with_c2a.cpp @@ -259,4 +259,4 @@ int OBC_C2A_GpioWrite(int port_id, const bool is_high) { return ObcWithC2a::Gpio bool OBC_C2A_GpioRead(int port_id) { return ObcWithC2a::GpioRead_C2A(port_id); } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/cdh/on_board_computer_with_c2a.hpp b/src/components/real/cdh/on_board_computer_with_c2a.hpp index 05685df17..4b3c043aa 100644 --- a/src/components/real/cdh/on_board_computer_with_c2a.hpp +++ b/src/components/real/cdh/on_board_computer_with_c2a.hpp @@ -273,6 +273,6 @@ class ObcWithC2a : public OnBoardComputer { static std::map gpio_ports_c2a_; //!< GPIO ports }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_CDH_OBC_C2A_HPP_ diff --git a/src/components/real/communication/antenna.cpp b/src/components/real/communication/antenna.cpp index 12f24c46b..9bb535a61 100644 --- a/src/components/real/communication/antenna.cpp +++ b/src/components/real/communication/antenna.cpp @@ -167,4 +167,4 @@ Antenna InitAntenna(const int antenna_id, const std::string file_name) { return antenna; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/communication/antenna.hpp b/src/components/real/communication/antenna.hpp index 7419e3219..841fb55d0 100644 --- a/src/components/real/communication/antenna.hpp +++ b/src/components/real/communication/antenna.hpp @@ -168,6 +168,6 @@ AntennaGainModel SetAntennaGainModel(const std::string gain_model_name); */ Antenna InitAntenna(const int antenna_id, const std::string file_name); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_COMMUNICATION_ANTENNA_HPP_ diff --git a/src/components/real/communication/antenna_radiation_pattern.cpp b/src/components/real/communication/antenna_radiation_pattern.cpp index 59e1ac935..b1a3bced7 100644 --- a/src/components/real/communication/antenna_radiation_pattern.cpp +++ b/src/components/real/communication/antenna_radiation_pattern.cpp @@ -40,4 +40,4 @@ double AntennaRadiationPattern::GetGain_dBi(const double theta_rad, const double return gain_dBi_[theta_idx][phi_idx]; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/communication/antenna_radiation_pattern.hpp b/src/components/real/communication/antenna_radiation_pattern.hpp index f924fbe41..a6efac136 100644 --- a/src/components/real/communication/antenna_radiation_pattern.hpp +++ b/src/components/real/communication/antenna_radiation_pattern.hpp @@ -64,6 +64,6 @@ class AntennaRadiationPattern { std::vector> gain_dBi_; //!< Antenna gain table [dBi] }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_COMMUNICATION_ANTENNA_RADIATION_PATTERN_HPP_ diff --git a/src/components/real/communication/ground_station_calculator.cpp b/src/components/real/communication/ground_station_calculator.cpp index c3b3f9edd..8e00eeb82 100644 --- a/src/components/real/communication/ground_station_calculator.cpp +++ b/src/components/real/communication/ground_station_calculator.cpp @@ -42,8 +42,8 @@ void GroundStationCalculator::Update(const Spacecraft& spacecraft, const Antenna } // Private functions -double GroundStationCalculator::CalcMaxBitrate(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, - const Antenna& ground_station_rx_antenna) { +double GroundStationCalculator::CalcMaxBitrate(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, + const GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { double cn0_dBHz = CalcCn0OnGs(dynamics, spacecraft_tx_antenna, ground_station, ground_station_rx_antenna); double margin_for_bitrate_dB = cn0_dBHz - (ebn0_dB_ + hardware_deterioration_dB_ + coding_gain_dB_) - margin_requirement_dB_; @@ -62,8 +62,8 @@ double GroundStationCalculator::CalcReceiveMarginOnGs(const dynamics::Dynamics& return cn0_dB - cn0_requirement_dB; } -double GroundStationCalculator::CalcCn0OnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, - const Antenna& ground_station_rx_antenna) { +double GroundStationCalculator::CalcCn0OnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, + const GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { if (!spacecraft_tx_antenna.IsTransmitter() || !ground_station_rx_antenna.IsReceiver()) { // Check compatibility of transmitter and receiver return 0.0f; @@ -139,4 +139,4 @@ GroundStationCalculator InitGsCalculator(const std::string file_name) { return gs_calculator; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/communication/ground_station_calculator.hpp b/src/components/real/communication/ground_station_calculator.hpp index 9d7551b4f..dbda8d681 100644 --- a/src/components/real/communication/ground_station_calculator.hpp +++ b/src/components/real/communication/ground_station_calculator.hpp @@ -146,6 +146,6 @@ class GroundStationCalculator : public logger::ILoggable { GroundStationCalculator InitGsCalculator(const std::string file_name); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_COMMUNICATION_GROUND_STATION_CALCULATOR_HPP_ diff --git a/src/components/real/communication/wings_command_sender_to_c2a.cpp b/src/components/real/communication/wings_command_sender_to_c2a.cpp index e075b82ae..ec6c25da0 100644 --- a/src/components/real/communication/wings_command_sender_to_c2a.cpp +++ b/src/components/real/communication/wings_command_sender_to_c2a.cpp @@ -151,4 +151,4 @@ WingsCommandSenderToC2a InitWingsCommandSenderToC2a(environment::ClockGenerator* return WingsCommandSenderToC2a(prescaler, clock_generator, step_width_s, c2a_command_data_base_file, wings_operation_file, is_enabled); } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/communication/wings_command_sender_to_c2a.hpp b/src/components/real/communication/wings_command_sender_to_c2a.hpp index accc49546..167563cf9 100644 --- a/src/components/real/communication/wings_command_sender_to_c2a.hpp +++ b/src/components/real/communication/wings_command_sender_to_c2a.hpp @@ -24,8 +24,8 @@ class WingsCommandSenderToC2a : public Component { * @brief Constructor * @param [in] */ - WingsCommandSenderToC2a(int prescaler, environment::ClockGenerator* clock_generator, const double step_width_s, const std::string command_database_file, - const std::string operation_file, const bool is_enabled) + WingsCommandSenderToC2a(int prescaler, environment::ClockGenerator* clock_generator, const double step_width_s, + const std::string command_database_file, const std::string operation_file, const bool is_enabled) : Component(prescaler, clock_generator), c2a_command_database_(command_database_file), wings_operation_file_(operation_file), @@ -78,6 +78,6 @@ class WingsCommandSenderToC2a : public Component { WingsCommandSenderToC2a InitWingsCommandSenderToC2a(environment::ClockGenerator* clock_generator, const double compo_update_step_s, const std::string initialize_file); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_COMMUNICATION_C2A_COMMAND_SENDER_HPP_ diff --git a/src/components/real/mission/telescope.cpp b/src/components/real/mission/telescope.cpp index a05730683..7b704ba75 100644 --- a/src/components/real/mission/telescope.cpp +++ b/src/components/real/mission/telescope.cpp @@ -18,8 +18,8 @@ namespace s2e::components { Telescope::Telescope(environment::ClockGenerator* clock_generator, const math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, - const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, - const Orbit* orbit) + const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, + const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) : Component(1, clock_generator), quaternion_b2c_(quaternion_b2c), sun_forbidden_angle_rad_(sun_forbidden_angle_rad), @@ -247,8 +247,9 @@ string Telescope::GetLogValue() const { return str_tmp; } -Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, const dynamics::attitude::Attitude* attitude, - const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) { +Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, + const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, + const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) { using math::pi; setting_file_reader::IniAccess Telescope_conf(file_name); @@ -288,4 +289,4 @@ Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor return telescope; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/mission/telescope.hpp b/src/components/real/mission/telescope.hpp index d6c6310b5..c39a46854 100644 --- a/src/components/real/mission/telescope.hpp +++ b/src/components/real/mission/telescope.hpp @@ -126,7 +126,7 @@ class Telescope : public Component, public logger::ILoggable { */ void ObserveStars(); - const dynamics::attitude::Attitude* attitude_; //!< dynamics::attitude::Attitude information + const dynamics::attitude::Attitude* attitude_; //!< dynamics::attitude::Attitude information const HipparcosCatalogue* hipparcos_; //!< Star information const LocalCelestialInformation* local_celestial_information_; //!< Local celestial information /** @@ -168,10 +168,10 @@ class Telescope : public Component, public logger::ILoggable { * @param [in] hipparcos: Star information by Hipparcos catalogue * @param [in] local_celestial_information: Local celestial information */ -Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, const dynamics::attitude::Attitude* attitude, - const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, - const Orbit* orbit = nullptr); +Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, + const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, + const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_MISSION_TELESCOPE_HPP_P_ diff --git a/src/components/real/power/battery.cpp b/src/components/real/power/battery.cpp index 9637e5a23..247bf082b 100644 --- a/src/components/real/power/battery.cpp +++ b/src/components/real/power/battery.cpp @@ -10,9 +10,9 @@ namespace s2e::components { -Battery::Battery(const int prescaler, environment::ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, double cell_capacity_Ah, - const std::vector cell_discharge_curve_coefficients, double initial_dod, double cc_charge_c_rate, double cv_charge_voltage_V, - double battery_resistance_Ohm, double component_step_time_s) +Battery::Battery(const int prescaler, environment::ClockGenerator* clock_generator, int number_of_series, int number_of_parallel, + double cell_capacity_Ah, const std::vector cell_discharge_curve_coefficients, double initial_dod, double cc_charge_c_rate, + double cv_charge_voltage_V, double battery_resistance_Ohm, double component_step_time_s) : Component(prescaler, clock_generator), number_of_series_(number_of_series), number_of_parallel_(number_of_parallel), @@ -133,4 +133,4 @@ Battery InitBAT(environment::ClockGenerator* clock_generator, int bat_id, const return battery; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/power/battery.hpp b/src/components/real/power/battery.hpp index c1adf5d49..84d2edf9b 100644 --- a/src/components/real/power/battery.hpp +++ b/src/components/real/power/battery.hpp @@ -146,6 +146,6 @@ class Battery : public Component, public logger::ILoggable { */ Battery InitBAT(environment::ClockGenerator* clock_generator, int bat_id, const std::string file_name, double component_step_time_s); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_POWER_BATTERY_HPP_P_ diff --git a/src/components/real/power/csv_scenario_interface.cpp b/src/components/real/power/csv_scenario_interface.cpp index 999369a02..54ee31a01 100644 --- a/src/components/real/power/csv_scenario_interface.cpp +++ b/src/components/real/power/csv_scenario_interface.cpp @@ -95,4 +95,4 @@ double CsvScenarioInterface::GetValueFromBuffer(const std::string buffer_name, c return output; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/power/csv_scenario_interface.hpp b/src/components/real/power/csv_scenario_interface.hpp index 98dc2180d..d6c331ad9 100644 --- a/src/components/real/power/csv_scenario_interface.hpp +++ b/src/components/real/power/csv_scenario_interface.hpp @@ -80,6 +80,6 @@ class CsvScenarioInterface { static std::map buffers_; //!< Buffer }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_POWER_CSV_SCENARIO_INTERFACE_HPP_ diff --git a/src/components/real/power/pcu_initial_study.cpp b/src/components/real/power/pcu_initial_study.cpp index 5a20fb79e..b1d35c2a7 100644 --- a/src/components/real/power/pcu_initial_study.cpp +++ b/src/components/real/power/pcu_initial_study.cpp @@ -12,8 +12,8 @@ namespace s2e::components { -PcuInitialStudy::PcuInitialStudy(const int prescaler, environment::ClockGenerator* clock_generator, const std::vector saps, Battery* battery, - double component_step_time_s) +PcuInitialStudy::PcuInitialStudy(const int prescaler, environment::ClockGenerator* clock_generator, const std::vector saps, + Battery* battery, double component_step_time_s) : Component(prescaler, clock_generator), saps_(saps), battery_(battery), @@ -127,4 +127,4 @@ PcuInitialStudy InitPCU_InitialStudy(environment::ClockGenerator* clock_generato return pcu; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/power/pcu_initial_study.hpp b/src/components/real/power/pcu_initial_study.hpp index 5a8bd074b..65f5eda94 100644 --- a/src/components/real/power/pcu_initial_study.hpp +++ b/src/components/real/power/pcu_initial_study.hpp @@ -97,6 +97,6 @@ class PcuInitialStudy : public Component, public logger::ILoggable { PcuInitialStudy InitPCU_InitialStudy(environment::ClockGenerator* clock_generator, int pcu_id, const std::string file_name, const std::vector saps, Battery* battery, double component_step_time_s); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_POWER_PCU_INITIAL_STUDY_HPP_ diff --git a/src/components/real/power/power_control_unit.cpp b/src/components/real/power/power_control_unit.cpp index ce820b1c3..5ed558949 100644 --- a/src/components/real/power/power_control_unit.cpp +++ b/src/components/real/power/power_control_unit.cpp @@ -55,4 +55,4 @@ std::string PowerControlUnit::GetLogValue() const { return str_tmp; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/power/power_control_unit.hpp b/src/components/real/power/power_control_unit.hpp index d61bc397b..0e16e2547 100644 --- a/src/components/real/power/power_control_unit.hpp +++ b/src/components/real/power/power_control_unit.hpp @@ -97,6 +97,6 @@ class PowerControlUnit : public Component, public logger::ILoggable { std::map power_ports_; //!< Power port list }; -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_POWER_POWER_CONTROL_UNIT_HPP_ diff --git a/src/components/real/power/solar_array_panel.cpp b/src/components/real/power/solar_array_panel.cpp index 651e49eb2..f50959a07 100644 --- a/src/components/real/power/solar_array_panel.cpp +++ b/src/components/real/power/solar_array_panel.cpp @@ -11,9 +11,9 @@ namespace s2e::components { -SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, - double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, - const SolarRadiationPressureEnvironment* srp_environment, +SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, + int number_of_parallel, double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, + double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, double component_step_time_s) : Component(prescaler, clock_generator), component_id_(component_id), @@ -30,9 +30,10 @@ SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerato power_generation_W_ = 0.0; } -SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, - double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, - const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s) +SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, + int number_of_parallel, double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, + double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, + double component_step_time_s) : Component(prescaler, clock_generator), component_id_(component_id), number_of_series_(number_of_series), @@ -47,8 +48,8 @@ SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerato power_generation_W_ = 0.0; } -SolarArrayPanel::SolarArrayPanel(environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, - math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, +SolarArrayPanel::SolarArrayPanel(environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, + double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) : Component(10, clock_generator), @@ -183,4 +184,4 @@ SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id return sap; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/power/solar_array_panel.hpp b/src/components/real/power/solar_array_panel.hpp index e77eecd3a..e684062cf 100644 --- a/src/components/real/power/solar_array_panel.hpp +++ b/src/components/real/power/solar_array_panel.hpp @@ -159,6 +159,6 @@ SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_POWER_SOLAR_ARRAY_PANEL_HPP_ diff --git a/src/components/real/propulsion/simple_thruster.cpp b/src/components/real/propulsion/simple_thruster.cpp index 61b1ec766..dc07d7007 100644 --- a/src/components/real/propulsion/simple_thruster.cpp +++ b/src/components/real/propulsion/simple_thruster.cpp @@ -122,8 +122,8 @@ math::Vector<3> SimpleThruster::CalcThrustDirection() { return thrust_dir_b_true; } -SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, const Structure* structure, - const dynamics::Dynamics* dynamics) { +SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, + const Structure* structure, const dynamics::Dynamics* dynamics) { setting_file_reader::IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); auto* Section = section_str.c_str(); @@ -180,4 +180,4 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, return thruster; } -} // namespace s2e::components +} // namespace s2e::components diff --git a/src/components/real/propulsion/simple_thruster.hpp b/src/components/real/propulsion/simple_thruster.hpp index 6cb9ab066..ca0476e04 100644 --- a/src/components/real/propulsion/simple_thruster.hpp +++ b/src/components/real/propulsion/simple_thruster.hpp @@ -110,14 +110,14 @@ class SimpleThruster : public Component, public logger::ILoggable { protected: // parameters - const int component_id_; //!< Thruster ID - Vector<3> thruster_position_b_m_{0.0}; //!< Thruster position @ body frame [m] - Vector<3> thrust_direction_b_{0.0}; //!< Thrust direction @ body frame - double duty_ = 0.0; //!< PWM Duty [0.0 : 1.0] - double thrust_magnitude_max_N_ = 0.0; //!< Maximum thrust magnitude [N] - double direction_noise_standard_deviation_rad_ = 0.0; //!< Standard deviation of thrust direction error [rad] - s2e::randomization::NormalRand magnitude_random_noise_; //!< Normal random for thrust magnitude error - s2e::randomization::NormalRand direction_random_noise_; //!< Normal random for thrust direction error + const int component_id_; //!< Thruster ID + Vector<3> thruster_position_b_m_{0.0}; //!< Thruster position @ body frame [m] + Vector<3> thrust_direction_b_{0.0}; //!< Thrust direction @ body frame + double duty_ = 0.0; //!< PWM Duty [0.0 : 1.0] + double thrust_magnitude_max_N_ = 0.0; //!< Maximum thrust magnitude [N] + double direction_noise_standard_deviation_rad_ = 0.0; //!< Standard deviation of thrust direction error [rad] + s2e::randomization::NormalRand magnitude_random_noise_; //!< Normal random for thrust magnitude error + s2e::randomization::NormalRand direction_random_noise_; //!< Normal random for thrust direction error // outputs Vector<3> output_thrust_b_N_{0.0}; //!< Generated thrust on the body fixed frame [N] Vector<3> output_torque_b_Nm_{0.0}; //!< Generated torque on the body fixed frame [Nm] @@ -153,8 +153,8 @@ class SimpleThruster : public Component, public logger::ILoggable { */ void Initialize(const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad); - const Structure* structure_; //!< Spacecraft structure information - const dynamics::Dynamics* dynamics_; //!< Spacecraft dynamics information + const Structure* structure_; //!< Spacecraft structure information + const dynamics::Dynamics* dynamics_; //!< Spacecraft dynamics information }; /** @@ -166,8 +166,8 @@ class SimpleThruster : public Component, public logger::ILoggable { * @param [in] structure: Spacecraft structure information * @param [in] dynamics: Spacecraft dynamics information */ -SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, const Structure* structure, - const dynamics::Dynamics* dynamics); +SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, + const Structure* structure, const dynamics::Dynamics* dynamics); /** * @fn InitSimpleThruster * @brief Initialize function os SimpleThruster @@ -181,6 +181,6 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, const Structure* structure, const dynamics::Dynamics* dynamics); -} // namespace s2e::components +} // namespace s2e::components #endif // S2E_COMPONENTS_REAL_PROPULSION_SIMPLE_THRUSTER_HPP_ diff --git a/src/disturbances/air_drag.cpp b/src/disturbances/air_drag.cpp index c4f306523..8b7503776 100644 --- a/src/disturbances/air_drag.cpp +++ b/src/disturbances/air_drag.cpp @@ -112,4 +112,4 @@ AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, const Vector<3>& center_of_gravity_b_m); -} // namespace s2e::disturbances +} // namespace s2e::disturbances #endif // S2E_DISTURBANCES_AIR_DRAG_HPP_ diff --git a/src/disturbances/disturbance.hpp b/src/disturbances/disturbance.hpp index 6321f06d2..274517736 100644 --- a/src/disturbances/disturbance.hpp +++ b/src/disturbances/disturbance.hpp @@ -93,6 +93,6 @@ class Disturbance : public logger::ILoggable { math::Vector<3> acceleration_i_m_s2_; //!< Disturbance acceleration in the inertial frame [m/s2] }; -} // namespace s2e::disturbances +} // namespace s2e::disturbances #endif // S2E_DISTURBANCES_DISTURBANCE_HPP_ diff --git a/src/disturbances/disturbances.cpp b/src/disturbances/disturbances.cpp index 8d0c94037..e3351d989 100644 --- a/src/disturbances/disturbances.cpp +++ b/src/disturbances/disturbances.cpp @@ -99,4 +99,4 @@ void Disturbances::InitializeForceAndTorque() { void Disturbances::InitializeAcceleration() { total_acceleration_i_m_s2_ = Vector<3>(0.0); } -} // namespace s2e::disturbances +} // namespace s2e::disturbances diff --git a/src/disturbances/disturbances.hpp b/src/disturbances/disturbances.hpp index a3f57b39a..67d1f473d 100644 --- a/src/disturbances/disturbances.hpp +++ b/src/disturbances/disturbances.hpp @@ -101,6 +101,6 @@ class Disturbances { void InitializeAcceleration(); }; -} // namespace s2e::disturbances +} // namespace s2e::disturbances #endif // S2E_DISTURBANCES_DISTURBANCES_HPP_ diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index dffba8b93..382a10dc4 100644 --- a/src/disturbances/geopotential.cpp +++ b/src/disturbances/geopotential.cpp @@ -129,4 +129,4 @@ Geopotential InitGeopotential(const std::string initialize_file_path) { return geopotential_disturbance; } -} // namespace s2e::disturbances +} // namespace s2e::disturbances diff --git a/src/disturbances/geopotential.hpp b/src/disturbances/geopotential.hpp index c82f792b1..235c1fef8 100644 --- a/src/disturbances/geopotential.hpp +++ b/src/disturbances/geopotential.hpp @@ -88,6 +88,6 @@ class Geopotential : public Disturbance { */ Geopotential InitGeopotential(const std::string initialize_file_path); -} // namespace s2e::disturbances +} // namespace s2e::disturbances #endif // S2E_DISTURBANCES_GEOPOTENTIAL_HPP_ diff --git a/src/disturbances/gravity_gradient.cpp b/src/disturbances/gravity_gradient.cpp index 5129ec5be..4bb880cc1 100644 --- a/src/disturbances/gravity_gradient.cpp +++ b/src/disturbances/gravity_gradient.cpp @@ -73,4 +73,4 @@ GravityGradient InitGravityGradient(const std::string initialize_file_path, cons return gg_disturbance; } -} // namespace s2e::disturbances +} // namespace s2e::disturbances diff --git a/src/disturbances/gravity_gradient.hpp b/src/disturbances/gravity_gradient.hpp index b58b8df33..ad5e6fcea 100644 --- a/src/disturbances/gravity_gradient.hpp +++ b/src/disturbances/gravity_gradient.hpp @@ -85,6 +85,6 @@ GravityGradient InitGravityGradient(const std::string initialize_file_path); */ GravityGradient InitGravityGradient(const std::string initialize_file_path, const double gravity_constant_m3_s2); -} // namespace s2e::disturbances +} // namespace s2e::disturbances #endif // S2E_DISTURBANCES_GRAVITY_GRADIENT_HPP_ diff --git a/src/disturbances/lunar_gravity_field.cpp b/src/disturbances/lunar_gravity_field.cpp index 3511537e6..7d37794e4 100644 --- a/src/disturbances/lunar_gravity_field.cpp +++ b/src/disturbances/lunar_gravity_field.cpp @@ -153,4 +153,4 @@ LunarGravityField InitLunarGravityField(const std::string initialize_file_path) return lunar_gravity_field; } -} // namespace s2e::disturbances +} // namespace s2e::disturbances diff --git a/src/disturbances/lunar_gravity_field.hpp b/src/disturbances/lunar_gravity_field.hpp index 40b5d0e7d..a117464f5 100644 --- a/src/disturbances/lunar_gravity_field.hpp +++ b/src/disturbances/lunar_gravity_field.hpp @@ -92,6 +92,6 @@ class LunarGravityField : public Disturbance { */ LunarGravityField InitLunarGravityField(const std::string initialize_file_path); -} // namespace s2e::disturbances +} // namespace s2e::disturbances #endif // S2E_DISTURBANCES_LUNAR_GRAVITY_FIELD_HPP_ diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index 11fc64530..6165965f0 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -37,7 +37,7 @@ void MagneticDisturbance::CalcRMM() { static math::Vector<3> random_walk_limit(residual_magnetic_moment_.GetRandomWalkLimit_Am2()); static RandomWalk<3> random_walk(0.1, random_walk_std_dev, random_walk_limit); // [FIXME] step width is constant static s2e::randomization::NormalRand normal_random(0.0, residual_magnetic_moment_.GetRandomNoiseStandardDeviation_Am2(), - s2e::randomization::global_randomization.MakeSeed()); + s2e::randomization::global_randomization.MakeSeed()); rmm_b_Am2_ = residual_magnetic_moment_.GetConstantValue_b_Am2(); for (int i = 0; i < 3; ++i) { @@ -75,4 +75,4 @@ MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_pa return mag_disturbance; } -} // namespace s2e::disturbances +} // namespace s2e::disturbances diff --git a/src/disturbances/magnetic_disturbance.hpp b/src/disturbances/magnetic_disturbance.hpp index 7b5187598..f618f9de3 100644 --- a/src/disturbances/magnetic_disturbance.hpp +++ b/src/disturbances/magnetic_disturbance.hpp @@ -77,6 +77,6 @@ class MagneticDisturbance : public Disturbance { */ MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const ResidualMagneticMoment& rmm_params); -} // namespace s2e::disturbances +} // namespace s2e::disturbances #endif // S2E_DISTURBANCES_MAGNETIC_DISTURBANCE_HPP_ diff --git a/src/disturbances/solar_radiation_pressure_disturbance.cpp b/src/disturbances/solar_radiation_pressure_disturbance.cpp index 638a19833..bd26dec2a 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.cpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.cpp @@ -68,4 +68,4 @@ SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const st return srp_disturbance; } -} // namespace s2e::disturbances +} // namespace s2e::disturbances diff --git a/src/disturbances/solar_radiation_pressure_disturbance.hpp b/src/disturbances/solar_radiation_pressure_disturbance.hpp index 63f20e4e0..8cfcc3df1 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.hpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.hpp @@ -70,6 +70,6 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const std::string initialize_file_path, const std::vector& surfaces, const Vector<3>& center_of_gravity_b_m); -} // namespace s2e::disturbances +} // namespace s2e::disturbances #endif // S2E_DISTURBANCES_SOLAR_RADIATION_PRESSURE_DISTURBANCE_HPP_ diff --git a/src/disturbances/surface_force.cpp b/src/disturbances/surface_force.cpp index 0cef5b365..80ec260cf 100644 --- a/src/disturbances/surface_force.cpp +++ b/src/disturbances/surface_force.cpp @@ -55,4 +55,4 @@ void SurfaceForce::CalcTheta(math::Vector<3>& input_direction_b) { } } -} // namespace s2e::disturbances +} // namespace s2e::disturbances diff --git a/src/disturbances/surface_force.hpp b/src/disturbances/surface_force.hpp index c6c0c3d06..61eb8edba 100644 --- a/src/disturbances/surface_force.hpp +++ b/src/disturbances/surface_force.hpp @@ -73,6 +73,6 @@ class SurfaceForce : public Disturbance { virtual void CalcCoefficients(const math::Vector<3>& input_direction_b, const double item) = 0; }; -} // namespace s2e::disturbances +} // namespace s2e::disturbances #endif // S2E_DISTURBANCES_SURFACE_FORCE_HPP_ diff --git a/src/disturbances/third_body_gravity.cpp b/src/disturbances/third_body_gravity.cpp index 7d99ab2fb..205c129fa 100644 --- a/src/disturbances/third_body_gravity.cpp +++ b/src/disturbances/third_body_gravity.cpp @@ -102,4 +102,4 @@ ThirdBodyGravity InitThirdBodyGravity(const std::string initialize_file_path, co return third_body_disturbance; } -} // namespace s2e::disturbances +} // namespace s2e::disturbances diff --git a/src/disturbances/third_body_gravity.hpp b/src/disturbances/third_body_gravity.hpp index 7207f7e78..c55d5f2e3 100644 --- a/src/disturbances/third_body_gravity.hpp +++ b/src/disturbances/third_body_gravity.hpp @@ -78,6 +78,6 @@ class ThirdBodyGravity : public Disturbance { */ ThirdBodyGravity InitThirdBodyGravity(const std::string initialize_file_path, const std::string ini_path_celes); -} // namespace s2e::disturbances +} // namespace s2e::disturbances #endif // S2E_DISTURBANCES_THIRD_BODY_GRAVITY_HPP_ diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index 84a9040e2..b079c68b8 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -82,4 +82,4 @@ math::Matrix<4, 4> CalcAngularVelocityMatrix(math::Vector<3> angular_velocity_b_ return angular_velocity_matrix; } -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index 6c3c80fef..59076b6ef 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -144,6 +144,6 @@ class Attitude : public logger::ILoggable, public simulation::SimulationObject { */ math::Matrix<4, 4> CalcAngularVelocityMatrix(math::Vector<3> angular_velocity_b_rad_s); -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude #endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_HPP_ diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index 52af18344..f7cca9b68 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -122,4 +122,4 @@ void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { quaternion_i2b_.Normalize(); } -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/attitude_rk4.hpp b/src/dynamics/attitude/attitude_rk4.hpp index 2b3b4f06c..5924bd4cf 100644 --- a/src/dynamics/attitude/attitude_rk4.hpp +++ b/src/dynamics/attitude/attitude_rk4.hpp @@ -70,6 +70,6 @@ class AttitudeRk4 : public Attitude { void RungeKuttaOneStep(double t, double dt); }; -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude #endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_RK4_HPP_ diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp index 78292eb5c..a43ba941d 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp @@ -102,4 +102,4 @@ void AttitudeWithCantileverVibration::Propagate(const double end_time_s) { CalcAngularMomentum(); } -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp index 30469c2fd..0484e7c54 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp @@ -79,6 +79,6 @@ class AttitudeWithCantileverVibration : public Attitude { s2e::numerical_integration::NumericalIntegratorManager<13> numerical_integrator_; }; -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude #endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_WITH_CANTILEVER_VIBRATION_HPP_ diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index a1850b8fb..f564ebd21 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -181,4 +181,4 @@ void ControlledAttitude::CalcAngularVelocity(const double current_time_s) { previous_omega_b_rad_s_ = angular_velocity_b_rad_s_; } -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index 0b1b65ac7..c2853871e 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -58,8 +58,8 @@ class ControlledAttitude : public Attitude { */ ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const math::Quaternion quaternion_i2b, const math::Vector<3> main_target_direction_b, const math::Vector<3> sub_target_direction_b, - const math::Matrix<3, 3>& inertia_tensor_kgm2, const environment::LocalCelestialInformation* local_celestial_information, const orbit::Orbit* orbit, - const std::string& simulation_object_name = "attitude"); + const math::Matrix<3, 3>& inertia_tensor_kgm2, const environment::LocalCelestialInformation* local_celestial_information, + const orbit::Orbit* orbit, const std::string& simulation_object_name = "attitude"); /** * @fn ~ControlledAttitude * @brief Destructor @@ -114,7 +114,7 @@ class ControlledAttitude : public Attitude { // Inputs const environment::LocalCelestialInformation* local_celestial_information_; //!< Local celestial information - const orbit::Orbit* orbit_; //!< Orbit information + const orbit::Orbit* orbit_; //!< Orbit information // Local functions /** @@ -151,6 +151,6 @@ class ControlledAttitude : public Attitude { math::Matrix<3, 3> CalcDcm(const math::Vector<3> main_direction, const math::Vector<3> sub_direction); }; -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude #endif // S2E_DYNAMICS_ATTITUDE_CONTROLLED_ATTITUDE_HPP_ diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index 8460ea988..0aea27194 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -96,4 +96,4 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel return attitude; } -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude diff --git a/src/dynamics/attitude/initialize_attitude.hpp b/src/dynamics/attitude/initialize_attitude.hpp index a03141105..335ee0c44 100644 --- a/src/dynamics/attitude/initialize_attitude.hpp +++ b/src/dynamics/attitude/initialize_attitude.hpp @@ -26,6 +26,6 @@ namespace s2e::dynamics::attitude { Attitude* InitAttitude(std::string file_name, const orbit::Orbit* orbit, const environment::LocalCelestialInformation* local_celestial_information, const double step_width_s, const math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id); -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude #endif // S2E_DYNAMICS_ATTITUDE_INITIALIZE_ATTITUDE_HPP_ diff --git a/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp index 84591e9b3..c267b84f7 100644 --- a/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/ode_attitude_with_cantilever_vibration.hpp @@ -222,6 +222,6 @@ class AttitudeWithCantileverVibrationOde : public numerical_integration::Interfa math::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever_{0.0}; //!< Inverse of inertia tensor of the cantilever }; -} // namespace s2e::dynamics::attitude +} // namespace s2e::dynamics::attitude #endif // S2E_DYNAMICS_ATTITUDE_ODE_ATTITUDE_WITH_CANTILEVER_VIBRATION_HPP_ diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index e87f57dd0..49f226dde 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -9,8 +9,9 @@ namespace s2e::dynamics { -Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, const environment::LocalEnvironment* local_environment, - const int spacecraft_id, simulation::Structure* structure, simulation::RelativeInformation* relative_information) +Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, + const environment::LocalEnvironment* local_environment, const int spacecraft_id, simulation::Structure* structure, + simulation::RelativeInformation* relative_information) : structure_(structure), local_environment_(local_environment) { Initialize(simulation_configuration, simulation_time, spacecraft_id, structure, relative_information); } @@ -67,4 +68,4 @@ void Dynamics::LogSetup(Logger& logger) { logger.AddLogList(temperature_); } -} // namespace s2e::dynamics +} // namespace s2e::dynamics diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index 0681d6d4c..5267f7a64 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -17,8 +17,8 @@ #include "dynamics/thermal/node.hpp" #include "dynamics/thermal/temperature.hpp" -class RelativeInformation; -class LocalEnvironment; +class s2e::simulation::RelativeInformation; +class s2e::environment::LocalEnvironment; namespace s2e::dynamics { @@ -38,8 +38,9 @@ class Dynamics { * @param [in] structure: Structure of the spacecraft * @param [in] relative_information: Relative information */ - Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, const environment::LocalEnvironment* local_environment, - const int spacecraft_id, simulation::Structure* structure, simulation::RelativeInformation* relative_information = (simulation::RelativeInformation*)nullptr); + Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, + const environment::LocalEnvironment* local_environment, const int spacecraft_id, simulation::Structure* structure, + simulation::RelativeInformation* relative_information = (simulation::RelativeInformation*)nullptr); /** * @fn ~Dynamics * @brief Destructor @@ -109,10 +110,10 @@ class Dynamics { inline attitude::Attitude& SetAttitude() const { return *attitude_; } private: - attitude::Attitude* attitude_; //!< Attitude dynamics - orbit::Orbit* orbit_; //!< Orbit dynamics - thermal::Temperature* temperature_; //!< Thermal dynamics - const simulation::Structure* structure_; //!< Structure information + attitude::Attitude* attitude_; //!< Attitude dynamics + orbit::Orbit* orbit_; //!< Orbit dynamics + thermal::Temperature* temperature_; //!< Thermal dynamics + const simulation::Structure* structure_; //!< Structure information const environment::LocalEnvironment* local_environment_; //!< Local environment /** @@ -125,10 +126,11 @@ class Dynamics { * @param [in] structure: Structure of the spacecraft * @param [in] relative_information: Relative information */ - void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, const int spacecraft_id, - simulation::Structure* structure, simulation::RelativeInformation* relative_information = (simulation::RelativeInformation*)nullptr); + void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, + const int spacecraft_id, simulation::Structure* structure, + simulation::RelativeInformation* relative_information = (simulation::RelativeInformation*)nullptr); }; -} // namespace s2e::dynamics +} // namespace s2e::dynamics #endif // S2E_DYNAMICS_DYNAMICS_HPP_ diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index ac661f705..adf102ceb 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -127,4 +127,4 @@ double EnckeOrbitPropagation::CalcQFunction(math::Vector<3> difference_position_ return q_func; } -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index 7b2ba00a8..4101b6b71 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -65,8 +65,8 @@ class EnckeOrbitPropagation : public Orbit, public math::OrdinaryDifferentialEqu double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 // reference orbit - math::Vector<3> reference_position_i_m_; //!< Reference orbit position in the inertial frame [m] - math::Vector<3> reference_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] + math::Vector<3> reference_position_i_m_; //!< Reference orbit position in the inertial frame [m] + math::Vector<3> reference_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] s2e::orbit::KeplerOrbit reference_kepler_orbit; //!< Reference Kepler orbital element // difference orbit @@ -95,6 +95,6 @@ class EnckeOrbitPropagation : public Orbit, public math::OrdinaryDifferentialEqu double CalcQFunction(const math::Vector<3> difference_position_i_m); }; -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit #endif // S2E_DYNAMICS_ORBIT_ENCKE_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index a564b2521..79ebd0e13 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -48,7 +48,8 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // initialize orbit for relative dynamics of formation flying RelativeOrbit::RelativeOrbitUpdateMethod update_method = (RelativeOrbit::RelativeOrbitUpdateMethod)(conf.ReadInt(section_, "relative_orbit_update_method")); - s2e::orbit::RelativeOrbitModel relative_dynamics_model_type = (s2e::orbit::RelativeOrbitModel)(conf.ReadInt(section_, "relative_dynamics_model_type")); + s2e::orbit::RelativeOrbitModel relative_dynamics_model_type = + (s2e::orbit::RelativeOrbitModel)(conf.ReadInt(section_, "relative_dynamics_model_type")); s2e::orbit::StmModel stm_model_type = (s2e::orbit::StmModel)(conf.ReadInt(section_, "stm_model_type")); math::Vector<3> init_relative_position_lvlh; @@ -151,4 +152,4 @@ math::Vector<6> InitializePosVel(std::string initialize_file, double current_tim return pos_vel; } -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/initialize_orbit.hpp b/src/dynamics/orbit/initialize_orbit.hpp index ece960dd2..a006fe204 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -25,8 +25,8 @@ namespace s2e::dynamics::orbit { * @param [in] section: Section name * @param [in] relative_information: Relative information */ -Orbit* InitOrbit(const environment::CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, double current_time_jd, - double gravity_constant_m3_s2, std::string section = "ORBIT", +Orbit* InitOrbit(const environment::CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, + double current_time_jd, double gravity_constant_m3_s2, std::string section = "ORBIT", RelativeInformation* relative_information = (RelativeInformation*)nullptr); /** @@ -39,6 +39,6 @@ Orbit* InitOrbit(const environment::CelestialInformation* celestial_information, */ math::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double gravity_constant_m3_s2, std::string section = "ORBIT"); -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit #endif // S2E_DYNAMICS_ORBIT_INITIALIZE_ORBIT_HPP_ diff --git a/src/dynamics/orbit/kepler_orbit_propagation.cpp b/src/dynamics/orbit/kepler_orbit_propagation.cpp index d745a2c3a..dadb5ef50 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.cpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.cpp @@ -35,4 +35,4 @@ void KeplerOrbitPropagation::UpdateState(const double current_time_jd) { TransformEcefToGeodetic(); } -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/kepler_orbit_propagation.hpp b/src/dynamics/orbit/kepler_orbit_propagation.hpp index 117f8edc1..cd18bab8c 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.hpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.hpp @@ -50,6 +50,6 @@ class KeplerOrbitPropagation : public Orbit, public s2e::orbit::KeplerOrbit { void UpdateState(const double current_time_jd); }; -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit #endif // S2E_DYNAMICS_ORBIT_KEPLER_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index d08e642ad..dbdcb7791 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -96,4 +96,4 @@ std::string Orbit::GetLogValue() const { return str_tmp; } -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index f6d80c7e7..9594aa7a1 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -193,8 +193,8 @@ class Orbit : public logger::ILoggable { bool is_calc_enabled_ = false; //!< Calculate flag OrbitPropagateMode propagate_mode_; //!< Propagation mode - math::Vector<3> spacecraft_position_i_m_; //!< Spacecraft position in the inertial frame [m] - math::Vector<3> spacecraft_position_ecef_m_; //!< Spacecraft position in the ECEF frame [m] + math::Vector<3> spacecraft_position_i_m_; //!< Spacecraft position in the inertial frame [m] + math::Vector<3> spacecraft_position_ecef_m_; //!< Spacecraft position in the ECEF frame [m] s2e::geodesy::GeodeticPosition spacecraft_geodetic_position_; //!< Spacecraft position in the Geodetic frame math::Vector<3> spacecraft_velocity_i_m_s_; //!< Spacecraft velocity in the inertial frame [m/s] @@ -219,6 +219,6 @@ class Orbit : public logger::ILoggable { OrbitInitializeMode SetOrbitInitializeMode(const std::string initialize_mode); -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit #endif // S2E_DYNAMICS_ORBIT_ORBIT_HPP_ diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 7620b3866..af1ee1d88 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -160,4 +160,4 @@ void RelativeOrbit::DerivativeFunction(double t, const math::Vector<6>& state, (void)t; } -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 7aa505d88..5abf101cb 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -43,7 +43,8 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> */ RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, int reference_spacecraft_id, math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, - s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, s2e::orbit::StmModel stm_model_type, RelativeInformation* relative_information); + s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, s2e::orbit::StmModel stm_model_type, + RelativeInformation* relative_information); /** * @fn ~RelativeOrbit * @brief Destructor @@ -82,10 +83,10 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> math::Vector<3> relative_position_lvlh_m_; //!< Relative position in the LVLH frame math::Vector<3> relative_velocity_lvlh_m_s_; //!< Relative velocity in the LVLH frame - RelativeOrbitUpdateMethod update_method_; //!< Update method + RelativeOrbitUpdateMethod update_method_; //!< Update method s2e::orbit::RelativeOrbitModel relative_dynamics_model_type_; //!< Relative dynamics model type s2e::orbit::StmModel stm_model_type_; //!< State Transition Matrix model type - RelativeInformation* relative_information_; //!< Relative information + RelativeInformation* relative_information_; //!< Relative information /** * @fn InitializeState @@ -104,7 +105,8 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> * @param [in] reference_sat_orbit: Orbit information of reference satellite * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] */ - void CalculateSystemMatrix(s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2); + void CalculateSystemMatrix(s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, + double gravity_constant_m3_s2); /** * @fn CalculateStm * @brief Calculate State Transition Matrix @@ -128,6 +130,6 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> void PropagateStm(double elapsed_sec); }; -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit #endif // S2E_DYNAMICS_ORBIT_RELATIVE_ORBIT_HPP_ diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index 824637259..b9fea10c7 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -89,4 +89,4 @@ void Rk4OrbitPropagation::Propagate(const double end_time_s, const double curren TransformEcefToGeodetic(); } -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index b46b35646..dbda96d86 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -71,6 +71,6 @@ class Rk4OrbitPropagation : public Orbit, public math::OrdinaryDifferentialEquat void Initialize(math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s = 0); }; -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit #endif // S2E_DYNAMICS_ORBIT_RK4_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index f15c8b56c..1322cbeb6 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -69,4 +69,4 @@ void Sgp4OrbitPropagation::Propagate(const double end_time_s, const double curre TransformEcefToGeodetic(); } -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.hpp b/src/dynamics/orbit/sgp4_orbit_propagation.hpp index a31be3690..0bd59bfd3 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.hpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.hpp @@ -47,6 +47,6 @@ class Sgp4OrbitPropagation : public Orbit { elsetrec sgp4_data_; //!< Structure data for SGP4 library }; -} // namespace s2e::dynamics::orbit +} // namespace s2e::dynamics::orbit #endif // S2E_DYNAMICS_ORBIT_SGP4_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/thermal/heater.cpp b/src/dynamics/thermal/heater.cpp index 6444ae329..e43e32951 100644 --- a/src/dynamics/thermal/heater.cpp +++ b/src/dynamics/thermal/heater.cpp @@ -78,4 +78,4 @@ Heater InitHeater(const std::vector& heater_str) { return heater; } -} // namespace s2e::dynamics::thermal +} // namespace s2e::dynamics::thermal diff --git a/src/dynamics/thermal/heater.hpp b/src/dynamics/thermal/heater.hpp index ac1f47c30..5643efcdc 100644 --- a/src/dynamics/thermal/heater.hpp +++ b/src/dynamics/thermal/heater.hpp @@ -101,6 +101,6 @@ class Heater { */ Heater InitHeater(const std::vector& heater_str); -} // namespace s2e::dynamics::thermal +} // namespace s2e::dynamics::thermal #endif // S2E_DYNAMICS_THERMAL_HEATER_HPP_ diff --git a/src/dynamics/thermal/heater_controller.cpp b/src/dynamics/thermal/heater_controller.cpp index 6c346041e..e0c02658f 100644 --- a/src/dynamics/thermal/heater_controller.cpp +++ b/src/dynamics/thermal/heater_controller.cpp @@ -55,4 +55,4 @@ HeaterController InitHeaterController(const std::vector& heater_str return heater_controller; } -} // namespace s2e::dynamics::thermal +} // namespace s2e::dynamics::thermal diff --git a/src/dynamics/thermal/heater_controller.hpp b/src/dynamics/thermal/heater_controller.hpp index 70ff3fb6c..0d8f76bc5 100644 --- a/src/dynamics/thermal/heater_controller.hpp +++ b/src/dynamics/thermal/heater_controller.hpp @@ -91,6 +91,6 @@ class HeaterController { */ HeaterController InitHeaterController(const std::vector& heater_str); -} // namespace s2e::dynamics::thermal +} // namespace s2e::dynamics::thermal #endif // S2E_DYNAMICS_THERMAL_HEATER_CONTROLLER_HPP_ diff --git a/src/dynamics/thermal/heatload.cpp b/src/dynamics/thermal/heatload.cpp index 949bfa361..29b8cdb44 100644 --- a/src/dynamics/thermal/heatload.cpp +++ b/src/dynamics/thermal/heatload.cpp @@ -103,4 +103,4 @@ Heatload InitHeatload(const std::vector& time_str, const std::vecto return Heatload(node_id, time_table_s, internal_heatload_table_W); } -} // namespace s2e::dynamics::thermal +} // namespace s2e::dynamics::thermal diff --git a/src/dynamics/thermal/heatload.hpp b/src/dynamics/thermal/heatload.hpp index 0a3106a25..a4e4afb25 100644 --- a/src/dynamics/thermal/heatload.hpp +++ b/src/dynamics/thermal/heatload.hpp @@ -129,6 +129,6 @@ class Heatload { */ Heatload InitHeatload(const std::vector& time_str, const std::vector& internal_heatload_str); -} // namespace s2e::dynamics::thermal +} // namespace s2e::dynamics::thermal #endif // S2E_DYNAMICS_THERMAL_HEATLOAD_HPP_ diff --git a/src/dynamics/thermal/node.cpp b/src/dynamics/thermal/node.cpp index 5c586f634..24440ce5a 100644 --- a/src/dynamics/thermal/node.cpp +++ b/src/dynamics/thermal/node.cpp @@ -189,4 +189,4 @@ Node InitNode(const std::vector& node_str) { return node; } -} // namespace s2e::dynamics::thermal +} // namespace s2e::dynamics::thermal diff --git a/src/dynamics/thermal/node.hpp b/src/dynamics/thermal/node.hpp index fc42b4239..6d9cb0434 100644 --- a/src/dynamics/thermal/node.hpp +++ b/src/dynamics/thermal/node.hpp @@ -172,6 +172,7 @@ class Node { */ Node InitNode(const std::vector& node_str); -}namespace s2e::dynamics::thermal +} // namespace s2e::dynamics::thermal +namespace s2e::dynamics::thermal #endif // S2E_DYNAMICS_THERMAL_NODE_HPP_ diff --git a/src/dynamics/thermal/temperature.cpp b/src/dynamics/thermal/temperature.cpp index 0583ac6c0..fecd97cd4 100644 --- a/src/dynamics/thermal/temperature.cpp +++ b/src/dynamics/thermal/temperature.cpp @@ -391,4 +391,4 @@ Temperature* InitTemperature(const std::string file_name, const double rk_prop_s return temperature; } -} // namespace s2e::dynamics::thermal +} // namespace s2e::dynamics::thermal diff --git a/src/dynamics/thermal/temperature.hpp b/src/dynamics/thermal/temperature.hpp index 8c99db6e1..292e432d5 100644 --- a/src/dynamics/thermal/temperature.hpp +++ b/src/dynamics/thermal/temperature.hpp @@ -45,9 +45,9 @@ class Temperature : public logger::ILoggable { double propagation_time_s_; //!< Incremented time inside class Temperature [s], finish propagation when reaching end_time const environment::SolarRadiationPressureEnvironment* srp_environment_; //!< SolarRadiationPressureEnvironment for calculating solar flux const environment::EarthAlbedo* earth_albedo_; //!< EarthAlbedo object for calculating earth albedo - bool is_calc_enabled_; //!< Whether temperature calculation is enabled - SolarCalcSetting solar_calc_setting_; //!< setting for solar calculation - bool debug_; //!< Activate debug output or not + bool is_calc_enabled_; //!< Whether temperature calculation is enabled + SolarCalcSetting solar_calc_setting_; //!< setting for solar calculation + bool debug_; //!< Activate debug output or not /** * @fn CalcRungeOneStep @@ -58,7 +58,8 @@ class Temperature : public logger::ILoggable { * @param[in] local_celestial_information: LocalCelestialInformation object for calculating radiation * @param[in] node_num: Number of nodes */ - void CalcRungeOneStep(double time_now_s, double time_step_s, const environment::LocalCelestialInformation* local_celestial_information, size_t node_num); + void CalcRungeOneStep(double time_now_s, double time_step_s, const environment::LocalCelestialInformation* local_celestial_information, + size_t node_num); /** * @fn CalcTemperatureDifferentials * @brief Calculate differential of thermal equilibrium equation @@ -119,7 +120,7 @@ class Temperature : public logger::ILoggable { // Getter /** * @fn GetNodes - * + * * @brief Return Nodes * @return std::vector */ @@ -165,9 +166,9 @@ class Temperature : public logger::ILoggable { * @param[in] srp_environment: SolarRadiationPressureEnvironment object for calculating solar flux * @return Temperature* */ -Temperature* InitTemperature(const std::string file_name, const double rk_prop_step_s, const environment::SolarRadiationPressureEnvironment* srp_environment, - const environment::EarthAlbedo* earth_albedo); +Temperature* InitTemperature(const std::string file_name, const double rk_prop_step_s, + const environment::SolarRadiationPressureEnvironment* srp_environment, const environment::EarthAlbedo* earth_albedo); -} // namespace s2e::dynamics::thermal +} // namespace s2e::dynamics::thermal #endif // S2E_DYNAMICS_THERMAL_TEMPERATURE_HPP_ diff --git a/src/environment/global/celestial_information.cpp b/src/environment/global/celestial_information.cpp index af94cfcb6..a8ffde6eb 100644 --- a/src/environment/global/celestial_information.cpp +++ b/src/environment/global/celestial_information.cpp @@ -247,4 +247,4 @@ CelestialInformation* InitCelestialInformation(std::string file_name) { return celestial_info; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/global/celestial_information.hpp b/src/environment/global/celestial_information.hpp index a00ceac53..dbb9777d9 100644 --- a/src/environment/global/celestial_information.hpp +++ b/src/environment/global/celestial_information.hpp @@ -283,6 +283,6 @@ class CelestialInformation : public logger::ILoggable { */ CelestialInformation* InitCelestialInformation(std::string file_name); -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_GLOBAL_CELESTIAL_INFORMATION_HPP_ diff --git a/src/environment/global/clock_generator.cpp b/src/environment/global/clock_generator.cpp index db573baf3..5fd61999c 100644 --- a/src/environment/global/clock_generator.cpp +++ b/src/environment/global/clock_generator.cpp @@ -41,4 +41,4 @@ void ClockGenerator::UpdateComponents(const SimulationTime* simulation_time) { } } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/global/clock_generator.hpp b/src/environment/global/clock_generator.hpp index d11e837f7..87361d772 100644 --- a/src/environment/global/clock_generator.hpp +++ b/src/environment/global/clock_generator.hpp @@ -56,9 +56,9 @@ class ClockGenerator { private: std::vector components_; //!< Component list fot tick - unsigned int timer_count_; //!< Timer count TODO: change to long? + unsigned int timer_count_; //!< Timer count TODO: change to long? }; -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_GLOBAL_CLOCK_GENERATOR_HPP_ diff --git a/src/environment/global/earth_rotation.cpp b/src/environment/global/earth_rotation.cpp index 73d5811c8..f7eedde82 100644 --- a/src/environment/global/earth_rotation.cpp +++ b/src/environment/global/earth_rotation.cpp @@ -278,4 +278,4 @@ EarthRotationMode ConvertEarthRotationMode(const std::string mode) { return rotation_mode; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/global/earth_rotation.hpp b/src/environment/global/earth_rotation.hpp index 25cfa1992..4c1668adb 100644 --- a/src/environment/global/earth_rotation.hpp +++ b/src/environment/global/earth_rotation.hpp @@ -124,6 +124,6 @@ class EarthRotation { EarthRotationMode ConvertEarthRotationMode(const std::string mode); -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_GLOBAL_EARTH_ROTATION_HPP_ diff --git a/src/environment/global/global_environment.cpp b/src/environment/global/global_environment.cpp index 3657594c5..0244387fe 100644 --- a/src/environment/global/global_environment.cpp +++ b/src/environment/global/global_environment.cpp @@ -47,4 +47,4 @@ void GlobalEnvironment::LogSetup(logger::Logger& logger) { void GlobalEnvironment::Reset(void) { simulation_time_->ResetClock(); } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/global/global_environment.hpp b/src/environment/global/global_environment.hpp index f067c33b8..2ce5da7c3 100644 --- a/src/environment/global/global_environment.hpp +++ b/src/environment/global/global_environment.hpp @@ -84,6 +84,6 @@ class GlobalEnvironment { GnssSatellites* gnss_satellites_; //!< GNSS satellites }; -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_GLOBAL_GLOBAL_ENVIRONMENT_HPP_ diff --git a/src/environment/global/gnss_satellites.cpp b/src/environment/global/gnss_satellites.cpp index 9dbf72393..17219887c 100644 --- a/src/environment/global/gnss_satellites.cpp +++ b/src/environment/global/gnss_satellites.cpp @@ -63,7 +63,7 @@ void GnssSatellites::Update(const SimulationTime& simulation_time) { // Get time UTC current_utc = simulation_time.GetCurrentUtc(); s2e::time_system::DateTime current_date_time((size_t)current_utc.year, (size_t)current_utc.month, (size_t)current_utc.day, (size_t)current_utc.hour, - (size_t)current_utc.minute, current_utc.second); + (size_t)current_utc.minute, current_utc.second); current_epoch_time_ = s2e::time_system::EpochTime(current_date_time); // Check interpolation update @@ -229,12 +229,12 @@ GnssSatellites* InitGnssSatellites(const std::string file_name, const EarthRotat // s2e::time_system::DateTime start_date_time((size_t)simulation_time.GetStartYear(), (size_t)simulation_time.GetStartMonth(), - (size_t)simulation_time.GetStartDay(), (size_t)simulation_time.GetStartHour(), - (size_t)simulation_time.GetStartMinute(), simulation_time.GetStartSecond()); + (size_t)simulation_time.GetStartDay(), (size_t)simulation_time.GetStartHour(), + (size_t)simulation_time.GetStartMinute(), simulation_time.GetStartSecond()); s2e::time_system::EpochTime start_epoch_time(start_date_time); gnss_satellites->Initialize(sp3_file_readers, start_epoch_time); return gnss_satellites; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/global/gnss_satellites.hpp b/src/environment/global/gnss_satellites.hpp index 49e73f2a1..c88f86a69 100644 --- a/src/environment/global/gnss_satellites.hpp +++ b/src/environment/global/gnss_satellites.hpp @@ -89,7 +89,8 @@ class GnssSatellites : public logger::ILoggable { * @param [in] time: Target time to get the GNSS satellite. When the argument is not set, the last updated time is used for the calculation. * @return GNSS satellite position at ECEF frame at the time. Or return zero vector when the arguments are out of range. */ - math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time = s2e::time_system::EpochTime(0, 0.0)) const; + math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, + const s2e::time_system::EpochTime time = s2e::time_system::EpochTime(0, 0.0)) const; /** * @fn GetGetClock_s @@ -115,15 +116,15 @@ class GnssSatellites : public logger::ILoggable { private: bool is_calc_enabled_ = false; //!< Flag to manage the GNSS satellite position calculation - std::vector sp3_files_; //!< List of SP3 files - size_t number_of_calculated_gnss_satellites_; //!< Number of calculated GNSS satellites - size_t sp3_file_id_; //!< Current SP3 file ID - s2e::time_system::EpochTime reference_time_; //!< Reference start time of the SP3 handling - size_t reference_interpolation_id_ = 0; //!< Reference epoch ID of the interpolation - s2e::time_system::EpochTime current_epoch_time_; //!< The last updated time + std::vector sp3_files_; //!< List of SP3 files + size_t number_of_calculated_gnss_satellites_; //!< Number of calculated GNSS satellites + size_t sp3_file_id_; //!< Current SP3 file ID + s2e::time_system::EpochTime reference_time_; //!< Reference start time of the SP3 handling + size_t reference_interpolation_id_ = 0; //!< Reference epoch ID of the interpolation + s2e::time_system::EpochTime current_epoch_time_; //!< The last updated time std::vector orbit_; //!< GNSS satellite orbit with interpolation - std::vector clock_; //!< GNSS satellite clock offset with interpolation + std::vector clock_; //!< GNSS satellite clock offset with interpolation // References const EarthRotation& earth_rotation_; //!< Earth rotation @@ -155,6 +156,6 @@ class GnssSatellites : public logger::ILoggable { */ GnssSatellites* InitGnssSatellites(const std::string file_name, const EarthRotation& earth_rotation, const SimulationTime& simulation_time); -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_GLOBAL_GNSS_SATELLITES_HPP_ diff --git a/src/environment/global/hipparcos_catalogue.cpp b/src/environment/global/hipparcos_catalogue.cpp index 83e378997..8888085ac 100644 --- a/src/environment/global/hipparcos_catalogue.cpp +++ b/src/environment/global/hipparcos_catalogue.cpp @@ -102,4 +102,4 @@ HipparcosCatalogue* InitHipparcosCatalogue(std::string file_name) { return hipparcos_catalogue_; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/global/hipparcos_catalogue.hpp b/src/environment/global/hipparcos_catalogue.hpp index 8067bfcd5..23dd55630 100644 --- a/src/environment/global/hipparcos_catalogue.hpp +++ b/src/environment/global/hipparcos_catalogue.hpp @@ -120,6 +120,6 @@ class HipparcosCatalogue : public logger::ILoggable { */ HipparcosCatalogue* InitHipparcosCatalogue(std::string file_name); -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_GLOBAL_HIPPARCOS_CATALOGUE_HPP_ diff --git a/src/environment/global/moon_rotation.cpp b/src/environment/global/moon_rotation.cpp index b40692284..d2f93c659 100644 --- a/src/environment/global/moon_rotation.cpp +++ b/src/environment/global/moon_rotation.cpp @@ -54,4 +54,4 @@ MoonRotationMode ConvertMoonRotationMode(const std::string mode) { return rotation_mode; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/global/moon_rotation.hpp b/src/environment/global/moon_rotation.hpp index 196e2d6a6..1c5ea0d68 100644 --- a/src/environment/global/moon_rotation.hpp +++ b/src/environment/global/moon_rotation.hpp @@ -64,6 +64,6 @@ class MoonRotation { const CelestialInformation &celestial_information_; //!< Celestial Information to get moon orbit }; -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_GLOBAL_MOON_ROTATION_HPP_ diff --git a/src/environment/global/simulation_time.cpp b/src/environment/global/simulation_time.cpp index 600e70440..6438e5011 100644 --- a/src/environment/global/simulation_time.cpp +++ b/src/environment/global/simulation_time.cpp @@ -278,4 +278,4 @@ SimulationTime* InitSimulationTime(std::string file_name) { return simTime; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/global/simulation_time.hpp b/src/environment/global/simulation_time.hpp index eacefcc71..7a2093c19 100644 --- a/src/environment/global/simulation_time.hpp +++ b/src/environment/global/simulation_time.hpp @@ -336,6 +336,6 @@ class SimulationTime : public logger::ILoggable { */ SimulationTime* InitSimulationTime(std::string file_name); -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_GLOBAL_SIMULATION_TIME_HPP_ diff --git a/src/environment/local/atmosphere.cpp b/src/environment/local/atmosphere.cpp index 4659f8c43..0e31d6b17 100644 --- a/src/environment/local/atmosphere.cpp +++ b/src/environment/local/atmosphere.cpp @@ -136,4 +136,4 @@ Atmosphere InitAtmosphere(const std::string initialize_file_path, const LocalCel return atmosphere; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/local/atmosphere.hpp b/src/environment/local/atmosphere.hpp index 197c0972a..81dd063eb 100644 --- a/src/environment/local/atmosphere.hpp +++ b/src/environment/local/atmosphere.hpp @@ -82,8 +82,8 @@ class Atmosphere : public logger::ILoggable { double air_density_kg_m3_; //!< Atmospheric density [kg/m^3] // NRLMSISE-00 model information - std::vector space_weather_table_; //!< Space weather table - bool is_manual_param_used_; //!< Flag to use manual parameters + std::vector space_weather_table_; //!< Space weather table + bool is_manual_param_used_; //!< Flag to use manual parameters // Reference of the following setting parameters https://www.swpc.noaa.gov/phenomena/f107-cm-radio-emissions double manual_daily_f107_; //!< Manual daily f10.7 value double manual_average_f107_; //!< Manual 3-month averaged f10.7 value @@ -118,6 +118,6 @@ class Atmosphere : public logger::ILoggable { Atmosphere InitAtmosphere(const std::string initialize_file_path, const LocalCelestialInformation* local_celestial_information, const SimulationTime* simulation_time); -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_LOCAL_ATMOSPHERE_HPP_ diff --git a/src/environment/local/earth_albedo.cpp b/src/environment/local/earth_albedo.cpp index 1a38af8da..2e1ab6902 100644 --- a/src/environment/local/earth_albedo.cpp +++ b/src/environment/local/earth_albedo.cpp @@ -60,4 +60,4 @@ EarthAlbedo InitEarthAlbedo(std::string initialize_file_path, LocalCelestialInfo return earth_albedo; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/local/earth_albedo.hpp b/src/environment/local/earth_albedo.hpp index 0635afd7a..8fbd6521b 100644 --- a/src/environment/local/earth_albedo.hpp +++ b/src/environment/local/earth_albedo.hpp @@ -104,6 +104,6 @@ class EarthAlbedo : public logger::ILoggable { EarthAlbedo InitEarthAlbedo(std::string initialize_file_path, LocalCelestialInformation* local_celestial_information, SolarRadiationPressureEnvironment* srp_environment); -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_LOCAL_EARTH_ALBEDO_HPP_ diff --git a/src/environment/local/geomagnetic_field.cpp b/src/environment/local/geomagnetic_field.cpp index 5b713a11d..ba49d575d 100644 --- a/src/environment/local/geomagnetic_field.cpp +++ b/src/environment/local/geomagnetic_field.cpp @@ -44,7 +44,7 @@ void GeomagneticField::CalcMagneticField(const double decimal_year, const double void GeomagneticField::AddNoise(double* magnetic_field_array_i_nT) { static math::Vector<3> standard_deviation(random_walk_standard_deviation_nT_); static math::Vector<3> limit(random_walk_limit_nT_); - static RandomWalk<3> random_walk(0.1, standard_deviation, limit); + static randomization::RandomWalk<3> random_walk(0.1, standard_deviation, limit); static s2e::randomization::NormalRand white_noise(0.0, white_noise_standard_deviation_nT_, s2e::randomization::global_randomization.MakeSeed()); @@ -88,4 +88,4 @@ GeomagneticField InitGeomagneticField(std::string initialize_file_path) { return geomagnetic_field; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/local/geomagnetic_field.hpp b/src/environment/local/geomagnetic_field.hpp index 6b9857946..0317952f3 100644 --- a/src/environment/local/geomagnetic_field.hpp +++ b/src/environment/local/geomagnetic_field.hpp @@ -94,6 +94,6 @@ class GeomagneticField : public logger::ILoggable { */ GeomagneticField InitGeomagneticField(std::string initialize_file_path); -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_LOCAL_GEOMAGNETIC_FIELD_HPP_ diff --git a/src/environment/local/local_celestial_information.cpp b/src/environment/local/local_celestial_information.cpp index 40d78c536..dab63eec9 100644 --- a/src/environment/local/local_celestial_information.cpp +++ b/src/environment/local/local_celestial_information.cpp @@ -200,4 +200,4 @@ std::string LocalCelestialInformation::GetLogValue() const { return str_tmp; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/local/local_celestial_information.hpp b/src/environment/local/local_celestial_information.hpp index 93a2c13c8..1d15a2ca6 100644 --- a/src/environment/local/local_celestial_information.hpp +++ b/src/environment/local/local_celestial_information.hpp @@ -122,6 +122,6 @@ class LocalCelestialInformation : public logger::ILoggable { const math::Vector<3> angular_velocity_b); }; -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_LOCAL_LOCAL_CELESTIAL_INFORMATION_HPP_ diff --git a/src/environment/local/local_environment.cpp b/src/environment/local/local_environment.cpp index 3fbe777b1..3f53239d1 100644 --- a/src/environment/local/local_environment.cpp +++ b/src/environment/local/local_environment.cpp @@ -8,7 +8,7 @@ #include "dynamics/orbit/orbit.hpp" #include "setting_file_reader/initialize_file_access.hpp" -namespace s2e::environment{ +namespace s2e::environment { LocalEnvironment::LocalEnvironment(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id) { @@ -79,4 +79,4 @@ void LocalEnvironment::LogSetup(logger::Logger& logger) { logger.AddLogList(celestial_information_); } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/local/local_environment.hpp b/src/environment/local/local_environment.hpp index f6d698f67..ef671a499 100644 --- a/src/environment/local/local_environment.hpp +++ b/src/environment/local/local_environment.hpp @@ -15,8 +15,6 @@ #include "simulation/simulation_configuration.hpp" #include "solar_radiation_pressure_environment.hpp" -class dynamics::Dynamics; - namespace s2e::environment { /** @@ -32,7 +30,8 @@ class LocalEnvironment { * @param [in] global_environment: Global environment * @param [in] spacecraft_id: Satellite ID */ - LocalEnvironment(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id); + LocalEnvironment(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, + const int spacecraft_id); /** * @fn ~LocalEnvironment * @brief Destructor @@ -93,9 +92,10 @@ class LocalEnvironment { * @param [in] global_environment: Global environment * @param [in] spacecraft_id: Satellite ID */ - void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, const int spacecraft_id); + void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, + const int spacecraft_id); }; -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_LOCAL_LOCAL_ENVIRONMENT_HPP_ diff --git a/src/environment/local/solar_radiation_pressure_environment.cpp b/src/environment/local/solar_radiation_pressure_environment.cpp index ba259567b..c571f309f 100644 --- a/src/environment/local/solar_radiation_pressure_environment.cpp +++ b/src/environment/local/solar_radiation_pressure_environment.cpp @@ -122,4 +122,4 @@ SolarRadiationPressureEnvironment InitSolarRadiationPressureEnvironment(std::str return srp_env; } -} // namespace s2e::environment +} // namespace s2e::environment diff --git a/src/environment/local/solar_radiation_pressure_environment.hpp b/src/environment/local/solar_radiation_pressure_environment.hpp index f49bc8106..98f833b6c 100644 --- a/src/environment/local/solar_radiation_pressure_environment.hpp +++ b/src/environment/local/solar_radiation_pressure_environment.hpp @@ -124,6 +124,6 @@ class SolarRadiationPressureEnvironment : public logger::ILoggable { SolarRadiationPressureEnvironment InitSolarRadiationPressureEnvironment(std::string initialize_file_path, LocalCelestialInformation* local_celestial_information); -} // namespace s2e::environment +} // namespace s2e::environment #endif // S2E_ENVIRONMENT_LOCAL_SOLAR_RADIATION_PRESSURE_ENVIRONMENT_HPP_ diff --git a/src/logger/initialize_log.cpp b/src/logger/initialize_log.cpp index 30c1036bb..84098e57b 100644 --- a/src/logger/initialize_log.cpp +++ b/src/logger/initialize_log.cpp @@ -31,4 +31,4 @@ Logger* InitMonteCarloLog(std::string file_name, bool enable) { return log; } -} // namespace s2e::logger +} // namespace s2e::logger diff --git a/src/logger/initialize_log.hpp b/src/logger/initialize_log.hpp index 630425134..9d7c7a45b 100644 --- a/src/logger/initialize_log.hpp +++ b/src/logger/initialize_log.hpp @@ -25,6 +25,6 @@ Logger* InitLog(std::string file_name); */ Logger* InitMonteCarloLog(std::string file_name, bool enable); -} // namespace s2e::logger +} // namespace s2e::logger #endif // S2E_LIBRARY_LOGGER_INITIALIZE_LOG_HPP_ diff --git a/src/logger/log_utility.hpp b/src/logger/log_utility.hpp index 40f47f8f1..3328d358e 100644 --- a/src/logger/log_utility.hpp +++ b/src/logger/log_utility.hpp @@ -158,6 +158,6 @@ std::string WriteQuaternion(const std::string name, const std::string frame) { return str_tmp.str(); } -} // namespace s2e::logger +} // namespace s2e::logger #endif // S2E_LIBRARY_LOGGER_LOG_UTILITY_HPP_ diff --git a/src/logger/loggable.hpp b/src/logger/loggable.hpp index 064bbf952..b8616f1a7 100644 --- a/src/logger/loggable.hpp +++ b/src/logger/loggable.hpp @@ -36,6 +36,6 @@ class ILoggable { bool is_log_enabled_ = true; //!< Log enable flag }; -} // namespace s2e::logger +} // namespace s2e::logger #endif // S2E_LIBRARY_LOGGER_LOGGABLE_HPP_ diff --git a/src/logger/logger.cpp b/src/logger/logger.cpp index 4299127db..3bc823cad 100644 --- a/src/logger/logger.cpp +++ b/src/logger/logger.cpp @@ -106,4 +106,4 @@ void Logger::CopyFileToLogDirectory(const fs::path &ini_file_name) { return; } -} // namespace s2e::logger +} // namespace s2e::logger diff --git a/src/logger/logger.hpp b/src/logger/logger.hpp index fa85e21ed..066c6814f 100644 --- a/src/logger/logger.hpp +++ b/src/logger/logger.hpp @@ -123,6 +123,6 @@ class Logger { std::filesystem::path CreateDirectory(const std::filesystem::path &data_path, const std::string &time); }; -} // namespace s2e::logger +} // namespace s2e::logger #endif // S2E_LIBRARY_LOGGER_LOGGER_HPP_ diff --git a/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp b/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp index c000ca49e..c467efe37 100644 --- a/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp +++ b/src/math_physics/atmosphere/wrapper_nrlmsise00.cpp @@ -23,6 +23,8 @@ extern "C" { using namespace std; +namespace s2e::atmosphere { + /* ------------------------------------------------------------------- */ /* ------------------------------ DEFINES ---------------------------- */ /* ------------------------------------------------------------------- */ @@ -305,3 +307,5 @@ size_t GetSpaceWeatherTable_(double decyear, double endsec, const string& filena return table.size(); } + +} // namespace s2e::atmosphere diff --git a/src/math_physics/atmosphere/wrapper_nrlmsise00.hpp b/src/math_physics/atmosphere/wrapper_nrlmsise00.hpp index 88bf51ee2..ea1e6e43f 100644 --- a/src/math_physics/atmosphere/wrapper_nrlmsise00.hpp +++ b/src/math_physics/atmosphere/wrapper_nrlmsise00.hpp @@ -12,6 +12,8 @@ #include #include +namespace s2e::atmosphere { + /** * @struct nrlmsise_table * @brief Parameters for NRLMSISE calculation @@ -74,4 +76,6 @@ size_t GetSpaceWeatherTable_(double decyear, double endsec, const std::string& f #define __inline_double double #endif +} // namespace s2e::atmosphere + #endif // S2E_LIBRARY_EXTERNAL_NRLMSISE00_WRAPPER_NRLMSISE00__HPP_ diff --git a/src/math_physics/gnss/antex_file_reader.hpp b/src/math_physics/gnss/antex_file_reader.hpp index 9e30ebce6..67efe3133 100644 --- a/src/math_physics/gnss/antex_file_reader.hpp +++ b/src/math_physics/gnss/antex_file_reader.hpp @@ -256,8 +256,8 @@ class AntexSatelliteData { private: std::string antenna_type_; //!< Antenna type std::string serial_number_; //!< Serial number or satellite code - s2e::time_system::DateTime valid_start_time_; //!< Valid start time - s2e::time_system::DateTime valid_end_time_; //!< Valid end time (The latest data does not have the end time) + s2e::time_system::DateTime valid_start_time_; //!< Valid start time + s2e::time_system::DateTime valid_end_time_; //!< Valid end time (The latest data does not have the end time) size_t number_of_frequency_ = 1; //!< Number of frequency std::vector phase_center_data_; //!< Phase center data for each frequency }; diff --git a/src/math_physics/gnss/sp3_file_reader.hpp b/src/math_physics/gnss/sp3_file_reader.hpp index 1b7164dbf..e551e186d 100644 --- a/src/math_physics/gnss/sp3_file_reader.hpp +++ b/src/math_physics/gnss/sp3_file_reader.hpp @@ -51,16 +51,16 @@ enum class Sp3OrbitType { struct Sp3Header { // 1st line information // version -> not implemented yet - Sp3Mode mode_; //!< position or velocity + Sp3Mode mode_; //!< position or velocity s2e::time_system::DateTime start_epoch_; //!< Time of start epoch - size_t number_of_epoch_ = 0; //!< Number of epoch in the SP3 file - std::string used_data_; //!< Used data to generate the SP3 file - std::string coordinate_system_; //!< Coordinate system for the position and velocity data - Sp3OrbitType orbit_type_; //!< Orbit type - std::string agency_name_; //!< Agency name who generates the SP3 file + size_t number_of_epoch_ = 0; //!< Number of epoch in the SP3 file + std::string used_data_; //!< Used data to generate the SP3 file + std::string coordinate_system_; //!< Coordinate system for the position and velocity data + Sp3OrbitType orbit_type_; //!< Orbit type + std::string agency_name_; //!< Agency name who generates the SP3 file // 2nd line information - s2e::time_system::GpsTime start_gps_time_; //!< Start time of orbit + s2e::time_system::GpsTime start_gps_time_; //!< Start time of orbit double epoch_interval_s_ = 1.0; //!< Epoch interval (0.0, 100000.0) size_t start_time_mjday_; //!< Start time of the orbit data (44244 = 6th Jan. 1980) [Modified Julian day] double start_time_mjday_fractional_day_ = 0.0; //!< Fractional part of the start time [0.0, 1.0) [day] @@ -183,7 +183,7 @@ class Sp3FileReader { size_t SearchNearestEpochId(const s2e::time_system::EpochTime time); private: - Sp3Header header_; //!< SP3 header information + Sp3Header header_; //!< SP3 header information std::vector epoch_; //!< Epoch data list // Orbit and clock data (Use as position_clock_[satellite_id][epoch_id]) diff --git a/src/math_physics/numerical_integration/numerical_integrator.hpp b/src/math_physics/numerical_integration/numerical_integrator.hpp index 461bfa225..ae708a753 100644 --- a/src/math_physics/numerical_integration/numerical_integrator.hpp +++ b/src/math_physics/numerical_integration/numerical_integrator.hpp @@ -32,7 +32,7 @@ class NumericalIntegrator { * @fn ~NumericalIntegrator * @brief Destructor */ - inline virtual ~NumericalIntegrator(){}; + inline virtual ~NumericalIntegrator() {}; /** * @fn Integrate diff --git a/src/math_physics/numerical_integration/runge_kutta.hpp b/src/math_physics/numerical_integration/runge_kutta.hpp index d2072d4cb..a24b4f53b 100644 --- a/src/math_physics/numerical_integration/runge_kutta.hpp +++ b/src/math_physics/numerical_integration/runge_kutta.hpp @@ -29,7 +29,7 @@ class RungeKutta : public NumericalIntegrator { * @fn ~RungeKutta * @brief Destructor */ - inline virtual ~RungeKutta(){}; + inline virtual ~RungeKutta() {}; /** * @fn Integrate diff --git a/src/math_physics/numerical_integration/test_runge_kutta.cpp b/src/math_physics/numerical_integration/test_runge_kutta.cpp index 2223a9a9b..60293ab18 100644 --- a/src/math_physics/numerical_integration/test_runge_kutta.cpp +++ b/src/math_physics/numerical_integration/test_runge_kutta.cpp @@ -113,7 +113,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRkf) { double step_width_s = 0.1; s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, - s2e::numerical_integration::NumericalIntegrationMethod::kRkf); + s2e::numerical_integration::NumericalIntegrationMethod::kRkf); math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); @@ -139,7 +139,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerDp5) { double step_width_s = 0.1; s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, - s2e::numerical_integration::NumericalIntegrationMethod::kDp5); + s2e::numerical_integration::NumericalIntegrationMethod::kDp5); math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); diff --git a/src/math_physics/randomization/global_randomization.hpp b/src/math_physics/randomization/global_randomization.hpp index fd9dac4a0..df464b974 100644 --- a/src/math_physics/randomization/global_randomization.hpp +++ b/src/math_physics/randomization/global_randomization.hpp @@ -34,9 +34,9 @@ class GlobalRandomization { long MakeSeed(); private: - static const unsigned int kMaxSeed = 0xffffffff; //!< Maximum value of seed + static const unsigned int kMaxSeed = 0xffffffff; //!< Maximum value of seed s2e::randomization::MinimalStandardLcg base_randomizer_; //!< Base of global randomization - long seed_; //!< Seed of global randomization + long seed_; //!< Seed of global randomization }; extern GlobalRandomization global_randomization; //!< Global randomization diff --git a/src/math_physics/randomization/random_walk.hpp b/src/math_physics/randomization/random_walk.hpp index ee7db5bb6..aed70512b 100644 --- a/src/math_physics/randomization/random_walk.hpp +++ b/src/math_physics/randomization/random_walk.hpp @@ -10,6 +10,8 @@ #include "../math/vector.hpp" #include "./normal_randomization.hpp" +namespace s2e::randomization { + /** * @class RandomWalk * @brief Class to calculate random wark value @@ -36,10 +38,12 @@ class RandomWalk : public math::OrdinaryDifferentialEquation { virtual void DerivativeFunction(double x, const math::Vector& state, math::Vector& rhs); private: - math::Vector limit_; //!< Limit of random walk - s2e::randomization::NormalRand normal_randomizer_[N]; //!< Random walk excitation noise + math::Vector limit_; //!< Limit of random walk + NormalRand normal_randomizer_[N]; //!< Random walk excitation noise }; +} // namespace s2e::randomization + #include "random_walk_template_functions.hpp" // template function definisions. #endif // S2E_LIBRARY_RANDOMIZATION_RANDOM_WALK_HPP_ \ No newline at end of file diff --git a/src/math_physics/randomization/random_walk_template_functions.hpp b/src/math_physics/randomization/random_walk_template_functions.hpp index 68fa37eea..1f782b7cb 100644 --- a/src/math_physics/randomization/random_walk_template_functions.hpp +++ b/src/math_physics/randomization/random_walk_template_functions.hpp @@ -9,6 +9,8 @@ #include #include +namespace s2e::randomization { + template RandomWalk::RandomWalk(double step_width_s, const math::Vector& standard_deviation, const math::Vector& limit) : math::OrdinaryDifferentialEquation(step_width_s), limit_(limit) { @@ -32,4 +34,6 @@ void RandomWalk::DerivativeFunction(double x, const math::Vector& state, m } } +} // namespace s2e::randomization + #endif // S2E_LIBRARY_RANDOMIZATION_RANDOM_WALK_TEMPLATE_FUNCTIONS_HPP_ diff --git a/src/s2e.cpp b/src/s2e.cpp index 853b04bee..3c9da8fa6 100644 --- a/src/s2e.cpp +++ b/src/s2e.cpp @@ -82,4 +82,4 @@ int main(int argc, char *argv[]) return EXIT_SUCCESS; } -} // namespace s2e +} // namespace s2e diff --git a/src/setting_file_reader/c2a_command_database.cpp b/src/setting_file_reader/c2a_command_database.cpp index 76572c1ab..7811b36ee 100644 --- a/src/setting_file_reader/c2a_command_database.cpp +++ b/src/setting_file_reader/c2a_command_database.cpp @@ -161,4 +161,4 @@ void DecodeC2aCommandArgument(const C2aArgumentType type, const std::string argu } } -} // namespace s2e::setting_file_reader +} // namespace s2e::setting_file_reader diff --git a/src/setting_file_reader/c2a_command_database.hpp b/src/setting_file_reader/c2a_command_database.hpp index fce12028f..f8905f49a 100644 --- a/src/setting_file_reader/c2a_command_database.hpp +++ b/src/setting_file_reader/c2a_command_database.hpp @@ -117,6 +117,6 @@ class C2aCommandDatabase { */ void DecodeC2aCommandArgument(const C2aArgumentType type, const std::string argument_string, uint8_t* param, size_t& size_param); -} // namespace s2e::setting_file_reader +} // namespace s2e::setting_file_reader #endif // S2E_LIBRARY_INITIALIZE_C2A_COMMAND_DATABASE_HPP_ diff --git a/src/setting_file_reader/initialize_file_access.cpp b/src/setting_file_reader/initialize_file_access.cpp index 4ca4c0769..187b55998 100644 --- a/src/setting_file_reader/initialize_file_access.cpp +++ b/src/setting_file_reader/initialize_file_access.cpp @@ -286,4 +286,4 @@ void IniAccess::ReadCsvString(std::vector>& output_valu } } -} // namespace s2e::setting_file_reader +} // namespace s2e::setting_file_reader diff --git a/src/setting_file_reader/initialize_file_access.hpp b/src/setting_file_reader/initialize_file_access.hpp index 9af1a3a8a..e63cd424f 100644 --- a/src/setting_file_reader/initialize_file_access.hpp +++ b/src/setting_file_reader/initialize_file_access.hpp @@ -218,6 +218,6 @@ void IniAccess::ReadVector(const char* section_name, const char* key_name, math: } } -} // namespace s2e::setting_file_reader +} // namespace s2e::setting_file_reader #endif // S2E_LIBRARY_INITIALIZE_INITIALIZE_FILE_ACCESS_HPP_ diff --git a/src/setting_file_reader/wings_operation_file.cpp b/src/setting_file_reader/wings_operation_file.cpp index 4b9b56d0a..78efa4d9d 100644 --- a/src/setting_file_reader/wings_operation_file.cpp +++ b/src/setting_file_reader/wings_operation_file.cpp @@ -48,4 +48,4 @@ std::string WingsOperationFile::GetLatestLine() { return line; } -} // namespace s2e::setting_file_reader +} // namespace s2e::setting_file_reader diff --git a/src/simulation/case/simulation_case.cpp b/src/simulation/case/simulation_case.cpp index cda5c0786..e8cfb56e6 100644 --- a/src/simulation/case/simulation_case.cpp +++ b/src/simulation/case/simulation_case.cpp @@ -108,4 +108,4 @@ void SimulationCase::InitializeSimulationConfiguration(const std::string initial global_environment_->LogSetup(*(simulation_configuration_.main_logger_)); } -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/case/simulation_case.hpp b/src/simulation/case/simulation_case.hpp index 47800dee7..26f012dc4 100644 --- a/src/simulation/case/simulation_case.hpp +++ b/src/simulation/case/simulation_case.hpp @@ -77,8 +77,8 @@ class SimulationCase : public logger::ILoggable { inline const environment::GlobalEnvironment& GetGlobalEnvironment() const { return *global_environment_; } protected: - SimulationConfiguration simulation_configuration_; //!< Simulation setting - environment::GlobalEnvironment* global_environment_; //!< Global Environment + SimulationConfiguration simulation_configuration_; //!< Simulation setting + environment::GlobalEnvironment* global_environment_; //!< Global Environment /** * @fn InitializeSimulationConfiguration @@ -100,6 +100,6 @@ class SimulationCase : public logger::ILoggable { virtual void UpdateTargetObjects() = 0; }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_CASE_SIMULATION_CASE_HPP_ diff --git a/src/simulation/ground_station/ground_station.cpp b/src/simulation/ground_station/ground_station.cpp index d6f230149..f0d233936 100644 --- a/src/simulation/ground_station/ground_station.cpp +++ b/src/simulation/ground_station/ground_station.cpp @@ -93,4 +93,4 @@ std::string GroundStation::GetLogValue() const { return str_tmp; } -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/ground_station/ground_station.hpp b/src/simulation/ground_station/ground_station.hpp index 12ca04fec..c88f96666 100644 --- a/src/simulation/ground_station/ground_station.hpp +++ b/src/simulation/ground_station/ground_station.hpp @@ -95,8 +95,8 @@ class GroundStation : public logger::ILoggable { protected: unsigned int ground_station_id_; //!< Ground station ID geodesy::GeodeticPosition geodetic_position_; //!< Ground Station Position in the geodetic frame - math::Vector<3> position_ecef_m_{0.0}; //!< Ground Station Position in the ECEF frame [m] - math::Vector<3> position_i_m_{0.0}; //!< Ground Station Position in the inertial frame [m] + math::Vector<3> position_ecef_m_{0.0}; //!< Ground Station Position in the ECEF frame [m] + math::Vector<3> position_i_m_{0.0}; //!< Ground Station Position in the inertial frame [m] double elevation_limit_angle_deg_; //!< Minimum elevation angle to work the ground station [deg] std::map is_visible_; //!< Visible flag for each spacecraft ID (not care antenna) @@ -111,6 +111,6 @@ class GroundStation : public logger::ILoggable { bool CalcIsVisible(const math::Vector<3> spacecraft_position_ecef_m); }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_GROUND_STATION_GROUND_STATION_HPP_ diff --git a/src/simulation/hils/hils_port_manager.cpp b/src/simulation/hils/hils_port_manager.cpp index ee0e7f1e1..8f68bff4f 100644 --- a/src/simulation/hils/hils_port_manager.cpp +++ b/src/simulation/hils/hils_port_manager.cpp @@ -261,4 +261,4 @@ int HilsPortManager::I2cControllerSend(unsigned int port_id, const unsigned char return UartSend(port_id, buffer, offset, length); } -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/hils/hils_port_manager.hpp b/src/simulation/hils/hils_port_manager.hpp index 8753429e6..cd04dc173 100644 --- a/src/simulation/hils/hils_port_manager.hpp +++ b/src/simulation/hils/hils_port_manager.hpp @@ -170,6 +170,6 @@ class HilsPortManager { #endif }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_HILS_HILS_PORT_MANAGER_HPP_ diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp index 0a8cfb0e5..967b92b1d 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.cpp @@ -318,4 +318,4 @@ void InitializedMonteCarloParameters::GenerateQuaternionNormal() { } } -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp index 6b7493ee4..c44529766 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp @@ -234,6 +234,6 @@ void InitializedMonteCarloParameters::GetRandomizedVector(math::Vector(size, math::Quaternion(0, 0, 0, 1))); } -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/multiple_spacecraft/relative_information.hpp b/src/simulation/multiple_spacecraft/relative_information.hpp index e0dbf1a78..9e77403d0 100644 --- a/src/simulation/multiple_spacecraft/relative_information.hpp +++ b/src/simulation/multiple_spacecraft/relative_information.hpp @@ -172,6 +172,6 @@ class RelativeInformation : public logger::ILoggable { void ResizeLists(); }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_MULTIPLE_SPACECRAFT_RELATIVE_INFORMATION_HPP_ diff --git a/src/simulation/simulation_configuration.hpp b/src/simulation/simulation_configuration.hpp index d8ca3078f..e3adeb87d 100644 --- a/src/simulation/simulation_configuration.hpp +++ b/src/simulation/simulation_configuration.hpp @@ -19,7 +19,7 @@ namespace s2e::simulation { */ struct SimulationConfiguration { std::string initialize_base_file_name_; //!< Base file name for initialization - logger::Logger* main_logger_; //!< Main logger + logger::Logger* main_logger_; //!< Main logger unsigned int number_of_simulated_spacecraft_; //!< Number of simulated spacecraft std::vector spacecraft_file_list_; //!< File name list for spacecraft initialization @@ -37,6 +37,6 @@ struct SimulationConfiguration { ~SimulationConfiguration() { delete main_logger_; } }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_SIMULATION_CONFIGURATION_HPP_ diff --git a/src/simulation/spacecraft/installed_components.cpp b/src/simulation/spacecraft/installed_components.cpp index db5899965..7302e2ed5 100644 --- a/src/simulation/spacecraft/installed_components.cpp +++ b/src/simulation/spacecraft/installed_components.cpp @@ -21,4 +21,4 @@ math::Vector<3> InstalledComponents::GenerateTorque_b_Nm() { void InstalledComponents::LogSetup(logger::Logger& logger) { UNUSED(logger); } -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/installed_components.hpp b/src/simulation/spacecraft/installed_components.hpp index d0a731355..5b9f44b8c 100644 --- a/src/simulation/spacecraft/installed_components.hpp +++ b/src/simulation/spacecraft/installed_components.hpp @@ -51,6 +51,6 @@ class InstalledComponents { virtual void LogSetup(logger::Logger& logger); }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_SPACECRAFT_INSTALLED_COMPONENTS_HPP_ diff --git a/src/simulation/spacecraft/spacecraft.cpp b/src/simulation/spacecraft/spacecraft.cpp index b0efbad8b..36a6f91a8 100644 --- a/src/simulation/spacecraft/spacecraft.cpp +++ b/src/simulation/spacecraft/spacecraft.cpp @@ -10,8 +10,8 @@ namespace s2e::simulation { -Spacecraft::Spacecraft(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const int spacecraft_id, - RelativeInformation* relative_information) +Spacecraft::Spacecraft(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, + const int spacecraft_id, RelativeInformation* relative_information) : spacecraft_id_(spacecraft_id) { Initialize(simulation_configuration, global_environment, spacecraft_id, relative_information); } @@ -32,8 +32,8 @@ void Spacecraft::Initialize(const SimulationConfiguration* simulation_configurat clock_generator_.ClearTimerCount(); structure_ = new Structure(simulation_configuration, spacecraft_id); local_environment_ = new environment::LocalEnvironment(simulation_configuration, global_environment, spacecraft_id); - dynamics_ = new dynamics::dynamics::Dynamics(simulation_configuration, &(global_environment->GetSimulationTime()), local_environment_, spacecraft_id, structure_, - relative_information); + dynamics_ = new dynamics::dynamics::Dynamics(simulation_configuration, &(global_environment->GetSimulationTime()), local_environment_, + spacecraft_id, structure_, relative_information); disturbances_ = new disturbances::Disturbances(simulation_configuration, spacecraft_id, structure_, global_environment); simulation_configuration->main_logger_->CopyFileToLogDirectory(simulation_configuration->spacecraft_file_list_[spacecraft_id]); @@ -77,4 +77,4 @@ void Spacecraft::Update(const environment::SimulationTime* simulation_time) { void Spacecraft::Clear(void) { dynamics_->ClearForceTorque(); } -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/spacecraft.hpp b/src/simulation/spacecraft/spacecraft.hpp index 5b490ec1e..d1e99a444 100644 --- a/src/simulation/spacecraft/spacecraft.hpp +++ b/src/simulation/spacecraft/spacecraft.hpp @@ -27,8 +27,8 @@ class Spacecraft { * @fn Spacecraft * @brief Constructor for single satellite simulation */ - Spacecraft(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const int spacecraft_id, - RelativeInformation* relative_information = nullptr); + Spacecraft(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, + const int spacecraft_id, RelativeInformation* relative_information = nullptr); /** * @fn ~Spacecraft @@ -99,16 +99,16 @@ class Spacecraft { inline unsigned int GetSpacecraftId() const { return spacecraft_id_; } protected: - environment::ClockGenerator clock_generator_; //!< Origin of clock for the spacecraft - dynamics::dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft - RelativeInformation* relative_information_; //!< Relative information with respect to the other spacecraft - environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft - disturbances::Disturbances* disturbances_; //!< Disturbance information acting on the spacecraft - Structure* structure_; //!< Structure information of the spacecraft - InstalledComponents* components_; //!< Components information installed on the spacecraft - const unsigned int spacecraft_id_; //!< ID of the spacecraft + environment::ClockGenerator clock_generator_; //!< Origin of clock for the spacecraft + dynamics::dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft + RelativeInformation* relative_information_; //!< Relative information with respect to the other spacecraft + environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft + disturbances::Disturbances* disturbances_; //!< Disturbance information acting on the spacecraft + Structure* structure_; //!< Structure information of the spacecraft + InstalledComponents* components_; //!< Components information installed on the spacecraft + const unsigned int spacecraft_id_; //!< ID of the spacecraft }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_SPACECRAFT_SPACECRAFT_HPP_ diff --git a/src/simulation/spacecraft/structure/initialize_structure.cpp b/src/simulation/spacecraft/structure/initialize_structure.cpp index b52091e23..44ed97d14 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.cpp +++ b/src/simulation/spacecraft/structure/initialize_structure.cpp @@ -122,4 +122,4 @@ ResidualMagneticMoment InitResidualMagneticMoment(std::string file_name) { return rmm_params; } -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/initialize_structure.hpp b/src/simulation/spacecraft/structure/initialize_structure.hpp index 53065ff2e..0ab741839 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.hpp +++ b/src/simulation/spacecraft/structure/initialize_structure.hpp @@ -28,6 +28,6 @@ std::vector InitSurfaces(std::string file_name); */ ResidualMagneticMoment InitResidualMagneticMoment(std::string file_name); -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_INITIALIZE_STRUCTURE_HPP_ diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.cpp b/src/simulation/spacecraft/structure/kinematics_parameters.cpp index 403487a3f..0734e849d 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.cpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.cpp @@ -10,4 +10,4 @@ namespace s2e::simulation { KinematicsParameters::KinematicsParameters(math::Vector<3> center_of_gravity_b_m, double mass_kg, math::Matrix<3, 3> inertia_tensor_b_kgm2) : center_of_gravity_b_m_(center_of_gravity_b_m), mass_kg_(mass_kg), inertia_tensor_b_kgm2_(inertia_tensor_b_kgm2) {} -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.hpp b/src/simulation/spacecraft/structure/kinematics_parameters.hpp index 2a1805dc3..c4dfadeb2 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.hpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.hpp @@ -26,7 +26,7 @@ class KinematicsParameters { * @fn ~KinematicsParameters * @brief Destructor */ - ~KinematicsParameters(){}; + ~KinematicsParameters() {}; // Getter /** @@ -88,6 +88,6 @@ class KinematicsParameters { math::Matrix<3, 3> inertia_tensor_b_kgm2_; //!< Inertia tensor at body frame [kgm2] }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_KINEMATICS_PARAMETERS_HPP_ diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp index 0f8fc1377..7f7108f61 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp @@ -14,4 +14,4 @@ ResidualMagneticMoment::ResidualMagneticMoment(const math::Vector<3> constant_va random_walk_limit_Am2_(random_walk_limit_Am2), random_noise_standard_deviation_Am2_(random_noise_standard_deviation_Am2) {} -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp index 138e8ddf0..ae06bb85d 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp @@ -20,13 +20,13 @@ class ResidualMagneticMoment { * @fn ResidualMagneticMoment * @brief Constructor */ - ResidualMagneticMoment(const math::Vector<3> constant_value_b_Am2_, const double random_walk_standard_deviation_Am2, const double random_walk_limit_Am2, - const double random_noise_standard_deviation_Am2); + ResidualMagneticMoment(const math::Vector<3> constant_value_b_Am2_, const double random_walk_standard_deviation_Am2, + const double random_walk_limit_Am2, const double random_noise_standard_deviation_Am2); /** * @fn ~ResidualMagneticMoment * @brief Destructor */ - ~ResidualMagneticMoment(){}; + ~ResidualMagneticMoment() {}; // Getter /** @@ -65,12 +65,12 @@ class ResidualMagneticMoment { inline void AddRmmConstant_b_Am2(const math::Vector<3> rmm_const_b_Am2) { constant_value_b_Am2_ += rmm_const_b_Am2; } private: - math::Vector<3> constant_value_b_Am2_; //!< Constant value of RMM at body frame [Am2] + math::Vector<3> constant_value_b_Am2_; //!< Constant value of RMM at body frame [Am2] double random_walk_standard_deviation_Am2_; //!< Random walk standard deviation of RMM [Am2] double random_walk_limit_Am2_; //!< Random walk limit of RMM [Am2] double random_noise_standard_deviation_Am2_; //!< Standard deviation of white noise of RMM [Am2] }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_RESIDUAL_MAGNETIC_MOMENT_HPP_ \ No newline at end of file diff --git a/src/simulation/spacecraft/structure/structure.cpp b/src/simulation/spacecraft/structure/structure.cpp index ffbe55e05..72260265d 100644 --- a/src/simulation/spacecraft/structure/structure.cpp +++ b/src/simulation/spacecraft/structure/structure.cpp @@ -31,4 +31,4 @@ void Structure::Initialize(const SimulationConfiguration* simulation_configurati residual_magnetic_moment_ = new ResidualMagneticMoment(InitResidualMagneticMoment(ini_fname)); } -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/structure.hpp b/src/simulation/spacecraft/structure/structure.hpp index c56943f58..fe099cd04 100644 --- a/src/simulation/spacecraft/structure/structure.hpp +++ b/src/simulation/spacecraft/structure/structure.hpp @@ -76,6 +76,6 @@ class Structure { ResidualMagneticMoment* residual_magnetic_moment_; //!< Residual Magnetic Moment }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_STRUCTURE_HPP_ diff --git a/src/simulation/spacecraft/structure/surface.cpp b/src/simulation/spacecraft/structure/surface.cpp index 6ea2356b0..16b8b5f24 100644 --- a/src/simulation/spacecraft/structure/surface.cpp +++ b/src/simulation/spacecraft/structure/surface.cpp @@ -16,4 +16,4 @@ Surface::Surface(const math::Vector<3> position_b_m, const math::Vector<3> norma specularity_(specularity), air_specularity_(air_specularity) {} -} // namespace s2e::simulation +} // namespace s2e::simulation diff --git a/src/simulation/spacecraft/structure/surface.hpp b/src/simulation/spacecraft/structure/surface.hpp index 5f9007fe4..73a09d6e6 100644 --- a/src/simulation/spacecraft/structure/surface.hpp +++ b/src/simulation/spacecraft/structure/surface.hpp @@ -26,7 +26,7 @@ class Surface { * @fn ~Surface * @brief Destructor */ - ~Surface(){}; + ~Surface() {}; // Getter /** @@ -115,6 +115,6 @@ class Surface { double air_specularity_; //!< Specularity for air drag }; -} // namespace s2e::simulation +} // namespace s2e::simulation #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_SURFACE_HPP_ diff --git a/src/simulation_sample/case/sample_case.cpp b/src/simulation_sample/case/sample_case.cpp index 5487e0e08..4697ecd07 100644 --- a/src/simulation_sample/case/sample_case.cpp +++ b/src/simulation_sample/case/sample_case.cpp @@ -50,4 +50,4 @@ std::string SampleCase::GetLogValue() const { return str_tmp; } -} // namespace s2e::sample +} // namespace s2e::sample diff --git a/src/simulation_sample/case/sample_case.hpp b/src/simulation_sample/case/sample_case.hpp index 66302d589..af3fc5b74 100644 --- a/src/simulation_sample/case/sample_case.hpp +++ b/src/simulation_sample/case/sample_case.hpp @@ -59,6 +59,6 @@ class SampleCase : public simulation::SimulationCase { void UpdateTargetObjects(); }; -} // namespace s2e::sample +} // namespace s2e::sample #endif // S2E_SIMULATION_SAMPLE_CASE_SAMPLE_CASE_HPP_ diff --git a/src/simulation_sample/ground_station/sample_ground_station.cpp b/src/simulation_sample/ground_station/sample_ground_station.cpp index 5489d4423..4ec523260 100644 --- a/src/simulation_sample/ground_station/sample_ground_station.cpp +++ b/src/simulation_sample/ground_station/sample_ground_station.cpp @@ -26,4 +26,4 @@ void SampleGroundStation::Update(const environment::EarthRotation& celestial_rot components_->GetGsCalculator()->Update(spacecraft, spacecraft.GetInstalledComponents().GetAntenna(), *this, *(components_->GetAntenna())); } -} // namespace s2e::sample +} // namespace s2e::sample diff --git a/src/simulation_sample/ground_station/sample_ground_station.hpp b/src/simulation_sample/ground_station/sample_ground_station.hpp index 9acc8d613..aafafada0 100644 --- a/src/simulation_sample/ground_station/sample_ground_station.hpp +++ b/src/simulation_sample/ground_station/sample_ground_station.hpp @@ -51,6 +51,6 @@ class SampleGroundStation : public simulation::GroundStation { SampleGsComponents* components_; //!< Ground station related components }; -} // namespace s2e::sample +} // namespace s2e::sample #endif // S2E_SIMULATION_SAMPLE_GROUND_STATION_SAMPLE_GROUND_STATION_HPP_ diff --git a/src/simulation_sample/ground_station/sample_ground_station_components.cpp b/src/simulation_sample/ground_station/sample_ground_station_components.cpp index c18528bb2..45ba2ee24 100644 --- a/src/simulation_sample/ground_station/sample_ground_station_components.cpp +++ b/src/simulation_sample/ground_station/sample_ground_station_components.cpp @@ -29,4 +29,4 @@ void SampleGsComponents::CompoLogSetUp(logger::Logger& logger) { logger.AddLogList(gs_calculator_); } -} // namespace s2e::sample +} // namespace s2e::sample diff --git a/src/simulation_sample/ground_station/sample_ground_station_components.hpp b/src/simulation_sample/ground_station/sample_ground_station_components.hpp index 1de5eec24..35d5571da 100644 --- a/src/simulation_sample/ground_station/sample_ground_station_components.hpp +++ b/src/simulation_sample/ground_station/sample_ground_station_components.hpp @@ -51,6 +51,6 @@ class SampleGsComponents { const simulation::SimulationConfiguration* configuration_; //!< Simulation setting }; -} // namespace s2e::sample +} // namespace s2e::sample #endif // S2E_SIMULATION_SAMPLE_GROUND_STATION_SAMPLE_GROUND_STATION_COMPONENTS_HPP_ diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index 4a9d9e852..1554f6ccb 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -13,9 +13,10 @@ namespace s2e::sample { using namespace components; -SampleComponents::SampleComponents(const dynamics::dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, - const environment::GlobalEnvironment* global_environment, const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, - const unsigned int spacecraft_id) +SampleComponents::SampleComponents(const dynamics::dynamics::Dynamics* dynamics, simulation::Structure* structure, + const environment::LocalEnvironment* local_environment, const environment::GlobalEnvironment* global_environment, + const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, + const unsigned int spacecraft_id) : configuration_(configuration), dynamics_(dynamics), structure_(structure), @@ -227,4 +228,4 @@ void SampleComponents::LogSetup(logger::Logger& logger) { logger.AddLogList(orbit_observer_); } -} // namespace s2e::sample +} // namespace s2e::sample diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index 27c1118fc..428ec9645 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -62,8 +62,9 @@ class SampleComponents : public simulation::InstalledComponents { * @fn SampleComponents * @brief Constructor */ - SampleComponents(const dynamics::dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, - const environment::GlobalEnvironment* global_environment, const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, + SampleComponents(const dynamics::dynamics::Dynamics* dynamics, simulation::Structure* structure, + const environment::LocalEnvironment* local_environment, const environment::GlobalEnvironment* global_environment, + const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, const unsigned int spacecraft_id); /** * @fn ~SampleComponents @@ -139,12 +140,12 @@ class SampleComponents : public simulation::InstalledComponents { // States const simulation::SimulationConfiguration* configuration_; //!< Simulation settings - const dynamics::dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft + const dynamics::dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft simulation::Structure* structure_; //!< Structure information of the spacecraft - const environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft - const environment::GlobalEnvironment* global_environment_; //!< Global environment information + const environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft + const environment::GlobalEnvironment* global_environment_; //!< Global environment information }; -} // namespace s2e::sample +} // namespace s2e::sample #endif // S2E_SIMULATION_SAMPLE_SPACECRAFT_SAMPLE_COMPONENTS_HPP_ diff --git a/src/simulation_sample/spacecraft/sample_port_configuration.hpp b/src/simulation_sample/spacecraft/sample_port_configuration.hpp index bfa1a57db..dd67bddab 100644 --- a/src/simulation_sample/spacecraft/sample_port_configuration.hpp +++ b/src/simulation_sample/spacecraft/sample_port_configuration.hpp @@ -29,6 +29,6 @@ enum class UARTPortConfig { kUartComponentMax //!< Maximum port number. Do not remove. Place on the bottom. }; -} // namespace s2e::sample +} // namespace s2e::sample #endif // S2E_SIMULATION_SAMPLE_SPACECRAFT_SAMPLE_PORT_CONFIGURATION_HPP_ diff --git a/src/simulation_sample/spacecraft/sample_spacecraft.cpp b/src/simulation_sample/spacecraft/sample_spacecraft.cpp index bfc1bdcc9..13ab3289d 100644 --- a/src/simulation_sample/spacecraft/sample_spacecraft.cpp +++ b/src/simulation_sample/spacecraft/sample_spacecraft.cpp @@ -12,12 +12,12 @@ namespace s2e::sample { -SampleSpacecraft::SampleSpacecraft(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, - const unsigned int spacecraft_id) +SampleSpacecraft::SampleSpacecraft(const simulation::SimulationConfiguration* simulation_configuration, + const environment::GlobalEnvironment* global_environment, const unsigned int spacecraft_id) : simulation::Spacecraft(simulation_configuration, global_environment, spacecraft_id) { sample_components_ = new SampleComponents(dynamics_, structure_, local_environment_, global_environment, simulation_configuration, &clock_generator_, spacecraft_id); components_ = sample_components_; } -} // namespace s2e::sample +} // namespace s2e::sample diff --git a/src/simulation_sample/spacecraft/sample_spacecraft.hpp b/src/simulation_sample/spacecraft/sample_spacecraft.hpp index 43f9ff564..4a3767acc 100644 --- a/src/simulation_sample/spacecraft/sample_spacecraft.hpp +++ b/src/simulation_sample/spacecraft/sample_spacecraft.hpp @@ -37,6 +37,6 @@ class SampleSpacecraft : public simulation::Spacecraft { SampleComponents* sample_components_; }; -} // namespace s2e::sample +} // namespace s2e::sample #endif // S2E_SIMULATION_SAMPLE_SPACECRAFT_SAMPLE_SPACECRAFT_HPP_ diff --git a/src/utilities/com_port_interface.hpp b/src/utilities/com_port_interface.hpp index aca6d663d..3da5bd6eb 100644 --- a/src/utilities/com_port_interface.hpp +++ b/src/utilities/com_port_interface.hpp @@ -108,6 +108,6 @@ class ComPortInterface { msclr::gcroot rx_buf_; //!< RX Buffer }; -} // namespace s2e::utilities +} // namespace s2e::utilities #endif // S2E_LIBRARY_COMMUNICATION_COM_PORT_INTERFACE_HPP_ diff --git a/src/utilities/endian.cpp b/src/utilities/endian.cpp index 8286204ea..6aa2111a1 100644 --- a/src/utilities/endian.cpp +++ b/src/utilities/endian.cpp @@ -29,4 +29,4 @@ void *endian_memcpy(void *dst, const void *src, size_t size) { #endif // IS_LITTLE_ENDIAN } -} // namespace s2e::utilities +} // namespace s2e::utilities diff --git a/src/utilities/endian.hpp b/src/utilities/endian.hpp index 966800f36..0660d78f9 100644 --- a/src/utilities/endian.hpp +++ b/src/utilities/endian.hpp @@ -22,6 +22,6 @@ namespace s2e::utilities { */ void *endian_memcpy(void *dst, const void *src, size_t count); -} // namespace s2e::utilities +} // namespace s2e::utilities #endif // S2E_LIBRARY_UTILITIES_ENDIAN_HPP_ diff --git a/src/utilities/macros.hpp b/src/utilities/macros.hpp index f78b16fe0..cbd37f012 100644 --- a/src/utilities/macros.hpp +++ b/src/utilities/macros.hpp @@ -10,6 +10,6 @@ namespace s2e::utilities { #define UNUSED(x) (void)(x) //!< Macro to avoid unused warnings -} // namespace s2e::utilities +} // namespace s2e::utilities #endif // S2E_LIBRARY_UTILITIES_MACROS_HPP_ diff --git a/src/utilities/quantization.hpp b/src/utilities/quantization.hpp index 47bdd4f71..9fd075e3b 100644 --- a/src/utilities/quantization.hpp +++ b/src/utilities/quantization.hpp @@ -26,6 +26,6 @@ double quantization(const double continuous_number, const double resolution); */ float quantization_float(const double continuous_number, const double resolution); -} // namespace s2e::utilities +} // namespace s2e::utilities #endif // S2E_LIBRARY_UTILITIES_QUANTIZATION_HPP_ diff --git a/src/utilities/ring_buffer.cpp b/src/utilities/ring_buffer.cpp index feb9ec73e..40aed8adb 100644 --- a/src/utilities/ring_buffer.cpp +++ b/src/utilities/ring_buffer.cpp @@ -47,4 +47,4 @@ int RingBuffer::Read(byte* buffer, const unsigned int offset, const unsigned int return read_count; } -} // namespace s2e::utilities +} // namespace s2e::utilities diff --git a/src/utilities/ring_buffer.hpp b/src/utilities/ring_buffer.hpp index 95a45d60f..3855c8179 100644 --- a/src/utilities/ring_buffer.hpp +++ b/src/utilities/ring_buffer.hpp @@ -54,6 +54,6 @@ class RingBuffer { unsigned int write_pointer_; //!< Write pointer }; -} // namespace s2e::utilities +} // namespace s2e::utilities #endif // S2E_LIBRARY_UTILITIES_RING_BUFFER_HPP_ diff --git a/src/utilities/slip.cpp b/src/utilities/slip.cpp index beeef8e53..b1f0fbb03 100644 --- a/src/utilities/slip.cpp +++ b/src/utilities/slip.cpp @@ -80,4 +80,4 @@ std::vector encode_slip_with_header(const std::vector in) { return out; } -} // namespace s2e::utilities +} // namespace s2e::utilities diff --git a/src/utilities/slip.hpp b/src/utilities/slip.hpp index e001295b3..da9e222d5 100644 --- a/src/utilities/slip.hpp +++ b/src/utilities/slip.hpp @@ -42,6 +42,6 @@ std::vector encode_slip(std::vector in); */ std::vector encode_slip_with_header(std::vector in); -} // namespace s2e::utilities +} // namespace s2e::utilities #endif // S2E_LIBRARY_UTILITIES_SLIP_HPP_ From 282cead6e273feec5402e7f00dd863c072968ef6 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 1 Oct 2024 13:12:49 +0900 Subject: [PATCH 41/49] Fix format --- src/dynamics/dynamics.cpp | 2 -- src/dynamics/dynamics.hpp | 11 ++++++++--- src/dynamics/orbit/orbit.hpp | 2 +- .../orbit/time_series_file_orbit_propagation.cpp | 1 - .../orbit/time_series_file_orbit_propagation.hpp | 1 - src/environment/local/local_environment.hpp | 4 ++++ src/setting_file_reader/wings_operation_file.hpp | 2 +- .../multiple_spacecraft/relative_information.hpp | 11 ++++++++--- src/utilities/com_port_interface.cpp | 2 +- src/utilities/quantization.cpp | 2 +- 10 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index 49f226dde..1a9da635e 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -5,8 +5,6 @@ #include "dynamics.hpp" -#include "../simulation/multiple_spacecraft/relative_information.hpp" - namespace s2e::dynamics { Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index 5267f7a64..be8534181 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -10,6 +10,7 @@ #include "../environment/global/simulation_time.hpp" #include "../environment/local/local_environment.hpp" #include "../math_physics/math/vector.hpp" +#include "../simulation/multiple_spacecraft/relative_information.hpp" #include "../simulation/simulation_configuration.hpp" #include "../simulation/spacecraft/structure/structure.hpp" #include "dynamics/attitude/initialize_attitude.hpp" @@ -17,8 +18,12 @@ #include "dynamics/thermal/node.hpp" #include "dynamics/thermal/temperature.hpp" -class s2e::simulation::RelativeInformation; -class s2e::environment::LocalEnvironment; +namespace s2e::simulation { +class RelativeInformation; +} +namespace s2e::environment { +class LocalEnvironment; +} namespace s2e::dynamics { @@ -53,7 +58,7 @@ class Dynamics { * @param [in] simulation_time: Simulation time * @param [in] local_celestial_information: Local celestial information */ - void Update(const simulation::SimulationTime* simulation_time, const environment::LocalCelestialInformation* local_celestial_information); + void Update(const environment::SimulationTime* simulation_time, const environment::LocalCelestialInformation* local_celestial_information); /** * @fn LogSetup diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index 9bdfc4851..c51f0dc61 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -27,7 +27,7 @@ enum class OrbitPropagateMode { kSgp4, //!< SGP4 propagation using TLE without thruster maneuver kRelativeOrbit, //!< Relative dynamics (for formation flying simulation) kKepler, //!< Kepler orbit propagation without disturbances and thruster maneuver - kEncke, //!< Encke orbit propagation with disturbances and thruster maneuver + kEncke, //!< Encke orbit propagation with disturbances and thruster maneuver kTimeSeriesFile //!< Orbit propagation using time series file }; diff --git a/src/dynamics/orbit/time_series_file_orbit_propagation.cpp b/src/dynamics/orbit/time_series_file_orbit_propagation.cpp index 709b23335..8fe9893a0 100644 --- a/src/dynamics/orbit/time_series_file_orbit_propagation.cpp +++ b/src/dynamics/orbit/time_series_file_orbit_propagation.cpp @@ -152,4 +152,3 @@ bool TimeSeriesFileOrbitPropagation::UpdateInterpolationInformation() { return true; } - diff --git a/src/dynamics/orbit/time_series_file_orbit_propagation.hpp b/src/dynamics/orbit/time_series_file_orbit_propagation.hpp index 0db88a7de..0de5a7278 100644 --- a/src/dynamics/orbit/time_series_file_orbit_propagation.hpp +++ b/src/dynamics/orbit/time_series_file_orbit_propagation.hpp @@ -85,4 +85,3 @@ class TimeSeriesFileOrbitPropagation : public Orbit { }; #endif // S2E_DYNAMICS_ORBIT_TIME_SERIES_FILE_ORBIT_PROPAGATION_HPP_ - diff --git a/src/environment/local/local_environment.hpp b/src/environment/local/local_environment.hpp index ef671a499..b486d4226 100644 --- a/src/environment/local/local_environment.hpp +++ b/src/environment/local/local_environment.hpp @@ -15,6 +15,10 @@ #include "simulation/simulation_configuration.hpp" #include "solar_radiation_pressure_environment.hpp" +namespace s2e::math { +class Dynamics; +} + namespace s2e::environment { /** diff --git a/src/setting_file_reader/wings_operation_file.hpp b/src/setting_file_reader/wings_operation_file.hpp index 848010aac..51ed0d1d5 100644 --- a/src/setting_file_reader/wings_operation_file.hpp +++ b/src/setting_file_reader/wings_operation_file.hpp @@ -37,6 +37,6 @@ class WingsOperationFile { size_t line_pointer_ = 0; //!< Line pointer }; -} // namespace s2e::setting_file_reader +} // namespace s2e::setting_file_reader #endif // S2E_LIBRARY_INITIALIZE_WINGS_OPERATION_FILE_HPP_ diff --git a/src/simulation/multiple_spacecraft/relative_information.hpp b/src/simulation/multiple_spacecraft/relative_information.hpp index 9e77403d0..d49f4c74a 100644 --- a/src/simulation/multiple_spacecraft/relative_information.hpp +++ b/src/simulation/multiple_spacecraft/relative_information.hpp @@ -6,12 +6,17 @@ #ifndef S2E_MULTIPLE_SPACECRAFT_RELATIVE_INFORMATION_HPP_ #define S2E_MULTIPLE_SPACECRAFT_RELATIVE_INFORMATION_HPP_ +#include #include #include "../../dynamics/dynamics.hpp" #include "../../logger/loggable.hpp" #include "../../logger/logger.hpp" +namespace s2e::dynamics { +class Dynamics; +} + namespace s2e::simulation { /** @@ -42,7 +47,7 @@ class RelativeInformation : public logger::ILoggable { * @param [in] spacecraft_id: ID of target spacecraft * @param [in] dynamics: Dynamics information of the target spacecraft */ - void RegisterDynamicsInfo(const size_t spacecraft_id, const dynamics::dynamics::Dynamics* dynamics); + void RegisterDynamicsInfo(const size_t spacecraft_id, const dynamics::Dynamics* dynamics); /** * @fn RegisterDynamicsInfo * @brief Remove dynamics information of target spacecraft @@ -129,12 +134,12 @@ class RelativeInformation : public logger::ILoggable { * @brief Return the dynamics information of a spacecraft * @param [in] target_spacecraft_id: ID of the spacecraft */ - inline const dynamics::dynamics::Dynamics* GetReferenceSatDynamics(const size_t reference_spacecraft_id) const { + inline const dynamics::Dynamics* GetReferenceSatDynamics(const size_t reference_spacecraft_id) const { return dynamics_database_.at(reference_spacecraft_id); }; private: - std::map dynamics_database_; //!< Dynamics database of all spacecraft + std::map dynamics_database_; //!< Dynamics database of all spacecraft std::vector>> relative_position_list_i_m_; //!< Relative position list in the inertial frame in unit [m] std::vector>> relative_velocity_list_i_m_s_; //!< Relative velocity list in the inertial frame in unit [m/s] diff --git a/src/utilities/com_port_interface.cpp b/src/utilities/com_port_interface.cpp index 70209cde8..4644fa5c9 100644 --- a/src/utilities/com_port_interface.cpp +++ b/src/utilities/com_port_interface.cpp @@ -117,4 +117,4 @@ int ComPortInterface::DiscardInBuffer() { return 0; } -} // namespace s2e::utilities +} // namespace s2e::utilities diff --git a/src/utilities/quantization.cpp b/src/utilities/quantization.cpp index 9b8aa00d5..1cfd3268e 100644 --- a/src/utilities/quantization.cpp +++ b/src/utilities/quantization.cpp @@ -14,4 +14,4 @@ double quantization(const double continuous_number, const double resolution) { float quantization_float(const double continuous_number, const double resolution) { return (float)quantization(continuous_number, resolution); } -} // namespace s2e::utilities +} // namespace s2e::utilities From e7539781844ca081986cbc9bcce107758e1bc4f7 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 1 Oct 2024 13:19:48 +0900 Subject: [PATCH 42/49] Fix format --- src/components/base/component.hpp | 4 ++-- src/components/base/interface_gpio_component.hpp | 2 +- .../numerical_integration/numerical_integrator.hpp | 2 +- src/math_physics/numerical_integration/runge_kutta.hpp | 2 +- src/simulation/spacecraft/structure/kinematics_parameters.hpp | 2 +- .../spacecraft/structure/residual_magnetic_moment.hpp | 2 +- src/simulation/spacecraft/structure/surface.hpp | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/components/base/component.hpp b/src/components/base/component.hpp index 2cfc64db5..2b541dffd 100644 --- a/src/components/base/component.hpp +++ b/src/components/base/component.hpp @@ -78,13 +78,13 @@ class Component : public ITickable { * @brief Pure virtual function used to calculate high-frequency disturbances(e.g. RW jitter) * @note Override only when high-frequency disturbances need to be calculated. */ - virtual void FastUpdate() {}; + virtual void FastUpdate(){}; /** * @fn PowerOffRoutine * @brief Pure virtual function executed when the power switch is off. */ - virtual void PowerOffRoutine() {}; + virtual void PowerOffRoutine(){}; environment::ClockGenerator* clock_generator_; //!< Clock generator PowerPort* power_port_; //!< Power port diff --git a/src/components/base/interface_gpio_component.hpp b/src/components/base/interface_gpio_component.hpp index 6a5d3bb75..9df3db260 100644 --- a/src/components/base/interface_gpio_component.hpp +++ b/src/components/base/interface_gpio_component.hpp @@ -18,7 +18,7 @@ class IGPIOCompo { * @fn ~IGPIOCompo * @brief Destructor */ - virtual ~IGPIOCompo() {}; + virtual ~IGPIOCompo(){}; /** * @fn GpioStateChanged diff --git a/src/math_physics/numerical_integration/numerical_integrator.hpp b/src/math_physics/numerical_integration/numerical_integrator.hpp index ae708a753..461bfa225 100644 --- a/src/math_physics/numerical_integration/numerical_integrator.hpp +++ b/src/math_physics/numerical_integration/numerical_integrator.hpp @@ -32,7 +32,7 @@ class NumericalIntegrator { * @fn ~NumericalIntegrator * @brief Destructor */ - inline virtual ~NumericalIntegrator() {}; + inline virtual ~NumericalIntegrator(){}; /** * @fn Integrate diff --git a/src/math_physics/numerical_integration/runge_kutta.hpp b/src/math_physics/numerical_integration/runge_kutta.hpp index a24b4f53b..d2072d4cb 100644 --- a/src/math_physics/numerical_integration/runge_kutta.hpp +++ b/src/math_physics/numerical_integration/runge_kutta.hpp @@ -29,7 +29,7 @@ class RungeKutta : public NumericalIntegrator { * @fn ~RungeKutta * @brief Destructor */ - inline virtual ~RungeKutta() {}; + inline virtual ~RungeKutta(){}; /** * @fn Integrate diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.hpp b/src/simulation/spacecraft/structure/kinematics_parameters.hpp index c4dfadeb2..b97209727 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.hpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.hpp @@ -26,7 +26,7 @@ class KinematicsParameters { * @fn ~KinematicsParameters * @brief Destructor */ - ~KinematicsParameters() {}; + ~KinematicsParameters(){}; // Getter /** diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp index ae06bb85d..1e00b7e89 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp @@ -26,7 +26,7 @@ class ResidualMagneticMoment { * @fn ~ResidualMagneticMoment * @brief Destructor */ - ~ResidualMagneticMoment() {}; + ~ResidualMagneticMoment(){}; // Getter /** diff --git a/src/simulation/spacecraft/structure/surface.hpp b/src/simulation/spacecraft/structure/surface.hpp index 73a09d6e6..c98386bae 100644 --- a/src/simulation/spacecraft/structure/surface.hpp +++ b/src/simulation/spacecraft/structure/surface.hpp @@ -26,7 +26,7 @@ class Surface { * @fn ~Surface * @brief Destructor */ - ~Surface() {}; + ~Surface(){}; // Getter /** From af60184a44c5243c633122256baf29b73b4520b3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 1 Oct 2024 14:39:20 +0900 Subject: [PATCH 43/49] Fix small --- src/environment/global/celestial_information.hpp | 4 ++-- src/math_physics/math/vector.hpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/environment/global/celestial_information.hpp b/src/environment/global/celestial_information.hpp index dbb9777d9..ea04e562d 100644 --- a/src/environment/global/celestial_information.hpp +++ b/src/environment/global/celestial_information.hpp @@ -15,10 +15,10 @@ #include "moon_rotation.hpp" #include "simulation_time.hpp" -class MoonRotation; - namespace s2e::environment { +class MoonRotation; + /** * @class CelestialInformation * @brief Class to manage the information related with the celestial bodies diff --git a/src/math_physics/math/vector.hpp b/src/math_physics/math/vector.hpp index 333c62056..b210759e6 100644 --- a/src/math_physics/math/vector.hpp +++ b/src/math_physics/math/vector.hpp @@ -9,10 +9,11 @@ #include // for size_t #include // for ostream, cout +namespace s2e::math { + #define dot InnerProduct #define cross OuterProduct -namespace s2e::math { /** * @class Vector * @brief Class for mathematical vector From 7875268dccc9b56538875a79d1803f230045491e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 1 Oct 2024 23:09:35 +0900 Subject: [PATCH 44/49] Fix bugs --- src/components/base/component.hpp | 4 +- .../base/interface_gpio_component.hpp | 2 +- src/components/base/sensor.hpp | 10 ++-- src/components/ideal/force_generator.hpp | 8 ++-- src/components/ideal/orbit_observer.cpp | 5 +- src/components/ideal/orbit_observer.hpp | 6 +-- src/components/ideal/torque_generator.hpp | 2 +- src/components/ports/uart_port.cpp | 4 +- src/components/ports/uart_port.hpp | 4 +- src/components/real/aocs/gnss_receiver.cpp | 13 ++--- src/components/real/aocs/gnss_receiver.hpp | 24 +++++----- src/components/real/aocs/magnetometer.cpp | 9 ++-- src/components/real/aocs/magnetometer.hpp | 10 ++-- src/components/real/aocs/magnetorquer.cpp | 10 ++-- src/components/real/aocs/magnetorquer.hpp | 12 ++--- src/components/real/aocs/star_sensor.cpp | 16 ++++--- src/components/real/aocs/star_sensor.hpp | 18 +++---- src/components/real/aocs/sun_sensor.cpp | 12 +++-- src/components/real/aocs/sun_sensor.hpp | 17 ++++--- src/components/real/communication/antenna.cpp | 4 +- src/components/real/communication/antenna.hpp | 16 +++---- .../ground_station_calculator.cpp | 28 +++++------ .../ground_station_calculator.hpp | 10 ++-- src/components/real/mission/telescope.cpp | 35 +++++++------- src/components/real/mission/telescope.hpp | 19 ++++---- .../real/power/solar_array_panel.cpp | 16 +++---- .../real/power/solar_array_panel.hpp | 19 ++++---- .../real/propulsion/simple_thruster.cpp | 20 ++++---- .../real/propulsion/simple_thruster.hpp | 47 ++++++++++--------- src/disturbances/air_drag.cpp | 9 ++-- src/disturbances/air_drag.hpp | 7 +-- src/disturbances/disturbance.hpp | 4 +- src/disturbances/disturbances.cpp | 19 ++++---- src/disturbances/disturbances.hpp | 19 ++++---- src/disturbances/geopotential.cpp | 2 +- src/disturbances/geopotential.hpp | 10 ++-- src/disturbances/gravity_gradient.cpp | 2 +- src/disturbances/gravity_gradient.hpp | 2 +- src/disturbances/lunar_gravity_field.cpp | 4 +- src/disturbances/lunar_gravity_field.hpp | 10 ++-- src/disturbances/magnetic_disturbance.cpp | 12 ++--- src/disturbances/magnetic_disturbance.hpp | 10 ++-- .../solar_radiation_pressure_disturbance.cpp | 9 ++-- .../solar_radiation_pressure_disturbance.hpp | 9 ++-- src/disturbances/surface_force.cpp | 3 +- src/disturbances/surface_force.hpp | 7 +-- src/disturbances/third_body_gravity.cpp | 2 +- src/disturbances/third_body_gravity.hpp | 2 +- src/dynamics/attitude/attitude.cpp | 2 +- src/dynamics/attitude/attitude_rk4.cpp | 2 +- .../attitude_with_cantilever_vibration.cpp | 2 +- src/dynamics/attitude/controlled_attitude.cpp | 5 +- src/dynamics/attitude/initialize_attitude.cpp | 2 +- src/dynamics/dynamics.cpp | 33 ++++++------- src/dynamics/dynamics.hpp | 2 +- .../orbit/encke_orbit_propagation.cpp | 2 +- .../orbit/encke_orbit_propagation.hpp | 6 +-- src/dynamics/orbit/initialize_orbit.cpp | 4 +- src/dynamics/orbit/initialize_orbit.hpp | 4 +- .../orbit/kepler_orbit_propagation.cpp | 2 +- .../orbit/kepler_orbit_propagation.hpp | 3 +- src/dynamics/orbit/relative_orbit.cpp | 4 +- src/dynamics/orbit/relative_orbit.hpp | 12 ++--- src/dynamics/orbit/rk4_orbit_propagation.cpp | 4 +- src/dynamics/orbit/rk4_orbit_propagation.hpp | 2 +- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 4 +- src/dynamics/orbit/sgp4_orbit_propagation.hpp | 2 +- .../time_series_file_orbit_propagation.cpp | 17 ++++--- .../time_series_file_orbit_propagation.hpp | 9 ++-- src/dynamics/thermal/node.hpp | 1 - src/dynamics/thermal/temperature.cpp | 15 +++--- src/environment/local/local_environment.cpp | 8 ++-- src/environment/local/local_environment.hpp | 6 +-- .../numerical_integrator.hpp | 2 +- .../numerical_integration/runge_kutta.hpp | 2 +- src/simulation/spacecraft/spacecraft.cpp | 6 +-- src/simulation/spacecraft/spacecraft.hpp | 8 ++-- .../structure/initialize_structure.cpp | 8 ++-- .../structure/initialize_structure.hpp | 2 +- .../structure/kinematics_parameters.hpp | 2 +- .../structure/residual_magnetic_moment.hpp | 2 +- .../spacecraft/structure/structure.hpp | 6 +-- .../spacecraft/structure/surface.hpp | 2 +- .../ground_station/sample_ground_station.cpp | 2 +- .../ground_station/sample_ground_station.hpp | 2 +- .../sample_ground_station_components.hpp | 2 +- .../spacecraft/sample_components.cpp | 2 +- .../spacecraft/sample_components.hpp | 43 ++++++++--------- 88 files changed, 411 insertions(+), 375 deletions(-) diff --git a/src/components/base/component.hpp b/src/components/base/component.hpp index 2b541dffd..2cfc64db5 100644 --- a/src/components/base/component.hpp +++ b/src/components/base/component.hpp @@ -78,13 +78,13 @@ class Component : public ITickable { * @brief Pure virtual function used to calculate high-frequency disturbances(e.g. RW jitter) * @note Override only when high-frequency disturbances need to be calculated. */ - virtual void FastUpdate(){}; + virtual void FastUpdate() {}; /** * @fn PowerOffRoutine * @brief Pure virtual function executed when the power switch is off. */ - virtual void PowerOffRoutine(){}; + virtual void PowerOffRoutine() {}; environment::ClockGenerator* clock_generator_; //!< Clock generator PowerPort* power_port_; //!< Power port diff --git a/src/components/base/interface_gpio_component.hpp b/src/components/base/interface_gpio_component.hpp index 9df3db260..6a5d3bb75 100644 --- a/src/components/base/interface_gpio_component.hpp +++ b/src/components/base/interface_gpio_component.hpp @@ -18,7 +18,7 @@ class IGPIOCompo { * @fn ~IGPIOCompo * @brief Destructor */ - virtual ~IGPIOCompo(){}; + virtual ~IGPIOCompo() {}; /** * @fn GpioStateChanged diff --git a/src/components/base/sensor.hpp b/src/components/base/sensor.hpp index 351a1275d..3204a1783 100644 --- a/src/components/base/sensor.hpp +++ b/src/components/base/sensor.hpp @@ -54,11 +54,11 @@ class Sensor { math::Vector Measure(const math::Vector true_value_c); private: - math::Matrix scale_factor_; //!< Scale factor matrix - math::Vector range_to_const_c_; //!< Output range limit to be constant output value at the component frame - math::Vector range_to_zero_c_; //!< Output range limit to be zero output value at the component frame - s2e::randomization::NormalRand normal_random_noise_c_[N]; //!< Normal random - RandomWalk random_walk_noise_c_; //!< Random Walk + math::Matrix scale_factor_; //!< Scale factor matrix + math::Vector range_to_const_c_; //!< Output range limit to be constant output value at the component frame + math::Vector range_to_zero_c_; //!< Output range limit to be zero output value at the component frame + randomization::NormalRand normal_random_noise_c_[N]; //!< Normal random + randomization::RandomWalk random_walk_noise_c_; //!< Random Walk /** * @fn Clip diff --git a/src/components/ideal/force_generator.hpp b/src/components/ideal/force_generator.hpp index b5d254414..ac781993d 100644 --- a/src/components/ideal/force_generator.hpp +++ b/src/components/ideal/force_generator.hpp @@ -30,7 +30,7 @@ class ForceGenerator : public Component, public logger::ILoggable { * @param [in] dynamics: Dynamics information */ ForceGenerator(const int prescaler, environment::ClockGenerator* clock_generator, const double magnitude_error_standard_deviation_N, - const double direction_error_standard_deviation_rad, const Dynamics* dynamics); + const double direction_error_standard_deviation_rad, const dynamics::Dynamics* dynamics); /** * @fn ~ForceGenerator * @brief Destructor @@ -66,17 +66,17 @@ class ForceGenerator : public Component, public logger::ILoggable { * @fn GetGeneratedForce_b_N * @brief Return generated force in the body fixed frame [N] */ - inline const Vector<3> GetGeneratedForce_b_N() const { return generated_force_b_N_; }; + inline const math::Vector<3> GetGeneratedForce_b_N() const { return generated_force_b_N_; }; /** * @fn GetGeneratedForce_i_N * @brief Return generated force in the inertial frame [N] */ - inline const Vector<3> GetGeneratedForce_i_N() const { return generated_force_i_N_; }; + inline const math::Vector<3> GetGeneratedForce_i_N() const { return generated_force_i_N_; }; /** * @fn GetGeneratedForce_rtn_N * @brief Return generated force in the RTN frame [N] */ - inline const Vector<3> GetGeneratedForce_rtn_N() const { return generated_force_rtn_N_; }; + inline const math::Vector<3> GetGeneratedForce_rtn_N() const { return generated_force_rtn_N_; }; // Setter /** diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 37e5c4dc3..ca3fa09bc 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -11,7 +11,7 @@ namespace s2e::components { OrbitObserver::OrbitObserver(const int prescaler, environment::ClockGenerator* clock_generator, const NoiseFrame noise_frame, - const math::Vector<6> error_standard_deviation, const Orbit& orbit) + const math::Vector<6> error_standard_deviation, const dynamics::orbit::Orbit& orbit) : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], s2e::randomization::global_randomization.MakeSeed()); @@ -85,7 +85,8 @@ NoiseFrame SetNoiseFrame(const std::string noise_frame) { } } -OrbitObserver InitializeOrbitObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { +OrbitObserver InitializeOrbitObserver(environment::ClockGenerator* clock_generator, const std::string file_name, + const dynamics::orbit::Orbit& orbit) { // General setting_file_reader::IniAccess ini_file(file_name); diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 6e56fdd59..a9216dde2 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -40,7 +40,7 @@ class OrbitObserver : public Component, public logger::ILoggable { * @param [in] orbit: Orbit information */ OrbitObserver(const int prescaler, environment::ClockGenerator* clock_generator, const NoiseFrame noise_frame, - const math::Vector<6> error_standard_deviation, const Orbit& orbit); + const math::Vector<6> error_standard_deviation, const dynamics::orbit::Orbit& orbit); /** * @fn ~AttitudeObserver @@ -87,7 +87,7 @@ class OrbitObserver : public Component, public logger::ILoggable { s2e::randomization::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] // Observed variables - const Orbit& orbit_; //!< Orbit information + const dynamics::orbit::Orbit& orbit_; //!< Orbit information }; /** @@ -105,7 +105,7 @@ NoiseFrame SetNoiseFrame(const std::string noise_frame); * @param [in] file_name: Path to the initialize file * @param [in] orbit: Orbit information */ -OrbitObserver InitializeOrbitObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit); +OrbitObserver InitializeOrbitObserver(environment::ClockGenerator* clock_generator, const std::string file_name, const dynamics::orbit::Orbit& orbit); } // namespace s2e::components diff --git a/src/components/ideal/torque_generator.hpp b/src/components/ideal/torque_generator.hpp index b94a54919..4ea760c30 100644 --- a/src/components/ideal/torque_generator.hpp +++ b/src/components/ideal/torque_generator.hpp @@ -66,7 +66,7 @@ class TorqueGenerator : public Component, public logger::ILoggable { * @fn GetGeneratedTorque_b_Nm * @brief Return generated torque in the body fixed frame [Nm] */ - inline const Vector<3> GetGeneratedTorque_b_Nm() const { return generated_torque_b_Nm_; }; + inline const math::Vector<3> GetGeneratedTorque_b_Nm() const { return generated_torque_b_Nm_; }; // Setter /** diff --git a/src/components/ports/uart_port.cpp b/src/components/ports/uart_port.cpp index 6969b9361..070bd59ad 100644 --- a/src/components/ports/uart_port.cpp +++ b/src/components/ports/uart_port.cpp @@ -14,8 +14,8 @@ UartPort::UartPort(const unsigned int rx_buffer_size, const unsigned int tx_buff unsigned int checked_tx_buffer_size = tx_buffer_size; if (rx_buffer_size <= 0) checked_rx_buffer_size = kDefaultBufferSize; if (tx_buffer_size <= 0) checked_tx_buffer_size = kDefaultBufferSize; - rx_buffer_ = new RingBuffer(checked_rx_buffer_size); - tx_buffer_ = new RingBuffer(checked_tx_buffer_size); + rx_buffer_ = new utilities::RingBuffer(checked_rx_buffer_size); + tx_buffer_ = new utilities::RingBuffer(checked_tx_buffer_size); } UartPort::~UartPort() { diff --git a/src/components/ports/uart_port.hpp b/src/components/ports/uart_port.hpp index 2d00a0101..c83ddc456 100644 --- a/src/components/ports/uart_port.hpp +++ b/src/components/ports/uart_port.hpp @@ -76,8 +76,8 @@ class UartPort { private: const static unsigned int kDefaultBufferSize = 1024; //!< Default buffer size - RingBuffer* rx_buffer_; //!< Receive buffer (Component -> OBC) - RingBuffer* tx_buffer_; //!< Transmit buffer (OBC-> Component) + utilities::RingBuffer* rx_buffer_; //!< Receive buffer (Component -> OBC) + utilities::RingBuffer* tx_buffer_; //!< Transmit buffer (OBC-> Component) }; } // namespace s2e::components diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index e6b528f94..330844dd9 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -17,7 +17,7 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m, const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, - const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) + const environment::GnssSatellites* gnss_satellites, const environment::SimulationTime* simulation_time) : Component(prescaler, clock_generator), component_id_(component_id), antenna_position_b_m_(antenna_position_b_m), @@ -39,7 +39,7 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m, const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, - const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) + const environment::GnssSatellites* gnss_satellites, const environment::SimulationTime* simulation_time) : Component(prescaler, clock_generator, power_port), component_id_(component_id), antenna_position_b_m_(antenna_position_b_m), @@ -265,7 +265,7 @@ typedef struct _gnss_receiver_param { math::Vector<3> velocity_noise_standard_deviation_ecef_m_s; } GnssReceiverParam; -GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const size_t component_id) { +GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const environment::GnssSatellites* gnss_satellites, const size_t component_id) { GnssReceiverParam gnss_receiver_param; setting_file_reader::IniAccess gnssr_conf(file_name); @@ -296,7 +296,8 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat } GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, - const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { + const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites, + const environment::SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.antenna_model, gr_param.antenna_pos_b, gr_param.quaternion_b2c, @@ -306,8 +307,8 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, cons } GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const std::string file_name, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, - const SimulationTime* simulation_time) { + const std::string file_name, const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites, + const environment::SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); // PowerPort diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 2661d856b..604b11927 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -63,7 +63,8 @@ class GnssReceiver : public Component, public logger::ILoggable { GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m, const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, - const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); + const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites, + const environment::SimulationTime* simulation_time); /** * @fn GnssReceiver * @brief Constructor with power port @@ -84,7 +85,7 @@ class GnssReceiver : public Component, public logger::ILoggable { const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m, const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics, - const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); + const environment::GnssSatellites* gnss_satellites, const environment::SimulationTime* simulation_time); // Override functions for Component /** @@ -146,9 +147,9 @@ class GnssReceiver : public Component, public logger::ILoggable { s2e::geodesy::GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame // Time observation - UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] - unsigned int gps_time_week_ = 0; //!< Observed GPS time week part - double gps_time_s_ = 0.0; //!< Observed GPS time second part + environment::UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] + unsigned int gps_time_week_ = 0; //!< Observed GPS time week part + double gps_time_s_ = 0.0; //!< Observed GPS time second part // Satellite visibility bool is_gnss_visible_ = false; //!< Flag for GNSS satellite is visible or not @@ -156,9 +157,9 @@ class GnssReceiver : public Component, public logger::ILoggable { std::vector gnss_information_list_; //!< Information List of visible GNSS satellites // References - const dynamics::Dynamics* dynamics_; //!< Dynamics of spacecraft - const GnssSatellites* gnss_satellites_; //!< Information of GNSS satellites - const SimulationTime* simulation_time_; //!< Simulation time + const dynamics::Dynamics* dynamics_; //!< Dynamics of spacecraft + const environment::GnssSatellites* gnss_satellites_; //!< Information of GNSS satellites + const environment::SimulationTime* simulation_time_; //!< Simulation time // Internal Functions /** @@ -227,7 +228,8 @@ AntennaModel SetAntennaModel(const std::string antenna_model); * @param [in] simulation_time: Simulation time information */ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, - const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); + const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites, + const environment::SimulationTime* simulation_time); /** * @fn InitGnssReceiver * @brief Initialize functions for GNSS Receiver with power port @@ -240,8 +242,8 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, cons * @param [in] simulation_time: Simulation time information */ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const std::string file_name, const dynamics::Dynamics* dynamics, const GnssSatellites* gnss_satellites, - const SimulationTime* simulation_time); + const std::string file_name, const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites, + const environment::SimulationTime* simulation_time); } // namespace s2e::components diff --git a/src/components/real/aocs/magnetometer.cpp b/src/components/real/aocs/magnetometer.cpp index a709c91b1..0958e312c 100644 --- a/src/components/real/aocs/magnetometer.cpp +++ b/src/components/real/aocs/magnetometer.cpp @@ -10,14 +10,15 @@ namespace s2e::components { Magnetometer::Magnetometer(int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) + const math::Quaternion& quaternion_b2c, const environment::GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), geomagnetic_field_(geomagnetic_field) {} Magnetometer::Magnetometer(int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, - const unsigned int sensor_id, const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field) + const unsigned int sensor_id, const math::Quaternion& quaternion_b2c, + const environment::GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator, power_port), Sensor(sensor_base), sensor_id_(sensor_id), @@ -50,7 +51,7 @@ std::string Magnetometer::GetLogValue() const { } Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, - const GeomagneticField* geomagnetic_field) { + const environment::GeomagneticField* geomagnetic_field) { setting_file_reader::IniAccess magsensor_conf(file_name); const char* sensor_name = "MAGNETOMETER_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); @@ -71,7 +72,7 @@ Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, int } Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, - double component_step_time_s, const GeomagneticField* geomagnetic_field) { + double component_step_time_s, const environment::GeomagneticField* geomagnetic_field) { setting_file_reader::IniAccess magsensor_conf(file_name); const char* sensor_name = "MAGNETOMETER_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); diff --git a/src/components/real/aocs/magnetometer.hpp b/src/components/real/aocs/magnetometer.hpp index 7a52c30cd..2d788b366 100644 --- a/src/components/real/aocs/magnetometer.hpp +++ b/src/components/real/aocs/magnetometer.hpp @@ -34,7 +34,7 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] geomagnetic_field: Geomagnetic environment */ Magnetometer(const int prescaler, environment::ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id, - const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); + const math::Quaternion& quaternion_b2c, const environment::GeomagneticField* geomagnetic_field); /** * @fn Magnetometer * @brief Constructor with power port @@ -47,7 +47,7 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] geomagnetic_field: Geomagnetic environment */ Magnetometer(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, - const unsigned int sensor_id, const math::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field); + const unsigned int sensor_id, const math::Quaternion& quaternion_b2c, const environment::GeomagneticField* geomagnetic_field); /** * @fn ~Magnetometer * @brief Destructor @@ -104,7 +104,7 @@ class Magnetometer : public Component, public Sensor, pu unsigned int sensor_id_ = 0; //!< Sensor ID math::Quaternion quaternion_b2c_{0.0, 0.0, 0.0, 1.0}; //!< Quaternion from body frame to component frame - const GeomagneticField* geomagnetic_field_; //!< Geomagnetic environment + const environment::GeomagneticField* geomagnetic_field_; //!< Geomagnetic environment }; /** @@ -117,7 +117,7 @@ class Magnetometer : public Component, public Sensor, pu * @param [in] geomagnetic_field: Geomegnetic environment */ Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, - const GeomagneticField* geomagnetic_field); + const environment::GeomagneticField* geomagnetic_field); /** * @fn InitMagnetometer * @brief Initialize functions for magnetometer with power port @@ -129,7 +129,7 @@ Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, int * @param [in] geomagnetic_field: Geomegnetic environment */ Magnetometer InitMagnetometer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, - double component_step_time_s, const GeomagneticField* geomagnetic_field); + double component_step_time_s, const environment::GeomagneticField* geomagnetic_field); } // namespace s2e::components diff --git a/src/components/real/aocs/magnetorquer.cpp b/src/components/real/aocs/magnetorquer.cpp index 2dcbe762a..0ee096e5e 100644 --- a/src/components/real/aocs/magnetorquer.cpp +++ b/src/components/real/aocs/magnetorquer.cpp @@ -19,7 +19,8 @@ Magnetorquer::Magnetorquer(const int prescaler, environment::ClockGenerator* clo const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, const math::Vector& random_walk_standard_deviation_c_Am2, const math::Vector& random_walk_limit_c_Am2, - const math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field) + const math::Vector& normal_random_standard_deviation_c_Am2, + const environment::GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -41,7 +42,8 @@ Magnetorquer::Magnetorquer(const int prescaler, environment::ClockGenerator* clo const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, const math::Vector& random_walk_standard_deviation_c_Am2, const math::Vector& random_walk_limit_c_Am2, - const math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field) + const math::Vector& normal_random_standard_deviation_c_Am2, + const environment::GeomagneticField* geomagnetic_field) : Component(prescaler, clock_generator, power_port), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -114,7 +116,7 @@ std::string Magnetorquer::GetLogValue() const { } Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int actuator_id, const std::string file_name, - double component_step_time_s, const GeomagneticField* geomagnetic_field) { + double component_step_time_s, const environment::GeomagneticField* geomagnetic_field) { setting_file_reader::IniAccess magtorquer_conf(file_name); const char* sensor_name = "MAGNETORQUER_"; const std::string section_name = sensor_name + std::to_string(static_cast(actuator_id)); @@ -159,7 +161,7 @@ Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int } Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, const std::string file_name, - double component_step_time_s, const GeomagneticField* geomagnetic_field) { + double component_step_time_s, const environment::GeomagneticField* geomagnetic_field) { setting_file_reader::IniAccess magtorquer_conf(file_name); const char* sensor_name = "MAGNETORQUER_"; const std::string section_name = sensor_name + std::to_string(static_cast(actuator_id)); diff --git a/src/components/real/aocs/magnetorquer.hpp b/src/components/real/aocs/magnetorquer.hpp index f18dec450..b3f0b6ff1 100644 --- a/src/components/real/aocs/magnetorquer.hpp +++ b/src/components/real/aocs/magnetorquer.hpp @@ -48,7 +48,7 @@ class Magnetorquer : public Component, public logger::ILoggable { const math::Vector& min_magnetic_moment_c_Am2, const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, const math::Vector& random_walk_standard_deviation_c_Am2, const math::Vector& random_walk_limit_c_Am2, const math::Vector& normal_random_standard_deviation_c_Am2, - const GeomagneticField* geomagnetic_field); + const environment::GeomagneticField* geomagnetic_field); /** * @fn Magnetorquer * @brief Constructor with power port @@ -72,7 +72,7 @@ class Magnetorquer : public Component, public logger::ILoggable { const math::Vector& max_magnetic_moment_c_Am2, const math::Vector& min_magnetic_moment_c_Am2, const math::Vector& bias_noise_c_Am2_, double random_walk_step_width_s, const math::Vector& random_walk_standard_deviation_c_Am2, const math::Vector& random_walk_limit_c_Am2, - const math::Vector& normal_random_standard_deviation_c_Am2, const GeomagneticField* geomagnetic_field); + const math::Vector& normal_random_standard_deviation_c_Am2, const environment::GeomagneticField* geomagnetic_field); // Override functions for Component /** @@ -135,10 +135,10 @@ class Magnetorquer : public Component, public logger::ILoggable { math::Vector min_magnetic_moment_c_Am2_{-100.0}; //!< Minimum magnetic moment in the component frame [Am2] math::Vector bias_noise_c_Am2_{0.0}; //!< Constant bias noise in the component frame [Am2] - RandomWalk random_walk_c_Am2_; //!< Random walk noise + randomization::RandomWalk random_walk_c_Am2_; //!< Random walk noise s2e::randomization::NormalRand random_noise_c_Am2_[kMtqDimension]; //!< Normal random noise - const GeomagneticField* geomagnetic_field_; //!< Geomagnetic environment + const environment::GeomagneticField* geomagnetic_field_; //!< Geomagnetic environment /** * @fn CalcOutputTorque @@ -158,7 +158,7 @@ class Magnetorquer : public Component, public logger::ILoggable { * @param [in] geomagnetic_field: Geomegnetic environment */ Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int actuator_id, const std::string file_name, - double component_step_time_s, const GeomagneticField* geomagnetic_field); + double component_step_time_s, const environment::GeomagneticField* geomagnetic_field); /** * @fn InitMagnetorquer * @brief Initialize functions for magnetometer with power port @@ -170,7 +170,7 @@ Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, int * @param [in] geomagnetic_field: Geomegnetic environment */ Magnetorquer InitMagnetorquer(environment::ClockGenerator* clock_generator, PowerPort* power_port, int actuator_id, const std::string file_name, - double component_step_time_s, const GeomagneticField* geomagnetic_field); + double component_step_time_s, const environment::GeomagneticField* geomagnetic_field); } // namespace s2e::components diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index be7ddd363..1e57e269b 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -23,7 +23,7 @@ StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_g const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const dynamics::Dynamics* dynamics, - const LocalEnvironment* local_environment) + const environment::LocalEnvironment* local_environment) : Component(prescaler, clock_generator), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -48,7 +48,7 @@ StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_g const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const dynamics::Dynamics* dynamics, - const LocalEnvironment* local_environment) + const environment::LocalEnvironment* local_environment) : Component(prescaler, clock_generator, power_port), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -91,7 +91,8 @@ void StarSensor::Initialize() { error_flag_ = true; } -Quaternion StarSensor::Measure(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude) { +Quaternion StarSensor::Measure(const environment::LocalCelestialInformation* local_celestial_information, + const dynamics::attitude::Attitude* attitude) { update(local_celestial_information, attitude); // update delay buffer if (update_count_ == 0) { int hist = buffer_position_ - output_delay_ - 1; @@ -107,7 +108,7 @@ Quaternion StarSensor::Measure(const LocalCelestialInformation* local_celestial_ return measured_quaternion_i2c_; } -void StarSensor::update(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude) { +void StarSensor::update(const environment::LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude) { Quaternion quaternion_i2b = attitude->GetQuaternion_i2b(); // Read true value Quaternion q_stt_temp = quaternion_i2b * quaternion_b2c_; // Convert to component frame // Add noise on sight direction @@ -128,7 +129,8 @@ void StarSensor::update(const LocalCelestialInformation* local_celestial_informa buffer_position_ %= max_delay_; } -void StarSensor::AllJudgement(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude) { +void StarSensor::AllJudgement(const environment::LocalCelestialInformation* local_celestial_information, + const dynamics::attitude::Attitude* attitude) { int judgement = 0; judgement = SunJudgement(local_celestial_information->GetPositionFromSpacecraft_b_m("SUN")); judgement += EarthJudgement(local_celestial_information->GetPositionFromSpacecraft_b_m("EARTH")); @@ -217,7 +219,7 @@ void StarSensor::MainRoutine(const int time_count) { } StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, double component_step_time_s, - const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment) { + const dynamics::Dynamics* dynamics, const environment::LocalEnvironment* local_environment) { setting_file_reader::IniAccess STT_conf(file_name); const char* sensor_name = "STAR_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); @@ -250,7 +252,7 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sens } StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const string file_name, - double component_step_time_s, const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment) { + double component_step_time_s, const dynamics::Dynamics* dynamics, const environment::LocalEnvironment* local_environment) { setting_file_reader::IniAccess STT_conf(file_name); const char* sensor_name = "STAR_SENSOR_"; const std::string section_name = sensor_name + std::to_string(static_cast(sensor_id)); diff --git a/src/components/real/aocs/star_sensor.hpp b/src/components/real/aocs/star_sensor.hpp index 8ad44ae86..99bc47819 100644 --- a/src/components/real/aocs/star_sensor.hpp +++ b/src/components/real/aocs/star_sensor.hpp @@ -49,7 +49,7 @@ class StarSensor : public Component, public logger::ILoggable { const double standard_deviation_orthogonal_direction, const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, - const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment); + const dynamics::Dynamics* dynamics, const environment::LocalEnvironment* local_environment); /** * @fn StarSensor * @brief Constructor with power port @@ -75,7 +75,7 @@ class StarSensor : public Component, public logger::ILoggable { const double standard_deviation_sight_direction, const double step_time_s, const unsigned int output_delay, const unsigned int output_interval, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const double capture_rate_limit_rad_s, const dynamics::Dynamics* dynamics, - const LocalEnvironment* local_environment); + const environment::LocalEnvironment* local_environment); // Override functions for Component /** @@ -138,8 +138,8 @@ class StarSensor : public Component, public logger::ILoggable { double capture_rate_limit_rad_s_; //!< Angular rate limit to get correct attitude [rad/s] // Observed variables - const dynamics::Dynamics* dynamics_; //!< Dynamics information - const LocalEnvironment* local_environment_; //!< Local environment information + const dynamics::Dynamics* dynamics_; //!< Dynamics information + const environment::LocalEnvironment* local_environment_; //!< Local environment information // Internal functions /** @@ -148,14 +148,14 @@ class StarSensor : public Component, public logger::ILoggable { * @param [in] local_celestial_information: Local celestial information * @param [in] attitude: dynamics::attitude::Attitude information */ - void update(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); + void update(const environment::LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); /** * @fn Measure * @brief Calculate measured quaternion * @param [in] local_celestial_information: Local celestial information * @param [in] attitude: dynamics::attitude::Attitude information */ - math::Quaternion Measure(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); + math::Quaternion Measure(const environment::LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); /** * @fn AllJudgement @@ -163,7 +163,7 @@ class StarSensor : public Component, public logger::ILoggable { * @param [in] local_celestial_information: Local celestial information * @param [in] attitude: dynamics::attitude::Attitude information */ - void AllJudgement(const LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); + void AllJudgement(const environment::LocalCelestialInformation* local_celestial_information, const dynamics::attitude::Attitude* attitude); /** * @fn SunJudgement * @brief Judge violation of sun forbidden angle @@ -219,7 +219,7 @@ class StarSensor : public Component, public logger::ILoggable { * @param [in] local_environment: Local environment information */ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, double component_step_time_s, - const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment); + const dynamics::Dynamics* dynamics, const environment::LocalEnvironment* local_environment); /** * @fn InitStarSensor * @brief Initialize functions for StarSensor with power port @@ -232,7 +232,7 @@ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, int sens * @param [in] local_environment: Local environment information */ StarSensor InitStarSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, - double component_step_time_s, const dynamics::Dynamics* dynamics, const LocalEnvironment* local_environment); + double component_step_time_s, const dynamics::Dynamics* dynamics, const environment::LocalEnvironment* local_environment); } // namespace s2e::components diff --git a/src/components/real/aocs/sun_sensor.cpp b/src/components/real/aocs/sun_sensor.cpp index 63a5ea09d..0ef97c8db 100644 --- a/src/components/real/aocs/sun_sensor.cpp +++ b/src/components/real/aocs/sun_sensor.cpp @@ -19,7 +19,8 @@ namespace s2e::components { SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information) : Component(prescaler, clock_generator), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -33,7 +34,8 @@ SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_gen SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information) : Component(prescaler, clock_generator, power_port), component_id_(component_id), quaternion_b2c_(quaternion_b2c), @@ -154,7 +156,8 @@ string SunSensor::GetLogValue() const { } SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int ss_id, std::string file_name, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) { + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information) { setting_file_reader::IniAccess ss_conf(file_name); const char* sensor_name = "SUN_SENSOR_"; const std::string section_tmp = sensor_name + std::to_string(static_cast(ss_id)); @@ -187,7 +190,8 @@ SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int ss_id, } SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int ss_id, std::string file_name, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information) { + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information) { setting_file_reader::IniAccess ss_conf(file_name); const char* sensor_name = "SUN_SENSOR_"; const std::string section_tmp = sensor_name + std::to_string(static_cast(ss_id)); diff --git a/src/components/real/aocs/sun_sensor.hpp b/src/components/real/aocs/sun_sensor.hpp index 4d988d7ac..e235bc618 100644 --- a/src/components/real/aocs/sun_sensor.hpp +++ b/src/components/real/aocs/sun_sensor.hpp @@ -39,8 +39,8 @@ class SunSensor : public Component, public logger::ILoggable { */ SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, - const double intensity_lower_threshold_percent, const SolarRadiationPressureEnvironment* srp_environment, - const LocalCelestialInformation* local_celestial_information); + const double intensity_lower_threshold_percent, const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information); /** * @fn SunSensor * @brief Constructor with power port @@ -59,7 +59,8 @@ class SunSensor : public Component, public logger::ILoggable { SunSensor(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const math::Quaternion& quaternion_b2c, const double detectable_angle_rad, const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad, const double intensity_lower_threshold_percent, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information); // Override functions for Component /** @@ -108,8 +109,8 @@ class SunSensor : public Component, public logger::ILoggable { double bias_noise_beta_rad_ = 0.0; //!< Constant bias for beta angle (Value is calculated by random number generator) // Measured variables - const SolarRadiationPressureEnvironment* srp_environment_; //!< Solar Radiation Pressure environment - const LocalCelestialInformation* local_celestial_information_; //!< Local celestial information + const environment::SolarRadiationPressureEnvironment* srp_environment_; //!< Solar Radiation Pressure environment + const environment::LocalCelestialInformation* local_celestial_information_; //!< Local celestial information // functions /** @@ -153,7 +154,8 @@ class SunSensor : public Component, public logger::ILoggable { * @param [in] local_environment: Local environment information */ SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information); /** * @fn InitSunSensor * @brief Initialize functions for sun sensor with power port @@ -165,7 +167,8 @@ SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, int sensor * @param [in] local_environment: Local environment information */ SunSensor InitSunSensor(environment::ClockGenerator* clock_generator, PowerPort* power_port, int sensor_id, const std::string file_name, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information); } // namespace s2e::components diff --git a/src/components/real/communication/antenna.cpp b/src/components/real/communication/antenna.cpp index 9bb535a61..9d9fec92d 100644 --- a/src/components/real/communication/antenna.cpp +++ b/src/components/real/communication/antenna.cpp @@ -14,7 +14,7 @@ namespace s2e::components { Antenna::Antenna(const int component_id, const math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, - const double frequency_MHz, const Vector<5> tx_parameters, const Vector<4> rx_parameters) + const double frequency_MHz, const math::Vector<5> tx_parameters, const math::Vector<4> rx_parameters) : component_id_(component_id), is_transmitter_(is_transmitter), is_receiver_(is_receiver), frequency_MHz_(frequency_MHz) { quaternion_b2c_ = quaternion_b2c; @@ -112,7 +112,7 @@ Antenna InitAntenna(const int antenna_id, const std::string file_name) { const std::string section_name = "ANTENNA_" + std::to_string(static_cast(antenna_id)); - Quaternion quaternion_b2c; + math::Quaternion quaternion_b2c; antenna_conf.ReadQuaternion(section_name.c_str(), "quaternion_b2c", quaternion_b2c); bool is_transmitter = antenna_conf.ReadBoolean(section_name.c_str(), "is_transmitter"); diff --git a/src/components/real/communication/antenna.hpp b/src/components/real/communication/antenna.hpp index 841fb55d0..1d317140e 100644 --- a/src/components/real/communication/antenna.hpp +++ b/src/components/real/communication/antenna.hpp @@ -8,8 +8,6 @@ #include #include -using math::Quaternion; -using math::Vector; #include #include "./antenna_radiation_pattern.hpp" @@ -57,7 +55,7 @@ class Antenna { * @param [in] rx_parameters: gain, loss_feeder, loss_pointing, system_temperature for RX */ Antenna(const int component_id, const math::Quaternion& quaternion_b2c, const bool is_transmitter, const bool is_receiver, - const double frequency_MHz, const Vector<5> tx_parameters, const Vector<4> rx_parameters); + const double frequency_MHz, const math::Vector<5> tx_parameters, const math::Vector<4> rx_parameters); /** * @fn Antenna @@ -115,7 +113,7 @@ class Antenna { * @fn GetQuaternion_b2c * @brief Return quaternion from body to component */ - inline Quaternion GetQuaternion_b2c() const { return quaternion_b2c_; } + inline math::Quaternion GetQuaternion_b2c() const { return quaternion_b2c_; } /** * @fn IsTransmitter @@ -130,11 +128,11 @@ class Antenna { protected: // General info - int component_id_; //!< Antenna ID - Quaternion quaternion_b2c_; //!< Coordinate transform from body to component - bool is_transmitter_; //!< Antenna for transmitter or not - bool is_receiver_; //!< Antenna for receiver or not - double frequency_MHz_; //!< Center Frequency [MHz] + int component_id_; //!< Antenna ID + math::Quaternion quaternion_b2c_; //!< Coordinate transform from body to component + bool is_transmitter_; //!< Antenna for transmitter or not + bool is_receiver_; //!< Antenna for receiver or not + double frequency_MHz_; //!< Center Frequency [MHz] // Tx info double tx_bitrate_bps_; //!< Transmit bitrate [bps] diff --git a/src/components/real/communication/ground_station_calculator.cpp b/src/components/real/communication/ground_station_calculator.cpp index 8e00eeb82..9c83ae730 100644 --- a/src/components/real/communication/ground_station_calculator.cpp +++ b/src/components/real/communication/ground_station_calculator.cpp @@ -29,8 +29,8 @@ GroundStationCalculator::GroundStationCalculator(const double loss_polarization_ GroundStationCalculator::~GroundStationCalculator() {} -void GroundStationCalculator::Update(const Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, - const Antenna& ground_station_rx_antenna) { +void GroundStationCalculator::Update(const simulation::Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, + const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { bool is_visible = ground_station.IsVisible(spacecraft.GetSpacecraftId()); if (is_visible) { max_bitrate_Mbps_ = CalcMaxBitrate(spacecraft.GetDynamics(), spacecraft_tx_antenna, ground_station, ground_station_rx_antenna); @@ -43,7 +43,7 @@ void GroundStationCalculator::Update(const Spacecraft& spacecraft, const Antenna // Private functions double GroundStationCalculator::CalcMaxBitrate(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, - const GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { + const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { double cn0_dBHz = CalcCn0OnGs(dynamics, spacecraft_tx_antenna, ground_station, ground_station_rx_antenna); double margin_for_bitrate_dB = cn0_dBHz - (ebn0_dB_ + hardware_deterioration_dB_ + coding_gain_dB_) - margin_requirement_dB_; @@ -56,39 +56,39 @@ double GroundStationCalculator::CalcMaxBitrate(const dynamics::Dynamics& dynamic } double GroundStationCalculator::CalcReceiveMarginOnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, - const GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { + const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { double cn0_dB = CalcCn0OnGs(dynamics, spacecraft_tx_antenna, ground_station, ground_station_rx_antenna); double cn0_requirement_dB = ebn0_dB_ + hardware_deterioration_dB_ + coding_gain_dB_ + 10.0 * log10(spacecraft_tx_antenna.GetBitrate_bps()); return cn0_dB - cn0_requirement_dB; } double GroundStationCalculator::CalcCn0OnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, - const GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { + const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { if (!spacecraft_tx_antenna.IsTransmitter() || !ground_station_rx_antenna.IsReceiver()) { // Check compatibility of transmitter and receiver return 0.0f; } // Free space path loss - Vector<3> sc_pos_i = dynamics.GetOrbit().GetPosition_i_m(); - Vector<3> gs_pos_i = ground_station.GetPosition_i_m(); - Vector<3> pos_gs2sc_i = sc_pos_i - gs_pos_i; + math::Vector<3> sc_pos_i = dynamics.GetOrbit().GetPosition_i_m(); + math::Vector<3> gs_pos_i = ground_station.GetPosition_i_m(); + math::Vector<3> pos_gs2sc_i = sc_pos_i - gs_pos_i; double dist_sc_gs_km = pos_gs2sc_i.CalcNorm() / 1000.0; double loss_space_dB = -20.0 * log10(4.0 * math::pi * dist_sc_gs_km / (300.0 / spacecraft_tx_antenna.GetFrequency_MHz() / 1000.0)); // GS direction on SC TX antenna frame - Vector<3> sc_to_gs_i = gs_pos_i - sc_pos_i; + math::Vector<3> sc_to_gs_i = gs_pos_i - sc_pos_i; sc_to_gs_i = sc_to_gs_i.CalcNormalizedVector(); - Quaternion q_i_to_sc_ant = spacecraft_tx_antenna.GetQuaternion_b2c() * dynamics.GetAttitude().GetQuaternion_i2b(); - Vector<3> gs_direction_on_sc_frame = q_i_to_sc_ant.FrameConversion(sc_to_gs_i); + math::Quaternion q_i_to_sc_ant = spacecraft_tx_antenna.GetQuaternion_b2c() * dynamics.GetAttitude().GetQuaternion_i2b(); + math::Vector<3> gs_direction_on_sc_frame = q_i_to_sc_ant.FrameConversion(sc_to_gs_i); double theta_on_sc_antenna_rad = acos(gs_direction_on_sc_frame[2]); double phi_on_sc_antenna_rad = atan2(gs_direction_on_sc_frame[1], gs_direction_on_sc_frame[0]); // SC direction on GS RX antenna frame - Vector<3> gs_to_sc_ecef = dynamics.GetOrbit().GetPosition_ecef_m() - ground_station.GetPosition_ecef_m(); + math::Vector<3> gs_to_sc_ecef = dynamics.GetOrbit().GetPosition_ecef_m() - ground_station.GetPosition_ecef_m(); gs_to_sc_ecef = gs_to_sc_ecef.CalcNormalizedVector(); - Quaternion q_ecef_to_gs_ant = ground_station_rx_antenna.GetQuaternion_b2c() * ground_station.GetGeodeticPosition().GetQuaternionXcxfToLtc(); - Vector<3> sc_direction_on_gs_frame = q_ecef_to_gs_ant.FrameConversion(gs_to_sc_ecef); + math::Quaternion q_ecef_to_gs_ant = ground_station_rx_antenna.GetQuaternion_b2c() * ground_station.GetGeodeticPosition().GetQuaternionXcxfToLtc(); + math::Vector<3> sc_direction_on_gs_frame = q_ecef_to_gs_ant.FrameConversion(gs_to_sc_ecef); double theta_on_gs_antenna_rad = acos(sc_direction_on_gs_frame[2]); double phi_on_gs_antenna_rad = atan2(sc_direction_on_gs_frame[1], sc_direction_on_gs_frame[0]); diff --git a/src/components/real/communication/ground_station_calculator.hpp b/src/components/real/communication/ground_station_calculator.hpp index dbda8d681..bc5f8c0f4 100644 --- a/src/components/real/communication/ground_station_calculator.hpp +++ b/src/components/real/communication/ground_station_calculator.hpp @@ -51,7 +51,7 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station: Ground station information * @param [in] ground_station_rx_antenna: Antenna mounted on ground station */ - void Update(const Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, + void Update(const simulation::Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna); // Override logger::ILoggable TODO: Maybe we don't need logabble, and this class should be used as library. @@ -111,7 +111,7 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station_rx_antenna: Rx Antenna mounted on ground station * @return Max bitrate [Mbps] */ - double CalcMaxBitrate(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, + double CalcMaxBitrate(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna); /** * @fn CalcReceiveMarginOnGs @@ -122,8 +122,8 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station_rx_antenna: Rx Antenna mounted on ground station * @return Receive margin [dB] */ - double CalcReceiveMarginOnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, - const Antenna& ground_station_rx_antenna); + double CalcReceiveMarginOnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, + const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna); /** * @fn CalcCn0 @@ -134,7 +134,7 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station_rx_antenna: Rx Antenna mounted on ground station * @return CN0 [dB] */ - double CalcCn0OnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const GroundStation& ground_station, + double CalcCn0OnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna); }; diff --git a/src/components/real/mission/telescope.cpp b/src/components/real/mission/telescope.cpp index 7b704ba75..cf17359e4 100644 --- a/src/components/real/mission/telescope.cpp +++ b/src/components/real/mission/telescope.cpp @@ -10,16 +10,13 @@ #include #include -using namespace std; -using namespace s2e::math; - namespace s2e::components { Telescope::Telescope(environment::ClockGenerator* clock_generator, const math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, - const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, - const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) + const dynamics::attitude::Attitude* attitude, const environment::HipparcosCatalogue* hipparcos, + const environment::LocalCelestialInformation* local_celestial_information, const dynamics::orbit::Orbit* orbit) : Component(1, clock_generator), quaternion_b2c_(quaternion_b2c), sun_forbidden_angle_rad_(sun_forbidden_angle_rad), @@ -43,7 +40,7 @@ Telescope::Telescope(environment::ClockGenerator* clock_generator, const math::Q assert(x_field_of_view_rad < math::pi_2); // Avoid the case that the field of view is over 90 degrees assert(y_field_of_view_rad < math::pi_2); - sight_direction_c_ = Vector<3>(0); + sight_direction_c_ = math::Vector<3>(0); sight_direction_c_[0] = 1; // (1,0,0) at component frame, Sight direction vector // Set 0 when t=0 @@ -118,7 +115,7 @@ void Telescope::Observe(math::Vector<2>& position_image_sensor, const math::Vect } void Telescope::ObserveStars() { - Quaternion quaternion_i2b = attitude_->GetQuaternion_i2b(); + math::Quaternion quaternion_i2b = attitude_->GetQuaternion_i2b(); star_list_in_sight.clear(); // Clear first size_t count = 0; // Counter for while loop @@ -175,7 +172,7 @@ void Telescope::ObserveGroundPositionDeviation() { return; } - Quaternion quaternion_i2b = attitude_->GetQuaternion_i2b(); + math::Quaternion quaternion_i2b = attitude_->GetQuaternion_i2b(); math::Vector<3> spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m(); math::Vector<3> direction_ecef = (initial_ground_position_ecef_m_ - spacecraft_position_ecef_m).CalcNormalizedVector(); math::Matrix<3, 3> dcm_ecef_to_i = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose(); @@ -190,8 +187,8 @@ void Telescope::ObserveGroundPositionDeviation() { ground_position_y_image_sensor_ = ground_angle_y_rad / y_fov_per_pix_; } -string Telescope::GetLogHeader() const { - string str_tmp = ""; +std::string Telescope::GetLogHeader() const { + std::string str_tmp = ""; std::string component_name = "telescope_"; @@ -206,9 +203,9 @@ string Telescope::GetLogHeader() const { // When Hipparcos Catalogue was not read, no output of ObserveStars if (hipparcos_->IsCalcEnabled) { for (size_t i = 0; i < number_of_logged_stars_; i++) { - str_tmp += logger::WriteScalar(component_name + "hipparcos_id (" + to_string(i) + ")", " "); - str_tmp += logger::WriteScalar(component_name + "visible_magnitude (" + to_string(i) + ")", " "); - str_tmp += logger::WriteVector(component_name + "star_position (" + to_string(i) + ")", "img", "pix", 2); + str_tmp += logger::WriteScalar(component_name + "hipparcos_id (" + std::to_string(i) + ")", " "); + str_tmp += logger::WriteScalar(component_name + "visible_magnitude (" + std::to_string(i) + ")", " "); + str_tmp += logger::WriteVector(component_name + "star_position (" + std::to_string(i) + ")", "img", "pix", 2); } } @@ -220,8 +217,8 @@ string Telescope::GetLogHeader() const { return str_tmp; } -string Telescope::GetLogValue() const { - string str_tmp = ""; +std::string Telescope::GetLogValue() const { + std::string str_tmp = ""; str_tmp += logger::WriteScalar(is_sun_in_forbidden_angle); str_tmp += logger::WriteScalar(is_earth_in_forbidden_angle); str_tmp += logger::WriteScalar(is_moon_in_forbidden_angle); @@ -247,13 +244,13 @@ string Telescope::GetLogValue() const { return str_tmp; } -Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const string file_name, - const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, - const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) { +Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, + const dynamics::attitude::Attitude* attitude, const environment::HipparcosCatalogue* hipparcos, + const environment::LocalCelestialInformation* local_celestial_information, const dynamics::orbit::Orbit* orbit) { using math::pi; setting_file_reader::IniAccess Telescope_conf(file_name); - const string st_sensor_id = std::to_string(static_cast(sensor_id)); + const std::string st_sensor_id = std::to_string(static_cast(sensor_id)); const char* cs = st_sensor_id.data(); char TelescopeSection[30] = "TELESCOPE_"; diff --git a/src/components/real/mission/telescope.hpp b/src/components/real/mission/telescope.hpp index c39a46854..dc663de90 100644 --- a/src/components/real/mission/telescope.hpp +++ b/src/components/real/mission/telescope.hpp @@ -24,8 +24,8 @@ namespace s2e::components { * @brief Information of stars in the telescope's field of view */ struct Star { - HipparcosData hipparcos_data; //!< Hipparcos data - math::Vector<2> position_image_sensor; //!< Position of image sensor + environment::HipparcosData hipparcos_data; //!< Hipparcos data + math::Vector<2> position_image_sensor; //!< Position of image sensor }; /* @@ -55,7 +55,8 @@ class Telescope : public Component, public logger::ILoggable { Telescope(environment::ClockGenerator* clock_generator, const math::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad, const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, const dynamics::attitude::Attitude* attitude, - const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr); + const environment::HipparcosCatalogue* hipparcos, const environment::LocalCelestialInformation* local_celestial_information, + const dynamics::orbit::Orbit* orbit = nullptr); /** * @fn ~Telescope * @brief Destructor @@ -126,16 +127,16 @@ class Telescope : public Component, public logger::ILoggable { */ void ObserveStars(); - const dynamics::attitude::Attitude* attitude_; //!< dynamics::attitude::Attitude information - const HipparcosCatalogue* hipparcos_; //!< Star information - const LocalCelestialInformation* local_celestial_information_; //!< Local celestial information + const dynamics::attitude::Attitude* attitude_; //!< dynamics::attitude::Attitude information + const environment::HipparcosCatalogue* hipparcos_; //!< Star information + const environment::LocalCelestialInformation* local_celestial_information_; //!< Local celestial information /** * @fn ObserveGroundPositionDeviation * @brief Calculate the deviation of the ground position from its initial value in the image sensor */ void ObserveGroundPositionDeviation(); - const Orbit* orbit_; //!< Orbit information + const dynamics::orbit::Orbit* orbit_; //!< Orbit information // Override logger::ILoggable /** * @fn GetLogHeader @@ -169,8 +170,8 @@ class Telescope : public Component, public logger::ILoggable { * @param [in] local_celestial_information: Local celestial information */ Telescope InitTelescope(environment::ClockGenerator* clock_generator, int sensor_id, const std::string file_name, - const dynamics::attitude::Attitude* attitude, const HipparcosCatalogue* hipparcos, - const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr); + const dynamics::attitude::Attitude* attitude, const environment::HipparcosCatalogue* hipparcos, + const environment::LocalCelestialInformation* local_celestial_information, const dynamics::orbit::Orbit* orbit = nullptr); } // namespace s2e::components diff --git a/src/components/real/power/solar_array_panel.cpp b/src/components/real/power/solar_array_panel.cpp index f50959a07..2bd8632b4 100644 --- a/src/components/real/power/solar_array_panel.cpp +++ b/src/components/real/power/solar_array_panel.cpp @@ -13,8 +13,8 @@ namespace s2e::components { SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, - double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, - const LocalCelestialInformation* local_celestial_information, double component_step_time_s) + double transmission_efficiency, const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information, double component_step_time_s) : Component(prescaler, clock_generator), component_id_(component_id), number_of_series_(number_of_series), @@ -32,7 +32,7 @@ SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerato SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, - double transmission_efficiency, const SolarRadiationPressureEnvironment* srp_environment, + double transmission_efficiency, const environment::SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s) : Component(prescaler, clock_generator), component_id_(component_id), @@ -50,8 +50,8 @@ SolarArrayPanel::SolarArrayPanel(const int prescaler, environment::ClockGenerato SolarArrayPanel::SolarArrayPanel(environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, - const SolarRadiationPressureEnvironment* srp_environment, - const LocalCelestialInformation* local_celestial_information) + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information) : Component(10, clock_generator), component_id_(component_id), number_of_series_(number_of_series), @@ -118,8 +118,8 @@ void SolarArrayPanel::MainRoutine(const int time_count) { } SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, - double component_step_time_s) { + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information, double component_step_time_s) { setting_file_reader::IniAccess sap_conf(file_name); const std::string section_name = "SOLAR_ARRAY_PANEL_" + std::to_string(static_cast(sap_id)); @@ -152,7 +152,7 @@ SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id } SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, - const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s) { + const environment::SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s) { setting_file_reader::IniAccess sap_conf(file_name); const std::string section_name = "SOLAR_ARRAY_PANEL_" + std::to_string(static_cast(sap_id)); diff --git a/src/components/real/power/solar_array_panel.hpp b/src/components/real/power/solar_array_panel.hpp index e684062cf..63e7d796f 100644 --- a/src/components/real/power/solar_array_panel.hpp +++ b/src/components/real/power/solar_array_panel.hpp @@ -35,8 +35,8 @@ class SolarArrayPanel : public Component, public logger::ILoggable { */ SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, - double component_step_time_s); + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information, double component_step_time_s); /** * @fn SolarArrayPanel * @brief Constructor with prescaler @@ -54,7 +54,7 @@ class SolarArrayPanel : public Component, public logger::ILoggable { */ SolarArrayPanel(const int prescaler, environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, - const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s); + const environment::SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s); /** * @fn SolarArrayPanel * @brief Constructor without prescaler @@ -72,7 +72,8 @@ class SolarArrayPanel : public Component, public logger::ILoggable { */ SolarArrayPanel(environment::ClockGenerator* clock_generator, int component_id, int number_of_series, int number_of_parallel, double cell_area_m2, math::Vector<3> normal_vector, double cell_efficiency, double transmission_efficiency, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information); + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information); /** * @fn SolarArrayPanel * @brief Copy constructor @@ -117,8 +118,8 @@ class SolarArrayPanel : public Component, public logger::ILoggable { const double cell_efficiency_; //!< Power generation efficiency of solar cell const double transmission_efficiency_; //!< Efficiency of transmission to PCU - const SolarRadiationPressureEnvironment* const srp_environment_; //!< Solar Radiation Pressure environment - const LocalCelestialInformation* local_celestial_information_; //!< Local celestial information + const environment::SolarRadiationPressureEnvironment* const srp_environment_; //!< Solar Radiation Pressure environment + const environment::LocalCelestialInformation* local_celestial_information_; //!< Local celestial information double voltage_V_; //!< Voltage [V] double power_generation_W_; //!< Generated power [W] @@ -144,8 +145,8 @@ class SolarArrayPanel : public Component, public logger::ILoggable { * @param [in] component_step_time_s: Component step time [sec] */ SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, - const SolarRadiationPressureEnvironment* srp_environment, const LocalCelestialInformation* local_celestial_information, - double component_step_time_s); + const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::LocalCelestialInformation* local_celestial_information, double component_step_time_s); /* * @fn InitSAP @@ -157,7 +158,7 @@ SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id * @param [in] component_step_time_s: Component step time [sec] */ SolarArrayPanel InitSAP(environment::ClockGenerator* clock_generator, int sap_id, const std::string file_name, - const SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s); + const environment::SolarRadiationPressureEnvironment* srp_environment, double component_step_time_s); } // namespace s2e::components diff --git a/src/components/real/propulsion/simple_thruster.cpp b/src/components/real/propulsion/simple_thruster.cpp index dc07d7007..9bde027e3 100644 --- a/src/components/real/propulsion/simple_thruster.cpp +++ b/src/components/real/propulsion/simple_thruster.cpp @@ -14,8 +14,8 @@ namespace s2e::components { // Constructor SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, - const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, - const dynamics::Dynamics* dynamics) + const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, + const simulation::Structure* structure, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator), component_id_(component_id), thruster_position_b_m_(thruster_position_b_m), @@ -29,8 +29,8 @@ SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, - const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, - const dynamics::Dynamics* dynamics) + const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, + const simulation::Structure* structure, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator, power_port), component_id_(component_id), thruster_position_b_m_(thruster_position_b_m), @@ -123,7 +123,7 @@ math::Vector<3> SimpleThruster::CalcThrustDirection() { } SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, - const Structure* structure, const dynamics::Dynamics* dynamics) { + const simulation::Structure* structure, const dynamics::Dynamics* dynamics) { setting_file_reader::IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); auto* Section = section_str.c_str(); @@ -131,10 +131,10 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int prescaler = thruster_conf.ReadInt(Section, "prescaler"); if (prescaler <= 1) prescaler = 1; - Vector<3> thruster_pos; + math::Vector<3> thruster_pos; thruster_conf.ReadVector(Section, "thruster_position_b_m", thruster_pos); - Vector<3> thruster_dir; + math::Vector<3> thruster_dir; thruster_conf.ReadVector(Section, "thruster_direction_b", thruster_dir); double max_magnitude_N = thruster_conf.ReadDouble(Section, "thrust_magnitude_N"); @@ -151,7 +151,7 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, } SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, - const Structure* structure, const dynamics::Dynamics* dynamics) { + const simulation::Structure* structure, const dynamics::Dynamics* dynamics) { setting_file_reader::IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); auto* Section = section_str.c_str(); @@ -159,10 +159,10 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int prescaler = thruster_conf.ReadInt(Section, "prescaler"); if (prescaler <= 1) prescaler = 1; - Vector<3> thruster_pos; + math::Vector<3> thruster_pos; thruster_conf.ReadVector(Section, "thruster_position_b_m", thruster_pos); - Vector<3> thruster_dir; + math::Vector<3> thruster_dir; thruster_conf.ReadVector(Section, "thruster_direction_b", thruster_dir); double max_magnitude_N = thruster_conf.ReadDouble(Section, "thrust_magnitude_N"); diff --git a/src/components/real/propulsion/simple_thruster.hpp b/src/components/real/propulsion/simple_thruster.hpp index ca0476e04..c1e30b2e4 100644 --- a/src/components/real/propulsion/simple_thruster.hpp +++ b/src/components/real/propulsion/simple_thruster.hpp @@ -37,9 +37,10 @@ class SimpleThruster : public Component, public logger::ILoggable { * @param [in] structure: Spacecraft structure information * @param [in] dynamics: Spacecraft dynamics information */ - SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const Vector<3> thruster_position_b_m, - const Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, - const double direction_standard_deviation_rad, const Structure* structure, const dynamics::Dynamics* dynamics); + SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, + const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, + const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const simulation::Structure* structure, + const dynamics::Dynamics* dynamics); /** * @fn SimpleThruster * @brief Constructor with power port @@ -56,8 +57,8 @@ class SimpleThruster : public Component, public logger::ILoggable { * @param [in] dynamics: Spacecraft dynamics information */ SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const Vector<3> thruster_position_b_m, const Vector<3> thrust_direction_b, const double max_magnitude_N, - const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const Structure* structure, + const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, + const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const simulation::Structure* structure, const dynamics::Dynamics* dynamics); /** * @fn ~SimpleThruster @@ -94,12 +95,12 @@ class SimpleThruster : public Component, public logger::ILoggable { * @fn GetOutputThrust_b_N * @brief Return generated thrust on the body fixed frame [N] */ - inline const Vector<3> GetOutputThrust_b_N() const { return output_thrust_b_N_; }; + inline const math::Vector<3> GetOutputThrust_b_N() const { return output_thrust_b_N_; }; /** * @fn GetOutputTorque_b_Nm * @brief Return generated torque on the body fixed frame [Nm] */ - inline const Vector<3> GetOutputTorque_b_Nm() const { return output_torque_b_Nm_; }; + inline const math::Vector<3> GetOutputTorque_b_Nm() const { return output_torque_b_Nm_; }; // Setter /** @@ -110,17 +111,17 @@ class SimpleThruster : public Component, public logger::ILoggable { protected: // parameters - const int component_id_; //!< Thruster ID - Vector<3> thruster_position_b_m_{0.0}; //!< Thruster position @ body frame [m] - Vector<3> thrust_direction_b_{0.0}; //!< Thrust direction @ body frame - double duty_ = 0.0; //!< PWM Duty [0.0 : 1.0] - double thrust_magnitude_max_N_ = 0.0; //!< Maximum thrust magnitude [N] - double direction_noise_standard_deviation_rad_ = 0.0; //!< Standard deviation of thrust direction error [rad] - s2e::randomization::NormalRand magnitude_random_noise_; //!< Normal random for thrust magnitude error - s2e::randomization::NormalRand direction_random_noise_; //!< Normal random for thrust direction error + const int component_id_; //!< Thruster ID + math::Vector<3> thruster_position_b_m_{0.0}; //!< Thruster position @ body frame [m] + math::Vector<3> thrust_direction_b_{0.0}; //!< Thrust direction @ body frame + double duty_ = 0.0; //!< PWM Duty [0.0 : 1.0] + double thrust_magnitude_max_N_ = 0.0; //!< Maximum thrust magnitude [N] + double direction_noise_standard_deviation_rad_ = 0.0; //!< Standard deviation of thrust direction error [rad] + randomization::NormalRand magnitude_random_noise_; //!< Normal random for thrust magnitude error + randomization::NormalRand direction_random_noise_; //!< Normal random for thrust direction error // outputs - Vector<3> output_thrust_b_N_{0.0}; //!< Generated thrust on the body fixed frame [N] - Vector<3> output_torque_b_Nm_{0.0}; //!< Generated torque on the body fixed frame [Nm] + math::Vector<3> output_thrust_b_N_{0.0}; //!< Generated thrust on the body fixed frame [N] + math::Vector<3> output_torque_b_Nm_{0.0}; //!< Generated torque on the body fixed frame [Nm] /** * @fn CalcThrust @@ -132,7 +133,7 @@ class SimpleThruster : public Component, public logger::ILoggable { * @brief Calculate generated torque * @param [in] center_of_mass_b_m: Center of mass_kg position at body frame [m] */ - void CalcTorque(const Vector<3> center_of_mass_b_m); + void CalcTorque(const math::Vector<3> center_of_mass_b_m); /** * @fn CalcThrustMagnitude * @brief Calculate thrust magnitude @@ -144,7 +145,7 @@ class SimpleThruster : public Component, public logger::ILoggable { * @brief Calculate thrust direction * @return Thrust direction */ - Vector<3> CalcThrustDirection(); + math::Vector<3> CalcThrustDirection(); /** * @fn Initialize * @brief Initialize function @@ -153,8 +154,8 @@ class SimpleThruster : public Component, public logger::ILoggable { */ void Initialize(const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad); - const Structure* structure_; //!< Spacecraft structure information - const dynamics::Dynamics* dynamics_; //!< Spacecraft dynamics information + const simulation::Structure* structure_; //!< Spacecraft structure information + const dynamics::Dynamics* dynamics_; //!< Spacecraft dynamics information }; /** @@ -167,7 +168,7 @@ class SimpleThruster : public Component, public logger::ILoggable { * @param [in] dynamics: Spacecraft dynamics information */ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, - const Structure* structure, const dynamics::Dynamics* dynamics); + const simulation::Structure* structure, const dynamics::Dynamics* dynamics); /** * @fn InitSimpleThruster * @brief Initialize function os SimpleThruster @@ -179,7 +180,7 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, * @param [in] dynamics: Spacecraft dynamics information */ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, - const Structure* structure, const dynamics::Dynamics* dynamics); + const simulation::Structure* structure, const dynamics::Dynamics* dynamics); } // namespace s2e::components diff --git a/src/disturbances/air_drag.cpp b/src/disturbances/air_drag.cpp index 8b7503776..001422dbc 100644 --- a/src/disturbances/air_drag.cpp +++ b/src/disturbances/air_drag.cpp @@ -14,7 +14,7 @@ namespace s2e::disturbances { -AirDrag::AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, +AirDrag::AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, const double molecular_temperature_K, const double molecular_weight_g_mol, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled), wall_temperature_K_(wall_temperature_K), @@ -25,7 +25,7 @@ AirDrag::AirDrag(const std::vector& surfaces, const math::Vector<3>& ce cn_.assign(num, 0.0); } -void AirDrag::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { +void AirDrag::Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { double air_density_kg_m3 = local_environment.GetAtmosphere().GetAirDensity_kg_m3(); math::Matrix<3, 3> dcm_ecef2eci = @@ -60,7 +60,7 @@ double AirDrag::CalcFunctionChi(const double s) { return x; } -void AirDrag::CalcCnCt(const Vector<3>& velocity_b_m_s) { +void AirDrag::CalcCnCt(const math::Vector<3>& velocity_b_m_s) { double velocity_norm_m_s = velocity_b_m_s.CalcNorm(); // Re-emitting speed @@ -95,7 +95,8 @@ std::string AirDrag::GetLogValue() const { return str_tmp; } -AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, const Vector<3>& center_of_gravity_b_m) { +AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, + const math::Vector<3>& center_of_gravity_b_m) { auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "AIR_DRAG"; diff --git a/src/disturbances/air_drag.hpp b/src/disturbances/air_drag.hpp index 483c388f3..73d5cbd9b 100644 --- a/src/disturbances/air_drag.hpp +++ b/src/disturbances/air_drag.hpp @@ -32,7 +32,7 @@ class AirDrag : public SurfaceForce { * @param [in] molecular_weight_g_mol: Molecular weight [g/mol] * @param [in] is_calculation_enabled: Calculation flag */ - AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, + AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, const double molecular_temperature_K, const double molecular_weight_g_mol, const bool is_calculation_enabled = true); /** @@ -41,7 +41,7 @@ class AirDrag : public SurfaceForce { * @param [in] local_environment: Local environment information * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); + virtual void Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); // Override logger::ILoggable /** @@ -98,7 +98,8 @@ class AirDrag : public SurfaceForce { * @param [in] surfaces: surface information of the spacecraft * @param [in] center_of_gravity_b_m: Center of gravity position vector at body frame [m] */ -AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, const Vector<3>& center_of_gravity_b_m); +AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, + const math::Vector<3>& center_of_gravity_b_m); } // namespace s2e::disturbances diff --git a/src/disturbances/disturbance.hpp b/src/disturbances/disturbance.hpp index 274517736..d9de38dd9 100644 --- a/src/disturbances/disturbance.hpp +++ b/src/disturbances/disturbance.hpp @@ -41,7 +41,7 @@ class Disturbance : public logger::ILoggable { * @fn UpdateIfEnabled * @brief Update calculated disturbance when the calculation flag is true */ - virtual inline void UpdateIfEnabled(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { + virtual inline void UpdateIfEnabled(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { if (is_calculation_enabled_) { Update(local_environment, dynamics); } else { @@ -56,7 +56,7 @@ class Disturbance : public logger::ILoggable { * @fn Update * @brief Pure virtual function to define the disturbance calculation */ - virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) = 0; + virtual void Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) = 0; /** * @fn GetTorque_b_Nm diff --git a/src/disturbances/disturbances.cpp b/src/disturbances/disturbances.cpp index e3351d989..86e7da768 100644 --- a/src/disturbances/disturbances.cpp +++ b/src/disturbances/disturbances.cpp @@ -17,8 +17,8 @@ namespace s2e::disturbances { -Disturbances::Disturbances(const SimulationConfiguration* simulation_configuration, const int spacecraft_id, const Structure* structure, - const GlobalEnvironment* global_environment) { +Disturbances::Disturbances(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id, + const simulation::Structure* structure, const environment::GlobalEnvironment* global_environment) { InitializeInstances(simulation_configuration, spacecraft_id, structure, global_environment); InitializeForceAndTorque(); InitializeAcceleration(); @@ -30,7 +30,8 @@ Disturbances::~Disturbances() { } } -void Disturbances::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics, const SimulationTime* simulation_time) { +void Disturbances::Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics, + const environment::SimulationTime* simulation_time) { InitializeForceAndTorque(); InitializeAcceleration(); @@ -50,15 +51,15 @@ void Disturbances::Update(const LocalEnvironment& local_environment, const dynam } } -void Disturbances::LogSetup(Logger& logger) { +void Disturbances::LogSetup(logger::Logger& logger) { for (auto disturbance : disturbances_list_) { logger.AddLogList(disturbance); } logger.CopyFileToLogDirectory(initialize_file_name_); } -void Disturbances::InitializeInstances(const SimulationConfiguration* simulation_configuration, const int spacecraft_id, const Structure* structure, - const GlobalEnvironment* global_environment) { +void Disturbances::InitializeInstances(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id, + const simulation::Structure* structure, const environment::GlobalEnvironment* global_environment) { setting_file_reader::IniAccess ini_access = setting_file_reader::IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); initialize_file_name_ = ini_access.ReadString("SETTING_FILES", "disturbance_file"); @@ -93,10 +94,10 @@ void Disturbances::InitializeInstances(const SimulationConfiguration* simulation } void Disturbances::InitializeForceAndTorque() { - total_torque_b_Nm_ = Vector<3>(0.0); - total_force_b_N_ = Vector<3>(0.0); + total_torque_b_Nm_ = math::Vector<3>(0.0); + total_force_b_N_ = math::Vector<3>(0.0); } -void Disturbances::InitializeAcceleration() { total_acceleration_i_m_s2_ = Vector<3>(0.0); } +void Disturbances::InitializeAcceleration() { total_acceleration_i_m_s2_ = math::Vector<3>(0.0); } } // namespace s2e::disturbances diff --git a/src/disturbances/disturbances.hpp b/src/disturbances/disturbances.hpp index 67d1f473d..243f446b1 100644 --- a/src/disturbances/disturbances.hpp +++ b/src/disturbances/disturbances.hpp @@ -30,8 +30,8 @@ class Disturbances { * @param [in] structure: Structure information of spacecraft * @param [in] global_environment: Global environment information */ - Disturbances(const SimulationConfiguration* simulation_configuration, const int spacecraft_id, const Structure* structure, - const GlobalEnvironment* global_environment); + Disturbances(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id, const simulation::Structure* structure, + const environment::GlobalEnvironment* global_environment); /** * @fn ~Disturbances * @brief Destructor @@ -45,13 +45,14 @@ class Disturbances { * @param [in] dynamics: dynamics::Dynamics information * @param [in] simulation_time: Simulation time */ - void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics, const SimulationTime* simulation_time); + void Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics, + const environment::SimulationTime* simulation_time); /** * @fn LogSetup * @brief log setup for all disturbances * @param [in] logger: Logger */ - void LogSetup(Logger& logger); + void LogSetup(logger::Logger& logger); /** * @fn GetTorque @@ -75,9 +76,9 @@ class Disturbances { std::string initialize_file_name_; //!< Initialization file name std::vector disturbances_list_; //!< List of disturbances - Vector<3> total_torque_b_Nm_; //!< Total disturbance torque in the body frame [Nm] - Vector<3> total_force_b_N_; //!< Total disturbance force in the body frame [N] - Vector<3> total_acceleration_i_m_s2_; //!< Total disturbance acceleration in the inertial frame [m/s2] + math::Vector<3> total_torque_b_Nm_; //!< Total disturbance torque in the body frame [Nm] + math::Vector<3> total_force_b_N_; //!< Total disturbance force in the body frame [N] + math::Vector<3> total_acceleration_i_m_s2_; //!< Total disturbance acceleration in the inertial frame [m/s2] /** * @fn InitializeInstances @@ -87,8 +88,8 @@ class Disturbances { * @param [in] structure: Structure information of spacecraft * @param [in] global_environment: Global environment information */ - void InitializeInstances(const SimulationConfiguration* simulation_configuration, const int spacecraft_id, const Structure* structure, - const GlobalEnvironment* global_environment); + void InitializeInstances(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id, + const simulation::Structure* structure, const environment::GlobalEnvironment* global_environment); /** * @fn InitializeForceAndTorque * @brief Initialize disturbance force and torque diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index 382a10dc4..966a5497f 100644 --- a/src/disturbances/geopotential.cpp +++ b/src/disturbances/geopotential.cpp @@ -70,7 +70,7 @@ bool Geopotential::ReadCoefficientsEgm96(std::string file_name) { return true; } -void Geopotential::Update(const LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics) { +void Geopotential::Update(const environment::LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics) { #ifdef DEBUG_GEOPOTENTIAL chrono::system_clock::time_point start, end; start = chrono::system_clock::now(); diff --git a/src/disturbances/geopotential.hpp b/src/disturbances/geopotential.hpp index 235c1fef8..13be1f20c 100644 --- a/src/disturbances/geopotential.hpp +++ b/src/disturbances/geopotential.hpp @@ -48,7 +48,7 @@ class Geopotential : public Disturbance { * @param [in] local_environment: Local environment information * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics); + virtual void Update(const environment::LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics); // Override logger::ILoggable /** @@ -64,10 +64,10 @@ class Geopotential : public Disturbance { private: s2e::gravity::GravityPotential geopotential_; - size_t degree_; //!< Maximum degree setting to calculate the geo-potential - std::vector> c_; //!< Cosine coefficients - std::vector> s_; //!< Sine coefficients - Vector<3> acceleration_ecef_m_s2_; //!< Calculated acceleration in the ECEF frame [m/s2] + size_t degree_; //!< Maximum degree setting to calculate the geo-potential + std::vector> c_; //!< Cosine coefficients + std::vector> s_; //!< Sine coefficients + math::Vector<3> acceleration_ecef_m_s2_; //!< Calculated acceleration in the ECEF frame [m/s2] // debug math::Vector<3> debug_pos_ecef_m_; //!< Spacecraft position in ECEF frame [m] diff --git a/src/disturbances/gravity_gradient.cpp b/src/disturbances/gravity_gradient.cpp index 4bb880cc1..bf1b0ff1a 100644 --- a/src/disturbances/gravity_gradient.cpp +++ b/src/disturbances/gravity_gradient.cpp @@ -19,7 +19,7 @@ GravityGradient::GravityGradient(const bool is_calculation_enabled) GravityGradient::GravityGradient(const double gravity_constant_m3_s2, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, true), gravity_constant_m3_s2_(gravity_constant_m3_s2) {} -void GravityGradient::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { +void GravityGradient::Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { // TODO: use structure information to get inertia tensor CalcTorque_b_Nm(local_environment.GetCelestialInformation().GetCenterBodyPositionFromSpacecraft_b_m(), dynamics.GetAttitude().GetInertiaTensor_b_kgm2()); diff --git a/src/disturbances/gravity_gradient.hpp b/src/disturbances/gravity_gradient.hpp index ad5e6fcea..4a5f99e65 100644 --- a/src/disturbances/gravity_gradient.hpp +++ b/src/disturbances/gravity_gradient.hpp @@ -43,7 +43,7 @@ class GravityGradient : public Disturbance { * @param [in] local_environment: Local environment information * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); + virtual void Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); // Override logger::ILoggable /** diff --git a/src/disturbances/lunar_gravity_field.cpp b/src/disturbances/lunar_gravity_field.cpp index 7d37794e4..dd94cc74c 100644 --- a/src/disturbances/lunar_gravity_field.cpp +++ b/src/disturbances/lunar_gravity_field.cpp @@ -88,8 +88,8 @@ bool LunarGravityField::ReadCoefficientsGrgm1200a(std::string file_name) { return true; } -void LunarGravityField::Update(const LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics) { - const CelestialInformation global_celestial_information = local_environment.GetCelestialInformation().GetGlobalInformation(); +void LunarGravityField::Update(const environment::LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics) { + const environment::CelestialInformation global_celestial_information = local_environment.GetCelestialInformation().GetGlobalInformation(); math::Matrix<3, 3> dcm_mci2mcmf_ = global_celestial_information.GetMoonRotation().GetDcmJ2000ToMcmf(); math::Vector<3> spacecraft_position_mci_m = dynamics.GetOrbit().GetPosition_i_m(); diff --git a/src/disturbances/lunar_gravity_field.hpp b/src/disturbances/lunar_gravity_field.hpp index a117464f5..2faf53420 100644 --- a/src/disturbances/lunar_gravity_field.hpp +++ b/src/disturbances/lunar_gravity_field.hpp @@ -50,7 +50,7 @@ class LunarGravityField : public Disturbance { * @param [in] local_environment: Local environment information * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics); + virtual void Update(const environment::LocalEnvironment &local_environment, const dynamics::Dynamics &dynamics); // Override logger::ILoggable /** @@ -68,10 +68,10 @@ class LunarGravityField : public Disturbance { s2e::gravity::GravityPotential lunar_potential_; double reference_radius_km_; double gravity_constants_km3_s2_; - size_t degree_; //!< Maximum degree setting to calculate the geo-potential - std::vector> c_; //!< Cosine coefficients - std::vector> s_; //!< Sine coefficients - Vector<3> acceleration_mcmf_m_s2_; //!< Calculated acceleration in the MCMF(Moon Centered Moon Fixed) frame [m/s2] + size_t degree_; //!< Maximum degree setting to calculate the geo-potential + std::vector> c_; //!< Cosine coefficients + std::vector> s_; //!< Sine coefficients + math::Vector<3> acceleration_mcmf_m_s2_; //!< Calculated acceleration in the MCMF(Moon Centered Moon Fixed) frame [m/s2] // debug math::Vector<3> debug_pos_mcmf_m_; //!< Spacecraft position in MCMF frame [m] diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index 6165965f0..f39f3539b 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -15,18 +15,18 @@ namespace s2e::disturbances { -MagneticDisturbance::MagneticDisturbance(const ResidualMagneticMoment& rmm_params, const bool is_calculation_enabled) +MagneticDisturbance::MagneticDisturbance(const simulation::ResidualMagneticMoment& rmm_params, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, true), residual_magnetic_moment_(rmm_params) { rmm_b_Am2_ = residual_magnetic_moment_.GetConstantValue_b_Am2(); } -Vector<3> MagneticDisturbance::CalcTorque_b_Nm(const Vector<3>& magnetic_field_b_nT) { +math::Vector<3> MagneticDisturbance::CalcTorque_b_Nm(const math::Vector<3>& magnetic_field_b_nT) { CalcRMM(); - torque_b_Nm_ = kMagUnit_ * OuterProduct(rmm_b_Am2_, magnetic_field_b_nT); + torque_b_Nm_ = kMagUnit_ * math::OuterProduct(rmm_b_Am2_, magnetic_field_b_nT); return torque_b_Nm_; } -void MagneticDisturbance::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { +void MagneticDisturbance::Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { UNUSED(dynamics); CalcTorque_b_Nm(local_environment.GetGeomagneticField().GetGeomagneticField_b_nT()); @@ -35,7 +35,7 @@ void MagneticDisturbance::Update(const LocalEnvironment& local_environment, cons void MagneticDisturbance::CalcRMM() { static math::Vector<3> random_walk_std_dev(residual_magnetic_moment_.GetRandomWalkStandardDeviation_Am2()); static math::Vector<3> random_walk_limit(residual_magnetic_moment_.GetRandomWalkLimit_Am2()); - static RandomWalk<3> random_walk(0.1, random_walk_std_dev, random_walk_limit); // [FIXME] step width is constant + static randomization::RandomWalk<3> random_walk(0.1, random_walk_std_dev, random_walk_limit); // [FIXME] step width is constant static s2e::randomization::NormalRand normal_random(0.0, residual_magnetic_moment_.GetRandomNoiseStandardDeviation_Am2(), s2e::randomization::global_randomization.MakeSeed()); @@ -64,7 +64,7 @@ std::string MagneticDisturbance::GetLogValue() const { return str_tmp; } -MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const ResidualMagneticMoment& rmm_params) { +MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const simulation::ResidualMagneticMoment& rmm_params) { auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "MAGNETIC_DISTURBANCE"; diff --git a/src/disturbances/magnetic_disturbance.hpp b/src/disturbances/magnetic_disturbance.hpp index f618f9de3..166060674 100644 --- a/src/disturbances/magnetic_disturbance.hpp +++ b/src/disturbances/magnetic_disturbance.hpp @@ -27,7 +27,7 @@ class MagneticDisturbance : public Disturbance { * @param [in] rmm_parameters: RMM parameters of the spacecraft * @param [in] is_calculation_enabled: Calculation flag */ - MagneticDisturbance(const ResidualMagneticMoment& rmm_parameters, const bool is_calculation_enabled = true); + MagneticDisturbance(const simulation::ResidualMagneticMoment& rmm_parameters, const bool is_calculation_enabled = true); /** * @fn Update @@ -35,7 +35,7 @@ class MagneticDisturbance : public Disturbance { * @param [in] local_environment: Local environment information * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); + virtual void Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); // Override logger::ILoggable /** @@ -52,8 +52,8 @@ class MagneticDisturbance : public Disturbance { private: const double kMagUnit_ = 1.0e-9; //!< Constant value to change the unit [nT] -> [T] - math::Vector<3> rmm_b_Am2_; //!< True RMM of the spacecraft in the body frame [Am2] - const ResidualMagneticMoment& residual_magnetic_moment_; //!< RMM parameters + math::Vector<3> rmm_b_Am2_; //!< True RMM of the spacecraft in the body frame [Am2] + const simulation::ResidualMagneticMoment& residual_magnetic_moment_; //!< RMM parameters /** * @fn CalcRMM @@ -75,7 +75,7 @@ class MagneticDisturbance : public Disturbance { * @param [in] initialize_file_path: Initialize file path * @param [in] rmm_params: RMM parameters */ -MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const ResidualMagneticMoment& rmm_params); +MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const simulation::ResidualMagneticMoment& rmm_params); } // namespace s2e::disturbances diff --git a/src/disturbances/solar_radiation_pressure_disturbance.cpp b/src/disturbances/solar_radiation_pressure_disturbance.cpp index bd26dec2a..c5f6b7af1 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.cpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.cpp @@ -12,11 +12,11 @@ namespace s2e::disturbances { -SolarRadiationPressureDisturbance::SolarRadiationPressureDisturbance(const std::vector& surfaces, +SolarRadiationPressureDisturbance::SolarRadiationPressureDisturbance(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled) {} -void SolarRadiationPressureDisturbance::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { +void SolarRadiationPressureDisturbance::Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { UNUSED(dynamics); math::Vector<3> sun_position_from_sc_b_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_b_m("SUN"); @@ -54,8 +54,9 @@ std::string SolarRadiationPressureDisturbance::GetLogValue() const { return str_tmp; } -SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const std::string initialize_file_path, const std::vector& surfaces, - const Vector<3>& center_of_gravity_b_m) { +SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const std::string initialize_file_path, + const std::vector& surfaces, + const math::Vector<3>& center_of_gravity_b_m) { auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "SOLAR_RADIATION_PRESSURE_DISTURBANCE"; diff --git a/src/disturbances/solar_radiation_pressure_disturbance.hpp b/src/disturbances/solar_radiation_pressure_disturbance.hpp index 8cfcc3df1..73daceccd 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.hpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.hpp @@ -27,7 +27,7 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { * @param [in] center_of_gravity_b_m: Center of gravity position at the body frame [m] * @param [in] is_calculation_enabled: Calculation flag */ - SolarRadiationPressureDisturbance(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, + SolarRadiationPressureDisturbance(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled = true); /** @@ -36,7 +36,7 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { * @param [in] local_environment: Local environment information * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); + virtual void Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); // Override logger::ILoggable /** @@ -67,8 +67,9 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { * @param [in] surfaces: surface information of the spacecraft * @param [in] center_of_gravity_b_m: Center of gravity position vector at body frame [m] */ -SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const std::string initialize_file_path, const std::vector& surfaces, - const Vector<3>& center_of_gravity_b_m); +SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const std::string initialize_file_path, + const std::vector& surfaces, + const math::Vector<3>& center_of_gravity_b_m); } // namespace s2e::disturbances diff --git a/src/disturbances/surface_force.cpp b/src/disturbances/surface_force.cpp index 80ec260cf..4825640b7 100644 --- a/src/disturbances/surface_force.cpp +++ b/src/disturbances/surface_force.cpp @@ -9,7 +9,8 @@ namespace s2e::disturbances { -SurfaceForce::SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) +SurfaceForce::SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, + const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, true), surfaces_(surfaces), center_of_gravity_b_m_(center_of_gravity_b_m) { // Initialize vectors size_t num = surfaces_.size(); diff --git a/src/disturbances/surface_force.hpp b/src/disturbances/surface_force.hpp index 61eb8edba..53d145557 100644 --- a/src/disturbances/surface_force.hpp +++ b/src/disturbances/surface_force.hpp @@ -30,7 +30,8 @@ class SurfaceForce : public Disturbance { * @param [in] center_of_gravity_b_m: Center of gravity position at the body frame [m] * @param [in] is_calculation_enabled: Calculation flag */ - SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled = true); + SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, + const bool is_calculation_enabled = true); /** * @fn ~SurfaceForce * @brief Destructor @@ -39,8 +40,8 @@ class SurfaceForce : public Disturbance { protected: // Spacecraft Structure parameters - const std::vector& surfaces_; //!< List of surfaces - const math::Vector<3>& center_of_gravity_b_m_; //!< Position vector of the center of mass_kg at body frame [m] + const std::vector& surfaces_; //!< List of surfaces + const math::Vector<3>& center_of_gravity_b_m_; //!< Position vector of the center of mass_kg at body frame [m] // Internal calculated variables std::vector normal_coefficients_; //!< coefficients for out-plane force for each surface diff --git a/src/disturbances/third_body_gravity.cpp b/src/disturbances/third_body_gravity.cpp index 205c129fa..9e2d6881c 100644 --- a/src/disturbances/third_body_gravity.cpp +++ b/src/disturbances/third_body_gravity.cpp @@ -16,7 +16,7 @@ ThirdBodyGravity::ThirdBodyGravity(std::set third_body_list, const ThirdBodyGravity::~ThirdBodyGravity() {} -void ThirdBodyGravity::Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { +void ThirdBodyGravity::Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics) { acceleration_i_m_s2_ = math::Vector<3>(0.0); // initialize math::Vector<3> sc_position_i_m = dynamics.GetOrbit().GetPosition_i_m(); diff --git a/src/disturbances/third_body_gravity.hpp b/src/disturbances/third_body_gravity.hpp index c55d5f2e3..bb45bec30 100644 --- a/src/disturbances/third_body_gravity.hpp +++ b/src/disturbances/third_body_gravity.hpp @@ -41,7 +41,7 @@ class ThirdBodyGravity : public Disturbance { * @param [in] local_environment: Local environment information * @param [in] dynamics: dynamics::Dynamics information */ - virtual void Update(const LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); + virtual void Update(const environment::LocalEnvironment& local_environment, const dynamics::Dynamics& dynamics); private: std::set third_body_list_; //!< List of celestial bodies to calculate the third body disturbances diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index b079c68b8..86a6b2443 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -45,7 +45,7 @@ std::string Attitude::GetLogValue() const { return str_tmp; } -void Attitude::SetParameters(const MonteCarloSimulationExecutor& mc_simulator) { +void Attitude::SetParameters(const simulation::MonteCarloSimulationExecutor& mc_simulator) { GetInitializedMonteCarloParameterQuaternion(mc_simulator, "quaternion_i2b", quaternion_i2b_); } diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index f7cca9b68..e432e665c 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -28,7 +28,7 @@ AttitudeRk4::AttitudeRk4(const math::Vector<3>& angular_velocity_b_rad_s, const AttitudeRk4::~AttitudeRk4() {} -void AttitudeRk4::SetParameters(const MonteCarloSimulationExecutor& mc_simulator) { +void AttitudeRk4::SetParameters(const simulation::MonteCarloSimulationExecutor& mc_simulator) { Attitude::SetParameters(mc_simulator); GetInitializedMonteCarloParameterVector(mc_simulator, "angular_velocity_b_rad_s", angular_velocity_b_rad_s_); diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp index a43ba941d..dbfd569d4 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp @@ -61,7 +61,7 @@ std::string AttitudeWithCantileverVibration::GetLogValue() const { return str_tmp; } -void AttitudeWithCantileverVibration::SetParameters(const MonteCarloSimulationExecutor& mc_simulator) { +void AttitudeWithCantileverVibration::SetParameters(const simulation::MonteCarloSimulationExecutor& mc_simulator) { Attitude::SetParameters(mc_simulator); GetInitializedMonteCarloParameterVector(mc_simulator, "angular_velocity_b_rad_s", angular_velocity_b_rad_s_); diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index f564ebd21..d852dc9e3 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -11,8 +11,9 @@ namespace s2e::dynamics::attitude { ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const math::Quaternion quaternion_i2b, const math::Vector<3> main_target_direction_b, const math::Vector<3> sub_target_direction_b, - const math::Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, - const Orbit* orbit, const std::string& simulation_object_name) + const math::Matrix<3, 3>& inertia_tensor_kgm2, + const environment::LocalCelestialInformation* local_celestial_information, const orbit::Orbit* orbit, + const std::string& simulation_object_name) : Attitude(inertia_tensor_kgm2, simulation_object_name), main_mode_(main_mode), sub_mode_(sub_mode), diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index 0aea27194..f5dbb7b82 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -8,7 +8,7 @@ namespace s2e::dynamics::attitude { -Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* local_celestial_information, +Attitude* InitAttitude(std::string file_name, const orbit::Orbit* orbit, const environment::LocalCelestialInformation* local_celestial_information, const double step_width_s, const math::Matrix<3, 3>& inertia_tensor_kgm2, const int spacecraft_id) { setting_file_reader::IniAccess ini_file(file_name); const char* section_ = "ATTITUDE"; diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index 1a9da635e..30f5379ba 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -7,36 +7,37 @@ namespace s2e::dynamics { -Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, - const environment::LocalEnvironment* local_environment, const int spacecraft_id, simulation::Structure* structure, - simulation::RelativeInformation* relative_information) +Dynamics::Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, + const environment::LocalEnvironment* local_environment, const int spacecraft_id, simulation::Structure* structure, + simulation::RelativeInformation* relative_information) : structure_(structure), local_environment_(local_environment) { Initialize(simulation_configuration, simulation_time, spacecraft_id, structure, relative_information); } -Dynamics::~dynamics::Dynamics() { +Dynamics::~Dynamics() { delete attitude_; delete orbit_; delete temperature_; } -void Dynamics::Initialize(const SimulationConfiguration* simulation_configuration, const SimulationTime* simulation_time, const int spacecraft_id, - Structure* structure, RelativeInformation* relative_information) { - const LocalCelestialInformation& local_celestial_information = local_environment_->GetCelestialInformation(); +void Dynamics::Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, + const int spacecraft_id, simulation::Structure* structure, simulation::RelativeInformation* relative_information) { + const environment::LocalCelestialInformation& local_celestial_information = local_environment_->GetCelestialInformation(); // Initialize - orbit_ = InitOrbit(&(local_celestial_information.GetGlobalInformation()), simulation_configuration->spacecraft_file_list_[spacecraft_id], - simulation_time->GetOrbitRkStepTime_s(), simulation_time->GetCurrentTime_jd(), - local_celestial_information.GetGlobalInformation().GetCenterBodyGravityConstant_m3_s2(), "ORBIT", relative_information); - attitude_ = InitAttitude(simulation_configuration->spacecraft_file_list_[spacecraft_id], orbit_, &local_celestial_information, - simulation_time->GetAttitudeRkStepTime_s(), structure->GetKinematicsParameters().GetInertiaTensor_b_kgm2(), spacecraft_id); - temperature_ = InitTemperature(simulation_configuration->spacecraft_file_list_[spacecraft_id], simulation_time->GetThermalRkStepTime_s(), - &(local_environment_->GetSolarRadiationPressure()), &(local_environment_->GetEarthAlbedo())); + orbit_ = orbit::InitOrbit(&(local_celestial_information.GetGlobalInformation()), simulation_configuration->spacecraft_file_list_[spacecraft_id], + simulation_time->GetOrbitRkStepTime_s(), simulation_time->GetCurrentTime_jd(), + local_celestial_information.GetGlobalInformation().GetCenterBodyGravityConstant_m3_s2(), "ORBIT", relative_information); + attitude_ = attitude::InitAttitude(simulation_configuration->spacecraft_file_list_[spacecraft_id], orbit_, &local_celestial_information, + simulation_time->GetAttitudeRkStepTime_s(), structure->GetKinematicsParameters().GetInertiaTensor_b_kgm2(), + spacecraft_id); + temperature_ = thermal::InitTemperature(simulation_configuration->spacecraft_file_list_[spacecraft_id], simulation_time->GetThermalRkStepTime_s(), + &(local_environment_->GetSolarRadiationPressure()), &(local_environment_->GetEarthAlbedo())); // To get initial value orbit_->UpdateByAttitude(attitude_->GetQuaternion_i2b()); } -void Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestialInformation* local_celestial_information) { +void Dynamics::Update(const environment::SimulationTime* simulation_time, const environment::LocalCelestialInformation* local_celestial_information) { // Attitude propagation if (simulation_time->GetAttitudePropagateFlag()) { attitude_->Propagate(simulation_time->GetElapsedTime_s()); @@ -60,7 +61,7 @@ void Dynamics::ClearForceTorque(void) { orbit_->SetAcceleration_i_m_s2(zero); } -void Dynamics::LogSetup(Logger& logger) { +void Dynamics::LogSetup(logger::Logger& logger) { logger.AddLogList(attitude_); logger.AddLogList(orbit_); logger.AddLogList(temperature_); diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index be8534181..05f986807 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -40,7 +40,7 @@ class Dynamics { * @param [in] simulation_time: Simulation time * @param [in] local_celestial_information: Local celestial information * @param [in] spacecraft_id: Spacecraft ID of the spacecraft - * @param [in] structure: Structure of the spacecraft + * @param [in] structure: simulation::Structure of the spacecraft * @param [in] relative_information: Relative information */ Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index adf102ceb..5a1ae0d0b 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -11,7 +11,7 @@ namespace s2e::dynamics::orbit { -EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double gravity_constant_m3_s2, +EnckeOrbitPropagation::EnckeOrbitPropagation(const environment::CelestialInformation* celestial_information, const double gravity_constant_m3_s2, const double propagation_step_s, const double current_time_jd, const math::Vector<3> position_i_m, const math::Vector<3> velocity_i_m_s, const double error_tolerance) : Orbit(celestial_information), diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index 4101b6b71..68542f198 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -29,9 +29,9 @@ class EnckeOrbitPropagation : public Orbit, public math::OrdinaryDifferentialEqu * @param [in] velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] * @param [in] error_tolerance: Error tolerance threshold */ - EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double gravity_constant_m3_s2, const double propagation_step_s, - const double current_time_jd, const math::Vector<3> position_i_m, const math::Vector<3> velocity_i_m_s, - const double error_tolerance); + EnckeOrbitPropagation(const environment::CelestialInformation* celestial_information, const double gravity_constant_m3_s2, + const double propagation_step_s, const double current_time_jd, const math::Vector<3> position_i_m, + const math::Vector<3> velocity_i_m_s, const double error_tolerance); /** * @fn ~EnckeOrbitPropagation * @brief Destructor diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index 7daf57ec6..e039adb98 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -15,8 +15,8 @@ namespace s2e::dynamics::orbit { -Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, double current_time_jd, - double gravity_constant_m3_s2, std::string section, RelativeInformation* relative_information) { +Orbit* InitOrbit(const environment::CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, + double current_time_jd, double gravity_constant_m3_s2, std::string section, simulation::RelativeInformation* relative_information) { auto conf = setting_file_reader::IniAccess(initialize_file); const char* section_ = section.c_str(); Orbit* orbit; diff --git a/src/dynamics/orbit/initialize_orbit.hpp b/src/dynamics/orbit/initialize_orbit.hpp index a006fe204..dd38c127c 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -10,7 +10,9 @@ #include "orbit.hpp" +namespace s2e::simulation { class RelativeInformation; +} namespace s2e::dynamics::orbit { @@ -27,7 +29,7 @@ namespace s2e::dynamics::orbit { */ Orbit* InitOrbit(const environment::CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, double current_time_jd, double gravity_constant_m3_s2, std::string section = "ORBIT", - RelativeInformation* relative_information = (RelativeInformation*)nullptr); + simulation::RelativeInformation* relative_information = (simulation::RelativeInformation*)nullptr); /** * @fn InitializePosVel diff --git a/src/dynamics/orbit/kepler_orbit_propagation.cpp b/src/dynamics/orbit/kepler_orbit_propagation.cpp index dadb5ef50..4e9050844 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.cpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.cpp @@ -10,7 +10,7 @@ namespace s2e::dynamics::orbit { -KeplerOrbitPropagation::KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_time_jd, +KeplerOrbitPropagation::KeplerOrbitPropagation(const environment::CelestialInformation* celestial_information, const double current_time_jd, KeplerOrbit kepler_orbit) : Orbit(celestial_information), KeplerOrbit(kepler_orbit) { UpdateState(current_time_jd); diff --git a/src/dynamics/orbit/kepler_orbit_propagation.hpp b/src/dynamics/orbit/kepler_orbit_propagation.hpp index cd18bab8c..a43aff5c4 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.hpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.hpp @@ -25,7 +25,8 @@ class KeplerOrbitPropagation : public Orbit, public s2e::orbit::KeplerOrbit { * @param [in] current_time_jd: Current Julian day [day] * @param [in] kepler_orbit: Kepler orbit */ - KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_time_jd, s2e::orbit::KeplerOrbit kepler_orbit); + KeplerOrbitPropagation(const environment::CelestialInformation* celestial_information, const double current_time_jd, + s2e::orbit::KeplerOrbit kepler_orbit); /** * @fn ~KeplerOrbitPropagation * @brief Destructor diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index af1ee1d88..6f4b2608d 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -10,10 +10,10 @@ namespace s2e::dynamics::orbit { -RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, +RelativeOrbit::RelativeOrbit(const environment::CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, int reference_spacecraft_id, math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, - s2e::orbit::StmModel stm_model_type, RelativeInformation* relative_information) + s2e::orbit::StmModel stm_model_type, simulation::RelativeInformation* relative_information) : Orbit(celestial_information), math::OrdinaryDifferentialEquation<6>(time_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2), diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 5abf101cb..313ac9429 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -41,10 +41,10 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> * @param [in] stm_model_type: State transition matrix type * @param [in] relative_information: Relative information */ - RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, int reference_spacecraft_id, - math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, - s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, s2e::orbit::StmModel stm_model_type, - RelativeInformation* relative_information); + RelativeOrbit(const environment::CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, + int reference_spacecraft_id, math::Vector<3> relative_position_lvlh_m, math::Vector<3> relative_velocity_lvlh_m_s, + RelativeOrbitUpdateMethod update_method, s2e::orbit::RelativeOrbitModel relative_dynamics_model_type, + s2e::orbit::StmModel stm_model_type, simulation::RelativeInformation* relative_information); /** * @fn ~RelativeOrbit * @brief Destructor @@ -68,7 +68,7 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> * @param [in] state: Position and velocity as state vector * @param [out] rhs: Output of the function */ - virtual void DerivativeFunction(double t, const Vector<6>& state, Vector<6>& rhs); + virtual void DerivativeFunction(double t, const math::Vector<6>& state, math::Vector<6>& rhs); private: double gravity_constant_m3_s2_; //!< Gravity constant of the center body [m3/s2] @@ -86,7 +86,7 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> RelativeOrbitUpdateMethod update_method_; //!< Update method s2e::orbit::RelativeOrbitModel relative_dynamics_model_type_; //!< Relative dynamics model type s2e::orbit::StmModel stm_model_type_; //!< State Transition Matrix model type - RelativeInformation* relative_information_; //!< Relative information + simulation::RelativeInformation* relative_information_; //!< Relative information /** * @fn InitializeState diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index b9fea10c7..930677292 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -10,8 +10,8 @@ namespace s2e::dynamics::orbit { -Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, - math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s) +Rk4OrbitPropagation::Rk4OrbitPropagation(const environment::CelestialInformation* celestial_information, double gravity_constant_m3_s2, + double time_step_s, math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s) : Orbit(celestial_information), OrdinaryDifferentialEquation<6>(time_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2) { propagate_mode_ = OrbitPropagateMode::kRk4; diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index dbda96d86..30be7b161 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -29,7 +29,7 @@ class Rk4OrbitPropagation : public Orbit, public math::OrdinaryDifferentialEquat * @param [in] velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] * @param [in] initial_time_s: Initial time [sec] */ - Rk4OrbitPropagation(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, + Rk4OrbitPropagation(const environment::CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s, double initial_time_s = 0); /** * @fn ~Rk4OrbitPropagation diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index 1322cbeb6..691644184 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -11,8 +11,8 @@ namespace s2e::dynamics::orbit { -Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, const int wgs_setting, - const double current_time_jd) +Sgp4OrbitPropagation::Sgp4OrbitPropagation(const environment::CelestialInformation* celestial_information, char* tle1, char* tle2, + const int wgs_setting, const double current_time_jd) : Orbit(celestial_information) { propagate_mode_ = OrbitPropagateMode::kSgp4; diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.hpp b/src/dynamics/orbit/sgp4_orbit_propagation.hpp index 0bd59bfd3..5ccfea6ed 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.hpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.hpp @@ -30,7 +30,7 @@ class Sgp4OrbitPropagation : public Orbit { * @param [in] wgs_setting: Wold Geodetic System * @param [in] current_time_jd: Current Julian day [day] */ - Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, const int wgs_setting, + Sgp4OrbitPropagation(const environment::CelestialInformation* celestial_information, char* tle1, char* tle2, const int wgs_setting, const double current_time_jd); // Override Orbit diff --git a/src/dynamics/orbit/time_series_file_orbit_propagation.cpp b/src/dynamics/orbit/time_series_file_orbit_propagation.cpp index 8fe9893a0..5230a18c2 100644 --- a/src/dynamics/orbit/time_series_file_orbit_propagation.cpp +++ b/src/dynamics/orbit/time_series_file_orbit_propagation.cpp @@ -14,9 +14,12 @@ #include "math_physics/math/constants.hpp" #include "setting_file_reader/initialize_file_access.hpp" -TimeSeriesFileOrbitPropagation::TimeSeriesFileOrbitPropagation(const CelestialInformation* celestial_information, std::string time_series_file_path, - int number_of_interpolation, int interpolation_method, - double orbital_period_correction_s, const double current_time_jd) +namespace s2e::dynamics::orbit { + +TimeSeriesFileOrbitPropagation::TimeSeriesFileOrbitPropagation(const environment::CelestialInformation* celestial_information, + std::string time_series_file_path, int number_of_interpolation, + int interpolation_method, double orbital_period_correction_s, + const double current_time_jd) : Orbit(celestial_information), is_time_range_warning_displayed_(false), is_interpolation_method_error_displayed_(false), @@ -27,7 +30,7 @@ TimeSeriesFileOrbitPropagation::TimeSeriesFileOrbitPropagation(const CelestialIn propagate_mode_ = OrbitPropagateMode::kTimeSeriesFile; // Read time series file - IniAccess time_series_file(time_series_file_path); + setting_file_reader::IniAccess time_series_file(time_series_file_path); time_series_file.ReadCsvDoubleWithHeader(time_series_data_, 7, 1, 0); // Get general info @@ -41,8 +44,8 @@ TimeSeriesFileOrbitPropagation::TimeSeriesFileOrbitPropagation(const CelestialIn reference_time_ = CalcEphemerisTimeData(reference_interpolation_id_); // Initialize orbit - orbit_position_i_m_.assign(1, orbit::InterpolationOrbit(number_of_interpolation)); - orbit_velocity_i_m_s_.assign(1, orbit::InterpolationOrbit(number_of_interpolation)); + orbit_position_i_m_.assign(1, s2e::orbit::InterpolationOrbit(number_of_interpolation)); + orbit_velocity_i_m_s_.assign(1, s2e::orbit::InterpolationOrbit(number_of_interpolation)); // Initialize interpolation for (int i = 0; i < number_of_interpolation; i++) { @@ -152,3 +155,5 @@ bool TimeSeriesFileOrbitPropagation::UpdateInterpolationInformation() { return true; } + +} // namespace s2e::dynamics::orbit diff --git a/src/dynamics/orbit/time_series_file_orbit_propagation.hpp b/src/dynamics/orbit/time_series_file_orbit_propagation.hpp index 0de5a7278..722796b00 100644 --- a/src/dynamics/orbit/time_series_file_orbit_propagation.hpp +++ b/src/dynamics/orbit/time_series_file_orbit_propagation.hpp @@ -12,6 +12,7 @@ #include "orbit.hpp" +namespace s2e::dynamics::orbit { /** * @class TimeSeriesFileOrbitPropagation * @brief Class to calculate satellite orbit using interpolation with orbit time series input @@ -28,7 +29,7 @@ class TimeSeriesFileOrbitPropagation : public Orbit { * @param [in] orbital_period_correction_s: Orbital period correction [s] * @param [in] current_time_jd: Current Julian day [day] */ - TimeSeriesFileOrbitPropagation(const CelestialInformation* celestial_information, const std::string time_series_file_path, + TimeSeriesFileOrbitPropagation(const environment::CelestialInformation* celestial_information, const std::string time_series_file_path, const int number_of_interpolation, const int interpolation_method, const double orbital_period_correction_s, const double current_time_jd); @@ -73,8 +74,8 @@ class TimeSeriesFileOrbitPropagation : public Orbit { double reference_time_; //!< Reference start time of the time series data handling size_t reference_interpolation_id_; //!< Reference EphemerisTime ID of the interpolation - std::vector orbit_position_i_m_; //!< Position with interpolation - std::vector orbit_velocity_i_m_s_; //!< Velocity with interpolation + std::vector orbit_position_i_m_; //!< Position with interpolation + std::vector orbit_velocity_i_m_s_; //!< Velocity with interpolation /** * @fn UpdateInterpolationInformation @@ -84,4 +85,6 @@ class TimeSeriesFileOrbitPropagation : public Orbit { bool UpdateInterpolationInformation(); }; +} // namespace s2e::dynamics::orbit + #endif // S2E_DYNAMICS_ORBIT_TIME_SERIES_FILE_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/thermal/node.hpp b/src/dynamics/thermal/node.hpp index 6d9cb0434..6c9da2660 100644 --- a/src/dynamics/thermal/node.hpp +++ b/src/dynamics/thermal/node.hpp @@ -173,6 +173,5 @@ class Node { Node InitNode(const std::vector& node_str); } // namespace s2e::dynamics::thermal -namespace s2e::dynamics::thermal #endif // S2E_DYNAMICS_THERMAL_NODE_HPP_ diff --git a/src/dynamics/thermal/temperature.cpp b/src/dynamics/thermal/temperature.cpp index fecd97cd4..4fd6cc251 100644 --- a/src/dynamics/thermal/temperature.cpp +++ b/src/dynamics/thermal/temperature.cpp @@ -18,8 +18,9 @@ namespace s2e::dynamics::thermal { Temperature::Temperature(const vector> conductance_matrix_W_K, const vector> radiation_matrix_m2, vector nodes, vector heatloads, vector heaters, vector heater_controllers, const size_t node_num, - const double propagation_step_s, const SolarRadiationPressureEnvironment* srp_environment, const EarthAlbedo* earth_albedo, - const bool is_calc_enabled, const SolarCalcSetting solar_calc_setting, const bool debug) + const double propagation_step_s, const environment::SolarRadiationPressureEnvironment* srp_environment, + const environment::EarthAlbedo* earth_albedo, const bool is_calc_enabled, const SolarCalcSetting solar_calc_setting, + const bool debug) : conductance_matrix_W_K_(conductance_matrix_W_K), radiation_matrix_m2_(radiation_matrix_m2), nodes_(nodes), @@ -50,7 +51,7 @@ Temperature::Temperature() { Temperature::~Temperature() {} -void Temperature::Propagate(const LocalCelestialInformation* local_celestial_information, const double time_end_s) { +void Temperature::Propagate(const environment::LocalCelestialInformation* local_celestial_information, const double time_end_s) { if (!is_calc_enabled_) return; while (time_end_s - propagation_time_s_ - propagation_step_s_ > 1.0e-6) { CalcRungeOneStep(propagation_time_s_, propagation_step_s_, local_celestial_information, node_num_); @@ -93,7 +94,7 @@ void Temperature::Propagate(const LocalCelestialInformation* local_celestial_inf } } -void Temperature::CalcRungeOneStep(double time_now_s, double time_step_s, const LocalCelestialInformation* local_celestial_information, +void Temperature::CalcRungeOneStep(double time_now_s, double time_step_s, const environment::LocalCelestialInformation* local_celestial_information, size_t node_num) { vector temperatures_now_K(node_num); for (size_t i = 0; i < node_num; i++) { @@ -131,7 +132,7 @@ void Temperature::CalcRungeOneStep(double time_now_s, double time_step_s, const } vector Temperature::CalcTemperatureDifferentials(vector temperatures_K, double t, - const LocalCelestialInformation* local_celestial_information, size_t node_num) { + const environment::LocalCelestialInformation* local_celestial_information, size_t node_num) { // TODO: consider the following unused arguments are really needed UNUSED(temperatures_K); @@ -291,8 +292,8 @@ First row is time data using std::string; using std::vector; -Temperature* InitTemperature(const std::string file_name, const double rk_prop_step_s, const SolarRadiationPressureEnvironment* srp_environment, - const EarthAlbedo* earth_albedo) { +Temperature* InitTemperature(const std::string file_name, const double rk_prop_step_s, + const environment::SolarRadiationPressureEnvironment* srp_environment, const environment::EarthAlbedo* earth_albedo) { auto mainIni = setting_file_reader::IniAccess(file_name); vector node_list; diff --git a/src/environment/local/local_environment.cpp b/src/environment/local/local_environment.cpp index 3f53239d1..7be568bce 100644 --- a/src/environment/local/local_environment.cpp +++ b/src/environment/local/local_environment.cpp @@ -10,8 +10,8 @@ namespace s2e::environment { -LocalEnvironment::LocalEnvironment(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, - const int spacecraft_id) { +LocalEnvironment::LocalEnvironment(const simulation::SimulationConfiguration* simulation_configuration, + const environment::GlobalEnvironment* global_environment, const int spacecraft_id) { Initialize(simulation_configuration, global_environment, spacecraft_id); } @@ -23,8 +23,8 @@ LocalEnvironment::~LocalEnvironment() { delete celestial_information_; } -void LocalEnvironment::Initialize(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, - const int spacecraft_id) { +void LocalEnvironment::Initialize(const simulation::SimulationConfiguration* simulation_configuration, + const environment::GlobalEnvironment* global_environment, const int spacecraft_id) { // Read file name setting_file_reader::IniAccess iniAccess = setting_file_reader::IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); std::string ini_fname = iniAccess.ReadString("SETTING_FILES", "local_environment_file"); diff --git a/src/environment/local/local_environment.hpp b/src/environment/local/local_environment.hpp index b486d4226..e35b4973d 100644 --- a/src/environment/local/local_environment.hpp +++ b/src/environment/local/local_environment.hpp @@ -15,7 +15,7 @@ #include "simulation/simulation_configuration.hpp" #include "solar_radiation_pressure_environment.hpp" -namespace s2e::math { +namespace s2e::dynamics { class Dynamics; } @@ -34,7 +34,7 @@ class LocalEnvironment { * @param [in] global_environment: Global environment * @param [in] spacecraft_id: Satellite ID */ - LocalEnvironment(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, + LocalEnvironment(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const int spacecraft_id); /** * @fn ~LocalEnvironment @@ -96,7 +96,7 @@ class LocalEnvironment { * @param [in] global_environment: Global environment * @param [in] spacecraft_id: Satellite ID */ - void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const GlobalEnvironment* global_environment, + void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const int spacecraft_id); }; diff --git a/src/math_physics/numerical_integration/numerical_integrator.hpp b/src/math_physics/numerical_integration/numerical_integrator.hpp index 461bfa225..ae708a753 100644 --- a/src/math_physics/numerical_integration/numerical_integrator.hpp +++ b/src/math_physics/numerical_integration/numerical_integrator.hpp @@ -32,7 +32,7 @@ class NumericalIntegrator { * @fn ~NumericalIntegrator * @brief Destructor */ - inline virtual ~NumericalIntegrator(){}; + inline virtual ~NumericalIntegrator() {}; /** * @fn Integrate diff --git a/src/math_physics/numerical_integration/runge_kutta.hpp b/src/math_physics/numerical_integration/runge_kutta.hpp index d2072d4cb..a24b4f53b 100644 --- a/src/math_physics/numerical_integration/runge_kutta.hpp +++ b/src/math_physics/numerical_integration/runge_kutta.hpp @@ -29,7 +29,7 @@ class RungeKutta : public NumericalIntegrator { * @fn ~RungeKutta * @brief Destructor */ - inline virtual ~RungeKutta(){}; + inline virtual ~RungeKutta() {}; /** * @fn Integrate diff --git a/src/simulation/spacecraft/spacecraft.cpp b/src/simulation/spacecraft/spacecraft.cpp index 36a6f91a8..19f4db5c5 100644 --- a/src/simulation/spacecraft/spacecraft.cpp +++ b/src/simulation/spacecraft/spacecraft.cpp @@ -30,10 +30,10 @@ Spacecraft::~Spacecraft() { void Spacecraft::Initialize(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const int spacecraft_id, RelativeInformation* relative_information) { clock_generator_.ClearTimerCount(); - structure_ = new Structure(simulation_configuration, spacecraft_id); + structure_ = new simulation::Structure(simulation_configuration, spacecraft_id); local_environment_ = new environment::LocalEnvironment(simulation_configuration, global_environment, spacecraft_id); - dynamics_ = new dynamics::dynamics::Dynamics(simulation_configuration, &(global_environment->GetSimulationTime()), local_environment_, - spacecraft_id, structure_, relative_information); + dynamics_ = new dynamics::Dynamics(simulation_configuration, &(global_environment->GetSimulationTime()), local_environment_, spacecraft_id, + structure_, relative_information); disturbances_ = new disturbances::Disturbances(simulation_configuration, spacecraft_id, structure_, global_environment); simulation_configuration->main_logger_->CopyFileToLogDirectory(simulation_configuration->spacecraft_file_list_[spacecraft_id]); diff --git a/src/simulation/spacecraft/spacecraft.hpp b/src/simulation/spacecraft/spacecraft.hpp index d1e99a444..dc673105b 100644 --- a/src/simulation/spacecraft/spacecraft.hpp +++ b/src/simulation/spacecraft/spacecraft.hpp @@ -71,7 +71,7 @@ class Spacecraft { * @fn GetDynamics * @brief Get dynamics of the spacecraft */ - inline const dynamics::dynamics::Dynamics& GetDynamics() const { return *dynamics_; } + inline const dynamics::Dynamics& GetDynamics() const { return *dynamics_; } /** * @fn GetLocalEnvironment * @brief Get local environment around the spacecraft @@ -86,7 +86,7 @@ class Spacecraft { * @fn GetStructure * @brief Get structure of the spacecraft */ - inline const Structure& GetStructure() const { return *structure_; } + inline const simulation::Structure& GetStructure() const { return *structure_; } /** * @fn GetInstalledComponents * @brief Get components installed on the spacecraft @@ -100,11 +100,11 @@ class Spacecraft { protected: environment::ClockGenerator clock_generator_; //!< Origin of clock for the spacecraft - dynamics::dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft + dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft RelativeInformation* relative_information_; //!< Relative information with respect to the other spacecraft environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft disturbances::Disturbances* disturbances_; //!< Disturbance information acting on the spacecraft - Structure* structure_; //!< Structure information of the spacecraft + simulation::Structure* structure_; //!< Structure information of the spacecraft InstalledComponents* components_; //!< Components information installed on the spacecraft const unsigned int spacecraft_id_; //!< ID of the spacecraft }; diff --git a/src/simulation/spacecraft/structure/initialize_structure.cpp b/src/simulation/spacecraft/structure/initialize_structure.cpp index 44ed97d14..bfdea9145 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.cpp +++ b/src/simulation/spacecraft/structure/initialize_structure.cpp @@ -31,14 +31,14 @@ KinematicsParameters InitKinematicsParameters(std::string file_name) { return kinematics_params; } -std::vector InitSurfaces(std::string file_name) { +std::vector InitSurfaces(std::string file_name) { using std::cout; auto conf = setting_file_reader::IniAccess(file_name); const char* section = "SURFACES"; const int num_surface = conf.ReadInt(section, "number_of_surfaces"); - std::vector surfaces; + std::vector surfaces; for (int i = 0; i < num_surface; i++) { std::string idx = std::to_string(i); @@ -89,7 +89,7 @@ std::vector InitSurfaces(std::string file_name) { break; } - Vector<3> position, normal; + math::Vector<3> position, normal; keyword = "position" + idx + "_b_m"; conf.ReadVector(section, keyword.c_str(), position); @@ -103,7 +103,7 @@ std::vector InitSurfaces(std::string file_name) { } // Add a surface - surfaces.push_back(Surface(position, normal, area, ref, spe, air_spe)); + surfaces.push_back(simulation::Surface(position, normal, area, ref, spe, air_spe)); } return surfaces; } diff --git a/src/simulation/spacecraft/structure/initialize_structure.hpp b/src/simulation/spacecraft/structure/initialize_structure.hpp index 0ab741839..34a58888e 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.hpp +++ b/src/simulation/spacecraft/structure/initialize_structure.hpp @@ -21,7 +21,7 @@ KinematicsParameters InitKinematicsParameters(std::string file_name); * @fn InitSurfaces * @brief Initialize the multiple surfaces with an ini file */ -std::vector InitSurfaces(std::string file_name); +std::vector InitSurfaces(std::string file_name); /** * @fn InitResidualMagneticMoment * @brief Initialize the RMM(Residual Magnetic Moment) parameters with an ini file diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.hpp b/src/simulation/spacecraft/structure/kinematics_parameters.hpp index b97209727..c4dfadeb2 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.hpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.hpp @@ -26,7 +26,7 @@ class KinematicsParameters { * @fn ~KinematicsParameters * @brief Destructor */ - ~KinematicsParameters(){}; + ~KinematicsParameters() {}; // Getter /** diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp index 1e00b7e89..ae06bb85d 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp @@ -26,7 +26,7 @@ class ResidualMagneticMoment { * @fn ~ResidualMagneticMoment * @brief Destructor */ - ~ResidualMagneticMoment(){}; + ~ResidualMagneticMoment() {}; // Getter /** diff --git a/src/simulation/spacecraft/structure/structure.hpp b/src/simulation/spacecraft/structure/structure.hpp index fe099cd04..6a1b87ae0 100644 --- a/src/simulation/spacecraft/structure/structure.hpp +++ b/src/simulation/spacecraft/structure/structure.hpp @@ -42,7 +42,7 @@ class Structure { * @fn GetSurfaces * @brief Return surface information */ - inline const std::vector& GetSurfaces() const { return surfaces_; } + inline const std::vector& GetSurfaces() const { return surfaces_; } /** * @fn GetKinematicsParameters * @brief Return kinematics information @@ -58,7 +58,7 @@ class Structure { * @fn GetToSetSurfaces * @brief Return surface information */ - inline std::vector& GetToSetSurfaces() { return surfaces_; } + inline std::vector& GetToSetSurfaces() { return surfaces_; } /** * @fn GetToSetKinematicsParameters * @brief Return kinematics information @@ -72,7 +72,7 @@ class Structure { private: KinematicsParameters* kinematics_parameters_; //!< Kinematics parameters - std::vector surfaces_; //!< Surface information + std::vector surfaces_; //!< Surface information ResidualMagneticMoment* residual_magnetic_moment_; //!< Residual Magnetic Moment }; diff --git a/src/simulation/spacecraft/structure/surface.hpp b/src/simulation/spacecraft/structure/surface.hpp index c98386bae..73a09d6e6 100644 --- a/src/simulation/spacecraft/structure/surface.hpp +++ b/src/simulation/spacecraft/structure/surface.hpp @@ -26,7 +26,7 @@ class Surface { * @fn ~Surface * @brief Destructor */ - ~Surface(){}; + ~Surface() {}; // Getter /** diff --git a/src/simulation_sample/ground_station/sample_ground_station.cpp b/src/simulation_sample/ground_station/sample_ground_station.cpp index 4ec523260..d75acb258 100644 --- a/src/simulation_sample/ground_station/sample_ground_station.cpp +++ b/src/simulation_sample/ground_station/sample_ground_station.cpp @@ -16,7 +16,7 @@ SampleGroundStation::SampleGroundStation(const simulation::SimulationConfigurati SampleGroundStation::~SampleGroundStation() { delete components_; } -void SampleGroundStation::LogSetup(Logger& logger) { +void SampleGroundStation::LogSetup(logger::Logger& logger) { simulation::GroundStation::LogSetup(logger); components_->CompoLogSetUp(logger); } diff --git a/src/simulation_sample/ground_station/sample_ground_station.hpp b/src/simulation_sample/ground_station/sample_ground_station.hpp index aafafada0..bee3024f9 100644 --- a/src/simulation_sample/ground_station/sample_ground_station.hpp +++ b/src/simulation_sample/ground_station/sample_ground_station.hpp @@ -39,7 +39,7 @@ class SampleGroundStation : public simulation::GroundStation { * @fn LogSetup * @brief Override function of LogSetup in GroundStation class */ - virtual void LogSetup(Logger& logger); + virtual void LogSetup(logger::Logger& logger); /** * @fn Update * @brief Override function of Update in GroundStation class diff --git a/src/simulation_sample/ground_station/sample_ground_station_components.hpp b/src/simulation_sample/ground_station/sample_ground_station_components.hpp index 35d5571da..35df9ce22 100644 --- a/src/simulation_sample/ground_station/sample_ground_station_components.hpp +++ b/src/simulation_sample/ground_station/sample_ground_station_components.hpp @@ -31,7 +31,7 @@ class SampleGsComponents { * @fn CompoLogSetUp * @brief Log setup for ground station components */ - void CompoLogSetUp(Logger& logger); + void CompoLogSetUp(logger::Logger& logger); // Getter /** diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index 1554f6ccb..2226ce032 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -13,7 +13,7 @@ namespace s2e::sample { using namespace components; -SampleComponents::SampleComponents(const dynamics::dynamics::Dynamics* dynamics, simulation::Structure* structure, +SampleComponents::SampleComponents(const dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, const environment::GlobalEnvironment* global_environment, const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, const unsigned int spacecraft_id) diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index 428ec9645..b15bc5b2c 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -34,23 +34,25 @@ #include #include -namespace s2e::sample { +namespace s2e::components { +class OnBoardComputer; +class PowerControlUnit; +class GyroSensor; +class Magnetometer; +class StarSensor; +class SunSensor; +class GnssReceiver; +class Magnetorquer; +class ReactionWheel; +class SimpleThruster; +class ForceGenerator; +class TorqueGenerator; +class AngularVelocityObserver; +class AttitudeObserver; +class Telescope; +} // namespace s2e::components -class components::OnBoardComputer; -class components::PowerControlUnit; -class components::GyroSensor; -class components::Magnetometer; -class components::StarSensor; -class components::SunSensor; -class components::GnssReceiver; -class components::Magnetorquer; -class components::ReactionWheel; -class components::SimpleThruster; -class components::ForceGenerator; -class components::TorqueGenerator; -class components::AngularVelocityObserver; -class components::AttitudeObserver; -class components::Telescope; +namespace s2e::sample { /** * @class SampleComponents @@ -62,10 +64,9 @@ class SampleComponents : public simulation::InstalledComponents { * @fn SampleComponents * @brief Constructor */ - SampleComponents(const dynamics::dynamics::Dynamics* dynamics, simulation::Structure* structure, - const environment::LocalEnvironment* local_environment, const environment::GlobalEnvironment* global_environment, - const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, - const unsigned int spacecraft_id); + SampleComponents(const dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, + const environment::GlobalEnvironment* global_environment, const simulation::SimulationConfiguration* configuration, + environment::ClockGenerator* clock_generator, const unsigned int spacecraft_id); /** * @fn ~SampleComponents * @brief Destructor @@ -140,7 +141,7 @@ class SampleComponents : public simulation::InstalledComponents { // States const simulation::SimulationConfiguration* configuration_; //!< Simulation settings - const dynamics::dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft + const dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft simulation::Structure* structure_; //!< Structure information of the spacecraft const environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft const environment::GlobalEnvironment* global_environment_; //!< Global environment information From b50e583508222a56d0bcd234413c8b45cdbe8465 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 1 Oct 2024 23:35:54 +0900 Subject: [PATCH 45/49] Fix bug --- .../base/sensor_template_functions.hpp | 2 +- src/components/ideal/attitude_observer.hpp | 4 +-- src/components/ideal/force_generator.hpp | 6 ++-- src/components/ideal/orbit_observer.cpp | 2 +- src/components/ideal/orbit_observer.hpp | 4 +-- src/components/ideal/torque_generator.hpp | 6 ++-- src/components/real/aocs/gnss_receiver.cpp | 10 +++--- src/components/real/aocs/gnss_receiver.hpp | 12 +++---- src/components/real/aocs/magnetorquer.hpp | 6 ++-- src/components/real/aocs/reaction_wheel.hpp | 8 ++--- src/components/real/aocs/star_sensor.cpp | 12 +++---- src/components/real/aocs/star_sensor.hpp | 6 ++-- src/components/real/aocs/sun_sensor.cpp | 7 ++-- src/components/real/aocs/sun_sensor.hpp | 8 ++--- src/disturbances/geopotential.cpp | 2 +- src/disturbances/lunar_gravity_field.cpp | 2 +- src/disturbances/lunar_gravity_field.hpp | 2 +- src/disturbances/magnetic_disturbance.cpp | 4 +-- .../attitude_with_cantilever_vibration.cpp | 2 +- .../attitude_with_cantilever_vibration.hpp | 2 +- src/environment/global/gnss_satellites.cpp | 34 +++++++++---------- src/environment/global/gnss_satellites.hpp | 25 +++++++------- src/environment/global/moon_rotation.cpp | 2 +- src/environment/local/atmosphere.cpp | 6 ++-- src/environment/local/geomagnetic_field.cpp | 4 +-- src/environment/local/geomagnetic_field.hpp | 2 +- .../atmosphere/harris_priester_model.cpp | 2 +- .../atmosphere/harris_priester_model.hpp | 2 +- src/math_physics/gnss/antex_file_reader.cpp | 4 +-- src/math_physics/gnss/antex_file_reader.hpp | 14 ++++---- src/math_physics/gnss/sp3_file_reader.cpp | 18 +++++----- src/math_physics/gnss/sp3_file_reader.hpp | 28 +++++++-------- .../randomization/global_randomization.hpp | 6 ++-- src/s2e.cpp | 6 +--- .../ground_station/ground_station.cpp | 2 +- .../initialize_monte_carlo_parameters.hpp | 2 +- 36 files changed, 128 insertions(+), 136 deletions(-) diff --git a/src/components/base/sensor_template_functions.hpp b/src/components/base/sensor_template_functions.hpp index 89bcd73ab..469a3e2b1 100644 --- a/src/components/base/sensor_template_functions.hpp +++ b/src/components/base/sensor_template_functions.hpp @@ -22,7 +22,7 @@ Sensor::Sensor(const math::Matrix& scale_factor, const math::Vector& range_to_zero_c_(range_to_zero_c), random_walk_noise_c_(random_walk_step_width_s, random_walk_standard_deviation_c, random_walk_limit_c) { for (size_t i = 0; i < N; i++) { - normal_random_noise_c_[i].SetParameters(0.0, normal_random_standard_deviation_c[i], s2e::randomization::global_randomization.MakeSeed()); + normal_random_noise_c_[i].SetParameters(0.0, normal_random_standard_deviation_c[i], randomization::global_randomization.MakeSeed()); } RangeCheck(); } diff --git a/src/components/ideal/attitude_observer.hpp b/src/components/ideal/attitude_observer.hpp index 0dfd29fe7..410913884 100644 --- a/src/components/ideal/attitude_observer.hpp +++ b/src/components/ideal/attitude_observer.hpp @@ -65,8 +65,8 @@ class AttitudeObserver : public Component, public logger::ILoggable { protected: math::Quaternion observed_quaternion_i2b_ = {0.0, 0.0, 0.0, 1.0}; //!< Observed quaternion - s2e::randomization::NormalRand angle_noise_; //!< Normal random for magnitude noise - s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise + randomization::NormalRand angle_noise_; //!< Normal random for magnitude noise + randomization::NormalRand direction_noise_; //!< Normal random for direction noise const dynamics::attitude::Attitude& attitude_; //!< dynamics::attitude::Attitude information }; diff --git a/src/components/ideal/force_generator.hpp b/src/components/ideal/force_generator.hpp index ac781993d..66fdcadea 100644 --- a/src/components/ideal/force_generator.hpp +++ b/src/components/ideal/force_generator.hpp @@ -102,9 +102,9 @@ class ForceGenerator : public Component, public logger::ILoggable { math::Vector<3> generated_force_rtn_N_{0.0}; //!< Generated force in the RTN frame [N] // Noise - s2e::randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise - s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise - double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad] + randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise + randomization::NormalRand direction_noise_; //!< Normal random for direction noise + double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad] /** * @fn GenerateDirectionNoiseQuaternion diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index ca3fa09bc..ce68afc76 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -14,7 +14,7 @@ OrbitObserver::OrbitObserver(const int prescaler, environment::ClockGenerator* c const math::Vector<6> error_standard_deviation, const dynamics::orbit::Orbit& orbit) : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { - normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], s2e::randomization::global_randomization.MakeSeed()); + normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], randomization::global_randomization.MakeSeed()); } } diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index a9216dde2..17997925b 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -83,8 +83,8 @@ class OrbitObserver : public Component, public logger::ILoggable { math::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] math::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] - NoiseFrame noise_frame_; //!< Noise definition frame - s2e::randomization::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] + NoiseFrame noise_frame_; //!< Noise definition frame + randomization::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] // Observed variables const dynamics::orbit::Orbit& orbit_; //!< Orbit information diff --git a/src/components/ideal/torque_generator.hpp b/src/components/ideal/torque_generator.hpp index 4ea760c30..726f7aed8 100644 --- a/src/components/ideal/torque_generator.hpp +++ b/src/components/ideal/torque_generator.hpp @@ -80,9 +80,9 @@ class TorqueGenerator : public Component, public logger::ILoggable { math::Vector<3> generated_torque_b_Nm_{0.0}; //!< Generated torque in the body fixed frame [Nm] // Noise - s2e::randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise - s2e::randomization::NormalRand direction_noise_; //!< Normal random for direction noise - double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad] + randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise + randomization::NormalRand direction_noise_; //!< Normal random for direction noise + double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad] /** * @fn GenerateDirectionNoiseQuaternion diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 330844dd9..8acf57890 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -28,10 +28,9 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) { for (size_t i = 0; i < 3; i++) { - position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], - s2e::randomization::global_randomization.MakeSeed()); + position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], randomization::global_randomization.MakeSeed()); velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], - s2e::randomization::global_randomization.MakeSeed()); + randomization::global_randomization.MakeSeed()); } } @@ -50,10 +49,9 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) { for (size_t i = 0; i < 3; i++) { - position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], - s2e::randomization::global_randomization.MakeSeed()); + position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], randomization::global_randomization.MakeSeed()); velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], - s2e::randomization::global_randomization.MakeSeed()); + randomization::global_randomization.MakeSeed()); } } diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 604b11927..7c1b586d0 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -110,7 +110,7 @@ class GnssReceiver : public Component, public logger::ILoggable { * @fn GetMeasuredGeodeticPosition * @brief Return Observed position in the LLH frame [m] */ - inline const s2e::geodesy::GeodeticPosition GetMeasuredGeodeticPosition(void) const { return geodetic_position_; } + inline const geodesy::GeodeticPosition GetMeasuredGeodeticPosition(void) const { return geodetic_position_; } /** * @fn GetMeasuredVelocity_ecef_m_s * @brief Return Observed velocity in the ECEF frame [m/s] @@ -140,11 +140,11 @@ class GnssReceiver : public Component, public logger::ILoggable { AntennaModel antenna_model_; //!< Antenna model // Simple position observation - s2e::randomization::NormalRand position_random_noise_ecef_m_[3]; //!< Random noise for position at the ECEF frame [m] - s2e::randomization::NormalRand velocity_random_noise_ecef_m_s_[3]; //!< Random noise for velocity at the ECEF frame [m] - math::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] - math::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] - s2e::geodesy::GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame + randomization::NormalRand position_random_noise_ecef_m_[3]; //!< Random noise for position at the ECEF frame [m] + randomization::NormalRand velocity_random_noise_ecef_m_s_[3]; //!< Random noise for velocity at the ECEF frame [m] + math::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] + math::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] + geodesy::GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame // Time observation environment::UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] diff --git a/src/components/real/aocs/magnetorquer.hpp b/src/components/real/aocs/magnetorquer.hpp index b3f0b6ff1..0ef0a5e4b 100644 --- a/src/components/real/aocs/magnetorquer.hpp +++ b/src/components/real/aocs/magnetorquer.hpp @@ -134,9 +134,9 @@ class Magnetorquer : public Component, public logger::ILoggable { math::Vector max_magnetic_moment_c_Am2_{100.0}; //!< Maximum magnetic moment in the component frame [Am2] math::Vector min_magnetic_moment_c_Am2_{-100.0}; //!< Minimum magnetic moment in the component frame [Am2] - math::Vector bias_noise_c_Am2_{0.0}; //!< Constant bias noise in the component frame [Am2] - randomization::RandomWalk random_walk_c_Am2_; //!< Random walk noise - s2e::randomization::NormalRand random_noise_c_Am2_[kMtqDimension]; //!< Normal random noise + math::Vector bias_noise_c_Am2_{0.0}; //!< Constant bias noise in the component frame [Am2] + randomization::RandomWalk random_walk_c_Am2_; //!< Random walk noise + randomization::NormalRand random_noise_c_Am2_[kMtqDimension]; //!< Normal random noise const environment::GeomagneticField* geomagnetic_field_; //!< Geomagnetic environment diff --git a/src/components/real/aocs/reaction_wheel.hpp b/src/components/real/aocs/reaction_wheel.hpp index e59f704b2..39965a692 100644 --- a/src/components/real/aocs/reaction_wheel.hpp +++ b/src/components/real/aocs/reaction_wheel.hpp @@ -182,10 +182,10 @@ class ReactionWheel : public Component, public logger::ILoggable { math::Vector<3> rotation_axis_b_; //!< Wheel rotation vector in the body fixed frame. // Parameters for control delay - const double step_width_s_; //!< step width for ReactionWheelOde [sec] - const double dead_time_s_; //!< dead time [sec] - std::vector acceleration_delay_buffer_; //!< Delay buffer for acceleration - s2e::control_utilities::FirstOrderLag delayed_acceleration_rad_s2_; //!< Delayed acceleration [rad/s2] + const double step_width_s_; //!< step width for ReactionWheelOde [sec] + const double dead_time_s_; //!< dead time [sec] + std::vector acceleration_delay_buffer_; //!< Delay buffer for acceleration + control_utilities::FirstOrderLag delayed_acceleration_rad_s2_; //!< Delayed acceleration [rad/s2] // Coasting friction // f_rad_s2 = v_rad_s * coefficients(0) + (v_rad_s)^2 * coefficients(1) + ... diff --git a/src/components/real/aocs/star_sensor.cpp b/src/components/real/aocs/star_sensor.cpp index 1e57e269b..1ce9315a3 100644 --- a/src/components/real/aocs/star_sensor.cpp +++ b/src/components/real/aocs/star_sensor.cpp @@ -27,9 +27,9 @@ StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_g : Component(prescaler, clock_generator), component_id_(component_id), quaternion_b2c_(quaternion_b2c), - rotation_noise_(s2e::randomization::global_randomization.MakeSeed()), - orthogonal_direction_noise_(0.0, standard_deviation_orthogonal_direction, s2e::randomization::global_randomization.MakeSeed()), - sight_direction_noise_(0.0, standard_deviation_sight_direction, s2e::randomization::global_randomization.MakeSeed()), + rotation_noise_(randomization::global_randomization.MakeSeed()), + orthogonal_direction_noise_(0.0, standard_deviation_orthogonal_direction, randomization::global_randomization.MakeSeed()), + sight_direction_noise_(0.0, standard_deviation_sight_direction, randomization::global_randomization.MakeSeed()), buffer_position_(0), step_time_s_(step_time_s), output_delay_(output_delay), @@ -52,9 +52,9 @@ StarSensor::StarSensor(const int prescaler, environment::ClockGenerator* clock_g : Component(prescaler, clock_generator, power_port), component_id_(component_id), quaternion_b2c_(quaternion_b2c), - rotation_noise_(s2e::randomization::global_randomization.MakeSeed()), - orthogonal_direction_noise_(0.0, standard_deviation_orthogonal_direction, s2e::randomization::global_randomization.MakeSeed()), - sight_direction_noise_(0.0, standard_deviation_sight_direction, s2e::randomization::global_randomization.MakeSeed()), + rotation_noise_(randomization::global_randomization.MakeSeed()), + orthogonal_direction_noise_(0.0, standard_deviation_orthogonal_direction, randomization::global_randomization.MakeSeed()), + sight_direction_noise_(0.0, standard_deviation_sight_direction, randomization::global_randomization.MakeSeed()), buffer_position_(0), step_time_s_(step_time_s), output_delay_(output_delay), diff --git a/src/components/real/aocs/star_sensor.hpp b/src/components/real/aocs/star_sensor.hpp index 99bc47819..0f3f7e321 100644 --- a/src/components/real/aocs/star_sensor.hpp +++ b/src/components/real/aocs/star_sensor.hpp @@ -117,9 +117,9 @@ class StarSensor : public Component, public logger::ILoggable { math::Vector<3> second_orthogonal_direction_c; //!< The second orthogonal direction of sight at component frame // Noise parameters - s2e::randomization::MinimalStandardLcgWithShuffle rotation_noise_; //!< Randomize object for orthogonal direction - s2e::randomization::NormalRand orthogonal_direction_noise_; //!< Random noise for orthogonal direction of sight - s2e::randomization::NormalRand sight_direction_noise_; //!< Random noise for sight direction + randomization::MinimalStandardLcgWithShuffle rotation_noise_; //!< Randomize object for orthogonal direction + randomization::NormalRand orthogonal_direction_noise_; //!< Random noise for orthogonal direction of sight + randomization::NormalRand sight_direction_noise_; //!< Random noise for sight direction // Delay emulation parameters int max_delay_; //!< Max delay diff --git a/src/components/real/aocs/sun_sensor.cpp b/src/components/real/aocs/sun_sensor.cpp index 0ef97c8db..3745d99b7 100644 --- a/src/components/real/aocs/sun_sensor.cpp +++ b/src/components/real/aocs/sun_sensor.cpp @@ -5,11 +5,10 @@ #include "sun_sensor.hpp" -#include -#include -using s2e::randomization::NormalRand; #include +#include #include +#include #include using namespace std; @@ -48,7 +47,7 @@ SunSensor::SunSensor(const int prescaler, environment::ClockGenerator* clock_gen void SunSensor::Initialize(const double random_noise_standard_deviation_rad, const double bias_noise_standard_deviation_rad) { // Bias - NormalRand nr(0.0, bias_noise_standard_deviation_rad, s2e::randomization::global_randomization.MakeSeed()); + randomization::NormalRand nr(0.0, bias_noise_standard_deviation_rad, randomization::global_randomization.MakeSeed()); bias_noise_alpha_rad_ += nr; bias_noise_beta_rad_ += nr; diff --git a/src/components/real/aocs/sun_sensor.hpp b/src/components/real/aocs/sun_sensor.hpp index e235bc618..33dd9c5fd 100644 --- a/src/components/real/aocs/sun_sensor.hpp +++ b/src/components/real/aocs/sun_sensor.hpp @@ -103,10 +103,10 @@ class SunSensor : public Component, public logger::ILoggable { double detectable_angle_rad_; //!< half angle (>0) [rad] bool sun_detected_flag_ = false; //!< Sun detected flag // Noise parameters - s2e::randomization::NormalRand random_noise_alpha_; //!< Normal random for alpha angle - s2e::randomization::NormalRand random_noise_beta_; //!< Normal random for beta angle - double bias_noise_alpha_rad_ = 0.0; //!< Constant bias for alpha angle (Value is calculated by random number generator) - double bias_noise_beta_rad_ = 0.0; //!< Constant bias for beta angle (Value is calculated by random number generator) + randomization::NormalRand random_noise_alpha_; //!< Normal random for alpha angle + randomization::NormalRand random_noise_beta_; //!< Normal random for beta angle + double bias_noise_alpha_rad_ = 0.0; //!< Constant bias for alpha angle (Value is calculated by random number generator) + double bias_noise_beta_rad_ = 0.0; //!< Constant bias for beta angle (Value is calculated by random number generator) // Measured variables const environment::SolarRadiationPressureEnvironment* srp_environment_; //!< Solar Radiation Pressure environment diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index 966a5497f..cb6ba222c 100644 --- a/src/disturbances/geopotential.cpp +++ b/src/disturbances/geopotential.cpp @@ -45,7 +45,7 @@ Geopotential::Geopotential(const int degree, const std::string file_path, const } } // Initialize GravityPotential - geopotential_ = s2e::gravity::GravityPotential(degree, c_, s_); + geopotential_ = gravity::GravityPotential(degree, c_, s_); } bool Geopotential::ReadCoefficientsEgm96(std::string file_name) { diff --git a/src/disturbances/lunar_gravity_field.cpp b/src/disturbances/lunar_gravity_field.cpp index dd94cc74c..182b4472d 100644 --- a/src/disturbances/lunar_gravity_field.cpp +++ b/src/disturbances/lunar_gravity_field.cpp @@ -48,7 +48,7 @@ LunarGravityField::LunarGravityField(const int degree, const std::string file_pa } } // Initialize GravityPotential - lunar_potential_ = s2e::gravity::GravityPotential(degree, c_, s_, gravity_constants_km3_s2_ * 1e9, reference_radius_km_ * 1e3); + lunar_potential_ = gravity::GravityPotential(degree, c_, s_, gravity_constants_km3_s2_ * 1e9, reference_radius_km_ * 1e3); } bool LunarGravityField::ReadCoefficientsGrgm1200a(std::string file_name) { diff --git a/src/disturbances/lunar_gravity_field.hpp b/src/disturbances/lunar_gravity_field.hpp index 2faf53420..04727d670 100644 --- a/src/disturbances/lunar_gravity_field.hpp +++ b/src/disturbances/lunar_gravity_field.hpp @@ -65,7 +65,7 @@ class LunarGravityField : public Disturbance { virtual std::string GetLogValue() const; private: - s2e::gravity::GravityPotential lunar_potential_; + gravity::GravityPotential lunar_potential_; double reference_radius_km_; double gravity_constants_km3_s2_; size_t degree_; //!< Maximum degree setting to calculate the geo-potential diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index f39f3539b..ad48b6bed 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -36,8 +36,8 @@ void MagneticDisturbance::CalcRMM() { static math::Vector<3> random_walk_std_dev(residual_magnetic_moment_.GetRandomWalkStandardDeviation_Am2()); static math::Vector<3> random_walk_limit(residual_magnetic_moment_.GetRandomWalkLimit_Am2()); static randomization::RandomWalk<3> random_walk(0.1, random_walk_std_dev, random_walk_limit); // [FIXME] step width is constant - static s2e::randomization::NormalRand normal_random(0.0, residual_magnetic_moment_.GetRandomNoiseStandardDeviation_Am2(), - s2e::randomization::global_randomization.MakeSeed()); + static randomization::NormalRand normal_random(0.0, residual_magnetic_moment_.GetRandomNoiseStandardDeviation_Am2(), + randomization::global_randomization.MakeSeed()); rmm_b_Am2_ = residual_magnetic_moment_.GetConstantValue_b_Am2(); for (int i = 0; i < 3; ++i) { diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp index dbfd569d4..bcda0d5c2 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.cpp @@ -17,7 +17,7 @@ AttitudeWithCantileverVibration::AttitudeWithCantileverVibration( const std::string& simulation_object_name) : Attitude(inertia_tensor_kgm2, simulation_object_name), numerical_integrator_(propagation_step_s, attitude_ode_, - s2e::numerical_integration::NumericalIntegrationMethod::kRk4) { //!< TODO: Set NumericalIntegrationMethod in *.ini file + numerical_integration::NumericalIntegrationMethod::kRk4) { //!< TODO: Set NumericalIntegrationMethod in *.ini file angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; quaternion_i2b_ = quaternion_i2b; torque_b_Nm_ = torque_b_Nm; diff --git a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp index 0484e7c54..f5f7747fa 100644 --- a/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp +++ b/src/dynamics/attitude/attitude_with_cantilever_vibration.hpp @@ -76,7 +76,7 @@ class AttitudeWithCantileverVibration : public Attitude { math::Vector<3> euler_angular_cantilever_rad_{0.0}; //!< Euler angle of the cantilever with respect to the body frame [rad/s] AttitudeWithCantileverVibrationOde attitude_ode_; - s2e::numerical_integration::NumericalIntegratorManager<13> numerical_integrator_; + numerical_integration::NumericalIntegratorManager<13> numerical_integrator_; }; } // namespace s2e::dynamics::attitude diff --git a/src/environment/global/gnss_satellites.cpp b/src/environment/global/gnss_satellites.cpp index 17219887c..25724b5e2 100644 --- a/src/environment/global/gnss_satellites.cpp +++ b/src/environment/global/gnss_satellites.cpp @@ -21,7 +21,7 @@ namespace s2e::environment { const size_t kNumberOfInterpolation = 9; -void GnssSatellites::Initialize(const std::vector& sp3_files, const s2e::time_system::EpochTime start_time) { +void GnssSatellites::Initialize(const std::vector& sp3_files, const time_system::EpochTime start_time) { sp3_files_ = sp3_files; current_epoch_time_ = start_time; @@ -39,10 +39,10 @@ void GnssSatellites::Initialize(const std::vector& sp3_files, con if (nearest_epoch_id >= half_interpolation_number) { reference_interpolation_id_ = nearest_epoch_id - half_interpolation_number; } - reference_time_ = s2e::time_system::EpochTime(initial_sp3_file.GetEpochData(reference_interpolation_id_)); + reference_time_ = time_system::EpochTime(initial_sp3_file.GetEpochData(reference_interpolation_id_)); // Initialize orbit - orbit_.assign(number_of_calculated_gnss_satellites_, s2e::orbit::InterpolationOrbit(kNumberOfInterpolation)); + orbit_.assign(number_of_calculated_gnss_satellites_, orbit::InterpolationOrbit(kNumberOfInterpolation)); // Initialize clock std::vector temp; @@ -62,9 +62,9 @@ void GnssSatellites::Update(const SimulationTime& simulation_time) { // Get time UTC current_utc = simulation_time.GetCurrentUtc(); - s2e::time_system::DateTime current_date_time((size_t)current_utc.year, (size_t)current_utc.month, (size_t)current_utc.day, (size_t)current_utc.hour, - (size_t)current_utc.minute, current_utc.second); - current_epoch_time_ = s2e::time_system::EpochTime(current_date_time); + time_system::DateTime current_date_time((size_t)current_utc.year, (size_t)current_utc.month, (size_t)current_utc.day, (size_t)current_utc.hour, + (size_t)current_utc.minute, current_utc.second); + current_epoch_time_ = time_system::EpochTime(current_date_time); // Check interpolation update double diff_s = current_epoch_time_.GetTimeWithFraction_s() - reference_time_.GetTimeWithFraction_s(); @@ -76,10 +76,10 @@ void GnssSatellites::Update(const SimulationTime& simulation_time) { return; } -math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time) const { +math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_id, const time_system::EpochTime time) const { if (gnss_satellite_id > number_of_calculated_gnss_satellites_) return math::Vector<3>(0.0); - s2e::time_system::EpochTime target_time; + time_system::EpochTime target_time; if (time.GetTime_s() == 0) { target_time = current_epoch_time_; @@ -94,10 +94,10 @@ math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_i return orbit_[gnss_satellite_id].CalcPositionWithTrigonometric(diff_s, math::tau / kOrbitalPeriodCorrection_s); } -double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time) const { +double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const time_system::EpochTime time) const { if (gnss_satellite_id > number_of_calculated_gnss_satellites_) return 0.0; - s2e::time_system::EpochTime target_time; + time_system::EpochTime target_time; if (time.GetTime_s() == 0) { target_time = current_epoch_time_; @@ -111,9 +111,9 @@ double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const s2e::tim return clock_[gnss_satellite_id].CalcPolynomial(diff_s) * 1e-6; } -bool GnssSatellites::GetCurrentSp3File(Sp3FileReader& current_sp3_file, const s2e::time_system::EpochTime current_time) { +bool GnssSatellites::GetCurrentSp3File(Sp3FileReader& current_sp3_file, const time_system::EpochTime current_time) { for (size_t i = 0; i < sp3_files_.size(); i++) { - s2e::time_system::EpochTime sp3_start_time(sp3_files_[i].GetStartEpochDateTime()); + time_system::EpochTime sp3_start_time(sp3_files_[i].GetStartEpochDateTime()); double diff_s = current_time.GetTimeWithFraction_s() - sp3_start_time.GetTimeWithFraction_s(); if (diff_s < 0.0) { // Error @@ -131,7 +131,7 @@ bool GnssSatellites::UpdateInterpolationInformation() { Sp3FileReader sp3_file = sp3_files_[sp3_file_id_]; for (size_t gnss_id = 0; gnss_id < number_of_calculated_gnss_satellites_; gnss_id++) { - s2e::time_system::EpochTime sp3_time = s2e::time_system::EpochTime(sp3_file.GetEpochData(reference_interpolation_id_)); + time_system::EpochTime sp3_time = time_system::EpochTime(sp3_file.GetEpochData(reference_interpolation_id_)); double time_diff_s = sp3_time.GetTimeWithFraction_s() - reference_time_.GetTimeWithFraction_s(); math::Vector<3> sp3_position_m = 1000.0 * sp3_file.GetSatellitePosition_km(reference_interpolation_id_, gnss_id); @@ -228,10 +228,10 @@ GnssSatellites* InitGnssSatellites(const std::string file_name, const EarthRotat } // - s2e::time_system::DateTime start_date_time((size_t)simulation_time.GetStartYear(), (size_t)simulation_time.GetStartMonth(), - (size_t)simulation_time.GetStartDay(), (size_t)simulation_time.GetStartHour(), - (size_t)simulation_time.GetStartMinute(), simulation_time.GetStartSecond()); - s2e::time_system::EpochTime start_epoch_time(start_date_time); + time_system::DateTime start_date_time((size_t)simulation_time.GetStartYear(), (size_t)simulation_time.GetStartMonth(), + (size_t)simulation_time.GetStartDay(), (size_t)simulation_time.GetStartHour(), + (size_t)simulation_time.GetStartMinute(), simulation_time.GetStartSecond()); + time_system::EpochTime start_epoch_time(start_date_time); gnss_satellites->Initialize(sp3_file_readers, start_epoch_time); return gnss_satellites; diff --git a/src/environment/global/gnss_satellites.hpp b/src/environment/global/gnss_satellites.hpp index c88f86a69..065dc5840 100644 --- a/src/environment/global/gnss_satellites.hpp +++ b/src/environment/global/gnss_satellites.hpp @@ -56,7 +56,7 @@ class GnssSatellites : public logger::ILoggable { * @param [in] sp3_files: List of SP3 files * @param [in] start_time: The simulation start time */ - void Initialize(const std::vector& sp3_files, const s2e::time_system::EpochTime start_time); + void Initialize(const std::vector& sp3_files, const time_system::EpochTime start_time); /** * @fn IsCalcEnabled @@ -89,8 +89,7 @@ class GnssSatellites : public logger::ILoggable { * @param [in] time: Target time to get the GNSS satellite. When the argument is not set, the last updated time is used for the calculation. * @return GNSS satellite position at ECEF frame at the time. Or return zero vector when the arguments are out of range. */ - math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, - const s2e::time_system::EpochTime time = s2e::time_system::EpochTime(0, 0.0)) const; + math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, const time_system::EpochTime time = time_system::EpochTime(0, 0.0)) const; /** * @fn GetGetClock_s @@ -99,7 +98,7 @@ class GnssSatellites : public logger::ILoggable { * @param [in] time: Target time to get the GNSS satellite. When the argument is not set, the last updated time is used for the calculation. * @return GNSS satellite clock offset at the time. Or return zero when the arguments are out of range. */ - double GetClock_s(const size_t gnss_satellite_id, const s2e::time_system::EpochTime time = s2e::time_system::EpochTime(0, 0.0)) const; + double GetClock_s(const size_t gnss_satellite_id, const time_system::EpochTime time = time_system::EpochTime(0, 0.0)) const; // Override logger::ILoggable /** @@ -116,15 +115,15 @@ class GnssSatellites : public logger::ILoggable { private: bool is_calc_enabled_ = false; //!< Flag to manage the GNSS satellite position calculation - std::vector sp3_files_; //!< List of SP3 files - size_t number_of_calculated_gnss_satellites_; //!< Number of calculated GNSS satellites - size_t sp3_file_id_; //!< Current SP3 file ID - s2e::time_system::EpochTime reference_time_; //!< Reference start time of the SP3 handling - size_t reference_interpolation_id_ = 0; //!< Reference epoch ID of the interpolation - s2e::time_system::EpochTime current_epoch_time_; //!< The last updated time + std::vector sp3_files_; //!< List of SP3 files + size_t number_of_calculated_gnss_satellites_; //!< Number of calculated GNSS satellites + size_t sp3_file_id_; //!< Current SP3 file ID + time_system::EpochTime reference_time_; //!< Reference start time of the SP3 handling + size_t reference_interpolation_id_ = 0; //!< Reference epoch ID of the interpolation + time_system::EpochTime current_epoch_time_; //!< The last updated time - std::vector orbit_; //!< GNSS satellite orbit with interpolation - std::vector clock_; //!< GNSS satellite clock offset with interpolation + std::vector orbit_; //!< GNSS satellite orbit with interpolation + std::vector clock_; //!< GNSS satellite clock offset with interpolation // References const EarthRotation& earth_rotation_; //!< Earth rotation @@ -136,7 +135,7 @@ class GnssSatellites : public logger::ILoggable { * @param [in] current_time: Target time * @return true means no error, false means the time argument is out of range */ - bool GetCurrentSp3File(s2e::gnss::Sp3FileReader& current_sp3_file, const s2e::time_system::EpochTime current_time); + bool GetCurrentSp3File(gnss::Sp3FileReader& current_sp3_file, const time_system::EpochTime current_time); /** * @fn UpdateInterpolationInformation diff --git a/src/environment/global/moon_rotation.cpp b/src/environment/global/moon_rotation.cpp index d2f93c659..c2b107b63 100644 --- a/src/environment/global/moon_rotation.cpp +++ b/src/environment/global/moon_rotation.cpp @@ -21,7 +21,7 @@ void MoonRotation::Update(const SimulationTime& simulation_time) { if (mode_ == MoonRotationMode::kSimple) { math::Vector<3> moon_position_eci_m = celestial_information_.GetPositionFromSelectedBody_i_m("MOON", "EARTH"); math::Vector<3> moon_velocity_eci_m_s = celestial_information_.GetVelocityFromSelectedBody_i_m_s("MOON", "EARTH"); - dcm_j2000_to_mcmf_ = s2e::planet_rotation::CalcDcmEciToPrincipalAxis(moon_position_eci_m, moon_velocity_eci_m_s); + dcm_j2000_to_mcmf_ = planet_rotation::CalcDcmEciToPrincipalAxis(moon_position_eci_m, moon_velocity_eci_m_s); } else if (mode_ == MoonRotationMode::kIauMoon) { ConstSpiceChar from[] = "J2000"; ConstSpiceChar to[] = "IAU_MOON"; diff --git a/src/environment/local/atmosphere.cpp b/src/environment/local/atmosphere.cpp index 0e31d6b17..74b232364 100644 --- a/src/environment/local/atmosphere.cpp +++ b/src/environment/local/atmosphere.cpp @@ -58,7 +58,7 @@ double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const dynamic if (model_ == "STANDARD") { // Standard model double altitude_m = orbit.GetGeodeticPosition().GetAltitude_m(); - air_density_kg_m3_ = s2e::atmosphere::CalcAirDensityWithSimpleModel(altitude_m); + air_density_kg_m3_ = atmosphere::CalcAirDensityWithSimpleModel(altitude_m); } else if (model_ == "NRLMSISE00") { // NRLMSISE00 model double lat_rad = orbit.GetGeodeticPosition().GetLatitude_rad(); @@ -69,7 +69,7 @@ double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const dynamic } else if (model_ == "HARRIS_PRIESTER") { // Harris-Priester math::Vector<3> sun_direction_eci = local_celestial_information_->GetGlobalInformation().GetPositionFromCenter_i_m("SUN").CalcNormalizedVector(); - air_density_kg_m3_ = s2e::atmosphere::CalcAirDensityWithHarrisPriester_kg_m3(orbit.GetGeodeticPosition(), sun_direction_eci); + air_density_kg_m3_ = atmosphere::CalcAirDensityWithHarrisPriester_kg_m3(orbit.GetGeodeticPosition(), sun_direction_eci); } else { // No suitable model return air_density_kg_m3_ = 0.0; @@ -80,7 +80,7 @@ double Atmosphere::CalcAirDensity_kg_m3(const double decimal_year, const dynamic double Atmosphere::AddNoise(const double rho_kg_m3) { // RandomWalk rw(rho_kg_m3*rw_stepwidth_,rho_kg_m3*rw_stddev_,rho_kg_m3*rw_limit_); - s2e::randomization::NormalRand nr(0.0, rho_kg_m3 * gauss_standard_deviation_rate_, s2e::randomization::global_randomization.MakeSeed()); + randomization::NormalRand nr(0.0, rho_kg_m3 * gauss_standard_deviation_rate_, randomization::global_randomization.MakeSeed()); double nrd = nr; return rho_kg_m3 + nrd; diff --git a/src/environment/local/geomagnetic_field.cpp b/src/environment/local/geomagnetic_field.cpp index ba49d575d..8fff876a9 100644 --- a/src/environment/local/geomagnetic_field.cpp +++ b/src/environment/local/geomagnetic_field.cpp @@ -24,7 +24,7 @@ GeomagneticField::GeomagneticField(const std::string igrf_file_name, const doubl set_file_path(igrf_file_name_.c_str()); } -void GeomagneticField::CalcMagneticField(const double decimal_year, const double sidereal_day, const s2e::geodesy::GeodeticPosition position, +void GeomagneticField::CalcMagneticField(const double decimal_year, const double sidereal_day, const geodesy::GeodeticPosition position, const math::Quaternion quaternion_i2b) { if (!IsCalcEnabled) return; @@ -46,7 +46,7 @@ void GeomagneticField::AddNoise(double* magnetic_field_array_i_nT) { static math::Vector<3> limit(random_walk_limit_nT_); static randomization::RandomWalk<3> random_walk(0.1, standard_deviation, limit); - static s2e::randomization::NormalRand white_noise(0.0, white_noise_standard_deviation_nT_, s2e::randomization::global_randomization.MakeSeed()); + static randomization::NormalRand white_noise(0.0, white_noise_standard_deviation_nT_, randomization::global_randomization.MakeSeed()); for (int i = 0; i < 3; ++i) { magnetic_field_array_i_nT[i] += random_walk[i] + white_noise; diff --git a/src/environment/local/geomagnetic_field.hpp b/src/environment/local/geomagnetic_field.hpp index 0317952f3..1f38c03c6 100644 --- a/src/environment/local/geomagnetic_field.hpp +++ b/src/environment/local/geomagnetic_field.hpp @@ -45,7 +45,7 @@ class GeomagneticField : public logger::ILoggable { * @param [in] position: Position of target point to calculate the magnetic field * @param [in] quaternion_i2b: Spacecraft attitude quaternion from the inertial frame to the body fixed frame */ - void CalcMagneticField(const double decimal_year, const double sidereal_day, const s2e::geodesy::GeodeticPosition position, + void CalcMagneticField(const double decimal_year, const double sidereal_day, const geodesy::GeodeticPosition position, const math::Quaternion quaternion_i2b); /** diff --git a/src/math_physics/atmosphere/harris_priester_model.cpp b/src/math_physics/atmosphere/harris_priester_model.cpp index 09d7a2143..6973caf98 100644 --- a/src/math_physics/atmosphere/harris_priester_model.cpp +++ b/src/math_physics/atmosphere/harris_priester_model.cpp @@ -28,7 +28,7 @@ double CalcScaleHeight_km(const std::map::const_iterator density */ double CalcApexDensity_g_km3(const std::map::const_iterator density_itr, const double altitude_km); -double CalcAirDensityWithHarrisPriester_kg_m3(const s2e::geodesy::GeodeticPosition geodetic_position, const math::Vector<3> sun_direction_eci, +double CalcAirDensityWithHarrisPriester_kg_m3(const geodesy::GeodeticPosition geodetic_position, const math::Vector<3> sun_direction_eci, const double f10_7, const double exponent_parameter) { // Altitude double altitude_km = geodetic_position.GetAltitude_m() / 1000.0; diff --git a/src/math_physics/atmosphere/harris_priester_model.hpp b/src/math_physics/atmosphere/harris_priester_model.hpp index 266682c88..8b64550ad 100644 --- a/src/math_physics/atmosphere/harris_priester_model.hpp +++ b/src/math_physics/atmosphere/harris_priester_model.hpp @@ -20,7 +20,7 @@ namespace s2e::atmosphere { * @param [in] exponent_parameter: n in the equation. n=2 for low inclination orbit and n=6 for polar orbit. * @return Atmospheric density [kg/m^3] */ -double CalcAirDensityWithHarrisPriester_kg_m3(const s2e::geodesy::GeodeticPosition geodetic_position, const math::Vector<3> sun_direction_eci, +double CalcAirDensityWithHarrisPriester_kg_m3(const geodesy::GeodeticPosition geodetic_position, const math::Vector<3> sun_direction_eci, const double f10_7 = 100.0, const double exponent_parameter = 4); } // namespace s2e::atmosphere diff --git a/src/math_physics/gnss/antex_file_reader.cpp b/src/math_physics/gnss/antex_file_reader.cpp index e3008d6b0..4a3c74f23 100644 --- a/src/math_physics/gnss/antex_file_reader.cpp +++ b/src/math_physics/gnss/antex_file_reader.cpp @@ -192,11 +192,11 @@ AntexPhaseCenterData AntexFileReader::ReadPhaseCenterData(std::ifstream& antex_f return phase_center_data; } -s2e::time_system::DateTime AntexFileReader::ReadDateTime(std::string line) { +time_system::DateTime AntexFileReader::ReadDateTime(std::string line) { size_t year, month, day, hour, minute; double second; sscanf(line.c_str(), "%zu %2zu %2zu %2zu %2zu %10lf", &year, &month, &day, &hour, &minute, &second); - return s2e::time_system::DateTime(year, month, day, hour, minute, second); + return time_system::DateTime(year, month, day, hour, minute, second); } } // namespace s2e::gnss diff --git a/src/math_physics/gnss/antex_file_reader.hpp b/src/math_physics/gnss/antex_file_reader.hpp index 67efe3133..b463c5f4d 100644 --- a/src/math_physics/gnss/antex_file_reader.hpp +++ b/src/math_physics/gnss/antex_file_reader.hpp @@ -203,12 +203,12 @@ class AntexSatelliteData { * @fn SetValidStartTime * @param[in] valid_start_time: Valid start time */ - inline void SetValidStartTime(const s2e::time_system::DateTime valid_start_time) { valid_start_time_ = valid_start_time; }; + inline void SetValidStartTime(const time_system::DateTime valid_start_time) { valid_start_time_ = valid_start_time; }; /** * @fn SetValidEndTime * @param[in] valid_end_time: Valid end time */ - inline void SetValidEndTime(const s2e::time_system::DateTime valid_end_time) { valid_end_time_ = valid_end_time; }; + inline void SetValidEndTime(const time_system::DateTime valid_end_time) { valid_end_time_ = valid_end_time; }; /** * @fn SetNumberOfFrequency * @param[in] number_of_frequency: Number of frequency @@ -235,12 +235,12 @@ class AntexSatelliteData { * @fn GetValidStartTime * @return Valid start time */ - inline s2e::time_system::DateTime GetValidStartTime() const { return valid_start_time_; }; + inline time_system::DateTime GetValidStartTime() const { return valid_start_time_; }; /** * @fn GetValidEndTime * @return Valid end time */ - inline s2e::time_system::DateTime GetValidEndTime() const { return valid_end_time_; }; + inline time_system::DateTime GetValidEndTime() const { return valid_end_time_; }; /** * @fn GetNumberOfFrequency * @return Number of frequency @@ -256,8 +256,8 @@ class AntexSatelliteData { private: std::string antenna_type_; //!< Antenna type std::string serial_number_; //!< Serial number or satellite code - s2e::time_system::DateTime valid_start_time_; //!< Valid start time - s2e::time_system::DateTime valid_end_time_; //!< Valid end time (The latest data does not have the end time) + time_system::DateTime valid_start_time_; //!< Valid start time + time_system::DateTime valid_end_time_; //!< Valid end time (The latest data does not have the end time) size_t number_of_frequency_ = 1; //!< Number of frequency std::vector phase_center_data_; //!< Phase center data for each frequency }; @@ -342,7 +342,7 @@ class AntexFileReader { * @param[in] line: A single line in ANTEX file * @return Read date time */ - s2e::time_system::DateTime ReadDateTime(std::string line); + time_system::DateTime ReadDateTime(std::string line); }; } // namespace s2e::gnss diff --git a/src/math_physics/gnss/sp3_file_reader.cpp b/src/math_physics/gnss/sp3_file_reader.cpp index aa857965a..2ae136626 100644 --- a/src/math_physics/gnss/sp3_file_reader.cpp +++ b/src/math_physics/gnss/sp3_file_reader.cpp @@ -15,9 +15,9 @@ namespace s2e::gnss { Sp3FileReader::Sp3FileReader(const std::string file_name) { ReadFile(file_name); } -s2e::time_system::DateTime Sp3FileReader::GetEpochData(const size_t epoch_id) const { +time_system::DateTime Sp3FileReader::GetEpochData(const size_t epoch_id) const { if (epoch_id > epoch_.size()) { - s2e::time_system::DateTime zero; + time_system::DateTime zero; return zero; } return epoch_[epoch_id]; @@ -69,7 +69,7 @@ bool Sp3FileReader::ReadFile(const std::string file_name) { size_t year, month, day, hour, minute; double second; sscanf(line.substr(3, 28).c_str(), "%zu %2zu %2zu %2zu %2zu %12lf", &year, &month, &day, &hour, &minute, &second); - epoch_.push_back(s2e::time_system::DateTime(year, month, day, hour, minute, second)); + epoch_.push_back(time_system::DateTime(year, month, day, hour, minute, second)); // Orbit and Clock information for (size_t satellite_id = 0; satellite_id < header_.number_of_satellites_; satellite_id++) { @@ -117,7 +117,7 @@ bool Sp3FileReader::ReadFile(const std::string file_name) { } // Test - s2e::time_system::DateTime test = epoch_[0]; + time_system::DateTime test = epoch_[0]; test = epoch_[1]; std::vector test_p = position_clock_[0]; test_p = position_clock_[1]; @@ -126,7 +126,7 @@ bool Sp3FileReader::ReadFile(const std::string file_name) { return true; } -size_t Sp3FileReader::SearchNearestEpochId(const s2e::time_system::EpochTime time) { +size_t Sp3FileReader::SearchNearestEpochId(const time_system::EpochTime time) { size_t nearest_epoch_id = 0; // Get header info @@ -134,10 +134,10 @@ size_t Sp3FileReader::SearchNearestEpochId(const s2e::time_system::EpochTime tim const double interval_s = header_.epoch_interval_s_; // Check range - s2e::time_system::EpochTime start_epoch(epoch_[0]); + time_system::EpochTime start_epoch(epoch_[0]); if (start_epoch > time) { nearest_epoch_id = 0; - } else if ((s2e::time_system::EpochTime)(epoch_[num_epoch - 1]) < time) { + } else if ((time_system::EpochTime)(epoch_[num_epoch - 1]) < time) { nearest_epoch_id = num_epoch - 1; } else { // Calc nearest point double diff_s = time.GetTimeWithFraction_s() - start_epoch.GetTimeWithFraction_s(); @@ -171,7 +171,7 @@ size_t Sp3FileReader::ReadHeader(std::ifstream& sp3_file) { size_t year, month, day, hour, minute; double second; sscanf(line.substr(3, 28).c_str(), "%zu %2zu %2zu %2zu %2zu %12lf", &year, &month, &day, &hour, &minute, &second); - header_.start_epoch_ = s2e::time_system::DateTime(year, month, day, hour, minute, second); + header_.start_epoch_ = time_system::DateTime(year, month, day, hour, minute, second); header_.number_of_epoch_ = std::stoi(line.substr(32, 7)); header_.used_data_ = line.substr(40, 5); header_.coordinate_system_ = line.substr(46, 5); @@ -199,7 +199,7 @@ size_t Sp3FileReader::ReadHeader(std::ifstream& sp3_file) { return 0; } // Read contents - header_.start_gps_time_ = s2e::time_system::GpsTime(std::stoi(line.substr(3, 4)), std::stod(line.substr(8, 15))); + header_.start_gps_time_ = time_system::GpsTime(std::stoi(line.substr(3, 4)), std::stod(line.substr(8, 15))); header_.epoch_interval_s_ = std::stod(line.substr(24, 14)); header_.start_time_mjday_ = std::stoi(line.substr(39, 5)); header_.start_time_mjday_fractional_day_ = std::stod(line.substr(45, 15)); diff --git a/src/math_physics/gnss/sp3_file_reader.hpp b/src/math_physics/gnss/sp3_file_reader.hpp index e551e186d..4e98933aa 100644 --- a/src/math_physics/gnss/sp3_file_reader.hpp +++ b/src/math_physics/gnss/sp3_file_reader.hpp @@ -51,16 +51,16 @@ enum class Sp3OrbitType { struct Sp3Header { // 1st line information // version -> not implemented yet - Sp3Mode mode_; //!< position or velocity - s2e::time_system::DateTime start_epoch_; //!< Time of start epoch - size_t number_of_epoch_ = 0; //!< Number of epoch in the SP3 file - std::string used_data_; //!< Used data to generate the SP3 file - std::string coordinate_system_; //!< Coordinate system for the position and velocity data - Sp3OrbitType orbit_type_; //!< Orbit type - std::string agency_name_; //!< Agency name who generates the SP3 file + Sp3Mode mode_; //!< position or velocity + time_system::DateTime start_epoch_; //!< Time of start epoch + size_t number_of_epoch_ = 0; //!< Number of epoch in the SP3 file + std::string used_data_; //!< Used data to generate the SP3 file + std::string coordinate_system_; //!< Coordinate system for the position and velocity data + Sp3OrbitType orbit_type_; //!< Orbit type + std::string agency_name_; //!< Agency name who generates the SP3 file // 2nd line information - s2e::time_system::GpsTime start_gps_time_; //!< Start time of orbit + time_system::GpsTime start_gps_time_; //!< Start time of orbit double epoch_interval_s_ = 1.0; //!< Epoch interval (0.0, 100000.0) size_t start_time_mjday_; //!< Start time of the orbit data (44244 = 6th Jan. 1980) [Modified Julian day] double start_time_mjday_fractional_day_ = 0.0; //!< Fractional part of the start time [0.0, 1.0) [day] @@ -172,19 +172,19 @@ class Sp3FileReader { inline Sp3Header GetHeader() const { return header_; } inline size_t GetNumberOfEpoch() const { return header_.number_of_epoch_; } inline size_t GetNumberOfSatellites() const { return header_.number_of_satellites_; } - inline s2e::time_system::DateTime GetStartEpochDateTime() const { return header_.start_epoch_; } - inline s2e::time_system::GpsTime GetStartEpochGpsTime() const { return header_.start_gps_time_; } + inline time_system::DateTime GetStartEpochDateTime() const { return header_.start_epoch_; } + inline time_system::GpsTime GetStartEpochGpsTime() const { return header_.start_gps_time_; } // Data - s2e::time_system::DateTime GetEpochData(const size_t epoch_id) const; + time_system::DateTime GetEpochData(const size_t epoch_id) const; Sp3PositionClock GetPositionClock(const size_t epoch_id, const size_t satellite_id); double GetSatelliteClockOffset(const size_t epoch_id, const size_t satellite_id); math::Vector<3> GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id); - size_t SearchNearestEpochId(const s2e::time_system::EpochTime time); + size_t SearchNearestEpochId(const time_system::EpochTime time); private: - Sp3Header header_; //!< SP3 header information - std::vector epoch_; //!< Epoch data list + Sp3Header header_; //!< SP3 header information + std::vector epoch_; //!< Epoch data list // Orbit and clock data (Use as position_clock_[satellite_id][epoch_id]) std::map> position_clock_; //!< Position and Clock data diff --git a/src/math_physics/randomization/global_randomization.hpp b/src/math_physics/randomization/global_randomization.hpp index df464b974..5220420cf 100644 --- a/src/math_physics/randomization/global_randomization.hpp +++ b/src/math_physics/randomization/global_randomization.hpp @@ -34,9 +34,9 @@ class GlobalRandomization { long MakeSeed(); private: - static const unsigned int kMaxSeed = 0xffffffff; //!< Maximum value of seed - s2e::randomization::MinimalStandardLcg base_randomizer_; //!< Base of global randomization - long seed_; //!< Seed of global randomization + static const unsigned int kMaxSeed = 0xffffffff; //!< Maximum value of seed + MinimalStandardLcg base_randomizer_; //!< Base of global randomization + long seed_; //!< Seed of global randomization }; extern GlobalRandomization global_randomization; //!< Global randomization diff --git a/src/s2e.cpp b/src/s2e.cpp index 3c9da8fa6..f0d19356e 100644 --- a/src/s2e.cpp +++ b/src/s2e.cpp @@ -23,8 +23,6 @@ // #include "interface/hils/COSMOSWrapper.h" // #include "interface/hils/HardwareMessage.h" -namespace s2e { - void print_path(std::string path) { #ifdef WIN32 std::cout << path << std::endl; @@ -71,7 +69,7 @@ int main(int argc, char *argv[]) std::cout << "\tIni file: "; print_path(ini_file); - auto simulation_case = sample::SampleCase(ini_file); + auto simulation_case = s2e::sample::SampleCase(ini_file); simulation_case.Initialize(); simulation_case.Main(); @@ -81,5 +79,3 @@ int main(int argc, char *argv[]) return EXIT_SUCCESS; } - -} // namespace s2e diff --git a/src/simulation/ground_station/ground_station.cpp b/src/simulation/ground_station/ground_station.cpp index f0d233936..f03e018eb 100644 --- a/src/simulation/ground_station/ground_station.cpp +++ b/src/simulation/ground_station/ground_station.cpp @@ -37,7 +37,7 @@ void GroundStation::Initialize(const SimulationConfiguration* configuration, con double latitude_deg = conf.ReadDouble(Section, "latitude_deg"); double longitude_deg = conf.ReadDouble(Section, "longitude_deg"); double height_m = conf.ReadDouble(Section, "height_m"); - geodetic_position_ = s2e::geodesy::GeodeticPosition(latitude_deg * math::deg_to_rad, longitude_deg * math::deg_to_rad, height_m); + geodetic_position_ = geodesy::GeodeticPosition(latitude_deg * math::deg_to_rad, longitude_deg * math::deg_to_rad, height_m); position_ecef_m_ = geodetic_position_.CalcEcefPosition(); elevation_limit_angle_deg_ = conf.ReadDouble(Section, "elevation_limit_angle_deg"); diff --git a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp index c44529766..e4bc3469f 100644 --- a/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp +++ b/src/simulation/monte_carlo_simulation/initialize_monte_carlo_parameters.hpp @@ -33,7 +33,7 @@ class InitializedMonteCarloParameters { kCircularNormalNormal, //!< r and θ follow normal distribution in Circular frame kSphericalNormalUniformUniform, //!< r follows normal distribution, and θ and φ follow uniform distribution in Spherical frame kSphericalNormalNormal, //!< r and θ follow normal distribution, and mean vector angle φ follows uniform distribution [0,2*pi] - kQuaternionUniform, //!< Perfectly Randomized s2e::math::Quaternion + kQuaternionUniform, //!< Perfectly Randomized Quaternion kQuaternionNormal, //!< Angle from the default quaternion θ follows normal distribution }; From 30b06647f7bfeb5b67db4bbb1d0c38ae494fd80d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 1 Oct 2024 23:51:43 +0900 Subject: [PATCH 46/49] Fix namespace in test --- .../gravity/test_gravity_potential.cpp | 48 +++---- src/math_physics/math/test_interpolation.cpp | 30 ++-- src/math_physics/math/test_matrix.cpp | 60 ++++---- src/math_physics/math/test_matrix_vector.cpp | 10 +- src/math_physics/math/test_quaternion.cpp | 134 +++++++++--------- src/math_physics/math/test_s2e_math.cpp | 14 +- src/math_physics/math/test_vector.cpp | 94 ++++++------ .../test_runge_kutta.cpp | 86 +++++------ .../orbit/test_interpolation_orbit.cpp | 4 +- 9 files changed, 239 insertions(+), 241 deletions(-) diff --git a/src/math_physics/gravity/test_gravity_potential.cpp b/src/math_physics/gravity/test_gravity_potential.cpp index 0ac2fa038..0dd54376e 100644 --- a/src/math_physics/gravity/test_gravity_potential.cpp +++ b/src/math_physics/gravity/test_gravity_potential.cpp @@ -6,8 +6,6 @@ #include "gravity_potential.hpp" -using namespace s2e::gravity; - /** * @brief Test for Acceleration calculation */ @@ -22,11 +20,11 @@ TEST(GravityPotential, Acceleration) { s_.assign(degree + 1, std::vector(degree + 1, 1.0)); // Initialize GravityPotential - GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); + s2e::gravity::GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); // Acceleration Calculation check - math::Vector<3> position_xcxf_m; - math::Vector<3> acceleration_xcxf_m_s2; + s2e::math::Vector<3> position_xcxf_m; + s2e::math::Vector<3> acceleration_xcxf_m_s2; const double accuracy = 1.0e-3; // Calc Acceleration @@ -64,11 +62,11 @@ TEST(GravityPotential, PartialDerivative1) { s_.assign(degree + 1, std::vector(degree + 1, 1.0)); // Initialize GravityPotential - GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); + s2e::gravity::GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); // Calculation check - math::Vector<3> position_xcxf_m; - math::Matrix<3, 3> partial_derivative_xcxf_s2; + s2e::math::Vector<3> position_xcxf_m; + s2e::math::Matrix<3, 3> partial_derivative_xcxf_s2; const double accuracy = 1.0e-3; // Calc Partial Derivative @@ -79,22 +77,22 @@ TEST(GravityPotential, PartialDerivative1) { // Calc Acceleration and numerical partial derivatives double d_r = 1e-9; - math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; + s2e::math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; for (size_t i = 0; i < 3; i++) { - math::Vector<3> position_1_xcxf_m = position_xcxf_m; - math::Vector<3> position_2_xcxf_m = position_xcxf_m; + s2e::math::Vector<3> position_1_xcxf_m = position_xcxf_m; + s2e::math::Vector<3> position_2_xcxf_m = position_xcxf_m; position_1_xcxf_m[i] = position_xcxf_m[i] - d_r / 2.0; position_2_xcxf_m[i] = position_xcxf_m[i] + d_r / 2.0; - math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); - math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); - math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; + s2e::math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); + s2e::math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); + s2e::math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; for (size_t j = 0; j < 3; j++) { numerical_partial_derivative_xcxf_s2[i][j] = diff_acceleration_xcxf_m_s2[j] / d_r; } } // Compare numerical and analytical calculation - math::Matrix<3, 3> diff; + s2e::math::Matrix<3, 3> diff; for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { EXPECT_NEAR(numerical_partial_derivative_xcxf_s2[i][j], partial_derivative_xcxf_s2[i][j], accuracy); @@ -117,11 +115,11 @@ TEST(GravityPotential, PartialDerivative2) { s_.assign(degree + 1, std::vector(degree + 1, 1.0)); // Initialize GravityPotential - GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); + s2e::gravity::GravityPotential gravity_potential_(degree, c_, s_, 1.0, 1.0); // Calculation check - math::Vector<3> position_xcxf_m; - math::Matrix<3, 3> partial_derivative_xcxf_s2; + s2e::math::Vector<3> position_xcxf_m; + s2e::math::Matrix<3, 3> partial_derivative_xcxf_s2; const double accuracy = 1.0e-3; // Calc Partial Derivative @@ -132,22 +130,22 @@ TEST(GravityPotential, PartialDerivative2) { // Calc Acceleration and numerical partial derivatives double d_r = 1e-9; - math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; + s2e::math::Matrix<3, 3> numerical_partial_derivative_xcxf_s2; for (size_t i = 0; i < 3; i++) { - math::Vector<3> position_1_xcxf_m = position_xcxf_m; - math::Vector<3> position_2_xcxf_m = position_xcxf_m; + s2e::math::Vector<3> position_1_xcxf_m = position_xcxf_m; + s2e::math::Vector<3> position_2_xcxf_m = position_xcxf_m; position_1_xcxf_m[i] = position_xcxf_m[i] - d_r / 2.0; position_2_xcxf_m[i] = position_xcxf_m[i] + d_r / 2.0; - math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); - math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); - math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; + s2e::math::Vector<3> acceleration_1_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_1_xcxf_m); + s2e::math::Vector<3> acceleration_2_xcxf_m_s2 = gravity_potential_.CalcAcceleration_xcxf_m_s2(position_2_xcxf_m); + s2e::math::Vector<3> diff_acceleration_xcxf_m_s2 = acceleration_2_xcxf_m_s2 - acceleration_1_xcxf_m_s2; for (size_t j = 0; j < 3; j++) { numerical_partial_derivative_xcxf_s2[i][j] = diff_acceleration_xcxf_m_s2[j] / d_r; } } // Compare numerical and analytical calculation - math::Matrix<3, 3> diff; + s2e::math::Matrix<3, 3> diff; for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { EXPECT_NEAR(numerical_partial_derivative_xcxf_s2[i][j], partial_derivative_xcxf_s2[i][j], accuracy); diff --git a/src/math_physics/math/test_interpolation.cpp b/src/math_physics/math/test_interpolation.cpp index 674b12419..a29f3b330 100644 --- a/src/math_physics/math/test_interpolation.cpp +++ b/src/math_physics/math/test_interpolation.cpp @@ -15,7 +15,7 @@ TEST(Interpolation, PolynomialLinearFunction) { std::vector x{0.0, 1.0, 2.0, 3.0, 4.0}; std::vector y{0.0, 2.0, 4.0, 6.0, 8.0}; - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); double xx = 0.4; EXPECT_DOUBLE_EQ(2.0 * xx, interpolation.CalcPolynomial(xx)); @@ -31,7 +31,7 @@ TEST(Interpolation, PolynomialLinearFunction) { TEST(Interpolation, PolynomialQuadraticFunction) { std::vector x{0.0, 1.0, 2.0, 3.0, 4.0}; std::vector y{0.0, 1.0, 4.0, 9.0, 16.0}; - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); double xx = 0.4; EXPECT_DOUBLE_EQ(pow(xx, 2.0), interpolation.CalcPolynomial(xx)); @@ -45,16 +45,16 @@ TEST(Interpolation, PolynomialQuadraticFunction) { * @brief Test for sin function with trigonometric interpolation */ TEST(Interpolation, TrigonometricSinFunction) { - std::vector x{0.0, math::pi_2, math::pi, math::pi * 3.0 / 2.0, math::tau}; + std::vector x{0.0, s2e::math::pi_2, s2e::math::pi, s2e::math::pi * 3.0 / 2.0, s2e::math::tau}; std::vector y; for (size_t i = 0; i < x.size(); i++) { y.push_back(sin(x[i])); } - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); - double xx = 0.4 * math::pi; + double xx = 0.4 * s2e::math::pi; EXPECT_DOUBLE_EQ(sin(xx), interpolation.CalcTrigonometric(xx)); - xx = 1.4 * math::pi; + xx = 1.4 * s2e::math::pi; EXPECT_DOUBLE_EQ(sin(xx), interpolation.CalcTrigonometric(xx)); } @@ -62,16 +62,16 @@ TEST(Interpolation, TrigonometricSinFunction) { * @brief Test for cos function with trigonometric interpolation */ TEST(Interpolation, TrigonometricCosFunction) { - std::vector x{0.0, 0.3 * math::pi_2, 0.6 * math::pi_2, 0.9 * math::pi_2, 1.2 * math::pi_2}; + std::vector x{0.0, 0.3 * s2e::math::pi_2, 0.6 * s2e::math::pi_2, 0.9 * s2e::math::pi_2, 1.2 * s2e::math::pi_2}; std::vector y; for (size_t i = 0; i < x.size(); i++) { y.push_back(cos(x[i])); } - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); - double xx = 0.1 * math::pi_2; + double xx = 0.1 * s2e::math::pi_2; EXPECT_NEAR(cos(xx), interpolation.CalcTrigonometric(xx), 1e-6); - xx = 0.8 * math::pi_2; + xx = 0.8 * s2e::math::pi_2; EXPECT_NEAR(cos(xx), interpolation.CalcTrigonometric(xx), 1e-6); } @@ -79,16 +79,16 @@ TEST(Interpolation, TrigonometricCosFunction) { * @brief Test for cos function with trigonometric interpolation */ TEST(Interpolation, TrigonometricCosSinFunctionOddDegree) { - std::vector x{0.0, 0.3 * math::pi_2, 0.6 * math::pi_2, 0.9 * math::pi_2, 1.2 * math::pi_2, 1.5 * math::pi_2}; + std::vector x{0.0, 0.3 * s2e::math::pi_2, 0.6 * s2e::math::pi_2, 0.9 * s2e::math::pi_2, 1.2 * s2e::math::pi_2, 1.5 * s2e::math::pi_2}; std::vector y; for (size_t i = 0; i < x.size(); i++) { y.push_back(cos(x[i]) + sin(x[i])); } - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); - double xx = 0.1 * math::pi_2; + double xx = 0.1 * s2e::math::pi_2; EXPECT_NEAR(cos(xx) + sin(xx), interpolation.CalcTrigonometric(xx), 1e-6); - xx = 0.8 * math::pi_2; + xx = 0.8 * s2e::math::pi_2; EXPECT_NEAR(cos(xx) + sin(xx), interpolation.CalcTrigonometric(xx), 1e-6); } @@ -98,7 +98,7 @@ TEST(Interpolation, TrigonometricCosSinFunctionOddDegree) { TEST(Interpolation, PushAndPop) { std::vector x{0.0, 1.0, 2.0, 3.0, 4.0}; std::vector y{0.0, 2.0, 4.0, 6.0, 8.0}; - math::Interpolation interpolation(x, y); + s2e::math::Interpolation interpolation(x, y); EXPECT_EQ(x.size(), interpolation.GetDegree()); for (size_t i = 0; i < x.size(); i++) { diff --git a/src/math_physics/math/test_matrix.cpp b/src/math_physics/math/test_matrix.cpp index 53dee0b75..9b23c15d5 100644 --- a/src/math_physics/math/test_matrix.cpp +++ b/src/math_physics/math/test_matrix.cpp @@ -14,7 +14,7 @@ TEST(Matrix, ConstructorWithNumber) { const size_t R = 6; const size_t C = 3; double initialize_value = 2.0; - math::Matrix m(initialize_value); + s2e::math::Matrix m(initialize_value); for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -30,7 +30,7 @@ TEST(Matrix, ConstructorWithNumber) { TEST(Matrix, GetLength) { const size_t R = 6; const size_t C = 3; - math::Matrix m; + s2e::math::Matrix m; EXPECT_EQ(R, m.GetRowLength()); EXPECT_EQ(C, m.GetColumnLength()); @@ -43,8 +43,8 @@ TEST(Matrix, OperatorPlusEqual) { const size_t R = 6; const size_t C = 3; double initialize_value = 2.0; - math::Matrix m(initialize_value); - math::Matrix adding; + s2e::math::Matrix m(initialize_value); + s2e::math::Matrix adding; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -71,8 +71,8 @@ TEST(Matrix, OperatorMinusEqual) { const size_t R = 6; const size_t C = 3; double initialize_value = 2.0; - math::Matrix m(initialize_value); - math::Matrix subtracting; + s2e::math::Matrix m(initialize_value); + s2e::math::Matrix subtracting; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -99,7 +99,7 @@ TEST(Matrix, OperatorMultiplyEqual) { const size_t R = 6; const size_t C = 3; - math::Matrix m; + s2e::math::Matrix m; double multiplying = 2.0; for (size_t r = 0; r < R; r++) { @@ -125,7 +125,7 @@ TEST(Matrix, OperatorDivideEqual) { const size_t R = 6; const size_t C = 3; - math::Matrix m; + s2e::math::Matrix m; double dividing = 2.0; for (size_t r = 0; r < R; r++) { @@ -152,7 +152,7 @@ TEST(Matrix, FillUp) { const size_t C = 3; double value = 3.0; - math::Matrix m; + s2e::math::Matrix m; m.FillUp(value); @@ -169,7 +169,7 @@ TEST(Matrix, FillUp) { */ TEST(Matrix, CalcTrace) { const size_t N = 6; - math::Matrix m; + s2e::math::Matrix m; for (size_t r = 0; r < N; r++) { for (size_t c = 0; c < N; c++) { @@ -201,8 +201,8 @@ TEST(Matrix, OperatorPlus) { const size_t R = 6; const size_t C = 3; double initialize_value = -2.0; - math::Matrix m(initialize_value); - math::Matrix adding; + s2e::math::Matrix m(initialize_value); + s2e::math::Matrix adding; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -210,7 +210,7 @@ TEST(Matrix, OperatorPlus) { } } - math::Matrix added = m + adding; + s2e::math::Matrix added = m + adding; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -230,8 +230,8 @@ TEST(Matrix, OperatorMinus) { const size_t R = 6; const size_t C = 3; double initialize_value = 0.6; - math::Matrix m(initialize_value); - math::Matrix subtracting; + s2e::math::Matrix m(initialize_value); + s2e::math::Matrix subtracting; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -239,7 +239,7 @@ TEST(Matrix, OperatorMinus) { } } - math::Matrix subtracted = m - subtracting; + s2e::math::Matrix subtracted = m - subtracting; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -259,7 +259,7 @@ TEST(Matrix, OperatorMultiplyScalar) { const size_t R = 6; const size_t C = 3; - math::Matrix m; + s2e::math::Matrix m; double multiplying = 0.3; for (size_t r = 0; r < R; r++) { @@ -268,7 +268,7 @@ TEST(Matrix, OperatorMultiplyScalar) { } } - math::Matrix subtracted = multiplying * m; + s2e::math::Matrix subtracted = multiplying * m; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -287,8 +287,8 @@ TEST(Matrix, OperatorMultiplyMatrix) { const size_t R = 2; const size_t C = 3; - math::Matrix a; - math::Matrix b; + s2e::math::Matrix a; + s2e::math::Matrix b; a[0][0] = 1.0; a[0][1] = 2.0; @@ -304,7 +304,7 @@ TEST(Matrix, OperatorMultiplyMatrix) { b[2][0] = 5.0; b[2][1] = 6.0; - math::Matrix result = a * b; + s2e::math::Matrix result = a * b; EXPECT_DOUBLE_EQ(22.0, result[0][0]); EXPECT_DOUBLE_EQ(28.0, result[0][1]); @@ -319,14 +319,14 @@ TEST(Matrix, Transpose) { const size_t R = 6; const size_t C = 3; - math::Matrix m; + s2e::math::Matrix m; for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { m[r][c] = r * c; } } - math::Matrix transposed = m.Transpose(); + s2e::math::Matrix transposed = m.Transpose(); for (size_t r = 0; r < R; r++) { for (size_t c = 0; c < C; c++) { @@ -344,7 +344,7 @@ TEST(Matrix, Transpose) { TEST(Matrix, MakeIdentityMatrix) { const size_t N = 6; - math::Matrix m = math::MakeIdentityMatrix(); + s2e::math::Matrix m = s2e::math::MakeIdentityMatrix(); for (size_t r = 0; r < N; r++) { for (size_t c = 0; c < N; c++) { @@ -362,9 +362,9 @@ TEST(Matrix, MakeIdentityMatrix) { */ TEST(Matrix, MakeRotationMatrixX) { const size_t N = 3; - double theta_rad = -45.0 * math::deg_to_rad; + double theta_rad = -45.0 * s2e::math::deg_to_rad; - math::Matrix m = math::MakeRotationMatrixX(theta_rad); + s2e::math::Matrix m = s2e::math::MakeRotationMatrixX(theta_rad); EXPECT_DOUBLE_EQ(1.0, m[0][0]); EXPECT_DOUBLE_EQ(0.0, m[0][1]); @@ -382,9 +382,9 @@ TEST(Matrix, MakeRotationMatrixX) { */ TEST(Matrix, MakeRotationMatrixY) { const size_t N = 3; - double theta_rad = 120.0 * math::deg_to_rad; + double theta_rad = 120.0 * s2e::math::deg_to_rad; - math::Matrix m = math::MakeRotationMatrixY(theta_rad); + s2e::math::Matrix m = s2e::math::MakeRotationMatrixY(theta_rad); EXPECT_DOUBLE_EQ(cos(theta_rad), m[0][0]); EXPECT_DOUBLE_EQ(0.0, m[0][1]); @@ -402,9 +402,9 @@ TEST(Matrix, MakeRotationMatrixY) { */ TEST(Matrix, MakeRotationMatrixZ) { const size_t N = 3; - double theta_rad = 30.0 * math::deg_to_rad; + double theta_rad = 30.0 * s2e::math::deg_to_rad; - math::Matrix m = math::MakeRotationMatrixZ(theta_rad); + s2e::math::Matrix m = s2e::math::MakeRotationMatrixZ(theta_rad); EXPECT_DOUBLE_EQ(cos(theta_rad), m[0][0]); EXPECT_DOUBLE_EQ(sin(theta_rad), m[0][1]); diff --git a/src/math_physics/math/test_matrix_vector.cpp b/src/math_physics/math/test_matrix_vector.cpp index 78fc9babf..958a409b6 100644 --- a/src/math_physics/math/test_matrix_vector.cpp +++ b/src/math_physics/math/test_matrix_vector.cpp @@ -14,8 +14,8 @@ TEST(MatrixVector, MultiplyMatrixVector) { const size_t R = 3; const size_t C = 2; - math::Matrix m; - math::Vector v; + s2e::math::Matrix m; + s2e::math::Vector v; m[0][0] = 1.0; m[0][1] = 2.0; @@ -27,7 +27,7 @@ TEST(MatrixVector, MultiplyMatrixVector) { v[0] = 7.0; v[1] = 1.0; - math::Vector result = m * v; + s2e::math::Vector result = m * v; EXPECT_DOUBLE_EQ(9.0, result[0]); EXPECT_DOUBLE_EQ(-7.0, result[1]); @@ -40,7 +40,7 @@ TEST(MatrixVector, MultiplyMatrixVector) { TEST(MatrixVector, CalcInverseMatrix) { const size_t N = 3; - math::Matrix m; + s2e::math::Matrix m; m[0][0] = 1.0; m[0][1] = 1.0; @@ -52,7 +52,7 @@ TEST(MatrixVector, CalcInverseMatrix) { m[2][1] = -2.0; m[2][2] = 1.0; - math::Matrix inverse = math::CalcInverseMatrix(m); + s2e::math::Matrix inverse = s2e::math::CalcInverseMatrix(m); EXPECT_NEAR(-1.0, inverse[0][0], 1e-10); EXPECT_NEAR(-1.0, inverse[0][1], 1e-10); diff --git a/src/math_physics/math/test_quaternion.cpp b/src/math_physics/math/test_quaternion.cpp index 4995afccc..2821eccee 100644 --- a/src/math_physics/math/test_quaternion.cpp +++ b/src/math_physics/math/test_quaternion.cpp @@ -11,7 +11,7 @@ * @brief Test for constructor from four numbers */ TEST(Quaternion, ConstructorFourNumber) { - math::Quaternion q(0.5, 0.5, 0.5, 0.5); + s2e::math::Quaternion q(0.5, 0.5, 0.5, 0.5); EXPECT_DOUBLE_EQ(0.5, q[0]); EXPECT_DOUBLE_EQ(0.5, q[1]); @@ -23,8 +23,8 @@ TEST(Quaternion, ConstructorFourNumber) { * @brief Test for constructor from Vector */ TEST(Quaternion, ConstructorVector) { - math::Vector<4> v(0.5); - math::Quaternion q(v); + s2e::math::Vector<4> v(0.5); + s2e::math::Quaternion q(v); EXPECT_DOUBLE_EQ(0.5, q[0]); EXPECT_DOUBLE_EQ(0.5, q[1]); @@ -36,12 +36,12 @@ TEST(Quaternion, ConstructorVector) { * @brief Test for constructor from axis and rotation angle X rotation */ TEST(Quaternion, ConstructorAxisAndAngleX) { - math::Vector<3> axis; + s2e::math::Vector<3> axis; axis[0] = 1.0; axis[1] = 0.0; axis[2] = 0.0; - double theta_rad = 90 * math::deg_to_rad; - math::Quaternion q(axis, theta_rad); + double theta_rad = 90 * s2e::math::deg_to_rad; + s2e::math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -53,12 +53,12 @@ TEST(Quaternion, ConstructorAxisAndAngleX) { * @brief Test for constructor from axis and rotation angle Y rotation */ TEST(Quaternion, ConstructorAxisAndAngleY) { - math::Vector<3> axis; + s2e::math::Vector<3> axis; axis[0] = 0.0; axis[1] = 1.0; axis[2] = 0.0; - double theta_rad = 45 * math::deg_to_rad; - math::Quaternion q(axis, theta_rad); + double theta_rad = 45 * s2e::math::deg_to_rad; + s2e::math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -70,12 +70,12 @@ TEST(Quaternion, ConstructorAxisAndAngleY) { * @brief Test for constructor from axis and rotation angle Z rotation */ TEST(Quaternion, ConstructorAxisAndAngleZ) { - math::Vector<3> axis; + s2e::math::Vector<3> axis; axis[0] = 0.0; axis[1] = 0.0; axis[2] = 1.0; - double theta_rad = -60 * math::deg_to_rad; - math::Quaternion q(axis, theta_rad); + double theta_rad = -60 * s2e::math::deg_to_rad; + s2e::math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -87,12 +87,12 @@ TEST(Quaternion, ConstructorAxisAndAngleZ) { * @brief Test for constructor from axis and rotation angle All axes rotation */ TEST(Quaternion, ConstructorAxisAndAngleAll) { - math::Vector<3> axis; + s2e::math::Vector<3> axis; axis[0] = 1.0; axis[1] = 1.0; axis[2] = 1.0; - double theta_rad = 180 * math::deg_to_rad; - math::Quaternion q(axis, theta_rad); + double theta_rad = 180 * s2e::math::deg_to_rad; + s2e::math::Quaternion q(axis, theta_rad); EXPECT_NEAR(axis[0] * sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(axis[1] * sin(theta_rad / 2.0), q[1], 1e-5); @@ -104,16 +104,16 @@ TEST(Quaternion, ConstructorAxisAndAngleAll) { * @brief Test for constructor from two vectors: No rotation */ TEST(Quaternion, ConstructorTwoVectorsNoRotation) { - math::Vector<3> before; + s2e::math::Vector<3> before; before[0] = 0.0; before[1] = 0.0; before[2] = 2.0; // To check normalization - math::Vector<3> after; + s2e::math::Vector<3> after; after[0] = 0.0; after[1] = 0.0; after[2] = 1.0; - math::Quaternion q(before, after); + s2e::math::Quaternion q(before, after); EXPECT_NEAR(0.0, q[0], 1e-5); EXPECT_NEAR(0.0, q[1], 1e-5); @@ -125,18 +125,18 @@ TEST(Quaternion, ConstructorTwoVectorsNoRotation) { * @brief Test for constructor from two vectors: X rotation */ TEST(Quaternion, ConstructorTwoVectorsX) { - math::Vector<3> before; + s2e::math::Vector<3> before; before[0] = 0.0; before[1] = 0.0; before[2] = 1.0; - math::Vector<3> after; + s2e::math::Vector<3> after; after[0] = 0.0; after[1] = 1.0; after[2] = 0.0; - math::Quaternion q(before, after); + s2e::math::Quaternion q(before, after); - double theta_rad = -90 * math::deg_to_rad; + double theta_rad = -90 * s2e::math::deg_to_rad; EXPECT_NEAR(sin(theta_rad / 2.0), q[0], 1e-5); EXPECT_NEAR(0.0, q[1], 1e-5); EXPECT_NEAR(0.0, q[2], 1e-5); @@ -147,18 +147,18 @@ TEST(Quaternion, ConstructorTwoVectorsX) { * @brief Test for constructor from two vectors: Y rotation */ TEST(Quaternion, ConstructorTwoVectorsY) { - math::Vector<3> before; + s2e::math::Vector<3> before; before[0] = 0.0; before[1] = 0.0; before[2] = 1.0; - math::Vector<3> after; + s2e::math::Vector<3> after; after[0] = 1.0; after[1] = 0.0; after[2] = 0.0; - math::Quaternion q(before, after); + s2e::math::Quaternion q(before, after); - double theta_rad = 90 * math::deg_to_rad; + double theta_rad = 90 * s2e::math::deg_to_rad; EXPECT_NEAR(0.0, q[0], 1e-5); EXPECT_NEAR(sin(theta_rad / 2.0), q[1], 1e-5); EXPECT_NEAR(0.0, q[2], 1e-5); @@ -169,18 +169,18 @@ TEST(Quaternion, ConstructorTwoVectorsY) { * @brief Test for constructor from two vectors: Z rotation */ TEST(Quaternion, ConstructorTwoVectorsZ) { - math::Vector<3> before; + s2e::math::Vector<3> before; before[0] = 1.0; before[1] = 0.0; before[2] = 0.0; - math::Vector<3> after; + s2e::math::Vector<3> after; after[0] = 0.0; after[1] = 1.0; after[2] = 0.0; - math::Quaternion q(before, after); + s2e::math::Quaternion q(before, after); - double theta_rad = 90 * math::deg_to_rad; + double theta_rad = 90 * s2e::math::deg_to_rad; EXPECT_NEAR(0.0, q[0], 1e-5); EXPECT_NEAR(0.0, q[1], 1e-5); EXPECT_NEAR(sin(theta_rad / 2.0), q[2], 1e-5); @@ -192,7 +192,7 @@ TEST(Quaternion, ConstructorTwoVectorsZ) { * @note TODO: Fix to nondestructive function */ TEST(Quaternion, Normalize) { - math::Quaternion q(1.0, 1.0, 1.0, 1.0); + s2e::math::Quaternion q(1.0, 1.0, 1.0, 1.0); EXPECT_DOUBLE_EQ(1.0, q[0]); EXPECT_DOUBLE_EQ(1.0, q[1]); EXPECT_DOUBLE_EQ(1.0, q[2]); @@ -209,9 +209,9 @@ TEST(Quaternion, Normalize) { * @brief Test for Conjugate */ TEST(Quaternion, Conjugate) { - math::Quaternion q(0.5, 0.5, 0.5, 0.5); + s2e::math::Quaternion q(0.5, 0.5, 0.5, 0.5); - math::Quaternion q_conjugate = q.Conjugate(); + s2e::math::Quaternion q_conjugate = q.Conjugate(); // Check nondestructive function EXPECT_DOUBLE_EQ(0.5, q[0]); @@ -229,10 +229,10 @@ TEST(Quaternion, Conjugate) { * @brief Test for ConvertToDcm Y rotation */ TEST(Quaternion, ConvertToDcmY) { - math::Quaternion q(0.0, 1.0, 0.0, 1.0); + s2e::math::Quaternion q(0.0, 1.0, 0.0, 1.0); q.Normalize(); - math::Matrix<3, 3> dcm = q.ConvertToDcm(); + s2e::math::Matrix<3, 3> dcm = q.ConvertToDcm(); // Check nondestructive function EXPECT_DOUBLE_EQ(0.0, q[0]); @@ -253,7 +253,7 @@ TEST(Quaternion, ConvertToDcmY) { EXPECT_NEAR(0.0, dcm[2][2], accuracy); // Inverse Conversion - math::Quaternion q_from_dcm = math::Quaternion::ConvertFromDcm(dcm); + s2e::math::Quaternion q_from_dcm = s2e::math::Quaternion::ConvertFromDcm(dcm); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_dcm[i], accuracy); } @@ -263,10 +263,10 @@ TEST(Quaternion, ConvertToDcmY) { * @brief Test for ConvertToDcm */ TEST(Quaternion, ConvertToDcm) { - math::Quaternion q(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); q.Normalize(); - math::Matrix<3, 3> dcm = q.ConvertToDcm(); + s2e::math::Matrix<3, 3> dcm = q.ConvertToDcm(); // Check nondestructive function const double accuracy = 1.0e-5; @@ -281,7 +281,7 @@ TEST(Quaternion, ConvertToDcm) { EXPECT_NEAR(0.4962963, dcm[2][2], accuracy); // Inverse Conversion - math::Quaternion q_from_dcm = math::Quaternion::ConvertFromDcm(dcm); + s2e::math::Quaternion q_from_dcm = s2e::math::Quaternion::ConvertFromDcm(dcm); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_dcm[i], accuracy); } @@ -291,10 +291,10 @@ TEST(Quaternion, ConvertToDcm) { * @brief Test for ConvertToEuler X rotation */ TEST(Quaternion, ConvertToEulerX) { - math::Quaternion q(1.0, 0.0, 0.0, 1.0); + s2e::math::Quaternion q(1.0, 0.0, 0.0, 1.0); q.Normalize(); - math::Vector<3> euler = q.ConvertToEuler(); + s2e::math::Vector<3> euler = q.ConvertToEuler(); // Check nondestructive function EXPECT_DOUBLE_EQ(1.0 / sqrt(2.0), q[0]); @@ -304,12 +304,12 @@ TEST(Quaternion, ConvertToEulerX) { // Check nondestructive function const double accuracy = 1.0e-7; - EXPECT_NEAR(90 * math::deg_to_rad, euler[0], accuracy); + EXPECT_NEAR(90 * s2e::math::deg_to_rad, euler[0], accuracy); EXPECT_NEAR(0.0, euler[1], accuracy); EXPECT_NEAR(0.0, euler[2], accuracy); // Inverse Conversion - math::Quaternion q_from_euler = math::Quaternion::ConvertFromEuler(euler); + s2e::math::Quaternion q_from_euler = s2e::math::Quaternion::ConvertFromEuler(euler); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_euler[i], accuracy); } @@ -319,10 +319,10 @@ TEST(Quaternion, ConvertToEulerX) { * @brief Test for ConvertToEuler */ TEST(Quaternion, ConvertToEuler) { - math::Quaternion q(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); q.Normalize(); - math::Vector<3> euler = q.ConvertToEuler(); + s2e::math::Vector<3> euler = q.ConvertToEuler(); // Check nondestructive function const double accuracy = 1.0e-7; @@ -331,7 +331,7 @@ TEST(Quaternion, ConvertToEuler) { EXPECT_NEAR(0.41012734, euler[2], accuracy); // Inverse Conversion - math::Quaternion q_from_euler = math::Quaternion::ConvertFromEuler(euler); + s2e::math::Quaternion q_from_euler = s2e::math::Quaternion::ConvertFromEuler(euler); for (size_t i = 0; i < 4; i++) { EXPECT_NEAR(q[i], q_from_euler[i], accuracy); } @@ -341,22 +341,22 @@ TEST(Quaternion, ConvertToEuler) { * @brief Test for FrameConversion Z rotation */ TEST(Quaternion, FrameConversionZ) { - math::Quaternion q(0.0, 0.0, 1.0, 1.0); + s2e::math::Quaternion q(0.0, 0.0, 1.0, 1.0); q.Normalize(); - math::Vector<3> v; + s2e::math::Vector<3> v; v[0] = 1.0; v[1] = 0.0; v[2] = 0.0; - math::Vector<3> v_frame_conv = q.FrameConversion(v); + s2e::math::Vector<3> v_frame_conv = q.FrameConversion(v); const double accuracy = 1.0e-7; EXPECT_NEAR(0.0, v_frame_conv[0], accuracy); EXPECT_NEAR(-1.0, v_frame_conv[1], accuracy); EXPECT_NEAR(0.0, v_frame_conv[2], accuracy); - math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); + s2e::math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); for (size_t i = 0; i < 3; i++) { EXPECT_NEAR(v[i], v_frame_conv_inv[i], accuracy); @@ -367,15 +367,15 @@ TEST(Quaternion, FrameConversionZ) { * @brief Test for FrameConversion */ TEST(Quaternion, FrameConversion) { - math::Quaternion q(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); q.Normalize(); - math::Vector<3> v; + s2e::math::Vector<3> v; v[0] = 1.0; v[1] = 0.0; v[2] = 0.0; - math::Vector<3> v_frame_conv = q.FrameConversion(v); - math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); + s2e::math::Vector<3> v_frame_conv = q.FrameConversion(v); + s2e::math::Vector<3> v_frame_conv_inv = q.InverseFrameConversion(v_frame_conv); const double accuracy = 1.0e-7; for (size_t i = 0; i < 3; i++) { @@ -387,9 +387,9 @@ TEST(Quaternion, FrameConversion) { * @brief Test for ConvertToVector */ TEST(Quaternion, ConvertToVector) { - math::Quaternion q(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q(0.5, 0.3, 0.1, 1.0); - math::Vector<4> v = q.ConvertToVector(); + s2e::math::Vector<4> v = q.ConvertToVector(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(q[i], v[i]); @@ -400,10 +400,10 @@ TEST(Quaternion, ConvertToVector) { * @brief Test for operator+ */ TEST(Quaternion, OperatorPlus) { - math::Quaternion q1(0.5, 0.3, 0.1, 1.0); - math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); + s2e::math::Quaternion q1(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); - math::Quaternion result = q1 + q2; + s2e::math::Quaternion result = q1 + q2; EXPECT_DOUBLE_EQ(0.2, result[0]); EXPECT_DOUBLE_EQ(0.4, result[1]); @@ -415,10 +415,10 @@ TEST(Quaternion, OperatorPlus) { * @brief Test for operator- */ TEST(Quaternion, OperatorMinus) { - math::Quaternion q1(0.5, 0.3, 0.1, 1.0); - math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); + s2e::math::Quaternion q1(0.5, 0.3, 0.1, 1.0); + s2e::math::Quaternion q2(-0.3, 0.1, -1.0, 0.4); - math::Quaternion result = q1 - q2; + s2e::math::Quaternion result = q1 - q2; EXPECT_DOUBLE_EQ(0.8, result[0]); EXPECT_DOUBLE_EQ(0.2, result[1]); @@ -430,10 +430,10 @@ TEST(Quaternion, OperatorMinus) { * @brief Test for operator* quaternion */ TEST(Quaternion, OperatorQuaternionMultiply) { - math::Quaternion q1(0.289271, -0.576012, -0.420972, 0.638212); - math::Quaternion q2(-0.0821846, 0.501761, 0.721995, -0.469259); + s2e::math::Quaternion q1(0.289271, -0.576012, -0.420972, 0.638212); + s2e::math::Quaternion q2(-0.0821846, 0.501761, 0.721995, -0.469259); - math::Quaternion result = q1 * q2; + s2e::math::Quaternion result = q1 * q2; const double accuracy = 1.0e-7; EXPECT_NEAR(-0.3928446703722, result[0], accuracy); @@ -446,10 +446,10 @@ TEST(Quaternion, OperatorQuaternionMultiply) { * @brief Test for operator* scalar */ TEST(Quaternion, OperatorScalarMultiply) { - math::Quaternion q(0.289271, -0.576012, -0.420972, 0.638212); + s2e::math::Quaternion q(0.289271, -0.576012, -0.420972, 0.638212); double scalar = 2.3; - math::Quaternion result = scalar * q; + s2e::math::Quaternion result = scalar * q; const double accuracy = 1.0e-7; EXPECT_NEAR(q[0] * 2.3, result[0], accuracy); diff --git a/src/math_physics/math/test_s2e_math.cpp b/src/math_physics/math/test_s2e_math.cpp index 949751a3b..7a293b967 100644 --- a/src/math_physics/math/test_s2e_math.cpp +++ b/src/math_physics/math/test_s2e_math.cpp @@ -14,18 +14,18 @@ TEST(S2eMath, WrapTo2Pi) { const double accuracy = 1.0e-7; double input_angle_rad = 0.0; - double wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); + double wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); EXPECT_NEAR(0.0, wrapped_angle_rad, accuracy); input_angle_rad = -1.0e-5; - wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); - EXPECT_NEAR(math::tau + input_angle_rad, wrapped_angle_rad, accuracy); + wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); + EXPECT_NEAR(s2e::math::tau + input_angle_rad, wrapped_angle_rad, accuracy); - input_angle_rad = math::tau + 1.0e-5; - wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); + input_angle_rad = s2e::math::tau + 1.0e-5; + wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); EXPECT_NEAR(1.0e-5, wrapped_angle_rad, accuracy); - input_angle_rad = 10 * math::tau + 1.0e-5; - wrapped_angle_rad = math::WrapTo2Pi(input_angle_rad); + input_angle_rad = 10 * s2e::math::tau + 1.0e-5; + wrapped_angle_rad = s2e::math::WrapTo2Pi(input_angle_rad); EXPECT_NEAR(1.0e-5, wrapped_angle_rad, accuracy); } diff --git a/src/math_physics/math/test_vector.cpp b/src/math_physics/math/test_vector.cpp index 5f636960d..e920dc0b8 100644 --- a/src/math_physics/math/test_vector.cpp +++ b/src/math_physics/math/test_vector.cpp @@ -13,7 +13,7 @@ TEST(Vector, ConstructorWithNumber) { const size_t N = 6; double initialize_value = 2.0; - math::Vector v(initialize_value); + s2e::math::Vector v(initialize_value); for (size_t i = 0; i < N; i++) { EXPECT_DOUBLE_EQ(initialize_value, v[i]); @@ -26,7 +26,7 @@ TEST(Vector, ConstructorWithNumber) { */ TEST(Vector, GetLength) { const size_t N = 6; - math::Vector v; + s2e::math::Vector v; EXPECT_EQ(N, v.GetLength()); } @@ -37,8 +37,8 @@ TEST(Vector, GetLength) { TEST(Vector, OperatorPlusEqual) { const size_t N = 6; double initialize_value = 2.0; - math::Vector v(initialize_value); - math::Vector adding; + s2e::math::Vector v(initialize_value); + s2e::math::Vector adding; for (size_t i = 0; i < N; i++) { adding[i] = double(i); @@ -60,8 +60,8 @@ TEST(Vector, OperatorPlusEqual) { TEST(Vector, OperatorMinusEqual) { const size_t N = 6; double initialize_value = 2.0; - math::Vector v(initialize_value); - math::Vector subtracting; + s2e::math::Vector v(initialize_value); + s2e::math::Vector subtracting; for (size_t i = 0; i < N; i++) { subtracting[i] = double(i); @@ -82,7 +82,7 @@ TEST(Vector, OperatorMinusEqual) { */ TEST(Vector, OperatorMultiplyEqual) { const size_t N = 6; - math::Vector v; + s2e::math::Vector v; double multiplying = 2.0; for (size_t i = 0; i < N; i++) { @@ -104,7 +104,7 @@ TEST(Vector, OperatorMultiplyEqual) { */ TEST(Vector, OperatorDivideEqual) { const size_t N = 6; - math::Vector v; + s2e::math::Vector v; double dividing = 3.0; for (size_t i = 0; i < N; i++) { @@ -126,13 +126,13 @@ TEST(Vector, OperatorDivideEqual) { */ TEST(Vector, OperatorNegative) { const size_t N = 6; - math::Vector v; + s2e::math::Vector v; for (size_t i = 0; i < N; i++) { v[i] = double(i); } - math::Vector v_negative = -v; + s2e::math::Vector v_negative = -v; for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -147,7 +147,7 @@ TEST(Vector, OperatorNegative) { */ TEST(Vector, FillUp) { const size_t N = 6; - math::Vector v; + s2e::math::Vector v; for (size_t i = 0; i < N; i++) { v[i] = double(i); @@ -171,14 +171,14 @@ TEST(Vector, FillUp) { TEST(Vector, OperatorPlus) { const size_t N = 6; double initialize_value = 2.0; - math::Vector v(initialize_value); - math::Vector adding; + s2e::math::Vector v(initialize_value); + s2e::math::Vector adding; for (size_t i = 0; i < N; i++) { adding[i] = double(i); } - math::Vector added = v + adding; + s2e::math::Vector added = v + adding; for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -195,14 +195,14 @@ TEST(Vector, OperatorPlus) { TEST(Vector, OperatorMinus) { const size_t N = 6; double initialize_value = 2.0; - math::Vector v(initialize_value); - math::Vector subtracting; + s2e::math::Vector v(initialize_value); + s2e::math::Vector subtracting; for (size_t i = 0; i < N; i++) { subtracting[i] = double(i); } - math::Vector subtracted = v - subtracting; + s2e::math::Vector subtracted = v - subtracting; for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -218,8 +218,8 @@ TEST(Vector, OperatorMinus) { */ TEST(Vector, InnerProduct) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; for (size_t i = 0; i < N; i++) { a[i] = double(i + 1); @@ -235,8 +235,8 @@ TEST(Vector, InnerProduct) { */ TEST(Vector, InnerProductZero) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -255,8 +255,8 @@ TEST(Vector, InnerProductZero) { */ TEST(Vector, OuterProductZero) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -266,7 +266,7 @@ TEST(Vector, OuterProductZero) { b[1] = 0.0; b[2] = 0.0; - math::Vector<3> result = OuterProduct(a, b); + s2e::math::Vector<3> result = OuterProduct(a, b); for (size_t i = 0; i < N; i++) { EXPECT_DOUBLE_EQ(0.0, result[i]); @@ -278,8 +278,8 @@ TEST(Vector, OuterProductZero) { */ TEST(Vector, OuterProductX) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 0.0; a[1] = 0.0; @@ -289,7 +289,7 @@ TEST(Vector, OuterProductX) { b[1] = 1.0; b[2] = 0.0; - math::Vector<3> result = OuterProduct(a, b); + s2e::math::Vector<3> result = OuterProduct(a, b); EXPECT_DOUBLE_EQ(-1.0, result[0]); EXPECT_DOUBLE_EQ(0.0, result[1]); @@ -301,8 +301,8 @@ TEST(Vector, OuterProductX) { */ TEST(Vector, OuterProductY) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 0.0; a[1] = 0.0; @@ -312,7 +312,7 @@ TEST(Vector, OuterProductY) { b[1] = 0.0; b[2] = 0.0; - math::Vector<3> result = OuterProduct(a, b); + s2e::math::Vector<3> result = OuterProduct(a, b); EXPECT_DOUBLE_EQ(0.0, result[0]); EXPECT_DOUBLE_EQ(1.0, result[1]); @@ -324,8 +324,8 @@ TEST(Vector, OuterProductY) { */ TEST(Vector, OuterProductZ) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -335,7 +335,7 @@ TEST(Vector, OuterProductZ) { b[1] = 1.0; b[2] = 0.0; - math::Vector<3> result = OuterProduct(a, b); + s2e::math::Vector<3> result = OuterProduct(a, b); EXPECT_DOUBLE_EQ(0.0, result[0]); EXPECT_DOUBLE_EQ(0.0, result[1]); @@ -347,7 +347,7 @@ TEST(Vector, OuterProductZ) { */ TEST(Vector, CalcNorm) { const size_t N = 10; - math::Vector v(1.0); + s2e::math::Vector v(1.0); double norm = v.CalcNorm(); @@ -364,9 +364,9 @@ TEST(Vector, CalcNorm) { */ TEST(Vector, Normalize) { const size_t N = 5; - math::Vector v(1.0); + s2e::math::Vector v(1.0); - math::Vector normalized = v.CalcNormalizedVector(); + s2e::math::Vector normalized = v.CalcNormalizedVector(); for (size_t i = 0; i < N; i++) { // Check nondestructive @@ -381,8 +381,8 @@ TEST(Vector, Normalize) { */ TEST(Vector, CalcAngleTwoVectors90deg) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 1.0; a[1] = 0.0; @@ -394,7 +394,7 @@ TEST(Vector, CalcAngleTwoVectors90deg) { double angle_rad = CalcAngleTwoVectors_rad(a, b); - EXPECT_DOUBLE_EQ(90.0 * math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(90.0 * s2e::math::deg_to_rad, angle_rad); } /** @@ -402,7 +402,7 @@ TEST(Vector, CalcAngleTwoVectors90deg) { */ TEST(Vector, CalcAngleTwoVectors0deg) { const size_t N = 3; - math::Vector a; + s2e::math::Vector a; a[0] = 1.0; a[1] = 0.0; @@ -410,7 +410,7 @@ TEST(Vector, CalcAngleTwoVectors0deg) { double angle_rad = CalcAngleTwoVectors_rad(a, a); - EXPECT_DOUBLE_EQ(0.0 * math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(0.0 * s2e::math::deg_to_rad, angle_rad); } /** @@ -418,8 +418,8 @@ TEST(Vector, CalcAngleTwoVectors0deg) { */ TEST(Vector, CalcAngleTwoVectors45deg) { const size_t N = 3; - math::Vector a; - math::Vector b; + s2e::math::Vector a; + s2e::math::Vector b; a[0] = 0.0; a[1] = 1.0; @@ -431,7 +431,7 @@ TEST(Vector, CalcAngleTwoVectors45deg) { double angle_rad = CalcAngleTwoVectors_rad(a, b); - EXPECT_DOUBLE_EQ(45.0 * math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(45.0 * s2e::math::deg_to_rad, angle_rad); } /** @@ -439,11 +439,11 @@ TEST(Vector, CalcAngleTwoVectors45deg) { */ TEST(Vector, GenerateOrthogonalUnitVector) { const size_t N = 3; - math::Vector a(1.0); + s2e::math::Vector a(1.0); - math::Vector b = GenerateOrthogonalUnitVector(a); + s2e::math::Vector b = GenerateOrthogonalUnitVector(a); double angle_rad = CalcAngleTwoVectors_rad(a, b); - EXPECT_DOUBLE_EQ(90.0 * math::deg_to_rad, angle_rad); + EXPECT_DOUBLE_EQ(90.0 * s2e::math::deg_to_rad, angle_rad); } diff --git a/src/math_physics/numerical_integration/test_runge_kutta.cpp b/src/math_physics/numerical_integration/test_runge_kutta.cpp index 60293ab18..d544caae3 100644 --- a/src/math_physics/numerical_integration/test_runge_kutta.cpp +++ b/src/math_physics/numerical_integration/test_runge_kutta.cpp @@ -18,7 +18,7 @@ TEST(NUMERICAL_INTEGRATION, Constructor) { s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::RungeKutta4<1> linear_ode(0.1, ode); - math::Vector<1> state = linear_ode.GetState(); + s2e::math::Vector<1> state = linear_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); } @@ -30,7 +30,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearRk4) { s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::RungeKutta4<1> rk4_ode(step_width_s, ode); - math::Vector<1> state = rk4_ode.GetState(); + s2e::math::Vector<1> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -51,7 +51,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearRkf) { s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); - math::Vector<1> state = rkf_ode.GetState(); + s2e::math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -72,7 +72,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearDp5) { s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); - math::Vector<1> state = dp5_ode.GetState(); + s2e::math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -93,7 +93,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRk4) { s2e::numerical_integration::ExampleLinearOde ode; s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode); - math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); + s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -115,7 +115,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerRkf) { s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, s2e::numerical_integration::NumericalIntegrationMethod::kRkf); - math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); + s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -141,7 +141,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateLinearNumericalIntegratorManagerDp5) { s2e::numerical_integration::NumericalIntegratorManager<1> numerical_integrator(step_width_s, ode, s2e::numerical_integration::NumericalIntegrationMethod::kDp5); - math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); + s2e::math::Vector<1> state = numerical_integrator.GetIntegrator()->GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -166,7 +166,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticRk4) { s2e::numerical_integration::ExampleQuadraticOde ode; s2e::numerical_integration::RungeKutta4<1> rk4_ode(step_width_s, ode); - math::Vector<1> state = rk4_ode.GetState(); + s2e::math::Vector<1> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -187,7 +187,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticRkf) { s2e::numerical_integration::ExampleQuadraticOde ode; s2e::numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); - math::Vector<1> state = rkf_ode.GetState(); + s2e::math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -208,7 +208,7 @@ TEST(NUMERICAL_INTEGRATION, InterpolationQuadraticRkf) { s2e::numerical_integration::ExampleQuadraticOde ode; s2e::numerical_integration::RungeKuttaFehlberg<1> rkf_ode(step_width_s, ode); - math::Vector<1> state = rkf_ode.GetState(); + s2e::math::Vector<1> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); rkf_ode.Integrate(); @@ -237,7 +237,7 @@ TEST(NUMERICAL_INTEGRATION, IntegrateQuadraticDp5) { s2e::numerical_integration::ExampleQuadraticOde ode; s2e::numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); - math::Vector<1> state = dp5_ode.GetState(); + s2e::math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); size_t step_num = 10000; @@ -258,7 +258,7 @@ TEST(NUMERICAL_INTEGRATION, InterpolationQuadraticDp5) { s2e::numerical_integration::ExampleQuadraticOde ode; s2e::numerical_integration::DormandPrince5<1> dp5_ode(step_width_s, ode); - math::Vector<1> state = dp5_ode.GetState(); + s2e::math::Vector<1> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(0.0, state[0]); dp5_ode.Integrate(); @@ -287,12 +287,12 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRk4) { s2e::numerical_integration::Example1dPositionVelocityOde ode; s2e::numerical_integration::RungeKutta4<2> rk4_ode(step_width_s, ode); - math::Vector<2> initial_state(0.0); + s2e::math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; initial_state[1] = 0.1; rk4_ode.SetState(0.0, initial_state); - math::Vector<2> state = rk4_ode.GetState(); + s2e::math::Vector<2> state = rk4_ode.GetState(); EXPECT_DOUBLE_EQ(initial_state[0], state[0]); EXPECT_DOUBLE_EQ(initial_state[1], state[1]); @@ -301,7 +301,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRk4) { rk4_ode.Integrate(); } state = rk4_ode.GetState(); - math::Vector<2> estimated_result(0.0); + s2e::math::Vector<2> estimated_result(0.0); estimated_result[0] = (step_width_s * step_num) * initial_state[1] + initial_state[0]; estimated_result[1] = initial_state[1]; @@ -317,12 +317,12 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRkf) { s2e::numerical_integration::Example1dPositionVelocityOde ode; s2e::numerical_integration::RungeKuttaFehlberg<2> rkf_ode(step_width_s, ode); - math::Vector<2> initial_state(0.0); + s2e::math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; initial_state[1] = 0.1; rkf_ode.SetState(0.0, initial_state); - math::Vector<2> state = rkf_ode.GetState(); + s2e::math::Vector<2> state = rkf_ode.GetState(); EXPECT_DOUBLE_EQ(initial_state[0], state[0]); EXPECT_DOUBLE_EQ(initial_state[1], state[1]); @@ -331,7 +331,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityRkf) { rkf_ode.Integrate(); } state = rkf_ode.GetState(); - math::Vector<2> estimated_result(0.0); + s2e::math::Vector<2> estimated_result(0.0); estimated_result[0] = (step_width_s * step_num) * initial_state[1] + initial_state[0]; estimated_result[1] = initial_state[1]; @@ -347,12 +347,12 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityDp5) { s2e::numerical_integration::Example1dPositionVelocityOde ode; s2e::numerical_integration::DormandPrince5<2> dp5_ode(step_width_s, ode); - math::Vector<2> initial_state(0.0); + s2e::math::Vector<2> initial_state(0.0); initial_state[0] = 0.0; initial_state[1] = 0.1; dp5_ode.SetState(0.0, initial_state); - math::Vector<2> state = dp5_ode.GetState(); + s2e::math::Vector<2> state = dp5_ode.GetState(); EXPECT_DOUBLE_EQ(initial_state[0], state[0]); EXPECT_DOUBLE_EQ(initial_state[1], state[1]); @@ -361,7 +361,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate1dPositionVelocityDp5) { dp5_ode.Integrate(); } state = dp5_ode.GetState(); - math::Vector<2> estimated_result(0.0); + s2e::math::Vector<2> estimated_result(0.0); estimated_result[0] = (step_width_s * step_num) * initial_state[1] + initial_state[0]; estimated_result[1] = initial_state[1]; @@ -379,7 +379,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - math::Vector<4> initial_state(0.0); + s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.1; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -389,9 +389,9 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - math::Vector<4> state_rk4 = rk4_ode.GetState(); - math::Vector<4> state_rkf = rkf_ode.GetState(); - math::Vector<4> state_dp5 = dp5_ode.GetState(); + s2e::math::Vector<4> state_rk4 = rk4_ode.GetState(); + s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); + s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rk4[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); @@ -409,8 +409,8 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitSmallEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - math::Vector<3> initial_position(0.0); - math::Vector<3> initial_velocity(0.0); + s2e::math::Vector<3> initial_position(0.0); + s2e::math::Vector<3> initial_velocity(0.0); initial_position[0] = initial_state[0]; initial_position[1] = initial_state[1]; @@ -449,7 +449,7 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - math::Vector<4> initial_state(0.0); + s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.9; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -459,9 +459,9 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - math::Vector<4> state_rk4 = rk4_ode.GetState(); - math::Vector<4> state_rkf = rkf_ode.GetState(); - math::Vector<4> state_dp5 = dp5_ode.GetState(); + s2e::math::Vector<4> state_rk4 = rk4_ode.GetState(); + s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); + s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rk4[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); @@ -479,8 +479,8 @@ TEST(NUMERICAL_INTEGRATION, Integrate2dTwoBodyOrbitLargeEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - math::Vector<3> initial_position(0.0); - math::Vector<3> initial_velocity(0.0); + s2e::math::Vector<3> initial_position(0.0); + s2e::math::Vector<3> initial_velocity(0.0); initial_position[0] = initial_state[0]; initial_position[1] = initial_state[1]; @@ -518,7 +518,7 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - math::Vector<4> initial_state(0.0); + s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.1; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -527,8 +527,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - math::Vector<4> state_rkf = rkf_ode.GetState(); - math::Vector<4> state_dp5 = dp5_ode.GetState(); + s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); + s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_dp5[i]); @@ -543,8 +543,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitSmallEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - math::Vector<3> initial_position(0.0); - math::Vector<3> initial_velocity(0.0); + s2e::math::Vector<3> initial_position(0.0); + s2e::math::Vector<3> initial_velocity(0.0); // Final value initial_position[0] = initial_state[0]; @@ -613,7 +613,7 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { s2e::numerical_integration::RungeKuttaFehlberg<4> rkf_ode(step_width_s, ode); s2e::numerical_integration::DormandPrince5<4> dp5_ode(step_width_s, ode); - math::Vector<4> initial_state(0.0); + s2e::math::Vector<4> initial_state(0.0); const double eccentricity = 0.8; initial_state[0] = 1.0 - eccentricity; initial_state[1] = 0.0; @@ -622,8 +622,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { rkf_ode.SetState(0.0, initial_state); dp5_ode.SetState(0.0, initial_state); - math::Vector<4> state_rkf = rkf_ode.GetState(); - math::Vector<4> state_dp5 = dp5_ode.GetState(); + s2e::math::Vector<4> state_rkf = rkf_ode.GetState(); + s2e::math::Vector<4> state_dp5 = dp5_ode.GetState(); for (size_t i = 0; i < 4; i++) { EXPECT_DOUBLE_EQ(initial_state[i], state_rkf[i]); EXPECT_DOUBLE_EQ(initial_state[i], state_dp5[i]); @@ -638,8 +638,8 @@ TEST(NUMERICAL_INTEGRATION, Interpolation2dTwoBodyOrbitLargeEccentricity) { state_dp5 = dp5_ode.GetState(); // Estimation by Kepler Orbit calculation - math::Vector<3> initial_position(0.0); - math::Vector<3> initial_velocity(0.0); + s2e::math::Vector<3> initial_position(0.0); + s2e::math::Vector<3> initial_velocity(0.0); // Final value initial_position[0] = initial_state[0]; diff --git a/src/math_physics/orbit/test_interpolation_orbit.cpp b/src/math_physics/orbit/test_interpolation_orbit.cpp index 1e05a13fb..f83a3f0cf 100644 --- a/src/math_physics/orbit/test_interpolation_orbit.cpp +++ b/src/math_physics/orbit/test_interpolation_orbit.cpp @@ -33,7 +33,7 @@ TEST(InterpolationOrbit, PushAndPop) { EXPECT_EQ(degree, interpolation_orbit.GetDegree()); for (size_t i = 0; i < degree; i++) { double time = (double)i; - math::Vector<3> position{i * 2.0}; + s2e::math::Vector<3> position{i * 2.0}; bool ret = interpolation_orbit.PushAndPopData(time, position); EXPECT_TRUE(ret); } @@ -46,7 +46,7 @@ TEST(InterpolationOrbit, PushAndPop) { // False test double time = 2.0; - math::Vector<3> position{-100.0}; + s2e::math::Vector<3> position{-100.0}; bool ret = interpolation_orbit.PushAndPopData(time, position); EXPECT_FALSE(ret); } From 2fb5f5270015d90f343d21db25f7dc8f792b2292 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 2 Oct 2024 00:01:38 +0900 Subject: [PATCH 47/49] Rename namespace ground_station --- .../real/communication/ground_station_calculator.cpp | 8 ++++---- .../real/communication/ground_station_calculator.hpp | 8 ++++---- src/simulation/ground_station/ground_station.cpp | 10 +++++----- src/simulation/ground_station/ground_station.hpp | 10 +++++----- .../ground_station/sample_ground_station.cpp | 6 +++--- .../ground_station/sample_ground_station.hpp | 2 +- 6 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/components/real/communication/ground_station_calculator.cpp b/src/components/real/communication/ground_station_calculator.cpp index 9c83ae730..864cbd10f 100644 --- a/src/components/real/communication/ground_station_calculator.cpp +++ b/src/components/real/communication/ground_station_calculator.cpp @@ -30,7 +30,7 @@ GroundStationCalculator::GroundStationCalculator(const double loss_polarization_ GroundStationCalculator::~GroundStationCalculator() {} void GroundStationCalculator::Update(const simulation::Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, - const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { + const ground_station::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { bool is_visible = ground_station.IsVisible(spacecraft.GetSpacecraftId()); if (is_visible) { max_bitrate_Mbps_ = CalcMaxBitrate(spacecraft.GetDynamics(), spacecraft_tx_antenna, ground_station, ground_station_rx_antenna); @@ -43,7 +43,7 @@ void GroundStationCalculator::Update(const simulation::Spacecraft& spacecraft, c // Private functions double GroundStationCalculator::CalcMaxBitrate(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, - const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { + const ground_station::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { double cn0_dBHz = CalcCn0OnGs(dynamics, spacecraft_tx_antenna, ground_station, ground_station_rx_antenna); double margin_for_bitrate_dB = cn0_dBHz - (ebn0_dB_ + hardware_deterioration_dB_ + coding_gain_dB_) - margin_requirement_dB_; @@ -56,14 +56,14 @@ double GroundStationCalculator::CalcMaxBitrate(const dynamics::Dynamics& dynamic } double GroundStationCalculator::CalcReceiveMarginOnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, - const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { + const ground_station::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { double cn0_dB = CalcCn0OnGs(dynamics, spacecraft_tx_antenna, ground_station, ground_station_rx_antenna); double cn0_requirement_dB = ebn0_dB_ + hardware_deterioration_dB_ + coding_gain_dB_ + 10.0 * log10(spacecraft_tx_antenna.GetBitrate_bps()); return cn0_dB - cn0_requirement_dB; } double GroundStationCalculator::CalcCn0OnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, - const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { + const ground_station::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { if (!spacecraft_tx_antenna.IsTransmitter() || !ground_station_rx_antenna.IsReceiver()) { // Check compatibility of transmitter and receiver return 0.0f; diff --git a/src/components/real/communication/ground_station_calculator.hpp b/src/components/real/communication/ground_station_calculator.hpp index bc5f8c0f4..2c91ffeb5 100644 --- a/src/components/real/communication/ground_station_calculator.hpp +++ b/src/components/real/communication/ground_station_calculator.hpp @@ -51,7 +51,7 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station: Ground station information * @param [in] ground_station_rx_antenna: Antenna mounted on ground station */ - void Update(const simulation::Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, const simulation::GroundStation& ground_station, + void Update(const simulation::Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, const ground_station::GroundStation& ground_station, const Antenna& ground_station_rx_antenna); // Override logger::ILoggable TODO: Maybe we don't need logabble, and this class should be used as library. @@ -111,7 +111,7 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station_rx_antenna: Rx Antenna mounted on ground station * @return Max bitrate [Mbps] */ - double CalcMaxBitrate(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const simulation::GroundStation& ground_station, + double CalcMaxBitrate(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const ground_station::GroundStation& ground_station, const Antenna& ground_station_rx_antenna); /** * @fn CalcReceiveMarginOnGs @@ -123,7 +123,7 @@ class GroundStationCalculator : public logger::ILoggable { * @return Receive margin [dB] */ double CalcReceiveMarginOnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, - const simulation::GroundStation& ground_station, const Antenna& ground_station_rx_antenna); + const ground_station::GroundStation& ground_station, const Antenna& ground_station_rx_antenna); /** * @fn CalcCn0 @@ -134,7 +134,7 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station_rx_antenna: Rx Antenna mounted on ground station * @return CN0 [dB] */ - double CalcCn0OnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const simulation::GroundStation& ground_station, + double CalcCn0OnGs(const dynamics::Dynamics& dynamics, const Antenna& spacecraft_tx_antenna, const ground_station::GroundStation& ground_station, const Antenna& ground_station_rx_antenna); }; diff --git a/src/simulation/ground_station/ground_station.cpp b/src/simulation/ground_station/ground_station.cpp index f03e018eb..d848115fd 100644 --- a/src/simulation/ground_station/ground_station.cpp +++ b/src/simulation/ground_station/ground_station.cpp @@ -13,9 +13,9 @@ #include #include -namespace s2e::simulation { +namespace s2e::ground_station { -GroundStation::GroundStation(const SimulationConfiguration* configuration, const unsigned int ground_station_id) +GroundStation::GroundStation(const simulation::SimulationConfiguration* configuration, const unsigned int ground_station_id) : ground_station_id_(ground_station_id) { Initialize(configuration, ground_station_id_); number_of_spacecraft_ = configuration->number_of_simulated_spacecraft_; @@ -26,7 +26,7 @@ GroundStation::GroundStation(const SimulationConfiguration* configuration, const GroundStation::~GroundStation() {} -void GroundStation::Initialize(const SimulationConfiguration* configuration, const unsigned int ground_station_id) { +void GroundStation::Initialize(const simulation::SimulationConfiguration* configuration, const unsigned int ground_station_id) { std::string gs_ini_path = configuration->ground_station_file_list_[0]; auto conf = setting_file_reader::IniAccess(gs_ini_path); @@ -47,7 +47,7 @@ void GroundStation::Initialize(const SimulationConfiguration* configuration, con void GroundStation::LogSetup(logger::Logger& logger) { logger.AddLogList(this); } -void GroundStation::Update(const environment::EarthRotation& celestial_rotation, const Spacecraft& spacecraft) { +void GroundStation::Update(const environment::EarthRotation& celestial_rotation, const simulation::Spacecraft& spacecraft) { math::Matrix<3, 3> dcm_ecef2eci = celestial_rotation.GetDcmJ2000ToEcef().Transpose(); position_i_m_ = dcm_ecef2eci * position_ecef_m_; @@ -93,4 +93,4 @@ std::string GroundStation::GetLogValue() const { return str_tmp; } -} // namespace s2e::simulation +} // namespace s2e::ground_station diff --git a/src/simulation/ground_station/ground_station.hpp b/src/simulation/ground_station/ground_station.hpp index c88f96666..b5179ddd3 100644 --- a/src/simulation/ground_station/ground_station.hpp +++ b/src/simulation/ground_station/ground_station.hpp @@ -12,7 +12,7 @@ #include "../simulation_configuration.hpp" -namespace s2e::simulation { +namespace s2e::ground_station { /** * @class GroundStation @@ -24,7 +24,7 @@ class GroundStation : public logger::ILoggable { * @fn GroundStation * @brief Constructor */ - GroundStation(const SimulationConfiguration* configuration, const unsigned int ground_station_id_); + GroundStation(const simulation::SimulationConfiguration* configuration, const unsigned int ground_station_id_); /** * @fn ~GroundStation * @brief Destructor @@ -35,7 +35,7 @@ class GroundStation : public logger::ILoggable { * @fn Initialize * @brief Virtual function to initialize the ground station */ - virtual void Initialize(const SimulationConfiguration* configuration, const unsigned int ground_station_id); + virtual void Initialize(const simulation::SimulationConfiguration* configuration, const unsigned int ground_station_id); /** * @fn LogSetup * @brief Virtual function to log output setting for ground station related components @@ -45,7 +45,7 @@ class GroundStation : public logger::ILoggable { * @fn Update * @brief Virtual function of main routine */ - virtual void Update(const environment::EarthRotation& celestial_rotation, const Spacecraft& spacecraft); + virtual void Update(const environment::EarthRotation& celestial_rotation, const simulation::Spacecraft& spacecraft); // Override functions for ILoggable /** @@ -111,6 +111,6 @@ class GroundStation : public logger::ILoggable { bool CalcIsVisible(const math::Vector<3> spacecraft_position_ecef_m); }; -} // namespace s2e::simulation +} // namespace s2e::ground_station #endif // S2E_SIMULATION_GROUND_STATION_GROUND_STATION_HPP_ diff --git a/src/simulation_sample/ground_station/sample_ground_station.cpp b/src/simulation_sample/ground_station/sample_ground_station.cpp index d75acb258..5ca22e707 100644 --- a/src/simulation_sample/ground_station/sample_ground_station.cpp +++ b/src/simulation_sample/ground_station/sample_ground_station.cpp @@ -10,19 +10,19 @@ namespace s2e::sample { SampleGroundStation::SampleGroundStation(const simulation::SimulationConfiguration* configuration, const unsigned int ground_station_id) - : simulation::GroundStation(configuration, ground_station_id) { + : ground_station::GroundStation(configuration, ground_station_id) { components_ = new SampleGsComponents(configuration); } SampleGroundStation::~SampleGroundStation() { delete components_; } void SampleGroundStation::LogSetup(logger::Logger& logger) { - simulation::GroundStation::LogSetup(logger); + ground_station::GroundStation::LogSetup(logger); components_->CompoLogSetUp(logger); } void SampleGroundStation::Update(const environment::EarthRotation& celestial_rotation, const SampleSpacecraft& spacecraft) { - simulation::GroundStation::Update(celestial_rotation, spacecraft); + ground_station::GroundStation::Update(celestial_rotation, spacecraft); components_->GetGsCalculator()->Update(spacecraft, spacecraft.GetInstalledComponents().GetAntenna(), *this, *(components_->GetAntenna())); } diff --git a/src/simulation_sample/ground_station/sample_ground_station.hpp b/src/simulation_sample/ground_station/sample_ground_station.hpp index bee3024f9..0d45f86f4 100644 --- a/src/simulation_sample/ground_station/sample_ground_station.hpp +++ b/src/simulation_sample/ground_station/sample_ground_station.hpp @@ -22,7 +22,7 @@ class SampleGsComponents; * @class SampleGroundStation * @brief An example of user defined ground station class */ -class SampleGroundStation : public simulation::GroundStation { +class SampleGroundStation : public ground_station::GroundStation { public: /** * @fn SampleGroundStation From 5993e4a9daa1f948479727d962ecd9c25f9db970 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 2 Oct 2024 00:16:18 +0900 Subject: [PATCH 48/49] Rename spacecraft namespace --- .../examples/example_change_structure.cpp | 2 +- .../examples/example_change_structure.hpp | 4 ++-- .../ground_station_calculator.cpp | 2 +- .../ground_station_calculator.hpp | 2 +- .../real/propulsion/simple_thruster.cpp | 8 ++++---- .../real/propulsion/simple_thruster.hpp | 10 +++++----- src/disturbances/air_drag.cpp | 4 ++-- src/disturbances/air_drag.hpp | 4 ++-- src/disturbances/disturbances.cpp | 4 ++-- src/disturbances/disturbances.hpp | 4 ++-- src/disturbances/magnetic_disturbance.cpp | 4 ++-- src/disturbances/magnetic_disturbance.hpp | 6 +++--- .../solar_radiation_pressure_disturbance.cpp | 4 ++-- .../solar_radiation_pressure_disturbance.hpp | 4 ++-- src/disturbances/surface_force.cpp | 2 +- src/disturbances/surface_force.hpp | 4 ++-- src/dynamics/dynamics.cpp | 4 ++-- src/dynamics/dynamics.hpp | 8 ++++---- .../ground_station/ground_station.cpp | 2 +- .../ground_station/ground_station.hpp | 2 +- .../spacecraft/installed_components.cpp | 4 ++-- .../spacecraft/installed_components.hpp | 4 ++-- src/simulation/spacecraft/spacecraft.cpp | 14 +++++++------- src/simulation/spacecraft/spacecraft.hpp | 18 +++++++++--------- .../structure/initialize_structure.cpp | 10 +++++----- .../structure/initialize_structure.hpp | 8 +++----- .../structure/kinematics_parameters.cpp | 4 ++-- .../structure/kinematics_parameters.hpp | 4 ++-- .../structure/residual_magnetic_moment.cpp | 4 ++-- .../structure/residual_magnetic_moment.hpp | 4 ++-- .../spacecraft/structure/structure.cpp | 8 ++++---- .../spacecraft/structure/structure.hpp | 14 +++++++------- .../spacecraft/structure/surface.cpp | 4 ++-- .../spacecraft/structure/surface.hpp | 4 ++-- .../spacecraft/sample_components.cpp | 2 +- .../spacecraft/sample_components.hpp | 6 +++--- .../spacecraft/sample_spacecraft.cpp | 2 +- .../spacecraft/sample_spacecraft.hpp | 2 +- 38 files changed, 99 insertions(+), 101 deletions(-) diff --git a/src/components/examples/example_change_structure.cpp b/src/components/examples/example_change_structure.cpp index 62e2a0cc6..e16ca7ac4 100644 --- a/src/components/examples/example_change_structure.cpp +++ b/src/components/examples/example_change_structure.cpp @@ -9,7 +9,7 @@ namespace s2e::components { -ExampleChangeStructure::ExampleChangeStructure(environment::ClockGenerator* clock_generator, simulation::Structure* structure) +ExampleChangeStructure::ExampleChangeStructure(environment::ClockGenerator* clock_generator, spacecraft::Structure* structure) : Component(1, clock_generator), structure_(structure) {} ExampleChangeStructure::~ExampleChangeStructure() {} diff --git a/src/components/examples/example_change_structure.hpp b/src/components/examples/example_change_structure.hpp index bdc4f74c5..7207ebc73 100644 --- a/src/components/examples/example_change_structure.hpp +++ b/src/components/examples/example_change_structure.hpp @@ -25,7 +25,7 @@ class ExampleChangeStructure : public Component, public logger::ILoggable { * @param [in] clock_generator: Clock generator * @param [in] structure: Structure information */ - ExampleChangeStructure(environment::ClockGenerator* clock_generator, simulation::Structure* structure); + ExampleChangeStructure(environment::ClockGenerator* clock_generator, spacecraft::Structure* structure); /** * @fn ~ChangeStructure * @brief Destructor @@ -52,7 +52,7 @@ class ExampleChangeStructure : public Component, public logger::ILoggable { virtual std::string GetLogValue() const override; protected: - simulation::Structure* structure_; //!< Structure information + spacecraft::Structure* structure_; //!< Structure information }; } // namespace s2e::components diff --git a/src/components/real/communication/ground_station_calculator.cpp b/src/components/real/communication/ground_station_calculator.cpp index 864cbd10f..bec0c2fd7 100644 --- a/src/components/real/communication/ground_station_calculator.cpp +++ b/src/components/real/communication/ground_station_calculator.cpp @@ -29,7 +29,7 @@ GroundStationCalculator::GroundStationCalculator(const double loss_polarization_ GroundStationCalculator::~GroundStationCalculator() {} -void GroundStationCalculator::Update(const simulation::Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, +void GroundStationCalculator::Update(const spacecraft::Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, const ground_station::GroundStation& ground_station, const Antenna& ground_station_rx_antenna) { bool is_visible = ground_station.IsVisible(spacecraft.GetSpacecraftId()); if (is_visible) { diff --git a/src/components/real/communication/ground_station_calculator.hpp b/src/components/real/communication/ground_station_calculator.hpp index 2c91ffeb5..f0192f2b1 100644 --- a/src/components/real/communication/ground_station_calculator.hpp +++ b/src/components/real/communication/ground_station_calculator.hpp @@ -51,7 +51,7 @@ class GroundStationCalculator : public logger::ILoggable { * @param [in] ground_station: Ground station information * @param [in] ground_station_rx_antenna: Antenna mounted on ground station */ - void Update(const simulation::Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, const ground_station::GroundStation& ground_station, + void Update(const spacecraft::Spacecraft& spacecraft, const Antenna& spacecraft_tx_antenna, const ground_station::GroundStation& ground_station, const Antenna& ground_station_rx_antenna); // Override logger::ILoggable TODO: Maybe we don't need logabble, and this class should be used as library. diff --git a/src/components/real/propulsion/simple_thruster.cpp b/src/components/real/propulsion/simple_thruster.cpp index 9bde027e3..e30d4b0ce 100644 --- a/src/components/real/propulsion/simple_thruster.cpp +++ b/src/components/real/propulsion/simple_thruster.cpp @@ -15,7 +15,7 @@ namespace s2e::components { SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, - const simulation::Structure* structure, const dynamics::Dynamics* dynamics) + const spacecraft::Structure* structure, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator), component_id_(component_id), thruster_position_b_m_(thruster_position_b_m), @@ -30,7 +30,7 @@ SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* SimpleThruster::SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, - const simulation::Structure* structure, const dynamics::Dynamics* dynamics) + const spacecraft::Structure* structure, const dynamics::Dynamics* dynamics) : Component(prescaler, clock_generator, power_port), component_id_(component_id), thruster_position_b_m_(thruster_position_b_m), @@ -123,7 +123,7 @@ math::Vector<3> SimpleThruster::CalcThrustDirection() { } SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, - const simulation::Structure* structure, const dynamics::Dynamics* dynamics) { + const spacecraft::Structure* structure, const dynamics::Dynamics* dynamics) { setting_file_reader::IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); auto* Section = section_str.c_str(); @@ -151,7 +151,7 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, } SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, - const simulation::Structure* structure, const dynamics::Dynamics* dynamics) { + const spacecraft::Structure* structure, const dynamics::Dynamics* dynamics) { setting_file_reader::IniAccess thruster_conf(file_name); std::string section_str = "THRUSTER_" + std::to_string(thruster_id); auto* Section = section_str.c_str(); diff --git a/src/components/real/propulsion/simple_thruster.hpp b/src/components/real/propulsion/simple_thruster.hpp index c1e30b2e4..c6d1901b7 100644 --- a/src/components/real/propulsion/simple_thruster.hpp +++ b/src/components/real/propulsion/simple_thruster.hpp @@ -39,7 +39,7 @@ class SimpleThruster : public Component, public logger::ILoggable { */ SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, const int component_id, const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, - const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const simulation::Structure* structure, + const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const spacecraft::Structure* structure, const dynamics::Dynamics* dynamics); /** * @fn SimpleThruster @@ -58,7 +58,7 @@ class SimpleThruster : public Component, public logger::ILoggable { */ SimpleThruster(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, const math::Vector<3> thruster_position_b_m, const math::Vector<3> thrust_direction_b, const double max_magnitude_N, - const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const simulation::Structure* structure, + const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad, const spacecraft::Structure* structure, const dynamics::Dynamics* dynamics); /** * @fn ~SimpleThruster @@ -154,7 +154,7 @@ class SimpleThruster : public Component, public logger::ILoggable { */ void Initialize(const double magnitude_standard_deviation_N, const double direction_standard_deviation_rad); - const simulation::Structure* structure_; //!< Spacecraft structure information + const spacecraft::Structure* structure_; //!< Spacecraft structure information const dynamics::Dynamics* dynamics_; //!< Spacecraft dynamics information }; @@ -168,7 +168,7 @@ class SimpleThruster : public Component, public logger::ILoggable { * @param [in] dynamics: Spacecraft dynamics information */ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, int thruster_id, const std::string file_name, - const simulation::Structure* structure, const dynamics::Dynamics* dynamics); + const spacecraft::Structure* structure, const dynamics::Dynamics* dynamics); /** * @fn InitSimpleThruster * @brief Initialize function os SimpleThruster @@ -180,7 +180,7 @@ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, * @param [in] dynamics: Spacecraft dynamics information */ SimpleThruster InitSimpleThruster(environment::ClockGenerator* clock_generator, PowerPort* power_port, int thruster_id, const std::string file_name, - const simulation::Structure* structure, const dynamics::Dynamics* dynamics); + const spacecraft::Structure* structure, const dynamics::Dynamics* dynamics); } // namespace s2e::components diff --git a/src/disturbances/air_drag.cpp b/src/disturbances/air_drag.cpp index 001422dbc..2c9254070 100644 --- a/src/disturbances/air_drag.cpp +++ b/src/disturbances/air_drag.cpp @@ -14,7 +14,7 @@ namespace s2e::disturbances { -AirDrag::AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, +AirDrag::AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, const double molecular_temperature_K, const double molecular_weight_g_mol, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled), wall_temperature_K_(wall_temperature_K), @@ -95,7 +95,7 @@ std::string AirDrag::GetLogValue() const { return str_tmp; } -AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, +AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m) { auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "AIR_DRAG"; diff --git a/src/disturbances/air_drag.hpp b/src/disturbances/air_drag.hpp index 73d5cbd9b..54e5d3896 100644 --- a/src/disturbances/air_drag.hpp +++ b/src/disturbances/air_drag.hpp @@ -32,7 +32,7 @@ class AirDrag : public SurfaceForce { * @param [in] molecular_weight_g_mol: Molecular weight [g/mol] * @param [in] is_calculation_enabled: Calculation flag */ - AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, + AirDrag(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const double wall_temperature_K, const double molecular_temperature_K, const double molecular_weight_g_mol, const bool is_calculation_enabled = true); /** @@ -98,7 +98,7 @@ class AirDrag : public SurfaceForce { * @param [in] surfaces: surface information of the spacecraft * @param [in] center_of_gravity_b_m: Center of gravity position vector at body frame [m] */ -AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, +AirDrag InitAirDrag(const std::string initialize_file_path, const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m); } // namespace s2e::disturbances diff --git a/src/disturbances/disturbances.cpp b/src/disturbances/disturbances.cpp index 86e7da768..c3e3fd018 100644 --- a/src/disturbances/disturbances.cpp +++ b/src/disturbances/disturbances.cpp @@ -18,7 +18,7 @@ namespace s2e::disturbances { Disturbances::Disturbances(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id, - const simulation::Structure* structure, const environment::GlobalEnvironment* global_environment) { + const spacecraft::Structure* structure, const environment::GlobalEnvironment* global_environment) { InitializeInstances(simulation_configuration, spacecraft_id, structure, global_environment); InitializeForceAndTorque(); InitializeAcceleration(); @@ -59,7 +59,7 @@ void Disturbances::LogSetup(logger::Logger& logger) { } void Disturbances::InitializeInstances(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id, - const simulation::Structure* structure, const environment::GlobalEnvironment* global_environment) { + const spacecraft::Structure* structure, const environment::GlobalEnvironment* global_environment) { setting_file_reader::IniAccess ini_access = setting_file_reader::IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); initialize_file_name_ = ini_access.ReadString("SETTING_FILES", "disturbance_file"); diff --git a/src/disturbances/disturbances.hpp b/src/disturbances/disturbances.hpp index 243f446b1..6e242e058 100644 --- a/src/disturbances/disturbances.hpp +++ b/src/disturbances/disturbances.hpp @@ -30,7 +30,7 @@ class Disturbances { * @param [in] structure: Structure information of spacecraft * @param [in] global_environment: Global environment information */ - Disturbances(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id, const simulation::Structure* structure, + Disturbances(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id, const spacecraft::Structure* structure, const environment::GlobalEnvironment* global_environment); /** * @fn ~Disturbances @@ -89,7 +89,7 @@ class Disturbances { * @param [in] global_environment: Global environment information */ void InitializeInstances(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id, - const simulation::Structure* structure, const environment::GlobalEnvironment* global_environment); + const spacecraft::Structure* structure, const environment::GlobalEnvironment* global_environment); /** * @fn InitializeForceAndTorque * @brief Initialize disturbance force and torque diff --git a/src/disturbances/magnetic_disturbance.cpp b/src/disturbances/magnetic_disturbance.cpp index ad48b6bed..ea0029218 100644 --- a/src/disturbances/magnetic_disturbance.cpp +++ b/src/disturbances/magnetic_disturbance.cpp @@ -15,7 +15,7 @@ namespace s2e::disturbances { -MagneticDisturbance::MagneticDisturbance(const simulation::ResidualMagneticMoment& rmm_params, const bool is_calculation_enabled) +MagneticDisturbance::MagneticDisturbance(const spacecraft::ResidualMagneticMoment& rmm_params, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, true), residual_magnetic_moment_(rmm_params) { rmm_b_Am2_ = residual_magnetic_moment_.GetConstantValue_b_Am2(); } @@ -64,7 +64,7 @@ std::string MagneticDisturbance::GetLogValue() const { return str_tmp; } -MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const simulation::ResidualMagneticMoment& rmm_params) { +MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const spacecraft::ResidualMagneticMoment& rmm_params) { auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "MAGNETIC_DISTURBANCE"; diff --git a/src/disturbances/magnetic_disturbance.hpp b/src/disturbances/magnetic_disturbance.hpp index 166060674..f51b3924b 100644 --- a/src/disturbances/magnetic_disturbance.hpp +++ b/src/disturbances/magnetic_disturbance.hpp @@ -27,7 +27,7 @@ class MagneticDisturbance : public Disturbance { * @param [in] rmm_parameters: RMM parameters of the spacecraft * @param [in] is_calculation_enabled: Calculation flag */ - MagneticDisturbance(const simulation::ResidualMagneticMoment& rmm_parameters, const bool is_calculation_enabled = true); + MagneticDisturbance(const spacecraft::ResidualMagneticMoment& rmm_parameters, const bool is_calculation_enabled = true); /** * @fn Update @@ -53,7 +53,7 @@ class MagneticDisturbance : public Disturbance { const double kMagUnit_ = 1.0e-9; //!< Constant value to change the unit [nT] -> [T] math::Vector<3> rmm_b_Am2_; //!< True RMM of the spacecraft in the body frame [Am2] - const simulation::ResidualMagneticMoment& residual_magnetic_moment_; //!< RMM parameters + const spacecraft::ResidualMagneticMoment& residual_magnetic_moment_; //!< RMM parameters /** * @fn CalcRMM @@ -75,7 +75,7 @@ class MagneticDisturbance : public Disturbance { * @param [in] initialize_file_path: Initialize file path * @param [in] rmm_params: RMM parameters */ -MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const simulation::ResidualMagneticMoment& rmm_params); +MagneticDisturbance InitMagneticDisturbance(const std::string initialize_file_path, const spacecraft::ResidualMagneticMoment& rmm_params); } // namespace s2e::disturbances diff --git a/src/disturbances/solar_radiation_pressure_disturbance.cpp b/src/disturbances/solar_radiation_pressure_disturbance.cpp index c5f6b7af1..369048683 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.cpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.cpp @@ -12,7 +12,7 @@ namespace s2e::disturbances { -SolarRadiationPressureDisturbance::SolarRadiationPressureDisturbance(const std::vector& surfaces, +SolarRadiationPressureDisturbance::SolarRadiationPressureDisturbance(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) : SurfaceForce(surfaces, center_of_gravity_b_m, is_calculation_enabled) {} @@ -55,7 +55,7 @@ std::string SolarRadiationPressureDisturbance::GetLogValue() const { } SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const std::string initialize_file_path, - const std::vector& surfaces, + const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m) { auto conf = setting_file_reader::IniAccess(initialize_file_path); const char* section = "SOLAR_RADIATION_PRESSURE_DISTURBANCE"; diff --git a/src/disturbances/solar_radiation_pressure_disturbance.hpp b/src/disturbances/solar_radiation_pressure_disturbance.hpp index 73daceccd..8f2d22f82 100644 --- a/src/disturbances/solar_radiation_pressure_disturbance.hpp +++ b/src/disturbances/solar_radiation_pressure_disturbance.hpp @@ -27,7 +27,7 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { * @param [in] center_of_gravity_b_m: Center of gravity position at the body frame [m] * @param [in] is_calculation_enabled: Calculation flag */ - SolarRadiationPressureDisturbance(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, + SolarRadiationPressureDisturbance(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled = true); /** @@ -68,7 +68,7 @@ class SolarRadiationPressureDisturbance : public SurfaceForce { * @param [in] center_of_gravity_b_m: Center of gravity position vector at body frame [m] */ SolarRadiationPressureDisturbance InitSolarRadiationPressureDisturbance(const std::string initialize_file_path, - const std::vector& surfaces, + const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m); } // namespace s2e::disturbances diff --git a/src/disturbances/surface_force.cpp b/src/disturbances/surface_force.cpp index 4825640b7..391a78274 100644 --- a/src/disturbances/surface_force.cpp +++ b/src/disturbances/surface_force.cpp @@ -9,7 +9,7 @@ namespace s2e::disturbances { -SurfaceForce::SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, +SurfaceForce::SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled) : Disturbance(is_calculation_enabled, true), surfaces_(surfaces), center_of_gravity_b_m_(center_of_gravity_b_m) { // Initialize vectors diff --git a/src/disturbances/surface_force.hpp b/src/disturbances/surface_force.hpp index 53d145557..36af265a5 100644 --- a/src/disturbances/surface_force.hpp +++ b/src/disturbances/surface_force.hpp @@ -30,7 +30,7 @@ class SurfaceForce : public Disturbance { * @param [in] center_of_gravity_b_m: Center of gravity position at the body frame [m] * @param [in] is_calculation_enabled: Calculation flag */ - SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, + SurfaceForce(const std::vector& surfaces, const math::Vector<3>& center_of_gravity_b_m, const bool is_calculation_enabled = true); /** * @fn ~SurfaceForce @@ -40,7 +40,7 @@ class SurfaceForce : public Disturbance { protected: // Spacecraft Structure parameters - const std::vector& surfaces_; //!< List of surfaces + const std::vector& surfaces_; //!< List of surfaces const math::Vector<3>& center_of_gravity_b_m_; //!< Position vector of the center of mass_kg at body frame [m] // Internal calculated variables diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index 30f5379ba..a096eedad 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -8,7 +8,7 @@ namespace s2e::dynamics { Dynamics::Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, - const environment::LocalEnvironment* local_environment, const int spacecraft_id, simulation::Structure* structure, + const environment::LocalEnvironment* local_environment, const int spacecraft_id, spacecraft::Structure* structure, simulation::RelativeInformation* relative_information) : structure_(structure), local_environment_(local_environment) { Initialize(simulation_configuration, simulation_time, spacecraft_id, structure, relative_information); @@ -21,7 +21,7 @@ Dynamics::~Dynamics() { } void Dynamics::Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, - const int spacecraft_id, simulation::Structure* structure, simulation::RelativeInformation* relative_information) { + const int spacecraft_id, spacecraft::Structure* structure, simulation::RelativeInformation* relative_information) { const environment::LocalCelestialInformation& local_celestial_information = local_environment_->GetCelestialInformation(); // Initialize orbit_ = orbit::InitOrbit(&(local_celestial_information.GetGlobalInformation()), simulation_configuration->spacecraft_file_list_[spacecraft_id], diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index 05f986807..4bfd91673 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -40,11 +40,11 @@ class Dynamics { * @param [in] simulation_time: Simulation time * @param [in] local_celestial_information: Local celestial information * @param [in] spacecraft_id: Spacecraft ID of the spacecraft - * @param [in] structure: simulation::Structure of the spacecraft + * @param [in] structure: spacecraft::Structure of the spacecraft * @param [in] relative_information: Relative information */ Dynamics(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, - const environment::LocalEnvironment* local_environment, const int spacecraft_id, simulation::Structure* structure, + const environment::LocalEnvironment* local_environment, const int spacecraft_id, spacecraft::Structure* structure, simulation::RelativeInformation* relative_information = (simulation::RelativeInformation*)nullptr); /** * @fn ~Dynamics @@ -118,7 +118,7 @@ class Dynamics { attitude::Attitude* attitude_; //!< Attitude dynamics orbit::Orbit* orbit_; //!< Orbit dynamics thermal::Temperature* temperature_; //!< Thermal dynamics - const simulation::Structure* structure_; //!< Structure information + const spacecraft::Structure* structure_; //!< Structure information const environment::LocalEnvironment* local_environment_; //!< Local environment /** @@ -132,7 +132,7 @@ class Dynamics { * @param [in] relative_information: Relative information */ void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::SimulationTime* simulation_time, - const int spacecraft_id, simulation::Structure* structure, + const int spacecraft_id, spacecraft::Structure* structure, simulation::RelativeInformation* relative_information = (simulation::RelativeInformation*)nullptr); }; diff --git a/src/simulation/ground_station/ground_station.cpp b/src/simulation/ground_station/ground_station.cpp index d848115fd..2c06c7b4c 100644 --- a/src/simulation/ground_station/ground_station.cpp +++ b/src/simulation/ground_station/ground_station.cpp @@ -47,7 +47,7 @@ void GroundStation::Initialize(const simulation::SimulationConfiguration* config void GroundStation::LogSetup(logger::Logger& logger) { logger.AddLogList(this); } -void GroundStation::Update(const environment::EarthRotation& celestial_rotation, const simulation::Spacecraft& spacecraft) { +void GroundStation::Update(const environment::EarthRotation& celestial_rotation, const spacecraft::Spacecraft& spacecraft) { math::Matrix<3, 3> dcm_ecef2eci = celestial_rotation.GetDcmJ2000ToEcef().Transpose(); position_i_m_ = dcm_ecef2eci * position_ecef_m_; diff --git a/src/simulation/ground_station/ground_station.hpp b/src/simulation/ground_station/ground_station.hpp index b5179ddd3..866b31edf 100644 --- a/src/simulation/ground_station/ground_station.hpp +++ b/src/simulation/ground_station/ground_station.hpp @@ -45,7 +45,7 @@ class GroundStation : public logger::ILoggable { * @fn Update * @brief Virtual function of main routine */ - virtual void Update(const environment::EarthRotation& celestial_rotation, const simulation::Spacecraft& spacecraft); + virtual void Update(const environment::EarthRotation& celestial_rotation, const spacecraft::Spacecraft& spacecraft); // Override functions for ILoggable /** diff --git a/src/simulation/spacecraft/installed_components.cpp b/src/simulation/spacecraft/installed_components.cpp index 7302e2ed5..c6a482fce 100644 --- a/src/simulation/spacecraft/installed_components.cpp +++ b/src/simulation/spacecraft/installed_components.cpp @@ -7,7 +7,7 @@ #include -namespace s2e::simulation { +namespace s2e::spacecraft { math::Vector<3> InstalledComponents::GenerateForce_b_N() { math::Vector<3> force_b_N_(0.0); @@ -21,4 +21,4 @@ math::Vector<3> InstalledComponents::GenerateTorque_b_Nm() { void InstalledComponents::LogSetup(logger::Logger& logger) { UNUSED(logger); } -} // namespace s2e::simulation +} // namespace s2e::spacecraft diff --git a/src/simulation/spacecraft/installed_components.hpp b/src/simulation/spacecraft/installed_components.hpp index 5b9f44b8c..096bba236 100644 --- a/src/simulation/spacecraft/installed_components.hpp +++ b/src/simulation/spacecraft/installed_components.hpp @@ -9,7 +9,7 @@ #include #include -namespace s2e::simulation { +namespace s2e::spacecraft { /** * @class InstalledComponents @@ -51,6 +51,6 @@ class InstalledComponents { virtual void LogSetup(logger::Logger& logger); }; -} // namespace s2e::simulation +} // namespace s2e::spacecraft #endif // S2E_SIMULATION_SPACECRAFT_INSTALLED_COMPONENTS_HPP_ diff --git a/src/simulation/spacecraft/spacecraft.cpp b/src/simulation/spacecraft/spacecraft.cpp index 19f4db5c5..24dd7a067 100644 --- a/src/simulation/spacecraft/spacecraft.cpp +++ b/src/simulation/spacecraft/spacecraft.cpp @@ -8,10 +8,10 @@ #include #include -namespace s2e::simulation { +namespace s2e::spacecraft { -Spacecraft::Spacecraft(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, - const int spacecraft_id, RelativeInformation* relative_information) +Spacecraft::Spacecraft(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, + const int spacecraft_id, simulation::RelativeInformation* relative_information) : spacecraft_id_(spacecraft_id) { Initialize(simulation_configuration, global_environment, spacecraft_id, relative_information); } @@ -27,10 +27,10 @@ Spacecraft::~Spacecraft() { delete components_; } -void Spacecraft::Initialize(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, - const int spacecraft_id, RelativeInformation* relative_information) { +void Spacecraft::Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, + const int spacecraft_id, simulation::RelativeInformation* relative_information) { clock_generator_.ClearTimerCount(); - structure_ = new simulation::Structure(simulation_configuration, spacecraft_id); + structure_ = new spacecraft::Structure(simulation_configuration, spacecraft_id); local_environment_ = new environment::LocalEnvironment(simulation_configuration, global_environment, spacecraft_id); dynamics_ = new dynamics::Dynamics(simulation_configuration, &(global_environment->GetSimulationTime()), local_environment_, spacecraft_id, structure_, relative_information); @@ -77,4 +77,4 @@ void Spacecraft::Update(const environment::SimulationTime* simulation_time) { void Spacecraft::Clear(void) { dynamics_->ClearForceTorque(); } -} // namespace s2e::simulation +} // namespace s2e::spacecraft diff --git a/src/simulation/spacecraft/spacecraft.hpp b/src/simulation/spacecraft/spacecraft.hpp index dc673105b..05afc0b96 100644 --- a/src/simulation/spacecraft/spacecraft.hpp +++ b/src/simulation/spacecraft/spacecraft.hpp @@ -15,7 +15,7 @@ #include "installed_components.hpp" #include "structure/structure.hpp" -namespace s2e::simulation { +namespace s2e::spacecraft { /** * @class Spacecraft @@ -27,8 +27,8 @@ class Spacecraft { * @fn Spacecraft * @brief Constructor for single satellite simulation */ - Spacecraft(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, - const int spacecraft_id, RelativeInformation* relative_information = nullptr); + Spacecraft(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, + const int spacecraft_id, simulation::RelativeInformation* relative_information = nullptr); /** * @fn ~Spacecraft @@ -45,8 +45,8 @@ class Spacecraft { * @fn Initialize * @brief Initialize function for multiple spacecraft simulation */ - virtual void Initialize(const SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, - const int spacecraft_id, RelativeInformation* relative_information = nullptr); + virtual void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, + const int spacecraft_id, simulation::RelativeInformation* relative_information = nullptr); /** * @fn Update @@ -86,7 +86,7 @@ class Spacecraft { * @fn GetStructure * @brief Get structure of the spacecraft */ - inline const simulation::Structure& GetStructure() const { return *structure_; } + inline const spacecraft::Structure& GetStructure() const { return *structure_; } /** * @fn GetInstalledComponents * @brief Get components installed on the spacecraft @@ -101,14 +101,14 @@ class Spacecraft { protected: environment::ClockGenerator clock_generator_; //!< Origin of clock for the spacecraft dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft - RelativeInformation* relative_information_; //!< Relative information with respect to the other spacecraft + simulation::RelativeInformation* relative_information_; //!< Relative information with respect to the other spacecraft environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft disturbances::Disturbances* disturbances_; //!< Disturbance information acting on the spacecraft - simulation::Structure* structure_; //!< Structure information of the spacecraft + spacecraft::Structure* structure_; //!< Structure information of the spacecraft InstalledComponents* components_; //!< Components information installed on the spacecraft const unsigned int spacecraft_id_; //!< ID of the spacecraft }; -} // namespace s2e::simulation +} // namespace s2e::spacecraft #endif // S2E_SIMULATION_SPACECRAFT_SPACECRAFT_HPP_ diff --git a/src/simulation/spacecraft/structure/initialize_structure.cpp b/src/simulation/spacecraft/structure/initialize_structure.cpp index bfdea9145..4fa2b131c 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.cpp +++ b/src/simulation/spacecraft/structure/initialize_structure.cpp @@ -8,7 +8,7 @@ #include #include -namespace s2e::simulation { +namespace s2e::spacecraft { #define MIN_VAL 1e-6 KinematicsParameters InitKinematicsParameters(std::string file_name) { @@ -31,14 +31,14 @@ KinematicsParameters InitKinematicsParameters(std::string file_name) { return kinematics_params; } -std::vector InitSurfaces(std::string file_name) { +std::vector InitSurfaces(std::string file_name) { using std::cout; auto conf = setting_file_reader::IniAccess(file_name); const char* section = "SURFACES"; const int num_surface = conf.ReadInt(section, "number_of_surfaces"); - std::vector surfaces; + std::vector surfaces; for (int i = 0; i < num_surface; i++) { std::string idx = std::to_string(i); @@ -103,7 +103,7 @@ std::vector InitSurfaces(std::string file_name) { } // Add a surface - surfaces.push_back(simulation::Surface(position, normal, area, ref, spe, air_spe)); + surfaces.push_back(spacecraft::Surface(position, normal, area, ref, spe, air_spe)); } return surfaces; } @@ -122,4 +122,4 @@ ResidualMagneticMoment InitResidualMagneticMoment(std::string file_name) { return rmm_params; } -} // namespace s2e::simulation +} // namespace s2e::spacecraft diff --git a/src/simulation/spacecraft/structure/initialize_structure.hpp b/src/simulation/spacecraft/structure/initialize_structure.hpp index 34a58888e..8995a9689 100644 --- a/src/simulation/spacecraft/structure/initialize_structure.hpp +++ b/src/simulation/spacecraft/structure/initialize_structure.hpp @@ -6,11 +6,9 @@ #ifndef S2E_SIMULATION_SPACECRAFT_STRUCTURE_INITIALIZE_STRUCTURE_HPP_ #define S2E_SIMULATION_SPACECRAFT_STRUCTURE_INITIALIZE_STRUCTURE_HPP_ -#pragma once - #include -namespace s2e::simulation { +namespace s2e::spacecraft { /** * @fn InitKinematicsParameters @@ -21,13 +19,13 @@ KinematicsParameters InitKinematicsParameters(std::string file_name); * @fn InitSurfaces * @brief Initialize the multiple surfaces with an ini file */ -std::vector InitSurfaces(std::string file_name); +std::vector InitSurfaces(std::string file_name); /** * @fn InitResidualMagneticMoment * @brief Initialize the RMM(Residual Magnetic Moment) parameters with an ini file */ ResidualMagneticMoment InitResidualMagneticMoment(std::string file_name); -} // namespace s2e::simulation +} // namespace s2e::spacecraft #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_INITIALIZE_STRUCTURE_HPP_ diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.cpp b/src/simulation/spacecraft/structure/kinematics_parameters.cpp index 0734e849d..5768c73e7 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.cpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.cpp @@ -5,9 +5,9 @@ #include "kinematics_parameters.hpp" -namespace s2e::simulation { +namespace s2e::spacecraft { KinematicsParameters::KinematicsParameters(math::Vector<3> center_of_gravity_b_m, double mass_kg, math::Matrix<3, 3> inertia_tensor_b_kgm2) : center_of_gravity_b_m_(center_of_gravity_b_m), mass_kg_(mass_kg), inertia_tensor_b_kgm2_(inertia_tensor_b_kgm2) {} -} // namespace s2e::simulation +} // namespace s2e::spacecraft diff --git a/src/simulation/spacecraft/structure/kinematics_parameters.hpp b/src/simulation/spacecraft/structure/kinematics_parameters.hpp index c4dfadeb2..33cc7cb9b 100644 --- a/src/simulation/spacecraft/structure/kinematics_parameters.hpp +++ b/src/simulation/spacecraft/structure/kinematics_parameters.hpp @@ -9,7 +9,7 @@ #include #include -namespace s2e::simulation { +namespace s2e::spacecraft { /** * @class KinematicsParameters @@ -88,6 +88,6 @@ class KinematicsParameters { math::Matrix<3, 3> inertia_tensor_b_kgm2_; //!< Inertia tensor at body frame [kgm2] }; -} // namespace s2e::simulation +} // namespace s2e::spacecraft #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_KINEMATICS_PARAMETERS_HPP_ diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp index 7f7108f61..7e10c5014 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.cpp @@ -5,7 +5,7 @@ #include "residual_magnetic_moment.hpp" -namespace s2e::simulation { +namespace s2e::spacecraft { ResidualMagneticMoment::ResidualMagneticMoment(const math::Vector<3> constant_value_b_Am2_, const double random_walk_standard_deviation_Am2, const double random_walk_limit_Am2, const double random_noise_standard_deviation_Am2) @@ -14,4 +14,4 @@ ResidualMagneticMoment::ResidualMagneticMoment(const math::Vector<3> constant_va random_walk_limit_Am2_(random_walk_limit_Am2), random_noise_standard_deviation_Am2_(random_noise_standard_deviation_Am2) {} -} // namespace s2e::simulation +} // namespace s2e::spacecraft diff --git a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp index ae06bb85d..7fbb338d6 100644 --- a/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp +++ b/src/simulation/spacecraft/structure/residual_magnetic_moment.hpp @@ -8,7 +8,7 @@ #include -namespace s2e::simulation { +namespace s2e::spacecraft { /** * @class ResidualMagneticMoment @@ -71,6 +71,6 @@ class ResidualMagneticMoment { double random_noise_standard_deviation_Am2_; //!< Standard deviation of white noise of RMM [Am2] }; -} // namespace s2e::simulation +} // namespace s2e::spacecraft #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_RESIDUAL_MAGNETIC_MOMENT_HPP_ \ No newline at end of file diff --git a/src/simulation/spacecraft/structure/structure.cpp b/src/simulation/spacecraft/structure/structure.cpp index 72260265d..af7e345d7 100644 --- a/src/simulation/spacecraft/structure/structure.cpp +++ b/src/simulation/spacecraft/structure/structure.cpp @@ -8,9 +8,9 @@ #include #include -namespace s2e::simulation { +namespace s2e::spacecraft { -Structure::Structure(const SimulationConfiguration* simulation_configuration, const int spacecraft_id) { +Structure::Structure(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id) { Initialize(simulation_configuration, spacecraft_id); } @@ -19,7 +19,7 @@ Structure::~Structure() { delete residual_magnetic_moment_; } -void Structure::Initialize(const SimulationConfiguration* simulation_configuration, const int spacecraft_id) { +void Structure::Initialize(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id) { // Read file name setting_file_reader::IniAccess conf = setting_file_reader::IniAccess(simulation_configuration->spacecraft_file_list_[spacecraft_id]); std::string ini_fname = conf.ReadString("SETTING_FILES", "structure_file"); @@ -31,4 +31,4 @@ void Structure::Initialize(const SimulationConfiguration* simulation_configurati residual_magnetic_moment_ = new ResidualMagneticMoment(InitResidualMagneticMoment(ini_fname)); } -} // namespace s2e::simulation +} // namespace s2e::spacecraft diff --git a/src/simulation/spacecraft/structure/structure.hpp b/src/simulation/spacecraft/structure/structure.hpp index 6a1b87ae0..f1bea3580 100644 --- a/src/simulation/spacecraft/structure/structure.hpp +++ b/src/simulation/spacecraft/structure/structure.hpp @@ -13,7 +13,7 @@ #include "residual_magnetic_moment.hpp" #include "surface.hpp" -namespace s2e::simulation { +namespace s2e::spacecraft { /** * @class Structure @@ -25,7 +25,7 @@ class Structure { * @fn Structure * @brief Constructor */ - Structure(const SimulationConfiguration* simulation_configuration, const int spacecraft_id); + Structure(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id); /** * @fn ~Structure * @brief Destructor @@ -35,14 +35,14 @@ class Structure { * @fn Initialize * @brief Initialize function */ - void Initialize(const SimulationConfiguration* simulation_configuration, const int spacecraft_id); + void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const int spacecraft_id); // Getter /** * @fn GetSurfaces * @brief Return surface information */ - inline const std::vector& GetSurfaces() const { return surfaces_; } + inline const std::vector& GetSurfaces() const { return surfaces_; } /** * @fn GetKinematicsParameters * @brief Return kinematics information @@ -58,7 +58,7 @@ class Structure { * @fn GetToSetSurfaces * @brief Return surface information */ - inline std::vector& GetToSetSurfaces() { return surfaces_; } + inline std::vector& GetToSetSurfaces() { return surfaces_; } /** * @fn GetToSetKinematicsParameters * @brief Return kinematics information @@ -72,10 +72,10 @@ class Structure { private: KinematicsParameters* kinematics_parameters_; //!< Kinematics parameters - std::vector surfaces_; //!< Surface information + std::vector surfaces_; //!< Surface information ResidualMagneticMoment* residual_magnetic_moment_; //!< Residual Magnetic Moment }; -} // namespace s2e::simulation +} // namespace s2e::spacecraft #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_STRUCTURE_HPP_ diff --git a/src/simulation/spacecraft/structure/surface.cpp b/src/simulation/spacecraft/structure/surface.cpp index 16b8b5f24..316d6165e 100644 --- a/src/simulation/spacecraft/structure/surface.cpp +++ b/src/simulation/spacecraft/structure/surface.cpp @@ -5,7 +5,7 @@ #include "surface.hpp" -namespace s2e::simulation { +namespace s2e::spacecraft { Surface::Surface(const math::Vector<3> position_b_m, const math::Vector<3> normal_b, const double area_m2, const double reflectivity, const double specularity, const double air_specularity) @@ -16,4 +16,4 @@ Surface::Surface(const math::Vector<3> position_b_m, const math::Vector<3> norma specularity_(specularity), air_specularity_(air_specularity) {} -} // namespace s2e::simulation +} // namespace s2e::spacecraft diff --git a/src/simulation/spacecraft/structure/surface.hpp b/src/simulation/spacecraft/structure/surface.hpp index 73a09d6e6..9f5375a29 100644 --- a/src/simulation/spacecraft/structure/surface.hpp +++ b/src/simulation/spacecraft/structure/surface.hpp @@ -8,7 +8,7 @@ #include -namespace s2e::simulation { +namespace s2e::spacecraft { /** * @class Surface @@ -115,6 +115,6 @@ class Surface { double air_specularity_; //!< Specularity for air drag }; -} // namespace s2e::simulation +} // namespace s2e::spacecraft #endif // S2E_SIMULATION_SPACECRAFT_STRUCTURE_SURFACE_HPP_ diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index 2226ce032..12cd92b38 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -13,7 +13,7 @@ namespace s2e::sample { using namespace components; -SampleComponents::SampleComponents(const dynamics::Dynamics* dynamics, simulation::Structure* structure, +SampleComponents::SampleComponents(const dynamics::Dynamics* dynamics, spacecraft::Structure* structure, const environment::LocalEnvironment* local_environment, const environment::GlobalEnvironment* global_environment, const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, const unsigned int spacecraft_id) diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index b15bc5b2c..11a534e95 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -58,13 +58,13 @@ namespace s2e::sample { * @class SampleComponents * @brief An example of user side components management class */ -class SampleComponents : public simulation::InstalledComponents { +class SampleComponents : public spacecraft::InstalledComponents { public: /** * @fn SampleComponents * @brief Constructor */ - SampleComponents(const dynamics::Dynamics* dynamics, simulation::Structure* structure, const environment::LocalEnvironment* local_environment, + SampleComponents(const dynamics::Dynamics* dynamics, spacecraft::Structure* structure, const environment::LocalEnvironment* local_environment, const environment::GlobalEnvironment* global_environment, const simulation::SimulationConfiguration* configuration, environment::ClockGenerator* clock_generator, const unsigned int spacecraft_id); /** @@ -142,7 +142,7 @@ class SampleComponents : public simulation::InstalledComponents { // States const simulation::SimulationConfiguration* configuration_; //!< Simulation settings const dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft - simulation::Structure* structure_; //!< Structure information of the spacecraft + spacecraft::Structure* structure_; //!< Structure information of the spacecraft const environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft const environment::GlobalEnvironment* global_environment_; //!< Global environment information }; diff --git a/src/simulation_sample/spacecraft/sample_spacecraft.cpp b/src/simulation_sample/spacecraft/sample_spacecraft.cpp index 13ab3289d..4a4970ab6 100644 --- a/src/simulation_sample/spacecraft/sample_spacecraft.cpp +++ b/src/simulation_sample/spacecraft/sample_spacecraft.cpp @@ -14,7 +14,7 @@ namespace s2e::sample { SampleSpacecraft::SampleSpacecraft(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, const unsigned int spacecraft_id) - : simulation::Spacecraft(simulation_configuration, global_environment, spacecraft_id) { + : spacecraft::Spacecraft(simulation_configuration, global_environment, spacecraft_id) { sample_components_ = new SampleComponents(dynamics_, structure_, local_environment_, global_environment, simulation_configuration, &clock_generator_, spacecraft_id); components_ = sample_components_; diff --git a/src/simulation_sample/spacecraft/sample_spacecraft.hpp b/src/simulation_sample/spacecraft/sample_spacecraft.hpp index 4a3767acc..d745e1774 100644 --- a/src/simulation_sample/spacecraft/sample_spacecraft.hpp +++ b/src/simulation_sample/spacecraft/sample_spacecraft.hpp @@ -18,7 +18,7 @@ namespace s2e::sample { * @class SampleSpacecraft * @brief An example of user side spacecraft class */ -class SampleSpacecraft : public simulation::Spacecraft { +class SampleSpacecraft : public spacecraft::Spacecraft { public: /** * @fn SampleSpacecraft From a4b50bc1f816687a15ebeb6a2d579b94f7bc281c Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 2 Oct 2024 00:18:01 +0900 Subject: [PATCH 49/49] Fix format --- src/simulation/spacecraft/spacecraft.cpp | 5 +++-- src/simulation/spacecraft/spacecraft.hpp | 21 ++++++++++--------- .../spacecraft/structure/structure.hpp | 2 +- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/simulation/spacecraft/spacecraft.cpp b/src/simulation/spacecraft/spacecraft.cpp index 24dd7a067..59e060a3a 100644 --- a/src/simulation/spacecraft/spacecraft.cpp +++ b/src/simulation/spacecraft/spacecraft.cpp @@ -27,8 +27,9 @@ Spacecraft::~Spacecraft() { delete components_; } -void Spacecraft::Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, - const int spacecraft_id, simulation::RelativeInformation* relative_information) { +void Spacecraft::Initialize(const simulation::SimulationConfiguration* simulation_configuration, + const environment::GlobalEnvironment* global_environment, const int spacecraft_id, + simulation::RelativeInformation* relative_information) { clock_generator_.ClearTimerCount(); structure_ = new spacecraft::Structure(simulation_configuration, spacecraft_id); local_environment_ = new environment::LocalEnvironment(simulation_configuration, global_environment, spacecraft_id); diff --git a/src/simulation/spacecraft/spacecraft.hpp b/src/simulation/spacecraft/spacecraft.hpp index 05afc0b96..b7de8189c 100644 --- a/src/simulation/spacecraft/spacecraft.hpp +++ b/src/simulation/spacecraft/spacecraft.hpp @@ -45,8 +45,9 @@ class Spacecraft { * @fn Initialize * @brief Initialize function for multiple spacecraft simulation */ - virtual void Initialize(const simulation::SimulationConfiguration* simulation_configuration, const environment::GlobalEnvironment* global_environment, - const int spacecraft_id, simulation::RelativeInformation* relative_information = nullptr); + virtual void Initialize(const simulation::SimulationConfiguration* simulation_configuration, + const environment::GlobalEnvironment* global_environment, const int spacecraft_id, + simulation::RelativeInformation* relative_information = nullptr); /** * @fn Update @@ -99,14 +100,14 @@ class Spacecraft { inline unsigned int GetSpacecraftId() const { return spacecraft_id_; } protected: - environment::ClockGenerator clock_generator_; //!< Origin of clock for the spacecraft - dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft - simulation::RelativeInformation* relative_information_; //!< Relative information with respect to the other spacecraft - environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft - disturbances::Disturbances* disturbances_; //!< Disturbance information acting on the spacecraft - spacecraft::Structure* structure_; //!< Structure information of the spacecraft - InstalledComponents* components_; //!< Components information installed on the spacecraft - const unsigned int spacecraft_id_; //!< ID of the spacecraft + environment::ClockGenerator clock_generator_; //!< Origin of clock for the spacecraft + dynamics::Dynamics* dynamics_; //!< Dynamics information of the spacecraft + simulation::RelativeInformation* relative_information_; //!< Relative information with respect to the other spacecraft + environment::LocalEnvironment* local_environment_; //!< Local environment information around the spacecraft + disturbances::Disturbances* disturbances_; //!< Disturbance information acting on the spacecraft + spacecraft::Structure* structure_; //!< Structure information of the spacecraft + InstalledComponents* components_; //!< Components information installed on the spacecraft + const unsigned int spacecraft_id_; //!< ID of the spacecraft }; } // namespace s2e::spacecraft diff --git a/src/simulation/spacecraft/structure/structure.hpp b/src/simulation/spacecraft/structure/structure.hpp index f1bea3580..09001aaeb 100644 --- a/src/simulation/spacecraft/structure/structure.hpp +++ b/src/simulation/spacecraft/structure/structure.hpp @@ -72,7 +72,7 @@ class Structure { private: KinematicsParameters* kinematics_parameters_; //!< Kinematics parameters - std::vector surfaces_; //!< Surface information + std::vector surfaces_; //!< Surface information ResidualMagneticMoment* residual_magnetic_moment_; //!< Residual Magnetic Moment };