From d0a11aebf0ec0642ee6c53e14facc547b5ca4109 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 14:44:18 +0100 Subject: [PATCH 01/14] Add corner cube reflector --- .../components/aocs/corner_cube_reflector.cpp | 54 ++++++++++ .../components/aocs/corner_cube_reflector.hpp | 101 ++++++++++++++++++ 2 files changed, 155 insertions(+) create mode 100644 s2e-ff/src/components/aocs/corner_cube_reflector.cpp create mode 100644 s2e-ff/src/components/aocs/corner_cube_reflector.hpp diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.cpp b/s2e-ff/src/components/aocs/corner_cube_reflector.cpp new file mode 100644 index 00000000..6e17d545 --- /dev/null +++ b/s2e-ff/src/components/aocs/corner_cube_reflector.cpp @@ -0,0 +1,54 @@ +/** + * @file corner_cube_reflector.cpp + * @brief Corner cube reflector + */ + +#include "corner_cube_reflector.hpp" + +CornerCubeReflector::CornerCubeReflector(const int prescaler, ClockGenerator* clock_gen, const Dynamics* dynamics) + : Component(prescaler, clock_gen), dynamics_(dynamics) { + normal_direction_c_[0] = 1.0; + normal_direction_c_[1] = 0.0; + normal_direction_c_[2] = 0.0; + reflectable_angle_rad_ = 0.1; + + libra::Quaternion q_rot(0.0, 0.0, 0.0, 1.0); + libra::Vector<3> v_translation_m; + v_translation_m[0] = 0.5; + v_translation_m[1] = 0.0; + v_translation_m[2] = 0.0; + dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-v_translation_m, q_rot); +} + +void CornerCubeReflector::MainRoutine(int count) { + if (count < 10) return; + + // Body -> Inertial frame + libra::Vector<3> spacecraft_position_i2b_m = dynamics_->GetOrbit().GetPosition_i_m(); + libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b().Conjugate(); + libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b); + + libra::Vector<3> position_i_m = dual_quaternion_i2b.InverseTransformVector(libra::Vector<3>{0.0}); + + // Component -> Inertial frame + libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_; + + reflector_position_i_m_ = dual_quaternion_c2i.TransformVector(libra::Vector<3>{0.0}); + normal_direction_i_ = dual_quaternion_c2i.TransformVector(normal_direction_c_); + normal_direction_i_ -= reflector_position_i_m_; +} + +std::string CornerCubeReflector::GetLogHeader() const { + std::string str_tmp = ""; + str_tmp += WriteVector("normal_direction", "i", "", 3); + + return str_tmp; +} + +std::string CornerCubeReflector::GetLogValue() const { + std::string str_tmp = ""; + + str_tmp += WriteVector(normal_direction_i_); + + return str_tmp; +} diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp new file mode 100644 index 00000000..1843f321 --- /dev/null +++ b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp @@ -0,0 +1,101 @@ +/** + * @file corner_cube_reflector.hpp + * @brief Corner cube reflector + */ + +#ifndef S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_ +#define S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_ + +#include +#include +#include +#include + +#include "../../library/math/translation_first_dual_quaternion.hpp" + +/** + * @class CornerCubeReflector + * @brief Corner Cube Reflector + */ +class CornerCubeReflector : public Component, public ILoggable { + public: + /** + * @fn CornerCubeReflector + * @brief Constructor + */ + CornerCubeReflector() : Component(1, nullptr), dynamics_(nullptr) {} + /** + * @fn CornerCubeReflector + * @brief Constructor + */ + CornerCubeReflector(const int prescaler, ClockGenerator* clock_gen, const Dynamics* dynamics); + /** + * @fn ~CornerCubeReflector + * @brief Destructor + */ + ~CornerCubeReflector() {} + + // ComponentBase override function + /** + * @fn MainRoutine + * @brief Main routine + */ + void MainRoutine(int count); + + // Override ILoggable + /** + * @fn GetLogHeader + * @brief Override GetLogHeader function of ILoggable + */ + virtual std::string GetLogHeader() const; + /** + * @fn GetLogValue + * @brief Override GetLogValue function of ILoggable + */ + virtual std::string GetLogValue() const; + + inline libra::Vector<3> GetReflectorPosition_i_m() const { + libra::Vector<3> spacecraft_position_i2b_m = dynamics_->GetOrbit().GetPosition_i_m(); + libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b); + + libra::Vector<3> position_i_m = dual_quaternion_i2b.InverseTransformVector(libra::Vector<3>{0.0}); + + // Component -> Inertial frame + libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_; + + return dual_quaternion_c2i.TransformVector(libra::Vector<3>{0.0}); + // return reflector_position_i_m_; + } + inline libra::Vector<3> GetNormalDirection_i() const { + // Body -> Inertial frame + libra::Vector<3> spacecraft_position_i2b_m = dynamics_->GetOrbit().GetPosition_i_m(); + libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b().Conjugate(); + libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b); + + libra::Vector<3> position_i_m = dual_quaternion_i2b.InverseTransformVector(libra::Vector<3>{0.0}); + + // Component -> Inertial frame + libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_; + + libra::Vector<3> reflector_position_i_m = dual_quaternion_c2i.TransformVector(libra::Vector<3>{0.0}); + libra::Vector<3> normal_direction_i = dual_quaternion_c2i.TransformVector(normal_direction_c_); + normal_direction_i -= reflector_position_i_m; + + return normal_direction_i; + } + inline double GetReflectableAngle_rad() const { return reflectable_angle_rad_; } + + protected: + libra::Vector<3> normal_direction_c_{0.0}; //!< Reflection surface normal direction vector @ component frame + double reflectable_angle_rad_; //!< Reflectable half angle from the normal direction [rad] + libra::TranslationFirstDualQuaternion dual_quaternion_c2b_; //!< Dual quaternion from body to component frame + + libra::Vector<3> reflector_position_i_m_{0.0}; //!< Position of reflector @ inertia frame [m] + libra::Vector<3> normal_direction_i_{0.0}; + + // Reference + const Dynamics* dynamics_; +}; + +#endif // S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_ From 70c0629ef0a0d1333132db77252258698d5c295e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 14:44:44 +0100 Subject: [PATCH 02/14] Add laser distance meter --- .../components/aocs/laser_distance_meter.cpp | 94 +++++++++++++++++++ .../components/aocs/laser_distance_meter.hpp | 78 +++++++++++++++ 2 files changed, 172 insertions(+) create mode 100644 s2e-ff/src/components/aocs/laser_distance_meter.cpp create mode 100644 s2e-ff/src/components/aocs/laser_distance_meter.hpp diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.cpp b/s2e-ff/src/components/aocs/laser_distance_meter.cpp new file mode 100644 index 00000000..2caefb7f --- /dev/null +++ b/s2e-ff/src/components/aocs/laser_distance_meter.cpp @@ -0,0 +1,94 @@ +/** + * @file laser_distance_meter.hpp + * @brief Laser distance meter + */ + +#include "laser_distance_meter.hpp" + +LaserDistanceMeter::LaserDistanceMeter(const int prescaler, ClockGenerator* clock_gen, const Dynamics& dynamics, + const FfInterSpacecraftCommunication& inter_spacecraft_communication) + : Component(prescaler, clock_gen), dynamics_(dynamics), inter_spacecraft_communication_(inter_spacecraft_communication) { + laser_emitting_direction_c_[0] = -1.0; + laser_emitting_direction_c_[1] = 0.0; + laser_emitting_direction_c_[2] = 0.0; + emission_angle_rad_ = 0.1; + + libra::Quaternion q_b2c(0.0, 0.0, 0.0, 1.0); + libra::Vector<3> position_b2c_m; + position_b2c_m[0] = -0.5; + position_b2c_m[1] = 0.0; + position_b2c_m[2] = 0.0; + dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b2c_m, q_b2c); + + laser_emission_position_b_m_ = dual_quaternion_c2b_.TransformVector(libra::Vector<3>{0.0}); + laser_emitting_direction_b_ = dual_quaternion_c2b_.TransformVector(laser_emitting_direction_c_); + laser_emitting_direction_b_ -= laser_emission_position_b_m_; +} + +void LaserDistanceMeter::MainRoutine(int count) { + if (count < 10) return; + + // Body -> Inertial frame + libra::Vector<3> spacecraft_position_i2b_m = dynamics_.GetOrbit().GetPosition_i_m(); + libra::Quaternion spacecraft_attitude_i2b = dynamics_.GetAttitude().GetQuaternion_i2b().Conjugate(); + libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b); + + // Component -> Inertial frame + libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_; + + // Get reflector information + libra::Vector<3> reflector_position_i_m = inter_spacecraft_communication_.GetCornerCubeReflector().GetReflectorPosition_i_m(); + libra::Vector<3> reflector_normal_direction_i = inter_spacecraft_communication_.GetCornerCubeReflector().GetNormalDirection_i(); + + // Conversion + libra::Vector<3> reflector_position_c_m = dual_quaternion_c2i.InverseTransformVector(reflector_position_i_m); + libra::Vector<3> reflector_normal_direction_c = dual_quaternion_c2i.GetRotationQuaternion().FrameConversion(reflector_normal_direction_i); // ? + + // Calc relative distance + observed_distance_m_ = reflector_position_c_m.CalcNorm(); + + // Check reflection + // Is the reflector in the laser radius? + double laser_radius_m = observed_distance_m_ * tan(emission_angle_rad_); + double closest_distance_m = CalcDistanceBwPointAndLine(reflector_position_c_m, libra::Vector<3>{0.0}, laser_emitting_direction_c_); + if (closest_distance_m > laser_radius_m) { + is_reflected_ = false; + return; + } + // Is the laser is reflected? + double reflectable_angle_rad = inter_spacecraft_communication_.GetCornerCubeReflector().GetReflectableAngle_rad(); + double laser_incident_angle_rad = acos(libra::InnerProduct(reflector_normal_direction_c, -laser_emitting_direction_c_)); + if (laser_incident_angle_rad > reflectable_angle_rad) { + is_reflected_ = false; + return; + } + is_reflected_ = true; + + // Add noise +} + +std::string LaserDistanceMeter::GetLogHeader() const { + std::string str_tmp = ""; + std::string head = "laser_distance_meter_"; + str_tmp += WriteScalar(head + "is_reflected"); + str_tmp += WriteScalar(head + "observed_distance[m]"); + + return str_tmp; +} + +std::string LaserDistanceMeter::GetLogValue() const { + std::string str_tmp = ""; + + str_tmp += WriteScalar(is_reflected_); + str_tmp += WriteScalar(observed_distance_m_); + + return str_tmp; +} + +double LaserDistanceMeter::CalcDistanceBwPointAndLine(libra::Vector<3> point_position, libra::Vector<3> position_on_line, + libra::Vector<3> line_direction) { + libra::Vector<3> q_p = point_position - position_on_line; + double temp = libra::InnerProduct(q_p, line_direction) / pow(line_direction.CalcNorm(), 2.0); + libra::Vector<3> position = q_p - temp * line_direction; + return position.CalcNorm(); +} diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.hpp b/s2e-ff/src/components/aocs/laser_distance_meter.hpp new file mode 100644 index 00000000..b914b6c4 --- /dev/null +++ b/s2e-ff/src/components/aocs/laser_distance_meter.hpp @@ -0,0 +1,78 @@ +/** + * @file laser_distance_meter.hpp + * @brief Laser distance meter + */ + +#ifndef S2E_COMPONENTS_LASER_DISTANCE_METER_HPP_ +#define S2E_COMPONENTS_LASER_DISTANCE_METER_HPP_ + +#include +#include +#include +#include + +#include "../../library/math/translation_first_dual_quaternion.hpp" +#include "../../simulation/case/ff_inter_spacecraft_communication.hpp" + +/** + * @class LaserDistanceMeter + * @brief Relative distance sensor + */ +class LaserDistanceMeter : public Component, public ILoggable { + public: + /** + * @fn LaserDistanceMeter + * @brief Constructor + */ + LaserDistanceMeter(const int prescaler, ClockGenerator* clock_gen, const Dynamics& dynamics, + const FfInterSpacecraftCommunication& inter_spacecraft_communication); + /** + * @fn ~LaserDistanceMeter + * @brief Destructor + */ + ~LaserDistanceMeter() {} + + // ComponentBase override function + /** + * @fn MainRoutine + * @brief Main routine + */ + void MainRoutine(int count); + + // Override ILoggable + /** + * @fn GetLogHeader + * @brief Override GetLogHeader function of ILoggable + */ + virtual std::string GetLogHeader() const; + /** + * @fn GetLogValue + * @brief Override GetLogValue function of ILoggable + */ + virtual std::string GetLogValue() const; + + inline libra::Vector<3> GetLaserEmissionPosition_i_m() const { return laser_emission_position_i_m_; } + inline libra::Vector<3> GetLaserEmittingDirection_i() const { return laser_emitting_direction_i_; } + + protected: + libra::Vector<3> laser_emitting_direction_c_; //!< Laser emitting direction @ component frame + double emission_angle_rad_; //!< Emission half angle from the normal direction [rad] + libra::TranslationFirstDualQuaternion dual_quaternion_c2b_; //!< Dual quaternion from component to body frame + + libra::Vector<3> laser_emission_position_b_m_{0.0}; //!< Position of laser emission point @ inertia frame [m] + libra::Vector<3> laser_emitting_direction_b_{0.0}; + + libra::Vector<3> laser_emission_position_i_m_{0.0}; //!< Position of laser emission point @ inertia frame [m] + libra::Vector<3> laser_emitting_direction_i_{0.0}; + + bool is_reflected_ = false; //!< Flag to detect reflected light + double observed_distance_m_ = 0.0; //!< Observed direction + + // Reference + const Dynamics& dynamics_; + const FfInterSpacecraftCommunication& inter_spacecraft_communication_; + + double CalcDistanceBwPointAndLine(libra::Vector<3> point_position, libra::Vector<3> line_start_position, libra::Vector<3> line_direction); +}; + +#endif // S2E_COMPONENTS_LASER_DISTANCE_METER_HPP_ From e4eef4b255bedd6b32ae28bc2e767a954938aff9 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 15:01:34 +0100 Subject: [PATCH 03/14] Fix small --- s2e-ff/src/components/aocs/corner_cube_reflector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp index 1843f321..38a94d0e 100644 --- a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp +++ b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp @@ -56,7 +56,7 @@ class CornerCubeReflector : public Component, public ILoggable { inline libra::Vector<3> GetReflectorPosition_i_m() const { libra::Vector<3> spacecraft_position_i2b_m = dynamics_->GetOrbit().GetPosition_i_m(); - libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b().Conjugate(); libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b); libra::Vector<3> position_i_m = dual_quaternion_i2b.InverseTransformVector(libra::Vector<3>{0.0}); From 11aac9d353e43ae07653619429981de048b3dad8 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 16:33:31 +0100 Subject: [PATCH 04/14] Implement relative sensor communication --- s2e-ff/CMakeLists.txt | 2 ++ s2e-ff/data/initialize_files/ff_satellite.ini | 12 +++---- .../data/initialize_files/ff_satellite_2.ini | 17 +++++---- .../components/aocs/corner_cube_reflector.cpp | 8 ++--- .../components/aocs/laser_distance_meter.cpp | 10 +++--- s2e-ff/src/library/math/dual_quaternion.hpp | 2 +- s2e-ff/src/simulation/case/ff_case.cpp | 2 +- s2e-ff/src/simulation/case/ff_case.hpp | 1 + .../ff_inter_spacecraft_communication.hpp | 36 +++++++++++++++++++ .../simulation/spacecraft/ff_components.cpp | 15 ++++++-- .../simulation/spacecraft/ff_components.hpp | 8 ++++- .../simulation/spacecraft/ff_components_2.cpp | 14 ++++++-- .../simulation/spacecraft/ff_components_2.hpp | 8 ++++- .../simulation/spacecraft/ff_satellite.cpp | 11 +++--- .../simulation/spacecraft/ff_satellite.hpp | 6 ++-- 15 files changed, 114 insertions(+), 38 deletions(-) create mode 100644 s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 96215584..73ffadf0 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -51,6 +51,8 @@ set(SOURCE_FILES src/components/aocs/initialize_relative_position_sensor.cpp src/components/aocs/relative_velocity_sensor.cpp src/components/aocs/initialize_relative_velocity_sensor.cpp + src/components/aocs/laser_distance_meter.cpp + src/components/aocs/corner_cube_reflector.cpp src/components/ideal/relative_attitude_controller.cpp src/components/ideal/initialize_relative_attitude_controller.cpp src/library/math/dual_quaternion.cpp diff --git a/s2e-ff/data/initialize_files/ff_satellite.ini b/s2e-ff/data/initialize_files/ff_satellite.ini index f5e84376..91e802c3 100644 --- a/s2e-ff/data/initialize_files/ff_satellite.ini +++ b/s2e-ff/data/initialize_files/ff_satellite.ini @@ -34,19 +34,19 @@ initial_torque_b_Nm(2) = 0.000 // EARTH_CENTER_POINTING // VELOCITY_DIRECTION_POINTING // ORBIT_NORMAL_POINTING -main_mode = INERTIAL_STABILIZE -sub_mode = SUN_POINTING +main_mode = VELOCITY_DIRECTION_POINTING +sub_mode = ORBIT_NORMAL_POINTING // Pointing direction @ body frame for main pointing mode -main_pointing_direction_b(0) = 0.707 -main_pointing_direction_b(1) = 0.707 +main_pointing_direction_b(0) = 1.0 +main_pointing_direction_b(1) = 0.0 main_pointing_direction_b(2) = 0.0 // Pointing direction @ body frame for sub pointing mode // main_pointing_direction_b and sub_pointing_direction_b should separate larger than 30 degrees. sub_pointing_direction_b(0) = 0.0 -sub_pointing_direction_b(1) = 0.0 -sub_pointing_direction_b(2) = 1.0 +sub_pointing_direction_b(1) = 1.0 +sub_pointing_direction_b(2) = 0.0 [ORBIT] calculation = ENABLE diff --git a/s2e-ff/data/initialize_files/ff_satellite_2.ini b/s2e-ff/data/initialize_files/ff_satellite_2.ini index 27f4b335..02fe3112 100644 --- a/s2e-ff/data/initialize_files/ff_satellite_2.ini +++ b/s2e-ff/data/initialize_files/ff_satellite_2.ini @@ -2,7 +2,7 @@ // Attitude propagation mode // RK4 : Attitude Propagation with RK4 including disturbances and control torque // CONTROLLED : Attitude Calculation with Controlled Attitude mode. All disturbances and control torque are ignored. -propagate_mode = RK4 +propagate_mode = CONTROLLED // Initialize Attitude mode // MANUAL : Initialize Quaternion_i2b manually below @@ -34,19 +34,19 @@ initial_torque_b_Nm(2) = 0.000 // EARTH_CENTER_POINTING // VELOCITY_DIRECTION_POINTING // ORBIT_NORMAL_POINTING -main_mode = INERTIAL_STABILIZE -sub_mode = SUN_POINTING +main_mode = VELOCITY_DIRECTION_POINTING +sub_mode = ORBIT_NORMAL_POINTING // Pointing direction @ body frame for main pointing mode -main_pointing_direction_b(0) = 0.707 -main_pointing_direction_b(1) = 0.707 +main_pointing_direction_b(0) = 1.0 +main_pointing_direction_b(1) = 0.0 main_pointing_direction_b(2) = 0.0 // Pointing direction @ body frame for sub pointing mode // main_pointing_direction_b and sub_pointing_direction_b should separate larger than 30 degrees. sub_pointing_direction_b(0) = 0.0 -sub_pointing_direction_b(1) = 0.0 -sub_pointing_direction_b(2) = 1.0 +sub_pointing_direction_b(1) = 1.0 +sub_pointing_direction_b(2) = 0.0 [ORBIT] calculation = ENABLE @@ -105,9 +105,8 @@ relative_dynamics_model_type = 0 // 0: HCW stm_model_type = 0 // Initial satellite position relative to the reference satellite in LVLH frame[m] -// * The coordinate system is defined at [PLANET_SELECTION] in SampleSimBase.ini initial_relative_position_lvlh_m(0) = 0.0 -initial_relative_position_lvlh_m(1) = 100.0 +initial_relative_position_lvlh_m(1) = -10.0 initial_relative_position_lvlh_m(2) = 0.0 // initial satellite velocity relative to the reference satellite in LVLH frame[m/s] initial_relative_velocity_lvlh_m_s(0) = 0.0 diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.cpp b/s2e-ff/src/components/aocs/corner_cube_reflector.cpp index 6e17d545..6086f24e 100644 --- a/s2e-ff/src/components/aocs/corner_cube_reflector.cpp +++ b/s2e-ff/src/components/aocs/corner_cube_reflector.cpp @@ -7,17 +7,17 @@ CornerCubeReflector::CornerCubeReflector(const int prescaler, ClockGenerator* clock_gen, const Dynamics* dynamics) : Component(prescaler, clock_gen), dynamics_(dynamics) { - normal_direction_c_[0] = 1.0; + normal_direction_c_[0] = 0.0; normal_direction_c_[1] = 0.0; - normal_direction_c_[2] = 0.0; + normal_direction_c_[2] = 1.0; reflectable_angle_rad_ = 0.1; - libra::Quaternion q_rot(0.0, 0.0, 0.0, 1.0); + libra::Quaternion q_rot(0.0, 1.0, 0.0, 1.0); libra::Vector<3> v_translation_m; v_translation_m[0] = 0.5; v_translation_m[1] = 0.0; v_translation_m[2] = 0.0; - dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-v_translation_m, q_rot); + dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-v_translation_m, q_rot.Conjugate()).QuaternionConjugate(); } void CornerCubeReflector::MainRoutine(int count) { diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.cpp b/s2e-ff/src/components/aocs/laser_distance_meter.cpp index 2caefb7f..91be5eeb 100644 --- a/s2e-ff/src/components/aocs/laser_distance_meter.cpp +++ b/s2e-ff/src/components/aocs/laser_distance_meter.cpp @@ -8,17 +8,17 @@ LaserDistanceMeter::LaserDistanceMeter(const int prescaler, ClockGenerator* clock_gen, const Dynamics& dynamics, const FfInterSpacecraftCommunication& inter_spacecraft_communication) : Component(prescaler, clock_gen), dynamics_(dynamics), inter_spacecraft_communication_(inter_spacecraft_communication) { - laser_emitting_direction_c_[0] = -1.0; - laser_emitting_direction_c_[1] = 0.0; + laser_emitting_direction_c_[0] = 0.0; + laser_emitting_direction_c_[1] = 1.0; laser_emitting_direction_c_[2] = 0.0; emission_angle_rad_ = 0.1; - libra::Quaternion q_b2c(0.0, 0.0, 0.0, 1.0); + libra::Quaternion q_b2c(0.0, 0.0, 1.0, 1.0); libra::Vector<3> position_b2c_m; position_b2c_m[0] = -0.5; position_b2c_m[1] = 0.0; position_b2c_m[2] = 0.0; - dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b2c_m, q_b2c); + dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b2c_m, q_b2c.Conjugate()).QuaternionConjugate(); laser_emission_position_b_m_ = dual_quaternion_c2b_.TransformVector(libra::Vector<3>{0.0}); laser_emitting_direction_b_ = dual_quaternion_c2b_.TransformVector(laser_emitting_direction_c_); @@ -42,7 +42,7 @@ void LaserDistanceMeter::MainRoutine(int count) { // Conversion libra::Vector<3> reflector_position_c_m = dual_quaternion_c2i.InverseTransformVector(reflector_position_i_m); - libra::Vector<3> reflector_normal_direction_c = dual_quaternion_c2i.GetRotationQuaternion().FrameConversion(reflector_normal_direction_i); // ? + libra::Vector<3> reflector_normal_direction_c = dual_quaternion_c2i.GetRotationQuaternion().InverseFrameConversion(reflector_normal_direction_i); // Calc relative distance observed_distance_m_ = reflector_position_c_m.CalcNorm(); diff --git a/s2e-ff/src/library/math/dual_quaternion.hpp b/s2e-ff/src/library/math/dual_quaternion.hpp index 1a8542b7..95cfc1ef 100644 --- a/s2e-ff/src/library/math/dual_quaternion.hpp +++ b/s2e-ff/src/library/math/dual_quaternion.hpp @@ -107,7 +107,7 @@ class DualQuaternion { // Getter inline Quaternion GetRealPart() const { return q_real_; } inline Quaternion GetDualPart() const { return q_dual_; } - inline Quaternion GetRotationQuaternion() const { return q_real_; } + inline Quaternion GetRotationQuaternion() const { return q_real_.Conjugate(); } // Quaternion is used as frame conversion not vector conversion protected: Quaternion q_real_; //!< Real part Quaternion diff --git a/s2e-ff/src/simulation/case/ff_case.cpp b/s2e-ff/src/simulation/case/ff_case.cpp index 4fbe8420..3e4443c4 100644 --- a/s2e-ff/src/simulation/case/ff_case.cpp +++ b/s2e-ff/src/simulation/case/ff_case.cpp @@ -16,7 +16,7 @@ FfCase::~FfCase() { void FfCase::InitializeTargetObjects() { // Instantiate the target of the simulation for (unsigned int sat_id = 0; sat_id < simulation_configuration_.number_of_simulated_spacecraft_; sat_id++) { - FfSat* sc = new FfSat(&simulation_configuration_, global_environment_, &relative_information_, sat_id); + FfSat* sc = new FfSat(&simulation_configuration_, global_environment_, &relative_information_, ff_isc_, sat_id); satellites_.push_back(sc); } // Register the log output diff --git a/s2e-ff/src/simulation/case/ff_case.hpp b/s2e-ff/src/simulation/case/ff_case.hpp index d814c1b4..b1b269d2 100644 --- a/s2e-ff/src/simulation/case/ff_case.hpp +++ b/s2e-ff/src/simulation/case/ff_case.hpp @@ -26,6 +26,7 @@ class FfCase : public SimulationCase { private: std::vector satellites_; RelativeInformation relative_information_; + FfInterSpacecraftCommunication ff_isc_; /** * @fn InitializeTargetObjects diff --git a/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp b/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp new file mode 100644 index 00000000..0d6029bf --- /dev/null +++ b/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp @@ -0,0 +1,36 @@ +/** + * @file ff_inter_spacecraft_communication.h + * @brief Base class of inter satellite communication + */ + +#ifndef S2E_SIMULATION_FF_SPACECRAFT_INTER_SPACECRAFT_COMMUNICATION_HPP_ +#define S2E_SIMULATION_FF_SPACECRAFT_INTER_SPACECRAFT_COMMUNICATION_HPP_ + +// #include +#include "../../components/aocs/corner_cube_reflector.hpp" + +/** + * @class InterSpacecraftCommunication + * @brief Base class of inter satellite communication + */ +class FfInterSpacecraftCommunication { + public: + /** + * @fn InterSpacecraftCommunication + * @brief Constructor + */ + FfInterSpacecraftCommunication() {} + /** + * @fn ~InterSpacecraftCommunication + * @brief Destructor + */ + ~FfInterSpacecraftCommunication() {} + + inline void SetCornerCubeReflector(CornerCubeReflector* corner_cube_reflector) { corner_cube_reflector_ = corner_cube_reflector; } + inline CornerCubeReflector& GetCornerCubeReflector() const { return *corner_cube_reflector_; } + + private: + CornerCubeReflector* corner_cube_reflector_; +}; + +#endif // S2E_SIMULATION_FF_SPACECRAFT_INTER_SPACECRAFT_COMMUNICATION_HPP_ diff --git a/s2e-ff/src/simulation/spacecraft/ff_components.cpp b/s2e-ff/src/simulation/spacecraft/ff_components.cpp index d495a0f0..a0591ebd 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components.cpp @@ -4,8 +4,15 @@ #include FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, const LocalEnvironment* local_env, const GlobalEnvironment* glo_env, - const SimulationConfiguration* config, ClockGenerator* clock_gen, const RelativeInformation* rel_info) - : dynamics_(dynamics), structure_(structure), local_env_(local_env), glo_env_(glo_env), config_(config), rel_info_(rel_info) { + const SimulationConfiguration* config, ClockGenerator* clock_gen, const RelativeInformation* rel_info, + FfInterSpacecraftCommunication& inter_spacecraft_communication) + : dynamics_(dynamics), + structure_(structure), + local_env_(local_env), + glo_env_(glo_env), + config_(config), + rel_info_(rel_info), + inter_spacecraft_communication_(inter_spacecraft_communication) { // General const int sat_id = 0; IniAccess sat_file = IniAccess(config->spacecraft_file_list_[sat_id]); @@ -27,6 +34,8 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, relative_velocity_sensor_ = new RelativeVelocitySensor(InitializeRelativeVelocitySensor(clock_gen, rel_vel_file, compo_step_sec, *rel_info_, *dynamics_, sat_id)); + laser_distance_meter_ = new LaserDistanceMeter(1, clock_gen, *dynamics_, inter_spacecraft_communication_); + const std::string force_generator_file = sat_file.ReadString(section_name.c_str(), "force_generator_file"); force_generator_ = new ForceGenerator(InitializeForceGenerator(clock_gen, force_generator_file, dynamics_)); @@ -48,6 +57,7 @@ FfComponents::~FfComponents() { delete relative_velocity_sensor_; delete force_generator_; delete relative_attitude_controller_; + delete laser_distance_meter_; // OBC must be deleted the last since it has com ports delete obc_; } @@ -69,4 +79,5 @@ void FfComponents::LogSetup(Logger& logger) { logger.AddLogList(relative_position_sensor_); logger.AddLogList(relative_velocity_sensor_); logger.AddLogList(force_generator_); + logger.AddLogList(laser_distance_meter_); } diff --git a/s2e-ff/src/simulation/spacecraft/ff_components.hpp b/s2e-ff/src/simulation/spacecraft/ff_components.hpp index f3204f82..3dd499ef 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components.hpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components.hpp @@ -10,6 +10,8 @@ #include #include +#include "../case/ff_inter_spacecraft_communication.hpp" + // include for components #include #include @@ -17,6 +19,7 @@ #include "../../components/aocs/initialize_relative_distance_sensor.hpp" #include "../../components/aocs/initialize_relative_position_sensor.hpp" #include "../../components/aocs/initialize_relative_velocity_sensor.hpp" +#include "../../components/aocs/laser_distance_meter.hpp" #include "../../components/ideal/initialize_relative_attitude_controller.hpp" /** @@ -30,7 +33,8 @@ class FfComponents : public InstalledComponents { * @brief Constructor */ FfComponents(const Dynamics* dynamics, const Structure* structure, const LocalEnvironment* local_env, const GlobalEnvironment* glo_env, - const SimulationConfiguration* config, ClockGenerator* clock_gen, const RelativeInformation* rel_info); + const SimulationConfiguration* config, ClockGenerator* clock_gen, const RelativeInformation* rel_info, + FfInterSpacecraftCommunication& inter_spacecraft_communication); /** * @fn ~FfComponents * @brief Destructor @@ -62,6 +66,7 @@ class FfComponents : public InstalledComponents { RelativeDistanceSensor* relative_distance_sensor_; //!< Example of Relative distance sensor RelativePositionSensor* relative_position_sensor_; //!< Example of Relative position sensor RelativeVelocitySensor* relative_velocity_sensor_; //!< Example of Relative velocity sensor + LaserDistanceMeter* laser_distance_meter_; // Actuators ForceGenerator* force_generator_; //!< Example of force generator RelativeAttitudeController* relative_attitude_controller_; //!< Example of attitude controller @@ -73,6 +78,7 @@ class FfComponents : public InstalledComponents { const GlobalEnvironment* glo_env_; //!< Global environment information const SimulationConfiguration* config_; //!< Simulation settings const RelativeInformation* rel_info_; //!< Relative information + FfInterSpacecraftCommunication& inter_spacecraft_communication_; }; #endif // S2E_FF_SIMULATION_SPACECRAFT_FF_COMPONENTS_HPP_ diff --git a/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp b/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp index c3ecdd3d..9b4d12c2 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp @@ -5,14 +5,23 @@ FfComponents2::FfComponents2(const Dynamics* dynamics, const Structure* structure, const LocalEnvironment* local_env, const GlobalEnvironment* glo_env, const SimulationConfiguration* config, ClockGenerator* clock_gen, - const RelativeInformation* rel_info) - : dynamics_(dynamics), structure_(structure), local_env_(local_env), glo_env_(glo_env), config_(config), rel_info_(rel_info) { + const RelativeInformation* rel_info, FfInterSpacecraftCommunication& inter_spacecraft_communication) + : dynamics_(dynamics), + structure_(structure), + local_env_(local_env), + glo_env_(glo_env), + config_(config), + rel_info_(rel_info), + inter_spacecraft_communication_(inter_spacecraft_communication) { // General IniAccess sat_file = IniAccess(config->spacecraft_file_list_[0]); // Component Instantiation obc_ = new OnBoardComputer(clock_gen); + corner_cube_reflector_ = new CornerCubeReflector(1, clock_gen, dynamics_); + inter_spacecraft_communication.SetCornerCubeReflector(corner_cube_reflector_); + // Debug for actuator output libra::Vector<3> force_N; force_N[0] = 1.0; @@ -22,6 +31,7 @@ FfComponents2::FfComponents2(const Dynamics* dynamics, const Structure* structur } FfComponents2::~FfComponents2() { + delete corner_cube_reflector_; // OBC must be deleted the last since it has com ports delete obc_; } diff --git a/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp b/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp index 774d7462..e1e3bba1 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp @@ -10,10 +10,13 @@ #include #include +#include "../case/ff_inter_spacecraft_communication.hpp" + // include for components #include #include +#include "../../components/aocs/corner_cube_reflector.hpp" #include "../../components/aocs/initialize_relative_distance_sensor.hpp" #include "../../components/aocs/initialize_relative_position_sensor.hpp" #include "../../components/ideal/initialize_relative_attitude_controller.hpp" @@ -29,7 +32,8 @@ class FfComponents2 : public InstalledComponents { * @brief Constructor */ FfComponents2(const Dynamics* dynamics, const Structure* structure, const LocalEnvironment* local_env, const GlobalEnvironment* glo_env, - const SimulationConfiguration* config, ClockGenerator* clock_gen, const RelativeInformation* rel_info); + const SimulationConfiguration* config, ClockGenerator* clock_gen, const RelativeInformation* rel_info, + FfInterSpacecraftCommunication& inter_spacecraft_communication); /** * @fn ~FfComponents * @brief Destructor @@ -56,6 +60,7 @@ class FfComponents2 : public InstalledComponents { private: // Components OnBoardComputer* obc_; //!< On board computer + CornerCubeReflector* corner_cube_reflector_; // References const Dynamics* dynamics_; //!< Dynamics information of the spacecraft @@ -64,6 +69,7 @@ class FfComponents2 : public InstalledComponents { const GlobalEnvironment* glo_env_; //!< Global environment information const SimulationConfiguration* config_; //!< Simulation settings const RelativeInformation* rel_info_; //!< Relative information + FfInterSpacecraftCommunication& inter_spacecraft_communication_; }; #endif // S2E_FF_SIMULATION_SPACECRAFT_FF_COMPONENTS_2_HPP_ diff --git a/s2e-ff/src/simulation/spacecraft/ff_satellite.cpp b/s2e-ff/src/simulation/spacecraft/ff_satellite.cpp index 3f058f57..6c6615ec 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_satellite.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_satellite.cpp @@ -1,11 +1,14 @@ #include "ff_satellite.hpp" -FfSat::FfSat(SimulationConfiguration* sim_config, const GlobalEnvironment* glo_env, RelativeInformation* relative_information, const int sat_id) - : Spacecraft(sim_config, glo_env, sat_id, relative_information) { +FfSat::FfSat(SimulationConfiguration* sim_config, const GlobalEnvironment* glo_env, RelativeInformation* relative_information, + FfInterSpacecraftCommunication& inter_spacecraft_communication, const int sat_id) + : Spacecraft(sim_config, glo_env, sat_id, relative_information), inter_spacecraft_communication_(inter_spacecraft_communication) { if (sat_id == 0) { - components_ = new FfComponents(dynamics_, structure_, local_environment_, glo_env, sim_config, &clock_generator_, relative_information); + components_ = new FfComponents(dynamics_, structure_, local_environment_, glo_env, sim_config, &clock_generator_, relative_information, + inter_spacecraft_communication_); } else if (sat_id == 1) { - components_ = new FfComponents2(dynamics_, structure_, local_environment_, glo_env, sim_config, &clock_generator_, relative_information); + components_ = new FfComponents2(dynamics_, structure_, local_environment_, glo_env, sim_config, &clock_generator_, relative_information, + inter_spacecraft_communication_); } else { // not reach } } diff --git a/s2e-ff/src/simulation/spacecraft/ff_satellite.hpp b/s2e-ff/src/simulation/spacecraft/ff_satellite.hpp index 1d3b6c13..ff4ee088 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_satellite.hpp +++ b/s2e-ff/src/simulation/spacecraft/ff_satellite.hpp @@ -8,9 +8,9 @@ #include +#include "../case/ff_inter_spacecraft_communication.hpp" #include "ff_components.hpp" #include "ff_components_2.hpp" - /** * @class FfSat * @brief A spacecraft class @@ -21,9 +21,11 @@ class FfSat : public Spacecraft { * @fn FfSat * @brief Constructor */ - FfSat(SimulationConfiguration *sim_config, const GlobalEnvironment *glo_env, RelativeInformation *relative_information, const int sat_id); + FfSat(SimulationConfiguration *sim_config, const GlobalEnvironment *glo_env, RelativeInformation *relative_information, + FfInterSpacecraftCommunication &inter_spacecraft_communication, const int sat_id); private: + FfInterSpacecraftCommunication &inter_spacecraft_communication_; }; #endif // S2E_FF_SIMULATION_SPACECRAFT_FF_SATELLITE_HPP_ From f868140fe2952cfe12f482f5551ed0c12aa97554 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 16:38:03 +0100 Subject: [PATCH 05/14] Fix small --- .../components/aocs/corner_cube_reflector.cpp | 30 +++++-------------- .../components/aocs/corner_cube_reflector.hpp | 15 ++++------ .../components/aocs/laser_distance_meter.cpp | 6 ++-- 3 files changed, 16 insertions(+), 35 deletions(-) diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.cpp b/s2e-ff/src/components/aocs/corner_cube_reflector.cpp index 6086f24e..bd5107ab 100644 --- a/s2e-ff/src/components/aocs/corner_cube_reflector.cpp +++ b/s2e-ff/src/components/aocs/corner_cube_reflector.cpp @@ -12,31 +12,15 @@ CornerCubeReflector::CornerCubeReflector(const int prescaler, ClockGenerator* cl normal_direction_c_[2] = 1.0; reflectable_angle_rad_ = 0.1; - libra::Quaternion q_rot(0.0, 1.0, 0.0, 1.0); - libra::Vector<3> v_translation_m; - v_translation_m[0] = 0.5; - v_translation_m[1] = 0.0; - v_translation_m[2] = 0.0; - dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-v_translation_m, q_rot.Conjugate()).QuaternionConjugate(); + libra::Quaternion q_b2c(0.0, 1.0, 0.0, 1.0); + libra::Vector<3> position_b2c_m; + position_b2c_m[0] = 0.5; + position_b2c_m[1] = 0.0; + position_b2c_m[2] = 0.0; + dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b2c_m, q_b2c.Conjugate()).QuaternionConjugate(); } -void CornerCubeReflector::MainRoutine(int count) { - if (count < 10) return; - - // Body -> Inertial frame - libra::Vector<3> spacecraft_position_i2b_m = dynamics_->GetOrbit().GetPosition_i_m(); - libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b().Conjugate(); - libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b); - - libra::Vector<3> position_i_m = dual_quaternion_i2b.InverseTransformVector(libra::Vector<3>{0.0}); - - // Component -> Inertial frame - libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_; - - reflector_position_i_m_ = dual_quaternion_c2i.TransformVector(libra::Vector<3>{0.0}); - normal_direction_i_ = dual_quaternion_c2i.TransformVector(normal_direction_c_); - normal_direction_i_ -= reflector_position_i_m_; -} +void CornerCubeReflector::MainRoutine(int count) { UNUSED(count); } std::string CornerCubeReflector::GetLogHeader() const { std::string str_tmp = ""; diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp index 38a94d0e..4bbc36e9 100644 --- a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp +++ b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp @@ -56,24 +56,20 @@ class CornerCubeReflector : public Component, public ILoggable { inline libra::Vector<3> GetReflectorPosition_i_m() const { libra::Vector<3> spacecraft_position_i2b_m = dynamics_->GetOrbit().GetPosition_i_m(); - libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b().Conjugate(); - libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b); - - libra::Vector<3> position_i_m = dual_quaternion_i2b.InverseTransformVector(libra::Vector<3>{0.0}); + libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b.Conjugate()); // Component -> Inertial frame libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_; return dual_quaternion_c2i.TransformVector(libra::Vector<3>{0.0}); - // return reflector_position_i_m_; } + inline libra::Vector<3> GetNormalDirection_i() const { // Body -> Inertial frame libra::Vector<3> spacecraft_position_i2b_m = dynamics_->GetOrbit().GetPosition_i_m(); - libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b().Conjugate(); - libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b); - - libra::Vector<3> position_i_m = dual_quaternion_i2b.InverseTransformVector(libra::Vector<3>{0.0}); + libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b.Conjugate()); // Component -> Inertial frame libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_; @@ -84,6 +80,7 @@ class CornerCubeReflector : public Component, public ILoggable { return normal_direction_i; } + inline double GetReflectableAngle_rad() const { return reflectable_angle_rad_; } protected: diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.cpp b/s2e-ff/src/components/aocs/laser_distance_meter.cpp index 91be5eeb..2b68f782 100644 --- a/s2e-ff/src/components/aocs/laser_distance_meter.cpp +++ b/s2e-ff/src/components/aocs/laser_distance_meter.cpp @@ -26,12 +26,12 @@ LaserDistanceMeter::LaserDistanceMeter(const int prescaler, ClockGenerator* cloc } void LaserDistanceMeter::MainRoutine(int count) { - if (count < 10) return; + UNUSED(count); // Body -> Inertial frame libra::Vector<3> spacecraft_position_i2b_m = dynamics_.GetOrbit().GetPosition_i_m(); - libra::Quaternion spacecraft_attitude_i2b = dynamics_.GetAttitude().GetQuaternion_i2b().Conjugate(); - libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b); + libra::Quaternion spacecraft_attitude_i2b = dynamics_.GetAttitude().GetQuaternion_i2b(); + libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b.Conjugate()); // Component -> Inertial frame libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_; From 674050d64f78255773b37c37e10da2f5a28082d7 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 20:57:34 +0100 Subject: [PATCH 06/14] Remove c source file --- s2e-ff/CMakeLists.txt | 1 - .../components/aocs/corner_cube_reflector.cpp | 38 ----------------- .../components/aocs/corner_cube_reflector.hpp | 42 +++++++------------ .../simulation/spacecraft/ff_components_2.cpp | 2 +- 4 files changed, 16 insertions(+), 67 deletions(-) delete mode 100644 s2e-ff/src/components/aocs/corner_cube_reflector.cpp diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 73ffadf0..930a95fb 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -52,7 +52,6 @@ set(SOURCE_FILES src/components/aocs/relative_velocity_sensor.cpp src/components/aocs/initialize_relative_velocity_sensor.cpp src/components/aocs/laser_distance_meter.cpp - src/components/aocs/corner_cube_reflector.cpp src/components/ideal/relative_attitude_controller.cpp src/components/ideal/initialize_relative_attitude_controller.cpp src/library/math/dual_quaternion.cpp diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.cpp b/s2e-ff/src/components/aocs/corner_cube_reflector.cpp deleted file mode 100644 index bd5107ab..00000000 --- a/s2e-ff/src/components/aocs/corner_cube_reflector.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/** - * @file corner_cube_reflector.cpp - * @brief Corner cube reflector - */ - -#include "corner_cube_reflector.hpp" - -CornerCubeReflector::CornerCubeReflector(const int prescaler, ClockGenerator* clock_gen, const Dynamics* dynamics) - : Component(prescaler, clock_gen), dynamics_(dynamics) { - normal_direction_c_[0] = 0.0; - normal_direction_c_[1] = 0.0; - normal_direction_c_[2] = 1.0; - reflectable_angle_rad_ = 0.1; - - libra::Quaternion q_b2c(0.0, 1.0, 0.0, 1.0); - libra::Vector<3> position_b2c_m; - position_b2c_m[0] = 0.5; - position_b2c_m[1] = 0.0; - position_b2c_m[2] = 0.0; - dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b2c_m, q_b2c.Conjugate()).QuaternionConjugate(); -} - -void CornerCubeReflector::MainRoutine(int count) { UNUSED(count); } - -std::string CornerCubeReflector::GetLogHeader() const { - std::string str_tmp = ""; - str_tmp += WriteVector("normal_direction", "i", "", 3); - - return str_tmp; -} - -std::string CornerCubeReflector::GetLogValue() const { - std::string str_tmp = ""; - - str_tmp += WriteVector(normal_direction_i_); - - return str_tmp; -} diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp index 4bbc36e9..61d25d88 100644 --- a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp +++ b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp @@ -6,9 +6,7 @@ #ifndef S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_ #define S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_ -#include #include -#include #include #include "../../library/math/translation_first_dual_quaternion.hpp" @@ -17,43 +15,36 @@ * @class CornerCubeReflector * @brief Corner Cube Reflector */ -class CornerCubeReflector : public Component, public ILoggable { +class CornerCubeReflector { public: /** * @fn CornerCubeReflector * @brief Constructor */ - CornerCubeReflector() : Component(1, nullptr), dynamics_(nullptr) {} + CornerCubeReflector() : dynamics_(nullptr) {} /** * @fn CornerCubeReflector * @brief Constructor */ - CornerCubeReflector(const int prescaler, ClockGenerator* clock_gen, const Dynamics* dynamics); + CornerCubeReflector(const Dynamics* dynamics) { + normal_direction_c_[0] = 0.0; + normal_direction_c_[1] = 0.0; + normal_direction_c_[2] = 1.0; + reflectable_angle_rad_ = 0.1; + + libra::Quaternion q_b2c(0.0, 1.0, 0.0, 1.0); + libra::Vector<3> position_b2c_m; + position_b2c_m[0] = 0.5; + position_b2c_m[1] = 0.0; + position_b2c_m[2] = 0.0; + dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b2c_m, q_b2c.Conjugate()).QuaternionConjugate(); + } /** * @fn ~CornerCubeReflector * @brief Destructor */ ~CornerCubeReflector() {} - // ComponentBase override function - /** - * @fn MainRoutine - * @brief Main routine - */ - void MainRoutine(int count); - - // Override ILoggable - /** - * @fn GetLogHeader - * @brief Override GetLogHeader function of ILoggable - */ - virtual std::string GetLogHeader() const; - /** - * @fn GetLogValue - * @brief Override GetLogValue function of ILoggable - */ - virtual std::string GetLogValue() const; - inline libra::Vector<3> GetReflectorPosition_i_m() const { libra::Vector<3> spacecraft_position_i2b_m = dynamics_->GetOrbit().GetPosition_i_m(); libra::Quaternion spacecraft_attitude_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); @@ -88,9 +79,6 @@ class CornerCubeReflector : public Component, public ILoggable { double reflectable_angle_rad_; //!< Reflectable half angle from the normal direction [rad] libra::TranslationFirstDualQuaternion dual_quaternion_c2b_; //!< Dual quaternion from body to component frame - libra::Vector<3> reflector_position_i_m_{0.0}; //!< Position of reflector @ inertia frame [m] - libra::Vector<3> normal_direction_i_{0.0}; - // Reference const Dynamics* dynamics_; }; diff --git a/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp b/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp index 9b4d12c2..6b098e61 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp @@ -19,7 +19,7 @@ FfComponents2::FfComponents2(const Dynamics* dynamics, const Structure* structur // Component Instantiation obc_ = new OnBoardComputer(clock_gen); - corner_cube_reflector_ = new CornerCubeReflector(1, clock_gen, dynamics_); + corner_cube_reflector_ = new CornerCubeReflector(dynamics_); inter_spacecraft_communication.SetCornerCubeReflector(corner_cube_reflector_); // Debug for actuator output From 08de3befd8b2b55637f8466b845019be151dfd4a Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 20:58:33 +0100 Subject: [PATCH 07/14] Fix small --- s2e-ff/src/components/aocs/corner_cube_reflector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp index 61d25d88..b50ee3de 100644 --- a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp +++ b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp @@ -26,7 +26,7 @@ class CornerCubeReflector { * @fn CornerCubeReflector * @brief Constructor */ - CornerCubeReflector(const Dynamics* dynamics) { + CornerCubeReflector(const Dynamics* dynamics) : dynamics_(dynamics) { normal_direction_c_[0] = 0.0; normal_direction_c_[1] = 0.0; normal_direction_c_[2] = 1.0; From 9e4ba4c26260891889e430abbecdbd013c50d914 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 22:10:35 +0100 Subject: [PATCH 08/14] Add corner cube ini file --- .../components/corner_cube_reflector.ini | 15 +++++++++ .../components/laser_distance_meter.ini | 15 +++++++++ .../data/initialize_files/ff_satellite_2.ini | 1 + .../components/aocs/corner_cube_reflector.hpp | 33 +++++++++++-------- .../simulation/spacecraft/ff_components_2.cpp | 7 ++-- 5 files changed, 55 insertions(+), 16 deletions(-) create mode 100644 s2e-ff/data/initialize_files/components/corner_cube_reflector.ini create mode 100644 s2e-ff/data/initialize_files/components/laser_distance_meter.ini diff --git a/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini b/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini new file mode 100644 index 00000000..09aa909d --- /dev/null +++ b/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini @@ -0,0 +1,15 @@ +[CORNER_CUBE_REFLECTOR_0] +quaternion_b2c(0) = 0.0 +quaternion_b2c(1) = 0.7071 +quaternion_b2c(2) = 0.0 +quaternion_b2c(3) = 0.7071 + +position_b_m(0) = 0.5 +position_b_m(1) = 0 +position_b_m(2) = 0 + +normal_direction_c(0) = 0 +normal_direction_c(1) = 0 +normal_direction_c(2) = 1.0 + +reflectable_angle_rad = 0.1 diff --git a/s2e-ff/data/initialize_files/components/laser_distance_meter.ini b/s2e-ff/data/initialize_files/components/laser_distance_meter.ini new file mode 100644 index 00000000..df5fb762 --- /dev/null +++ b/s2e-ff/data/initialize_files/components/laser_distance_meter.ini @@ -0,0 +1,15 @@ +[LASER_DISTANCE_METER] +quaternion_b2c(0) = 0.0 +quaternion_b2c(1) = 0.0 +quaternion_b2c(2) = 0.7071 +quaternion_b2c(3) = 0.7071 + +position_b_m(0) = -0.5 +position_b_m(1) = 0 +position_b_m(2) = 0 + +laser_emitting_direction_c(0) = 0 +laser_emitting_direction_c(1) = 0 +laser_emitting_direction_c(2) = 1.0 + +emission_angle_rad = 0.1 diff --git a/s2e-ff/data/initialize_files/ff_satellite_2.ini b/s2e-ff/data/initialize_files/ff_satellite_2.ini index 02fe3112..8bc6df1b 100644 --- a/s2e-ff/data/initialize_files/ff_satellite_2.ini +++ b/s2e-ff/data/initialize_files/ff_satellite_2.ini @@ -133,3 +133,4 @@ structure_file = ../../data/initialize_files/ff_satellite_structure.ini [COMPONENT_FILES] // Users can add the path for component initialize files here. +corner_cube_reflector_file = ../../data/initialize_files/components/corner_cube_reflector.ini diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp index b50ee3de..ff5ddda3 100644 --- a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp +++ b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp @@ -7,6 +7,7 @@ #define S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_ #include +#include #include #include "../../library/math/translation_first_dual_quaternion.hpp" @@ -26,19 +27,7 @@ class CornerCubeReflector { * @fn CornerCubeReflector * @brief Constructor */ - CornerCubeReflector(const Dynamics* dynamics) : dynamics_(dynamics) { - normal_direction_c_[0] = 0.0; - normal_direction_c_[1] = 0.0; - normal_direction_c_[2] = 1.0; - reflectable_angle_rad_ = 0.1; - - libra::Quaternion q_b2c(0.0, 1.0, 0.0, 1.0); - libra::Vector<3> position_b2c_m; - position_b2c_m[0] = 0.5; - position_b2c_m[1] = 0.0; - position_b2c_m[2] = 0.0; - dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b2c_m, q_b2c.Conjugate()).QuaternionConjugate(); - } + CornerCubeReflector(const std::string file_name, const Dynamics* dynamics, const size_t id = 0) : dynamics_(dynamics) { Initialize(file_name, id); } /** * @fn ~CornerCubeReflector * @brief Destructor @@ -76,11 +65,27 @@ class CornerCubeReflector { protected: libra::Vector<3> normal_direction_c_{0.0}; //!< Reflection surface normal direction vector @ component frame - double reflectable_angle_rad_; //!< Reflectable half angle from the normal direction [rad] + double reflectable_angle_rad_ = 0.0; //!< Reflectable half angle from the normal direction [rad] libra::TranslationFirstDualQuaternion dual_quaternion_c2b_; //!< Dual quaternion from body to component frame // Reference const Dynamics* dynamics_; + + // Functions + void Initialize(const std::string file_name, const size_t id = 0) { + IniAccess ini_file(file_name); + std::string name = "CORNER_CUBE_REFLECTOR_"; + const std::string section_name = name + std::to_string(static_cast(id)); + + libra::Quaternion quaternion_b2c; + ini_file.ReadQuaternion(section_name.c_str(), "quaternion_b2c", quaternion_b2c); + libra::Vector<3> position_b_m; + ini_file.ReadVector(section_name.c_str(), "position_b_m", position_b_m); + dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b_m, quaternion_b2c.Conjugate()).QuaternionConjugate(); + + ini_file.ReadVector(section_name.c_str(), "normal_direction_c", normal_direction_c_); + reflectable_angle_rad_ = ini_file.ReadDouble(section_name.c_str(), "reflectable_angle_rad "); + } }; #endif // S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_ diff --git a/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp b/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp index 6b098e61..1c056ec9 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp @@ -14,12 +14,15 @@ FfComponents2::FfComponents2(const Dynamics* dynamics, const Structure* structur rel_info_(rel_info), inter_spacecraft_communication_(inter_spacecraft_communication) { // General - IniAccess sat_file = IniAccess(config->spacecraft_file_list_[0]); + IniAccess sat_file = IniAccess(config->spacecraft_file_list_[1]); // Component Instantiation obc_ = new OnBoardComputer(clock_gen); - corner_cube_reflector_ = new CornerCubeReflector(dynamics_); + std::string file_name = sat_file.ReadString("COMPONENT_FILES", "corner_cube_reflector_file"); + config_->main_logger_->CopyFileToLogDirectory(file_name); + corner_cube_reflector_ = new CornerCubeReflector(file_name, dynamics_); + inter_spacecraft_communication.SetCornerCubeReflector(corner_cube_reflector_); // Debug for actuator output From 8cea0994f3e74b2fc2171e8a720823bcdf48a929 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 22:12:58 +0100 Subject: [PATCH 09/14] Fix small --- s2e-ff/src/components/aocs/corner_cube_reflector.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp index ff5ddda3..47bc7997 100644 --- a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp +++ b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp @@ -84,7 +84,7 @@ class CornerCubeReflector { dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b_m, quaternion_b2c.Conjugate()).QuaternionConjugate(); ini_file.ReadVector(section_name.c_str(), "normal_direction_c", normal_direction_c_); - reflectable_angle_rad_ = ini_file.ReadDouble(section_name.c_str(), "reflectable_angle_rad "); + reflectable_angle_rad_ = ini_file.ReadDouble(section_name.c_str(), "reflectable_angle_rad"); } }; From 2bd32f4546ac360081aff2bd8d6d988662605b3c Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 22:23:43 +0100 Subject: [PATCH 10/14] Add laser distance meter ini file --- .../components/laser_distance_meter.ini | 6 ++-- s2e-ff/data/initialize_files/ff_satellite.ini | 1 + .../components/aocs/laser_distance_meter.cpp | 32 +++++++++++-------- .../components/aocs/laser_distance_meter.hpp | 6 ++-- .../simulation/spacecraft/ff_components.cpp | 3 +- 5 files changed, 29 insertions(+), 19 deletions(-) diff --git a/s2e-ff/data/initialize_files/components/laser_distance_meter.ini b/s2e-ff/data/initialize_files/components/laser_distance_meter.ini index df5fb762..fed6d240 100644 --- a/s2e-ff/data/initialize_files/components/laser_distance_meter.ini +++ b/s2e-ff/data/initialize_files/components/laser_distance_meter.ini @@ -1,4 +1,4 @@ -[LASER_DISTANCE_METER] +[LASER_DISTANCE_METER_0] quaternion_b2c(0) = 0.0 quaternion_b2c(1) = 0.0 quaternion_b2c(2) = 0.7071 @@ -9,7 +9,7 @@ position_b_m(1) = 0 position_b_m(2) = 0 laser_emitting_direction_c(0) = 0 -laser_emitting_direction_c(1) = 0 -laser_emitting_direction_c(2) = 1.0 +laser_emitting_direction_c(1) = 1.0 +laser_emitting_direction_c(2) = 0 emission_angle_rad = 0.1 diff --git a/s2e-ff/data/initialize_files/ff_satellite.ini b/s2e-ff/data/initialize_files/ff_satellite.ini index 91e802c3..9aa01319 100644 --- a/s2e-ff/data/initialize_files/ff_satellite.ini +++ b/s2e-ff/data/initialize_files/ff_satellite.ini @@ -136,5 +136,6 @@ structure_file = ../../data/initialize_files/ff_satellite_structure.ini relative_distance_sensor_file = ../../data/initialize_files/components/relative_distance_sensor.ini relative_position_sensor_file = ../../data/initialize_files/components/relative_position_sensor.ini relative_velocity_sensor_file = ../../data/initialize_files/components/relative_velocity_sensor.ini +laser_distance_meter_file = ../../data/initialize_files/components/laser_distance_meter.ini force_generator_file = ../../data/initialize_files/components/force_generator.ini relative_attitude_controller_file = ../../data/initialize_files/components/relative_attitude_controller.ini diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.cpp b/s2e-ff/src/components/aocs/laser_distance_meter.cpp index 2b68f782..05076aa1 100644 --- a/s2e-ff/src/components/aocs/laser_distance_meter.cpp +++ b/s2e-ff/src/components/aocs/laser_distance_meter.cpp @@ -5,20 +5,10 @@ #include "laser_distance_meter.hpp" -LaserDistanceMeter::LaserDistanceMeter(const int prescaler, ClockGenerator* clock_gen, const Dynamics& dynamics, - const FfInterSpacecraftCommunication& inter_spacecraft_communication) +LaserDistanceMeter::LaserDistanceMeter(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics& dynamics, + const FfInterSpacecraftCommunication& inter_spacecraft_communication, const size_t id) : Component(prescaler, clock_gen), dynamics_(dynamics), inter_spacecraft_communication_(inter_spacecraft_communication) { - laser_emitting_direction_c_[0] = 0.0; - laser_emitting_direction_c_[1] = 1.0; - laser_emitting_direction_c_[2] = 0.0; - emission_angle_rad_ = 0.1; - - libra::Quaternion q_b2c(0.0, 0.0, 1.0, 1.0); - libra::Vector<3> position_b2c_m; - position_b2c_m[0] = -0.5; - position_b2c_m[1] = 0.0; - position_b2c_m[2] = 0.0; - dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b2c_m, q_b2c.Conjugate()).QuaternionConjugate(); + Initialize(file_name, id); laser_emission_position_b_m_ = dual_quaternion_c2b_.TransformVector(libra::Vector<3>{0.0}); laser_emitting_direction_b_ = dual_quaternion_c2b_.TransformVector(laser_emitting_direction_c_); @@ -92,3 +82,19 @@ double LaserDistanceMeter::CalcDistanceBwPointAndLine(libra::Vector<3> point_pos libra::Vector<3> position = q_p - temp * line_direction; return position.CalcNorm(); } + +// Functions +void LaserDistanceMeter::Initialize(const std::string file_name, const size_t id) { + IniAccess ini_file(file_name); + std::string name = "LASER_DISTANCE_METER_"; + const std::string section_name = name + std::to_string(static_cast(id)); + + libra::Quaternion quaternion_b2c; + ini_file.ReadQuaternion(section_name.c_str(), "quaternion_b2c", quaternion_b2c); + libra::Vector<3> position_b_m; + ini_file.ReadVector(section_name.c_str(), "position_b_m", position_b_m); + dual_quaternion_c2b_ = libra::TranslationFirstDualQuaternion(-position_b_m, quaternion_b2c.Conjugate()).QuaternionConjugate(); + + ini_file.ReadVector(section_name.c_str(), "laser_emitting_direction_c", laser_emitting_direction_c_); + emission_angle_rad_ = ini_file.ReadDouble(section_name.c_str(), "emission_angle_rad"); +} diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.hpp b/s2e-ff/src/components/aocs/laser_distance_meter.hpp index b914b6c4..92d27642 100644 --- a/s2e-ff/src/components/aocs/laser_distance_meter.hpp +++ b/s2e-ff/src/components/aocs/laser_distance_meter.hpp @@ -24,8 +24,8 @@ class LaserDistanceMeter : public Component, public ILoggable { * @fn LaserDistanceMeter * @brief Constructor */ - LaserDistanceMeter(const int prescaler, ClockGenerator* clock_gen, const Dynamics& dynamics, - const FfInterSpacecraftCommunication& inter_spacecraft_communication); + LaserDistanceMeter(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics& dynamics, + const FfInterSpacecraftCommunication& inter_spacecraft_communication, const size_t id = 0); /** * @fn ~LaserDistanceMeter * @brief Destructor @@ -73,6 +73,8 @@ class LaserDistanceMeter : public Component, public ILoggable { const FfInterSpacecraftCommunication& inter_spacecraft_communication_; double CalcDistanceBwPointAndLine(libra::Vector<3> point_position, libra::Vector<3> line_start_position, libra::Vector<3> line_direction); + + void Initialize(const std::string file_name, const size_t id = 0); }; #endif // S2E_COMPONENTS_LASER_DISTANCE_METER_HPP_ diff --git a/s2e-ff/src/simulation/spacecraft/ff_components.cpp b/s2e-ff/src/simulation/spacecraft/ff_components.cpp index a0591ebd..ffd653d0 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components.cpp @@ -34,7 +34,8 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, relative_velocity_sensor_ = new RelativeVelocitySensor(InitializeRelativeVelocitySensor(clock_gen, rel_vel_file, compo_step_sec, *rel_info_, *dynamics_, sat_id)); - laser_distance_meter_ = new LaserDistanceMeter(1, clock_gen, *dynamics_, inter_spacecraft_communication_); + const std::string ldm_file = sat_file.ReadString(section_name.c_str(), "Laser_distance_meter_file"); + laser_distance_meter_ = new LaserDistanceMeter(1, clock_gen, ldm_file, *dynamics_, inter_spacecraft_communication_); const std::string force_generator_file = sat_file.ReadString(section_name.c_str(), "force_generator_file"); force_generator_ = new ForceGenerator(InitializeForceGenerator(clock_gen, force_generator_file, dynamics_)); From 985cc1fad8f1ad76442f87ca185784b1412bc3c4 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 22:24:58 +0100 Subject: [PATCH 11/14] Remove unused variables --- s2e-ff/src/components/aocs/laser_distance_meter.cpp | 4 ---- s2e-ff/src/components/aocs/laser_distance_meter.hpp | 9 --------- 2 files changed, 13 deletions(-) diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.cpp b/s2e-ff/src/components/aocs/laser_distance_meter.cpp index 05076aa1..e5738858 100644 --- a/s2e-ff/src/components/aocs/laser_distance_meter.cpp +++ b/s2e-ff/src/components/aocs/laser_distance_meter.cpp @@ -9,10 +9,6 @@ LaserDistanceMeter::LaserDistanceMeter(const int prescaler, ClockGenerator* cloc const FfInterSpacecraftCommunication& inter_spacecraft_communication, const size_t id) : Component(prescaler, clock_gen), dynamics_(dynamics), inter_spacecraft_communication_(inter_spacecraft_communication) { Initialize(file_name, id); - - laser_emission_position_b_m_ = dual_quaternion_c2b_.TransformVector(libra::Vector<3>{0.0}); - laser_emitting_direction_b_ = dual_quaternion_c2b_.TransformVector(laser_emitting_direction_c_); - laser_emitting_direction_b_ -= laser_emission_position_b_m_; } void LaserDistanceMeter::MainRoutine(int count) { diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.hpp b/s2e-ff/src/components/aocs/laser_distance_meter.hpp index 92d27642..31a8ec97 100644 --- a/s2e-ff/src/components/aocs/laser_distance_meter.hpp +++ b/s2e-ff/src/components/aocs/laser_distance_meter.hpp @@ -51,20 +51,11 @@ class LaserDistanceMeter : public Component, public ILoggable { */ virtual std::string GetLogValue() const; - inline libra::Vector<3> GetLaserEmissionPosition_i_m() const { return laser_emission_position_i_m_; } - inline libra::Vector<3> GetLaserEmittingDirection_i() const { return laser_emitting_direction_i_; } - protected: libra::Vector<3> laser_emitting_direction_c_; //!< Laser emitting direction @ component frame double emission_angle_rad_; //!< Emission half angle from the normal direction [rad] libra::TranslationFirstDualQuaternion dual_quaternion_c2b_; //!< Dual quaternion from component to body frame - libra::Vector<3> laser_emission_position_b_m_{0.0}; //!< Position of laser emission point @ inertia frame [m] - libra::Vector<3> laser_emitting_direction_b_{0.0}; - - libra::Vector<3> laser_emission_position_i_m_{0.0}; //!< Position of laser emission point @ inertia frame [m] - libra::Vector<3> laser_emitting_direction_i_{0.0}; - bool is_reflected_ = false; //!< Flag to detect reflected light double observed_distance_m_ = 0.0; //!< Observed direction From 90c534deb33a511c3d36f919c5bb1e94116ac7e2 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 22:45:13 +0100 Subject: [PATCH 12/14] Multiple reflectors --- .../components/corner_cube_reflector.ini | 3 +++ s2e-ff/src/components/aocs/laser_distance_meter.cpp | 6 +++--- .../case/ff_inter_spacecraft_communication.hpp | 7 ++++--- .../src/simulation/spacecraft/ff_components_2.cpp | 13 +++++++++---- .../src/simulation/spacecraft/ff_components_2.hpp | 2 +- 5 files changed, 20 insertions(+), 11 deletions(-) diff --git a/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini b/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini index 09aa909d..249ec5f5 100644 --- a/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini +++ b/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini @@ -1,3 +1,6 @@ +[GENERAL] +number_of_reflectors = 1 + [CORNER_CUBE_REFLECTOR_0] quaternion_b2c(0) = 0.0 quaternion_b2c(1) = 0.7071 diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.cpp b/s2e-ff/src/components/aocs/laser_distance_meter.cpp index e5738858..b500f8e9 100644 --- a/s2e-ff/src/components/aocs/laser_distance_meter.cpp +++ b/s2e-ff/src/components/aocs/laser_distance_meter.cpp @@ -23,8 +23,8 @@ void LaserDistanceMeter::MainRoutine(int count) { libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_; // Get reflector information - libra::Vector<3> reflector_position_i_m = inter_spacecraft_communication_.GetCornerCubeReflector().GetReflectorPosition_i_m(); - libra::Vector<3> reflector_normal_direction_i = inter_spacecraft_communication_.GetCornerCubeReflector().GetNormalDirection_i(); + libra::Vector<3> reflector_position_i_m = inter_spacecraft_communication_.GetCornerCubeReflector(0).GetReflectorPosition_i_m(); + libra::Vector<3> reflector_normal_direction_i = inter_spacecraft_communication_.GetCornerCubeReflector(0).GetNormalDirection_i(); // Conversion libra::Vector<3> reflector_position_c_m = dual_quaternion_c2i.InverseTransformVector(reflector_position_i_m); @@ -42,7 +42,7 @@ void LaserDistanceMeter::MainRoutine(int count) { return; } // Is the laser is reflected? - double reflectable_angle_rad = inter_spacecraft_communication_.GetCornerCubeReflector().GetReflectableAngle_rad(); + double reflectable_angle_rad = inter_spacecraft_communication_.GetCornerCubeReflector(0).GetReflectableAngle_rad(); double laser_incident_angle_rad = acos(libra::InnerProduct(reflector_normal_direction_c, -laser_emitting_direction_c_)); if (laser_incident_angle_rad > reflectable_angle_rad) { is_reflected_ = false; diff --git a/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp b/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp index 0d6029bf..e7b9ac9d 100644 --- a/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp +++ b/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp @@ -26,11 +26,12 @@ class FfInterSpacecraftCommunication { */ ~FfInterSpacecraftCommunication() {} - inline void SetCornerCubeReflector(CornerCubeReflector* corner_cube_reflector) { corner_cube_reflector_ = corner_cube_reflector; } - inline CornerCubeReflector& GetCornerCubeReflector() const { return *corner_cube_reflector_; } + inline void SetCornerCubeReflector(std::vector corner_cube_reflectors) { corner_cube_reflectors_ = corner_cube_reflectors; } + inline CornerCubeReflector& GetCornerCubeReflector(const size_t id) const { return *corner_cube_reflectors_[id]; } + inline size_t GetNumberOfReflectors() { return corner_cube_reflectors_.size(); } private: - CornerCubeReflector* corner_cube_reflector_; + std::vector corner_cube_reflectors_; }; #endif // S2E_SIMULATION_FF_SPACECRAFT_INTER_SPACECRAFT_COMMUNICATION_HPP_ diff --git a/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp b/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp index 1c056ec9..60c7ec09 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp @@ -21,9 +21,12 @@ FfComponents2::FfComponents2(const Dynamics* dynamics, const Structure* structur std::string file_name = sat_file.ReadString("COMPONENT_FILES", "corner_cube_reflector_file"); config_->main_logger_->CopyFileToLogDirectory(file_name); - corner_cube_reflector_ = new CornerCubeReflector(file_name, dynamics_); - - inter_spacecraft_communication.SetCornerCubeReflector(corner_cube_reflector_); + IniAccess corner_cube_file(file_name); + size_t number_of_reflectors = corner_cube_file.ReadInt("GENERAL", "number_of_reflectors"); + for (size_t id = 0; id < number_of_reflectors; id++) { + corner_cube_reflectors_.push_back(new CornerCubeReflector(file_name, dynamics_, id)); + } + inter_spacecraft_communication.SetCornerCubeReflector(corner_cube_reflectors_); // Debug for actuator output libra::Vector<3> force_N; @@ -34,7 +37,9 @@ FfComponents2::FfComponents2(const Dynamics* dynamics, const Structure* structur } FfComponents2::~FfComponents2() { - delete corner_cube_reflector_; + for (auto corner_cube_reflector : corner_cube_reflectors_) { + delete corner_cube_reflector; + } // OBC must be deleted the last since it has com ports delete obc_; } diff --git a/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp b/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp index e1e3bba1..ade388ce 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp @@ -60,7 +60,7 @@ class FfComponents2 : public InstalledComponents { private: // Components OnBoardComputer* obc_; //!< On board computer - CornerCubeReflector* corner_cube_reflector_; + std::vector corner_cube_reflectors_; // References const Dynamics* dynamics_; //!< Dynamics information of the spacecraft From 4ddda323c9d184f5de2de2670b1aa862f32ac946 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 23:26:25 +0100 Subject: [PATCH 13/14] Apply multiple reflectors for laser distance meter --- .../components/corner_cube_reflector.ini | 18 ++++- .../components/aocs/laser_distance_meter.cpp | 73 ++++++++++++------- .../ff_inter_spacecraft_communication.hpp | 4 +- 3 files changed, 64 insertions(+), 31 deletions(-) diff --git a/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini b/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini index 249ec5f5..2255276b 100644 --- a/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini +++ b/s2e-ff/data/initialize_files/components/corner_cube_reflector.ini @@ -1,5 +1,5 @@ [GENERAL] -number_of_reflectors = 1 +number_of_reflectors = 2 [CORNER_CUBE_REFLECTOR_0] quaternion_b2c(0) = 0.0 @@ -16,3 +16,19 @@ normal_direction_c(1) = 0 normal_direction_c(2) = 1.0 reflectable_angle_rad = 0.1 + +[CORNER_CUBE_REFLECTOR_1] +quaternion_b2c(0) = 0.0 +quaternion_b2c(1) = 0.7071 +quaternion_b2c(2) = 0.0 +quaternion_b2c(3) = 0.7071 + +position_b_m(0) = 0.5 +position_b_m(1) = 0.5 +position_b_m(2) = 0.5 + +normal_direction_c(0) = 0 +normal_direction_c(1) = 0 +normal_direction_c(2) = 1.0 + +reflectable_angle_rad = 0.1 diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.cpp b/s2e-ff/src/components/aocs/laser_distance_meter.cpp index b500f8e9..3e39d13f 100644 --- a/s2e-ff/src/components/aocs/laser_distance_meter.cpp +++ b/s2e-ff/src/components/aocs/laser_distance_meter.cpp @@ -12,7 +12,7 @@ LaserDistanceMeter::LaserDistanceMeter(const int prescaler, ClockGenerator* cloc } void LaserDistanceMeter::MainRoutine(int count) { - UNUSED(count); + if (count < 10) return; // Body -> Inertial frame libra::Vector<3> spacecraft_position_i2b_m = dynamics_.GetOrbit().GetPosition_i_m(); @@ -22,35 +22,52 @@ void LaserDistanceMeter::MainRoutine(int count) { // Component -> Inertial frame libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_; - // Get reflector information - libra::Vector<3> reflector_position_i_m = inter_spacecraft_communication_.GetCornerCubeReflector(0).GetReflectorPosition_i_m(); - libra::Vector<3> reflector_normal_direction_i = inter_spacecraft_communication_.GetCornerCubeReflector(0).GetNormalDirection_i(); - - // Conversion - libra::Vector<3> reflector_position_c_m = dual_quaternion_c2i.InverseTransformVector(reflector_position_i_m); - libra::Vector<3> reflector_normal_direction_c = dual_quaternion_c2i.GetRotationQuaternion().InverseFrameConversion(reflector_normal_direction_i); - - // Calc relative distance - observed_distance_m_ = reflector_position_c_m.CalcNorm(); - - // Check reflection - // Is the reflector in the laser radius? - double laser_radius_m = observed_distance_m_ * tan(emission_angle_rad_); - double closest_distance_m = CalcDistanceBwPointAndLine(reflector_position_c_m, libra::Vector<3>{0.0}, laser_emitting_direction_c_); - if (closest_distance_m > laser_radius_m) { - is_reflected_ = false; - return; + // Corner cube info + size_t number_of_reflectors = inter_spacecraft_communication_.GetNumberOfReflectors(); + observed_distance_m_ = 1e30; + double observed_distance_m = 0.0; + is_reflected_ = false; + for (size_t reflector_id = 0; reflector_id < number_of_reflectors; reflector_id++) { + // Get reflector information + libra::Vector<3> reflector_position_i_m = inter_spacecraft_communication_.GetCornerCubeReflector(reflector_id).GetReflectorPosition_i_m(); + libra::Vector<3> reflector_normal_direction_i = inter_spacecraft_communication_.GetCornerCubeReflector(reflector_id).GetNormalDirection_i(); + + // Conversion + libra::Vector<3> reflector_position_c_m = dual_quaternion_c2i.InverseTransformVector(reflector_position_i_m); + libra::Vector<3> reflector_normal_direction_c = dual_quaternion_c2i.GetRotationQuaternion().InverseFrameConversion(reflector_normal_direction_i); + + // Calc relative distance + observed_distance_m = reflector_position_c_m.CalcNorm(); + + // Check reflection + // Is the reflector in the laser radius? + double laser_radius_m = observed_distance_m * tan(emission_angle_rad_); + double closest_distance_m = CalcDistanceBwPointAndLine(reflector_position_c_m, libra::Vector<3>{0.0}, laser_emitting_direction_c_); + if (closest_distance_m > laser_radius_m) { + continue; + } + // Is the laser is reflected? + double reflectable_angle_rad = inter_spacecraft_communication_.GetCornerCubeReflector(reflector_id).GetReflectableAngle_rad(); + double cos_theta = libra::InnerProduct(reflector_normal_direction_c, -laser_emitting_direction_c_); + if (cos_theta > 1.0) cos_theta = 1.0; + if (cos_theta < -1.0) cos_theta = -1.0; + double laser_incident_angle_rad = acos(cos_theta); + if (laser_incident_angle_rad > reflectable_angle_rad) { + continue; + } + is_reflected_ = true; + // Observe closest point + if (observed_distance_m < observed_distance_m_) { + observed_distance_m_ = observed_distance_m; + } } - // Is the laser is reflected? - double reflectable_angle_rad = inter_spacecraft_communication_.GetCornerCubeReflector(0).GetReflectableAngle_rad(); - double laser_incident_angle_rad = acos(libra::InnerProduct(reflector_normal_direction_c, -laser_emitting_direction_c_)); - if (laser_incident_angle_rad > reflectable_angle_rad) { - is_reflected_ = false; - return; - } - is_reflected_ = true; - // Add noise + if (is_reflected_ == true) { + // Add noise + // TBW + } else { + observed_distance_m_ = 0.0; + } } std::string LaserDistanceMeter::GetLogHeader() const { diff --git a/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp b/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp index e7b9ac9d..022df040 100644 --- a/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp +++ b/s2e-ff/src/simulation/case/ff_inter_spacecraft_communication.hpp @@ -27,8 +27,8 @@ class FfInterSpacecraftCommunication { ~FfInterSpacecraftCommunication() {} inline void SetCornerCubeReflector(std::vector corner_cube_reflectors) { corner_cube_reflectors_ = corner_cube_reflectors; } - inline CornerCubeReflector& GetCornerCubeReflector(const size_t id) const { return *corner_cube_reflectors_[id]; } - inline size_t GetNumberOfReflectors() { return corner_cube_reflectors_.size(); } + inline CornerCubeReflector& GetCornerCubeReflector(const size_t id) const { return *(corner_cube_reflectors_[id]); } + inline size_t GetNumberOfReflectors() const { return corner_cube_reflectors_.size(); } private: std::vector corner_cube_reflectors_; From 08d37af1728e153094f7712b623f77959b0b4b45 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 27 Dec 2023 13:04:29 +0100 Subject: [PATCH 14/14] Add getter functions --- s2e-ff/src/components/aocs/laser_distance_meter.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.hpp b/s2e-ff/src/components/aocs/laser_distance_meter.hpp index 31a8ec97..678a6855 100644 --- a/s2e-ff/src/components/aocs/laser_distance_meter.hpp +++ b/s2e-ff/src/components/aocs/laser_distance_meter.hpp @@ -51,6 +51,9 @@ class LaserDistanceMeter : public Component, public ILoggable { */ virtual std::string GetLogValue() const; + inline double GetObservedDistance_m() const { return observed_distance_m_; } + inline bool GetIsReflected() const { return is_reflected_; } + protected: libra::Vector<3> laser_emitting_direction_c_; //!< Laser emitting direction @ component frame double emission_angle_rad_; //!< Emission half angle from the normal direction [rad]