Fix component algorithm

s2e-ff/CMakeLists.txt

@@ -45,6 +45,7 @@ add_subdirectory(${S2E_CORE_DIR}/src/simulation s2e_core/simulation)
 
 set(SOURCE_FILES
   src/s2e_ff.cpp
+  src/components/aocs/corner_cube_reflector.cpp
   src/components/aocs/relative_distance_sensor.cpp
   src/components/aocs/relative_position_sensor.cpp
   src/components/aocs/relative_attitude_sensor.cpp

s2e-ff/src/components/aocs/corner_cube_reflector.cpp

@@ -0,0 +1,59 @@
+/**
+ * @file corner_cube_reflector.cpp
+ * @brief Corner cube reflector
+ */
+
+#include "./corner_cube_reflector.hpp"
+
+CornerCubeReflector::CornerCubeReflector(const int prescaler, ClockGenerator* clock_gen) : Component(prescaler, clock_gen), dynamics_(nullptr){};
+
+CornerCubeReflector::CornerCubeReflector(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics* dynamics,
+                                         const size_t id)
+    : Component(prescaler, clock_gen), dynamics_(dynamics) {
+  Initialize(file_name, id);
+}
+
+void CornerCubeReflector::MainRoutine(int count) { Update(count); }
+
+void CornerCubeReflector::Update(int count) {
+  if (count_ >= count) 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();
+  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_;
+
+  corner_cube_reflector_position_i_m_ = dual_quaternion_c2i.TransformVector(libra::Vector<3>{0.0});
+  corner_cube_reflector_normal_direction_i_ = dual_quaternion_c2i.TransformVector(normal_direction_c_) - corner_cube_reflector_position_i_m_;
+
+  count_ = count;
+}
+
+std::string CornerCubeReflector::GetLogHeader() const {
+  std::string str_tmp = "";
+
+  return str_tmp;
+}
+
+std::string CornerCubeReflector::GetLogValue() const {
+  std::string str_tmp = "";
+
+  return str_tmp;
+}
+
+void CornerCubeReflector::Initialize(const std::string file_name, const size_t id) {
+  IniAccess ini_file(file_name);
+  std::string name = "CORNER_CUBE_REFLECTOR_";
+  const std::string section_name = name + std::to_string(static_cast<long long>(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");
+}

s2e-ff/src/components/aocs/corner_cube_reflector.hpp

@@ -6,6 +6,7 @@
 #ifndef S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_
 #define S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_
 
+#include <components/base/component.hpp>
 #include <dynamics/dynamics.hpp>
 #include <library/initialize/initialize_file_access.hpp>
 #include <library/math/vector.hpp>
@@ -16,50 +17,47 @@
  * @class CornerCubeReflector
  * @brief Corner Cube Reflector
  */
-class CornerCubeReflector {
+class CornerCubeReflector : public Component {
  public:
   /**
    * @fn CornerCubeReflector
    * @brief Constructor
    */
-  CornerCubeReflector() : dynamics_(nullptr) {}
+  CornerCubeReflector(const int prescaler, ClockGenerator* clock_gen);
   /**
    * @fn CornerCubeReflector
    * @brief Constructor
    */
-  CornerCubeReflector(const std::string file_name, const Dynamics* dynamics, const size_t id = 0) : dynamics_(dynamics) { Initialize(file_name, id); }
+  CornerCubeReflector(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics* dynamics, const size_t id = 0);
   /**
    * @fn ~CornerCubeReflector
    * @brief Destructor
    */
   ~CornerCubeReflector() {}
 
-  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.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});
-  }
-
-  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();
-    libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b.Conjugate());
+  // ComponentBase override function
+  /**
+   * @fn MainRoutine
+   * @brief Main routine
+   */
+  void MainRoutine(int count);
 
-    // Component -> Inertial frame
-    libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_;
+  // 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;
 
-    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;
+  void Update(int count);
 
-    return normal_direction_i;
-  }
+  inline libra::Vector<3> GetReflectorPosition_i_m() const { return corner_cube_reflector_position_i_m_; }
+  inline libra::Vector<3> GetNormalDirection_i() const { return corner_cube_reflector_normal_direction_i_; }
 
   inline double GetReflectableAngle_rad() const { return reflectable_angle_rad_; }
 
@@ -68,24 +66,16 @@ class CornerCubeReflector {
   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
 
+  libra::Vector<3> corner_cube_reflector_position_i_m_{0.0};
+  libra::Vector<3> corner_cube_reflector_normal_direction_i_{0.0};
+
+  int count_ = 0;
+
   // 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<long long>(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");
-  }
+  void Initialize(const std::string file_name, const size_t id = 0);
 };
 
 #endif  // S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_

s2e-ff/src/components/aocs/laser_distance_meter.cpp

@@ -29,6 +29,7 @@ void LaserDistanceMeter::MainRoutine(int count) {
   is_reflected_ = false;
   for (size_t reflector_id = 0; reflector_id < number_of_reflectors; reflector_id++) {
     // Get reflector information
+    inter_spacecraft_communication_.GetCornerCubeReflector(reflector_id).Update(count);
     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();
 

s2e-ff/src/components/aocs/laser_emitter.cpp

@@ -8,18 +8,51 @@
  * @fn LaserEmitter
  * @brief Constructor
  */
-LaserEmitter::LaserEmitter(const Dynamics& dynamics, libra::Vector<3> emitting_direction_c, double emission_angle_rad,
-                           libra::TranslationFirstDualQuaternion dual_quaternion_c2b, double emission_power_W, double radius_beam_waist_m,
-                           double rayleigh_length_m, double rayleigh_length_offset_m, double wavelength_m)
-    : GaussianBeamBase(wavelength_m, radius_beam_waist_m, emission_power_W),
+LaserEmitter::LaserEmitter(const int prescaler, ClockGenerator* clock_gen, const Dynamics& dynamics, libra::Vector<3> emitting_direction_c,
+                           double emission_angle_rad, libra::TranslationFirstDualQuaternion dual_quaternion_c2b, double emission_power_W,
+                           double radius_beam_waist_m, double rayleigh_length_m, double rayleigh_length_offset_m, double wavelength_m)
+    : Component(prescaler, clock_gen),
+      GaussianBeamBase(wavelength_m, radius_beam_waist_m, emission_power_W),
       dynamics_(dynamics),
       emitting_direction_c_(emitting_direction_c),
       emission_angle_rad_(emission_angle_rad),
       dual_quaternion_c2b_(dual_quaternion_c2b),
       rayleigh_length_m_(rayleigh_length_m),
       rayleigh_length_offset_m_(rayleigh_length_offset_m) {}
 
-LaserEmitter InitializeLaserEmitter(const std::string file_name, const Dynamics& dynamics, const size_t id) {
+void LaserEmitter::MainRoutine(int count) { Update(count); }
+
+void LaserEmitter::Update(int count) {
+  if (count_ >= count) 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();
+  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_;
+
+  laser_position_i_m_ = dual_quaternion_c2i.TransformVector(libra::Vector<3>{0.0});
+
+  laser_emitting_direction_i_ = dual_quaternion_c2i.TransformVector(emitting_direction_c_) - laser_position_i_m_;
+
+  count_ = count;
+}
+
+std::string LaserEmitter::GetLogHeader() const {
+  std::string str_tmp = "";
+
+  return str_tmp;
+}
+
+std::string LaserEmitter::GetLogValue() const {
+  std::string str_tmp = "";
+
+  return str_tmp;
+}
+
+LaserEmitter InitializeLaserEmitter(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics& dynamics,
+                                    const size_t id) {
   IniAccess ini_file(file_name);
   std::string name = "LASER_EMITTER_";
   const std::string section_name = name + std::to_string(static_cast<long long>(id));
@@ -43,8 +76,8 @@ LaserEmitter InitializeLaserEmitter(const std::string file_name, const Dynamics&
 
   double wavelength_m = libra::pi * radius_beam_waist_m * radius_beam_waist_m / rayleigh_length_m;
 
-  LaserEmitter laser_emitter(dynamics, emitting_direction_c, emission_angle_rad, dual_quaternion_c2b, emission_power_W, radius_beam_waist_m,
-                             rayleigh_length_m, rayleigh_length_offset_m, wavelength_m);
+  LaserEmitter laser_emitter(prescaler, clock_gen, dynamics, emitting_direction_c, emission_angle_rad, dual_quaternion_c2b, emission_power_W,
+                             radius_beam_waist_m, rayleigh_length_m, rayleigh_length_offset_m, wavelength_m);
 
   return laser_emitter;
 };

s2e-ff/src/components/aocs/laser_emitter.hpp

@@ -6,6 +6,7 @@
 #ifndef S2E_COMPONENTS_LASER_EMITTER_HPP_
 #define S2E_COMPONENTS_LASER_EMITTER_HPP_
 
+#include <components/base/component.hpp>
 #include <dynamics/dynamics.hpp>
 #include <library/initialize/initialize_file_access.hpp>
 #include <library/math/vector.hpp>
@@ -17,48 +18,45 @@
  * @class LaserEmitter
  * @brief Laser Emitter
  */
-class LaserEmitter : public GaussianBeamBase {
+class LaserEmitter : public Component, public GaussianBeamBase {
  public:
   /**
    * @fn LaserEmitter
    * @brief Constructor
    */
-  LaserEmitter(const Dynamics& dynamics, libra::Vector<3> emitting_direction_c, double emission_angle_rad,
-               libra::TranslationFirstDualQuaternion dual_quaternion_c2b, double emission_power_W, double radius_beam_waist_m,
-               double rayleigh_length_m, double rayleigh_length_offset_m, double wavelength_m);
+  LaserEmitter(const int prescaler, ClockGenerator* clock_gen, const Dynamics& dynamics, libra::Vector<3> emitting_direction_c,
+               double emission_angle_rad, libra::TranslationFirstDualQuaternion dual_quaternion_c2b, double emission_power_W,
+               double radius_beam_waist_m, double rayleigh_length_m, double rayleigh_length_offset_m, double wavelength_m);
 
   /**
    * @fn ~LaserEmitter
    * @brief Destructor
    */
   ~LaserEmitter() {}
 
-  inline libra::Vector<3> GetLaserPosition_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.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});
-  }
-
-  inline libra::Vector<3> GetEmittingDirection_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();
-    libra::TranslationFirstDualQuaternion dual_quaternion_i2b(-spacecraft_position_i2b_m, spacecraft_attitude_i2b.Conjugate());
+  // ComponentBase override function
+  /**
+   * @fn MainRoutine
+   * @brief Main routine
+   */
+  void MainRoutine(int count);
 
-    // Component -> Inertial frame
-    libra::TranslationFirstDualQuaternion dual_quaternion_c2i = dual_quaternion_i2b.QuaternionConjugate() * dual_quaternion_c2b_;
+  // 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;
 
-    libra::Vector<3> laser_position_i_m = dual_quaternion_c2i.TransformVector(libra::Vector<3>{0.0});
-    libra::Vector<3> emitting_direction_i = dual_quaternion_c2i.TransformVector(emitting_direction_c_);
-    emitting_direction_i -= laser_position_i_m;
+  void Update(int count);
 
-    return emitting_direction_i;
-  }
+  inline libra::Vector<3> GetLaserPosition_i_m() const { return laser_position_i_m_; }
+  inline libra::Vector<3> GetEmittingDirection_i() const { return laser_emitting_direction_i_; }
 
   inline double GetEmissionAngle_rad() const { return emission_angle_rad_; }
   inline double GetRayleighLength_m() const { return rayleigh_length_m_; }
@@ -72,10 +70,16 @@ class LaserEmitter : public GaussianBeamBase {
   double rayleigh_length_m_ = 0.0;         //!< Rayleigh length (range) of the laser [m]
   double rayleigh_length_offset_m_ = 0.0;  //!< Rayleigh length (range) position offset of the laser [m]
 
+  libra::Vector<3> laser_position_i_m_{0.0};
+  libra::Vector<3> laser_emitting_direction_i_{0.0};
+
+  int count_ = 0;
+
   // Reference
   const Dynamics& dynamics_;
 };
 
-LaserEmitter InitializeLaserEmitter(const std::string file_name, const Dynamics& dynamics, const size_t id = 0);
+LaserEmitter InitializeLaserEmitter(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics& dynamics,
+                                    const size_t id = 0);
 
 #endif  // S2E_COMPONENTS_LASER_EMITTER_HPP_