From e12246068593af3658d1300d13e4e8d325beebf3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 15:58:00 +0100 Subject: [PATCH 01/57] clean include files --- src/dynamics/dynamics.cpp | 14 +++++------ src/dynamics/dynamics.hpp | 52 ++++++++++++++++++--------------------- 2 files changed, 30 insertions(+), 36 deletions(-) diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index a099a865c..d8c53db8e 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -7,8 +7,6 @@ #include "../simulation/multiple_spacecraft/relative_information.hpp" -using namespace std; - Dynamics::Dynamics(SimulationConfig* sim_config, const SimulationTime* sim_time, const LocalCelestialInformation* local_celes_info, const int sat_id, Structure* structure, RelativeInformation* rel_info) { Initialize(sim_config, sim_time, local_celes_info, sat_id, structure, rel_info); @@ -58,7 +56,7 @@ void Dynamics::Update(const SimulationTime* sim_time, const LocalCelestialInform } void Dynamics::ClearForceTorque(void) { - Vector<3> zero(0.0); + libra::Vector<3> zero(0.0); attitude_->SetTorque_b(zero); orbit_->SetAcceleration_i(zero); } @@ -68,14 +66,14 @@ void Dynamics::LogSetup(Logger& logger) { logger.AddLoggable(orbit_); } -void Dynamics::AddTorque_b(Vector<3> torque_b) { attitude_->AddTorque_b(torque_b); } +void Dynamics::AddTorque_b(libra::Vector<3> torque_b) { attitude_->AddTorque_b(torque_b); } -void Dynamics::AddForce_b(Vector<3> force_b) { +void Dynamics::AddForce_b(libra::Vector<3> force_b) { orbit_->AddForce_b(force_b, attitude_->GetQuaternion_i2b(), structure_->GetKinematicsParams().GetMass()); } -void Dynamics::AddAcceleration_i(Vector<3> acceleration_i) { orbit_->AddAcceleration_i(acceleration_i); } +void Dynamics::AddAcceleration_i(libra::Vector<3> acceleration_i) { orbit_->AddAcceleration_i(acceleration_i); } -Vector<3> Dynamics::GetPosition_i() const { return orbit_->GetSatPosition_i(); } +libra::Vector<3> Dynamics::GetPosition_i() const { return orbit_->GetSatPosition_i(); } -Quaternion Dynamics::GetQuaternion_i2b() const { return attitude_->GetQuaternion_i2b(); } +libra::Quaternion Dynamics::GetQuaternion_i2b() const { return attitude_->GetQuaternion_i2b(); } diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index d9b07d8cc..b77160845 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -7,20 +7,15 @@ #include -#include "../library/math/vector.hpp" -using libra::Vector; - -#include -#include -#include -#include - #include "../environment/global/simulation_time.hpp" #include "../environment/local/local_celestial_information.hpp" +#include "../library/math/vector.hpp" #include "../simulation/simulation_configuration.hpp" #include "../simulation/spacecraft/structure/structure.hpp" -#include "./orbit/orbit.hpp" -#include "./thermal/temperature.hpp" +#include "dynamics/attitude/initialize_attitude.hpp" +#include "dynamics/orbit/initialize_orbit.hpp" +#include "dynamics/thermal/initialize_node.hpp" +#include "dynamics/thermal/initialize_temperature.hpp" class RelativeInformation; @@ -48,19 +43,6 @@ class Dynamics { */ virtual ~Dynamics(); - /** - * @fn Initialize - * @brief Initialize function - * @param [in] sim_config: Simulation config - * @param [in] sim_time: Simulation time - * @param [in] local_celes_info: Local celestial information - * @param [in] sat_id: Spacecraft ID of the spacecraft - * @param [in] structure: Structure of the spacecraft - * @param [in] rel_info: Relative information - */ - void Initialize(SimulationConfig* sim_config, const SimulationTime* sim_time, const LocalCelestialInformation* local_celes_info, const int sat_id, - Structure* structure, RelativeInformation* rel_info = (RelativeInformation*)nullptr); - /** * @fn Update * @brief Update states of all dynamics calculations @@ -68,6 +50,7 @@ class Dynamics { * @param [in] local_celes_info: Local celestial information */ void Update(const SimulationTime* sim_time, const LocalCelestialInformation* local_celes_info); + /** * @fn LogSetup * @brief Log setup for dynamics calculation @@ -79,19 +62,19 @@ class Dynamics { * @brief Add input torque for the attitude dynamics propagation * @param [in] torue_b: Torque in the body fixed frame [Nm] */ - void AddTorque_b(Vector<3> torque_b); + void AddTorque_b(libra::Vector<3> torque_b); /** * @fn AddForce_b * @brief Add input force for the orbit dynamics propagation * @param [in] force_b: Force in the body fixed frame [N] */ - void AddForce_b(Vector<3> force_b); + void AddForce_b(libra::Vector<3> force_b); /** * @fn AddAcceleratione_b * @brief Add input acceleration for the orbit dynamics propagation * @param [in] acceleration_b: Force in the body fixed frame [N] */ - void AddAcceleration_i(Vector<3> acceleration_i); + void AddAcceleration_i(libra::Vector<3> acceleration_i); /** * @fn ClearForceTorque * @brief Clear force, acceleration, and torque for the dynamics propagation @@ -123,19 +106,32 @@ class Dynamics { * @fn GetPosition_i * @brief Return spacecraft position in the inertial frame [m] */ - virtual Vector<3> GetPosition_i() const; + virtual libra::Vector<3> GetPosition_i() const; /** * @fn GetQuaternion_i2b * @brief Return spacecraft attitude quaternion from the inertial frame to the body fixed frame */ - virtual Quaternion GetQuaternion_i2b() const; + virtual libra::Quaternion GetQuaternion_i2b() const; private: Attitude* attitude_; //!< Attitude dynamics Orbit* orbit_; //!< Orbit dynamics Temperature* temperature_; //!< Thermal dynamics const Structure* structure_; //!< Structure information + + /** + * @fn Initialize + * @brief Initialize function + * @param [in] sim_config: Simulation config + * @param [in] sim_time: Simulation time + * @param [in] local_celes_info: Local celestial information + * @param [in] sat_id: Spacecraft ID of the spacecraft + * @param [in] structure: Structure of the spacecraft + * @param [in] rel_info: Relative information + */ + void Initialize(SimulationConfig* sim_config, const SimulationTime* sim_time, const LocalCelestialInformation* local_celes_info, const int sat_id, + Structure* structure, RelativeInformation* rel_info = (RelativeInformation*)nullptr); }; #endif // S2E_DYNAMICS_DYNAMICS_HPP_ From 2161a67ae593b4ca8688758f610826f2342d5f27 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:02:20 +0100 Subject: [PATCH 02/57] Fix function argument names --- src/dynamics/dynamics.cpp | 45 +++++++++++++++++++---------------- src/dynamics/dynamics.hpp | 50 ++++++++++++++++++++------------------- 2 files changed, 50 insertions(+), 45 deletions(-) diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index d8c53db8e..634602e42 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -7,9 +7,10 @@ #include "../simulation/multiple_spacecraft/relative_information.hpp" -Dynamics::Dynamics(SimulationConfig* sim_config, const SimulationTime* sim_time, const LocalCelestialInformation* local_celes_info, const int sat_id, - Structure* structure, RelativeInformation* rel_info) { - Initialize(sim_config, sim_time, local_celes_info, sat_id, structure, rel_info); +Dynamics::Dynamics(SimulationConfig* simulation_configuration, const SimulationTime* simulation_time, + const LocalCelestialInformation* local_celestial_information, const int spacecraft_id, Structure* structure, + RelativeInformation* relative_information) { + Initialize(simulation_configuration, simulation_time, local_celestial_information, spacecraft_id, structure, relative_information); } Dynamics::~Dynamics() { @@ -18,39 +19,41 @@ Dynamics::~Dynamics() { delete temperature_; } -void Dynamics::Initialize(SimulationConfig* sim_config, const SimulationTime* sim_time, const LocalCelestialInformation* local_celes_info, - const int sat_id, Structure* structure, RelativeInformation* rel_info) { +void Dynamics::Initialize(SimulationConfig* simulation_configuration, const SimulationTime* simulation_time, + const LocalCelestialInformation* local_celestial_information, const int spacecraft_id, Structure* structure, + RelativeInformation* relative_information) { structure_ = structure; // Initialize - orbit_ = InitOrbit(&(local_celes_info->GetGlobalInformation()), sim_config->sat_file_[sat_id], sim_time->GetOrbitRkStepTime_s(), - sim_time->GetCurrentTime_jd(), local_celes_info->GetGlobalInformation().GetCenterBodyGravityConstant_m3_s2(), "ORBIT", rel_info); - attitude_ = InitAttitude(sim_config->sat_file_[sat_id], orbit_, local_celes_info, sim_time->GetAttitudeRkStepTime_s(), - structure->GetKinematicsParams().GetInertiaTensor(), sat_id); - temperature_ = InitTemperature(sim_config->sat_file_[sat_id], sim_time->GetThermalRkStepTime_s()); + orbit_ = InitOrbit(&(local_celestial_information->GetGlobalInformation()), simulation_configuration->sat_file_[spacecraft_id], + simulation_time->GetOrbitRkStepTime_s(), simulation_time->GetCurrentTime_jd(), + local_celestial_information->GetGlobalInformation().GetCenterBodyGravityConstant_m3_s2(), "ORBIT", relative_information); + attitude_ = InitAttitude(simulation_configuration->sat_file_[spacecraft_id], orbit_, local_celestial_information, + simulation_time->GetAttitudeRkStepTime_s(), structure->GetKinematicsParams().GetInertiaTensor(), spacecraft_id); + temperature_ = InitTemperature(simulation_configuration->sat_file_[spacecraft_id], simulation_time->GetThermalRkStepTime_s()); // To get initial value orbit_->UpdateAtt(attitude_->GetQuaternion_i2b()); } -void Dynamics::Update(const SimulationTime* sim_time, const LocalCelestialInformation* local_celes_info) { +void Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestialInformation* local_celestial_information) { // Attitude propagation - if (sim_time->GetAttitudePropagateFlag()) { - attitude_->Propagate(sim_time->GetElapsedTime_s()); + if (simulation_time->GetAttitudePropagateFlag()) { + attitude_->Propagate(simulation_time->GetElapsedTime_s()); } // Orbit Propagation - if (sim_time->GetOrbitPropagateFlag()) { - orbit_->Propagate(sim_time->GetElapsedTime_s(), sim_time->GetCurrentTime_jd()); + if (simulation_time->GetOrbitPropagateFlag()) { + orbit_->Propagate(simulation_time->GetElapsedTime_s(), simulation_time->GetCurrentTime_jd()); } // Attitude dependent update orbit_->UpdateAtt(attitude_->GetQuaternion_i2b()); // Thermal - if (sim_time->GetThermalPropagateFlag()) { + if (simulation_time->GetThermalPropagateFlag()) { std::string sun_str = "SUN"; char* c_sun = new char[sun_str.size() + 1]; std::char_traits::copy(c_sun, sun_str.c_str(), sun_str.size() + 1); // string -> char* - temperature_->Propagate(local_celes_info->GetPositionFromSpacecraft_b_m(c_sun), sim_time->GetElapsedTime_s()); + temperature_->Propagate(local_celestial_information->GetPositionFromSpacecraft_b_m(c_sun), simulation_time->GetElapsedTime_s()); delete[] c_sun; } } @@ -66,13 +69,13 @@ void Dynamics::LogSetup(Logger& logger) { logger.AddLoggable(orbit_); } -void Dynamics::AddTorque_b(libra::Vector<3> torque_b) { attitude_->AddTorque_b(torque_b); } +void Dynamics::AddTorque_b(libra::Vector<3> torque_b_Nm) { attitude_->AddTorque_b(torque_b_Nm); } -void Dynamics::AddForce_b(libra::Vector<3> force_b) { - orbit_->AddForce_b(force_b, attitude_->GetQuaternion_i2b(), structure_->GetKinematicsParams().GetMass()); +void Dynamics::AddForce_b(libra::Vector<3> force_b_N) { + orbit_->AddForce_b(force_b_N, attitude_->GetQuaternion_i2b(), structure_->GetKinematicsParams().GetMass()); } -void Dynamics::AddAcceleration_i(libra::Vector<3> acceleration_i) { orbit_->AddAcceleration_i(acceleration_i); } +void Dynamics::AddAcceleration_i(libra::Vector<3> acceleration_i_m_s2) { orbit_->AddAcceleration_i(acceleration_i_m_s2); } libra::Vector<3> Dynamics::GetPosition_i() const { return orbit_->GetSatPosition_i(); } diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index b77160845..48036962c 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -28,15 +28,16 @@ class Dynamics { /** * @fn Dynamics * @brief Constructor - * @param [in] sim_config: Simulation config - * @param [in] sim_time: Simulation time - * @param [in] local_celes_info: Local celestial information - * @param [in] sat_id: Spacecraft ID of the spacecraft + * @param [in] simulation_configuration: Simulation config + * @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] rel_info: Relative information + * @param [in] relative_information: Relative information */ - Dynamics(SimulationConfig* sim_config, const SimulationTime* sim_time, const LocalCelestialInformation* local_celes_info, const int sat_id, - Structure* structure, RelativeInformation* rel_info = (RelativeInformation*)nullptr); + Dynamics(SimulationConfig* simulation_configuration, const SimulationTime* simulation_time, + const LocalCelestialInformation* local_celestial_information, const int spacecraft_id, Structure* structure, + RelativeInformation* relative_information = (RelativeInformation*)nullptr); /** * @fn ~Dynamics * @brief Destructor @@ -46,10 +47,10 @@ class Dynamics { /** * @fn Update * @brief Update states of all dynamics calculations - * @param [in] sim_time: Simulation time - * @param [in] local_celes_info: Local celestial information + * @param [in] simulation_time: Simulation time + * @param [in] local_celestial_information: Local celestial information */ - void Update(const SimulationTime* sim_time, const LocalCelestialInformation* local_celes_info); + void Update(const SimulationTime* simulation_time, const LocalCelestialInformation* local_celestial_information); /** * @fn LogSetup @@ -60,21 +61,21 @@ class Dynamics { /** * @fn AddTorque_b * @brief Add input torque for the attitude dynamics propagation - * @param [in] torue_b: Torque in the body fixed frame [Nm] + * @param [in] torque_b_Nm: Torque in the body fixed frame [Nm] */ - void AddTorque_b(libra::Vector<3> torque_b); + void AddTorque_b(libra::Vector<3> torque_b_Nm); /** * @fn AddForce_b * @brief Add input force for the orbit dynamics propagation - * @param [in] force_b: Force in the body fixed frame [N] + * @param [in] force_b_N: Force in the body fixed frame [N] */ - void AddForce_b(libra::Vector<3> force_b); + void AddForce_b(libra::Vector<3> force_b_N); /** - * @fn AddAcceleratione_b + * @fn AddAcceleratione_i * @brief Add input acceleration for the orbit dynamics propagation - * @param [in] acceleration_b: Force in the body fixed frame [N] + * @param [in] acceleration_i_m_s2: Acceleration in the inertial fixed frame [N] */ - void AddAcceleration_i(libra::Vector<3> acceleration_i); + void AddAcceleration_i(libra::Vector<3> acceleration_i_m_s2); /** * @fn ClearForceTorque * @brief Clear force, acceleration, and torque for the dynamics propagation @@ -123,15 +124,16 @@ class Dynamics { /** * @fn Initialize * @brief Initialize function - * @param [in] sim_config: Simulation config - * @param [in] sim_time: Simulation time - * @param [in] local_celes_info: Local celestial information - * @param [in] sat_id: Spacecraft ID of the spacecraft + * @param [in] simulation_configuration: Simulation config + * @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] rel_info: Relative information + * @param [in] relative_information: Relative information */ - void Initialize(SimulationConfig* sim_config, const SimulationTime* sim_time, const LocalCelestialInformation* local_celes_info, const int sat_id, - Structure* structure, RelativeInformation* rel_info = (RelativeInformation*)nullptr); + void Initialize(SimulationConfig* simulation_configuration, const SimulationTime* simulation_time, + const LocalCelestialInformation* local_celestial_information, const int spacecraft_id, Structure* structure, + RelativeInformation* relative_information = (RelativeInformation*)nullptr); }; #endif // S2E_DYNAMICS_DYNAMICS_HPP_ From ecdea90e8d2c8141c1940be18898fcdd48927769 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:05:52 +0100 Subject: [PATCH 03/57] Move to inline functions --- src/dynamics/dynamics.cpp | 4 ---- src/dynamics/dynamics.hpp | 4 ++-- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index 634602e42..c4f3f39c9 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -76,7 +76,3 @@ void Dynamics::AddForce_b(libra::Vector<3> force_b_N) { } void Dynamics::AddAcceleration_i(libra::Vector<3> acceleration_i_m_s2) { orbit_->AddAcceleration_i(acceleration_i_m_s2); } - -libra::Vector<3> Dynamics::GetPosition_i() const { return orbit_->GetSatPosition_i(); } - -libra::Quaternion Dynamics::GetQuaternion_i2b() const { return attitude_->GetQuaternion_i2b(); } diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index 48036962c..836a3f0f4 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -107,13 +107,13 @@ class Dynamics { * @fn GetPosition_i * @brief Return spacecraft position in the inertial frame [m] */ - virtual libra::Vector<3> GetPosition_i() const; + inline libra::Vector<3> GetPosition_i() const { return orbit_->GetSatPosition_i(); } /** * @fn GetQuaternion_i2b * @brief Return spacecraft attitude quaternion from the inertial frame to the body fixed frame */ - virtual libra::Quaternion GetQuaternion_i2b() const; + inline libra::Quaternion GetQuaternion_i2b() const { return attitude_->GetQuaternion_i2b(); } private: Attitude* attitude_; //!< Attitude dynamics From 3c64e4b297b8ac40e71b91ff56b86d51d95e54b2 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:07:59 +0100 Subject: [PATCH 04/57] Remove unnecesarry Get function --- src/components/real/aocs/gnss_receiver.cpp | 2 +- src/dynamics/dynamics.hpp | 14 +------------- 2 files changed, 2 insertions(+), 14 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index d05b8d2de..c037bae9a 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -49,7 +49,7 @@ void GNSSReceiver::MainRoutine(int count) { UNUSED(count); Vector<3> pos_true_eci_ = dynamics_->GetOrbit().GetSatPosition_i(); - Quaternion q_i2b = dynamics_->GetQuaternion_i2b(); + Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); CheckAntenna(pos_true_eci_, q_i2b); diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index 836a3f0f4..e1338ee21 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -71,7 +71,7 @@ class Dynamics { */ void AddForce_b(libra::Vector<3> force_b_N); /** - * @fn AddAcceleratione_i + * @fn AddAcceleration_i * @brief Add input acceleration for the orbit dynamics propagation * @param [in] acceleration_i_m_s2: Acceleration in the inertial fixed frame [N] */ @@ -103,18 +103,6 @@ class Dynamics { */ inline Attitude& SetAttitude() const { return *attitude_; } - /** - * @fn GetPosition_i - * @brief Return spacecraft position in the inertial frame [m] - */ - inline libra::Vector<3> GetPosition_i() const { return orbit_->GetSatPosition_i(); } - - /** - * @fn GetQuaternion_i2b - * @brief Return spacecraft attitude quaternion from the inertial frame to the body fixed frame - */ - inline libra::Quaternion GetQuaternion_i2b() const { return attitude_->GetQuaternion_i2b(); } - private: Attitude* attitude_; //!< Attitude dynamics Orbit* orbit_; //!< Orbit dynamics From 1a8d1b6e8e863eb6c940519004e98aaf6e9eff95 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:11:16 +0100 Subject: [PATCH 05/57] Move to inline functions --- src/dynamics/dynamics.cpp | 8 -------- src/dynamics/dynamics.hpp | 9 ++++++--- 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index c4f3f39c9..c000bafc2 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -68,11 +68,3 @@ void Dynamics::LogSetup(Logger& logger) { logger.AddLoggable(attitude_); logger.AddLoggable(orbit_); } - -void Dynamics::AddTorque_b(libra::Vector<3> torque_b_Nm) { attitude_->AddTorque_b(torque_b_Nm); } - -void Dynamics::AddForce_b(libra::Vector<3> force_b_N) { - orbit_->AddForce_b(force_b_N, attitude_->GetQuaternion_i2b(), structure_->GetKinematicsParams().GetMass()); -} - -void Dynamics::AddAcceleration_i(libra::Vector<3> acceleration_i_m_s2) { orbit_->AddAcceleration_i(acceleration_i_m_s2); } diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index e1338ee21..fbf24735b 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -63,19 +63,22 @@ class Dynamics { * @brief Add input torque for the attitude dynamics propagation * @param [in] torque_b_Nm: Torque in the body fixed frame [Nm] */ - void AddTorque_b(libra::Vector<3> torque_b_Nm); + inline void AddTorque_b(libra::Vector<3> torque_b_Nm) { attitude_->AddTorque_b(torque_b_Nm); } /** * @fn AddForce_b * @brief Add input force for the orbit dynamics propagation * @param [in] force_b_N: Force in the body fixed frame [N] */ - void AddForce_b(libra::Vector<3> force_b_N); + inline void AddForce_b(libra::Vector<3> force_b_N) { + orbit_->AddForce_b(force_b_N, attitude_->GetQuaternion_i2b(), structure_->GetKinematicsParams().GetMass()); + } /** * @fn AddAcceleration_i * @brief Add input acceleration for the orbit dynamics propagation * @param [in] acceleration_i_m_s2: Acceleration in the inertial fixed frame [N] */ - void AddAcceleration_i(libra::Vector<3> acceleration_i_m_s2); + inline void AddAcceleration_i(libra::Vector<3> acceleration_i_m_s2) { orbit_->AddAcceleration_i(acceleration_i_m_s2); } + /** * @fn ClearForceTorque * @brief Clear force, acceleration, and torque for the dynamics propagation From 453c1dbe4241779c5227e32f5c2de2015bcc17f7 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:13:16 +0100 Subject: [PATCH 06/57] Rename public functions --- src/dynamics/attitude/attitude.hpp | 4 ++-- src/dynamics/attitude/controlled_attitude.cpp | 2 +- src/dynamics/dynamics.hpp | 14 +++++++------- src/dynamics/orbit/orbit.hpp | 6 +++--- src/simulation/spacecraft/spacecraft.cpp | 10 +++++----- 5 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index d1a66264b..f405ecf78 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -110,10 +110,10 @@ class Attitude : public ILoggable, public SimulationObject { */ inline void SetTorque_b(const libra::Vector<3> set) { torque_b_Nm_ = set; } /** - * @fn AddTorque_b + * @fn AddTorque_b_Nm * @brief Add torque acting on the spacecraft on the body fixed frame [Nm] */ - inline void AddTorque_b(const libra::Vector<3> set) { torque_b_Nm_ += set; } + inline void AddTorque_b_Nm(const libra::Vector<3> set) { torque_b_Nm_ += set; } /** * @fn SetAngMom_rw * @brief Set angular momentum of reaction wheel in the body fixed frame [Nms] diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index 2f86e16c3..0c8674bcd 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -160,7 +160,7 @@ void ControlledAttitude::CalcAngularVelocity(const double current_time_s) { controlled_torque_b_Nm = libra::Vector<3>(0.0); } // Add torque with disturbances - AddTorque_b(controlled_torque_b_Nm); + AddTorque_b_Nm(controlled_torque_b_Nm); // save previous values previous_calc_time_s_ = current_time_s; prev_quaternion_i2b_ = quaternion_i2b_; diff --git a/src/dynamics/dynamics.hpp b/src/dynamics/dynamics.hpp index fbf24735b..75cf2e727 100644 --- a/src/dynamics/dynamics.hpp +++ b/src/dynamics/dynamics.hpp @@ -59,25 +59,25 @@ class Dynamics { void LogSetup(Logger& logger); /** - * @fn AddTorque_b + * @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(libra::Vector<3> torque_b_Nm) { attitude_->AddTorque_b(torque_b_Nm); } + inline void AddTorque_b_Nm(libra::Vector<3> torque_b_Nm) { attitude_->AddTorque_b_Nm(torque_b_Nm); } /** - * @fn AddForce_b + * @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(libra::Vector<3> force_b_N) { - orbit_->AddForce_b(force_b_N, attitude_->GetQuaternion_i2b(), structure_->GetKinematicsParams().GetMass()); + inline void AddForce_b_N(libra::Vector<3> force_b_N) { + orbit_->AddForce_b_N(force_b_N, attitude_->GetQuaternion_i2b(), structure_->GetKinematicsParams().GetMass()); } /** - * @fn AddAcceleration_i + * @fn AddAcceleration_i_m_s2 * @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(libra::Vector<3> acceleration_i_m_s2) { orbit_->AddAcceleration_i(acceleration_i_m_s2); } + inline void AddAcceleration_i_m_s2(libra::Vector<3> acceleration_i_m_s2) { orbit_->AddAcceleration_i_m_s2(acceleration_i_m_s2); } /** * @fn ClearForceTorque diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index 03152777a..c9c033424 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -160,10 +160,10 @@ class Orbit : public ILoggable { acc_i_ += force_i; } /** - * @fn AddAcceleration_i + * @fn AddAcceleration_i_m_s2 * @brief Add acceleration in the inertial frame [m/s2] */ - inline void AddAcceleration_i(Vector<3> acceleration_i) { acc_i_ += acceleration_i; } + inline void AddAcceleration_i_m_s2(Vector<3> acceleration_i) { acc_i_ += acceleration_i; } /** * @fn AddForce_i * @brief Add force @@ -171,7 +171,7 @@ class Orbit : public ILoggable { * @param [in] q_i2b: Quaternion from the inertial frame to the body fixed frame * @param [in] spacecraft_mass: Mass of spacecraft [kg] */ - inline void AddForce_b(Vector<3> force_b, Quaternion q_i2b, double spacecraft_mass) { + inline void AddForce_b_N(Vector<3> force_b, Quaternion q_i2b, double spacecraft_mass) { auto force_i = q_i2b.frame_conv_inv(force_b); AddForce_i(force_i, spacecraft_mass); } diff --git a/src/simulation/spacecraft/spacecraft.cpp b/src/simulation/spacecraft/spacecraft.cpp index acf0fc873..297a3df6c 100644 --- a/src/simulation/spacecraft/spacecraft.cpp +++ b/src/simulation/spacecraft/spacecraft.cpp @@ -71,13 +71,13 @@ void Spacecraft::Update(const SimulationTime* sim_time) { clock_gen_.UpdateComponents(sim_time); // Add generated force and torque by disturbances - dynamics_->AddAcceleration_i(disturbances_->GetAcceleration_i_m_s2()); - dynamics_->AddTorque_b(disturbances_->GetTorque_b_Nm()); - dynamics_->AddForce_b(disturbances_->GetForce_b_N()); + dynamics_->AddAcceleration_i_m_s2(disturbances_->GetAcceleration_i_m_s2()); + dynamics_->AddTorque_b_Nm(disturbances_->GetTorque_b_Nm()); + dynamics_->AddForce_b_N(disturbances_->GetForce_b_N()); // Add generated force and torque by components - dynamics_->AddTorque_b(components_->GenerateTorque_Nm_b()); - dynamics_->AddForce_b(components_->GenerateForce_N_b()); + dynamics_->AddTorque_b_Nm(components_->GenerateTorque_Nm_b()); + dynamics_->AddForce_b_N(components_->GenerateForce_N_b()); // Propagate dynamics dynamics_->Update(sim_time, &(local_env_->GetCelestialInformation())); From 5aae154a68085098f1826cab9950b8216abd7205 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:19:52 +0100 Subject: [PATCH 07/57] Remove TODO --- src/dynamics/attitude/attitude.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index f405ecf78..a6257dae3 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -152,7 +152,7 @@ class Attitude : public ILoggable, public SimulationObject { protected: bool is_calc_enabled_ = true; //!< Calculation flag - double prop_step_s_; //!< Propagation step [sec] TODO: consider is it really need here + double prop_step_s_; //!< Propagation step [sec] libra::Vector<3> omega_b_rad_s_; //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s] libra::Quaternion quaternion_i2b_; //!< Attitude quaternion from the inertial frame to the body fixed frame libra::Vector<3> torque_b_Nm_; //!< Torque in the body fixed frame [Nm] From da88fdd78b34be91285b1e39fd6dab60be351709 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:25:15 +0100 Subject: [PATCH 08/57] Fix TODO --- src/dynamics/attitude/attitude.hpp | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index a6257dae3..fbd342636 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -46,21 +46,6 @@ class Attitude : public ILoggable, public SimulationObject { * @brief Return attitude quaternion from the inertial frame to the body fixed frame */ inline libra::Quaternion GetQuaternion_i2b() const { return quaternion_i2b_; } - /** - * @fn GetDCM_b2i - * @brief Return attitude direction cosine matrix from the body fixed frame to the inertial frame - * @note TODO: Check correctness i2b? b2i? - */ - inline libra::Matrix<3, 3> GetDCM_b2i() const { return quaternion_i2b_.toDCM(); } - /** - * @fn GetDCM_i2b - * @brief Return attitude direction cosine matrix from the inertial frame to the body fixed frame - * @note TODO: Check correctness i2b? b2i? - */ - inline libra::Matrix<3, 3> GetDCM_i2b() const { - libra::Matrix<3, 3> DCM_b2i = quaternion_i2b_.toDCM(); - return transpose(DCM_b2i); - } /** * @fn GetTotalAngMomNorm * @brief Return norm of total angular momentum of the spacecraft [Nms] @@ -69,9 +54,8 @@ class Attitude : public ILoggable, public SimulationObject { /** * @fn GetEnergy * @brief Return rotational Kinetic Energy of Spacecraft [J] - * @note TODO: Consider to use k_sc_J_ */ - inline double GetEnergy() const { return 0.5f * libra::inner_product(omega_b_rad_s_, inertia_tensor_kgm2_ * omega_b_rad_s_); } + inline double GetEnergy() const { return k_sc_J_; } /** * @fn GetInertiaTensor * @brief Return inertia tensor [kg m^2] From 610cbe90f7911be743bb36df1cfec0ede8b848b3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:39:14 +0100 Subject: [PATCH 09/57] Integrate private functions --- src/dynamics/attitude/attitude.cpp | 6 ++--- src/dynamics/attitude/attitude.hpp | 35 +++++++++++++------------- src/dynamics/attitude/attitude_rk4.cpp | 8 +++--- 3 files changed, 23 insertions(+), 26 deletions(-) diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index 58e17ac8e..e80eb1255 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -44,12 +44,12 @@ std::string Attitude::GetLogValue() const { void Attitude::SetParameters(const MCSimExecutor& mc_sim) { GetInitParameterQuaternion(mc_sim, "Q_i2b", quaternion_i2b_); } -void Attitude::CalcAngMom(void) { +void Attitude::CalcAngularMomentum(void) { h_sc_b_Nms_ = inertia_tensor_kgm2_ * omega_b_rad_s_; h_total_b_Nms_ = h_rw_b_Nms_ + h_sc_b_Nms_; Quaternion q_b2i = quaternion_i2b_.conjugate(); h_total_i_Nms_ = q_b2i.frame_conv(h_total_b_Nms_); h_total_Nms_ = norm(h_total_i_Nms_); -} -void Attitude::CalcSatRotationalKineticEnergy(void) { k_sc_J_ = 0.5 * libra::inner_product(h_sc_b_Nms_, omega_b_rad_s_); } + k_sc_J_ = 0.5 * libra::inner_product(h_sc_b_Nms_, omega_b_rad_s_); +} diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index fbd342636..1812ae995 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -88,6 +88,7 @@ class Attitude : public ILoggable, public SimulationObject { * @brief Add quaternion offset rotation */ inline void AddQuaternionOffset(const libra::Quaternion offset) { quaternion_i2b_ = quaternion_i2b_ * offset; } + /** * @fn SetTorque_b * @brief Set torque acting on the spacecraft on the body fixed frame [Nm] @@ -98,6 +99,7 @@ class Attitude : public ILoggable, public SimulationObject { * @brief Add torque acting on the spacecraft on the body fixed frame [Nm] */ inline void AddTorque_b_Nm(const libra::Vector<3> set) { torque_b_Nm_ += set; } + /** * @fn SetAngMom_rw * @brief Set angular momentum of reaction wheel in the body fixed frame [Nms] @@ -135,30 +137,27 @@ class Attitude : public ILoggable, public SimulationObject { virtual void SetParameters(const MCSimExecutor& mc_sim); protected: - bool is_calc_enabled_ = true; //!< Calculation flag - double prop_step_s_; //!< Propagation step [sec] - libra::Vector<3> omega_b_rad_s_; //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s] - libra::Quaternion quaternion_i2b_; //!< Attitude quaternion from the inertial frame to the body fixed frame - libra::Vector<3> torque_b_Nm_; //!< Torque in the body fixed frame [Nm] + bool is_calc_enabled_ = true; //!< Calculation flag + double prop_step_s_; //!< Propagation step [sec] + libra::Vector<3> omega_b_rad_s_; //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s] + libra::Quaternion quaternion_i2b_; //!< Attitude quaternion from the inertial frame to the body fixed frame + libra::Vector<3> torque_b_Nm_; //!< Torque in the body fixed frame [Nm] + libra::Matrix<3, 3> inertia_tensor_kgm2_; //!< Inertia tensor of the spacecraft [kg m^2] TODO: Move to Structure libra::Matrix<3, 3> inv_inertia_tensor_; //!< Inverse matrix of the inertia tensor - libra::Vector<3> h_sc_b_Nms_; //!< Angular momentum of spacecraft in the body fixed frame [Nms] - libra::Vector<3> h_rw_b_Nms_; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] - libra::Vector<3> h_total_b_Nms_; //!< Total angular momentum of spacecraft in the body fixed frame [Nms] - libra::Vector<3> h_total_i_Nms_; //!< Total angular momentum of spacecraft in the inertial frame [Nms] - double h_total_Nms_; //!< Norm of total angular momentum [Nms] - double k_sc_J_; //!< Rotational Kinetic Energy of Spacecraft [J] + + libra::Vector<3> h_sc_b_Nms_; //!< Angular momentum of spacecraft in the body fixed frame [Nms] + libra::Vector<3> h_rw_b_Nms_; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] + libra::Vector<3> h_total_b_Nms_; //!< Total angular momentum of spacecraft in the body fixed frame [Nms] + libra::Vector<3> h_total_i_Nms_; //!< Total angular momentum of spacecraft in the inertial frame [Nms] + double h_total_Nms_; //!< Norm of total angular momentum [Nms] + double k_sc_J_; //!< Rotational Kinetic Energy of Spacecraft [J] /** - * @fn CalcAngMom + * @fn CalcAngularMomentum * @brief Calculate angular momentum */ - void CalcAngMom(void); - /** - * @fn CalcSatRotationalKineticEnergy - * @brief Calculate rotational Kinetic Energy of Spacecraft - */ - void CalcSatRotationalKineticEnergy(void); + void CalcAngularMomentum(void); }; #endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_HPP_ diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index 1998ff286..a5e14db06 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -22,7 +22,7 @@ AttitudeRK4::AttitudeRK4(const Vector<3>& omega_b_ini, const Quaternion& quatern prop_time_s_ = 0.0; inv_inertia_tensor_ = invert(inertia_tensor_kgm2_); h_rw_b_Nms_ = libra::Vector<3>(0.0); - CalcAngMom(); + CalcAngularMomentum(); } AttitudeRK4::~AttitudeRK4() {} @@ -35,8 +35,7 @@ void AttitudeRK4::SetParameters(const MCSimExecutor& mc_sim) { prop_time_s_ = 0.0; inv_inertia_tensor_ = libra::invert(inertia_tensor_kgm2_); h_rw_b_Nms_ = Vector<3>(0.0); //!< Consider how to handle this variable - CalcAngMom(); - CalcSatRotationalKineticEnergy(); + CalcAngularMomentum(); } void AttitudeRK4::Propagate(const double endtime_s) { @@ -48,8 +47,7 @@ void AttitudeRK4::Propagate(const double endtime_s) { RungeOneStep(prop_time_s_, endtime_s - prop_time_s_); prop_time_s_ = endtime_s; - CalcAngMom(); - CalcSatRotationalKineticEnergy(); + CalcAngularMomentum(); } Matrix<4, 4> AttitudeRK4::Omega4Kinematics(Vector<3> omega) { From 6a4e81748763469770e4bc6b9501f896c8abff3f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:43:55 +0100 Subject: [PATCH 10/57] Fix function arguments --- src/dynamics/attitude/attitude.cpp | 2 +- src/dynamics/attitude/attitude.hpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index e80eb1255..e3d396cef 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -47,7 +47,7 @@ void Attitude::SetParameters(const MCSimExecutor& mc_sim) { GetInitParameterQuat void Attitude::CalcAngularMomentum(void) { h_sc_b_Nms_ = inertia_tensor_kgm2_ * omega_b_rad_s_; h_total_b_Nms_ = h_rw_b_Nms_ + h_sc_b_Nms_; - Quaternion q_b2i = quaternion_i2b_.conjugate(); + libra::Quaternion q_b2i = quaternion_i2b_.conjugate(); h_total_i_Nms_ = q_b2i.frame_conv(h_total_b_Nms_); h_total_Nms_ = norm(h_total_i_Nms_); diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index 1812ae995..8dd2bfcfe 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -72,17 +72,17 @@ class Attitude : public ILoggable, public SimulationObject { * @fn SetPropStep * @brief Set propagation step [sec] */ - inline void SetPropStep(double set) { prop_step_s_ = set; } + inline void SetPropStep(double prop_step_s) { prop_step_s_ = prop_step_s; } /** * @fn SetOmega_b * @brief Set angular velocity of the body fixed frame with respect to the inertial frame [rad/s] */ - inline void SetOmega_b(const libra::Vector<3> set) { omega_b_rad_s_ = set; } + inline void SetOmega_b(const libra::Vector<3> omega_b_rad_s) { omega_b_rad_s_ = omega_b_rad_s; } /** * @fn SetQuaternion_i2b * @brief Set attitude quaternion from the inertial frame to the body frame */ - inline void SetQuaternion_i2b(const libra::Quaternion set) { quaternion_i2b_ = set; } + inline void SetQuaternion_i2b(const libra::Quaternion quaternion_i2b) { quaternion_i2b_ = quaternion_i2b; } /** * @fn AddQuaternionOffset * @brief Add quaternion offset rotation @@ -93,24 +93,24 @@ class Attitude : public ILoggable, public SimulationObject { * @fn SetTorque_b * @brief Set torque acting on the spacecraft on the body fixed frame [Nm] */ - inline void SetTorque_b(const libra::Vector<3> set) { torque_b_Nm_ = set; } + inline void SetTorque_b(const libra::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 libra::Vector<3> set) { torque_b_Nm_ += set; } + inline void AddTorque_b_Nm(const libra::Vector<3> torque_b_Nm) { torque_b_Nm_ += torque_b_Nm; } /** * @fn SetAngMom_rw * @brief Set angular momentum of reaction wheel in the body fixed frame [Nms] */ - inline void SetAngMom_rw(const libra::Vector<3> set) { h_rw_b_Nms_ = set; } + inline void SetAngMom_rw(const libra::Vector<3> h_rw_b_Nms) { h_rw_b_Nms_ = h_rw_b_Nms; } /** * @fn SetInertiaTensor * @brief Set inertia tensor of the spacecraft [kg m2] */ - inline void SetInertiaTensor(const Matrix<3, 3>& set) { - inertia_tensor_kgm2_ = set; + inline void SetInertiaTensor(const Matrix<3, 3>& inertia_tensor_kgm2) { + inertia_tensor_kgm2_ = inertia_tensor_kgm2; inv_inertia_tensor_ = libra::invert(inertia_tensor_kgm2_); } From b2c7c47917abac514847df2ee54cd116a6699327 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:45:49 +0100 Subject: [PATCH 11/57] Remove unnecessary public functions --- src/dynamics/attitude/attitude.hpp | 25 ------------------------- 1 file changed, 25 deletions(-) diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index 8dd2bfcfe..9bdac75aa 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -61,18 +61,8 @@ class Attitude : public ILoggable, public SimulationObject { * @brief Return inertia tensor [kg m^2] */ inline libra::Matrix<3, 3> GetInertiaTensor() const { return inertia_tensor_kgm2_; } - /** - * @fn GetInvInertiaTensor - * @brief Return inverse matrix of inertia tensor - */ - inline libra::Matrix<3, 3> GetInvInertiaTensor() const { return inv_inertia_tensor_; } // Setter - /** - * @fn SetPropStep - * @brief Set propagation step [sec] - */ - inline void SetPropStep(double prop_step_s) { prop_step_s_ = prop_step_s; } /** * @fn SetOmega_b * @brief Set angular velocity of the body fixed frame with respect to the inertial frame [rad/s] @@ -83,12 +73,6 @@ class Attitude : public ILoggable, public SimulationObject { * @brief Set attitude quaternion from the inertial frame to the body frame */ inline void SetQuaternion_i2b(const libra::Quaternion quaternion_i2b) { quaternion_i2b_ = quaternion_i2b; } - /** - * @fn AddQuaternionOffset - * @brief Add quaternion offset rotation - */ - inline void AddQuaternionOffset(const libra::Quaternion offset) { quaternion_i2b_ = quaternion_i2b_ * offset; } - /** * @fn SetTorque_b * @brief Set torque acting on the spacecraft on the body fixed frame [Nm] @@ -99,20 +83,11 @@ class Attitude : public ILoggable, public SimulationObject { * @brief Add torque acting on the spacecraft on the body fixed frame [Nm] */ inline void AddTorque_b_Nm(const libra::Vector<3> torque_b_Nm) { torque_b_Nm_ += torque_b_Nm; } - /** * @fn SetAngMom_rw * @brief Set angular momentum of reaction wheel in the body fixed frame [Nms] */ inline void SetAngMom_rw(const libra::Vector<3> h_rw_b_Nms) { h_rw_b_Nms_ = h_rw_b_Nms; } - /** - * @fn SetInertiaTensor - * @brief Set inertia tensor of the spacecraft [kg m2] - */ - inline void SetInertiaTensor(const Matrix<3, 3>& inertia_tensor_kgm2) { - inertia_tensor_kgm2_ = inertia_tensor_kgm2; - inv_inertia_tensor_ = libra::invert(inertia_tensor_kgm2_); - } /** * @fn Propagate From b9843d11be0ee44f1e16888d755e3c58409cf0c3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:51:25 +0100 Subject: [PATCH 12/57] Fix private variables --- src/dynamics/attitude/attitude.cpp | 30 ++++++++-------- src/dynamics/attitude/attitude.hpp | 34 +++++++++---------- src/dynamics/attitude/attitude_rk4.cpp | 24 ++++++------- src/dynamics/attitude/controlled_attitude.cpp | 8 ++--- 4 files changed, 48 insertions(+), 48 deletions(-) diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index e3d396cef..be9649778 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -7,15 +7,15 @@ #include Attitude::Attitude(const std::string& sim_object_name) : SimulationObject(sim_object_name) { - omega_b_rad_s_ = libra::Vector<3>(0.0); + angular_velocity_b_rad_s_ = libra::Vector<3>(0.0); quaternion_i2b_ = libra::Quaternion(0.0, 0.0, 0.0, 1.0); torque_b_Nm_ = libra::Vector<3>(0.0); - h_sc_b_Nms_ = libra::Vector<3>(0.0); - h_rw_b_Nms_ = libra::Vector<3>(0.0); - h_total_b_Nms_ = libra::Vector<3>(0.0); - h_total_i_Nms_ = libra::Vector<3>(0.0); - h_total_Nms_ = 0.0; - k_sc_J_ = 0.0; + angular_momentum_spacecraft_b_Nms_ = libra::Vector<3>(0.0); + angular_momentum_reaction_wheel_b_Nms_ = libra::Vector<3>(0.0); + angular_momentum_total_b_Nms_ = libra::Vector<3>(0.0); + angular_momentum_total_i_Nms_ = libra::Vector<3>(0.0); + angular_momentum_total_Nms_ = 0.0; + kinetic_energy_J_ = 0.0; } std::string Attitude::GetLogHeader() const { @@ -33,11 +33,11 @@ std::string Attitude::GetLogHeader() const { std::string Attitude::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(omega_b_rad_s_); + str_tmp += WriteVector(angular_velocity_b_rad_s_); str_tmp += WriteQuaternion(quaternion_i2b_); str_tmp += WriteVector(torque_b_Nm_); - str_tmp += WriteScalar(h_total_Nms_); - str_tmp += WriteScalar(k_sc_J_); + str_tmp += WriteScalar(angular_momentum_total_Nms_); + str_tmp += WriteScalar(kinetic_energy_J_); return str_tmp; } @@ -45,11 +45,11 @@ std::string Attitude::GetLogValue() const { void Attitude::SetParameters(const MCSimExecutor& mc_sim) { GetInitParameterQuaternion(mc_sim, "Q_i2b", quaternion_i2b_); } void Attitude::CalcAngularMomentum(void) { - h_sc_b_Nms_ = inertia_tensor_kgm2_ * omega_b_rad_s_; - h_total_b_Nms_ = h_rw_b_Nms_ + h_sc_b_Nms_; + 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_; libra::Quaternion q_b2i = quaternion_i2b_.conjugate(); - h_total_i_Nms_ = q_b2i.frame_conv(h_total_b_Nms_); - h_total_Nms_ = norm(h_total_i_Nms_); + angular_momentum_total_i_Nms_ = q_b2i.frame_conv(angular_momentum_total_b_Nms_); + angular_momentum_total_Nms_ = norm(angular_momentum_total_i_Nms_); - k_sc_J_ = 0.5 * libra::inner_product(h_sc_b_Nms_, omega_b_rad_s_); + kinetic_energy_J_ = 0.5 * libra::inner_product(angular_momentum_spacecraft_b_Nms_, angular_velocity_b_rad_s_); } diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index 9bdac75aa..6768d7807 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -35,12 +35,12 @@ class Attitude : public ILoggable, public SimulationObject { * @fn GetPropStep * @brief Return propagation step [sec] */ - inline double GetPropStep() const { return prop_step_s_; } + inline double GetPropStep() const { return propagation_step_s_; } /** * @fn GetOmega_b * @brief Return angular velocity of spacecraft body-fixed frame with respect to the inertial frame [rad/s] */ - inline libra::Vector<3> GetOmega_b() const { return omega_b_rad_s_; } + inline libra::Vector<3> GetOmega_b() const { return angular_velocity_b_rad_s_; } /** * @fn GetQuaternion_i2b * @brief Return attitude quaternion from the inertial frame to the body fixed frame @@ -50,12 +50,12 @@ class Attitude : public ILoggable, public SimulationObject { * @fn GetTotalAngMomNorm * @brief Return norm of total angular momentum of the spacecraft [Nms] */ - inline double GetTotalAngMomNorm() const { return libra::norm(h_total_b_Nms_); } + inline double GetTotalAngMomNorm() const { return libra::norm(angular_momentum_total_b_Nms_); } /** * @fn GetEnergy * @brief Return rotational Kinetic Energy of Spacecraft [J] */ - inline double GetEnergy() const { return k_sc_J_; } + inline double GetEnergy() const { return kinetic_energy_J_; } /** * @fn GetInertiaTensor * @brief Return inertia tensor [kg m^2] @@ -67,7 +67,7 @@ class Attitude : public ILoggable, public SimulationObject { * @fn SetOmega_b * @brief Set angular velocity of the body fixed frame with respect to the inertial frame [rad/s] */ - inline void SetOmega_b(const libra::Vector<3> omega_b_rad_s) { omega_b_rad_s_ = omega_b_rad_s; } + inline void SetOmega_b(const libra::Vector<3> omega_b_rad_s) { angular_velocity_b_rad_s_ = omega_b_rad_s; } /** * @fn SetQuaternion_i2b * @brief Set attitude quaternion from the inertial frame to the body frame @@ -87,7 +87,7 @@ class Attitude : public ILoggable, public SimulationObject { * @fn SetAngMom_rw * @brief Set angular momentum of reaction wheel in the body fixed frame [Nms] */ - inline void SetAngMom_rw(const libra::Vector<3> h_rw_b_Nms) { h_rw_b_Nms_ = h_rw_b_Nms; } + inline void SetAngMom_rw(const libra::Vector<3> angular_momentum_rw_b_Nms) { angular_momentum_reaction_wheel_b_Nms_ = angular_momentum_rw_b_Nms; } /** * @fn Propagate @@ -112,21 +112,21 @@ class Attitude : public ILoggable, public SimulationObject { virtual void SetParameters(const MCSimExecutor& mc_sim); protected: - bool is_calc_enabled_ = true; //!< Calculation flag - double prop_step_s_; //!< Propagation step [sec] - libra::Vector<3> omega_b_rad_s_; //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s] - libra::Quaternion quaternion_i2b_; //!< Attitude quaternion from the inertial frame to the body fixed frame - libra::Vector<3> torque_b_Nm_; //!< Torque in the body fixed frame [Nm] + bool is_calc_enabled_ = true; //!< Calculation flag + double propagation_step_s_; //!< Propagation step [sec] + libra::Vector<3> angular_velocity_b_rad_s_; //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s] + libra::Quaternion quaternion_i2b_; //!< Attitude quaternion from the inertial frame to the body fixed frame + libra::Vector<3> torque_b_Nm_; //!< Torque in the body fixed frame [Nm] libra::Matrix<3, 3> inertia_tensor_kgm2_; //!< Inertia tensor of the spacecraft [kg m^2] TODO: Move to Structure libra::Matrix<3, 3> inv_inertia_tensor_; //!< Inverse matrix of the inertia tensor - libra::Vector<3> h_sc_b_Nms_; //!< Angular momentum of spacecraft in the body fixed frame [Nms] - libra::Vector<3> h_rw_b_Nms_; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] - libra::Vector<3> h_total_b_Nms_; //!< Total angular momentum of spacecraft in the body fixed frame [Nms] - libra::Vector<3> h_total_i_Nms_; //!< Total angular momentum of spacecraft in the inertial frame [Nms] - double h_total_Nms_; //!< Norm of total angular momentum [Nms] - double k_sc_J_; //!< Rotational Kinetic Energy of Spacecraft [J] + libra::Vector<3> angular_momentum_spacecraft_b_Nms_; //!< Angular momentum of spacecraft in the body fixed frame [Nms] + libra::Vector<3> angular_momentum_reaction_wheel_b_Nms_; //!< Angular momentum of reaction wheel in the body fixed frame [Nms] + libra::Vector<3> angular_momentum_total_b_Nms_; //!< Total angular momentum of spacecraft in the body fixed frame [Nms] + libra::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] /** * @fn CalcAngularMomentum diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index a5e14db06..e6d57bda3 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -14,14 +14,14 @@ using namespace std; AttitudeRK4::AttitudeRK4(const Vector<3>& omega_b_ini, const Quaternion& quaternion_i2b_ini, const Matrix<3, 3>& InertiaTensor_ini, const Vector<3>& torque_b_ini, const double prop_step_ini, const std::string& sim_object_name) : Attitude(sim_object_name) { - omega_b_rad_s_ = omega_b_ini; + angular_velocity_b_rad_s_ = omega_b_ini; quaternion_i2b_ = quaternion_i2b_ini; torque_b_Nm_ = torque_b_ini; inertia_tensor_kgm2_ = InertiaTensor_ini; - prop_step_s_ = prop_step_ini; + propagation_step_s_ = prop_step_ini; prop_time_s_ = 0.0; inv_inertia_tensor_ = invert(inertia_tensor_kgm2_); - h_rw_b_Nms_ = libra::Vector<3>(0.0); + angular_momentum_reaction_wheel_b_Nms_ = libra::Vector<3>(0.0); CalcAngularMomentum(); } @@ -29,20 +29,20 @@ AttitudeRK4::~AttitudeRK4() {} void AttitudeRK4::SetParameters(const MCSimExecutor& mc_sim) { Attitude::SetParameters(mc_sim); - GetInitParameterVec(mc_sim, "Omega_b", omega_b_rad_s_); + GetInitParameterVec(mc_sim, "Omega_b", angular_velocity_b_rad_s_); // TODO: Consider the following calculation is needed here? prop_time_s_ = 0.0; inv_inertia_tensor_ = libra::invert(inertia_tensor_kgm2_); - h_rw_b_Nms_ = Vector<3>(0.0); //!< Consider how to handle this variable + angular_momentum_reaction_wheel_b_Nms_ = Vector<3>(0.0); //!< Consider how to handle this variable CalcAngularMomentum(); } void AttitudeRK4::Propagate(const double endtime_s) { if (!is_calc_enabled_) return; - while (endtime_s - prop_time_s_ - prop_step_s_ > 1.0e-6) { - RungeOneStep(prop_time_s_, prop_step_s_); - prop_time_s_ += prop_step_s_; + while (endtime_s - prop_time_s_ - propagation_step_s_ > 1.0e-6) { + RungeOneStep(prop_time_s_, propagation_step_s_); + prop_time_s_ += propagation_step_s_; } RungeOneStep(prop_time_s_, endtime_s - prop_time_s_); prop_time_s_ = endtime_s; @@ -82,8 +82,8 @@ Vector<7> AttitudeRK4::DynamicsKinematics(Vector<7> x, double t) { for (int i = 0; i < 3; i++) { omega_b[i] = x[i]; } - h_total_b_Nms_ = (inertia_tensor_kgm2_ * omega_b) + h_rw_b_Nms_; - Vector<3> rhs = inv_inertia_tensor_ * (torque_b_Nm_ - libra::outer_product(omega_b, h_total_b_Nms_)); + angular_momentum_total_b_Nms_ = (inertia_tensor_kgm2_ * omega_b) + angular_momentum_reaction_wheel_b_Nms_; + Vector<3> rhs = inv_inertia_tensor_ * (torque_b_Nm_ - libra::outer_product(omega_b, angular_momentum_total_b_Nms_)); for (int i = 0; i < 3; ++i) { dxdt[i] = rhs[i]; @@ -106,7 +106,7 @@ Vector<7> AttitudeRK4::DynamicsKinematics(Vector<7> x, double t) { void AttitudeRK4::RungeOneStep(double t, double dt) { Vector<7> x; for (int i = 0; i < 3; i++) { - x[i] = omega_b_rad_s_[i]; + x[i] = angular_velocity_b_rad_s_[i]; } for (int i = 0; i < 4; i++) { x[i + 3] = quaternion_i2b_[i]; @@ -129,7 +129,7 @@ void AttitudeRK4::RungeOneStep(double t, double dt) { Vector<7> next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4); for (int i = 0; i < 3; i++) { - omega_b_rad_s_[i] = next_x[i]; + angular_velocity_b_rad_s_[i] = next_x[i]; } for (int i = 0; i < 4; i++) { quaternion_i2b_[i] = next_x[i + 3]; diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index 0c8674bcd..be150a3d4 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -151,12 +151,12 @@ void ControlledAttitude::CalcAngularVelocity(const double current_time_s) { libra::Vector<3> angular_acc_b_rad_s2_; for (int i = 0; i < 3; i++) { - omega_b_rad_s_[i] = q_diff[i]; - angular_acc_b_rad_s2_[i] = (prev_omega_b_rad_s_[i] - omega_b_rad_s_[i]) / time_diff_sec; + angular_velocity_b_rad_s_[i] = q_diff[i]; + angular_acc_b_rad_s2_[i] = (prev_omega_b_rad_s_[i] - angular_velocity_b_rad_s_[i]) / time_diff_sec; } controlled_torque_b_Nm = inv_inertia_tensor_ * angular_acc_b_rad_s2_; } else { - omega_b_rad_s_ = libra::Vector<3>(0.0); + angular_velocity_b_rad_s_ = libra::Vector<3>(0.0); controlled_torque_b_Nm = libra::Vector<3>(0.0); } // Add torque with disturbances @@ -164,5 +164,5 @@ void ControlledAttitude::CalcAngularVelocity(const double current_time_s) { // save previous values previous_calc_time_s_ = current_time_s; prev_quaternion_i2b_ = quaternion_i2b_; - prev_omega_b_rad_s_ = omega_b_rad_s_; + prev_omega_b_rad_s_ = angular_velocity_b_rad_s_; } From 40dc7404b893a553597a9df773d64321310d291e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 16:58:20 +0100 Subject: [PATCH 13/57] Fix private functions --- src/dynamics/attitude/attitude_rk4.cpp | 70 +++++++++++++------------- src/dynamics/attitude/attitude_rk4.hpp | 20 ++++---- 2 files changed, 45 insertions(+), 45 deletions(-) diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index e6d57bda3..0dbd4b41b 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -19,7 +19,7 @@ AttitudeRK4::AttitudeRK4(const Vector<3>& omega_b_ini, const Quaternion& quatern torque_b_Nm_ = torque_b_ini; inertia_tensor_kgm2_ = InertiaTensor_ini; propagation_step_s_ = prop_step_ini; - prop_time_s_ = 0.0; + current_propagation_time_s_ = 0.0; inv_inertia_tensor_ = invert(inertia_tensor_kgm2_); angular_momentum_reaction_wheel_b_Nms_ = libra::Vector<3>(0.0); CalcAngularMomentum(); @@ -32,7 +32,7 @@ void AttitudeRK4::SetParameters(const MCSimExecutor& mc_sim) { GetInitParameterVec(mc_sim, "Omega_b", angular_velocity_b_rad_s_); // TODO: Consider the following calculation is needed here? - prop_time_s_ = 0.0; + current_propagation_time_s_ = 0.0; inv_inertia_tensor_ = libra::invert(inertia_tensor_kgm2_); angular_momentum_reaction_wheel_b_Nms_ = Vector<3>(0.0); //!< Consider how to handle this variable CalcAngularMomentum(); @@ -40,40 +40,40 @@ void AttitudeRK4::SetParameters(const MCSimExecutor& mc_sim) { void AttitudeRK4::Propagate(const double endtime_s) { if (!is_calc_enabled_) return; - while (endtime_s - prop_time_s_ - propagation_step_s_ > 1.0e-6) { - RungeOneStep(prop_time_s_, propagation_step_s_); - prop_time_s_ += propagation_step_s_; + while (endtime_s - current_propagation_time_s_ - propagation_step_s_ > 1.0e-6) { + RungeKuttaOneStep(current_propagation_time_s_, propagation_step_s_); + current_propagation_time_s_ += propagation_step_s_; } - RungeOneStep(prop_time_s_, endtime_s - prop_time_s_); - prop_time_s_ = endtime_s; + RungeKuttaOneStep(current_propagation_time_s_, endtime_s - current_propagation_time_s_); + current_propagation_time_s_ = endtime_s; CalcAngularMomentum(); } -Matrix<4, 4> AttitudeRK4::Omega4Kinematics(Vector<3> omega) { - Matrix<4, 4> OMEGA; - - OMEGA[0][0] = 0.0f; - OMEGA[0][1] = omega[2]; - OMEGA[0][2] = -omega[1]; - OMEGA[0][3] = omega[0]; - OMEGA[1][0] = -omega[2]; - OMEGA[1][1] = 0.0f; - OMEGA[1][2] = omega[0]; - OMEGA[1][3] = omega[1]; - OMEGA[2][0] = omega[1]; - OMEGA[2][1] = -omega[0]; - OMEGA[2][2] = 0.0f; - OMEGA[2][3] = omega[2]; - OMEGA[3][0] = -omega[0]; - OMEGA[3][1] = -omega[1]; - OMEGA[3][2] = -omega[2]; - OMEGA[3][3] = 0.0f; - - return OMEGA; +Matrix<4, 4> AttitudeRK4::CalcAngularVelocityMatrix(Vector<3> angular_velocity_b_rad_s) { + Matrix<4, 4> angular_velocity_matrix; + + angular_velocity_matrix[0][0] = 0.0f; + angular_velocity_matrix[0][1] = angular_velocity_b_rad_s[2]; + angular_velocity_matrix[0][2] = -angular_velocity_b_rad_s[1]; + angular_velocity_matrix[0][3] = angular_velocity_b_rad_s[0]; + angular_velocity_matrix[1][0] = -angular_velocity_b_rad_s[2]; + angular_velocity_matrix[1][1] = 0.0f; + angular_velocity_matrix[1][2] = angular_velocity_b_rad_s[0]; + angular_velocity_matrix[1][3] = angular_velocity_b_rad_s[1]; + angular_velocity_matrix[2][0] = angular_velocity_b_rad_s[1]; + angular_velocity_matrix[2][1] = -angular_velocity_b_rad_s[0]; + angular_velocity_matrix[2][2] = 0.0f; + angular_velocity_matrix[2][3] = angular_velocity_b_rad_s[2]; + angular_velocity_matrix[3][0] = -angular_velocity_b_rad_s[0]; + angular_velocity_matrix[3][1] = -angular_velocity_b_rad_s[1]; + angular_velocity_matrix[3][2] = -angular_velocity_b_rad_s[2]; + angular_velocity_matrix[3][3] = 0.0f; + + return angular_velocity_matrix; } -Vector<7> AttitudeRK4::DynamicsKinematics(Vector<7> x, double t) { +Vector<7> AttitudeRK4::AttitudeDynamicsAndKinematics(Vector<7> x, double t) { UNUSED(t); Vector<7> dxdt; @@ -94,7 +94,7 @@ Vector<7> AttitudeRK4::DynamicsKinematics(Vector<7> x, double t) { quaternion_i2b[i] = x[i + 3]; } - Vector<4> d_quaternion = 0.5 * Omega4Kinematics(omega_b) * quaternion_i2b; + Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b) * quaternion_i2b; for (int i = 0; i < 4; i++) { dxdt[i + 3] = d_quaternion[i]; @@ -103,7 +103,7 @@ Vector<7> AttitudeRK4::DynamicsKinematics(Vector<7> x, double t) { return dxdt; } -void AttitudeRK4::RungeOneStep(double t, double dt) { +void AttitudeRK4::RungeKuttaOneStep(double t, double dt) { Vector<7> x; for (int i = 0; i < 3; i++) { x[i] = angular_velocity_b_rad_s_[i]; @@ -115,16 +115,16 @@ void AttitudeRK4::RungeOneStep(double t, double dt) { Vector<7> k1, k2, k3, k4; Vector<7> xk2, xk3, xk4; - k1 = DynamicsKinematics(x, t); + k1 = AttitudeDynamicsAndKinematics(x, t); xk2 = x + (dt / 2.0) * k1; - k2 = DynamicsKinematics(xk2, (t + dt / 2.0)); + k2 = AttitudeDynamicsAndKinematics(xk2, (t + dt / 2.0)); xk3 = x + (dt / 2.0) * k2; - k3 = DynamicsKinematics(xk3, (t + dt / 2.0)); + k3 = AttitudeDynamicsAndKinematics(xk3, (t + dt / 2.0)); xk4 = x + dt * k3; - k4 = DynamicsKinematics(xk4, (t + dt)); + k4 = AttitudeDynamicsAndKinematics(xk4, (t + dt)); Vector<7> next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4); diff --git a/src/dynamics/attitude/attitude_rk4.hpp b/src/dynamics/attitude/attitude_rk4.hpp index 88600bf1d..54d582b0c 100644 --- a/src/dynamics/attitude/attitude_rk4.hpp +++ b/src/dynamics/attitude/attitude_rk4.hpp @@ -36,13 +36,13 @@ class AttitudeRK4 : public Attitude { * @fn GetPropTime * @brief Return propagation time (current time) [sec] */ - inline double GetPropTime() const { return prop_time_s_; } + inline double GetPropTime() const { return current_propagation_time_s_; } /** * @fn SetTime * @brief Set propagation time (current time) [sec] */ - inline void SetTime(double set) { prop_time_s_ = set; } + inline void SetTime(double set) { current_propagation_time_s_ = set; } /** * @fn Propagate @@ -59,28 +59,28 @@ class AttitudeRK4 : public Attitude { virtual void SetParameters(const MCSimExecutor& mc_sim); private: - double prop_time_s_; //!< current time [sec] + double current_propagation_time_s_; //!< current time [sec] /** - * @fn Omega4Kinematics + * @fn CalcAngularVelocityMatrix * @brief Generate angular velocity matrix for kinematics calculation - * @param [in] omega: Angular velocity [rad/s] + * @param [in] angular_velocity_b_rad_s: Angular velocity [rad/s] */ - Matrix<4, 4> Omega4Kinematics(Vector<3> omega); + Matrix<4, 4> CalcAngularVelocityMatrix(Vector<3> angular_velocity_b_rad_s); /** - * @fn DynamicsKinematics + * @fn AttitudeDynamicsAndKinematics * @brief Dynamics equation with kinematics * @param [in] x: State vector (angular velocity and quaternion) * @param [in] t: Unused TODO: remove? */ - Vector<7> DynamicsKinematics(Vector<7> x, double t); + Vector<7> AttitudeDynamicsAndKinematics(Vector<7> x, double t); /** - * @fn RungeOneStep + * @fn RungeKuttaOneStep * @brief Equation for one step of Runge-Kutta method * @param [in] t: Unused TODO: remove? * @param [in] dt: Step width [sec] */ - void RungeOneStep(double t, double dt); + void RungeKuttaOneStep(double t, double dt); }; #endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_RK4_HPP_ From 1b9e32ea59451b07e1bdcf22113a04256b229063 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 18:44:59 +0100 Subject: [PATCH 14/57] Fix function argument --- src/dynamics/attitude/attitude.cpp | 4 +-- src/dynamics/attitude/attitude.hpp | 10 +++---- src/dynamics/attitude/attitude_rk4.cpp | 30 +++++++++---------- src/dynamics/attitude/attitude_rk4.hpp | 24 +++++++-------- src/dynamics/attitude/controlled_attitude.cpp | 9 +++--- src/dynamics/attitude/controlled_attitude.hpp | 8 ++--- 6 files changed, 43 insertions(+), 42 deletions(-) diff --git a/src/dynamics/attitude/attitude.cpp b/src/dynamics/attitude/attitude.cpp index be9649778..96311b4cc 100644 --- a/src/dynamics/attitude/attitude.cpp +++ b/src/dynamics/attitude/attitude.cpp @@ -6,7 +6,7 @@ #include -Attitude::Attitude(const std::string& sim_object_name) : SimulationObject(sim_object_name) { +Attitude::Attitude(const std::string& simulation_object_name) : SimulationObject(simulation_object_name) { angular_velocity_b_rad_s_ = libra::Vector<3>(0.0); quaternion_i2b_ = libra::Quaternion(0.0, 0.0, 0.0, 1.0); torque_b_Nm_ = libra::Vector<3>(0.0); @@ -42,7 +42,7 @@ std::string Attitude::GetLogValue() const { return str_tmp; } -void Attitude::SetParameters(const MCSimExecutor& mc_sim) { GetInitParameterQuaternion(mc_sim, "Q_i2b", quaternion_i2b_); } +void Attitude::SetParameters(const MCSimExecutor& mc_simulator) { GetInitParameterQuaternion(mc_simulator, "Q_i2b", quaternion_i2b_); } void Attitude::CalcAngularMomentum(void) { angular_momentum_spacecraft_b_Nms_ = inertia_tensor_kgm2_ * angular_velocity_b_rad_s_; diff --git a/src/dynamics/attitude/attitude.hpp b/src/dynamics/attitude/attitude.hpp index 6768d7807..b25997043 100644 --- a/src/dynamics/attitude/attitude.hpp +++ b/src/dynamics/attitude/attitude.hpp @@ -21,9 +21,9 @@ class Attitude : public ILoggable, public SimulationObject { /** * @fn Attitude * @brief Constructor - * @param [in] sim_object_name: Simulation object name for Monte-Carlo simulation + * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - Attitude(const std::string& sim_object_name = "Attitude"); + Attitude(const std::string& simulation_object_name = "Attitude"); /** * @fn ~Attitude * @brief Destructor @@ -92,9 +92,9 @@ class Attitude : public ILoggable, public SimulationObject { /** * @fn Propagate * @brief Pure virtual function of attitude propagation - * @param [in] endtime_s: Propagation endtime [sec] + * @param [in] end_time_s: Propagation endtime [sec] */ - virtual void Propagate(const double endtime_s) = 0; + virtual void Propagate(const double end_time_s) = 0; // Override ILoggable /** @@ -109,7 +109,7 @@ class Attitude : public ILoggable, public SimulationObject { virtual std::string GetLogValue() const; // SimulationObject for McSim - virtual void SetParameters(const MCSimExecutor& mc_sim); + virtual void SetParameters(const MCSimExecutor& mc_simulator); protected: bool is_calc_enabled_ = true; //!< Calculation flag diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index 0dbd4b41b..fbaa45e6f 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -11,14 +11,14 @@ using namespace std; #include #include -AttitudeRK4::AttitudeRK4(const Vector<3>& omega_b_ini, const Quaternion& quaternion_i2b_ini, const Matrix<3, 3>& InertiaTensor_ini, - const Vector<3>& torque_b_ini, const double prop_step_ini, const std::string& sim_object_name) - : Attitude(sim_object_name) { - angular_velocity_b_rad_s_ = omega_b_ini; - quaternion_i2b_ = quaternion_i2b_ini; - torque_b_Nm_ = torque_b_ini; - inertia_tensor_kgm2_ = InertiaTensor_ini; - propagation_step_s_ = prop_step_ini; +AttitudeRK4::AttitudeRK4(const Vector<3>& angular_velocity_b_rad_s, const Quaternion& quaternion_i2b, const Matrix<3, 3>& inertia_tensor_kgm2, + const Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name) + : Attitude(simulation_object_name) { + angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; + quaternion_i2b_ = quaternion_i2b; + torque_b_Nm_ = torque_b_Nm; + inertia_tensor_kgm2_ = inertia_tensor_kgm2; + propagation_step_s_ = propagation_step_s; current_propagation_time_s_ = 0.0; inv_inertia_tensor_ = invert(inertia_tensor_kgm2_); angular_momentum_reaction_wheel_b_Nms_ = libra::Vector<3>(0.0); @@ -27,9 +27,9 @@ AttitudeRK4::AttitudeRK4(const Vector<3>& omega_b_ini, const Quaternion& quatern AttitudeRK4::~AttitudeRK4() {} -void AttitudeRK4::SetParameters(const MCSimExecutor& mc_sim) { - Attitude::SetParameters(mc_sim); - GetInitParameterVec(mc_sim, "Omega_b", angular_velocity_b_rad_s_); +void AttitudeRK4::SetParameters(const MCSimExecutor& mc_simulator) { + Attitude::SetParameters(mc_simulator); + GetInitParameterVec(mc_simulator, "Omega_b", angular_velocity_b_rad_s_); // TODO: Consider the following calculation is needed here? current_propagation_time_s_ = 0.0; @@ -38,14 +38,14 @@ void AttitudeRK4::SetParameters(const MCSimExecutor& mc_sim) { CalcAngularMomentum(); } -void AttitudeRK4::Propagate(const double endtime_s) { +void AttitudeRK4::Propagate(const double end_time_s) { if (!is_calc_enabled_) return; - while (endtime_s - current_propagation_time_s_ - propagation_step_s_ > 1.0e-6) { + while (end_time_s - current_propagation_time_s_ - propagation_step_s_ > 1.0e-6) { RungeKuttaOneStep(current_propagation_time_s_, propagation_step_s_); current_propagation_time_s_ += propagation_step_s_; } - RungeKuttaOneStep(current_propagation_time_s_, endtime_s - current_propagation_time_s_); - current_propagation_time_s_ = endtime_s; + RungeKuttaOneStep(current_propagation_time_s_, end_time_s - current_propagation_time_s_); + current_propagation_time_s_ = end_time_s; CalcAngularMomentum(); } diff --git a/src/dynamics/attitude/attitude_rk4.hpp b/src/dynamics/attitude/attitude_rk4.hpp index 54d582b0c..aa5dd60af 100644 --- a/src/dynamics/attitude/attitude_rk4.hpp +++ b/src/dynamics/attitude/attitude_rk4.hpp @@ -17,15 +17,15 @@ class AttitudeRK4 : public Attitude { /** * @fn AttitudeRK4 * @brief Constructor - * @param [in] omega_b_ini: Initial value of spacecraft angular velocity of the body fixed frame [rad/s] - * @param [in] quaternion_i2b_ini: Initial value of attitude quaternion from the inertial frame to the body fixed frame - * @param [in] InertiaTensor_ini: Initial value of inertia tensor of the spacecraft [kg m^2] - * @param [in] torque_b_ini: Initial torque acting on the spacecraft in the body fixed frame [Nm] - * @param [in] prop_step_ini: Initial value of propagation step width [sec] - * @param [in] sim_object_name: Simulation object name for Monte-Carlo simulation + * @param [in] angular_velocity_b_rad_s: Initial value of spacecraft angular velocity of the body fixed frame [rad/s] + * @param [in] quaternion_i2b: Initial value of attitude quaternion from the inertial frame to the body fixed frame + * @param [in] inertia_tensor_kgm2: Initial value of inertia tensor of the spacecraft [kg m^2] + * @param [in] torque_b_Nm: Initial torque acting on the spacecraft in the body fixed frame [Nm] + * @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 Vector<3>& omega_b_ini, const Quaternion& quaternion_i2b_ini, const Matrix<3, 3>& InertiaTensor_ini, - const Vector<3>& torque_b_ini, const double prop_step_ini, const std::string& sim_object_name = "Attitude"); + AttitudeRK4(const Vector<3>& angular_velocity_b_rad_s, const Quaternion& quaternion_i2b, const Matrix<3, 3>& inertia_tensor_kgm2, + const Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name = "Attitude"); /** * @fn ~AttitudeRK4 * @brief Destructor @@ -47,16 +47,16 @@ class AttitudeRK4 : public Attitude { /** * @fn Propagate * @brief Attitude propagation - * @param [in] endtime_s: Propagation endtime [sec] + * @param [in] end_time_s: Propagation endtime [sec] */ - virtual void Propagate(const double endtime_s); + virtual void Propagate(const double end_time_s); /** * @fn SetParameters * @brief Set parameters for Monte-Carlo simulation - * @param [in] mc_sim: Monte-Carlo simulation executor + * @param [in] mc_simulator: Monte-Carlo simulation executor */ - virtual void SetParameters(const MCSimExecutor& mc_sim); + virtual void SetParameters(const MCSimExecutor& mc_simulator); private: double current_propagation_time_s_; //!< current time [sec] diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index be150a3d4..0ac713c42 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -14,8 +14,9 @@ using namespace std; ControlledAttitude::ControlledAttitude(const AttCtrlMode main_mode, const AttCtrlMode sub_mode, const Quaternion quaternion_i2b, const Vector<3> pointing_t_b, const Vector<3> pointing_sub_t_b, const Matrix<3, 3>& inertia_tensor_kgm2, - const LocalCelestialInformation* local_celes_info, const Orbit* orbit, const std::string& sim_object_name) - : Attitude(sim_object_name), + const LocalCelestialInformation* local_celes_info, const Orbit* orbit, + const std::string& simulation_object_name) + : Attitude(simulation_object_name), main_mode_(main_mode), sub_mode_(sub_mode), pointing_t_b_(pointing_t_b), @@ -58,7 +59,7 @@ void ControlledAttitude::Initialize(void) { return; } -void ControlledAttitude::Propagate(const double endtime_s) { +void ControlledAttitude::Propagate(const double end_time_s) { Vector<3> main_direction_i, sub_direction_i; if (!is_calc_enabled_) return; @@ -74,7 +75,7 @@ void ControlledAttitude::Propagate(const double endtime_s) { // Calc attitude PointingCtrl(main_direction_i, sub_direction_i); // Calc angular velocity - CalcAngularVelocity(endtime_s); + CalcAngularVelocity(end_time_s); return; } diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index ec02f6122..04bd7a6f7 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -50,11 +50,11 @@ class ControlledAttitude : public Attitude { * @param [in] inertia_tensor_kgm2: Inertia tensor of the spacecraft [kg m^2] * @param [in] local_celes_info: Local celestial information * @param [in] orbit: Orbit - * @param [in] sim_object_name: Simulation object name for Monte-Carlo simulation + * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ ControlledAttitude(const AttCtrlMode main_mode, const AttCtrlMode sub_mode, const Quaternion quaternion_i2b, const Vector<3> pointing_t_b, const Vector<3> pointing_sub_t_b, const Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celes_info, - const Orbit* orbit, const std::string& sim_object_name = "Attitude"); + const Orbit* orbit, const std::string& simulation_object_name = "Attitude"); /** * @fn ~ControlledAttitude * @brief Destructor @@ -91,9 +91,9 @@ class ControlledAttitude : public Attitude { /** * @fn Propagate * @brief Attitude propagation - * @param [in] endtime_s: Propagation endtime [sec] + * @param [in] end_time_s: Propagation endtime [sec] */ - virtual void Propagate(const double endtime_s); + virtual void Propagate(const double end_time_s); private: AttCtrlMode main_mode_; //!< Main control mode From 9800bf07a1dc0c388b9318bcffeb96d70c332419 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 18:45:41 +0100 Subject: [PATCH 15/57] Remove unused public functions --- src/dynamics/attitude/attitude_rk4.hpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/dynamics/attitude/attitude_rk4.hpp b/src/dynamics/attitude/attitude_rk4.hpp index aa5dd60af..5cf7e9396 100644 --- a/src/dynamics/attitude/attitude_rk4.hpp +++ b/src/dynamics/attitude/attitude_rk4.hpp @@ -32,18 +32,6 @@ class AttitudeRK4 : public Attitude { */ ~AttitudeRK4(); - /** - * @fn GetPropTime - * @brief Return propagation time (current time) [sec] - */ - inline double GetPropTime() const { return current_propagation_time_s_; } - - /** - * @fn SetTime - * @brief Set propagation time (current time) [sec] - */ - inline void SetTime(double set) { current_propagation_time_s_ = set; } - /** * @fn Propagate * @brief Attitude propagation From a290f8375694e7a08501cd18c346133951ca24fe Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 18:46:43 +0100 Subject: [PATCH 16/57] Rename AttitudeRK4 to AttitudeRk4 --- src/dynamics/attitude/attitude_rk4.cpp | 14 +++++++------- src/dynamics/attitude/attitude_rk4.hpp | 12 ++++++------ src/dynamics/attitude/initialize_attitude.cpp | 4 ++-- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index fbaa45e6f..465019404 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -11,7 +11,7 @@ using namespace std; #include #include -AttitudeRK4::AttitudeRK4(const Vector<3>& angular_velocity_b_rad_s, const Quaternion& quaternion_i2b, const Matrix<3, 3>& inertia_tensor_kgm2, +AttitudeRk4::AttitudeRk4(const Vector<3>& angular_velocity_b_rad_s, const Quaternion& quaternion_i2b, const Matrix<3, 3>& inertia_tensor_kgm2, const Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name) : Attitude(simulation_object_name) { angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; @@ -25,9 +25,9 @@ AttitudeRK4::AttitudeRK4(const Vector<3>& angular_velocity_b_rad_s, const Quater CalcAngularMomentum(); } -AttitudeRK4::~AttitudeRK4() {} +AttitudeRk4::~AttitudeRk4() {} -void AttitudeRK4::SetParameters(const MCSimExecutor& mc_simulator) { +void AttitudeRk4::SetParameters(const MCSimExecutor& mc_simulator) { Attitude::SetParameters(mc_simulator); GetInitParameterVec(mc_simulator, "Omega_b", angular_velocity_b_rad_s_); @@ -38,7 +38,7 @@ void AttitudeRK4::SetParameters(const MCSimExecutor& mc_simulator) { CalcAngularMomentum(); } -void AttitudeRK4::Propagate(const double end_time_s) { +void AttitudeRk4::Propagate(const double end_time_s) { if (!is_calc_enabled_) return; while (end_time_s - current_propagation_time_s_ - propagation_step_s_ > 1.0e-6) { RungeKuttaOneStep(current_propagation_time_s_, propagation_step_s_); @@ -50,7 +50,7 @@ void AttitudeRK4::Propagate(const double end_time_s) { CalcAngularMomentum(); } -Matrix<4, 4> AttitudeRK4::CalcAngularVelocityMatrix(Vector<3> angular_velocity_b_rad_s) { +Matrix<4, 4> AttitudeRk4::CalcAngularVelocityMatrix(Vector<3> angular_velocity_b_rad_s) { Matrix<4, 4> angular_velocity_matrix; angular_velocity_matrix[0][0] = 0.0f; @@ -73,7 +73,7 @@ Matrix<4, 4> AttitudeRK4::CalcAngularVelocityMatrix(Vector<3> angular_velocity_b return angular_velocity_matrix; } -Vector<7> AttitudeRK4::AttitudeDynamicsAndKinematics(Vector<7> x, double t) { +Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(Vector<7> x, double t) { UNUSED(t); Vector<7> dxdt; @@ -103,7 +103,7 @@ Vector<7> AttitudeRK4::AttitudeDynamicsAndKinematics(Vector<7> x, double t) { return dxdt; } -void AttitudeRK4::RungeKuttaOneStep(double t, double dt) { +void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { Vector<7> x; for (int i = 0; i < 3; i++) { x[i] = angular_velocity_b_rad_s_[i]; diff --git a/src/dynamics/attitude/attitude_rk4.hpp b/src/dynamics/attitude/attitude_rk4.hpp index 5cf7e9396..b7324e9ef 100644 --- a/src/dynamics/attitude/attitude_rk4.hpp +++ b/src/dynamics/attitude/attitude_rk4.hpp @@ -9,13 +9,13 @@ #include "attitude.hpp" /** - * @class AttitudeRK4 + * @class AttitudeRk4 * @brief Class to calculate spacecraft attitude with Runge-Kutta method */ -class AttitudeRK4 : public Attitude { +class AttitudeRk4 : public Attitude { public: /** - * @fn AttitudeRK4 + * @fn AttitudeRk4 * @brief Constructor * @param [in] angular_velocity_b_rad_s: Initial value of spacecraft angular velocity of the body fixed frame [rad/s] * @param [in] quaternion_i2b: Initial value of attitude quaternion from the inertial frame to the body fixed frame @@ -24,13 +24,13 @@ 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 Vector<3>& angular_velocity_b_rad_s, const Quaternion& quaternion_i2b, const Matrix<3, 3>& inertia_tensor_kgm2, + AttitudeRk4(const Vector<3>& angular_velocity_b_rad_s, const Quaternion& quaternion_i2b, const Matrix<3, 3>& inertia_tensor_kgm2, const Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name = "Attitude"); /** - * @fn ~AttitudeRK4 + * @fn ~AttitudeRk4 * @brief Destructor */ - ~AttitudeRK4(); + ~AttitudeRk4(); /** * @fn Propagate diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index 22277a2a6..6448d27d9 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -24,7 +24,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel Vector<3> torque_b; ini_file.ReadVector(section_, "initial_torque_b_Nm", torque_b); - attitude = new AttitudeRK4(omega_b, quaternion_i2b, inertia_tensor, torque_b, step_sec, mc_name); + attitude = new AttitudeRk4(omega_b, quaternion_i2b, inertia_tensor, torque_b, step_sec, mc_name); } else if (propagate_mode == "CONTROLLED") { // Controlled attitude IniAccess ini_file_ca(file_name); @@ -52,7 +52,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel Vector<3> torque_b; ini_file.ReadVector(section_, "initial_torque_b_Nm", torque_b); - attitude = new AttitudeRK4(omega_b, quaternion_i2b, inertia_tensor, torque_b, step_sec, mc_name); + attitude = new AttitudeRk4(omega_b, quaternion_i2b, inertia_tensor, torque_b, step_sec, mc_name); } return attitude; From 308ebf2575524281a286bd2e4dc0983a27495d2c Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 18:54:07 +0100 Subject: [PATCH 17/57] Fix private variables and function argument names --- src/dynamics/attitude/controlled_attitude.cpp | 32 ++++++++-------- src/dynamics/attitude/controlled_attitude.hpp | 37 ++++++++++--------- src/dynamics/attitude/initialize_attitude.cpp | 10 ++--- 3 files changed, 40 insertions(+), 39 deletions(-) diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index 0ac713c42..8a692799a 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -13,15 +13,15 @@ using namespace std; #define THRESHOLD_CA cos(30.0 / 180.0 * libra::pi) // fix me ControlledAttitude::ControlledAttitude(const AttCtrlMode main_mode, const AttCtrlMode sub_mode, const Quaternion quaternion_i2b, - const Vector<3> pointing_t_b, const Vector<3> pointing_sub_t_b, const Matrix<3, 3>& inertia_tensor_kgm2, - const LocalCelestialInformation* local_celes_info, const Orbit* orbit, - const std::string& simulation_object_name) + const Vector<3> main_target_direction_b, const Vector<3> sub_target_direction_b, + const Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, + const Orbit* orbit, const std::string& simulation_object_name) : Attitude(simulation_object_name), main_mode_(main_mode), sub_mode_(sub_mode), - pointing_t_b_(pointing_t_b), - pointing_sub_t_b_(pointing_sub_t_b), - local_celes_info_(local_celes_info), + main_target_direction_b_(main_target_direction_b), + sub_target_direction_b_(sub_target_direction_b), + local_celestial_information_(local_celestial_information), orbit_(orbit) { quaternion_i2b_ = quaternion_i2b; inertia_tensor_kgm2_ = inertia_tensor_kgm2; // FIXME: inertia tensor should be initialized in the Attitude base class @@ -46,9 +46,9 @@ void ControlledAttitude::Initialize(void) { return; } // pointing direction check - normalize(pointing_t_b_); - normalize(pointing_sub_t_b_); - double tmp = inner_product(pointing_t_b_, pointing_sub_t_b_); + normalize(main_target_direction_b_); + normalize(sub_target_direction_b_); + double tmp = inner_product(main_target_direction_b_, sub_target_direction_b_); tmp = std::abs(tmp); if (tmp > THRESHOLD_CA) { cout << "sub target direction should separate from the main target direction. \n"; @@ -82,9 +82,9 @@ void ControlledAttitude::Propagate(const double end_time_s) { Vector<3> ControlledAttitude::CalcTargetDirection(AttCtrlMode mode) { Vector<3> direction; if (mode == SUN_POINTING) { - direction = local_celes_info_->GetPositionFromSpacecraft_i_m("SUN"); + direction = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); } else if (mode == EARTH_CENTER_POINTING) { - direction = local_celes_info_->GetPositionFromSpacecraft_i_m("EARTH"); + direction = local_celestial_information_->GetPositionFromSpacecraft_i_m("EARTH"); } else if (mode == VELOCITY_DIRECTION_POINTING) { direction = orbit_->GetSatVelocity_i(); } else if (mode == ORBIT_NORMAL_POINTING) { @@ -98,7 +98,7 @@ void ControlledAttitude::PointingCtrl(const Vector<3> main_direction_i, const Ve // Calc DCM ECI->Target Matrix<3, 3> DCM_t2i = CalcDCM(main_direction_i, sub_direction_i); // Calc DCM Target->body - Matrix<3, 3> DCM_t2b = CalcDCM(pointing_t_b_, pointing_sub_t_b_); + Matrix<3, 3> DCM_t2b = CalcDCM(main_target_direction_b_, sub_target_direction_b_); // Calc DCM ECI->body Matrix<3, 3> DCM_i2b = DCM_t2b * transpose(DCM_t2i); // Convert to Quaternion @@ -146,14 +146,14 @@ void ControlledAttitude::CalcAngularVelocity(const double current_time_s) { if (previous_calc_time_s_ > 0.0) { double time_diff_sec = current_time_s - previous_calc_time_s_; - libra::Quaternion prev_q_b2i = prev_quaternion_i2b_.conjugate(); + libra::Quaternion prev_q_b2i = previous_quaternion_i2b_.conjugate(); libra::Quaternion q_diff = prev_q_b2i * quaternion_i2b_; q_diff = (2.0 / time_diff_sec) * q_diff; libra::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] = (prev_omega_b_rad_s_[i] - angular_velocity_b_rad_s_[i]) / time_diff_sec; + angular_acc_b_rad_s2_[i] = (previous_omega_b_rad_s_[i] - angular_velocity_b_rad_s_[i]) / time_diff_sec; } controlled_torque_b_Nm = inv_inertia_tensor_ * angular_acc_b_rad_s2_; } else { @@ -164,6 +164,6 @@ void ControlledAttitude::CalcAngularVelocity(const double current_time_s) { AddTorque_b_Nm(controlled_torque_b_Nm); // save previous values previous_calc_time_s_ = current_time_s; - prev_quaternion_i2b_ = quaternion_i2b_; - prev_omega_b_rad_s_ = angular_velocity_b_rad_s_; + previous_quaternion_i2b_ = quaternion_i2b_; + previous_omega_b_rad_s_ = angular_velocity_b_rad_s_; } diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index 04bd7a6f7..e253703a5 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -45,16 +45,17 @@ class ControlledAttitude : public Attitude { * @param [in] main_mode: Main control mode * @param [in] sub_mode: Sub control mode * @param [in] quaternion_i2b: Quaternion for INERTIAL_STABILIZE mode - * @param [in] pointing_t_b: Main target direction on the body fixed frame - * @param [in] pointing_sub_t_b: Sun target direction on the body fixed frame + * @param [in] main_target_direction_b: Main target direction on the body fixed frame + * @param [in] sub_target_direction_b: Sun target direction on the body fixed frame * @param [in] inertia_tensor_kgm2: Inertia tensor of the spacecraft [kg m^2] - * @param [in] local_celes_info: Local celestial information + * @param [in] local_celestial_information: Local celestial information * @param [in] orbit: Orbit * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - ControlledAttitude(const AttCtrlMode main_mode, const AttCtrlMode sub_mode, const Quaternion quaternion_i2b, const Vector<3> pointing_t_b, - const Vector<3> pointing_sub_t_b, const Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celes_info, - const Orbit* orbit, const std::string& simulation_object_name = "Attitude"); + ControlledAttitude(const AttCtrlMode main_mode, const AttCtrlMode sub_mode, const Quaternion quaternion_i2b, + const Vector<3> main_target_direction_b, const Vector<3> sub_target_direction_b, const Matrix<3, 3>& inertia_tensor_kgm2, + const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, + const std::string& simulation_object_name = "Attitude"); /** * @fn ~ControlledAttitude * @brief Destructor @@ -81,12 +82,12 @@ class ControlledAttitude : public Attitude { * @fn SetPointingTb * @brief Set main target direction on the body fixed frame */ - inline void SetPointingTb(Vector<3> pointing_t_b) { pointing_t_b_ = pointing_t_b; } + inline void SetPointingTb(Vector<3> main_target_direction_b) { main_target_direction_b_ = main_target_direction_b; } /** * @fn SetPointingSubTb * @brief Set sub target direction on the body fixed frame */ - inline void SetPointingSubTb(Vector<3> pointing_sub_t_b) { pointing_sub_t_b_ = pointing_sub_t_b; } + inline void SetPointingSubTb(Vector<3> sub_target_direction_b) { sub_target_direction_b_ = sub_target_direction_b; } /** * @fn Propagate @@ -96,17 +97,17 @@ class ControlledAttitude : public Attitude { virtual void Propagate(const double end_time_s); private: - AttCtrlMode main_mode_; //!< Main control mode - AttCtrlMode sub_mode_; //!< Sub control mode - libra::Vector<3> pointing_t_b_; //!< Main target direction on the body fixed frame - libra::Vector<3> pointing_sub_t_b_; //!< Sub target direction on tge body fixed frame - double previous_calc_time_s_ = -1.0; //!< Previous time of velocity calculation [sec] - libra::Quaternion prev_quaternion_i2b_; //!< Previous quaternion - libra::Vector<3> prev_omega_b_rad_s_; //!< Previous angular velocity [rad/s] + AttCtrlMode main_mode_; //!< Main control mode + AttCtrlMode sub_mode_; //!< Sub control mode + libra::Vector<3> main_target_direction_b_; //!< Main target direction on the body fixed frame + libra::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] + libra::Quaternion previous_quaternion_i2b_; //!< Previous quaternion + libra::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s] // Inputs - const LocalCelestialInformation* local_celes_info_; //!< Local celestial information - const Orbit* orbit_; //!< Orbit information + const LocalCelestialInformation* local_celestial_information_; //!< Local celestial information + const Orbit* orbit_; //!< Orbit information // Local functions /** @@ -137,7 +138,7 @@ class ControlledAttitude : public Attitude { * @fn CalcDCM * @brief Calculate direction cosine matrix with tow direction vectors * @param [in] main_direction: Main target direction - * @param [in] main_direction: Sub target direction + * @param [in] sub_direction: Sub target direction */ Matrix<3, 3> CalcDCM(const Vector<3> main_direction, const Vector<3> sub_direction); }; diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index 6448d27d9..38d7fcf3e 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -36,11 +36,11 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel AttCtrlMode sub_mode = ConvertStringToCtrlMode(sub_mode_in); Quaternion quaternion_i2b; ini_file_ca.ReadQuaternion(section_, "initial_quaternion_i2b", quaternion_i2b); - Vector<3> pointing_t_b, pointing_sub_t_b; - ini_file_ca.ReadVector(section_ca_, "main_pointing_direction_b", pointing_t_b); - ini_file_ca.ReadVector(section_ca_, "sub_pointing_direction_b", pointing_sub_t_b); - attitude = - new ControlledAttitude(main_mode, sub_mode, quaternion_i2b, pointing_t_b, pointing_sub_t_b, inertia_tensor, celes_info, orbit, mc_name); + 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); + attitude = new ControlledAttitude(main_mode, sub_mode, quaternion_i2b, main_target_direction_b, sub_target_direction_b, inertia_tensor, + celes_info, orbit, mc_name); } else { std::cerr << "ERROR: attitude propagation mode: " << propagate_mode << " is not defined!" << std::endl; std::cerr << "The attitude mode is automatically set as RK4" << std::endl; From 710935acbfe3d4bf880ce416668c0ea34d35c9fd Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 18:58:52 +0100 Subject: [PATCH 18/57] Fix public function --- src/dynamics/attitude/controlled_attitude.cpp | 16 +++++------ src/dynamics/attitude/controlled_attitude.hpp | 27 ++++++++++--------- 2 files changed, 22 insertions(+), 21 deletions(-) diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index 8a692799a..a139b9949 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -69,17 +69,17 @@ void ControlledAttitude::Propagate(const double end_time_s) { } // Calc main target direction - main_direction_i = CalcTargetDirection(main_mode_); + main_direction_i = CalcTargetDirection_i(main_mode_); // Calc sub target direction - sub_direction_i = CalcTargetDirection(sub_mode_); + sub_direction_i = CalcTargetDirection_i(sub_mode_); // Calc attitude - PointingCtrl(main_direction_i, sub_direction_i); + PointingControl(main_direction_i, sub_direction_i); // Calc angular velocity CalcAngularVelocity(end_time_s); return; } -Vector<3> ControlledAttitude::CalcTargetDirection(AttCtrlMode mode) { +Vector<3> ControlledAttitude::CalcTargetDirection_i(AttCtrlMode mode) { Vector<3> direction; if (mode == SUN_POINTING) { direction = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); @@ -94,18 +94,18 @@ Vector<3> ControlledAttitude::CalcTargetDirection(AttCtrlMode mode) { return direction; } -void ControlledAttitude::PointingCtrl(const Vector<3> main_direction_i, const Vector<3> sub_direction_i) { +void ControlledAttitude::PointingControl(const Vector<3> main_direction_i, const Vector<3> sub_direction_i) { // Calc DCM ECI->Target - Matrix<3, 3> DCM_t2i = CalcDCM(main_direction_i, sub_direction_i); + Matrix<3, 3> DCM_t2i = CalcDcm(main_direction_i, sub_direction_i); // Calc DCM Target->body - Matrix<3, 3> DCM_t2b = CalcDCM(main_target_direction_b_, sub_target_direction_b_); + Matrix<3, 3> DCM_t2b = CalcDcm(main_target_direction_b_, sub_target_direction_b_); // Calc DCM ECI->body Matrix<3, 3> DCM_i2b = DCM_t2b * transpose(DCM_t2i); // Convert to Quaternion quaternion_i2b_ = Quaternion::fromDCM(DCM_i2b); } -Matrix<3, 3> ControlledAttitude::CalcDCM(const Vector<3> main_direction, const Vector<3> sub_direction) { +Matrix<3, 3> ControlledAttitude::CalcDcm(const Vector<3> main_direction, const Vector<3> sub_direction) { // Calc basis vectors Vector<3> ex, ey, ez; ex = main_direction; diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index e253703a5..b7276ac22 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -74,20 +74,20 @@ class ControlledAttitude : public Attitude { */ inline void SetSubMode(const AttCtrlMode sub_mode) { sub_mode_ = sub_mode; } /** - * @fn SetQuaternionI2T + * @fn SetQuaternion_i2t * @brief Set quaternion for INERTIAL_STABILIZE mode */ - inline void SetQuaternionI2T(const Quaternion quaternion_i2t) { quaternion_i2b_ = quaternion_i2t; } + inline void SetQuaternion_i2t(const Quaternion quaternion_i2t) { quaternion_i2b_ = quaternion_i2t; } /** - * @fn SetPointingTb + * @fn SetMainTargetDirection_b * @brief Set main target direction on the body fixed frame */ - inline void SetPointingTb(Vector<3> main_target_direction_b) { main_target_direction_b_ = main_target_direction_b; } + inline void SetMainTargetDirection_b(Vector<3> main_target_direction_b) { main_target_direction_b_ = main_target_direction_b; } /** - * @fn SetPointingSubTb + * @fn SetSubTargetDirection_b * @brief Set sub target direction on the body fixed frame */ - inline void SetPointingSubTb(Vector<3> sub_target_direction_b) { sub_target_direction_b_ = sub_target_direction_b; } + inline void SetSubTargetDirection_b(Vector<3> sub_target_direction_b) { sub_target_direction_b_ = sub_target_direction_b; } /** * @fn Propagate @@ -116,18 +116,19 @@ class ControlledAttitude : public Attitude { */ void Initialize(void); /** - * @fn CalcTargetDirection + * @fn CalcTargetDirection_i * @brief Calculate target direction from attitude control mode * @param [in] mode: Attitude control mode + * @return Target direction at the inertia frame0 */ - Vector<3> CalcTargetDirection(AttCtrlMode mode); + Vector<3> CalcTargetDirection_i(AttCtrlMode mode); /** - * @fn PointingCtrl + * @fn PointingControl * @brief Calculate attitude quaternion * @param [in] main_direction_i: Main target direction in the inertial frame - * @param [in] main_direction_i: Sub target direction in the inertial frame + * @param [in] sub_direction_i: Sub target direction in the inertial frame */ - void PointingCtrl(const Vector<3> main_direction_i, const Vector<3> sub_direction_i); + void PointingControl(const Vector<3> main_direction_i, const Vector<3> sub_direction_i); /** * @fn CalcAngularVelocity * @brief Calculate angular velocity @@ -135,12 +136,12 @@ class ControlledAttitude : public Attitude { */ void CalcAngularVelocity(const double current_time_s); /** - * @fn CalcDCM + * @fn CalcDcm * @brief Calculate direction cosine matrix with tow direction vectors * @param [in] main_direction: Main target direction * @param [in] sub_direction: Sub target direction */ - Matrix<3, 3> CalcDCM(const Vector<3> main_direction, const Vector<3> sub_direction); + Matrix<3, 3> CalcDcm(const Vector<3> main_direction, const Vector<3> sub_direction); }; #endif // S2E_DYNAMICS_ATTITUDE_CONTROLLED_ATTITUDE_HPP_ From f306995fb0ba5475a413d70164104387882811ca Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:02:15 +0100 Subject: [PATCH 19/57] Fix local function variables --- src/dynamics/attitude/controlled_attitude.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index a139b9949..c4da1f796 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -96,13 +96,13 @@ Vector<3> ControlledAttitude::CalcTargetDirection_i(AttCtrlMode mode) { void ControlledAttitude::PointingControl(const Vector<3> main_direction_i, const Vector<3> sub_direction_i) { // Calc DCM ECI->Target - Matrix<3, 3> DCM_t2i = CalcDcm(main_direction_i, sub_direction_i); + Matrix<3, 3> dcm_t2i = CalcDcm(main_direction_i, sub_direction_i); // Calc DCM Target->body - Matrix<3, 3> DCM_t2b = CalcDcm(main_target_direction_b_, sub_target_direction_b_); + Matrix<3, 3> dcm_t2b = CalcDcm(main_target_direction_b_, sub_target_direction_b_); // Calc DCM ECI->body - Matrix<3, 3> DCM_i2b = DCM_t2b * transpose(DCM_t2i); + Matrix<3, 3> dcm_i2b = dcm_t2b * transpose(dcm_t2i); // Convert to Quaternion - quaternion_i2b_ = Quaternion::fromDCM(DCM_i2b); + quaternion_i2b_ = Quaternion::fromDCM(dcm_i2b); } Matrix<3, 3> ControlledAttitude::CalcDcm(const Vector<3> main_direction, const Vector<3> sub_direction) { @@ -116,13 +116,13 @@ Matrix<3, 3> ControlledAttitude::CalcDcm(const Vector<3> main_direction, const V ez = normalize(tmp3); // Generate DCM - Matrix<3, 3> DCM_; + Matrix<3, 3> dcm; for (int i = 0; i < 3; i++) { - DCM_[i][0] = ex[i]; - DCM_[i][1] = ey[i]; - DCM_[i][2] = ez[i]; + dcm[i][0] = ex[i]; + dcm[i][1] = ey[i]; + dcm[i][2] = ez[i]; } - return DCM_; + return dcm; } AttCtrlMode ConvertStringToCtrlMode(const std::string mode) { From 7981b2572775dfafaaa914b9e5f9bcb8d0b767b0 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:03:12 +0100 Subject: [PATCH 20/57] Fix enum name --- src/dynamics/attitude/controlled_attitude.cpp | 6 +++--- src/dynamics/attitude/controlled_attitude.hpp | 20 +++++++++---------- src/dynamics/attitude/initialize_attitude.cpp | 4 ++-- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index c4da1f796..d63519b16 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -12,7 +12,7 @@ using namespace std; #define THRESHOLD_CA cos(30.0 / 180.0 * libra::pi) // fix me -ControlledAttitude::ControlledAttitude(const AttCtrlMode main_mode, const AttCtrlMode sub_mode, const Quaternion quaternion_i2b, +ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const Quaternion quaternion_i2b, const Vector<3> main_target_direction_b, const Vector<3> sub_target_direction_b, const Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, const std::string& simulation_object_name) @@ -79,7 +79,7 @@ void ControlledAttitude::Propagate(const double end_time_s) { return; } -Vector<3> ControlledAttitude::CalcTargetDirection_i(AttCtrlMode mode) { +Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMode mode) { Vector<3> direction; if (mode == SUN_POINTING) { direction = local_celestial_information_->GetPositionFromSpacecraft_i_m("SUN"); @@ -125,7 +125,7 @@ Matrix<3, 3> ControlledAttitude::CalcDcm(const Vector<3> main_direction, const V return dcm; } -AttCtrlMode ConvertStringToCtrlMode(const std::string mode) { +AttitudeControlMode ConvertStringToCtrlMode(const std::string mode) { if (mode == "INERTIAL_STABILIZE") { return INERTIAL_STABILIZE; } else if (mode == "SUN_POINTING") { diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index b7276ac22..934bbad12 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -13,10 +13,10 @@ #include "attitude.hpp" /** - * @enum AttCtrlMode + * @enum AttitudeControlMode * @brief Attitude control mode */ -enum AttCtrlMode { +enum AttitudeControlMode { INERTIAL_STABILIZE, //!< Inertial stabilize SUN_POINTING, //!< Sun pointing EARTH_CENTER_POINTING, //!< Earth center pointing @@ -27,11 +27,11 @@ enum AttCtrlMode { /** * @fn ConvertStringToCtrlMode - * @brief Convert string to AttCtrlMode + * @brief Convert string to AttitudeControlMode * @param [in] mode: Control mode in string * @return Attitude control mode */ -AttCtrlMode ConvertStringToCtrlMode(const std::string mode); +AttitudeControlMode ConvertStringToCtrlMode(const std::string mode); /** * @class ControlledAttitude @@ -52,7 +52,7 @@ class ControlledAttitude : public Attitude { * @param [in] orbit: Orbit * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ - ControlledAttitude(const AttCtrlMode main_mode, const AttCtrlMode sub_mode, const Quaternion quaternion_i2b, + ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const Quaternion quaternion_i2b, const Vector<3> main_target_direction_b, const Vector<3> sub_target_direction_b, const Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, const std::string& simulation_object_name = "Attitude"); @@ -67,12 +67,12 @@ class ControlledAttitude : public Attitude { * @fn SetMainMode * @brief Set main control mode */ - inline void SetMainMode(const AttCtrlMode main_mode) { main_mode_ = main_mode; } + inline void SetMainMode(const AttitudeControlMode main_mode) { main_mode_ = main_mode; } /** * @fn SetSubMode * @brief Set sub control mode */ - inline void SetSubMode(const AttCtrlMode sub_mode) { sub_mode_ = sub_mode; } + inline void SetSubMode(const AttitudeControlMode sub_mode) { sub_mode_ = sub_mode; } /** * @fn SetQuaternion_i2t * @brief Set quaternion for INERTIAL_STABILIZE mode @@ -97,8 +97,8 @@ class ControlledAttitude : public Attitude { virtual void Propagate(const double end_time_s); private: - AttCtrlMode main_mode_; //!< Main control mode - AttCtrlMode sub_mode_; //!< Sub control mode + AttitudeControlMode main_mode_; //!< Main control mode + AttitudeControlMode sub_mode_; //!< Sub control mode libra::Vector<3> main_target_direction_b_; //!< Main target direction on the body fixed frame libra::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] @@ -121,7 +121,7 @@ class ControlledAttitude : public Attitude { * @param [in] mode: Attitude control mode * @return Target direction at the inertia frame0 */ - Vector<3> CalcTargetDirection_i(AttCtrlMode mode); + Vector<3> CalcTargetDirection_i(AttitudeControlMode mode); /** * @fn PointingControl * @brief Calculate attitude quaternion diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index 38d7fcf3e..65237243d 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -32,8 +32,8 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel 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"); - AttCtrlMode main_mode = ConvertStringToCtrlMode(main_mode_in); - AttCtrlMode sub_mode = ConvertStringToCtrlMode(sub_mode_in); + AttitudeControlMode main_mode = ConvertStringToCtrlMode(main_mode_in); + AttitudeControlMode sub_mode = ConvertStringToCtrlMode(sub_mode_in); Quaternion quaternion_i2b; ini_file_ca.ReadQuaternion(section_, "initial_quaternion_i2b", quaternion_i2b); Vector<3> main_target_direction_b, sub_target_direction_b; From c60dba913d729d960ce182b1276377097408cea8 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:08:15 +0100 Subject: [PATCH 21/57] Fix local function internal variable name --- src/dynamics/attitude/attitude_rk4.cpp | 35 +++++++++--------- src/dynamics/attitude/controlled_attitude.cpp | 37 +++++++++---------- 2 files changed, 35 insertions(+), 37 deletions(-) diff --git a/src/dynamics/attitude/attitude_rk4.cpp b/src/dynamics/attitude/attitude_rk4.cpp index 465019404..51b34f5df 100644 --- a/src/dynamics/attitude/attitude_rk4.cpp +++ b/src/dynamics/attitude/attitude_rk4.cpp @@ -4,15 +4,14 @@ */ #include "attitude_rk4.hpp" +#include #include #include -using namespace std; - -#include #include -AttitudeRk4::AttitudeRk4(const Vector<3>& angular_velocity_b_rad_s, const Quaternion& quaternion_i2b, const Matrix<3, 3>& inertia_tensor_kgm2, - const Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name) +AttitudeRk4::AttitudeRk4(const libra::Vector<3>& angular_velocity_b_rad_s, const libra::Quaternion& quaternion_i2b, + const libra::Matrix<3, 3>& inertia_tensor_kgm2, const libra::Vector<3>& torque_b_Nm, const double propagation_step_s, + const std::string& simulation_object_name) : Attitude(simulation_object_name) { angular_velocity_b_rad_s_ = angular_velocity_b_rad_s; quaternion_i2b_ = quaternion_i2b; @@ -34,7 +33,7 @@ void AttitudeRk4::SetParameters(const MCSimExecutor& mc_simulator) { // TODO: Consider the following calculation is needed here? current_propagation_time_s_ = 0.0; inv_inertia_tensor_ = libra::invert(inertia_tensor_kgm2_); - angular_momentum_reaction_wheel_b_Nms_ = Vector<3>(0.0); //!< Consider how to handle this variable + angular_momentum_reaction_wheel_b_Nms_ = libra::Vector<3>(0.0); //!< Consider how to handle this variable CalcAngularMomentum(); } @@ -50,8 +49,8 @@ void AttitudeRk4::Propagate(const double end_time_s) { CalcAngularMomentum(); } -Matrix<4, 4> AttitudeRk4::CalcAngularVelocityMatrix(Vector<3> angular_velocity_b_rad_s) { - Matrix<4, 4> angular_velocity_matrix; +libra::Matrix<4, 4> AttitudeRk4::CalcAngularVelocityMatrix(libra::Vector<3> angular_velocity_b_rad_s) { + libra::Matrix<4, 4> angular_velocity_matrix; angular_velocity_matrix[0][0] = 0.0f; angular_velocity_matrix[0][1] = angular_velocity_b_rad_s[2]; @@ -73,28 +72,28 @@ Matrix<4, 4> AttitudeRk4::CalcAngularVelocityMatrix(Vector<3> angular_velocity_b return angular_velocity_matrix; } -Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(Vector<7> x, double t) { +libra::Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(libra::Vector<7> x, double t) { UNUSED(t); - Vector<7> dxdt; + libra::Vector<7> dxdt; - Vector<3> omega_b; + libra::Vector<3> omega_b; for (int i = 0; i < 3; i++) { omega_b[i] = x[i]; } angular_momentum_total_b_Nms_ = (inertia_tensor_kgm2_ * omega_b) + angular_momentum_reaction_wheel_b_Nms_; - Vector<3> rhs = inv_inertia_tensor_ * (torque_b_Nm_ - libra::outer_product(omega_b, angular_momentum_total_b_Nms_)); + libra::Vector<3> rhs = inv_inertia_tensor_ * (torque_b_Nm_ - libra::outer_product(omega_b, angular_momentum_total_b_Nms_)); for (int i = 0; i < 3; ++i) { dxdt[i] = rhs[i]; } - Vector<4> quaternion_i2b; + libra::Vector<4> quaternion_i2b; for (int i = 0; i < 4; i++) { quaternion_i2b[i] = x[i + 3]; } - Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b) * quaternion_i2b; + libra::Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b) * quaternion_i2b; for (int i = 0; i < 4; i++) { dxdt[i + 3] = d_quaternion[i]; @@ -104,7 +103,7 @@ Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(Vector<7> x, double t) { } void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { - Vector<7> x; + libra::Vector<7> x; for (int i = 0; i < 3; i++) { x[i] = angular_velocity_b_rad_s_[i]; } @@ -112,8 +111,8 @@ void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { x[i + 3] = quaternion_i2b_[i]; } - Vector<7> k1, k2, k3, k4; - Vector<7> xk2, xk3, xk4; + libra::Vector<7> k1, k2, k3, k4; + libra::Vector<7> xk2, xk3, xk4; k1 = AttitudeDynamicsAndKinematics(x, t); xk2 = x + (dt / 2.0) * k1; @@ -126,7 +125,7 @@ void AttitudeRk4::RungeKuttaOneStep(double t, double dt) { k4 = AttitudeDynamicsAndKinematics(xk4, (t + dt)); - Vector<7> next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4); + libra::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/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index d63519b16..eeae34e91 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -8,14 +8,13 @@ #include #include -using namespace std; +#define THRESHOLD_CA cos(30.0 / 180.0 * libra::pi) // FIXME -#define THRESHOLD_CA cos(30.0 / 180.0 * libra::pi) // fix me - -ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const Quaternion quaternion_i2b, - const Vector<3> main_target_direction_b, const Vector<3> sub_target_direction_b, - const Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, - const Orbit* orbit, const std::string& simulation_object_name) +ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, + const libra::Quaternion quaternion_i2b, const libra::Vector<3> main_target_direction_b, + const libra::Vector<3> sub_target_direction_b, const libra::Matrix<3, 3>& inertia_tensor_kgm2, + const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, + const std::string& simulation_object_name) : Attitude(simulation_object_name), main_mode_(main_mode), sub_mode_(sub_mode), @@ -41,7 +40,7 @@ void ControlledAttitude::Initialize(void) { { // sub mode check if (main_mode_ == sub_mode_) { - cout << "sub mode should not equal to main mode. \n"; + std::cout << "sub mode should not equal to main mode. \n"; is_calc_enabled_ = false; return; } @@ -51,7 +50,7 @@ void ControlledAttitude::Initialize(void) { double tmp = inner_product(main_target_direction_b_, sub_target_direction_b_); tmp = std::abs(tmp); if (tmp > THRESHOLD_CA) { - cout << "sub target direction should separate from the main target direction. \n"; + std::cout << "sub target direction should separate from the main target direction. \n"; is_calc_enabled_ = false; return; } @@ -94,29 +93,29 @@ Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMode mode) { return direction; } -void ControlledAttitude::PointingControl(const Vector<3> main_direction_i, const Vector<3> sub_direction_i) { +void ControlledAttitude::PointingControl(const libra::Vector<3> main_direction_i, const libra::Vector<3> sub_direction_i) { // Calc DCM ECI->Target - Matrix<3, 3> dcm_t2i = CalcDcm(main_direction_i, sub_direction_i); + libra::Matrix<3, 3> dcm_t2i = CalcDcm(main_direction_i, sub_direction_i); // Calc DCM Target->body - Matrix<3, 3> dcm_t2b = CalcDcm(main_target_direction_b_, sub_target_direction_b_); + libra::Matrix<3, 3> dcm_t2b = CalcDcm(main_target_direction_b_, sub_target_direction_b_); // Calc DCM ECI->body - Matrix<3, 3> dcm_i2b = dcm_t2b * transpose(dcm_t2i); + libra::Matrix<3, 3> dcm_i2b = dcm_t2b * transpose(dcm_t2i); // Convert to Quaternion quaternion_i2b_ = Quaternion::fromDCM(dcm_i2b); } -Matrix<3, 3> ControlledAttitude::CalcDcm(const Vector<3> main_direction, const Vector<3> sub_direction) { +libra::Matrix<3, 3> ControlledAttitude::CalcDcm(const libra::Vector<3> main_direction, const libra::Vector<3> sub_direction) { // Calc basis vectors - Vector<3> ex, ey, ez; + libra::Vector<3> ex, ey, ez; ex = main_direction; - Vector<3> tmp1 = outer_product(ex, sub_direction); - Vector<3> tmp2 = outer_product(tmp1, ex); + libra::Vector<3> tmp1 = outer_product(ex, sub_direction); + libra::Vector<3> tmp2 = outer_product(tmp1, ex); ey = normalize(tmp2); - Vector<3> tmp3 = outer_product(ex, ey); + libra::Vector<3> tmp3 = outer_product(ex, ey); ez = normalize(tmp3); // Generate DCM - Matrix<3, 3> dcm; + libra::Matrix<3, 3> dcm; for (int i = 0; i < 3; i++) { dcm[i][0] = ex[i]; dcm[i][1] = ey[i]; From a1d70e565e5e34511f34dd1c570b78858af64ec1 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:11:23 +0100 Subject: [PATCH 22/57] Fix function arguments --- src/dynamics/attitude/initialize_attitude.cpp | 14 +++++++------- src/dynamics/attitude/initialize_attitude.hpp | 12 ++++++------ 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index 65237243d..1ecd982d6 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -6,11 +6,11 @@ #include -Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* celes_info, const double step_sec, - const Matrix<3, 3> inertia_tensor, const int sat_id) { +Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* local_celestial_information, + const double step_width_s, const Matrix<3, 3> inertia_tensor_kgm2, const int spacecraft_id) { IniAccess ini_file(file_name); const char* section_ = "ATTITUDE"; - std::string mc_name = section_ + std::to_string(sat_id); // FIXME + std::string mc_name = section_ + std::to_string(spacecraft_id); // FIXME Attitude* attitude; const std::string propagate_mode = ini_file.ReadString(section_, "propagate_mode"); @@ -24,7 +24,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel Vector<3> torque_b; ini_file.ReadVector(section_, "initial_torque_b_Nm", torque_b); - attitude = new AttitudeRk4(omega_b, quaternion_i2b, inertia_tensor, torque_b, step_sec, mc_name); + attitude = new AttitudeRk4(omega_b, quaternion_i2b, inertia_tensor_kgm2, torque_b, step_width_s, mc_name); } else if (propagate_mode == "CONTROLLED") { // Controlled attitude IniAccess ini_file_ca(file_name); @@ -39,8 +39,8 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel 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); - attitude = new ControlledAttitude(main_mode, sub_mode, quaternion_i2b, main_target_direction_b, sub_target_direction_b, inertia_tensor, - celes_info, orbit, mc_name); + attitude = new ControlledAttitude(main_mode, sub_mode, quaternion_i2b, main_target_direction_b, sub_target_direction_b, inertia_tensor_kgm2, + local_celestial_information, orbit, mc_name); } else { std::cerr << "ERROR: attitude propagation mode: " << propagate_mode << " is not defined!" << std::endl; std::cerr << "The attitude mode is automatically set as RK4" << std::endl; @@ -52,7 +52,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel Vector<3> torque_b; ini_file.ReadVector(section_, "initial_torque_b_Nm", torque_b); - attitude = new AttitudeRk4(omega_b, quaternion_i2b, inertia_tensor, torque_b, step_sec, mc_name); + attitude = new AttitudeRk4(omega_b, quaternion_i2b, inertia_tensor_kgm2, torque_b, step_width_s, mc_name); } return attitude; diff --git a/src/dynamics/attitude/initialize_attitude.hpp b/src/dynamics/attitude/initialize_attitude.hpp index dbe34e786..123e55a98 100644 --- a/src/dynamics/attitude/initialize_attitude.hpp +++ b/src/dynamics/attitude/initialize_attitude.hpp @@ -15,12 +15,12 @@ * @brief Initialize function for Attitude * @param [in] file_name: Path to the initialize file * @param [in] orbit: Orbit information - * @param [in] celes_info: Celestial information - * @param [in] step_sec: Step width [sec] - * @param [in] inertia_tensor: Inertia tensor [kg m^2] - * @param [in] sat_id: Satellite ID + * @param [in] local_celestial_information: Celestial information + * @param [in] step_width_s: Step width [sec] + * @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* celes_info, const double step_sec, - const Matrix<3, 3> inertia_tensor, const int sat_id); +Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCelestialInformation* local_celestial_information, + const double step_width_s, const Matrix<3, 3> inertia_tensor_kgm2, const int spacecraft_id); #endif // S2E_DYNAMICS_ATTITUDE_INITIALIZE_ATTITUDE_HPP_ From ffcedb33f4778d9547a61b5924128c44a2414d9f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:15:09 +0100 Subject: [PATCH 23/57] Remove using --- src/dynamics/orbit/orbit.cpp | 24 +++++++-------- src/dynamics/orbit/orbit.hpp | 57 ++++++++++++++++-------------------- 2 files changed, 38 insertions(+), 43 deletions(-) diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index 8f4f93ae6..f47681076 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -5,14 +5,14 @@ #include "orbit.hpp" Quaternion Orbit::CalcQuaternionI2LVLH() const { - Vector<3> lvlh_x = sat_position_i_; // x-axis in LVLH frame is position vector direction from geocenter to satellite - Vector<3> lvlh_ex = normalize(lvlh_x); - Vector<3> lvlh_z = outer_product(sat_position_i_, sat_velocity_i_); // z-axis in LVLH frame is angular momentum vector direction of orbit - Vector<3> lvlh_ez = normalize(lvlh_z); - Vector<3> lvlh_y = outer_product(lvlh_z, lvlh_x); - Vector<3> lvlh_ey = normalize(lvlh_y); + libra::Vector<3> lvlh_x = sat_position_i_; // x-axis in LVLH frame is position vector direction from geocenter to satellite + libra::Vector<3> lvlh_ex = normalize(lvlh_x); + libra::Vector<3> lvlh_z = outer_product(sat_position_i_, sat_velocity_i_); // z-axis in LVLH frame is angular momentum vector direction of orbit + libra::Vector<3> lvlh_ez = normalize(lvlh_z); + libra::Vector<3> lvlh_y = outer_product(lvlh_z, lvlh_x); + libra::Vector<3> lvlh_ey = normalize(lvlh_y); - Matrix<3, 3> dcm_i2lvlh; + libra::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]; @@ -23,19 +23,19 @@ Quaternion Orbit::CalcQuaternionI2LVLH() const { dcm_i2lvlh[2][1] = lvlh_ez[1]; dcm_i2lvlh[2][2] = lvlh_ez[2]; - Quaternion q_i2lvlh = Quaternion::fromDCM(dcm_i2lvlh); + libra::Quaternion q_i2lvlh = Quaternion::fromDCM(dcm_i2lvlh); return q_i2lvlh.normalize(); } void Orbit::TransEciToEcef(void) { - Matrix<3, 3> dcm_i_to_xcxf = celes_info_->GetEarthRotation().GetDcmJ2000ToXcxf(); + libra::Matrix<3, 3> dcm_i_to_xcxf = celes_info_->GetEarthRotation().GetDcmJ2000ToXcxf(); sat_position_ecef_ = dcm_i_to_xcxf * sat_position_i_; // convert velocity vector in ECI to the vector in ECEF - Vector<3> OmegaE{0.0}; + libra::Vector<3> OmegaE{0.0}; OmegaE[2] = environment::earth_mean_angular_velocity_rad_s; - Vector<3> wExr = outer_product(OmegaE, sat_position_i_); - Vector<3> V_wExr = sat_velocity_i_ - wExr; + libra::Vector<3> wExr = outer_product(OmegaE, sat_position_i_); + libra::Vector<3> V_wExr = sat_velocity_i_ - wExr; sat_velocity_ecef_ = dcm_i_to_xcxf * V_wExr; } diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index c9c033424..d0fb22ff2 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -6,21 +6,16 @@ #ifndef S2E_DYNAMICS_ORBIT_ORBIT_HPP_ #define S2E_DYNAMICS_ORBIT_ORBIT_HPP_ +#include +#include +#include +#include #include #include #include #include #include -using libra::Matrix; -using libra::Quaternion; -using libra::Vector; - -#include -#include -#include -#include - /** * @enum OrbitPropagateMode * @brief Propagation mode of orbit @@ -74,7 +69,7 @@ class Orbit : public ILoggable { * @brief Update attitude information * @param [in] q_i2b: End time of simulation [sec] */ - inline void UpdateAtt(Quaternion q_i2b) { sat_velocity_b_ = q_i2b.frame_conv(sat_velocity_i_); } + inline void UpdateAtt(libra::Quaternion q_i2b) { sat_velocity_b_ = q_i2b.frame_conv(sat_velocity_i_); } /** * @fn AddPositionOffset @@ -82,7 +77,7 @@ class Orbit : public ILoggable { * @note Is this really needed? * @param [in] offset_i: Offset vector in the inertial frame [m] */ - inline virtual void AddPositionOffset(Vector<3> offset_i) { (void)offset_i; } + inline virtual void AddPositionOffset(libra::Vector<3> offset_i) { (void)offset_i; } // Getters /** @@ -99,27 +94,27 @@ class Orbit : public ILoggable { * @fn GetSatPosition_i * @brief Return spacecraft position in the inertial frame [m] */ - inline Vector<3> GetSatPosition_i() const { return sat_position_i_; } + inline libra::Vector<3> GetSatPosition_i() const { return sat_position_i_; } /** * @fn GetSatPosition_ecef * @brief Return spacecraft position in the ECEF frame [m] */ - inline Vector<3> GetSatPosition_ecef() const { return sat_position_ecef_; } + inline libra::Vector<3> GetSatPosition_ecef() const { return sat_position_ecef_; } /** * @fn GetSatVelocity_i * @brief Return spacecraft velocity in the inertial frame [m/s] */ - inline Vector<3> GetSatVelocity_i() const { return sat_velocity_i_; } + inline libra::Vector<3> GetSatVelocity_i() const { return sat_velocity_i_; } /** * @fn GetSatVelocity_b * @brief Return spacecraft velocity in the body fixed frame [m/s] */ - inline Vector<3> GetSatVelocity_b() const { return sat_velocity_b_; } + inline libra::Vector<3> GetSatVelocity_b() const { return sat_velocity_b_; } /** * @fn GetSatVelocity_ecef * @brief Return spacecraft velocity in the ECEF frame [m/s] */ - inline Vector<3> GetSatVelocity_ecef() const { return sat_velocity_ecef_; } + inline libra::Vector<3> GetSatVelocity_ecef() const { return sat_velocity_ecef_; } /** * @fn GetGeodeticPosition * @brief Return spacecraft position in the geodetic frame [m] @@ -130,8 +125,8 @@ class Orbit : public ILoggable { inline double GetLat_rad() const { return sat_position_geo_.GetLat_rad(); } inline double GetLon_rad() const { return sat_position_geo_.GetLon_rad(); } inline double GetAlt_m() const { return sat_position_geo_.GetAlt_m(); } - inline Vector<3> GetLatLonAlt() const { - Vector<3> vec; + inline libra::Vector<3> GetLatLonAlt() const { + libra::Vector<3> vec; vec(0) = sat_position_geo_.GetLat_rad(); vec(1) = sat_position_geo_.GetLon_rad(); vec(2) = sat_position_geo_.GetAlt_m(); @@ -148,14 +143,14 @@ class Orbit : public ILoggable { * @fn SetAcceleration_i * @brief Set acceleration in the inertial frame [m/s2] */ - inline void SetAcceleration_i(Vector<3> acceleration_i) { acc_i_ = acceleration_i; } + inline void SetAcceleration_i(libra::Vector<3> acceleration_i) { acc_i_ = acceleration_i; } /** * @fn AddForce_i * @brief Add force * @param [in] force_i: Force in the inertial frame [N] * @param [in] spacecraft_mass: Mass of spacecraft [kg] */ - inline void AddForce_i(Vector<3> force_i, double spacecraft_mass) { + inline void AddForce_i(libra::Vector<3> force_i, double spacecraft_mass) { force_i /= spacecraft_mass; acc_i_ += force_i; } @@ -163,7 +158,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(Vector<3> acceleration_i) { acc_i_ += acceleration_i; } + inline void AddAcceleration_i_m_s2(libra::Vector<3> acceleration_i) { acc_i_ += acceleration_i; } /** * @fn AddForce_i * @brief Add force @@ -171,7 +166,7 @@ class Orbit : public ILoggable { * @param [in] q_i2b: Quaternion from the inertial frame to the body fixed frame * @param [in] spacecraft_mass: Mass of spacecraft [kg] */ - inline void AddForce_b_N(Vector<3> force_b, Quaternion q_i2b, double spacecraft_mass) { + inline void AddForce_b_N(libra::Vector<3> force_b, libra::Quaternion q_i2b, double spacecraft_mass) { auto force_i = q_i2b.frame_conv_inv(force_b); AddForce_i(force_i, spacecraft_mass); } @@ -180,7 +175,7 @@ class Orbit : public ILoggable { * @fn CalcQuaternionI2LVLH * @brief Calculate quaternion from the inertial frame to the LVLH frame */ - Quaternion CalcQuaternionI2LVLH() const; + libra::Quaternion CalcQuaternionI2LVLH() const; // Override ILoggable /** @@ -201,16 +196,16 @@ class Orbit : public ILoggable { bool is_calc_enabled_ = false; //!< Calculate flag OrbitPropagateMode propagate_mode_; //!< Propagation mode - Vector<3> sat_position_i_; //!< Spacecraft position in the inertial frame [m] - Vector<3> sat_position_ecef_; //!< Spacecraft position in the ECEF frame [m] - GeodeticPosition sat_position_geo_; //!< Spacecraft position in the Geodetic frame + libra::Vector<3> sat_position_i_; //!< Spacecraft position in the inertial frame [m] + libra::Vector<3> sat_position_ecef_; //!< Spacecraft position in the ECEF frame [m] + GeodeticPosition sat_position_geo_; //!< Spacecraft position in the Geodetic frame - Vector<3> sat_velocity_i_; //!< Spacecraft velocity in the inertial frame [m/s] - Vector<3> sat_velocity_b_; //!< Spacecraft velocity in the body frame [m/s] - Vector<3> sat_velocity_ecef_; //!< Spacecraft velocity in the ECEF frame [m/s] + libra::Vector<3> sat_velocity_i_; //!< Spacecraft velocity in the inertial frame [m/s] + libra::Vector<3> sat_velocity_b_; //!< Spacecraft velocity in the body frame [m/s] + libra::Vector<3> sat_velocity_ecef_; //!< Spacecraft velocity in the ECEF frame [m/s] - Vector<3> acc_i_; //!< Spacecraft acceleration in the inertial frame [m/s2] - //!< NOTE: Clear to zero at the end of the Propagate function + libra::Vector<3> acc_i_; //!< 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 /** From 2289076b5746a4cab61c34ba3aca1caad96ab33d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:22:44 +0100 Subject: [PATCH 24/57] Fix private variable names --- src/dynamics/orbit/encke_orbit_propagation.cpp | 4 ++-- src/dynamics/orbit/encke_orbit_propagation.hpp | 4 ++-- src/dynamics/orbit/initialize_orbit.cpp | 14 +++++++------- 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 | 2 +- src/dynamics/orbit/orbit.hpp | 6 +++--- src/dynamics/orbit/relative_orbit.cpp | 4 ++-- src/dynamics/orbit/relative_orbit.hpp | 8 ++++---- 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 | 10 +++++----- 14 files changed, 38 insertions(+), 38 deletions(-) diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index b1e8c6758..94f34cbd9 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -9,10 +9,10 @@ #include "../../library/orbit/orbital_elements.hpp" -EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celes_info, const double mu_m3_s2, const double prop_step_s, +EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, const double prop_step_s, const double current_jd, const Vector<3> init_position_i_m, const Vector<3> init_velocity_i_m_s, const double error_tolerance) - : Orbit(celes_info), libra::ODE<6>(prop_step_s), mu_m3_s2_(mu_m3_s2), error_tolerance_(error_tolerance), prop_step_s_(prop_step_s) { + : Orbit(celestial_information), libra::ODE<6>(prop_step_s), mu_m3_s2_(mu_m3_s2), error_tolerance_(error_tolerance), prop_step_s_(prop_step_s) { prop_time_s_ = 0.0; Initialize(current_jd, init_position_i_m, init_velocity_i_m_s); } diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index 220e86f6d..8c1fd62ba 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -19,7 +19,7 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { /** * @fn EnckeOrbitPropagation * @brief Constructor - * @param [in] celes_info: Celestial information + * @param [in] celestial_information: Celestial information * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] prop_step_s: Propagation step width [sec] * @param [in] current_jd: Current Julian day [day] @@ -27,7 +27,7 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] init_velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] * @param [in] error_tolerance: Error tolerance threshold */ - EnckeOrbitPropagation(const CelestialInformation* celes_info, const double mu_m3_s2, const double prop_step_s, const double current_jd, + EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, const double prop_step_s, const double current_jd, const Vector<3> init_position_i_m, const Vector<3> init_velocity_i_m_s, const double error_tolerance); /** * @fn ~EnckeOrbitPropagation diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index d11261ee6..f60e7f96a 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -12,7 +12,7 @@ #include "rk4_orbit_propagation.hpp" #include "sgp4_orbit_propagation.hpp" -Orbit* InitOrbit(const CelestialInformation* celes_info, std::string ini_path, double stepSec, double current_jd, double gravity_constant, +Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string ini_path, double stepSec, double current_jd, double gravity_constant, std::string section, RelativeInformation* rel_info) { auto conf = IniAccess(ini_path); const char* section_ = section.c_str(); @@ -33,7 +33,7 @@ Orbit* InitOrbit(const CelestialInformation* celes_info, std::string ini_path, d position_i_m[i] = pos_vel[i]; velocity_i_m_s[i] = pos_vel[i + 3]; } - orbit = new Rk4OrbitPropagation(celes_info, gravity_constant, stepSec, position_i_m, velocity_i_m_s); + orbit = new Rk4OrbitPropagation(celestial_information, gravity_constant, stepSec, position_i_m, velocity_i_m_s); } else if (propagate_mode == "SGP4") { // Initialize SGP4 orbit propagator int wgs = conf.ReadInt(section_, "wgs"); @@ -41,7 +41,7 @@ Orbit* InitOrbit(const CelestialInformation* celes_info, std::string ini_path, d conf.ReadChar(section_, "tle1", 80, tle1); conf.ReadChar(section_, "tle2", 80, tle2); - orbit = new Sgp4OrbitPropagation(celes_info, tle1, tle2, wgs, current_jd); + orbit = new Sgp4OrbitPropagation(celestial_information, tle1, tle2, wgs, current_jd); } else if (propagate_mode == "RELATIVE") { // initialize orbit for relative dynamics of formation flying RelativeOrbit::RelativeOrbitUpdateMethod update_method = @@ -58,7 +58,7 @@ Orbit* InitOrbit(const CelestialInformation* celes_info, std::string ini_path, d // the orbit of the reference sat is initialized, create temporary initial orbit of the reference sat int reference_sat_id = conf.ReadInt(section_, "reference_satellite_id"); - orbit = new RelativeOrbit(celes_info, gravity_constant, stepSec, reference_sat_id, init_relative_position_lvlh, init_relative_velocity_lvlh, + orbit = new RelativeOrbit(celestial_information, gravity_constant, stepSec, reference_sat_id, init_relative_position_lvlh, init_relative_velocity_lvlh, update_method, relative_dynamics_model_type, stm_model_type, rel_info); } else if (propagate_mode == "KEPLER") { // initialize orbit for Kepler propagation @@ -83,7 +83,7 @@ Orbit* InitOrbit(const CelestialInformation* celes_info, std::string ini_path, d oe = OrbitalElements(epoch_jday, semi_major_axis_m, eccentricity, inclination_rad, raan_rad, arg_perigee_rad); } KeplerOrbit kepler_orbit(mu_m3_s2, oe); - orbit = new KeplerOrbitPropagation(celes_info, current_jd, kepler_orbit); + orbit = new KeplerOrbitPropagation(celestial_information, current_jd, kepler_orbit); } else if (propagate_mode == "ENCKE") { // initialize orbit for Encke's method Vector<3> position_i_m; @@ -95,7 +95,7 @@ Orbit* InitOrbit(const CelestialInformation* celes_info, std::string ini_path, d } double error_tolerance = conf.ReadDouble(section_, "error_tolerance"); - orbit = new EnckeOrbitPropagation(celes_info, gravity_constant, stepSec, current_jd, position_i_m, velocity_i_m_s, error_tolerance); + orbit = new EnckeOrbitPropagation(celestial_information, gravity_constant, stepSec, current_jd, position_i_m, velocity_i_m_s, error_tolerance); } else { 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; @@ -107,7 +107,7 @@ Orbit* InitOrbit(const CelestialInformation* celes_info, std::string ini_path, d position_i_m[i] = pos_vel[i]; velocity_i_m_s[i] = pos_vel[i + 3]; } - orbit = new Rk4OrbitPropagation(celes_info, gravity_constant, stepSec, position_i_m, velocity_i_m_s); + orbit = new Rk4OrbitPropagation(celestial_information, gravity_constant, stepSec, position_i_m, velocity_i_m_s); } orbit->SetIsCalcEnabled(conf.ReadEnable(section_, "calculation")); diff --git a/src/dynamics/orbit/initialize_orbit.hpp b/src/dynamics/orbit/initialize_orbit.hpp index 3784dc8ad..4379d7ed9 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -15,7 +15,7 @@ class RelativeInformation; /** * @fn InitOrbit * @brief Initialize function for Orbit class - * @param [in] celes_info: Celestial information + * @param [in] celestial_information: Celestial information * @param [in] ini_path: Path to initialize file * @param [in] stepSec: Step width [sec] * @param [in] current_jd: Current Julian day [day] @@ -23,7 +23,7 @@ class RelativeInformation; * @param [in] section: Section name * @param [in] rel_info: Relative information */ -Orbit* InitOrbit(const CelestialInformation* celes_info, std::string ini_path, double stepSec, double current_jd, double gravity_constant, +Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string ini_path, double stepSec, double current_jd, double gravity_constant, std::string section = "ORBIT", RelativeInformation* rel_info = (RelativeInformation*)nullptr); /** diff --git a/src/dynamics/orbit/kepler_orbit_propagation.cpp b/src/dynamics/orbit/kepler_orbit_propagation.cpp index 5d37926a3..1afcd0e23 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.cpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.cpp @@ -8,8 +8,8 @@ #include "../../library/math/s2e_math.hpp" -KeplerOrbitPropagation::KeplerOrbitPropagation(const CelestialInformation* celes_info, const double current_jd, KeplerOrbit kepler_orbit) - : Orbit(celes_info), KeplerOrbit(kepler_orbit) { +KeplerOrbitPropagation::KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_jd, KeplerOrbit kepler_orbit) + : Orbit(celestial_information), KeplerOrbit(kepler_orbit) { UpdateState(current_jd); } diff --git a/src/dynamics/orbit/kepler_orbit_propagation.hpp b/src/dynamics/orbit/kepler_orbit_propagation.hpp index bf790ebec..ad401128f 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.hpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.hpp @@ -19,11 +19,11 @@ class KeplerOrbitPropagation : public Orbit, public KeplerOrbit { /** * @fn KeplerOrbitPropagation * @brief Constructor - * @param [in] celes_info: Celestial information + * @param [in] celestial_information: Celestial information * @param [in] current_jd: Current Julian day [day] * @param [in] kepler_orbit: Kepler orbit */ - KeplerOrbitPropagation(const CelestialInformation* celes_info, const double current_jd, KeplerOrbit kepler_orbit); + KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_jd, KeplerOrbit kepler_orbit); /** * @fn ~KeplerOrbitPropagation * @brief Destructor diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index f47681076..444ab6e17 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -28,7 +28,7 @@ Quaternion Orbit::CalcQuaternionI2LVLH() const { } void Orbit::TransEciToEcef(void) { - libra::Matrix<3, 3> dcm_i_to_xcxf = celes_info_->GetEarthRotation().GetDcmJ2000ToXcxf(); + libra::Matrix<3, 3> dcm_i_to_xcxf = celestial_information_->GetEarthRotation().GetDcmJ2000ToXcxf(); sat_position_ecef_ = dcm_i_to_xcxf * sat_position_i_; // convert velocity vector in ECI to the vector in ECEF diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index d0fb22ff2..ed0789e33 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -47,9 +47,9 @@ class Orbit : public ILoggable { /** * @fn Orbit * @brief Constructor - * @param [in] celes_info: Celestial information + * @param [in] celestial_information: Celestial information */ - Orbit(const CelestialInformation* celes_info) : celes_info_(celes_info) {} + Orbit(const CelestialInformation* celestial_information) : celestial_information_(celestial_information) {} /** * @fn ~Orbit * @brief Destructor @@ -190,7 +190,7 @@ class Orbit : public ILoggable { virtual std::string GetLogValue() const; protected: - const CelestialInformation* celes_info_; //!< Celestial information + const CelestialInformation* celestial_information_; //!< Celestial information // Settings bool is_calc_enabled_ = false; //!< Calculate flag diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index ebd84b487..968ad1972 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -8,11 +8,11 @@ #include "rk4_orbit_propagation.hpp" -RelativeOrbit::RelativeOrbit(const CelestialInformation* celes_info, double mu, double timestep, int reference_sat_id, +RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double mu, double timestep, int reference_sat_id, Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, RelativeOrbitUpdateMethod update_method, RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, RelativeInformation* rel_info) - : Orbit(celes_info), + : Orbit(celestial_information), libra::ODE<6>(timestep), mu_(mu), reference_sat_id_(reference_sat_id), diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index ac07e0010..13db23084 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -28,7 +28,7 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { /** * @fn RelativeOrbit * @brief Constructor - * @param [in] celes_info: Celestial information + * @param [in] celestial_information: Celestial information * @param [in] timestep: Time step [sec] * @param [in] reference_sat_id: Reference satellite ID * @param [in] initial_relative_position_lvlh: Initial value of relative position at the LVLH frame of reference satellite @@ -38,9 +38,9 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @param [in] stm_model_type: State transition matrix type * @param [in] rel_info: Relative information */ - RelativeOrbit(const CelestialInformation* celes_info, double mu, double timestep, int reference_sat_id, Vector<3> initial_relative_position_lvlh, - Vector<3> initial_relative_velocity_lvlh, RelativeOrbitUpdateMethod update_method, RelativeOrbitModel relative_dynamics_model_type, - STMModel stm_model_type, RelativeInformation* rel_info); + RelativeOrbit(const CelestialInformation* celestial_information, double mu, double timestep, int reference_sat_id, + Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, RelativeOrbitUpdateMethod update_method, + RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, RelativeInformation* rel_info); /** * @fn ~RelativeOrbit * @brief Destructor diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index 489de2b0a..7b3d03852 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -10,9 +10,9 @@ using std::string; -Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celes_info, double mu, double timestep, Vector<3> init_position, +Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu, double timestep, Vector<3> init_position, Vector<3> init_velocity, double init_time) - : Orbit(celes_info), ODE(timestep), mu(mu) { + : Orbit(celestial_information), ODE(timestep), mu(mu) { propagate_mode_ = OrbitPropagateMode::kRk4; prop_time_ = 0.0; diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index c973da654..2ddbaaf9a 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -24,14 +24,14 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { /** * @fn Rk4OrbitPropagation * @brief Constructor - * @param [in] celes_info: Celestial information + * @param [in] celestial_information: Celestial information * @param [in] mu: Gravity constant [m3/s2] * @param [in] timestep: Step width [sec] * @param [in] init_position: Initial value of position in the inertial frame [m] * @param [in] init_velocity: Initial value of velocity in the inertial frame [m/s] * @param [in] init_time: Initial time [sec] */ - Rk4OrbitPropagation(const CelestialInformation* celes_info, double mu, double timestep, Vector<3> init_position, Vector<3> init_velocity, + Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu, double timestep, Vector<3> init_position, Vector<3> init_velocity, double init_time = 0); /** * @fn ~Rk4OrbitPropagation diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index 10d20f9e9..cff8be535 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -10,8 +10,8 @@ #include using namespace std; -Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celes_info, char* tle1, char* tle2, int wgs, double current_jd) - : Orbit(celes_info) { +Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, int wgs, double current_jd) + : Orbit(celestial_information) { propagate_mode_ = OrbitPropagateMode::kSgp4; if (wgs == 0) { diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.hpp b/src/dynamics/orbit/sgp4_orbit_propagation.hpp index 823f69221..50181bfa8 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.hpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.hpp @@ -22,13 +22,13 @@ class Sgp4OrbitPropagation : public Orbit { /** * @fn Sgp4OrbitPropagation * @brief Constructor - * @param [in] celes_info: Celestial information + * @param [in] celestial_information: Celestial information * @param [in] tle1: The first line of TLE * @param [in] tle2: The second line of TLE * @param [in] wgs: Wold Geodetic System * @param [in] current_jd: Current Julian day [day] */ - Sgp4OrbitPropagation(const CelestialInformation* celes_info, char* tle1, char* tle2, int wgs, double current_jd); + Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, int wgs, double current_jd); // Override Orbit /** @@ -47,9 +47,9 @@ class Sgp4OrbitPropagation : public Orbit { Vector<3> GetESIOmega(); private: - gravconsttype whichconst_; //!< Gravity constant value type - elsetrec satrec_; //!< Structure data for SGP4 library - const CelestialInformation* celes_info_; //!< Celestial information + gravconsttype whichconst_; //!< Gravity constant value type + elsetrec satrec_; //!< Structure data for SGP4 library + const CelestialInformation* celestial_information_; //!< Celestial information }; #endif // S2E_DYNAMICS_ORBIT_SGP4_ORBIT_PROPAGATION_HPP_ From bc655ea4743554734969cccdb0523a29b4848de4 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:26:55 +0100 Subject: [PATCH 25/57] Fix private variable names --- .../orbit/encke_orbit_propagation.cpp | 16 +++---- .../orbit/kepler_orbit_propagation.cpp | 4 +- src/dynamics/orbit/orbit.cpp | 29 +++++------ src/dynamics/orbit/orbit.hpp | 48 +++++++++---------- src/dynamics/orbit/relative_orbit.cpp | 12 ++--- src/dynamics/orbit/rk4_orbit_propagation.cpp | 40 ++++++++-------- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 6 +-- 7 files changed, 78 insertions(+), 77 deletions(-) diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index 94f34cbd9..c97ebaa5a 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -24,10 +24,10 @@ void EnckeOrbitPropagation::Propagate(double endtime, double current_jd) { if (!is_calc_enabled_) return; // Rectification - double norm_sat_position_m = norm(sat_position_i_); + double norm_sat_position_m = norm(spacecraft_position_i_m_); double norm_diff_position_m = norm(diff_position_i_m_); if (norm_diff_position_m / norm_sat_position_m > error_tolerance_) { - Initialize(current_jd, sat_position_i_, sat_velocity_i_); + Initialize(current_jd, spacecraft_position_i_m_, spacecraft_velocity_i_m_s_); } // Update reference orbit @@ -67,7 +67,7 @@ void EnckeOrbitPropagation::RHS(double t, const Vector<6>& state, Vector<6>& rhs double r_m = norm(ref_position_i_m_); double r_m3 = pow(r_m, 3.0); - diff_acc_i_m_s2 = -(mu_m3_s2_ / r_m3) * (q_func * sat_position_i_ + diff_pos_i_m) + acc_i_; + diff_acc_i_m_s2 = -(mu_m3_s2_ / r_m3) * (q_func * spacecraft_position_i_m_ + diff_pos_i_m) + spacecraft_acceleration_i_m_s2_; rhs[0] = state[3]; rhs[1] = state[4]; @@ -80,7 +80,7 @@ void EnckeOrbitPropagation::RHS(double t, const Vector<6>& state, Vector<6>& rhs // Private Functions void EnckeOrbitPropagation::Initialize(double current_jd, Vector<3> init_ref_position_i_m, Vector<3> init_ref_velocity_i_m_s) { // General - fill_up(acc_i_, 0.0); + fill_up(spacecraft_acceleration_i_m_s2_, 0.0); // reference orbit ref_position_i_m_ = init_ref_position_i_m; @@ -99,8 +99,8 @@ void EnckeOrbitPropagation::Initialize(double current_jd, Vector<3> init_ref_pos } void EnckeOrbitPropagation::UpdateSatOrbit() { - sat_position_i_ = ref_position_i_m_ + diff_position_i_m_; - sat_velocity_i_ = ref_velocity_i_m_s_ + diff_velocity_i_m_s_; + spacecraft_position_i_m_ = ref_position_i_m_ + diff_position_i_m_; + spacecraft_velocity_i_m_s_ = ref_velocity_i_m_s_ + diff_velocity_i_m_s_; TransEciToEcef(); TransEcefToGeo(); @@ -108,10 +108,10 @@ void EnckeOrbitPropagation::UpdateSatOrbit() { double EnckeOrbitPropagation::CalcQFunction(Vector<3> diff_pos_i) { double r2; - r2 = inner_product(sat_position_i_, sat_position_i_); + r2 = inner_product(spacecraft_position_i_m_, spacecraft_position_i_m_); Vector<3> dr_2r; - dr_2r = diff_pos_i - 2.0 * sat_position_i_; + dr_2r = diff_pos_i - 2.0 * spacecraft_position_i_m_; double q = inner_product(diff_pos_i, dr_2r) / r2; diff --git a/src/dynamics/orbit/kepler_orbit_propagation.cpp b/src/dynamics/orbit/kepler_orbit_propagation.cpp index 1afcd0e23..2aedcaff4 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.cpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.cpp @@ -26,8 +26,8 @@ void KeplerOrbitPropagation::Propagate(double endtime, double current_jd) { // Private Function void KeplerOrbitPropagation::UpdateState(const double current_jd) { CalcPosVel(current_jd); - sat_position_i_ = position_i_m_; - sat_velocity_i_ = velocity_i_m_s_; + spacecraft_position_i_m_ = position_i_m_; + spacecraft_velocity_i_m_s_ = velocity_i_m_s_; TransEciToEcef(); TransEcefToGeo(); } diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index 444ab6e17..db05c0c7f 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -5,9 +5,10 @@ #include "orbit.hpp" Quaternion Orbit::CalcQuaternionI2LVLH() const { - libra::Vector<3> lvlh_x = sat_position_i_; // x-axis in LVLH frame is position vector direction from geocenter to satellite + libra::Vector<3> lvlh_x = spacecraft_position_i_m_; // x-axis in LVLH frame is position vector direction from geocenter to satellite libra::Vector<3> lvlh_ex = normalize(lvlh_x); - libra::Vector<3> lvlh_z = outer_product(sat_position_i_, sat_velocity_i_); // z-axis in LVLH frame is angular momentum vector direction of orbit + libra::Vector<3> lvlh_z = + outer_product(spacecraft_position_i_m_, spacecraft_velocity_i_m_s_); // z-axis in LVLH frame is angular momentum vector direction of orbit libra::Vector<3> lvlh_ez = normalize(lvlh_z); libra::Vector<3> lvlh_y = outer_product(lvlh_z, lvlh_x); libra::Vector<3> lvlh_ey = normalize(lvlh_y); @@ -29,17 +30,17 @@ Quaternion Orbit::CalcQuaternionI2LVLH() const { void Orbit::TransEciToEcef(void) { libra::Matrix<3, 3> dcm_i_to_xcxf = celestial_information_->GetEarthRotation().GetDcmJ2000ToXcxf(); - sat_position_ecef_ = dcm_i_to_xcxf * sat_position_i_; + spacecraft_position_ecef_m_ = dcm_i_to_xcxf * spacecraft_position_i_m_; // convert velocity vector in ECI to the vector in ECEF libra::Vector<3> OmegaE{0.0}; OmegaE[2] = environment::earth_mean_angular_velocity_rad_s; - libra::Vector<3> wExr = outer_product(OmegaE, sat_position_i_); - libra::Vector<3> V_wExr = sat_velocity_i_ - wExr; - sat_velocity_ecef_ = dcm_i_to_xcxf * V_wExr; + libra::Vector<3> wExr = outer_product(OmegaE, spacecraft_position_i_m_); + libra::Vector<3> V_wExr = spacecraft_velocity_i_m_s_ - wExr; + spacecraft_velocity_ecef_m_s_ = dcm_i_to_xcxf * V_wExr; } -void Orbit::TransEcefToGeo(void) { sat_position_geo_.UpdateFromEcef(sat_position_ecef_); } +void Orbit::TransEcefToGeo(void) { spacecraft_geodetic_position_.UpdateFromEcef(spacecraft_position_ecef_m_); } OrbitInitializeMode SetOrbitInitializeMode(const std::string initialize_mode) { if (initialize_mode == "DEFAULT") { @@ -70,13 +71,13 @@ std::string Orbit::GetLogHeader() const { std::string Orbit::GetLogValue() const { std::string str_tmp = ""; - str_tmp += WriteVector(sat_position_i_, 16); - str_tmp += WriteVector(sat_velocity_i_, 10); - str_tmp += WriteVector(sat_velocity_b_, 10); - str_tmp += WriteVector(acc_i_, 10); - str_tmp += WriteScalar(sat_position_geo_.GetLat_rad()); - str_tmp += WriteScalar(sat_position_geo_.GetLon_rad()); - str_tmp += WriteScalar(sat_position_geo_.GetAlt_m()); + str_tmp += WriteVector(spacecraft_position_i_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 += WriteScalar(spacecraft_geodetic_position_.GetLat_rad()); + str_tmp += WriteScalar(spacecraft_geodetic_position_.GetLon_rad()); + str_tmp += WriteScalar(spacecraft_geodetic_position_.GetAlt_m()); return str_tmp; } diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index ed0789e33..149110a72 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] q_i2b: End time of simulation [sec] */ - inline void UpdateAtt(libra::Quaternion q_i2b) { sat_velocity_b_ = q_i2b.frame_conv(sat_velocity_i_); } + inline void UpdateAtt(libra::Quaternion q_i2b) { spacecraft_velocity_b_m_s_ = q_i2b.frame_conv(spacecraft_velocity_i_m_s_); } /** * @fn AddPositionOffset @@ -94,42 +94,42 @@ class Orbit : public ILoggable { * @fn GetSatPosition_i * @brief Return spacecraft position in the inertial frame [m] */ - inline libra::Vector<3> GetSatPosition_i() const { return sat_position_i_; } + inline libra::Vector<3> GetSatPosition_i() const { return spacecraft_position_i_m_; } /** * @fn GetSatPosition_ecef * @brief Return spacecraft position in the ECEF frame [m] */ - inline libra::Vector<3> GetSatPosition_ecef() const { return sat_position_ecef_; } + inline libra::Vector<3> GetSatPosition_ecef() const { return spacecraft_position_ecef_m_; } /** * @fn GetSatVelocity_i * @brief Return spacecraft velocity in the inertial frame [m/s] */ - inline libra::Vector<3> GetSatVelocity_i() const { return sat_velocity_i_; } + inline libra::Vector<3> GetSatVelocity_i() const { return spacecraft_velocity_i_m_s_; } /** * @fn GetSatVelocity_b * @brief Return spacecraft velocity in the body fixed frame [m/s] */ - inline libra::Vector<3> GetSatVelocity_b() const { return sat_velocity_b_; } + inline libra::Vector<3> GetSatVelocity_b() const { return spacecraft_velocity_b_m_s_; } /** * @fn GetSatVelocity_ecef * @brief Return spacecraft velocity in the ECEF frame [m/s] */ - inline libra::Vector<3> GetSatVelocity_ecef() const { return sat_velocity_ecef_; } + inline libra::Vector<3> GetSatVelocity_ecef() const { return spacecraft_velocity_ecef_m_s_; } /** * @fn GetGeodeticPosition * @brief Return spacecraft position in the geodetic frame [m] */ - inline GeodeticPosition GetGeodeticPosition() const { return sat_position_geo_; } + inline GeodeticPosition GetGeodeticPosition() const { return spacecraft_geodetic_position_; } // TODO delete the following functions - inline double GetLat_rad() const { return sat_position_geo_.GetLat_rad(); } - inline double GetLon_rad() const { return sat_position_geo_.GetLon_rad(); } - inline double GetAlt_m() const { return sat_position_geo_.GetAlt_m(); } + inline double GetLat_rad() const { return spacecraft_geodetic_position_.GetLat_rad(); } + inline double GetLon_rad() const { return spacecraft_geodetic_position_.GetLon_rad(); } + inline double GetAlt_m() const { return spacecraft_geodetic_position_.GetAlt_m(); } inline libra::Vector<3> GetLatLonAlt() const { libra::Vector<3> vec; - vec(0) = sat_position_geo_.GetLat_rad(); - vec(1) = sat_position_geo_.GetLon_rad(); - vec(2) = sat_position_geo_.GetAlt_m(); + vec(0) = spacecraft_geodetic_position_.GetLat_rad(); + vec(1) = spacecraft_geodetic_position_.GetLon_rad(); + vec(2) = spacecraft_geodetic_position_.GetAlt_m(); return vec; } @@ -143,7 +143,7 @@ class Orbit : public ILoggable { * @fn SetAcceleration_i * @brief Set acceleration in the inertial frame [m/s2] */ - inline void SetAcceleration_i(libra::Vector<3> acceleration_i) { acc_i_ = acceleration_i; } + inline void SetAcceleration_i(libra::Vector<3> acceleration_i) { spacecraft_acceleration_i_m_s2_ = acceleration_i; } /** * @fn AddForce_i * @brief Add force @@ -152,13 +152,13 @@ class Orbit : public ILoggable { */ inline void AddForce_i(libra::Vector<3> force_i, double spacecraft_mass) { force_i /= spacecraft_mass; - acc_i_ += force_i; + spacecraft_acceleration_i_m_s2_ += force_i; } /** * @fn AddAcceleration_i_m_s2 * @brief Add acceleration in the inertial frame [m/s2] */ - inline void AddAcceleration_i_m_s2(libra::Vector<3> acceleration_i) { acc_i_ += acceleration_i; } + inline void AddAcceleration_i_m_s2(libra::Vector<3> acceleration_i) { spacecraft_acceleration_i_m_s2_ += acceleration_i; } /** * @fn AddForce_i * @brief Add force @@ -196,16 +196,16 @@ class Orbit : public ILoggable { bool is_calc_enabled_ = false; //!< Calculate flag OrbitPropagateMode propagate_mode_; //!< Propagation mode - libra::Vector<3> sat_position_i_; //!< Spacecraft position in the inertial frame [m] - libra::Vector<3> sat_position_ecef_; //!< Spacecraft position in the ECEF frame [m] - GeodeticPosition sat_position_geo_; //!< Spacecraft position in the Geodetic frame + libra::Vector<3> spacecraft_position_i_m_; //!< Spacecraft position in the inertial frame [m] + libra::Vector<3> spacecraft_position_ecef_m_; //!< Spacecraft position in the ECEF frame [m] + GeodeticPosition spacecraft_geodetic_position_; //!< Spacecraft position in the Geodetic frame - libra::Vector<3> sat_velocity_i_; //!< Spacecraft velocity in the inertial frame [m/s] - libra::Vector<3> sat_velocity_b_; //!< Spacecraft velocity in the body frame [m/s] - libra::Vector<3> sat_velocity_ecef_; //!< Spacecraft velocity in the ECEF frame [m/s] + libra::Vector<3> spacecraft_velocity_i_m_s_; //!< Spacecraft velocity in the inertial frame [m/s] + libra::Vector<3> spacecraft_velocity_b_m_s_; //!< Spacecraft velocity in the body frame [m/s] + libra::Vector<3> spacecraft_velocity_ecef_m_s_; //!< Spacecraft velocity in the ECEF frame [m/s] - libra::Vector<3> acc_i_; //!< Spacecraft acceleration in the inertial frame [m/s2] - //!< NOTE: Clear to zero at the end of the Propagate function + libra::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 968ad1972..cd6a1e4d2 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -35,14 +35,14 @@ void RelativeOrbit::InitializeState(Vector<3> initial_relative_position_lvlh, Ve relative_velocity_lvlh_ = initial_relative_velocity_lvlh; // Disturbance acceleration are not considered in relative orbit propagation - acc_i_ *= 0; + spacecraft_acceleration_i_m_s2_ *= 0; Vector<3> reference_sat_position_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetSatPosition_i(); Vector<3> reference_sat_velocity_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetSatVelocity_i(); Quaternion q_i2lvlh = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().CalcQuaternionI2LVLH(); Quaternion q_lvlh2i = q_i2lvlh.conjugate(); - sat_position_i_ = q_lvlh2i.frame_conv(relative_position_lvlh_) + reference_sat_position_i; - sat_velocity_i_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_) + reference_sat_velocity_i; + spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_) + reference_sat_position_i; + spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_) + reference_sat_velocity_i; initial_state_[0] = initial_relative_position_lvlh[0]; initial_state_[1] = initial_relative_position_lvlh[1]; @@ -94,7 +94,7 @@ void RelativeOrbit::Propagate(double endtime, double current_jd) { if (!is_calc_enabled_) return; - acc_i_ *= 0; // Disturbance acceleration are not considered in relative orbit propagation + spacecraft_acceleration_i_m_s2_ *= 0; // Disturbance acceleration are not considered in relative orbit propagation if (update_method_ == RK4) { PropagateRK4(endtime); @@ -108,8 +108,8 @@ void RelativeOrbit::Propagate(double endtime, double current_jd) { Quaternion q_i2lvlh = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().CalcQuaternionI2LVLH(); Quaternion q_lvlh2i = q_i2lvlh.conjugate(); - sat_position_i_ = q_lvlh2i.frame_conv(relative_position_lvlh_) + reference_sat_position_i; - sat_velocity_i_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_) + reference_sat_velocity_i; + spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_) + reference_sat_position_i; + spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_) + reference_sat_velocity_i; TransEciToEcef(); TransEcefToGeo(); } diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index 7b3d03852..2b65e4b22 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -17,7 +17,7 @@ Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_i prop_time_ = 0.0; prop_step_ = timestep; - acc_i_ *= 0; + spacecraft_acceleration_i_m_s2_ *= 0; Initialize(init_position, init_velocity, init_time); } @@ -33,9 +33,9 @@ void Rk4OrbitPropagation::RHS(double t, const Vector& state, Vector& rhs) rhs[0] = vx; rhs[1] = vy; rhs[2] = vz; - rhs[3] = acc_i_[0] - mu / r3 * x; - rhs[4] = acc_i_[1] - mu / r3 * y; - rhs[5] = acc_i_[2] - mu / r3 * z; + rhs[3] = spacecraft_acceleration_i_m_s2_[0] - mu / r3 * x; + rhs[4] = spacecraft_acceleration_i_m_s2_[1] - mu / r3 * y; + rhs[5] = spacecraft_acceleration_i_m_s2_[2] - mu / r3 * z; (void)t; } @@ -52,13 +52,13 @@ void Rk4OrbitPropagation::Initialize(Vector<3> init_position, Vector<3> init_vel setup(init_time, init_state); // initialize - acc_i_ *= 0; - sat_position_i_[0] = init_state[0]; - sat_position_i_[1] = init_state[1]; - sat_position_i_[2] = init_state[2]; - sat_velocity_i_[0] = init_state[3]; - sat_velocity_i_[1] = init_state[4]; - sat_velocity_i_[2] = init_state[5]; + spacecraft_acceleration_i_m_s2_ *= 0; + spacecraft_position_i_m_[0] = init_state[0]; + spacecraft_position_i_m_[1] = init_state[1]; + spacecraft_position_i_m_[2] = init_state[2]; + spacecraft_velocity_i_m_s_[0] = init_state[3]; + spacecraft_velocity_i_m_s_[1] = init_state[4]; + spacecraft_velocity_i_m_s_[2] = init_state[5]; TransEciToEcef(); TransEcefToGeo(); @@ -78,12 +78,12 @@ void Rk4OrbitPropagation::Propagate(double endtime, double current_jd) { Update(); prop_time_ = endtime; - sat_position_i_[0] = state()[0]; - sat_position_i_[1] = state()[1]; - sat_position_i_[2] = state()[2]; - sat_velocity_i_[0] = state()[3]; - sat_velocity_i_[1] = state()[4]; - sat_velocity_i_[2] = state()[5]; + spacecraft_position_i_m_[0] = state()[0]; + spacecraft_position_i_m_[1] = state()[1]; + spacecraft_position_i_m_[2] = state()[2]; + spacecraft_velocity_i_m_s_[0] = state()[3]; + spacecraft_velocity_i_m_s_[1] = state()[4]; + spacecraft_velocity_i_m_s_[2] = state()[5]; TransEciToEcef(); TransEcefToGeo(); @@ -95,7 +95,7 @@ void Rk4OrbitPropagation::AddPositionOffset(Vector<3> offset_i) { newstate[i] += offset_i[i]; } setup(x(), newstate); - sat_position_i_[0] = state()[0]; - sat_position_i_[1] = state()[1]; - sat_position_i_[2] = state()[2]; + spacecraft_position_i_m_[0] = state()[0]; + spacecraft_position_i_m_[1] = state()[1]; + spacecraft_position_i_m_[2] = state()[2]; } diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index cff8be535..2cd01023a 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -27,7 +27,7 @@ Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial twoline2rv(tle1, tle2, typerun, typeinput, whichconst_, startmfe, stopmfe, deltamin, satrec_); - acc_i_ *= 0; + spacecraft_acceleration_i_m_s2_ *= 0; // To calculate initial position and velocity is_calc_enabled_ = true; @@ -50,8 +50,8 @@ void Sgp4OrbitPropagation::Propagate(double endtime, double current_jd) { if (satrec_.error > 0) printf("# *** error: time:= %f *** code = %3d\n", satrec_.t, satrec_.error); for (int i = 0; i < 3; ++i) { - sat_position_i_[i] = r[i] * 1000; - sat_velocity_i_[i] = v[i] * 1000; + spacecraft_position_i_m_[i] = r[i] * 1000; + spacecraft_velocity_i_m_s_[i] = v[i] * 1000; } TransEciToEcef(); From 295c54f00343ca2d4c66f2d9998387ea9760db81 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:27:54 +0100 Subject: [PATCH 26/57] Fix private function name --- src/dynamics/orbit/encke_orbit_propagation.cpp | 4 ++-- src/dynamics/orbit/kepler_orbit_propagation.cpp | 4 ++-- src/dynamics/orbit/orbit.cpp | 4 ++-- src/dynamics/orbit/orbit.hpp | 8 ++++---- src/dynamics/orbit/relative_orbit.cpp | 8 ++++---- src/dynamics/orbit/rk4_orbit_propagation.cpp | 8 ++++---- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 4 ++-- 7 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index c97ebaa5a..df044315d 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -102,8 +102,8 @@ void EnckeOrbitPropagation::UpdateSatOrbit() { spacecraft_position_i_m_ = ref_position_i_m_ + diff_position_i_m_; spacecraft_velocity_i_m_s_ = ref_velocity_i_m_s_ + diff_velocity_i_m_s_; - TransEciToEcef(); - TransEcefToGeo(); + TransformEciToEcef(); + TransformEcefToGeodetic(); } double EnckeOrbitPropagation::CalcQFunction(Vector<3> diff_pos_i) { diff --git a/src/dynamics/orbit/kepler_orbit_propagation.cpp b/src/dynamics/orbit/kepler_orbit_propagation.cpp index 2aedcaff4..1f34aab83 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.cpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.cpp @@ -28,6 +28,6 @@ void KeplerOrbitPropagation::UpdateState(const double current_jd) { CalcPosVel(current_jd); spacecraft_position_i_m_ = position_i_m_; spacecraft_velocity_i_m_s_ = velocity_i_m_s_; - TransEciToEcef(); - TransEcefToGeo(); + TransformEciToEcef(); + TransformEcefToGeodetic(); } diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index db05c0c7f..8a8f6236f 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -28,7 +28,7 @@ Quaternion Orbit::CalcQuaternionI2LVLH() const { return q_i2lvlh.normalize(); } -void Orbit::TransEciToEcef(void) { +void Orbit::TransformEciToEcef(void) { libra::Matrix<3, 3> dcm_i_to_xcxf = celestial_information_->GetEarthRotation().GetDcmJ2000ToXcxf(); spacecraft_position_ecef_m_ = dcm_i_to_xcxf * spacecraft_position_i_m_; @@ -40,7 +40,7 @@ void Orbit::TransEciToEcef(void) { spacecraft_velocity_ecef_m_s_ = dcm_i_to_xcxf * V_wExr; } -void Orbit::TransEcefToGeo(void) { spacecraft_geodetic_position_.UpdateFromEcef(spacecraft_position_ecef_m_); } +void Orbit::TransformEcefToGeodetic(void) { spacecraft_geodetic_position_.UpdateFromEcef(spacecraft_position_ecef_m_); } OrbitInitializeMode SetOrbitInitializeMode(const std::string initialize_mode) { if (initialize_mode == "DEFAULT") { diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index 149110a72..455d6d09b 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -209,15 +209,15 @@ class Orbit : public ILoggable { // Frame Conversion TODO: consider other planet /** - * @fn TransEciToEcef + * @fn TransformEciToEcef * @brief Transform states from the ECI frame to ECEF frame */ - void TransEciToEcef(void); + void TransformEciToEcef(void); /** - * @fn TransEcefToGeo + * @fn TransformEcefToGeodetic * @brief Transform states from the ECEF frame to the geodetic frame */ - void TransEcefToGeo(void); + void TransformEcefToGeodetic(void); }; OrbitInitializeMode SetOrbitInitializeMode(const std::string initialize_mode); diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index cd6a1e4d2..80676ec9d 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -59,8 +59,8 @@ void RelativeOrbit::InitializeState(Vector<3> initial_relative_position_lvlh, Ve CalculateSTM(stm_model_type_, &(rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit()), mu, 0.0); } - TransEciToEcef(); - TransEcefToGeo(); + TransformEciToEcef(); + TransformEcefToGeodetic(); } void RelativeOrbit::CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double mu) { @@ -110,8 +110,8 @@ void RelativeOrbit::Propagate(double endtime, double current_jd) { spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_) + reference_sat_position_i; spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_) + reference_sat_velocity_i; - TransEciToEcef(); - TransEcefToGeo(); + TransformEciToEcef(); + TransformEcefToGeodetic(); } void RelativeOrbit::PropagateRK4(double elapsed_sec) { diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index 2b65e4b22..ccdee5a3f 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -60,8 +60,8 @@ void Rk4OrbitPropagation::Initialize(Vector<3> init_position, Vector<3> init_vel spacecraft_velocity_i_m_s_[1] = init_state[4]; spacecraft_velocity_i_m_s_[2] = init_state[5]; - TransEciToEcef(); - TransEcefToGeo(); + TransformEciToEcef(); + TransformEcefToGeodetic(); } void Rk4OrbitPropagation::Propagate(double endtime, double current_jd) { @@ -85,8 +85,8 @@ void Rk4OrbitPropagation::Propagate(double endtime, double current_jd) { spacecraft_velocity_i_m_s_[1] = state()[4]; spacecraft_velocity_i_m_s_[2] = state()[5]; - TransEciToEcef(); - TransEcefToGeo(); + TransformEciToEcef(); + TransformEcefToGeodetic(); } void Rk4OrbitPropagation::AddPositionOffset(Vector<3> offset_i) { diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index 2cd01023a..2fcd4092e 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -54,8 +54,8 @@ void Sgp4OrbitPropagation::Propagate(double endtime, double current_jd) { spacecraft_velocity_i_m_s_[i] = v[i] * 1000; } - TransEciToEcef(); - TransEcefToGeo(); + TransformEciToEcef(); + TransformEcefToGeodetic(); } Vector<3> Sgp4OrbitPropagation::GetESIOmega() { From d0d53c099cbde1e25c5b301512468a1cf0a2cca8 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:33:15 +0100 Subject: [PATCH 27/57] Fix function argument names --- .../orbit/encke_orbit_propagation.cpp | 20 ++++++------ .../orbit/encke_orbit_propagation.hpp | 17 +++++----- src/dynamics/orbit/initialize_orbit.cpp | 27 ++++++++-------- src/dynamics/orbit/initialize_orbit.hpp | 8 ++--- .../orbit/kepler_orbit_propagation.cpp | 15 +++++---- .../orbit/kepler_orbit_propagation.hpp | 14 ++++---- src/dynamics/orbit/orbit.hpp | 32 +++++++++---------- src/dynamics/orbit/relative_orbit.cpp | 8 ++--- src/dynamics/orbit/relative_orbit.hpp | 6 ++-- src/dynamics/orbit/rk4_orbit_propagation.cpp | 10 +++--- src/dynamics/orbit/rk4_orbit_propagation.hpp | 6 ++-- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 10 +++--- src/dynamics/orbit/sgp4_orbit_propagation.hpp | 10 +++--- 13 files changed, 93 insertions(+), 90 deletions(-) diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index df044315d..a9194032b 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -10,40 +10,40 @@ #include "../../library/orbit/orbital_elements.hpp" EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, const double prop_step_s, - const double current_jd, const Vector<3> init_position_i_m, const Vector<3> init_velocity_i_m_s, + const double current_time_jd, const Vector<3> init_position_i_m, const Vector<3> init_velocity_i_m_s, const double error_tolerance) : Orbit(celestial_information), libra::ODE<6>(prop_step_s), mu_m3_s2_(mu_m3_s2), error_tolerance_(error_tolerance), prop_step_s_(prop_step_s) { prop_time_s_ = 0.0; - Initialize(current_jd, init_position_i_m, init_velocity_i_m_s); + Initialize(current_time_jd, init_position_i_m, init_velocity_i_m_s); } EnckeOrbitPropagation::~EnckeOrbitPropagation() {} // Functions for Orbit -void EnckeOrbitPropagation::Propagate(double endtime, double current_jd) { +void EnckeOrbitPropagation::Propagate(double end_time_s, double current_time_jd) { if (!is_calc_enabled_) return; // Rectification double norm_sat_position_m = norm(spacecraft_position_i_m_); double norm_diff_position_m = norm(diff_position_i_m_); if (norm_diff_position_m / norm_sat_position_m > error_tolerance_) { - Initialize(current_jd, spacecraft_position_i_m_, spacecraft_velocity_i_m_s_); + Initialize(current_time_jd, spacecraft_position_i_m_, spacecraft_velocity_i_m_s_); } // Update reference orbit - ref_kepler_orbit.CalcPosVel(current_jd); + ref_kepler_orbit.CalcPosVel(current_time_jd); ref_position_i_m_ = ref_kepler_orbit.GetPosition_i_m(); ref_velocity_i_m_s_ = ref_kepler_orbit.GetVelocity_i_m_s(); // Propagate difference orbit setStepWidth(prop_step_s_); // Re-set propagation Δt - while (endtime - prop_time_s_ - prop_step_s_ > 1.0e-6) { + while (end_time_s - prop_time_s_ - prop_step_s_ > 1.0e-6) { Update(); // Propagation methods of the ODE class prop_time_s_ += prop_step_s_; } - setStepWidth(endtime - prop_time_s_); // Adjust the last propagation Δt + setStepWidth(end_time_s - prop_time_s_); // Adjust the last propagation Δt Update(); - prop_time_s_ = endtime; + prop_time_s_ = end_time_s; diff_position_i_m_[0] = state()[0]; diff_position_i_m_[1] = state()[1]; @@ -78,14 +78,14 @@ void EnckeOrbitPropagation::RHS(double t, const Vector<6>& state, Vector<6>& rhs } // Private Functions -void EnckeOrbitPropagation::Initialize(double current_jd, Vector<3> init_ref_position_i_m, Vector<3> init_ref_velocity_i_m_s) { +void EnckeOrbitPropagation::Initialize(double current_time_jd, Vector<3> init_ref_position_i_m, Vector<3> init_ref_velocity_i_m_s) { // General fill_up(spacecraft_acceleration_i_m_s2_, 0.0); // reference orbit ref_position_i_m_ = init_ref_position_i_m; ref_velocity_i_m_s_ = init_ref_velocity_i_m_s; - OrbitalElements oe_ref(mu_m3_s2_, current_jd, init_ref_position_i_m, init_ref_velocity_i_m_s); + OrbitalElements oe_ref(mu_m3_s2_, current_time_jd, init_ref_position_i_m, init_ref_velocity_i_m_s); ref_kepler_orbit = KeplerOrbit(mu_m3_s2_, oe_ref); // difference orbit diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index 8c1fd62ba..6461e021e 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -22,13 +22,14 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] celestial_information: Celestial information * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] prop_step_s: Propagation step width [sec] - * @param [in] current_jd: Current Julian day [day] + * @param [in] current_time_jd: Current Julian day [day] * @param [in] init_position_i_m: Initial value of position in the inertial frame [m] * @param [in] init_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 mu_m3_s2, const double prop_step_s, const double current_jd, - const Vector<3> init_position_i_m, const Vector<3> init_velocity_i_m_s, const double error_tolerance); + EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, const double prop_step_s, + const double current_time_jd, const Vector<3> init_position_i_m, const Vector<3> init_velocity_i_m_s, + const double error_tolerance); /** * @fn ~EnckeOrbitPropagation * @brief Destructor @@ -39,10 +40,10 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { /** * @fn Propagate * @brief Propagate orbit - * @param [in] endtime: End time of simulation [sec] - * @param [in] current_jd: Current Julian day [day] + * @param [in] end_time_s: End time of simulation [sec] + * @param [in] current_time_jd: Current Julian day [day] */ - virtual void Propagate(double endtime, double current_jd); + virtual void Propagate(double end_time_s, double current_time_jd); // Override ODE /** @@ -74,11 +75,11 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { /** * @fn Initialize * @brief Initialize function - * @param [in] current_jd: Current Julian day [day] + * @param [in] current_time_jd: Current Julian day [day] * @param [in] init_ref_position_i_m: Initial value of reference orbit position in the inertial frame [m] * @param [in] init_ref_velocity_i_m_s: Initial value of reference orbit position in the inertial frame [m] */ - void Initialize(double current_jd, Vector<3> init_ref_position_i_m, Vector<3> init_ref_velocity_i_m_s); + void Initialize(double current_time_jd, Vector<3> init_ref_position_i_m, Vector<3> init_ref_velocity_i_m_s); /** * @fn UpdateSatOrbit * @brief Update satellite orbit diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index f60e7f96a..908effed2 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -12,8 +12,8 @@ #include "rk4_orbit_propagation.hpp" #include "sgp4_orbit_propagation.hpp" -Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string ini_path, double stepSec, double current_jd, double gravity_constant, - std::string section, RelativeInformation* rel_info) { +Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string ini_path, double stepSec, double current_time_jd, + double gravity_constant, std::string section, RelativeInformation* rel_info) { auto conf = IniAccess(ini_path); const char* section_ = section.c_str(); Orbit* orbit; @@ -28,7 +28,7 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // initialize RK4 orbit propagator Vector<3> position_i_m; Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(ini_path, current_jd, gravity_constant); + Vector<6> pos_vel = InitializePosVel(ini_path, current_time_jd, gravity_constant); for (size_t i = 0; i < 3; i++) { position_i_m[i] = pos_vel[i]; velocity_i_m_s[i] = pos_vel[i + 3]; @@ -41,7 +41,7 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string conf.ReadChar(section_, "tle1", 80, tle1); conf.ReadChar(section_, "tle2", 80, tle2); - orbit = new Sgp4OrbitPropagation(celestial_information, tle1, tle2, wgs, current_jd); + orbit = new Sgp4OrbitPropagation(celestial_information, tle1, tle2, wgs, current_time_jd); } else if (propagate_mode == "RELATIVE") { // initialize orbit for relative dynamics of formation flying RelativeOrbit::RelativeOrbitUpdateMethod update_method = @@ -58,8 +58,8 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // the orbit of the reference sat is initialized, create temporary initial orbit of the reference sat int reference_sat_id = conf.ReadInt(section_, "reference_satellite_id"); - orbit = new RelativeOrbit(celestial_information, gravity_constant, stepSec, reference_sat_id, init_relative_position_lvlh, init_relative_velocity_lvlh, - update_method, relative_dynamics_model_type, stm_model_type, rel_info); + orbit = new RelativeOrbit(celestial_information, gravity_constant, stepSec, reference_sat_id, init_relative_position_lvlh, + init_relative_velocity_lvlh, update_method, relative_dynamics_model_type, stm_model_type, rel_info); } else if (propagate_mode == "KEPLER") { // initialize orbit for Kepler propagation double mu_m3_s2 = gravity_constant; @@ -71,7 +71,7 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string conf.ReadVector<3>(section_, "initial_position_i_m", init_pos_m); Vector<3> init_vel_m_s; conf.ReadVector<3>(section_, "initial_velocity_i_m_s", init_vel_m_s); - oe = OrbitalElements(mu_m3_s2, current_jd, init_pos_m, init_vel_m_s); + oe = OrbitalElements(mu_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"); @@ -83,26 +83,27 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string oe = OrbitalElements(epoch_jday, semi_major_axis_m, eccentricity, inclination_rad, raan_rad, arg_perigee_rad); } KeplerOrbit kepler_orbit(mu_m3_s2, oe); - orbit = new KeplerOrbitPropagation(celestial_information, current_jd, kepler_orbit); + orbit = new KeplerOrbitPropagation(celestial_information, current_time_jd, kepler_orbit); } else if (propagate_mode == "ENCKE") { // initialize orbit for Encke's method Vector<3> position_i_m; Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(ini_path, current_jd, gravity_constant); + Vector<6> pos_vel = InitializePosVel(ini_path, current_time_jd, gravity_constant); for (size_t i = 0; i < 3; i++) { position_i_m[i] = pos_vel[i]; velocity_i_m_s[i] = pos_vel[i + 3]; } double error_tolerance = conf.ReadDouble(section_, "error_tolerance"); - orbit = new EnckeOrbitPropagation(celestial_information, gravity_constant, stepSec, current_jd, position_i_m, velocity_i_m_s, error_tolerance); + orbit = + new EnckeOrbitPropagation(celestial_information, gravity_constant, stepSec, current_time_jd, position_i_m, velocity_i_m_s, error_tolerance); } else { 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; Vector<3> position_i_m; Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(ini_path, current_jd, gravity_constant); + Vector<6> pos_vel = InitializePosVel(ini_path, current_time_jd, gravity_constant); 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,7 +116,7 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string return orbit; } -Vector<6> InitializePosVel(std::string ini_path, double current_jd, double mu_m3_s2, std::string section) { +Vector<6> InitializePosVel(std::string ini_path, double current_time_jd, double mu_m3_s2, std::string section) { auto conf = IniAccess(ini_path); const char* section_ = section.c_str(); Vector<3> position_i_m; @@ -133,7 +134,7 @@ Vector<6> InitializePosVel(std::string ini_path, double current_jd, double mu_m3 OrbitalElements oe(epoch_jday, semi_major_axis_m, eccentricity, inclination_rad, raan_rad, arg_perigee_rad); KeplerOrbit kepler_orbit(mu_m3_s2, oe); - kepler_orbit.CalcPosVel(current_jd); + kepler_orbit.CalcPosVel(current_time_jd); position_i_m = kepler_orbit.GetPosition_i_m(); velocity_i_m_s = kepler_orbit.GetVelocity_i_m_s(); } else if (initialize_mode == OrbitInitializeMode::kInertialPositionAndVelocity) { diff --git a/src/dynamics/orbit/initialize_orbit.hpp b/src/dynamics/orbit/initialize_orbit.hpp index 4379d7ed9..881093eb0 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -18,22 +18,22 @@ class RelativeInformation; * @param [in] celestial_information: Celestial information * @param [in] ini_path: Path to initialize file * @param [in] stepSec: Step width [sec] - * @param [in] current_jd: Current Julian day [day] + * @param [in] current_time_jd: Current Julian day [day] * @param [in] gravity_constant: Gravity constant [m3/s2] * @param [in] section: Section name * @param [in] rel_info: Relative information */ -Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string ini_path, double stepSec, double current_jd, double gravity_constant, +Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string ini_path, double stepSec, double current_time_jd, double gravity_constant, std::string section = "ORBIT", RelativeInformation* rel_info = (RelativeInformation*)nullptr); /** * @fn InitializePosVel * @brief Initialize position and velocity depends on initialize mode * @param [in] ini_path: Path to initialize file - * @param [in] current_jd: Current Julian day [day] + * @param [in] current_time_jd: Current Julian day [day] * @param [in] mu_m3_s2: Gravity constant [m3/s2] * @param [in] section: Section name */ -Vector<6> InitializePosVel(std::string ini_path, double current_jd, double mu_m3_s2, std::string section = "ORBIT"); +Vector<6> InitializePosVel(std::string ini_path, double current_time_jd, double mu_m3_s2, std::string section = "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 1f34aab83..9ce051cd2 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.cpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.cpp @@ -8,24 +8,25 @@ #include "../../library/math/s2e_math.hpp" -KeplerOrbitPropagation::KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_jd, KeplerOrbit kepler_orbit) +KeplerOrbitPropagation::KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_time_jd, + KeplerOrbit kepler_orbit) : Orbit(celestial_information), KeplerOrbit(kepler_orbit) { - UpdateState(current_jd); + UpdateState(current_time_jd); } KeplerOrbitPropagation::~KeplerOrbitPropagation() {} -void KeplerOrbitPropagation::Propagate(double endtime, double current_jd) { - UNUSED(endtime); +void KeplerOrbitPropagation::Propagate(double end_time_s, double current_time_jd) { + UNUSED(end_time_s); if (!is_calc_enabled_) return; - UpdateState(current_jd); + UpdateState(current_time_jd); } // Private Function -void KeplerOrbitPropagation::UpdateState(const double current_jd) { - CalcPosVel(current_jd); +void KeplerOrbitPropagation::UpdateState(const double current_time_jd) { + CalcPosVel(current_time_jd); spacecraft_position_i_m_ = position_i_m_; spacecraft_velocity_i_m_s_ = velocity_i_m_s_; TransformEciToEcef(); diff --git a/src/dynamics/orbit/kepler_orbit_propagation.hpp b/src/dynamics/orbit/kepler_orbit_propagation.hpp index ad401128f..3a7122a8d 100644 --- a/src/dynamics/orbit/kepler_orbit_propagation.hpp +++ b/src/dynamics/orbit/kepler_orbit_propagation.hpp @@ -20,10 +20,10 @@ class KeplerOrbitPropagation : public Orbit, public KeplerOrbit { * @fn KeplerOrbitPropagation * @brief Constructor * @param [in] celestial_information: Celestial information - * @param [in] current_jd: Current Julian day [day] + * @param [in] current_time_jd: Current Julian day [day] * @param [in] kepler_orbit: Kepler orbit */ - KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_jd, KeplerOrbit kepler_orbit); + KeplerOrbitPropagation(const CelestialInformation* celestial_information, const double current_time_jd, KeplerOrbit kepler_orbit); /** * @fn ~KeplerOrbitPropagation * @brief Destructor @@ -34,18 +34,18 @@ class KeplerOrbitPropagation : public Orbit, public KeplerOrbit { /** * @fn Propagate * @brief Propagate orbit - * @param [in] endtime: End time of simulation [sec] - * @param [in] current_jd: Current Julian day [day] + * @param [in] end_time_s: End time of simulation [sec] + * @param [in] current_time_jd: Current Julian day [day] */ - virtual void Propagate(double endtime, double current_jd); + virtual void Propagate(double end_time_s, double current_time_jd); private: /** * @fn UpdateState * @brief Propagate orbit - * @param [in] current_jd: Current Julian day [day] + * @param [in] current_time_jd: Current Julian day [day] */ - void UpdateState(const double current_jd); + void UpdateState(const double current_time_jd); }; #endif // S2E_DYNAMICS_ORBIT_KEPLER_ORBIT_PROPAGATION_HPP_ diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index 455d6d09b..5a1a82b68 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -59,17 +59,17 @@ class Orbit : public ILoggable { /** * @fn Propagate * @brief Pure virtual function for orbit propagation - * @param [in] endtime: End time of simulation [sec] - * @param [in] current_jd: Current Julian day [day] + * @param [in] end_time_s: End time of simulation [sec] + * @param [in] current_time_jd: Current Julian day [day] */ - virtual void Propagate(double endtime, double current_jd) = 0; + virtual void Propagate(double end_time_s, double current_time_jd) = 0; /** * @fn UpdateAtt * @brief Update attitude information - * @param [in] q_i2b: End time of simulation [sec] + * @param [in] quaternion_i2b: End time of simulation [sec] */ - inline void UpdateAtt(libra::Quaternion q_i2b) { spacecraft_velocity_b_m_s_ = q_i2b.frame_conv(spacecraft_velocity_i_m_s_); } + inline void UpdateAtt(libra::Quaternion quaternion_i2b) { spacecraft_velocity_b_m_s_ = quaternion_i2b.frame_conv(spacecraft_velocity_i_m_s_); } /** * @fn AddPositionOffset @@ -143,32 +143,32 @@ class Orbit : public ILoggable { * @fn SetAcceleration_i * @brief Set acceleration in the inertial frame [m/s2] */ - inline void SetAcceleration_i(libra::Vector<3> acceleration_i) { spacecraft_acceleration_i_m_s2_ = acceleration_i; } + inline void SetAcceleration_i(libra::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ = acceleration_i_m_s2; } /** * @fn AddForce_i * @brief Add force * @param [in] force_i: Force in the inertial frame [N] - * @param [in] spacecraft_mass: Mass of spacecraft [kg] + * @param [in] spacecraft_mass_kg: Mass of spacecraft [kg] */ - inline void AddForce_i(libra::Vector<3> force_i, double spacecraft_mass) { - force_i /= spacecraft_mass; + inline void AddForce_i(libra::Vector<3> force_i, double spacecraft_mass_kg) { + force_i /= spacecraft_mass_kg; spacecraft_acceleration_i_m_s2_ += force_i; } /** * @fn AddAcceleration_i_m_s2 * @brief Add acceleration in the inertial frame [m/s2] */ - inline void AddAcceleration_i_m_s2(libra::Vector<3> acceleration_i) { spacecraft_acceleration_i_m_s2_ += acceleration_i; } + inline void AddAcceleration_i_m_s2(libra::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ += acceleration_i_m_s2; } /** * @fn AddForce_i * @brief Add force - * @param [in] force_b: Force in the body fixed frame [N] - * @param [in] q_i2b: Quaternion from the inertial frame to the body fixed frame - * @param [in] spacecraft_mass: Mass of spacecraft [kg] + * @param [in] force_b_N: Force in the body fixed frame [N] + * @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(libra::Vector<3> force_b, libra::Quaternion q_i2b, double spacecraft_mass) { - auto force_i = q_i2b.frame_conv_inv(force_b); - AddForce_i(force_i, spacecraft_mass); + inline void AddForce_b_N(libra::Vector<3> force_b_N, libra::Quaternion quaternion_i2b, double spacecraft_mass_kg) { + auto force_i = quaternion_i2b.frame_conv_inv(force_b_N); + AddForce_i(force_i, spacecraft_mass_kg); } /** diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 80676ec9d..fc87c9a63 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -89,18 +89,18 @@ void RelativeOrbit::CalculateSTM(STMModel stm_model_type, const Orbit* reference } } -void RelativeOrbit::Propagate(double endtime, double current_jd) { - UNUSED(current_jd); +void RelativeOrbit::Propagate(double end_time_s, double current_time_jd) { + UNUSED(current_time_jd); if (!is_calc_enabled_) return; spacecraft_acceleration_i_m_s2_ *= 0; // Disturbance acceleration are not considered in relative orbit propagation if (update_method_ == RK4) { - PropagateRK4(endtime); + PropagateRK4(end_time_s); } else // update_method_ == STM { - PropagateSTM(endtime); + PropagateSTM(end_time_s); } Vector<3> reference_sat_position_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetSatPosition_i(); diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 13db23084..5da09aad1 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -51,10 +51,10 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { /** * @fn Propagate * @brief Propagate orbit - * @param [in] endtime: End time of simulation [sec] - * @param [in] current_jd: Current Julian day [day] + * @param [in] end_time_s: End time of simulation [sec] + * @param [in] current_time_jd: Current Julian day [day] */ - virtual void Propagate(double endtime, double current_jd); + virtual void Propagate(double end_time_s, double current_time_jd); // Override ODE /** diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index ccdee5a3f..c77b8cca7 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -64,19 +64,19 @@ void Rk4OrbitPropagation::Initialize(Vector<3> init_position, Vector<3> init_vel TransformEcefToGeodetic(); } -void Rk4OrbitPropagation::Propagate(double endtime, double current_jd) { - UNUSED(current_jd); +void Rk4OrbitPropagation::Propagate(double end_time_s, double current_time_jd) { + UNUSED(current_time_jd); if (!is_calc_enabled_) return; setStepWidth(prop_step_); // Re-set propagation Δt - while (endtime - prop_time_ - prop_step_ > 1.0e-6) { + while (end_time_s - prop_time_ - prop_step_ > 1.0e-6) { Update(); // Propagation methods of the ODE class prop_time_ += prop_step_; } - setStepWidth(endtime - prop_time_); // Adjust the last propagation Δt + setStepWidth(end_time_s - prop_time_); // Adjust the last propagation Δt Update(); - prop_time_ = endtime; + prop_time_ = end_time_s; spacecraft_position_i_m_[0] = state()[0]; spacecraft_position_i_m_[1] = state()[1]; diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index 2ddbaaf9a..65986b7a0 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -53,10 +53,10 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { /** * @fn Propagate * @brief Propagate orbit - * @param [in] endtime: End time of simulation [sec] - * @param [in] current_jd: Current Julian day [day] + * @param [in] end_time_s: End time of simulation [sec] + * @param [in] current_time_jd: Current Julian day [day] */ - virtual void Propagate(double endtime, double current_jd); + virtual void Propagate(double end_time_s, double current_time_jd); /** * @fn AddPositionOffset diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index 2fcd4092e..02c23137d 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -10,7 +10,7 @@ #include using namespace std; -Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, int wgs, double current_jd) +Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, int wgs, double current_time_jd) : Orbit(celestial_information) { propagate_mode_ = OrbitPropagateMode::kSgp4; @@ -31,15 +31,15 @@ Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial // To calculate initial position and velocity is_calc_enabled_ = true; - Propagate(0.0, current_jd); + Propagate(0.0, current_time_jd); is_calc_enabled_ = false; } -void Sgp4OrbitPropagation::Propagate(double endtime, double current_jd) { - UNUSED(endtime); +void Sgp4OrbitPropagation::Propagate(double end_time_s, double current_time_jd) { + UNUSED(end_time_s); if (!is_calc_enabled_) return; - double elapse_time_min = (current_jd - satrec_.jdsatepoch) * (24.0 * 60.0); + double elapse_time_min = (current_time_jd - satrec_.jdsatepoch) * (24.0 * 60.0); double r[3]; double v[3]; diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.hpp b/src/dynamics/orbit/sgp4_orbit_propagation.hpp index 50181bfa8..9e84bc096 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.hpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.hpp @@ -26,18 +26,18 @@ class Sgp4OrbitPropagation : public Orbit { * @param [in] tle1: The first line of TLE * @param [in] tle2: The second line of TLE * @param [in] wgs: Wold Geodetic System - * @param [in] current_jd: Current Julian day [day] + * @param [in] current_time_jd: Current Julian day [day] */ - Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, int wgs, double current_jd); + Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, int wgs, double current_time_jd); // Override Orbit /** * @fn Propagate * @brief Propagate orbit - * @param [in] endtime: End time of simulation [sec] - * @param [in] current_jd: Current Julian day [day] + * @param [in] end_time_s: End time of simulation [sec] + * @param [in] current_time_jd: Current Julian day [day] */ - virtual void Propagate(double endtime, double current_jd); + virtual void Propagate(double end_time_s, double current_time_jd); /** * @fn GetESIOmega From 497cccd2b445c98bed84901cedadf4493cf395d7 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:34:18 +0100 Subject: [PATCH 28/57] Delete unused public functions --- src/dynamics/orbit/orbit.hpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index 5a1a82b68..0217c8137 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -71,14 +71,6 @@ class Orbit : public ILoggable { */ inline void UpdateAtt(libra::Quaternion quaternion_i2b) { spacecraft_velocity_b_m_s_ = quaternion_i2b.frame_conv(spacecraft_velocity_i_m_s_); } - /** - * @fn AddPositionOffset - * @brief Shift the position of the spacecraft - * @note Is this really needed? - * @param [in] offset_i: Offset vector in the inertial frame [m] - */ - inline virtual void AddPositionOffset(libra::Vector<3> offset_i) { (void)offset_i; } - // Getters /** * @fn GetIsCalcEnabled From 44464d6aa0ef5ef74d9d72857d6b5656e812eead Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:39:48 +0100 Subject: [PATCH 29/57] Fix local function variables --- src/dynamics/orbit/orbit.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index 8a8f6236f..6b195662a 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -33,11 +33,11 @@ void Orbit::TransformEciToEcef(void) { spacecraft_position_ecef_m_ = dcm_i_to_xcxf * spacecraft_position_i_m_; // convert velocity vector in ECI to the vector in ECEF - libra::Vector<3> OmegaE{0.0}; - OmegaE[2] = environment::earth_mean_angular_velocity_rad_s; - libra::Vector<3> wExr = outer_product(OmegaE, spacecraft_position_i_m_); - libra::Vector<3> V_wExr = spacecraft_velocity_i_m_s_ - wExr; - spacecraft_velocity_ecef_m_s_ = dcm_i_to_xcxf * V_wExr; + libra::Vector<3> earth_angular_velocity_i_rad_s{0.0}; + earth_angular_velocity_i_rad_s[2] = environment::earth_mean_angular_velocity_rad_s; + libra::Vector<3> we_cross_r = outer_product(earth_angular_velocity_i_rad_s, spacecraft_position_i_m_); + libra::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; } void Orbit::TransformEcefToGeodetic(void) { spacecraft_geodetic_position_.UpdateFromEcef(spacecraft_position_ecef_m_); } From 18ff49d5eb09633280953afa5e5535d437f2825a Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:47:59 +0100 Subject: [PATCH 30/57] Fix public function name --- src/components/ideal/force_generator.cpp | 4 +- src/components/real/aocs/gnss_receiver.cpp | 6 +-- .../ground_station_calculator.cpp | 4 +- src/disturbances/air_drag.cpp | 2 +- src/disturbances/geopotential.cpp | 4 +- src/disturbances/third_body_gravity.cpp | 2 +- src/dynamics/attitude/controlled_attitude.cpp | 4 +- src/dynamics/dynamics.cpp | 6 +-- src/dynamics/orbit/orbit.cpp | 2 +- src/dynamics/orbit/orbit.hpp | 42 ++++++++++--------- src/dynamics/orbit/relative_orbit.cpp | 16 +++---- src/environment/local/local_environment.cpp | 2 +- src/simulation/case/sample_case.cpp | 4 +- .../ground_station/ground_station.cpp | 2 +- .../relative_information.cpp | 24 +++++------ 15 files changed, 63 insertions(+), 61 deletions(-) diff --git a/src/components/ideal/force_generator.cpp b/src/components/ideal/force_generator.cpp index a88bd0e7e..2e47cdbef 100644 --- a/src/components/ideal/force_generator.cpp +++ b/src/components/ideal/force_generator.cpp @@ -37,7 +37,7 @@ void ForceGenerator::MainRoutine(int count) { // Convert frame libra::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); - libra::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternionI2LVLH(); + libra::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh(); generated_force_i_N_ = q_i2b.frame_conv_inv(generated_force_b_N_); generated_force_rtn_N_ = q_i2rtn.frame_conv(generated_force_i_N_); } @@ -55,7 +55,7 @@ void ForceGenerator::SetForce_i_N(const libra::Vector<3> force_i_N) { void ForceGenerator::SetForce_rtn_N(const libra::Vector<3> force_rtn_N) { libra::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); - libra::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternionI2LVLH(); + libra::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh(); libra::Vector<3> force_i_N = q_i2rtn.frame_conv_inv(force_rtn_N); ordered_force_b_N_ = q_i2b.frame_conv(force_i_N); diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index c037bae9a..215df98a8 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -48,15 +48,15 @@ GNSSReceiver::GNSSReceiver(const int prescaler, ClockGenerator* clock_gen, Power void GNSSReceiver::MainRoutine(int count) { UNUSED(count); - Vector<3> pos_true_eci_ = dynamics_->GetOrbit().GetSatPosition_i(); + Vector<3> pos_true_eci_ = dynamics_->GetOrbit().GetPosition_i_m(); Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); CheckAntenna(pos_true_eci_, q_i2b); if (is_gnss_sats_visible_ == 1) { // Antenna of GNSS-R can detect GNSS signal - position_ecef_ = dynamics_->GetOrbit().GetSatPosition_ecef(); + position_ecef_ = dynamics_->GetOrbit().GetPosition_ecef_m(); position_llh_ = dynamics_->GetOrbit().GetLatLonAlt(); - velocity_ecef_ = dynamics_->GetOrbit().GetSatVelocity_ecef(); + velocity_ecef_ = dynamics_->GetOrbit().GetVelocity_ecef_m_s(); AddNoise(pos_true_eci_, position_ecef_); utc_ = simtime_->GetCurrentUtc(); diff --git a/src/components/real/communication/ground_station_calculator.cpp b/src/components/real/communication/ground_station_calculator.cpp index ced6c75cd..6b53caeae 100644 --- a/src/components/real/communication/ground_station_calculator.cpp +++ b/src/components/real/communication/ground_station_calculator.cpp @@ -65,7 +65,7 @@ double GScalculator::CalcCn0OnGs(const Dynamics& dynamics, const Antenna& sc_tx_ } // Free space path loss - Vector<3> sc_pos_i = dynamics.GetOrbit().GetSatPosition_i(); + Vector<3> sc_pos_i = dynamics.GetOrbit().GetPosition_i_m(); Vector<3> gs_pos_i = ground_station.GetGSPosition_i(); double dist_sc_gs_km = norm(sc_pos_i - gs_pos_i) / 1000.0; double loss_space_dB = -20.0 * log10(4.0 * libra::pi * dist_sc_gs_km / (300.0 / sc_tx_ant.GetFrequency() / 1000.0)); @@ -79,7 +79,7 @@ double GScalculator::CalcCn0OnGs(const Dynamics& dynamics, const Antenna& sc_tx_ double phi_on_sc_ant_rad = acos(gs_direction_on_sc_frame[0] / sin(theta_on_sc_ant_rad)); // SC direction on GS RX antenna frame - Vector<3> gs_to_sc_ecef = dynamics.GetOrbit().GetSatPosition_ecef() - ground_station.GetGSPosition_ecef(); + Vector<3> gs_to_sc_ecef = dynamics.GetOrbit().GetPosition_ecef_m() - ground_station.GetGSPosition_ecef(); gs_to_sc_ecef = libra::normalize(gs_to_sc_ecef); Quaternion q_ecef_to_gs_ant = gs_rx_ant.GetQuaternion_b2c() * ground_station.GetGSPosition_geo().GetQuaternionXcxfToLtc(); Vector<3> sc_direction_on_gs_frame = q_ecef_to_gs_ant.frame_conv(gs_to_sc_ecef); diff --git a/src/disturbances/air_drag.cpp b/src/disturbances/air_drag.cpp index adfca7f9f..a57d4307c 100644 --- a/src/disturbances/air_drag.cpp +++ b/src/disturbances/air_drag.cpp @@ -24,7 +24,7 @@ AirDrag::AirDrag(const vector& surfaces, const libra::Vector<3>& center void AirDrag::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { double air_density_kg_m3 = local_environment.GetAtmosphere().GetAirDensity_kg_m3(); - Vector<3> velocity_b_m_s = dynamics.GetOrbit().GetSatVelocity_b(); + Vector<3> velocity_b_m_s = dynamics.GetOrbit().GetVelocity_b_m_s(); CalcTorqueForce(velocity_b_m_s, air_density_kg_m3); } diff --git a/src/disturbances/geopotential.cpp b/src/disturbances/geopotential.cpp index 9243178be..8b6628c73 100644 --- a/src/disturbances/geopotential.cpp +++ b/src/disturbances/geopotential.cpp @@ -69,10 +69,10 @@ void GeoPotential::Update(const LocalEnvironment &local_environment, const Dynam #ifdef DEBUG_GEOPOTENTIAL chrono::system_clock::time_point start, end; start = chrono::system_clock::now(); - debug_pos_ecef_m_ = spacecraft.dynamics_->orbit_->GetSatPosition_ecef(); + debug_pos_ecef_m_ = spacecraft.dynamics_->orbit_->GetPosition_ecef_m(); #endif - CalcAccelerationEcef(dynamics.GetOrbit().GetSatPosition_ecef()); + CalcAccelerationEcef(dynamics.GetOrbit().GetPosition_ecef_m()); #ifdef DEBUG_GEOPOTENTIAL end = chrono::system_clock::now(); time_ms_ = static_cast(chrono::duration_cast(end - start).count() / 1000.0); diff --git a/src/disturbances/third_body_gravity.cpp b/src/disturbances/third_body_gravity.cpp index aeaec6197..7cbeed512 100644 --- a/src/disturbances/third_body_gravity.cpp +++ b/src/disturbances/third_body_gravity.cpp @@ -15,7 +15,7 @@ ThirdBodyGravity::~ThirdBodyGravity() {} void ThirdBodyGravity::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { acceleration_i_m_s2_ = libra::Vector<3>(0.0); // initialize - libra::Vector<3> sc_position_i_m = dynamics.GetOrbit().GetSatPosition_i(); + libra::Vector<3> sc_position_i_m = dynamics.GetOrbit().GetPosition_i_m(); for (auto third_body : third_body_list_) { libra::Vector<3> third_body_position_from_sc_i_m = local_environment.GetCelestialInformation().GetPositionFromSpacecraft_i_m(third_body.c_str()); libra::Vector<3> third_body_pos_i_m = sc_position_i_m + third_body_position_from_sc_i_m; diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index eeae34e91..497f38cf0 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -85,9 +85,9 @@ Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMode mode) { } else if (mode == EARTH_CENTER_POINTING) { direction = local_celestial_information_->GetPositionFromSpacecraft_i_m("EARTH"); } else if (mode == VELOCITY_DIRECTION_POINTING) { - direction = orbit_->GetSatVelocity_i(); + direction = orbit_->GetVelocity_i_m_s(); } else if (mode == ORBIT_NORMAL_POINTING) { - direction = outer_product(orbit_->GetSatPosition_i(), orbit_->GetSatVelocity_i()); + direction = outer_product(orbit_->GetPosition_i_m(), orbit_->GetVelocity_i_m_s()); } normalize(direction); return direction; diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp index c000bafc2..82483971b 100644 --- a/src/dynamics/dynamics.cpp +++ b/src/dynamics/dynamics.cpp @@ -33,7 +33,7 @@ void Dynamics::Initialize(SimulationConfig* simulation_configuration, const Simu temperature_ = InitTemperature(simulation_configuration->sat_file_[spacecraft_id], simulation_time->GetThermalRkStepTime_s()); // To get initial value - orbit_->UpdateAtt(attitude_->GetQuaternion_i2b()); + orbit_->UpdateByAttitude(attitude_->GetQuaternion_i2b()); } void Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestialInformation* local_celestial_information) { @@ -46,7 +46,7 @@ void Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestia orbit_->Propagate(simulation_time->GetElapsedTime_s(), simulation_time->GetCurrentTime_jd()); } // Attitude dependent update - orbit_->UpdateAtt(attitude_->GetQuaternion_i2b()); + orbit_->UpdateByAttitude(attitude_->GetQuaternion_i2b()); // Thermal if (simulation_time->GetThermalPropagateFlag()) { @@ -61,7 +61,7 @@ void Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestia void Dynamics::ClearForceTorque(void) { libra::Vector<3> zero(0.0); attitude_->SetTorque_b(zero); - orbit_->SetAcceleration_i(zero); + orbit_->SetAcceleration_i_m_s2(zero); } void Dynamics::LogSetup(Logger& logger) { diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index 6b195662a..58e2bb9e9 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -4,7 +4,7 @@ */ #include "orbit.hpp" -Quaternion Orbit::CalcQuaternionI2LVLH() const { +Quaternion Orbit::CalcQuaternion_i2lvlh() const { libra::Vector<3> lvlh_x = spacecraft_position_i_m_; // x-axis in LVLH frame is position vector direction from geocenter to satellite libra::Vector<3> lvlh_ex = normalize(lvlh_x); libra::Vector<3> lvlh_z = diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index 0217c8137..a7d529d18 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -65,11 +65,13 @@ class Orbit : public ILoggable { virtual void Propagate(double end_time_s, double current_time_jd) = 0; /** - * @fn UpdateAtt + * @fn UpdateByAttitude * @brief Update attitude information * @param [in] quaternion_i2b: End time of simulation [sec] */ - inline void UpdateAtt(libra::Quaternion quaternion_i2b) { spacecraft_velocity_b_m_s_ = quaternion_i2b.frame_conv(spacecraft_velocity_i_m_s_); } + inline void UpdateByAttitude(libra::Quaternion quaternion_i2b) { + spacecraft_velocity_b_m_s_ = quaternion_i2b.frame_conv(spacecraft_velocity_i_m_s_); + } // Getters /** @@ -83,30 +85,30 @@ class Orbit : public ILoggable { */ inline OrbitPropagateMode GetPropagateMode() const { return propagate_mode_; } /** - * @fn GetSatPosition_i + * @fn GetPosition_i_m * @brief Return spacecraft position in the inertial frame [m] */ - inline libra::Vector<3> GetSatPosition_i() const { return spacecraft_position_i_m_; } + inline libra::Vector<3> GetPosition_i_m() const { return spacecraft_position_i_m_; } /** - * @fn GetSatPosition_ecef + * @fn GetPosition_ecef_m * @brief Return spacecraft position in the ECEF frame [m] */ - inline libra::Vector<3> GetSatPosition_ecef() const { return spacecraft_position_ecef_m_; } + inline libra::Vector<3> GetPosition_ecef_m() const { return spacecraft_position_ecef_m_; } /** - * @fn GetSatVelocity_i + * @fn GetVelocity_i_m_s * @brief Return spacecraft velocity in the inertial frame [m/s] */ - inline libra::Vector<3> GetSatVelocity_i() const { return spacecraft_velocity_i_m_s_; } + inline libra::Vector<3> GetVelocity_i_m_s() const { return spacecraft_velocity_i_m_s_; } /** - * @fn GetSatVelocity_b + * @fn GetVelocity_b_m_s * @brief Return spacecraft velocity in the body fixed frame [m/s] */ - inline libra::Vector<3> GetSatVelocity_b() const { return spacecraft_velocity_b_m_s_; } + inline libra::Vector<3> GetVelocity_b_m_s() const { return spacecraft_velocity_b_m_s_; } /** - * @fn GetSatVelocity_ecef + * @fn GetVelocity_ecef_m_s * @brief Return spacecraft velocity in the ECEF frame [m/s] */ - inline libra::Vector<3> GetSatVelocity_ecef() const { return spacecraft_velocity_ecef_m_s_; } + inline libra::Vector<3> GetVelocity_ecef_m_s() const { return spacecraft_velocity_ecef_m_s_; } /** * @fn GetGeodeticPosition * @brief Return spacecraft position in the geodetic frame [m] @@ -132,17 +134,17 @@ class Orbit : public ILoggable { */ inline void SetIsCalcEnabled(bool is_calc_enabled) { is_calc_enabled_ = is_calc_enabled; } /** - * @fn SetAcceleration_i + * @fn SetAcceleration_i_m_s2 * @brief Set acceleration in the inertial frame [m/s2] */ - inline void SetAcceleration_i(libra::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ = acceleration_i_m_s2; } + inline void SetAcceleration_i_m_s2(libra::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ = acceleration_i_m_s2; } /** - * @fn AddForce_i + * @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(libra::Vector<3> force_i, double spacecraft_mass_kg) { + inline void AddForce_i_N(libra::Vector<3> force_i, double spacecraft_mass_kg) { force_i /= spacecraft_mass_kg; spacecraft_acceleration_i_m_s2_ += force_i; } @@ -152,7 +154,7 @@ class Orbit : public ILoggable { */ inline void AddAcceleration_i_m_s2(libra::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ += acceleration_i_m_s2; } /** - * @fn AddForce_i + * @fn AddForce_i_N * @brief Add force * @param [in] force_b_N: Force in the body fixed frame [N] * @param [in] quaternion_i2b: Quaternion from the inertial frame to the body fixed frame @@ -160,14 +162,14 @@ class Orbit : public ILoggable { */ inline void AddForce_b_N(libra::Vector<3> force_b_N, libra::Quaternion quaternion_i2b, double spacecraft_mass_kg) { auto force_i = quaternion_i2b.frame_conv_inv(force_b_N); - AddForce_i(force_i, spacecraft_mass_kg); + AddForce_i_N(force_i, spacecraft_mass_kg); } /** - * @fn CalcQuaternionI2LVLH + * @fn CalcQuaternion_i2lvlh * @brief Calculate quaternion from the inertial frame to the LVLH frame */ - libra::Quaternion CalcQuaternionI2LVLH() const; + libra::Quaternion CalcQuaternion_i2lvlh() const; // Override ILoggable /** diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index fc87c9a63..69fd9dbc3 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -37,9 +37,9 @@ void RelativeOrbit::InitializeState(Vector<3> initial_relative_position_lvlh, Ve // Disturbance acceleration are not considered in relative orbit propagation spacecraft_acceleration_i_m_s2_ *= 0; - Vector<3> reference_sat_position_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetSatPosition_i(); - Vector<3> reference_sat_velocity_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetSatVelocity_i(); - Quaternion q_i2lvlh = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().CalcQuaternionI2LVLH(); + Vector<3> reference_sat_position_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetPosition_i_m(); + Vector<3> reference_sat_velocity_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetVelocity_i_m_s(); + Quaternion q_i2lvlh = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().CalcQuaternion_i2lvlh(); Quaternion q_lvlh2i = q_i2lvlh.conjugate(); spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_) + reference_sat_position_i; spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_) + reference_sat_velocity_i; @@ -66,7 +66,7 @@ void RelativeOrbit::InitializeState(Vector<3> initial_relative_position_lvlh, Ve void RelativeOrbit::CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double mu) { switch (relative_dynamics_model_type) { case RelativeOrbitModel::Hill: { - double reference_sat_orbit_radius = libra::norm(reference_sat_orbit->GetSatPosition_i()); + double reference_sat_orbit_radius = libra::norm(reference_sat_orbit->GetPosition_i_m()); system_matrix_ = CalculateHillSystemMatrix(reference_sat_orbit_radius, mu); } default: { @@ -79,7 +79,7 @@ void RelativeOrbit::CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_m void RelativeOrbit::CalculateSTM(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu, double elapsed_sec) { switch (stm_model_type) { case STMModel::HCW: { - double reference_sat_orbit_radius = libra::norm(reference_sat_orbit->GetSatPosition_i()); + double reference_sat_orbit_radius = libra::norm(reference_sat_orbit->GetPosition_i_m()); stm_ = CalculateHCWSTM(reference_sat_orbit_radius, mu, elapsed_sec); } default: { @@ -103,9 +103,9 @@ void RelativeOrbit::Propagate(double end_time_s, double current_time_jd) { PropagateSTM(end_time_s); } - Vector<3> reference_sat_position_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetSatPosition_i(); - Vector<3> reference_sat_velocity_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetSatVelocity_i(); - Quaternion q_i2lvlh = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().CalcQuaternionI2LVLH(); + Vector<3> reference_sat_position_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetPosition_i_m(); + Vector<3> reference_sat_velocity_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetVelocity_i_m_s(); + Quaternion q_i2lvlh = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().CalcQuaternion_i2lvlh(); Quaternion q_lvlh2i = q_i2lvlh.conjugate(); spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_) + reference_sat_position_i; diff --git a/src/environment/local/local_environment.cpp b/src/environment/local/local_environment.cpp index a6c8259e5..73e4d4077 100644 --- a/src/environment/local/local_environment.cpp +++ b/src/environment/local/local_environment.cpp @@ -54,7 +54,7 @@ void LocalEnvironment::Update(const Dynamics* dynamics, const SimulationTime* si // Update local environments that depend on the attitude (and the position) if (simulation_time->GetAttitudePropagateFlag()) { - celestial_information_->UpdateAllObjectsInformation(orbit.GetSatPosition_i(), orbit.GetSatVelocity_i(), attitude.GetQuaternion_i2b(), + celestial_information_->UpdateAllObjectsInformation(orbit.GetPosition_i_m(), orbit.GetVelocity_i_m_s(), attitude.GetQuaternion_i2b(), attitude.GetOmega_b()); geomagnetic_field_->CalcMagneticField(simulation_time->GetCurrentDecimalYear(), simulation_time->GetCurrentSiderealTime(), orbit.GetGeodeticPosition(), attitude.GetQuaternion_i2b()); diff --git a/src/simulation/case/sample_case.cpp b/src/simulation/case/sample_case.cpp index df3b3b44f..1de3c1b15 100644 --- a/src/simulation/case/sample_case.cpp +++ b/src/simulation/case/sample_case.cpp @@ -72,8 +72,8 @@ string SampleCase::GetLogHeader() const { string SampleCase::GetLogValue() const { string str_tmp = ""; - // auto pos_i = sample_sat->dynamics_->GetOrbit().GetSatPosition_i(); - // auto vel_i = sample_sat->dynamics_->GetOrbit().GetSatVelocity_i(); + // auto pos_i = sample_sat->dynamics_->GetOrbit().GetPosition_i_m(); + // auto vel_i = sample_sat->dynamics_->GetOrbit().GetVelocity_i_m_s(); // auto quat_i2b = sample_sat->dynamics_->GetAttitude().GetQuaternion_i2b(); // auto omega_b = sample_sat->dynamics_->GetAttitude().GetOmega_b(); diff --git a/src/simulation/ground_station/ground_station.cpp b/src/simulation/ground_station/ground_station.cpp index d536116f6..dd0b09717 100644 --- a/src/simulation/ground_station/ground_station.cpp +++ b/src/simulation/ground_station/ground_station.cpp @@ -48,7 +48,7 @@ void GroundStation::Update(const CelestialRotation& celes_rotation, const Spacec Matrix<3, 3> dcm_ecef2eci = transpose(celes_rotation.GetDcmJ2000ToXcxf()); gs_position_i_ = dcm_ecef2eci * gs_position_ecef_; - is_visible_[spacecraft.GetSatID()] = CalcIsVisible(spacecraft.GetDynamics().GetOrbit().GetSatPosition_ecef()); + is_visible_[spacecraft.GetSatID()] = CalcIsVisible(spacecraft.GetDynamics().GetOrbit().GetPosition_ecef_m()); } bool GroundStation::CalcIsVisible(const Vector<3> sc_pos_ecef_m) { diff --git a/src/simulation/multiple_spacecraft/relative_information.cpp b/src/simulation/multiple_spacecraft/relative_information.cpp index 9cfa1c924..ee5c76eeb 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_sat_id = 0; target_sat_id < dynamics_database_.size(); target_sat_id++) { for (size_t reference_sat_id = 0; reference_sat_id < dynamics_database_.size(); reference_sat_id++) { // Position - libra::Vector<3> target_sat_pos_i = dynamics_database_.at(target_sat_id)->GetOrbit().GetSatPosition_i(); - libra::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_sat_id)->GetOrbit().GetSatPosition_i(); + libra::Vector<3> target_sat_pos_i = dynamics_database_.at(target_sat_id)->GetOrbit().GetPosition_i_m(); + libra::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_sat_id)->GetOrbit().GetPosition_i_m(); rel_pos_list_i_m_[target_sat_id][reference_sat_id] = target_sat_pos_i - reference_sat_pos_i; rel_pos_list_rtn_m_[target_sat_id][reference_sat_id] = CalcRelativePosition_rtn_m(target_sat_id, reference_sat_id); @@ -22,8 +22,8 @@ void RelativeInformation::Update() { rel_distance_list_m_[target_sat_id][reference_sat_id] = norm(rel_pos_list_i_m_[target_sat_id][reference_sat_id]); // Velocity - libra::Vector<3> target_sat_vel_i = dynamics_database_.at(target_sat_id)->GetOrbit().GetSatVelocity_i(); - libra::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_sat_id)->GetOrbit().GetSatVelocity_i(); + libra::Vector<3> target_sat_vel_i = dynamics_database_.at(target_sat_id)->GetOrbit().GetVelocity_i_m_s(); + libra::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_sat_id)->GetOrbit().GetVelocity_i_m_s(); rel_vel_list_i_m_s_[target_sat_id][reference_sat_id] = target_sat_vel_i - reference_sat_vel_i; rel_vel_list_rtn_m_s_[target_sat_id][reference_sat_id] = CalcRelativeVelocity_rtn_m_s(target_sat_id, reference_sat_id); @@ -115,28 +115,28 @@ libra::Quaternion RelativeInformation::CalcRelativeAttitudeQuaternion(const int } libra::Vector<3> RelativeInformation::CalcRelativePosition_rtn_m(const int target_sat_id, const int reference_sat_id) { - libra::Vector<3> target_sat_pos_i = dynamics_database_.at(target_sat_id)->GetOrbit().GetSatPosition_i(); - libra::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_sat_id)->GetOrbit().GetSatPosition_i(); + libra::Vector<3> target_sat_pos_i = dynamics_database_.at(target_sat_id)->GetOrbit().GetPosition_i_m(); + libra::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_sat_id)->GetOrbit().GetPosition_i_m(); libra::Vector<3> relative_pos_i = target_sat_pos_i - reference_sat_pos_i; // RTN frame for the reference satellite - libra::Quaternion q_i2rtn = dynamics_database_.at(reference_sat_id)->GetOrbit().CalcQuaternionI2LVLH(); + libra::Quaternion q_i2rtn = dynamics_database_.at(reference_sat_id)->GetOrbit().CalcQuaternion_i2lvlh(); libra::Vector<3> relative_pos_rtn = q_i2rtn.frame_conv(relative_pos_i); return relative_pos_rtn; } libra::Vector<3> RelativeInformation::CalcRelativeVelocity_rtn_m_s(const int target_sat_id, const int reference_sat_id) { - libra::Vector<3> target_sat_pos_i = dynamics_database_.at(target_sat_id)->GetOrbit().GetSatPosition_i(); - libra::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_sat_id)->GetOrbit().GetSatPosition_i(); + libra::Vector<3> target_sat_pos_i = dynamics_database_.at(target_sat_id)->GetOrbit().GetPosition_i_m(); + libra::Vector<3> reference_sat_pos_i = dynamics_database_.at(reference_sat_id)->GetOrbit().GetPosition_i_m(); libra::Vector<3> relative_pos_i = target_sat_pos_i - reference_sat_pos_i; // RTN frame for the reference satellite - libra::Quaternion q_i2rtn = dynamics_database_.at(reference_sat_id)->GetOrbit().CalcQuaternionI2LVLH(); + libra::Quaternion q_i2rtn = dynamics_database_.at(reference_sat_id)->GetOrbit().CalcQuaternion_i2lvlh(); // Rotation vector of RTN frame - libra::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_sat_id)->GetOrbit().GetSatVelocity_i(); - libra::Vector<3> target_sat_vel_i = dynamics_database_.at(target_sat_id)->GetOrbit().GetSatVelocity_i(); + libra::Vector<3> reference_sat_vel_i = dynamics_database_.at(reference_sat_id)->GetOrbit().GetVelocity_i_m_s(); + libra::Vector<3> target_sat_vel_i = dynamics_database_.at(target_sat_id)->GetOrbit().GetVelocity_i_m_s(); libra::Vector<3> rot_vec_rtn_i = cross(reference_sat_pos_i, reference_sat_vel_i); double r2_ref = norm(reference_sat_pos_i) * norm(reference_sat_pos_i); rot_vec_rtn_i /= r2_ref; From 12e51cacd1d63dc127659f38c99764a259a9b2df Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:51:23 +0100 Subject: [PATCH 31/57] Add const --- src/dynamics/orbit/orbit.hpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/dynamics/orbit/orbit.hpp b/src/dynamics/orbit/orbit.hpp index a7d529d18..1178be861 100644 --- a/src/dynamics/orbit/orbit.hpp +++ b/src/dynamics/orbit/orbit.hpp @@ -62,14 +62,14 @@ class Orbit : public ILoggable { * @param [in] end_time_s: End time of simulation [sec] * @param [in] current_time_jd: Current Julian day [day] */ - virtual void Propagate(double end_time_s, double current_time_jd) = 0; + virtual void Propagate(const double end_time_s, const double current_time_jd) = 0; /** * @fn UpdateByAttitude * @brief Update attitude information * @param [in] quaternion_i2b: End time of simulation [sec] */ - inline void UpdateByAttitude(libra::Quaternion quaternion_i2b) { + inline void UpdateByAttitude(const libra::Quaternion quaternion_i2b) { spacecraft_velocity_b_m_s_ = quaternion_i2b.frame_conv(spacecraft_velocity_i_m_s_); } @@ -132,27 +132,28 @@ class Orbit : public ILoggable { * @fn SetIsCalcEnabled * @brief Set calculate flag */ - inline void SetIsCalcEnabled(bool is_calc_enabled) { is_calc_enabled_ = is_calc_enabled; } + inline void SetIsCalcEnabled(const bool is_calc_enabled) { is_calc_enabled_ = is_calc_enabled; } /** * @fn SetAcceleration_i_m_s2 * @brief Set acceleration in the inertial frame [m/s2] */ - inline void SetAcceleration_i_m_s2(libra::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ = acceleration_i_m_s2; } + inline void SetAcceleration_i_m_s2(const libra::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(libra::Vector<3> force_i, double spacecraft_mass_kg) { - force_i /= spacecraft_mass_kg; - spacecraft_acceleration_i_m_s2_ += force_i; + inline void AddForce_i_N(const libra::Vector<3> force_i_N, double spacecraft_mass_kg) { + libra::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; } /** * @fn AddAcceleration_i_m_s2 * @brief Add acceleration in the inertial frame [m/s2] */ - inline void AddAcceleration_i_m_s2(libra::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ += acceleration_i_m_s2; } + inline void AddAcceleration_i_m_s2(const libra::Vector<3> acceleration_i_m_s2) { spacecraft_acceleration_i_m_s2_ += acceleration_i_m_s2; } /** * @fn AddForce_i_N * @brief Add force @@ -160,14 +161,14 @@ 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(libra::Vector<3> force_b_N, libra::Quaternion quaternion_i2b, double spacecraft_mass_kg) { + inline void AddForce_b_N(const libra::Vector<3> force_b_N, const libra::Quaternion quaternion_i2b, const double spacecraft_mass_kg) { auto force_i = quaternion_i2b.frame_conv_inv(force_b_N); AddForce_i_N(force_i, spacecraft_mass_kg); } /** * @fn CalcQuaternion_i2lvlh - * @brief Calculate quaternion from the inertial frame to the LVLH frame + * @brief Calculate and return quaternion from the inertial frame to the LVLH frame */ libra::Quaternion CalcQuaternion_i2lvlh() const; From ed94b1d3d701e2d3b37af649f9fc555260baf4fe Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:55:14 +0100 Subject: [PATCH 32/57] Add libra:: --- .../orbit/encke_orbit_propagation.cpp | 16 +++++++-------- .../orbit/encke_orbit_propagation.hpp | 20 +++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index a9194032b..221946bc7 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -10,8 +10,8 @@ #include "../../library/orbit/orbital_elements.hpp" EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, const double prop_step_s, - const double current_time_jd, const Vector<3> init_position_i_m, const Vector<3> init_velocity_i_m_s, - const double error_tolerance) + const double current_time_jd, const libra::Vector<3> init_position_i_m, + const libra::Vector<3> init_velocity_i_m_s, const double error_tolerance) : Orbit(celestial_information), libra::ODE<6>(prop_step_s), mu_m3_s2_(mu_m3_s2), error_tolerance_(error_tolerance), prop_step_s_(prop_step_s) { prop_time_s_ = 0.0; Initialize(current_time_jd, init_position_i_m, init_velocity_i_m_s); @@ -56,9 +56,9 @@ void EnckeOrbitPropagation::Propagate(double end_time_s, double current_time_jd) } // Functions for ODE -void EnckeOrbitPropagation::RHS(double t, const Vector<6>& state, Vector<6>& rhs) { +void EnckeOrbitPropagation::RHS(double t, const libra::Vector<6>& state, libra::Vector<6>& rhs) { UNUSED(t); - Vector<3> diff_pos_i_m, diff_acc_i_m_s2; + libra::Vector<3> diff_pos_i_m, diff_acc_i_m_s2; for (int i = 0; i < 3; i++) { diff_pos_i_m[i] = state[i]; } @@ -78,7 +78,7 @@ void EnckeOrbitPropagation::RHS(double t, const Vector<6>& state, Vector<6>& rhs } // Private Functions -void EnckeOrbitPropagation::Initialize(double current_time_jd, Vector<3> init_ref_position_i_m, Vector<3> init_ref_velocity_i_m_s) { +void EnckeOrbitPropagation::Initialize(double current_time_jd, libra::Vector<3> init_ref_position_i_m, libra::Vector<3> init_ref_velocity_i_m_s) { // General fill_up(spacecraft_acceleration_i_m_s2_, 0.0); @@ -92,7 +92,7 @@ void EnckeOrbitPropagation::Initialize(double current_time_jd, Vector<3> init_re fill_up(diff_position_i_m_, 0.0); fill_up(diff_velocity_i_m_s_, 0.0); - Vector<6> zero(0.0f); + libra::Vector<6> zero(0.0f); setup(0.0, zero); UpdateSatOrbit(); @@ -106,11 +106,11 @@ void EnckeOrbitPropagation::UpdateSatOrbit() { TransformEcefToGeodetic(); } -double EnckeOrbitPropagation::CalcQFunction(Vector<3> diff_pos_i) { +double EnckeOrbitPropagation::CalcQFunction(libra::Vector<3> diff_pos_i) { double r2; r2 = inner_product(spacecraft_position_i_m_, spacecraft_position_i_m_); - Vector<3> dr_2r; + libra::Vector<3> dr_2r; dr_2r = diff_pos_i - 2.0 * spacecraft_position_i_m_; double q = inner_product(diff_pos_i, dr_2r) / r2; diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index 6461e021e..2d1dafbaf 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -28,7 +28,7 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] error_tolerance: Error tolerance threshold */ EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, const double prop_step_s, - const double current_time_jd, const Vector<3> init_position_i_m, const Vector<3> init_velocity_i_m_s, + const double current_time_jd, const libra::Vector<3> init_position_i_m, const libra::Vector<3> init_velocity_i_m_s, const double error_tolerance); /** * @fn ~EnckeOrbitPropagation @@ -43,7 +43,7 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] end_time_s: End time of simulation [sec] * @param [in] current_time_jd: Current Julian day [day] */ - virtual void Propagate(double end_time_s, double current_time_jd); + virtual void Propagate(const double end_time_s, const double current_time_jd); // Override ODE /** @@ -53,7 +53,7 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] state: Position and velocity as state vector * @param [out] rhs: Output of the function */ - virtual void RHS(double t, const Vector<6>& state, Vector<6>& rhs); + virtual void RHS(double t, const libra::Vector<6>& state, libra::Vector<6>& rhs); private: // General @@ -63,13 +63,13 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { double prop_time_s_; //!< Simulation current time for numerical integration by RK4 // reference orbit - Vector<3> ref_position_i_m_; //!< Reference orbit position in the inertial frame [m] - Vector<3> ref_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] - KeplerOrbit ref_kepler_orbit; //!< Reference Kepler orbital element + libra::Vector<3> ref_position_i_m_; //!< Reference orbit position in the inertial frame [m] + libra::Vector<3> ref_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] + KeplerOrbit ref_kepler_orbit; //!< Reference Kepler orbital element // difference orbit - Vector<3> diff_position_i_m_; //!< Difference orbit position in the inertial frame [m] - Vector<3> diff_velocity_i_m_s_; //!< Difference orbit velocity in the inertial frame [m/s] + libra::Vector<3> diff_position_i_m_; //!< Difference orbit position in the inertial frame [m] + libra::Vector<3> diff_velocity_i_m_s_; //!< Difference orbit velocity in the inertial frame [m/s] // functions /** @@ -79,7 +79,7 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] init_ref_position_i_m: Initial value of reference orbit position in the inertial frame [m] * @param [in] init_ref_velocity_i_m_s: Initial value of reference orbit position in the inertial frame [m] */ - void Initialize(double current_time_jd, Vector<3> init_ref_position_i_m, Vector<3> init_ref_velocity_i_m_s); + void Initialize(const double current_time_jd, const libra::Vector<3> init_ref_position_i_m, const libra::Vector<3> init_ref_velocity_i_m_s); /** * @fn UpdateSatOrbit * @brief Update satellite orbit @@ -90,7 +90,7 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { * @brief Calculate Q function * @param [in] diff_pos_i: Difference of position in the inertial frame [m] */ - double CalcQFunction(Vector<3> diff_pos_i); + double CalcQFunction(const libra::Vector<3> diff_pos_i); }; #endif // S2E_DYNAMICS_ORBIT_ENCKE_ORBIT_PROPAGATION_HPP_ From 8719723d9cdcc78179ad8ce9863aab7fd0996b49 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 19:59:41 +0100 Subject: [PATCH 33/57] Fix private variables --- .../orbit/encke_orbit_propagation.cpp | 80 ++++++++++--------- .../orbit/encke_orbit_propagation.hpp | 24 +++--- 2 files changed, 54 insertions(+), 50 deletions(-) diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index 221946bc7..522d28001 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -12,8 +12,12 @@ EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, const double prop_step_s, const double current_time_jd, const libra::Vector<3> init_position_i_m, const libra::Vector<3> init_velocity_i_m_s, const double error_tolerance) - : Orbit(celestial_information), libra::ODE<6>(prop_step_s), mu_m3_s2_(mu_m3_s2), error_tolerance_(error_tolerance), prop_step_s_(prop_step_s) { - prop_time_s_ = 0.0; + : Orbit(celestial_information), + libra::ODE<6>(prop_step_s), + mu_m3_s2_(mu_m3_s2), + error_tolerance_(error_tolerance), + propagation_step_s_(prop_step_s) { + propagation_time_s_ = 0.0; Initialize(current_time_jd, init_position_i_m, init_velocity_i_m_s); } @@ -25,32 +29,32 @@ void EnckeOrbitPropagation::Propagate(double end_time_s, double current_time_jd) // Rectification double norm_sat_position_m = norm(spacecraft_position_i_m_); - double norm_diff_position_m = norm(diff_position_i_m_); - if (norm_diff_position_m / norm_sat_position_m > error_tolerance_) { + double norm_difference_position_m = norm(difference_position_i_m_); + if (norm_difference_position_m / norm_sat_position_m > error_tolerance_) { Initialize(current_time_jd, spacecraft_position_i_m_, spacecraft_velocity_i_m_s_); } // Update reference orbit - ref_kepler_orbit.CalcPosVel(current_time_jd); - ref_position_i_m_ = ref_kepler_orbit.GetPosition_i_m(); - ref_velocity_i_m_s_ = ref_kepler_orbit.GetVelocity_i_m_s(); + reference_kepler_orbit.CalcPosVel(current_time_jd); + reference_position_i_m_ = reference_kepler_orbit.GetPosition_i_m(); + reference_velocity_i_m_s_ = reference_kepler_orbit.GetVelocity_i_m_s(); // Propagate difference orbit - setStepWidth(prop_step_s_); // Re-set propagation Δt - while (end_time_s - prop_time_s_ - prop_step_s_ > 1.0e-6) { + setStepWidth(propagation_step_s_); // Re-set propagation Δt + while (end_time_s - propagation_time_s_ - propagation_step_s_ > 1.0e-6) { Update(); // Propagation methods of the ODE class - prop_time_s_ += prop_step_s_; + propagation_time_s_ += propagation_step_s_; } - setStepWidth(end_time_s - prop_time_s_); // Adjust the last propagation Δt + setStepWidth(end_time_s - propagation_time_s_); // Adjust the last propagation Δt Update(); - prop_time_s_ = end_time_s; + propagation_time_s_ = end_time_s; - diff_position_i_m_[0] = state()[0]; - diff_position_i_m_[1] = state()[1]; - diff_position_i_m_[2] = state()[2]; - diff_velocity_i_m_s_[0] = state()[3]; - diff_velocity_i_m_s_[1] = state()[4]; - diff_velocity_i_m_s_[2] = state()[5]; + difference_position_i_m_[0] = state()[0]; + difference_position_i_m_[1] = state()[1]; + difference_position_i_m_[2] = state()[2]; + difference_velocity_i_m_s_[0] = state()[3]; + difference_velocity_i_m_s_[1] = state()[4]; + difference_velocity_i_m_s_[2] = state()[5]; UpdateSatOrbit(); } @@ -58,39 +62,39 @@ void EnckeOrbitPropagation::Propagate(double end_time_s, double current_time_jd) // Functions for ODE void EnckeOrbitPropagation::RHS(double t, const libra::Vector<6>& state, libra::Vector<6>& rhs) { UNUSED(t); - libra::Vector<3> diff_pos_i_m, diff_acc_i_m_s2; + libra::Vector<3> difference_position_i_m_m, difference_acc_i_m_s2; for (int i = 0; i < 3; i++) { - diff_pos_i_m[i] = state[i]; + difference_position_i_m_m[i] = state[i]; } - double q_func = CalcQFunction(diff_pos_i_m); - double r_m = norm(ref_position_i_m_); + double q_func = CalcQFunction(difference_position_i_m_m); + double r_m = norm(reference_position_i_m_); double r_m3 = pow(r_m, 3.0); - diff_acc_i_m_s2 = -(mu_m3_s2_ / r_m3) * (q_func * spacecraft_position_i_m_ + diff_pos_i_m) + spacecraft_acceleration_i_m_s2_; + difference_acc_i_m_s2 = -(mu_m3_s2_ / r_m3) * (q_func * spacecraft_position_i_m_ + difference_position_i_m_m) + spacecraft_acceleration_i_m_s2_; rhs[0] = state[3]; rhs[1] = state[4]; rhs[2] = state[5]; - rhs[3] = diff_acc_i_m_s2[0]; - rhs[4] = diff_acc_i_m_s2[1]; - rhs[5] = diff_acc_i_m_s2[2]; + rhs[3] = difference_acc_i_m_s2[0]; + rhs[4] = difference_acc_i_m_s2[1]; + rhs[5] = difference_acc_i_m_s2[2]; } // Private Functions -void EnckeOrbitPropagation::Initialize(double current_time_jd, libra::Vector<3> init_ref_position_i_m, libra::Vector<3> init_ref_velocity_i_m_s) { +void EnckeOrbitPropagation::Initialize(double current_time_jd, libra::Vector<3> reference_position_i_m, libra::Vector<3> reference_velocity_i_m_s) { // General fill_up(spacecraft_acceleration_i_m_s2_, 0.0); // reference orbit - ref_position_i_m_ = init_ref_position_i_m; - ref_velocity_i_m_s_ = init_ref_velocity_i_m_s; - OrbitalElements oe_ref(mu_m3_s2_, current_time_jd, init_ref_position_i_m, init_ref_velocity_i_m_s); - ref_kepler_orbit = KeplerOrbit(mu_m3_s2_, oe_ref); + reference_position_i_m_ = reference_position_i_m; + reference_velocity_i_m_s_ = reference_velocity_i_m_s; + OrbitalElements oe_ref(mu_m3_s2_, current_time_jd, reference_position_i_m, reference_velocity_i_m_s); + reference_kepler_orbit = KeplerOrbit(mu_m3_s2_, oe_ref); // difference orbit - fill_up(diff_position_i_m_, 0.0); - fill_up(diff_velocity_i_m_s_, 0.0); + fill_up(difference_position_i_m_, 0.0); + fill_up(difference_velocity_i_m_s_, 0.0); libra::Vector<6> zero(0.0f); setup(0.0, zero); @@ -99,21 +103,21 @@ void EnckeOrbitPropagation::Initialize(double current_time_jd, libra::Vector<3> } void EnckeOrbitPropagation::UpdateSatOrbit() { - spacecraft_position_i_m_ = ref_position_i_m_ + diff_position_i_m_; - spacecraft_velocity_i_m_s_ = ref_velocity_i_m_s_ + diff_velocity_i_m_s_; + spacecraft_position_i_m_ = reference_position_i_m_ + difference_position_i_m_; + spacecraft_velocity_i_m_s_ = reference_velocity_i_m_s_ + difference_velocity_i_m_s_; TransformEciToEcef(); TransformEcefToGeodetic(); } -double EnckeOrbitPropagation::CalcQFunction(libra::Vector<3> diff_pos_i) { +double EnckeOrbitPropagation::CalcQFunction(libra::Vector<3> difference_position_i_m) { double r2; r2 = inner_product(spacecraft_position_i_m_, spacecraft_position_i_m_); libra::Vector<3> dr_2r; - dr_2r = diff_pos_i - 2.0 * spacecraft_position_i_m_; + dr_2r = difference_position_i_m - 2.0 * spacecraft_position_i_m_; - double q = inner_product(diff_pos_i, dr_2r) / r2; + double q = inner_product(difference_position_i_m, dr_2r) / r2; double q_func = q * (q * q + 3.0 * q + 3.0) / (pow(1.0 + q, 1.5) + 1.0); diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index 2d1dafbaf..e31887464 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -59,27 +59,27 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { // General const double mu_m3_s2_; //!< Gravity constant of the center body [m3/s2] const double error_tolerance_; //!< Error tolerance ratio - double prop_step_s_; //!< Propagation step width for RK4 - double prop_time_s_; //!< Simulation current time for numerical integration by RK4 + double propagation_step_s_; //!< Propagation step width for RK4 + double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 // reference orbit - libra::Vector<3> ref_position_i_m_; //!< Reference orbit position in the inertial frame [m] - libra::Vector<3> ref_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] - KeplerOrbit ref_kepler_orbit; //!< Reference Kepler orbital element + libra::Vector<3> reference_position_i_m_; //!< Reference orbit position in the inertial frame [m] + libra::Vector<3> reference_velocity_i_m_s_; //!< Reference orbit velocity in the inertial frame [m/s] + KeplerOrbit reference_kepler_orbit; //!< Reference Kepler orbital element // difference orbit - libra::Vector<3> diff_position_i_m_; //!< Difference orbit position in the inertial frame [m] - libra::Vector<3> diff_velocity_i_m_s_; //!< Difference orbit velocity in the inertial frame [m/s] + libra::Vector<3> difference_position_i_m_; //!< Difference orbit position in the inertial frame [m] + libra::Vector<3> difference_velocity_i_m_s_; //!< Difference orbit velocity in the inertial frame [m/s] // functions /** * @fn Initialize * @brief Initialize function * @param [in] current_time_jd: Current Julian day [day] - * @param [in] init_ref_position_i_m: Initial value of reference orbit position in the inertial frame [m] - * @param [in] init_ref_velocity_i_m_s: Initial value of reference orbit position in the inertial frame [m] + * @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 libra::Vector<3> init_ref_position_i_m, const libra::Vector<3> init_ref_velocity_i_m_s); + void Initialize(const double current_time_jd, const libra::Vector<3> reference_position_i_m, const libra::Vector<3> reference_velocity_i_m_s); /** * @fn UpdateSatOrbit * @brief Update satellite orbit @@ -88,9 +88,9 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { /** * @fn CalcQFunction * @brief Calculate Q function - * @param [in] diff_pos_i: Difference of position in the inertial frame [m] + * @param [in] difference_position_i_m: Difference of position in the inertial frame [m] */ - double CalcQFunction(const libra::Vector<3> diff_pos_i); + double CalcQFunction(const libra::Vector<3> difference_position_i_m); }; #endif // S2E_DYNAMICS_ORBIT_ENCKE_ORBIT_PROPAGATION_HPP_ From d47e194f1efea5ff64ea3c7a00c3e5178ce1656d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:01:56 +0100 Subject: [PATCH 34/57] Fix function argument --- src/dynamics/orbit/encke_orbit_propagation.cpp | 12 ++++++------ src/dynamics/orbit/encke_orbit_propagation.hpp | 10 +++++----- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index 522d28001..9a9590440 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -9,16 +9,16 @@ #include "../../library/orbit/orbital_elements.hpp" -EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, const double prop_step_s, - const double current_time_jd, const libra::Vector<3> init_position_i_m, - const libra::Vector<3> init_velocity_i_m_s, const double error_tolerance) +EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, + const double propagation_step_s, const double current_time_jd, const libra::Vector<3> position_i_m, + const libra::Vector<3> velocity_i_m_s, const double error_tolerance) : Orbit(celestial_information), - libra::ODE<6>(prop_step_s), + libra::ODE<6>(propagation_step_s), mu_m3_s2_(mu_m3_s2), error_tolerance_(error_tolerance), - propagation_step_s_(prop_step_s) { + propagation_step_s_(propagation_step_s) { propagation_time_s_ = 0.0; - Initialize(current_time_jd, init_position_i_m, init_velocity_i_m_s); + Initialize(current_time_jd, position_i_m, velocity_i_m_s); } EnckeOrbitPropagation::~EnckeOrbitPropagation() {} diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index e31887464..f83421d17 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -21,14 +21,14 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { * @brief Constructor * @param [in] celestial_information: Celestial information * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] - * @param [in] prop_step_s: Propagation step width [sec] + * @param [in] propagation_step_s: Propagation step width [sec] * @param [in] current_time_jd: Current Julian day [day] - * @param [in] init_position_i_m: Initial value of position in the inertial frame [m] - * @param [in] init_velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] + * @param [in] position_i_m: Initial value of position in the inertial frame [m] + * @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 mu_m3_s2, const double prop_step_s, - const double current_time_jd, const libra::Vector<3> init_position_i_m, const libra::Vector<3> init_velocity_i_m_s, + EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, const double propagation_step_s, + const double current_time_jd, const libra::Vector<3> position_i_m, const libra::Vector<3> velocity_i_m_s, const double error_tolerance); /** * @fn ~EnckeOrbitPropagation From 3d16742fe666e954cfc2155cc415aafb212c62d8 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:04:53 +0100 Subject: [PATCH 35/57] Fix function argument names --- src/dynamics/orbit/initialize_orbit.cpp | 29 ++++++++++++------------- src/dynamics/orbit/initialize_orbit.hpp | 16 +++++++------- 2 files changed, 22 insertions(+), 23 deletions(-) diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index 908effed2..c665449a0 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -12,9 +12,9 @@ #include "rk4_orbit_propagation.hpp" #include "sgp4_orbit_propagation.hpp" -Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string ini_path, double stepSec, double current_time_jd, - double gravity_constant, std::string section, RelativeInformation* rel_info) { - auto conf = IniAccess(ini_path); +Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, double current_time_jd, + double mu_m3_s2, std::string section, RelativeInformation* relative_information) { + auto conf = IniAccess(initialize_file); const char* section_ = section.c_str(); Orbit* orbit; @@ -28,12 +28,12 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // initialize RK4 orbit propagator Vector<3> position_i_m; Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(ini_path, current_time_jd, gravity_constant); + Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_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]; } - orbit = new Rk4OrbitPropagation(celestial_information, gravity_constant, stepSec, position_i_m, velocity_i_m_s); + orbit = new Rk4OrbitPropagation(celestial_information, mu_m3_s2, step_width_s, position_i_m, velocity_i_m_s); } else if (propagate_mode == "SGP4") { // Initialize SGP4 orbit propagator int wgs = conf.ReadInt(section_, "wgs"); @@ -58,11 +58,11 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // the orbit of the reference sat is initialized, create temporary initial orbit of the reference sat int reference_sat_id = conf.ReadInt(section_, "reference_satellite_id"); - orbit = new RelativeOrbit(celestial_information, gravity_constant, stepSec, reference_sat_id, init_relative_position_lvlh, - init_relative_velocity_lvlh, update_method, relative_dynamics_model_type, stm_model_type, rel_info); + orbit = new RelativeOrbit(celestial_information, mu_m3_s2, step_width_s, reference_sat_id, init_relative_position_lvlh, + 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 - double mu_m3_s2 = gravity_constant; + double mu_m3_s2 = mu_m3_s2; OrbitalElements oe; // TODO: init_mode_kepler should be removed in the next major update if (initialize_mode == OrbitInitializeMode::kInertialPositionAndVelocity) { @@ -88,27 +88,26 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // initialize orbit for Encke's method Vector<3> position_i_m; Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(ini_path, current_time_jd, gravity_constant); + Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_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]; } double error_tolerance = conf.ReadDouble(section_, "error_tolerance"); - orbit = - new EnckeOrbitPropagation(celestial_information, gravity_constant, stepSec, current_time_jd, position_i_m, velocity_i_m_s, error_tolerance); + orbit = new EnckeOrbitPropagation(celestial_information, mu_m3_s2, step_width_s, current_time_jd, position_i_m, velocity_i_m_s, error_tolerance); } else { 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; Vector<3> position_i_m; Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(ini_path, current_time_jd, gravity_constant); + Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_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]; } - orbit = new Rk4OrbitPropagation(celestial_information, gravity_constant, stepSec, position_i_m, velocity_i_m_s); + orbit = new Rk4OrbitPropagation(celestial_information, mu_m3_s2, step_width_s, position_i_m, velocity_i_m_s); } orbit->SetIsCalcEnabled(conf.ReadEnable(section_, "calculation")); @@ -116,8 +115,8 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string return orbit; } -Vector<6> InitializePosVel(std::string ini_path, double current_time_jd, double mu_m3_s2, std::string section) { - auto conf = IniAccess(ini_path); +Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double mu_m3_s2, std::string section) { + auto conf = IniAccess(initialize_file); const char* section_ = section.c_str(); Vector<3> position_i_m; Vector<3> velocity_i_m_s; diff --git a/src/dynamics/orbit/initialize_orbit.hpp b/src/dynamics/orbit/initialize_orbit.hpp index 881093eb0..e41755506 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -16,24 +16,24 @@ class RelativeInformation; * @fn InitOrbit * @brief Initialize function for Orbit class * @param [in] celestial_information: Celestial information - * @param [in] ini_path: Path to initialize file - * @param [in] stepSec: Step width [sec] + * @param [in] initialize_file: Path to initialize file + * @param [in] step_width_s: Step width [sec] * @param [in] current_time_jd: Current Julian day [day] - * @param [in] gravity_constant: Gravity constant [m3/s2] + * @param [in] mu_m3_s2: Gravity constant [m3/s2] * @param [in] section: Section name - * @param [in] rel_info: Relative information + * @param [in] relative_information: Relative information */ -Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string ini_path, double stepSec, double current_time_jd, double gravity_constant, - std::string section = "ORBIT", RelativeInformation* rel_info = (RelativeInformation*)nullptr); +Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, double current_time_jd, + double mu_m3_s2, std::string section = "ORBIT", RelativeInformation* relative_information = (RelativeInformation*)nullptr); /** * @fn InitializePosVel * @brief Initialize position and velocity depends on initialize mode - * @param [in] ini_path: Path to initialize file + * @param [in] initialize_file: Path to initialize file * @param [in] current_time_jd: Current Julian day [day] * @param [in] mu_m3_s2: Gravity constant [m3/s2] * @param [in] section: Section name */ -Vector<6> InitializePosVel(std::string ini_path, double current_time_jd, double mu_m3_s2, std::string section = "ORBIT"); +Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double mu_m3_s2, std::string section = "ORBIT"); #endif // S2E_DYNAMICS_ORBIT_INITIALIZE_ORBIT_HPP_ From eb4725d67c710614f61b0fb95639f5c5ff3208bd Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:11:29 +0100 Subject: [PATCH 36/57] Fix private variables --- src/dynamics/orbit/initialize_orbit.cpp | 4 +- src/dynamics/orbit/relative_orbit.cpp | 92 ++++++++++---------- src/dynamics/orbit/relative_orbit.hpp | 32 +++---- src/dynamics/orbit/rk4_orbit_propagation.cpp | 24 ++--- src/dynamics/orbit/rk4_orbit_propagation.hpp | 12 +-- 5 files changed, 83 insertions(+), 81 deletions(-) diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index c665449a0..c191972ca 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -56,9 +56,9 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // There is a possibility that the orbit of the reference sat is not initialized when RelativeOrbit initialization is called To ensure that // the orbit of the reference sat is initialized, create temporary initial orbit of the reference sat - int reference_sat_id = conf.ReadInt(section_, "reference_satellite_id"); + int reference_spacecraft_id = conf.ReadInt(section_, "reference_satellite_id"); - orbit = new RelativeOrbit(celestial_information, mu_m3_s2, step_width_s, reference_sat_id, init_relative_position_lvlh, + orbit = new RelativeOrbit(celestial_information, mu_m3_s2, step_width_s, reference_spacecraft_id, init_relative_position_lvlh, 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 diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 69fd9dbc3..eb8ef6807 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -8,41 +8,42 @@ #include "rk4_orbit_propagation.hpp" -RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double mu, double timestep, int reference_sat_id, +RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double mu_m3_s2, double timestep, int reference_spacecraft_id, Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, RelativeOrbitUpdateMethod update_method, RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, RelativeInformation* rel_info) : Orbit(celestial_information), libra::ODE<6>(timestep), - mu_(mu), - reference_sat_id_(reference_sat_id), + mu_m3_s2_(mu_m3_s2), + reference_spacecraft_id_(reference_spacecraft_id), update_method_(update_method), relative_dynamics_model_type_(relative_dynamics_model_type), stm_model_type_(stm_model_type), - rel_info_(rel_info) { + relative_information_(rel_info) { propagate_mode_ = OrbitPropagateMode::kRelativeOrbit; - prop_time_ = 0.0; - prop_step_ = timestep; + propagation_time_s_ = 0.0; + propagation_step_s_ = timestep; - InitializeState(initial_relative_position_lvlh, initial_relative_velocity_lvlh, mu); + InitializeState(initial_relative_position_lvlh, initial_relative_velocity_lvlh, mu_m3_s2); } RelativeOrbit::~RelativeOrbit() {} -void RelativeOrbit::InitializeState(Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, double mu, double init_time) { - relative_position_lvlh_ = initial_relative_position_lvlh; - relative_velocity_lvlh_ = initial_relative_velocity_lvlh; +void RelativeOrbit::InitializeState(Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, double mu_m3_s2, + double init_time) { + relative_position_lvlh_m_ = initial_relative_position_lvlh; + relative_velocity_lvlh_m_s_ = initial_relative_velocity_lvlh; // Disturbance acceleration are not considered in relative orbit propagation spacecraft_acceleration_i_m_s2_ *= 0; - Vector<3> reference_sat_position_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetPosition_i_m(); - Vector<3> reference_sat_velocity_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetVelocity_i_m_s(); - Quaternion q_i2lvlh = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().CalcQuaternion_i2lvlh(); + Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); + Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); + Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); Quaternion q_lvlh2i = q_i2lvlh.conjugate(); - spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_) + reference_sat_position_i; - spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_) + reference_sat_velocity_i; + spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_m_) + reference_sat_position_i; + spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_m_s_) + reference_sat_velocity_i; initial_state_[0] = initial_relative_position_lvlh[0]; initial_state_[1] = initial_relative_position_lvlh[1]; @@ -53,21 +54,22 @@ void RelativeOrbit::InitializeState(Vector<3> initial_relative_position_lvlh, Ve if (update_method_ == RK4) { setup(init_time, initial_state_); - CalculateSystemMatrix(relative_dynamics_model_type_, &(rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit()), mu); + CalculateSystemMatrix(relative_dynamics_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), + mu_m3_s2); } else // update_method_ == STM { - CalculateSTM(stm_model_type_, &(rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit()), mu, 0.0); + CalculateSTM(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2, 0.0); } TransformEciToEcef(); TransformEcefToGeodetic(); } -void RelativeOrbit::CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double mu) { +void RelativeOrbit::CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2) { switch (relative_dynamics_model_type) { case RelativeOrbitModel::Hill: { double reference_sat_orbit_radius = libra::norm(reference_sat_orbit->GetPosition_i_m()); - system_matrix_ = CalculateHillSystemMatrix(reference_sat_orbit_radius, mu); + system_matrix_ = CalculateHillSystemMatrix(reference_sat_orbit_radius, mu_m3_s2); } default: { // NOT REACHED @@ -76,11 +78,11 @@ void RelativeOrbit::CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_m } } -void RelativeOrbit::CalculateSTM(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu, double elapsed_sec) { +void RelativeOrbit::CalculateSTM(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2, double elapsed_sec) { switch (stm_model_type) { case STMModel::HCW: { double reference_sat_orbit_radius = libra::norm(reference_sat_orbit->GetPosition_i_m()); - stm_ = CalculateHCWSTM(reference_sat_orbit_radius, mu, elapsed_sec); + stm_ = CalculateHCWSTM(reference_sat_orbit_radius, mu_m3_s2, elapsed_sec); } default: { // NOT REACHED @@ -103,46 +105,46 @@ void RelativeOrbit::Propagate(double end_time_s, double current_time_jd) { PropagateSTM(end_time_s); } - Vector<3> reference_sat_position_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetPosition_i_m(); - Vector<3> reference_sat_velocity_i = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().GetVelocity_i_m_s(); - Quaternion q_i2lvlh = rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit().CalcQuaternion_i2lvlh(); + Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); + Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); + Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); Quaternion q_lvlh2i = q_i2lvlh.conjugate(); - spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_) + reference_sat_position_i; - spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_) + reference_sat_velocity_i; + spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_m_) + reference_sat_position_i; + spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_m_s_) + reference_sat_velocity_i; TransformEciToEcef(); TransformEcefToGeodetic(); } void RelativeOrbit::PropagateRK4(double elapsed_sec) { - setStepWidth(prop_step_); // Re-set propagation dt - while (elapsed_sec - prop_time_ - prop_step_ > 1.0e-6) { + setStepWidth(propagation_step_s_); // Re-set propagation dt + while (elapsed_sec - propagation_time_s_ - propagation_step_s_ > 1.0e-6) { Update(); // Propagation methods of the ODE class - prop_time_ += prop_step_; + propagation_time_s_ += propagation_step_s_; } - setStepWidth(elapsed_sec - prop_time_); // Adjust the last propagation dt + setStepWidth(elapsed_sec - propagation_time_s_); // Adjust the last propagation dt Update(); - prop_time_ = elapsed_sec; - - relative_position_lvlh_[0] = state()[0]; - relative_position_lvlh_[1] = state()[1]; - relative_position_lvlh_[2] = state()[2]; - relative_velocity_lvlh_[0] = state()[3]; - relative_velocity_lvlh_[1] = state()[4]; - relative_velocity_lvlh_[2] = state()[5]; + propagation_time_s_ = elapsed_sec; + + relative_position_lvlh_m_[0] = state()[0]; + relative_position_lvlh_m_[1] = state()[1]; + relative_position_lvlh_m_[2] = state()[2]; + relative_velocity_lvlh_m_s_[0] = state()[3]; + relative_velocity_lvlh_m_s_[1] = state()[4]; + relative_velocity_lvlh_m_s_[2] = state()[5]; } void RelativeOrbit::PropagateSTM(double elapsed_sec) { Vector<6> current_state; - CalculateSTM(stm_model_type_, &(rel_info_->GetReferenceSatDynamics(reference_sat_id_)->GetOrbit()), mu_, elapsed_sec); + CalculateSTM(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2_, elapsed_sec); current_state = stm_ * initial_state_; - relative_position_lvlh_[0] = current_state[0]; - relative_position_lvlh_[1] = current_state[1]; - relative_position_lvlh_[2] = current_state[2]; - relative_velocity_lvlh_[0] = current_state[3]; - relative_velocity_lvlh_[1] = current_state[4]; - relative_velocity_lvlh_[2] = current_state[5]; + relative_position_lvlh_m_[0] = current_state[0]; + relative_position_lvlh_m_[1] = current_state[1]; + relative_position_lvlh_m_[2] = current_state[2]; + relative_velocity_lvlh_m_s_[0] = current_state[3]; + relative_velocity_lvlh_m_s_[1] = current_state[4]; + relative_velocity_lvlh_m_s_[2] = current_state[5]; } void RelativeOrbit::RHS(double t, const Vector<6>& state, Vector<6>& rhs) // only for RK4 relative dynamics propagation diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 5da09aad1..d457847b7 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -30,7 +30,7 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @brief Constructor * @param [in] celestial_information: Celestial information * @param [in] timestep: Time step [sec] - * @param [in] reference_sat_id: Reference satellite ID + * @param [in] reference_spacecraft_id: Reference satellite ID * @param [in] initial_relative_position_lvlh: Initial value of relative position at the LVLH frame of reference satellite * @param [in] initial_relative_velocity_lvlh: Initial value of relative velocity at the LVLH frame of reference satellite * @param [in] update_method: Update method @@ -38,7 +38,7 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @param [in] stm_model_type: State transition matrix type * @param [in] rel_info: Relative information */ - RelativeOrbit(const CelestialInformation* celestial_information, double mu, double timestep, int reference_sat_id, + RelativeOrbit(const CelestialInformation* celestial_information, double mu_m3_s2, double timestep, int reference_spacecraft_id, Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, RelativeOrbitUpdateMethod update_method, RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, RelativeInformation* rel_info); /** @@ -67,49 +67,49 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { virtual void RHS(double t, const Vector<6>& state, Vector<6>& rhs); private: - double mu_; //!< Gravity constant of the center body [m3/s2] - int reference_sat_id_; //!< Reference satellite ID - double prop_time_; //!< Simulation current time for numerical integration by RK4 [sec] - double prop_step_; //!< Step width for RK4 [sec] + double mu_m3_s2_; //!< Gravity constant of the center body [m3/s2] + unsigned int reference_spacecraft_id_; //!< Reference satellite ID + double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] + double propagation_step_s_; //!< Step width for RK4 [sec] Matrix<6, 6> system_matrix_; //!< System matrix Matrix<6, 6> stm_; //!< State transition matrix - Vector<6> initial_state_; //!< Initial state (Position and Velocity) - Vector<3> relative_position_lvlh_; //!< Relative position in the LVLH frame - Vector<3> relative_velocity_lvlh_; //!< Relative velocity in the LVLH frame + Vector<6> initial_state_; //!< Initial state (Position and Velocity) + Vector<3> relative_position_lvlh_m_; //!< Relative position in the LVLH frame + Vector<3> relative_velocity_lvlh_m_s_; //!< Relative velocity in the LVLH frame RelativeOrbitUpdateMethod update_method_; //!< Update method RelativeOrbitModel relative_dynamics_model_type_; //!< Relative dynamics model type STMModel stm_model_type_; //!< State Transition Matrix model type - RelativeInformation* rel_info_; //!< Relative information + RelativeInformation* relative_information_; //!< Relative information /** * @fn InitializeState * @brief Initialize state variables * @param [in] initial_relative_position_lvlh: Initial value of relative position at the LVLH frame of reference satellite * @param [in] initial_relative_velocity_lvlh: Initial value of relative velocity at the LVLH frame of reference satellite - * @param [in] mu: Gravity constant of the center body [m3/s2] + * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] init_time: Initialize time [sec] */ - void InitializeState(Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, double mu, double init_time = 0); + void InitializeState(Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, double mu_m3_s2, double init_time = 0); /** * @fn CalculateSystemMatrix * @brief Calculate system matrix * @param [in] relative_dynamics_model_type: Relative dynamics model type * @param [in] reference_sat_orbit: Orbit information of reference satellite - * @param [in] mu: Gravity constant of the center body [m3/s2] + * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] */ - void CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double mu); + void CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2); /** * @fn CalculateSTM * @brief Calculate State Transition Matrix * @param [in] stm_model_type: STM model type * @param [in] reference_sat_orbit: Orbit information of reference satellite - * @param [in] mu: Gravity constant of the center body [m3/s2] + * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] elapsed_sec: Elapsed time [sec] */ - void CalculateSTM(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu, double elapsed_sec); + void CalculateSTM(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2, double elapsed_sec); /** * @fn PropagateRK4 * @brief Propagate relative orbit with RK4 diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index c77b8cca7..7501a556b 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -10,13 +10,13 @@ using std::string; -Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu, double timestep, Vector<3> init_position, +Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double timestep, Vector<3> init_position, Vector<3> init_velocity, double init_time) - : Orbit(celestial_information), ODE(timestep), mu(mu) { + : Orbit(celestial_information), ODE(timestep), mu_m3_s2(mu_m3_s2) { propagate_mode_ = OrbitPropagateMode::kRk4; - prop_time_ = 0.0; - prop_step_ = timestep; + propagation_time_s_ = 0.0; + propagation_step_s_ = timestep; spacecraft_acceleration_i_m_s2_ *= 0; Initialize(init_position, init_velocity, init_time); @@ -33,9 +33,9 @@ void Rk4OrbitPropagation::RHS(double t, const Vector& state, Vector& rhs) rhs[0] = vx; rhs[1] = vy; rhs[2] = vz; - rhs[3] = spacecraft_acceleration_i_m_s2_[0] - mu / r3 * x; - rhs[4] = spacecraft_acceleration_i_m_s2_[1] - mu / r3 * y; - rhs[5] = spacecraft_acceleration_i_m_s2_[2] - mu / r3 * z; + rhs[3] = spacecraft_acceleration_i_m_s2_[0] - mu_m3_s2 / r3 * x; + rhs[4] = spacecraft_acceleration_i_m_s2_[1] - mu_m3_s2 / r3 * y; + rhs[5] = spacecraft_acceleration_i_m_s2_[2] - mu_m3_s2 / r3 * z; (void)t; } @@ -69,14 +69,14 @@ void Rk4OrbitPropagation::Propagate(double end_time_s, double current_time_jd) { if (!is_calc_enabled_) return; - setStepWidth(prop_step_); // Re-set propagation Δt - while (end_time_s - prop_time_ - prop_step_ > 1.0e-6) { + setStepWidth(propagation_step_s_); // Re-set propagation Δt + while (end_time_s - propagation_time_s_ - propagation_step_s_ > 1.0e-6) { Update(); // Propagation methods of the ODE class - prop_time_ += prop_step_; + propagation_time_s_ += propagation_step_s_; } - setStepWidth(end_time_s - prop_time_); // Adjust the last propagation Δt + setStepWidth(end_time_s - propagation_time_s_); // Adjust the last propagation Δt Update(); - prop_time_ = end_time_s; + propagation_time_s_ = end_time_s; spacecraft_position_i_m_[0] = state()[0]; spacecraft_position_i_m_[1] = state()[1]; diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index 65986b7a0..be781571a 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -18,21 +18,21 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { private: static const int N = 6; //!< Degrees of freedom in 3D space - double mu; //!< Gravity constant [m3/s2] + double mu_m3_s2; //!< Gravity constant [m3/s2] public: /** * @fn Rk4OrbitPropagation * @brief Constructor * @param [in] celestial_information: Celestial information - * @param [in] mu: Gravity constant [m3/s2] + * @param [in] mu_m3_s2: Gravity constant [m3/s2] * @param [in] timestep: Step width [sec] * @param [in] init_position: Initial value of position in the inertial frame [m] * @param [in] init_velocity: Initial value of velocity in the inertial frame [m/s] * @param [in] init_time: Initial time [sec] */ - Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu, double timestep, Vector<3> init_position, Vector<3> init_velocity, - double init_time = 0); + Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double timestep, Vector<3> init_position, + Vector<3> init_velocity, double init_time = 0); /** * @fn ~Rk4OrbitPropagation * @brief Destructor @@ -67,8 +67,8 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { virtual void AddPositionOffset(Vector<3> offset_i); private: - double prop_time_; //!< Simulation current time for numerical integration by RK4 [sec] - double prop_step_; //!< Step width for RK4 [sec] + double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] + double propagation_step_s_; //!< Step width for RK4 [sec] /** * @fn Initialize From c1a51f6b66e6d02bb9a0d1b099e924ea71fbb4ab Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:13:45 +0100 Subject: [PATCH 37/57] Add libra:: --- src/dynamics/orbit/relative_orbit.cpp | 26 ++++++++++++++------------ src/dynamics/orbit/relative_orbit.hpp | 18 ++++++++++-------- 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index eb8ef6807..68291ee52 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -9,7 +9,7 @@ #include "rk4_orbit_propagation.hpp" RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double mu_m3_s2, double timestep, int reference_spacecraft_id, - Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, + libra::Vector<3> initial_relative_position_lvlh, libra::Vector<3> initial_relative_velocity_lvlh, RelativeOrbitUpdateMethod update_method, RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, RelativeInformation* rel_info) : Orbit(celestial_information), @@ -30,7 +30,7 @@ RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, RelativeOrbit::~RelativeOrbit() {} -void RelativeOrbit::InitializeState(Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, double mu_m3_s2, +void RelativeOrbit::InitializeState(libra::Vector<3> initial_relative_position_lvlh, libra::Vector<3> initial_relative_velocity_lvlh, double mu_m3_s2, double init_time) { relative_position_lvlh_m_ = initial_relative_position_lvlh; relative_velocity_lvlh_m_s_ = initial_relative_velocity_lvlh; @@ -38,10 +38,11 @@ void RelativeOrbit::InitializeState(Vector<3> initial_relative_position_lvlh, Ve // Disturbance acceleration are not considered in relative orbit propagation spacecraft_acceleration_i_m_s2_ *= 0; - Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); - Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); - Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); - Quaternion q_lvlh2i = q_i2lvlh.conjugate(); + libra::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); + libra::Vector<3> reference_sat_velocity_i = + relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); + libra::Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); + libra::Quaternion q_lvlh2i = q_i2lvlh.conjugate(); spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_m_) + reference_sat_position_i; spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_m_s_) + reference_sat_velocity_i; @@ -105,10 +106,11 @@ void RelativeOrbit::Propagate(double end_time_s, double current_time_jd) { PropagateSTM(end_time_s); } - Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); - Vector<3> reference_sat_velocity_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); - Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); - Quaternion q_lvlh2i = q_i2lvlh.conjugate(); + libra::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); + libra::Vector<3> reference_sat_velocity_i = + relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s(); + libra::Quaternion q_i2lvlh = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().CalcQuaternion_i2lvlh(); + libra::Quaternion q_lvlh2i = q_i2lvlh.conjugate(); spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_m_) + reference_sat_position_i; spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_m_s_) + reference_sat_velocity_i; @@ -135,7 +137,7 @@ void RelativeOrbit::PropagateRK4(double elapsed_sec) { } void RelativeOrbit::PropagateSTM(double elapsed_sec) { - Vector<6> current_state; + libra::Vector<6> current_state; CalculateSTM(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2_, elapsed_sec); current_state = stm_ * initial_state_; @@ -147,7 +149,7 @@ void RelativeOrbit::PropagateSTM(double elapsed_sec) { relative_velocity_lvlh_m_s_[2] = current_state[5]; } -void RelativeOrbit::RHS(double t, const Vector<6>& state, Vector<6>& rhs) // only for RK4 relative dynamics propagation +void RelativeOrbit::RHS(double t, const libra::Vector<6>& state, libra::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 d457847b7..a9315776a 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -39,8 +39,9 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @param [in] rel_info: Relative information */ RelativeOrbit(const CelestialInformation* celestial_information, double mu_m3_s2, double timestep, int reference_spacecraft_id, - Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, RelativeOrbitUpdateMethod update_method, - RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, RelativeInformation* rel_info); + libra::Vector<3> initial_relative_position_lvlh, libra::Vector<3> initial_relative_velocity_lvlh, + RelativeOrbitUpdateMethod update_method, RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, + RelativeInformation* rel_info); /** * @fn ~RelativeOrbit * @brief Destructor @@ -72,12 +73,12 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] double propagation_step_s_; //!< Step width for RK4 [sec] - Matrix<6, 6> system_matrix_; //!< System matrix - Matrix<6, 6> stm_; //!< State transition matrix + libra::Matrix<6, 6> system_matrix_; //!< System matrix + libra::Matrix<6, 6> stm_; //!< State transition matrix - Vector<6> initial_state_; //!< Initial state (Position and Velocity) - Vector<3> relative_position_lvlh_m_; //!< Relative position in the LVLH frame - Vector<3> relative_velocity_lvlh_m_s_; //!< Relative velocity in the LVLH frame + libra::Vector<6> initial_state_; //!< Initial state (Position and Velocity) + libra::Vector<3> relative_position_lvlh_m_; //!< Relative position in the LVLH frame + libra::Vector<3> relative_velocity_lvlh_m_s_; //!< Relative velocity in the LVLH frame RelativeOrbitUpdateMethod update_method_; //!< Update method RelativeOrbitModel relative_dynamics_model_type_; //!< Relative dynamics model type @@ -92,7 +93,8 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] init_time: Initialize time [sec] */ - void InitializeState(Vector<3> initial_relative_position_lvlh, Vector<3> initial_relative_velocity_lvlh, double mu_m3_s2, double init_time = 0); + void InitializeState(libra::Vector<3> initial_relative_position_lvlh, libra::Vector<3> initial_relative_velocity_lvlh, double mu_m3_s2, + double init_time = 0); /** * @fn CalculateSystemMatrix * @brief Calculate system matrix From 88703ef56943339fc88dc09386e1ad3bf437c96d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:18:07 +0100 Subject: [PATCH 38/57] Fix private function names --- src/dynamics/orbit/relative_orbit.cpp | 50 ++++++++++---------- src/dynamics/orbit/relative_orbit.hpp | 34 +++++++------ src/dynamics/orbit/rk4_orbit_propagation.cpp | 8 ++-- src/dynamics/orbit/rk4_orbit_propagation.hpp | 4 +- 4 files changed, 47 insertions(+), 49 deletions(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 68291ee52..0bc9217ce 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -8,35 +8,35 @@ #include "rk4_orbit_propagation.hpp" -RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double mu_m3_s2, double timestep, int reference_spacecraft_id, - libra::Vector<3> initial_relative_position_lvlh, libra::Vector<3> initial_relative_velocity_lvlh, +RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, int reference_spacecraft_id, + libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, - RelativeInformation* rel_info) + RelativeInformation* relative_information) : Orbit(celestial_information), - libra::ODE<6>(timestep), + libra::ODE<6>(time_step_s), mu_m3_s2_(mu_m3_s2), reference_spacecraft_id_(reference_spacecraft_id), update_method_(update_method), relative_dynamics_model_type_(relative_dynamics_model_type), stm_model_type_(stm_model_type), - relative_information_(rel_info) { + relative_information_(relative_information) { propagate_mode_ = OrbitPropagateMode::kRelativeOrbit; propagation_time_s_ = 0.0; - propagation_step_s_ = timestep; + propagation_step_s_ = time_step_s; - InitializeState(initial_relative_position_lvlh, initial_relative_velocity_lvlh, mu_m3_s2); + InitializeState(relative_position_lvlh_m, relative_velocity_lvlh_m_s, mu_m3_s2); } RelativeOrbit::~RelativeOrbit() {} -void RelativeOrbit::InitializeState(libra::Vector<3> initial_relative_position_lvlh, libra::Vector<3> initial_relative_velocity_lvlh, double mu_m3_s2, +void RelativeOrbit::InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, double mu_m3_s2, double init_time) { - relative_position_lvlh_m_ = initial_relative_position_lvlh; - relative_velocity_lvlh_m_s_ = initial_relative_velocity_lvlh; + relative_position_lvlh_m_ = relative_position_lvlh_m; + relative_velocity_lvlh_m_s_ = relative_velocity_lvlh_m_s; // Disturbance acceleration are not considered in relative orbit propagation - spacecraft_acceleration_i_m_s2_ *= 0; + spacecraft_acceleration_i_m_s2_ *= 0.0; libra::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); libra::Vector<3> reference_sat_velocity_i = @@ -46,12 +46,12 @@ void RelativeOrbit::InitializeState(libra::Vector<3> initial_relative_position_l spacecraft_position_i_m_ = q_lvlh2i.frame_conv(relative_position_lvlh_m_) + reference_sat_position_i; spacecraft_velocity_i_m_s_ = q_lvlh2i.frame_conv(relative_velocity_lvlh_m_s_) + reference_sat_velocity_i; - initial_state_[0] = initial_relative_position_lvlh[0]; - initial_state_[1] = initial_relative_position_lvlh[1]; - initial_state_[2] = initial_relative_position_lvlh[2]; - initial_state_[3] = initial_relative_velocity_lvlh[0]; - initial_state_[4] = initial_relative_velocity_lvlh[1]; - initial_state_[5] = initial_relative_velocity_lvlh[2]; + initial_state_[0] = relative_position_lvlh_m[0]; + initial_state_[1] = relative_position_lvlh_m[1]; + initial_state_[2] = relative_position_lvlh_m[2]; + initial_state_[3] = relative_velocity_lvlh_m_s[0]; + initial_state_[4] = relative_velocity_lvlh_m_s[1]; + initial_state_[5] = relative_velocity_lvlh_m_s[2]; if (update_method_ == RK4) { setup(init_time, initial_state_); @@ -59,7 +59,7 @@ void RelativeOrbit::InitializeState(libra::Vector<3> initial_relative_position_l mu_m3_s2); } else // update_method_ == STM { - CalculateSTM(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2, 0.0); + CalculateStm(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2, 0.0); } TransformEciToEcef(); @@ -79,7 +79,7 @@ void RelativeOrbit::CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_m } } -void RelativeOrbit::CalculateSTM(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2, double elapsed_sec) { +void RelativeOrbit::CalculateStm(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2, double elapsed_sec) { switch (stm_model_type) { case STMModel::HCW: { double reference_sat_orbit_radius = libra::norm(reference_sat_orbit->GetPosition_i_m()); @@ -97,13 +97,13 @@ void RelativeOrbit::Propagate(double end_time_s, double current_time_jd) { if (!is_calc_enabled_) return; - spacecraft_acceleration_i_m_s2_ *= 0; // Disturbance acceleration are not considered in relative orbit propagation + spacecraft_acceleration_i_m_s2_ *= 0.0; // Disturbance acceleration are not considered in relative orbit propagation if (update_method_ == RK4) { - PropagateRK4(end_time_s); + PropagateRk4(end_time_s); } else // update_method_ == STM { - PropagateSTM(end_time_s); + PropagateStm(end_time_s); } libra::Vector<3> reference_sat_position_i = relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(); @@ -118,7 +118,7 @@ void RelativeOrbit::Propagate(double end_time_s, double current_time_jd) { TransformEcefToGeodetic(); } -void RelativeOrbit::PropagateRK4(double elapsed_sec) { +void RelativeOrbit::PropagateRk4(double elapsed_sec) { setStepWidth(propagation_step_s_); // Re-set propagation dt while (elapsed_sec - propagation_time_s_ - propagation_step_s_ > 1.0e-6) { Update(); // Propagation methods of the ODE class @@ -136,9 +136,9 @@ void RelativeOrbit::PropagateRK4(double elapsed_sec) { relative_velocity_lvlh_m_s_[2] = state()[5]; } -void RelativeOrbit::PropagateSTM(double elapsed_sec) { +void RelativeOrbit::PropagateStm(double elapsed_sec) { libra::Vector<6> current_state; - CalculateSTM(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2_, elapsed_sec); + CalculateStm(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2_, elapsed_sec); current_state = stm_ * initial_state_; relative_position_lvlh_m_[0] = current_state[0]; diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index a9315776a..97783419c 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -29,19 +29,18 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @fn RelativeOrbit * @brief Constructor * @param [in] celestial_information: Celestial information - * @param [in] timestep: Time step [sec] + * @param [in] time_step_s: Time step [sec] * @param [in] reference_spacecraft_id: Reference satellite ID - * @param [in] initial_relative_position_lvlh: Initial value of relative position at the LVLH frame of reference satellite - * @param [in] initial_relative_velocity_lvlh: Initial value of relative velocity at the LVLH frame of reference satellite + * @param [in] relative_position_lvlh_m: Initial value of relative position at the LVLH frame of reference satellite + * @param [in] relative_velocity_lvlh_m_s: Initial value of relative velocity at the LVLH frame of reference satellite * @param [in] update_method: Update method * @param [in] relative_dynamics_model_type: Relative dynamics model type * @param [in] stm_model_type: State transition matrix type - * @param [in] rel_info: Relative information + * @param [in] relative_information: Relative information */ - RelativeOrbit(const CelestialInformation* celestial_information, double mu_m3_s2, double timestep, int reference_spacecraft_id, - libra::Vector<3> initial_relative_position_lvlh, libra::Vector<3> initial_relative_velocity_lvlh, - RelativeOrbitUpdateMethod update_method, RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, - RelativeInformation* rel_info); + RelativeOrbit(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, int reference_spacecraft_id, + libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, + RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, RelativeInformation* relative_information); /** * @fn ~RelativeOrbit * @brief Destructor @@ -88,13 +87,12 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { /** * @fn InitializeState * @brief Initialize state variables - * @param [in] initial_relative_position_lvlh: Initial value of relative position at the LVLH frame of reference satellite - * @param [in] initial_relative_velocity_lvlh: Initial value of relative velocity at the LVLH frame of reference satellite + * @param [in] relative_position_lvlh_m: Initial value of relative position at the LVLH frame of reference satellite + * @param [in] relative_velocity_lvlh_m_s: Initial value of relative velocity at the LVLH frame of reference satellite * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] init_time: Initialize time [sec] */ - void InitializeState(libra::Vector<3> initial_relative_position_lvlh, libra::Vector<3> initial_relative_velocity_lvlh, double mu_m3_s2, - double init_time = 0); + void InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, double mu_m3_s2, double init_time = 0); /** * @fn CalculateSystemMatrix * @brief Calculate system matrix @@ -104,26 +102,26 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { */ void CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2); /** - * @fn CalculateSTM + * @fn CalculateStm * @brief Calculate State Transition Matrix * @param [in] stm_model_type: STM model type * @param [in] reference_sat_orbit: Orbit information of reference satellite * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] elapsed_sec: Elapsed time [sec] */ - void CalculateSTM(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2, double elapsed_sec); + void CalculateStm(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2, double elapsed_sec); /** - * @fn PropagateRK4 + * @fn PropagateRk4 * @brief Propagate relative orbit with RK4 * @param [in] elapsed_sec: Elapsed time [sec] */ - void PropagateRK4(double elapsed_sec); + void PropagateRk4(double elapsed_sec); /** - * @fn PropagateSTM + * @fn PropagateStm * @brief Propagate relative orbit with STM * @param [in] elapsed_sec: Elapsed time [sec] */ - void PropagateSTM(double elapsed_sec); + void PropagateStm(double elapsed_sec); }; #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 7501a556b..7cd1237c2 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -10,13 +10,13 @@ using std::string; -Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double timestep, Vector<3> init_position, - Vector<3> init_velocity, double init_time) - : Orbit(celestial_information), ODE(timestep), mu_m3_s2(mu_m3_s2) { +Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, + Vector<3> init_position, Vector<3> init_velocity, double init_time) + : Orbit(celestial_information), ODE(time_step_s), mu_m3_s2(mu_m3_s2) { propagate_mode_ = OrbitPropagateMode::kRk4; propagation_time_s_ = 0.0; - propagation_step_s_ = timestep; + propagation_step_s_ = time_step_s; spacecraft_acceleration_i_m_s2_ *= 0; Initialize(init_position, init_velocity, init_time); diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index be781571a..91f99b487 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -26,12 +26,12 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { * @brief Constructor * @param [in] celestial_information: Celestial information * @param [in] mu_m3_s2: Gravity constant [m3/s2] - * @param [in] timestep: Step width [sec] + * @param [in] time_step_s: Step width [sec] * @param [in] init_position: Initial value of position in the inertial frame [m] * @param [in] init_velocity: Initial value of velocity in the inertial frame [m/s] * @param [in] init_time: Initial time [sec] */ - Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double timestep, Vector<3> init_position, + Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, Vector<3> init_position, Vector<3> init_velocity, double init_time = 0); /** * @fn ~Rk4OrbitPropagation From f3e7ec3aee3eadc6999203fcd243992633031333 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:23:13 +0100 Subject: [PATCH 39/57] Fix unnecessary private variables --- src/dynamics/orbit/rk4_orbit_propagation.cpp | 6 +++--- src/dynamics/orbit/rk4_orbit_propagation.hpp | 9 ++++----- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index 7cd1237c2..79bdd4c4c 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -12,7 +12,7 @@ using std::string; Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, Vector<3> init_position, Vector<3> init_velocity, double init_time) - : Orbit(celestial_information), ODE(time_step_s), mu_m3_s2(mu_m3_s2) { + : Orbit(celestial_information), ODE<6>(time_step_s), mu_m3_s2(mu_m3_s2) { propagate_mode_ = OrbitPropagateMode::kRk4; propagation_time_s_ = 0.0; @@ -24,7 +24,7 @@ Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_i Rk4OrbitPropagation::~Rk4OrbitPropagation() {} -void Rk4OrbitPropagation::RHS(double t, const Vector& state, Vector& rhs) { +void Rk4OrbitPropagation::RHS(double t, const Vector<6>& state, Vector<6>& rhs) { double x = state[0], y = state[1], z = state[2]; double vx = state[3], vy = state[4], vz = state[5]; @@ -42,7 +42,7 @@ void Rk4OrbitPropagation::RHS(double t, const Vector& state, Vector& rhs) void Rk4OrbitPropagation::Initialize(Vector<3> init_position, Vector<3> init_velocity, double init_time) { // state vector [x,y,z,vx,vy,vz] - Vector init_state; + Vector<6> init_state; init_state[0] = init_position[0]; init_state[1] = init_position[1]; init_state[2] = init_position[2]; diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index 91f99b487..860c4e47f 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -16,10 +16,6 @@ * @brief Class to propagate spacecraft orbit with Runge-Kutta-4 method */ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { - private: - static const int N = 6; //!< Degrees of freedom in 3D space - double mu_m3_s2; //!< Gravity constant [m3/s2] - public: /** * @fn Rk4OrbitPropagation @@ -47,7 +43,7 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] state: Position and velocity as state vector * @param [out] rhs: Output of the function */ - virtual void RHS(double t, const Vector& state, Vector& rhs); + virtual void RHS(double t, const Vector<6>& state, Vector<6>& rhs); // Override Orbit /** @@ -78,6 +74,9 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] init_time: Initial time [sec] */ void Initialize(Vector<3> init_position, Vector<3> init_velocity, double init_time = 0); + + private: + double mu_m3_s2; //!< Gravity constant [m3/s2] }; #endif // S2E_DYNAMICS_ORBIT_RK4_ORBIT_PROPAGATION_HPP_ From e56bd09210451a116fef39107fb08e4f09d0154a Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:24:33 +0100 Subject: [PATCH 40/57] Remove unused public functions --- src/dynamics/orbit/rk4_orbit_propagation.cpp | 11 ----------- src/dynamics/orbit/rk4_orbit_propagation.hpp | 12 +----------- 2 files changed, 1 insertion(+), 22 deletions(-) diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index 79bdd4c4c..a29ed88a2 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -88,14 +88,3 @@ void Rk4OrbitPropagation::Propagate(double end_time_s, double current_time_jd) { TransformEciToEcef(); TransformEcefToGeodetic(); } - -void Rk4OrbitPropagation::AddPositionOffset(Vector<3> offset_i) { - auto newstate = state(); - for (auto i = 0; i < 3; i++) { - newstate[i] += offset_i[i]; - } - setup(x(), newstate); - spacecraft_position_i_m_[0] = state()[0]; - spacecraft_position_i_m_[1] = state()[1]; - spacecraft_position_i_m_[2] = state()[2]; -} diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index 860c4e47f..7207bfd78 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -54,15 +54,8 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { */ virtual void Propagate(double end_time_s, double current_time_jd); - /** - * @fn AddPositionOffset - * @brief Shift the position of the spacecraft - * @note Is this really needed? - * @param [in] offset_i: Offset vector in the inertial frame [m] - */ - virtual void AddPositionOffset(Vector<3> offset_i); - private: + double mu_m3_s2; //!< Gravity constant [m3/s2] double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] double propagation_step_s_; //!< Step width for RK4 [sec] @@ -74,9 +67,6 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] init_time: Initial time [sec] */ void Initialize(Vector<3> init_position, Vector<3> init_velocity, double init_time = 0); - - private: - double mu_m3_s2; //!< Gravity constant [m3/s2] }; #endif // S2E_DYNAMICS_ORBIT_RK4_ORBIT_PROPAGATION_HPP_ From 4cd96db764fbe2c7bfb38289d6ff4795641c964e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:26:29 +0100 Subject: [PATCH 41/57] Fix function argument name --- src/dynamics/orbit/relative_orbit.cpp | 4 ++-- src/dynamics/orbit/relative_orbit.hpp | 5 +++-- src/dynamics/orbit/rk4_orbit_propagation.cpp | 20 ++++++++++---------- src/dynamics/orbit/rk4_orbit_propagation.hpp | 18 +++++++++--------- 4 files changed, 24 insertions(+), 23 deletions(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 0bc9217ce..25827a635 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -31,7 +31,7 @@ RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, RelativeOrbit::~RelativeOrbit() {} void RelativeOrbit::InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, double mu_m3_s2, - double init_time) { + double initiali_time_s) { relative_position_lvlh_m_ = relative_position_lvlh_m; relative_velocity_lvlh_m_s_ = relative_velocity_lvlh_m_s; @@ -54,7 +54,7 @@ void RelativeOrbit::InitializeState(libra::Vector<3> relative_position_lvlh_m, l initial_state_[5] = relative_velocity_lvlh_m_s[2]; if (update_method_ == RK4) { - setup(init_time, initial_state_); + setup(initiali_time_s, initial_state_); CalculateSystemMatrix(relative_dynamics_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2); } else // update_method_ == STM diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 97783419c..8a45fff09 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -90,9 +90,10 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @param [in] relative_position_lvlh_m: Initial value of relative position at the LVLH frame of reference satellite * @param [in] relative_velocity_lvlh_m_s: Initial value of relative velocity at the LVLH frame of reference satellite * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] - * @param [in] init_time: Initialize time [sec] + * @param [in] initiali_time_s: Initialize time [sec] */ - void InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, double mu_m3_s2, double init_time = 0); + void InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, double mu_m3_s2, + double initiali_time_s = 0); /** * @fn CalculateSystemMatrix * @brief Calculate system matrix diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index a29ed88a2..190f13df2 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -11,7 +11,7 @@ using std::string; Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, - Vector<3> init_position, Vector<3> init_velocity, double init_time) + Vector<3> position_i_m, Vector<3> velocity_i_m_s, double initiali_time_s) : Orbit(celestial_information), ODE<6>(time_step_s), mu_m3_s2(mu_m3_s2) { propagate_mode_ = OrbitPropagateMode::kRk4; @@ -19,7 +19,7 @@ Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_i propagation_step_s_ = time_step_s; spacecraft_acceleration_i_m_s2_ *= 0; - Initialize(init_position, init_velocity, init_time); + Initialize(position_i_m, velocity_i_m_s, initiali_time_s); } Rk4OrbitPropagation::~Rk4OrbitPropagation() {} @@ -40,16 +40,16 @@ void Rk4OrbitPropagation::RHS(double t, const Vector<6>& state, Vector<6>& rhs) (void)t; } -void Rk4OrbitPropagation::Initialize(Vector<3> init_position, Vector<3> init_velocity, double init_time) { +void Rk4OrbitPropagation::Initialize(Vector<3> position_i_m, Vector<3> velocity_i_m_s, double initiali_time_s) { // state vector [x,y,z,vx,vy,vz] Vector<6> init_state; - init_state[0] = init_position[0]; - init_state[1] = init_position[1]; - init_state[2] = init_position[2]; - init_state[3] = init_velocity[0]; - init_state[4] = init_velocity[1]; - init_state[5] = init_velocity[2]; - setup(init_time, init_state); + init_state[0] = position_i_m[0]; + init_state[1] = position_i_m[1]; + init_state[2] = position_i_m[2]; + init_state[3] = velocity_i_m_s[0]; + init_state[4] = velocity_i_m_s[1]; + init_state[5] = velocity_i_m_s[2]; + setup(initiali_time_s, init_state); // initialize spacecraft_acceleration_i_m_s2_ *= 0; diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index 7207bfd78..90ed3cfac 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -23,12 +23,12 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] celestial_information: Celestial information * @param [in] mu_m3_s2: Gravity constant [m3/s2] * @param [in] time_step_s: Step width [sec] - * @param [in] init_position: Initial value of position in the inertial frame [m] - * @param [in] init_velocity: Initial value of velocity in the inertial frame [m/s] - * @param [in] init_time: Initial time [sec] + * @param [in] position_i_m: Initial value of position in the inertial frame [m] + * @param [in] velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] + * @param [in] initiali_time_s: Initial time [sec] */ - Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, Vector<3> init_position, - Vector<3> init_velocity, double init_time = 0); + Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, Vector<3> position_i_m, + Vector<3> velocity_i_m_s, double initiali_time_s = 0); /** * @fn ~Rk4OrbitPropagation * @brief Destructor @@ -62,11 +62,11 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { /** * @fn Initialize * @brief Initialize function - * @param [in] init_position: Initial value of position in the inertial frame [m] - * @param [in] init_velocity: Initial value of velocity in the inertial frame [m/s] - * @param [in] init_time: Initial time [sec] + * @param [in] position_i_m: Initial value of position in the inertial frame [m] + * @param [in] velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] + * @param [in] initiali_time_s: Initial time [sec] */ - void Initialize(Vector<3> init_position, Vector<3> init_velocity, double init_time = 0); + void Initialize(Vector<3> position_i_m, Vector<3> velocity_i_m_s, double initiali_time_s = 0); }; #endif // S2E_DYNAMICS_ORBIT_RK4_ORBIT_PROPAGATION_HPP_ From d9f8a884d11c60ddbc98fc282e429fb4bce07b99 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:32:20 +0100 Subject: [PATCH 42/57] Fix function argument name --- src/dynamics/orbit/relative_orbit.cpp | 4 ++-- src/dynamics/orbit/relative_orbit.hpp | 4 ++-- src/dynamics/orbit/rk4_orbit_propagation.cpp | 14 ++++++-------- src/dynamics/orbit/rk4_orbit_propagation.hpp | 12 ++++++------ 4 files changed, 16 insertions(+), 18 deletions(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 25827a635..73c19c145 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -31,7 +31,7 @@ RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, RelativeOrbit::~RelativeOrbit() {} void RelativeOrbit::InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, double mu_m3_s2, - double initiali_time_s) { + double initial_time_s) { relative_position_lvlh_m_ = relative_position_lvlh_m; relative_velocity_lvlh_m_s_ = relative_velocity_lvlh_m_s; @@ -54,7 +54,7 @@ void RelativeOrbit::InitializeState(libra::Vector<3> relative_position_lvlh_m, l initial_state_[5] = relative_velocity_lvlh_m_s[2]; if (update_method_ == RK4) { - setup(initiali_time_s, initial_state_); + setup(initial_time_s, initial_state_); CalculateSystemMatrix(relative_dynamics_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2); } else // update_method_ == STM diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 8a45fff09..1f15aa4dd 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -90,10 +90,10 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @param [in] relative_position_lvlh_m: Initial value of relative position at the LVLH frame of reference satellite * @param [in] relative_velocity_lvlh_m_s: Initial value of relative velocity at the LVLH frame of reference satellite * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] - * @param [in] initiali_time_s: Initialize time [sec] + * @param [in] initial_time_s: Initialize time [sec] */ void InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, double mu_m3_s2, - double initiali_time_s = 0); + double initial_time_s = 0); /** * @fn CalculateSystemMatrix * @brief Calculate system matrix diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index 190f13df2..befe3f427 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -8,10 +8,8 @@ #include #include -using std::string; - Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, - Vector<3> position_i_m, Vector<3> velocity_i_m_s, double initiali_time_s) + libra::Vector<3> position_i_m, libra::Vector<3> velocity_i_m_s, double initial_time_s) : Orbit(celestial_information), ODE<6>(time_step_s), mu_m3_s2(mu_m3_s2) { propagate_mode_ = OrbitPropagateMode::kRk4; @@ -19,12 +17,12 @@ Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_i propagation_step_s_ = time_step_s; spacecraft_acceleration_i_m_s2_ *= 0; - Initialize(position_i_m, velocity_i_m_s, initiali_time_s); + Initialize(position_i_m, velocity_i_m_s, initial_time_s); } Rk4OrbitPropagation::~Rk4OrbitPropagation() {} -void Rk4OrbitPropagation::RHS(double t, const Vector<6>& state, Vector<6>& rhs) { +void Rk4OrbitPropagation::RHS(double t, const libra::Vector<6>& state, libra::Vector<6>& rhs) { double x = state[0], y = state[1], z = state[2]; double vx = state[3], vy = state[4], vz = state[5]; @@ -40,16 +38,16 @@ void Rk4OrbitPropagation::RHS(double t, const Vector<6>& state, Vector<6>& rhs) (void)t; } -void Rk4OrbitPropagation::Initialize(Vector<3> position_i_m, Vector<3> velocity_i_m_s, double initiali_time_s) { +void Rk4OrbitPropagation::Initialize(Vector<3> position_i_m, libra::Vector<3> velocity_i_m_s, double initial_time_s) { // state vector [x,y,z,vx,vy,vz] - Vector<6> init_state; + libra::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]; init_state[3] = velocity_i_m_s[0]; init_state[4] = velocity_i_m_s[1]; init_state[5] = velocity_i_m_s[2]; - setup(initiali_time_s, init_state); + setup(initial_time_s, init_state); // initialize spacecraft_acceleration_i_m_s2_ *= 0; diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index 90ed3cfac..989cb1209 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -25,10 +25,10 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] time_step_s: Step width [sec] * @param [in] position_i_m: Initial value of position in the inertial frame [m] * @param [in] velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] - * @param [in] initiali_time_s: Initial time [sec] + * @param [in] initial_time_s: Initial time [sec] */ - Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, Vector<3> position_i_m, - Vector<3> velocity_i_m_s, double initiali_time_s = 0); + Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, libra::Vector<3> position_i_m, + libra::Vector<3> velocity_i_m_s, double initial_time_s = 0); /** * @fn ~Rk4OrbitPropagation * @brief Destructor @@ -43,7 +43,7 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { * @param [in] state: Position and velocity as state vector * @param [out] rhs: Output of the function */ - virtual void RHS(double t, const Vector<6>& state, Vector<6>& rhs); + virtual void RHS(double t, const libra::Vector<6>& state, libra::Vector<6>& rhs); // Override Orbit /** @@ -64,9 +64,9 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { * @brief Initialize function * @param [in] position_i_m: Initial value of position in the inertial frame [m] * @param [in] velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] - * @param [in] initiali_time_s: Initial time [sec] + * @param [in] initial_time_s: Initial time [sec] */ - void Initialize(Vector<3> position_i_m, Vector<3> velocity_i_m_s, double initiali_time_s = 0); + void Initialize(libra::Vector<3> position_i_m, libra::Vector<3> velocity_i_m_s, double initial_time_s = 0); }; #endif // S2E_DYNAMICS_ORBIT_RK4_ORBIT_PROPAGATION_HPP_ From 9aa77e1b0b28a6f0bb388b46820f01fb6b49fd2e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:34:19 +0100 Subject: [PATCH 43/57] Remove using --- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 7 +++---- src/dynamics/orbit/sgp4_orbit_propagation.hpp | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index 02c23137d..616db0b3b 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -8,7 +8,6 @@ #include #include #include -using namespace std; Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, int wgs, double current_time_jd) : Orbit(celestial_information) { @@ -58,8 +57,8 @@ void Sgp4OrbitPropagation::Propagate(double end_time_s, double current_time_jd) TransformEcefToGeodetic(); } -Vector<3> Sgp4OrbitPropagation::GetESIOmega() { - Vector<3> omega_peri = Vector<3>(); +libra::Vector<3> Sgp4OrbitPropagation::GetESIOmega() { + libra::Vector<3> omega_peri = libra::Vector<3>(); omega_peri[0] = 0.0; omega_peri[1] = 0.0; omega_peri[2] = satrec_.no / 60; @@ -75,7 +74,7 @@ Vector<3> Sgp4OrbitPropagation::GetESIOmega() { double sOMEGA = sin(OMEGA); double si = sin(i); - Matrix<3, 3> PERI2ECI = Matrix<3, 3>(); + libra::Matrix<3, 3> PERI2ECI = libra::Matrix<3, 3>(); PERI2ECI[0][0] = comega * cOMEGA - somega * ci * sOMEGA; PERI2ECI[1][0] = -1.0 * somega * cOMEGA - 1.0 * comega * ci * sOMEGA; PERI2ECI[2][0] = si * sOMEGA; diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.hpp b/src/dynamics/orbit/sgp4_orbit_propagation.hpp index 9e84bc096..05ff286c8 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.hpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.hpp @@ -44,7 +44,7 @@ class Sgp4OrbitPropagation : public Orbit { * @brief Calculate and return ? * @note Is this function needed? */ - Vector<3> GetESIOmega(); + libra::Vector<3> GetESIOmega(); private: gravconsttype whichconst_; //!< Gravity constant value type From 80599a615da3d852bb04f176292cf1c04a4af7a3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:35:23 +0100 Subject: [PATCH 44/57] Remove unused public function --- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 31 ------------------- src/dynamics/orbit/sgp4_orbit_propagation.hpp | 7 ----- 2 files changed, 38 deletions(-) diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index 616db0b3b..3c7630be3 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -56,34 +56,3 @@ void Sgp4OrbitPropagation::Propagate(double end_time_s, double current_time_jd) TransformEciToEcef(); TransformEcefToGeodetic(); } - -libra::Vector<3> Sgp4OrbitPropagation::GetESIOmega() { - libra::Vector<3> omega_peri = libra::Vector<3>(); - omega_peri[0] = 0.0; - omega_peri[1] = 0.0; - omega_peri[2] = satrec_.no / 60; - - double i = satrec_.inclo; // inclination - double OMEGA = satrec_.nodeo; // raan - double omega = satrec_.argpo; // argment of perigee - - double comega = cos(omega); - double cOMEGA = cos(OMEGA); - double ci = cos(i); - double somega = sin(omega); - double sOMEGA = sin(OMEGA); - double si = sin(i); - - libra::Matrix<3, 3> PERI2ECI = libra::Matrix<3, 3>(); - PERI2ECI[0][0] = comega * cOMEGA - somega * ci * sOMEGA; - PERI2ECI[1][0] = -1.0 * somega * cOMEGA - 1.0 * comega * ci * sOMEGA; - PERI2ECI[2][0] = si * sOMEGA; - PERI2ECI[0][1] = comega * sOMEGA + somega * ci * cOMEGA; - PERI2ECI[1][1] = -1.0 * somega * sOMEGA + comega * ci * cOMEGA; - PERI2ECI[2][1] = -1.0 * si * cOMEGA; - PERI2ECI[0][2] = somega * si; - PERI2ECI[1][2] = comega * si; - PERI2ECI[2][2] = ci; - - return PERI2ECI * omega_peri; -} diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.hpp b/src/dynamics/orbit/sgp4_orbit_propagation.hpp index 05ff286c8..727391294 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.hpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.hpp @@ -39,13 +39,6 @@ class Sgp4OrbitPropagation : public Orbit { */ virtual void Propagate(double end_time_s, double current_time_jd); - /** - * @fn GetESIOmega - * @brief Calculate and return ? - * @note Is this function needed? - */ - libra::Vector<3> GetESIOmega(); - private: gravconsttype whichconst_; //!< Gravity constant value type elsetrec satrec_; //!< Structure data for SGP4 library From 28c4ed997cbff741ce7fd11ad1bc9afb4c5104ab Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:40:00 +0100 Subject: [PATCH 45/57] Fix private variables --- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 14 +++++++------- src/dynamics/orbit/sgp4_orbit_propagation.hpp | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index 3c7630be3..d56059f7b 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -14,17 +14,17 @@ Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial propagate_mode_ = OrbitPropagateMode::kSgp4; if (wgs == 0) { - whichconst_ = wgs72old; + gravity_constant_setting_ = wgs72old; } else if (wgs == 1) { - whichconst_ = wgs72; + gravity_constant_setting_ = wgs72; } else if (wgs == 2) { - whichconst_ = wgs84; + gravity_constant_setting_ = wgs84; } char typerun = 'c', typeinput = 0; double startmfe, stopmfe, deltamin; - twoline2rv(tle1, tle2, typerun, typeinput, whichconst_, startmfe, stopmfe, deltamin, satrec_); + twoline2rv(tle1, tle2, typerun, typeinput, gravity_constant_setting_, startmfe, stopmfe, deltamin, sgp4_data_); spacecraft_acceleration_i_m_s2_ *= 0; @@ -38,15 +38,15 @@ void Sgp4OrbitPropagation::Propagate(double end_time_s, double current_time_jd) UNUSED(end_time_s); if (!is_calc_enabled_) return; - double elapse_time_min = (current_time_jd - satrec_.jdsatepoch) * (24.0 * 60.0); + double elapse_time_min = (current_time_jd - sgp4_data_.jdsatepoch) * (24.0 * 60.0); double r[3]; double v[3]; - sgp4(whichconst_, satrec_, elapse_time_min, r, v); + sgp4(gravity_constant_setting_, sgp4_data_, elapse_time_min, r, v); // Error in SGP4 - if (satrec_.error > 0) printf("# *** error: time:= %f *** code = %3d\n", satrec_.t, satrec_.error); + if (sgp4_data_.error > 0) printf("# *** error: time:= %f *** code = %3d\n", sgp4_data_.t, sgp4_data_.error); for (int i = 0; i < 3; ++i) { spacecraft_position_i_m_[i] = r[i] * 1000; diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.hpp b/src/dynamics/orbit/sgp4_orbit_propagation.hpp index 727391294..7b93a70a0 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.hpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.hpp @@ -40,8 +40,8 @@ class Sgp4OrbitPropagation : public Orbit { virtual void Propagate(double end_time_s, double current_time_jd); private: - gravconsttype whichconst_; //!< Gravity constant value type - elsetrec satrec_; //!< Structure data for SGP4 library + gravconsttype gravity_constant_setting_; //!< Gravity constant value type + elsetrec sgp4_data_; //!< Structure data for SGP4 library const CelestialInformation* celestial_information_; //!< Celestial information }; From e0df5a68fb83e8ec64963df85fa6e23f71f186ed Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:41:31 +0100 Subject: [PATCH 46/57] Add const --- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 3 ++- src/dynamics/orbit/sgp4_orbit_propagation.hpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index d56059f7b..521fc36e0 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -9,7 +9,8 @@ #include #include -Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, int wgs, double current_time_jd) +Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, const int wgs, + 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 7b93a70a0..80f5d6155 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.hpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.hpp @@ -28,7 +28,7 @@ class Sgp4OrbitPropagation : public Orbit { * @param [in] wgs: Wold Geodetic System * @param [in] current_time_jd: Current Julian day [day] */ - Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, int wgs, double current_time_jd); + Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, const int wgs, const double current_time_jd); // Override Orbit /** @@ -37,7 +37,7 @@ class Sgp4OrbitPropagation : public Orbit { * @param [in] end_time_s: End time of simulation [sec] * @param [in] current_time_jd: Current Julian day [day] */ - virtual void Propagate(double end_time_s, double current_time_jd); + virtual void Propagate(const double end_time_s, const double current_time_jd); private: gravconsttype gravity_constant_setting_; //!< Gravity constant value type From f9e67f430a4c163251322b2b96394efd98c5fe90 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 20:42:13 +0100 Subject: [PATCH 47/57] Fix function argument names --- src/dynamics/orbit/initialize_orbit.cpp | 4 ++-- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 8 ++++---- src/dynamics/orbit/sgp4_orbit_propagation.hpp | 5 +++-- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index c191972ca..d6540fd24 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -36,12 +36,12 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string orbit = new Rk4OrbitPropagation(celestial_information, mu_m3_s2, step_width_s, position_i_m, velocity_i_m_s); } else if (propagate_mode == "SGP4") { // Initialize SGP4 orbit propagator - int wgs = conf.ReadInt(section_, "wgs"); + int wgs_setting = conf.ReadInt(section_, "wgs_setting"); char tle1[80], tle2[80]; conf.ReadChar(section_, "tle1", 80, tle1); conf.ReadChar(section_, "tle2", 80, tle2); - orbit = new Sgp4OrbitPropagation(celestial_information, tle1, tle2, wgs, current_time_jd); + orbit = new Sgp4OrbitPropagation(celestial_information, tle1, tle2, wgs_setting, current_time_jd); } else if (propagate_mode == "RELATIVE") { // initialize orbit for relative dynamics of formation flying RelativeOrbit::RelativeOrbitUpdateMethod update_method = diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index 521fc36e0..37580e5a2 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -9,16 +9,16 @@ #include #include -Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, const int wgs, +Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, const int wgs_setting, const double current_time_jd) : Orbit(celestial_information) { propagate_mode_ = OrbitPropagateMode::kSgp4; - if (wgs == 0) { + if (wgs_setting == 0) { gravity_constant_setting_ = wgs72old; - } else if (wgs == 1) { + } else if (wgs_setting == 1) { gravity_constant_setting_ = wgs72; - } else if (wgs == 2) { + } else if (wgs_setting == 2) { gravity_constant_setting_ = wgs84; } diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.hpp b/src/dynamics/orbit/sgp4_orbit_propagation.hpp index 80f5d6155..5943ae359 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.hpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.hpp @@ -25,10 +25,11 @@ class Sgp4OrbitPropagation : public Orbit { * @param [in] celestial_information: Celestial information * @param [in] tle1: The first line of TLE * @param [in] tle2: The second line of TLE - * @param [in] wgs: Wold Geodetic System + * @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, const double current_time_jd); + Sgp4OrbitPropagation(const CelestialInformation* celestial_information, char* tle1, char* tle2, const int wgs_setting, + const double current_time_jd); // Override Orbit /** From e263a6cf8284b5dc19449db37383d40226b94cb2 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 23 Feb 2023 21:39:22 +0100 Subject: [PATCH 48/57] Fix local function variables --- src/dynamics/orbit/sgp4_orbit_propagation.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp index 37580e5a2..b4c741857 100644 --- a/src/dynamics/orbit/sgp4_orbit_propagation.cpp +++ b/src/dynamics/orbit/sgp4_orbit_propagation.cpp @@ -22,12 +22,12 @@ Sgp4OrbitPropagation::Sgp4OrbitPropagation(const CelestialInformation* celestial gravity_constant_setting_ = wgs84; } - char typerun = 'c', typeinput = 0; - double startmfe, stopmfe, deltamin; + char type_run = 'c', type_input = 0; + double start_mfe, stop_mfe, delta_min; - twoline2rv(tle1, tle2, typerun, typeinput, gravity_constant_setting_, startmfe, stopmfe, deltamin, sgp4_data_); + twoline2rv(tle1, tle2, type_run, type_input, gravity_constant_setting_, start_mfe, stop_mfe, delta_min, sgp4_data_); - spacecraft_acceleration_i_m_s2_ *= 0; + spacecraft_acceleration_i_m_s2_ *= 0.0; // To calculate initial position and velocity is_calc_enabled_ = true; @@ -41,17 +41,17 @@ void Sgp4OrbitPropagation::Propagate(double end_time_s, double current_time_jd) if (!is_calc_enabled_) return; double elapse_time_min = (current_time_jd - sgp4_data_.jdsatepoch) * (24.0 * 60.0); - double r[3]; - double v[3]; + double position_i_km[3]; + double velocity_i_km_s[3]; - sgp4(gravity_constant_setting_, sgp4_data_, elapse_time_min, r, v); + sgp4(gravity_constant_setting_, sgp4_data_, elapse_time_min, position_i_km, velocity_i_km_s); // Error in SGP4 if (sgp4_data_.error > 0) printf("# *** error: time:= %f *** code = %3d\n", sgp4_data_.t, sgp4_data_.error); for (int i = 0; i < 3; ++i) { - spacecraft_position_i_m_[i] = r[i] * 1000; - spacecraft_velocity_i_m_s_[i] = v[i] * 1000; + spacecraft_position_i_m_[i] = position_i_km[i] * 1000.0; + spacecraft_velocity_i_m_s_[i] = velocity_i_km_s[i] * 1000.0; } TransformEciToEcef(); From f19e74f8be7a20af2c4422f347adfffda13ad49b Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 24 Feb 2023 14:34:40 +0100 Subject: [PATCH 49/57] Add libra:: --- src/dynamics/orbit/initialize_orbit.cpp | 34 ++++++++++++------------- src/dynamics/orbit/initialize_orbit.hpp | 2 +- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index d6540fd24..d6e188662 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 - Vector<3> position_i_m; - Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_m3_s2); + libra::Vector<3> position_i_m; + libra::Vector<3> velocity_i_m_s; + libra::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_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 RelativeOrbitModel relative_dynamics_model_type = (RelativeOrbitModel)(conf.ReadInt(section_, "relative_dynamics_model_type")); STMModel stm_model_type = (STMModel)(conf.ReadInt(section_, "stm_model_type")); - Vector<3> init_relative_position_lvlh; + libra::Vector<3> init_relative_position_lvlh; conf.ReadVector<3>(section_, "initial_relative_position_lvlh_m", init_relative_position_lvlh); - Vector<3> init_relative_velocity_lvlh; + libra::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 @@ -67,9 +67,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 - Vector<3> init_pos_m; + libra::Vector<3> init_pos_m; conf.ReadVector<3>(section_, "initial_position_i_m", init_pos_m); - Vector<3> init_vel_m_s; + libra::Vector<3> init_vel_m_s; conf.ReadVector<3>(section_, "initial_velocity_i_m_s", init_vel_m_s); oe = OrbitalElements(mu_m3_s2, current_time_jd, init_pos_m, init_vel_m_s); } else { @@ -86,9 +86,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 - Vector<3> position_i_m; - Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_m3_s2); + libra::Vector<3> position_i_m; + libra::Vector<3> velocity_i_m_s; + libra::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_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; - Vector<3> position_i_m; - Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_m3_s2); + libra::Vector<3> position_i_m; + libra::Vector<3> velocity_i_m_s; + libra::Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_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; } -Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double mu_m3_s2, std::string section) { +libra::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double mu_m3_s2, std::string section) { auto conf = IniAccess(initialize_file); const char* section_ = section.c_str(); - Vector<3> position_i_m; - Vector<3> velocity_i_m_s; - Vector<6> pos_vel; + libra::Vector<3> position_i_m; + libra::Vector<3> velocity_i_m_s; + libra::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 e41755506..72bc8b82a 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -34,6 +34,6 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string * @param [in] mu_m3_s2: Gravity constant [m3/s2] * @param [in] section: Section name */ -Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double mu_m3_s2, std::string section = "ORBIT"); +libra::Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double mu_m3_s2, std::string section = "ORBIT"); #endif // S2E_DYNAMICS_ORBIT_INITIALIZE_ORBIT_HPP_ From 8a45282167be7bdc4f73e7c52b5227935b45ecfb Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 24 Feb 2023 14:43:24 +0100 Subject: [PATCH 50/57] Add libra:: --- src/dynamics/orbit/initialize_orbit.cpp | 1 - src/dynamics/orbit/orbit.cpp | 2 +- src/dynamics/orbit/rk4_orbit_propagation.cpp | 2 +- src/dynamics/thermal/initialize_node.cpp | 2 +- src/dynamics/thermal/node.cpp | 4 ++-- src/dynamics/thermal/node.hpp | 10 +++++----- src/dynamics/thermal/temperature.cpp | 6 +++--- src/dynamics/thermal/temperature.hpp | 6 +++--- 8 files changed, 16 insertions(+), 17 deletions(-) diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index d6e188662..e8940b2d1 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -62,7 +62,6 @@ 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 - double mu_m3_s2 = mu_m3_s2; OrbitalElements oe; // TODO: init_mode_kepler should be removed in the next major update if (initialize_mode == OrbitInitializeMode::kInertialPositionAndVelocity) { diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index 58e2bb9e9..24e2335ed 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -4,7 +4,7 @@ */ #include "orbit.hpp" -Quaternion Orbit::CalcQuaternion_i2lvlh() const { +libra::Quaternion Orbit::CalcQuaternion_i2lvlh() const { libra::Vector<3> lvlh_x = spacecraft_position_i_m_; // x-axis in LVLH frame is position vector direction from geocenter to satellite libra::Vector<3> lvlh_ex = normalize(lvlh_x); libra::Vector<3> lvlh_z = diff --git a/src/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index befe3f427..0a54ddc6f 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -38,7 +38,7 @@ void Rk4OrbitPropagation::RHS(double t, const libra::Vector<6>& state, libra::Ve (void)t; } -void Rk4OrbitPropagation::Initialize(Vector<3> position_i_m, libra::Vector<3> velocity_i_m_s, double initial_time_s) { +void Rk4OrbitPropagation::Initialize(libra::Vector<3> position_i_m, libra::Vector<3> velocity_i_m_s, double initial_time_s) { // state vector [x,y,z,vx,vy,vz] libra::Vector<6> init_state; init_state[0] = position_i_m[0]; diff --git a/src/dynamics/thermal/initialize_node.cpp b/src/dynamics/thermal/initialize_node.cpp index cbf26ecb2..85530b9ab 100644 --- a/src/dynamics/thermal/initialize_node.cpp +++ b/src/dynamics/thermal/initialize_node.cpp @@ -51,7 +51,7 @@ Node InitNode(const std::vector& node_str) { capacity = stod(node_str[3]); // column 4 alpha = stod(node_str[4]); // column 5 area = stod(node_str[5]); // column 6 - Vector<3> normal_v_b; // column 7-9 + libra::Vector<3> normal_v_b; // column 7-9 for (int i = 0; i < 3; i++) { normal_v_b[i] = stod(node_str[6 + i]); } // body frame diff --git a/src/dynamics/thermal/node.cpp b/src/dynamics/thermal/node.cpp index f4e88a40b..a82775434 100644 --- a/src/dynamics/thermal/node.cpp +++ b/src/dynamics/thermal/node.cpp @@ -12,7 +12,7 @@ using namespace std; using namespace libra; Node::Node(const int node_id, const string node_label, const int heater_node_id, const double temperature_ini, const double capacity_ini, - const double internal_heat_ini, const double alpha, const double area, Vector<3> normal_v_b) + const double internal_heat_ini, const double alpha, const double area, libra::Vector<3> normal_v_b) : node_id_(node_id), node_label_(node_label), heater_node_id_(heater_node_id), @@ -57,7 +57,7 @@ void Node::SetInternalHeat(double heat_power) { internal_heat_ = heat_power; // [W] } -double Node::CalcSolarRadiation(Vector<3> sun_direction) { +double Node::CalcSolarRadiation(libra::Vector<3> sun_direction) { // FIXME: constants double R = 6.96E+8; // Distance from sun double T = 5778; // sun surface temperature [K] diff --git a/src/dynamics/thermal/node.hpp b/src/dynamics/thermal/node.hpp index 1a429fb2a..e0932036e 100644 --- a/src/dynamics/thermal/node.hpp +++ b/src/dynamics/thermal/node.hpp @@ -19,19 +19,19 @@ class Node { double capacity_; // 熱容量[J/K] double internal_heat_; // 内部生成熱[J] double alpha_; - double area_; // 太陽熱が入射する面の面積[m^2] - Vector<3> normal_v_b_; // 太陽熱が入射する面の法線ベクトル(機体固定座標系) - double solar_radiation_; // 入射する太陽輻射熱[W]([J]に変換するためには時間をかけないといけないことに注意 + double area_; // 太陽熱が入射する面の面積[m^2] + libra::Vector<3> normal_v_b_; // 太陽熱が入射する面の法線ベクトル(機体固定座標系) + double solar_radiation_; // 入射する太陽輻射熱[W]([J]に変換するためには時間をかけないといけないことに注意 double K2deg(double kelvin) const; // 絶対温度からdegCに変換 public: Node(const int node_id, const std::string node_label, const int heater_node_id, const double temperature_ini, const double capacity_ini, - const double internal_heat_ini, const double alpha, const double area, Vector<3> normal_v_b); + const double internal_heat_ini, const double alpha, const double area, libra::Vector<3> normal_v_b); virtual ~Node(); // 熱計算用関数 - double CalcSolarRadiation(Vector<3> sun_direction); // 太陽入射熱を計算 + double CalcSolarRadiation(libra::Vector<3> sun_direction); // 太陽入射熱を計算 // Output from this class int GetNodeId(void) const; diff --git a/src/dynamics/thermal/temperature.cpp b/src/dynamics/thermal/temperature.cpp index 7108a0429..8eee383d7 100644 --- a/src/dynamics/thermal/temperature.cpp +++ b/src/dynamics/thermal/temperature.cpp @@ -36,7 +36,7 @@ Temperature::Temperature() { Temperature::~Temperature() {} -void Temperature::Propagate(Vector<3> sun_direction, const double endtime) { +void Temperature::Propagate(libra::Vector<3> sun_direction, const double endtime) { if (!is_calc_enabled_) return; while (endtime - prop_time_ - prop_step_ > 1.0e-6) { RungeOneStep(prop_time_, prop_step_, sun_direction, node_num_); @@ -64,7 +64,7 @@ void Temperature::Propagate(Vector<3> sun_direction, const double endtime) { } } -void Temperature::RungeOneStep(double t, double dt, Vector<3> sun_direction, int node_num) { +void Temperature::RungeOneStep(double t, double dt, libra::Vector<3> sun_direction, int node_num) { vector x(node_num); for (int i = 0; i < node_num; i++) { x[i] = vnodes_[i].GetTemperature_K(); @@ -100,7 +100,7 @@ void Temperature::RungeOneStep(double t, double dt, Vector<3> sun_direction, int } } -vector Temperature::OdeTemperature(vector x, double t, Vector<3> sun_direction, int node_num) { +vector Temperature::OdeTemperature(vector x, double t, libra::Vector<3> sun_direction, int node_num) { // TODO: consider the following unused arguments are really needed UNUSED(x); UNUSED(t); diff --git a/src/dynamics/thermal/temperature.hpp b/src/dynamics/thermal/temperature.hpp index 7bed885f8..63f4a3c79 100644 --- a/src/dynamics/thermal/temperature.hpp +++ b/src/dynamics/thermal/temperature.hpp @@ -23,8 +23,8 @@ class Temperature : public ILoggable { bool is_calc_enabled_; // 温度更新をするかどうかのブーリアン bool debug_; - void RungeOneStep(double t, double dt, Vector<3> sun_direction, int node_num); - std::vector OdeTemperature(std::vector x, double t, const Vector<3> sun_direction, + void RungeOneStep(double t, double dt, libra::Vector<3> sun_direction, int node_num); + std::vector OdeTemperature(std::vector x, double t, const libra::Vector<3> sun_direction, int node_num); // 温度に関する常微分方程式, xはnodeの温度をならべたもの public: @@ -32,7 +32,7 @@ class Temperature : public ILoggable { const double propstep, const bool is_calc_enabled, const bool debug); Temperature(); virtual ~Temperature(); - void Propagate(Vector<3> sun_direction, + void Propagate(libra::Vector<3> sun_direction, const double endtime); // 太陽入熱量計算のため, 太陽方向の情報を入手 std::vector GetVnodes() const; void AddHeaterPower(std::vector heater_power); From e8835faef5533a5a1f9577d6ec6a7df92329609e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 24 Feb 2023 14:45:15 +0100 Subject: [PATCH 51/57] Add libra:: --- src/dynamics/orbit/orbit.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index 24e2335ed..7470ce3c1 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -24,7 +24,7 @@ libra::Quaternion Orbit::CalcQuaternion_i2lvlh() const { dcm_i2lvlh[2][1] = lvlh_ez[1]; dcm_i2lvlh[2][2] = lvlh_ez[2]; - libra::Quaternion q_i2lvlh = Quaternion::fromDCM(dcm_i2lvlh); + libra::Quaternion q_i2lvlh = libra::Quaternion::fromDCM(dcm_i2lvlh); return q_i2lvlh.normalize(); } From 314407478dad7533189dbc2eb38d4941919b8085 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 24 Feb 2023 14:49:24 +0100 Subject: [PATCH 52/57] Add libra:: --- src/dynamics/attitude/attitude_rk4.hpp | 9 +++++---- src/dynamics/attitude/controlled_attitude.hpp | 14 +++++++------- src/dynamics/attitude/initialize_attitude.cpp | 12 ++++++------ src/dynamics/attitude/initialize_attitude.hpp | 2 +- 4 files changed, 19 insertions(+), 18 deletions(-) diff --git a/src/dynamics/attitude/attitude_rk4.hpp b/src/dynamics/attitude/attitude_rk4.hpp index b7324e9ef..dc7bc2d54 100644 --- a/src/dynamics/attitude/attitude_rk4.hpp +++ b/src/dynamics/attitude/attitude_rk4.hpp @@ -24,8 +24,9 @@ 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 Vector<3>& angular_velocity_b_rad_s, const Quaternion& quaternion_i2b, const Matrix<3, 3>& inertia_tensor_kgm2, - const Vector<3>& torque_b_Nm, const double propagation_step_s, const std::string& simulation_object_name = "Attitude"); + AttitudeRk4(const libra::Vector<3>& angular_velocity_b_rad_s, const libra::Quaternion& quaternion_i2b, + const libra::Matrix<3, 3>& inertia_tensor_kgm2, const libra::Vector<3>& torque_b_Nm, const double propagation_step_s, + const std::string& simulation_object_name = "Attitude"); /** * @fn ~AttitudeRk4 * @brief Destructor @@ -54,14 +55,14 @@ class AttitudeRk4 : public Attitude { * @brief Generate angular velocity matrix for kinematics calculation * @param [in] angular_velocity_b_rad_s: Angular velocity [rad/s] */ - Matrix<4, 4> CalcAngularVelocityMatrix(Vector<3> angular_velocity_b_rad_s); + libra::Matrix<4, 4> CalcAngularVelocityMatrix(libra::Vector<3> angular_velocity_b_rad_s); /** * @fn AttitudeDynamicsAndKinematics * @brief Dynamics equation with kinematics * @param [in] x: State vector (angular velocity and quaternion) * @param [in] t: Unused TODO: remove? */ - Vector<7> AttitudeDynamicsAndKinematics(Vector<7> x, double t); + libra::Vector<7> AttitudeDynamicsAndKinematics(libra::Vector<7> x, double t); /** * @fn RungeKuttaOneStep * @brief Equation for one step of Runge-Kutta method diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index 934bbad12..9627c8a0d 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -53,8 +53,8 @@ class ControlledAttitude : public Attitude { * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const Quaternion quaternion_i2b, - const Vector<3> main_target_direction_b, const Vector<3> sub_target_direction_b, const Matrix<3, 3>& inertia_tensor_kgm2, - const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, + const libra::Vector<3> main_target_direction_b, const libra::Vector<3> sub_target_direction_b, + const libra::Matrix<3, 3>& inertia_tensor_kgm2, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit, const std::string& simulation_object_name = "Attitude"); /** * @fn ~ControlledAttitude @@ -82,12 +82,12 @@ class ControlledAttitude : public Attitude { * @fn SetMainTargetDirection_b * @brief Set main target direction on the body fixed frame */ - inline void SetMainTargetDirection_b(Vector<3> main_target_direction_b) { main_target_direction_b_ = main_target_direction_b; } + inline void SetMainTargetDirection_b(libra::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(Vector<3> sub_target_direction_b) { sub_target_direction_b_ = sub_target_direction_b; } + inline void SetSubTargetDirection_b(libra::Vector<3> sub_target_direction_b) { sub_target_direction_b_ = sub_target_direction_b; } /** * @fn Propagate @@ -121,14 +121,14 @@ class ControlledAttitude : public Attitude { * @param [in] mode: Attitude control mode * @return Target direction at the inertia frame0 */ - Vector<3> CalcTargetDirection_i(AttitudeControlMode mode); + libra::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 Vector<3> main_direction_i, const Vector<3> sub_direction_i); + void PointingControl(const libra::Vector<3> main_direction_i, const libra::Vector<3> sub_direction_i); /** * @fn CalcAngularVelocity * @brief Calculate angular velocity @@ -141,7 +141,7 @@ class ControlledAttitude : public Attitude { * @param [in] main_direction: Main target direction * @param [in] sub_direction: Sub target direction */ - Matrix<3, 3> CalcDcm(const Vector<3> main_direction, const Vector<3> sub_direction); + libra::Matrix<3, 3> CalcDcm(const libra::Vector<3> main_direction, const libra::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 1ecd982d6..97bbeca92 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 Matrix<3, 3> inertia_tensor_kgm2, const int spacecraft_id) { + const double step_width_s, const libra::Matrix<3, 3> inertia_tensor_kgm2, const int spacecraft_id) { IniAccess ini_file(file_name); const char* section_ = "ATTITUDE"; std::string mc_name = section_ + std::to_string(spacecraft_id); // FIXME @@ -17,11 +17,11 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel if (propagate_mode == "RK4") { // RK4 propagator - Vector<3> omega_b; + libra::Vector<3> omega_b; ini_file.ReadVector(section_, "initial_angular_velocity_b_rad_s", omega_b); Quaternion quaternion_i2b; ini_file.ReadQuaternion(section_, "initial_quaternion_i2b", quaternion_i2b); - Vector<3> torque_b; + libra::Vector<3> torque_b; ini_file.ReadVector(section_, "initial_torque_b_Nm", torque_b); attitude = new AttitudeRk4(omega_b, quaternion_i2b, inertia_tensor_kgm2, torque_b, step_width_s, mc_name); @@ -36,7 +36,7 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel AttitudeControlMode sub_mode = ConvertStringToCtrlMode(sub_mode_in); Quaternion quaternion_i2b; ini_file_ca.ReadQuaternion(section_, "initial_quaternion_i2b", quaternion_i2b); - Vector<3> main_target_direction_b, sub_target_direction_b; + libra::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); attitude = new ControlledAttitude(main_mode, sub_mode, quaternion_i2b, main_target_direction_b, sub_target_direction_b, inertia_tensor_kgm2, @@ -45,11 +45,11 @@ Attitude* InitAttitude(std::string file_name, const Orbit* orbit, const LocalCel std::cerr << "ERROR: attitude propagation mode: " << propagate_mode << " is not defined!" << std::endl; std::cerr << "The attitude mode is automatically set as RK4" << std::endl; - Vector<3> omega_b; + libra::Vector<3> omega_b; ini_file.ReadVector(section_, "initial_angular_velocity_b_rad_s", omega_b); Quaternion quaternion_i2b; ini_file.ReadQuaternion(section_, "initial_quaternion_i2b", quaternion_i2b); - Vector<3> torque_b; + libra::Vector<3> torque_b; ini_file.ReadVector(section_, "initial_torque_b_Nm", torque_b); attitude = new AttitudeRk4(omega_b, quaternion_i2b, inertia_tensor_kgm2, torque_b, step_width_s, mc_name); diff --git a/src/dynamics/attitude/initialize_attitude.hpp b/src/dynamics/attitude/initialize_attitude.hpp index 123e55a98..6b74bfd2a 100644 --- a/src/dynamics/attitude/initialize_attitude.hpp +++ b/src/dynamics/attitude/initialize_attitude.hpp @@ -21,6 +21,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 Matrix<3, 3> inertia_tensor_kgm2, const int spacecraft_id); + const double step_width_s, const libra::Matrix<3, 3> inertia_tensor_kgm2, const int spacecraft_id); #endif // S2E_DYNAMICS_ATTITUDE_INITIALIZE_ATTITUDE_HPP_ From 5272a429df28a08e55feac1b91382638902028c0 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 25 Feb 2023 14:06:30 +0100 Subject: [PATCH 53/57] Fix MACRO to const value --- src/dynamics/attitude/controlled_attitude.cpp | 5 +---- src/dynamics/attitude/controlled_attitude.hpp | 4 ++++ 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index 497f38cf0..fa1cba923 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -5,11 +5,8 @@ #include "controlled_attitude.hpp" #include -#include #include -#define THRESHOLD_CA cos(30.0 / 180.0 * libra::pi) // FIXME - ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const libra::Quaternion quaternion_i2b, const libra::Vector<3> main_target_direction_b, const libra::Vector<3> sub_target_direction_b, const libra::Matrix<3, 3>& inertia_tensor_kgm2, @@ -49,7 +46,7 @@ void ControlledAttitude::Initialize(void) { normalize(sub_target_direction_b_); double tmp = inner_product(main_target_direction_b_, sub_target_direction_b_); tmp = std::abs(tmp); - if (tmp > THRESHOLD_CA) { + if (tmp > cos(kMinDirectionAngle_rad)) { std::cout << "sub target direction should separate from the main target direction. \n"; is_calc_enabled_ = false; return; diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index 934bbad12..bd39a375f 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -7,6 +7,7 @@ #define S2E_DYNAMICS_ATTITUDE_CONTROLLED_ATTITUDE_HPP_ #include +#include #include #include "../orbit/orbit.hpp" @@ -105,6 +106,9 @@ class ControlledAttitude : public Attitude { libra::Quaternion previous_quaternion_i2b_; //!< Previous quaternion libra::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s] + const double kMinDirectionAngle_rad = 30.0 * libra::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 From ffa9b6f7d3addb41ce6e911339480e9be09ce2cc Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 25 Feb 2023 14:10:14 +0100 Subject: [PATCH 54/57] Fix mu to gravity constant --- src/dynamics/orbit/initialize_orbit.cpp | 25 ++++++++++++------------- src/dynamics/orbit/initialize_orbit.hpp | 8 ++++---- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index d6540fd24..569bb9004 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -13,7 +13,7 @@ #include "sgp4_orbit_propagation.hpp" Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string initialize_file, double step_width_s, double current_time_jd, - double mu_m3_s2, std::string section, RelativeInformation* relative_information) { + double gravity_constant_m3_s2, std::string section, RelativeInformation* relative_information) { auto conf = IniAccess(initialize_file); const char* section_ = section.c_str(); Orbit* orbit; @@ -28,12 +28,12 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // initialize RK4 orbit propagator Vector<3> position_i_m; Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_m3_s2); + 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]; } - orbit = new Rk4OrbitPropagation(celestial_information, mu_m3_s2, step_width_s, position_i_m, velocity_i_m_s); + orbit = new Rk4OrbitPropagation(celestial_information, gravity_constant_m3_s2, step_width_s, position_i_m, velocity_i_m_s); } else if (propagate_mode == "SGP4") { // Initialize SGP4 orbit propagator int wgs_setting = conf.ReadInt(section_, "wgs_setting"); @@ -58,11 +58,10 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string // the orbit of the reference sat is initialized, create temporary initial orbit of the reference sat int reference_spacecraft_id = conf.ReadInt(section_, "reference_satellite_id"); - orbit = new RelativeOrbit(celestial_information, mu_m3_s2, step_width_s, reference_spacecraft_id, init_relative_position_lvlh, + orbit = new RelativeOrbit(celestial_information, gravity_constant_m3_s2, step_width_s, reference_spacecraft_id, init_relative_position_lvlh, 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 - double mu_m3_s2 = mu_m3_s2; OrbitalElements oe; // TODO: init_mode_kepler should be removed in the next major update if (initialize_mode == OrbitInitializeMode::kInertialPositionAndVelocity) { @@ -71,7 +70,7 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string conf.ReadVector<3>(section_, "initial_position_i_m", init_pos_m); Vector<3> init_vel_m_s; conf.ReadVector<3>(section_, "initial_velocity_i_m_s", init_vel_m_s); - oe = OrbitalElements(mu_m3_s2, current_time_jd, init_pos_m, init_vel_m_s); + oe = 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"); @@ -82,32 +81,32 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string double epoch_jday = conf.ReadDouble(section_, "epoch_jday"); oe = OrbitalElements(epoch_jday, semi_major_axis_m, eccentricity, inclination_rad, raan_rad, arg_perigee_rad); } - KeplerOrbit kepler_orbit(mu_m3_s2, oe); + 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 Vector<3> position_i_m; Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_m3_s2); + 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]; } double error_tolerance = conf.ReadDouble(section_, "error_tolerance"); - orbit = new EnckeOrbitPropagation(celestial_information, mu_m3_s2, step_width_s, current_time_jd, position_i_m, velocity_i_m_s, error_tolerance); + orbit = new EnckeOrbitPropagation(celestial_information, gravity_constant_m3_s2, step_width_s, current_time_jd, position_i_m, velocity_i_m_s, error_tolerance); } else { 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; Vector<3> position_i_m; Vector<3> velocity_i_m_s; - Vector<6> pos_vel = InitializePosVel(initialize_file, current_time_jd, mu_m3_s2); + 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]; } - orbit = new Rk4OrbitPropagation(celestial_information, mu_m3_s2, step_width_s, position_i_m, velocity_i_m_s); + orbit = new Rk4OrbitPropagation(celestial_information, gravity_constant_m3_s2, step_width_s, position_i_m, velocity_i_m_s); } orbit->SetIsCalcEnabled(conf.ReadEnable(section_, "calculation")); @@ -115,7 +114,7 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string return orbit; } -Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double mu_m3_s2, std::string section) { +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(); Vector<3> position_i_m; @@ -131,7 +130,7 @@ Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double arg_perigee_rad = conf.ReadDouble(section_, "argument_of_perigee_rad"); double epoch_jday = conf.ReadDouble(section_, "epoch_jday"); OrbitalElements oe(epoch_jday, semi_major_axis_m, eccentricity, inclination_rad, raan_rad, arg_perigee_rad); - KeplerOrbit kepler_orbit(mu_m3_s2, oe); + KeplerOrbit kepler_orbit(gravity_constant_m3_s2, oe); kepler_orbit.CalcPosVel(current_time_jd); position_i_m = kepler_orbit.GetPosition_i_m(); diff --git a/src/dynamics/orbit/initialize_orbit.hpp b/src/dynamics/orbit/initialize_orbit.hpp index e41755506..b51fea8e9 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -19,21 +19,21 @@ class RelativeInformation; * @param [in] initialize_file: Path to initialize file * @param [in] step_width_s: Step width [sec] * @param [in] current_time_jd: Current Julian day [day] - * @param [in] mu_m3_s2: Gravity constant [m3/s2] + * @param [in] gravity_constant_m3_s2: Gravity constant [m3/s2] * @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, - double mu_m3_s2, std::string section = "ORBIT", RelativeInformation* relative_information = (RelativeInformation*)nullptr); + double gravity_constant_m3_s2, std::string section = "ORBIT", RelativeInformation* relative_information = (RelativeInformation*)nullptr); /** * @fn InitializePosVel * @brief Initialize position and velocity depends on initialize mode * @param [in] initialize_file: Path to initialize file * @param [in] current_time_jd: Current Julian day [day] - * @param [in] mu_m3_s2: Gravity constant [m3/s2] + * @param [in] gravity_constant_m3_s2: Gravity constant [m3/s2] * @param [in] section: Section name */ -Vector<6> InitializePosVel(std::string initialize_file, double current_time_jd, double mu_m3_s2, std::string section = "ORBIT"); +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_ From 9a4a702d0daf4ec9c21d83b849d644569079f5b5 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 25 Feb 2023 14:25:04 +0100 Subject: [PATCH 55/57] Fix mu to gravity constant --- .../orbit/encke_orbit_propagation.cpp | 11 +++---- .../orbit/encke_orbit_propagation.hpp | 12 ++++---- src/dynamics/orbit/initialize_orbit.cpp | 3 +- src/dynamics/orbit/initialize_orbit.hpp | 3 +- src/dynamics/orbit/relative_orbit.cpp | 29 ++++++++++--------- src/dynamics/orbit/relative_orbit.hpp | 17 ++++++----- src/dynamics/orbit/rk4_orbit_propagation.cpp | 10 +++---- src/dynamics/orbit/rk4_orbit_propagation.hpp | 12 ++++---- 8 files changed, 52 insertions(+), 45 deletions(-) diff --git a/src/dynamics/orbit/encke_orbit_propagation.cpp b/src/dynamics/orbit/encke_orbit_propagation.cpp index 9a9590440..cdee38c8f 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.cpp +++ b/src/dynamics/orbit/encke_orbit_propagation.cpp @@ -9,12 +9,12 @@ #include "../../library/orbit/orbital_elements.hpp" -EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double mu_m3_s2, +EnckeOrbitPropagation::EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double gravity_constant_m3_s2, const double propagation_step_s, const double current_time_jd, const libra::Vector<3> position_i_m, const libra::Vector<3> velocity_i_m_s, const double error_tolerance) : Orbit(celestial_information), libra::ODE<6>(propagation_step_s), - mu_m3_s2_(mu_m3_s2), + gravity_constant_m3_s2_(gravity_constant_m3_s2), error_tolerance_(error_tolerance), propagation_step_s_(propagation_step_s) { propagation_time_s_ = 0.0; @@ -71,7 +71,8 @@ void EnckeOrbitPropagation::RHS(double t, const libra::Vector<6>& state, libra:: double r_m = norm(reference_position_i_m_); double r_m3 = pow(r_m, 3.0); - difference_acc_i_m_s2 = -(mu_m3_s2_ / r_m3) * (q_func * spacecraft_position_i_m_ + difference_position_i_m_m) + spacecraft_acceleration_i_m_s2_; + difference_acc_i_m_s2 = + -(gravity_constant_m3_s2_ / r_m3) * (q_func * spacecraft_position_i_m_ + difference_position_i_m_m) + spacecraft_acceleration_i_m_s2_; rhs[0] = state[3]; rhs[1] = state[4]; @@ -89,8 +90,8 @@ void EnckeOrbitPropagation::Initialize(double current_time_jd, libra::Vector<3> // reference orbit reference_position_i_m_ = reference_position_i_m; reference_velocity_i_m_s_ = reference_velocity_i_m_s; - OrbitalElements oe_ref(mu_m3_s2_, current_time_jd, reference_position_i_m, reference_velocity_i_m_s); - reference_kepler_orbit = KeplerOrbit(mu_m3_s2_, oe_ref); + OrbitalElements oe_ref(gravity_constant_m3_s2_, current_time_jd, reference_position_i_m, reference_velocity_i_m_s); + reference_kepler_orbit = KeplerOrbit(gravity_constant_m3_s2_, oe_ref); // difference orbit fill_up(difference_position_i_m_, 0.0); diff --git a/src/dynamics/orbit/encke_orbit_propagation.hpp b/src/dynamics/orbit/encke_orbit_propagation.hpp index f83421d17..671fb72f0 100644 --- a/src/dynamics/orbit/encke_orbit_propagation.hpp +++ b/src/dynamics/orbit/encke_orbit_propagation.hpp @@ -20,14 +20,14 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { * @fn EnckeOrbitPropagation * @brief Constructor * @param [in] celestial_information: Celestial information - * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] + * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] propagation_step_s: Propagation step width [sec] * @param [in] current_time_jd: Current Julian day [day] * @param [in] position_i_m: Initial value of position in the inertial frame [m] * @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 mu_m3_s2, const double propagation_step_s, + EnckeOrbitPropagation(const CelestialInformation* celestial_information, const double gravity_constant_m3_s2, const double propagation_step_s, const double current_time_jd, const libra::Vector<3> position_i_m, const libra::Vector<3> velocity_i_m_s, const double error_tolerance); /** @@ -57,10 +57,10 @@ class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> { private: // General - const double mu_m3_s2_; //!< Gravity constant of the center body [m3/s2] - const double error_tolerance_; //!< Error tolerance ratio - double propagation_step_s_; //!< Propagation step width for RK4 - double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 + const double gravity_constant_m3_s2_; //!< Gravity constant of the center body [m3/s2] + const double error_tolerance_; //!< Error tolerance ratio + double propagation_step_s_; //!< Propagation step width for RK4 + double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 // reference orbit libra::Vector<3> reference_position_i_m_; //!< Reference orbit position in the inertial frame [m] diff --git a/src/dynamics/orbit/initialize_orbit.cpp b/src/dynamics/orbit/initialize_orbit.cpp index 569bb9004..b23748d8a 100644 --- a/src/dynamics/orbit/initialize_orbit.cpp +++ b/src/dynamics/orbit/initialize_orbit.cpp @@ -94,7 +94,8 @@ Orbit* InitOrbit(const CelestialInformation* celestial_information, std::string } double error_tolerance = conf.ReadDouble(section_, "error_tolerance"); - orbit = new EnckeOrbitPropagation(celestial_information, gravity_constant_m3_s2, step_width_s, current_time_jd, position_i_m, velocity_i_m_s, error_tolerance); + orbit = new EnckeOrbitPropagation(celestial_information, gravity_constant_m3_s2, step_width_s, current_time_jd, position_i_m, velocity_i_m_s, + error_tolerance); } else { 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; diff --git a/src/dynamics/orbit/initialize_orbit.hpp b/src/dynamics/orbit/initialize_orbit.hpp index b51fea8e9..405966f2b 100644 --- a/src/dynamics/orbit/initialize_orbit.hpp +++ b/src/dynamics/orbit/initialize_orbit.hpp @@ -24,7 +24,8 @@ class RelativeInformation; * @param [in] relative_information: Relative information */ 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 = "ORBIT", RelativeInformation* relative_information = (RelativeInformation*)nullptr); + double gravity_constant_m3_s2, std::string section = "ORBIT", + RelativeInformation* relative_information = (RelativeInformation*)nullptr); /** * @fn InitializePosVel diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 73c19c145..dbfcade46 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -8,13 +8,13 @@ #include "rk4_orbit_propagation.hpp" -RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, int reference_spacecraft_id, - libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, +RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, + int reference_spacecraft_id, libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, RelativeInformation* relative_information) : Orbit(celestial_information), libra::ODE<6>(time_step_s), - mu_m3_s2_(mu_m3_s2), + gravity_constant_m3_s2_(gravity_constant_m3_s2), reference_spacecraft_id_(reference_spacecraft_id), update_method_(update_method), relative_dynamics_model_type_(relative_dynamics_model_type), @@ -25,13 +25,13 @@ RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, propagation_time_s_ = 0.0; propagation_step_s_ = time_step_s; - InitializeState(relative_position_lvlh_m, relative_velocity_lvlh_m_s, mu_m3_s2); + InitializeState(relative_position_lvlh_m, relative_velocity_lvlh_m_s, gravity_constant_m3_s2); } RelativeOrbit::~RelativeOrbit() {} -void RelativeOrbit::InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, double mu_m3_s2, - double initial_time_s) { +void RelativeOrbit::InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::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; @@ -56,21 +56,23 @@ void RelativeOrbit::InitializeState(libra::Vector<3> relative_position_lvlh_m, l if (update_method_ == RK4) { setup(initial_time_s, initial_state_); CalculateSystemMatrix(relative_dynamics_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), - mu_m3_s2); + gravity_constant_m3_s2); } else // update_method_ == STM { - CalculateStm(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2, 0.0); + CalculateStm(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), gravity_constant_m3_s2, + 0.0); } TransformEciToEcef(); TransformEcefToGeodetic(); } -void RelativeOrbit::CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2) { +void RelativeOrbit::CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, + double gravity_constant_m3_s2) { switch (relative_dynamics_model_type) { case RelativeOrbitModel::Hill: { double reference_sat_orbit_radius = libra::norm(reference_sat_orbit->GetPosition_i_m()); - system_matrix_ = CalculateHillSystemMatrix(reference_sat_orbit_radius, mu_m3_s2); + system_matrix_ = CalculateHillSystemMatrix(reference_sat_orbit_radius, gravity_constant_m3_s2); } default: { // NOT REACHED @@ -79,11 +81,11 @@ void RelativeOrbit::CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_m } } -void RelativeOrbit::CalculateStm(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2, double elapsed_sec) { +void RelativeOrbit::CalculateStm(STMModel stm_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2, double elapsed_sec) { switch (stm_model_type) { case STMModel::HCW: { double reference_sat_orbit_radius = libra::norm(reference_sat_orbit->GetPosition_i_m()); - stm_ = CalculateHCWSTM(reference_sat_orbit_radius, mu_m3_s2, elapsed_sec); + stm_ = CalculateHCWSTM(reference_sat_orbit_radius, gravity_constant_m3_s2, elapsed_sec); } default: { // NOT REACHED @@ -138,7 +140,8 @@ void RelativeOrbit::PropagateRk4(double elapsed_sec) { void RelativeOrbit::PropagateStm(double elapsed_sec) { libra::Vector<6> current_state; - CalculateStm(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), mu_m3_s2_, elapsed_sec); + CalculateStm(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), gravity_constant_m3_s2_, + elapsed_sec); current_state = stm_ * initial_state_; relative_position_lvlh_m_[0] = current_state[0]; diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 1f15aa4dd..b10ddce63 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -30,6 +30,7 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @brief Constructor * @param [in] celestial_information: Celestial information * @param [in] time_step_s: Time step [sec] + * @param [in] gravity_constant_m3_s2: Gravity constant [m3/s2] * @param [in] reference_spacecraft_id: Reference satellite ID * @param [in] relative_position_lvlh_m: Initial value of relative position at the LVLH frame of reference satellite * @param [in] relative_velocity_lvlh_m_s: Initial value of relative velocity at the LVLH frame of reference satellite @@ -38,7 +39,7 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @param [in] stm_model_type: State transition matrix type * @param [in] relative_information: Relative information */ - RelativeOrbit(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, int reference_spacecraft_id, + RelativeOrbit(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, int reference_spacecraft_id, libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, RelativeOrbitUpdateMethod update_method, RelativeOrbitModel relative_dynamics_model_type, STMModel stm_model_type, RelativeInformation* relative_information); /** @@ -67,7 +68,7 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { virtual void RHS(double t, const Vector<6>& state, Vector<6>& rhs); private: - double mu_m3_s2_; //!< Gravity constant of the center body [m3/s2] + double gravity_constant_m3_s2_; //!< Gravity constant of the center body [m3/s2] unsigned int reference_spacecraft_id_; //!< Reference satellite ID double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] double propagation_step_s_; //!< Step width for RK4 [sec] @@ -89,28 +90,28 @@ class RelativeOrbit : public Orbit, public libra::ODE<6> { * @brief Initialize state variables * @param [in] relative_position_lvlh_m: Initial value of relative position at the LVLH frame of reference satellite * @param [in] relative_velocity_lvlh_m_s: Initial value of relative velocity at the LVLH frame of reference satellite - * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] + * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] initial_time_s: Initialize time [sec] */ - void InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, double mu_m3_s2, + void InitializeState(libra::Vector<3> relative_position_lvlh_m, libra::Vector<3> relative_velocity_lvlh_m_s, double gravity_constant_m3_s2, double initial_time_s = 0); /** * @fn CalculateSystemMatrix * @brief Calculate system matrix * @param [in] relative_dynamics_model_type: Relative dynamics model type * @param [in] reference_sat_orbit: Orbit information of reference satellite - * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] + * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] */ - void CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2); + void CalculateSystemMatrix(RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2); /** * @fn CalculateStm * @brief Calculate State Transition Matrix * @param [in] stm_model_type: STM model type * @param [in] reference_sat_orbit: Orbit information of reference satellite - * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] + * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] elapsed_sec: Elapsed time [sec] */ - void CalculateStm(STMModel stm_model_type, const Orbit* reference_sat_orbit, double mu_m3_s2, double elapsed_sec); + void CalculateStm(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/dynamics/orbit/rk4_orbit_propagation.cpp b/src/dynamics/orbit/rk4_orbit_propagation.cpp index befe3f427..0f756945f 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.cpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.cpp @@ -8,9 +8,9 @@ #include #include -Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double mu_m3_s2, double time_step_s, +Rk4OrbitPropagation::Rk4OrbitPropagation(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, libra::Vector<3> position_i_m, libra::Vector<3> velocity_i_m_s, double initial_time_s) - : Orbit(celestial_information), ODE<6>(time_step_s), mu_m3_s2(mu_m3_s2) { + : Orbit(celestial_information), ODE<6>(time_step_s), gravity_constant_m3_s2_(gravity_constant_m3_s2) { propagate_mode_ = OrbitPropagateMode::kRk4; propagation_time_s_ = 0.0; @@ -31,9 +31,9 @@ void Rk4OrbitPropagation::RHS(double t, const libra::Vector<6>& state, libra::Ve rhs[0] = vx; rhs[1] = vy; rhs[2] = vz; - rhs[3] = spacecraft_acceleration_i_m_s2_[0] - mu_m3_s2 / r3 * x; - rhs[4] = spacecraft_acceleration_i_m_s2_[1] - mu_m3_s2 / r3 * y; - rhs[5] = spacecraft_acceleration_i_m_s2_[2] - mu_m3_s2 / r3 * z; + rhs[3] = spacecraft_acceleration_i_m_s2_[0] - gravity_constant_m3_s2_ / r3 * x; + rhs[4] = spacecraft_acceleration_i_m_s2_[1] - gravity_constant_m3_s2_ / r3 * y; + rhs[5] = spacecraft_acceleration_i_m_s2_[2] - gravity_constant_m3_s2_ / r3 * z; (void)t; } diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index 989cb1209..6c745668d 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -21,14 +21,14 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { * @fn Rk4OrbitPropagation * @brief Constructor * @param [in] celestial_information: Celestial information - * @param [in] mu_m3_s2: Gravity constant [m3/s2] + * @param [in] gravity_constant_m3_s2: Gravity constant [m3/s2] * @param [in] time_step_s: Step width [sec] * @param [in] position_i_m: Initial value of position in the inertial frame [m] * @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 mu_m3_s2, double time_step_s, libra::Vector<3> position_i_m, - libra::Vector<3> velocity_i_m_s, double initial_time_s = 0); + Rk4OrbitPropagation(const CelestialInformation* celestial_information, double gravity_constant_m3_s2, double time_step_s, + libra::Vector<3> position_i_m, libra::Vector<3> velocity_i_m_s, double initial_time_s = 0); /** * @fn ~Rk4OrbitPropagation * @brief Destructor @@ -55,9 +55,9 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { virtual void Propagate(double end_time_s, double current_time_jd); private: - double mu_m3_s2; //!< Gravity constant [m3/s2] - double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] - double propagation_step_s_; //!< Step width for RK4 [sec] + double gravity_constant_m3_s2_; //!< Gravity constant [m3/s2] + double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] + double propagation_step_s_; //!< Step width for RK4 [sec] /** * @fn Initialize From f4d584e999d4a45b74d8af71f2e02aec177df5dc Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 25 Feb 2023 14:27:00 +0100 Subject: [PATCH 56/57] Fix mu to gravity constant --- src/disturbances/gravity_gradient.cpp | 6 +++--- src/disturbances/gravity_gradient.hpp | 6 +++--- src/disturbances/initialize_disturbances.cpp | 4 ++-- src/disturbances/initialize_disturbances.hpp | 4 ++-- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/disturbances/gravity_gradient.cpp b/src/disturbances/gravity_gradient.cpp index 479450d36..83a3be60d 100644 --- a/src/disturbances/gravity_gradient.cpp +++ b/src/disturbances/gravity_gradient.cpp @@ -13,8 +13,8 @@ GravityGradient::GravityGradient(const bool is_calculation_enabled) : GravityGradient(environment::earth_gravitational_constant_m3_s2, is_calculation_enabled) {} -GravityGradient::GravityGradient(const double mu_m3_s2, const bool is_calculation_enabled) - : SimpleDisturbance(is_calculation_enabled), mu_m3_s2_(mu_m3_s2) {} +GravityGradient::GravityGradient(const double gravity_constant_m3_s2, const bool is_calculation_enabled) + : SimpleDisturbance(is_calculation_enabled), gravity_constant_m3_s2_(gravity_constant_m3_s2) {} void GravityGradient::Update(const LocalEnvironment& local_environment, const Dynamics& dynamics) { // TODO: use structure information to get inertia tensor @@ -27,7 +27,7 @@ libra::Vector<3> GravityGradient::CalcTorque_b_Nm(const libra::Vector<3> earth_p libra::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 * mu_m3_s2_ / pow(r_norm_m, 3.0); + double coeff = 3.0 * gravity_constant_m3_s2_ / pow(r_norm_m, 3.0); torque_b_Nm_ = coeff * outer_product(u_b, inertia_tensor_b_kgm2 * u_b); return torque_b_Nm_; } diff --git a/src/disturbances/gravity_gradient.hpp b/src/disturbances/gravity_gradient.hpp index bb86857e2..11fbd7802 100644 --- a/src/disturbances/gravity_gradient.hpp +++ b/src/disturbances/gravity_gradient.hpp @@ -30,10 +30,10 @@ class GravityGradient : public SimpleDisturbance { /** * @fn GeoPotential * @brief Constructor - * @param [in] mu_m3_s2: Gravitational constant [m3/s2] + * @param [in] gravity_constant_m3_s2: Gravitational constant [m3/s2] * @param [in] is_calculation_enabled: Calculation flag */ - GravityGradient(const double mu_m3_s2, const bool is_calculation_enabled = true); + GravityGradient(const double gravity_constant_m3_s2, const bool is_calculation_enabled = true); /** * @fn Update @@ -56,7 +56,7 @@ class GravityGradient : public SimpleDisturbance { virtual std::string GetLogValue() const; private: - double mu_m3_s2_; //!< Gravitational constant [m3/s2] + double gravity_constant_m3_s2_; //!< Gravitational constant [m3/s2] /** * @fn CalcTorque diff --git a/src/disturbances/initialize_disturbances.cpp b/src/disturbances/initialize_disturbances.cpp index 98a560bff..249385f25 100644 --- a/src/disturbances/initialize_disturbances.cpp +++ b/src/disturbances/initialize_disturbances.cpp @@ -52,12 +52,12 @@ GravityGradient InitGravityGradient(const std::string initialize_file_path) { return gg_disturbance; } -GravityGradient InitGravityGradient(const std::string initialize_file_path, const double mu_m3_s2) { +GravityGradient InitGravityGradient(const std::string initialize_file_path, const double gravity_constant_m3_s2) { auto conf = IniAccess(initialize_file_path); const char* section = "GRAVITY_GRADIENT"; const bool is_calc_enable = conf.ReadEnable(section, CALC_LABEL); - GravityGradient gg_disturbance(mu_m3_s2, is_calc_enable); + GravityGradient gg_disturbance(gravity_constant_m3_s2, is_calc_enable); gg_disturbance.IsLogEnabled = conf.ReadEnable(section, LOG_LABEL); return gg_disturbance; diff --git a/src/disturbances/initialize_disturbances.hpp b/src/disturbances/initialize_disturbances.hpp index 7692d42cb..e5fea9d90 100644 --- a/src/disturbances/initialize_disturbances.hpp +++ b/src/disturbances/initialize_disturbances.hpp @@ -43,9 +43,9 @@ GravityGradient InitGravityGradient(const std::string initialize_file_path); * @fn InitGravityGradient * @brief Initialize GravityGradient class with earth gravitational constant * @param [in] initialize_file_path: Initialize file path - * @param [in] mu_m3_s2: Gravitational constant [m3/s2] + * @param [in] gravity_constant_m3_s2: Gravitational constant [m3/s2] */ -GravityGradient InitGravityGradient(const std::string initialize_file_path, const double mu_m3_s2); +GravityGradient InitGravityGradient(const std::string initialize_file_path, const double gravity_constant_m3_s2); /** * @fn InitMagneticDisturbance From edd7ba5f733a7c5ba4ba1ab6419d0f6ebc982e2d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 25 Feb 2023 14:28:51 +0100 Subject: [PATCH 57/57] Fix format --- src/dynamics/attitude/controlled_attitude.hpp | 2 +- src/dynamics/orbit/rk4_orbit_propagation.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index bd39a375f..1e7a5c96d 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -107,7 +107,7 @@ class ControlledAttitude : public Attitude { libra::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s] const double kMinDirectionAngle_rad = 30.0 * libra::deg_to_rad; //!< Minimum angle b/w main and sub direction - // TODO Change with ini file + // TODO Change with ini file // Inputs const LocalCelestialInformation* local_celestial_information_; //!< Local celestial information diff --git a/src/dynamics/orbit/rk4_orbit_propagation.hpp b/src/dynamics/orbit/rk4_orbit_propagation.hpp index 6c745668d..66dfd51a3 100644 --- a/src/dynamics/orbit/rk4_orbit_propagation.hpp +++ b/src/dynamics/orbit/rk4_orbit_propagation.hpp @@ -56,8 +56,8 @@ class Rk4OrbitPropagation : public Orbit, public libra::ODE<6> { private: double gravity_constant_m3_s2_; //!< Gravity constant [m3/s2] - double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] - double propagation_step_s_; //!< Step width for RK4 [sec] + double propagation_time_s_; //!< Simulation current time for numerical integration by RK4 [sec] + double propagation_step_s_; //!< Step width for RK4 [sec] /** * @fn Initialize