Add laser distance meter and corner cube reflector

s2e-ff/CMakeLists.txt

@@ -51,6 +51,7 @@ 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/ideal/relative_attitude_controller.cpp
   src/components/ideal/initialize_relative_attitude_controller.cpp
   src/library/math/dual_quaternion.cpp

s2e-ff/data/initialize_files/components/corner_cube_reflector.ini

@@ -0,0 +1,34 @@
+[GENERAL]
+number_of_reflectors = 2
+
+[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
+
+[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

s2e-ff/data/initialize_files/components/laser_distance_meter.ini

@@ -0,0 +1,15 @@
+[LASER_DISTANCE_METER_0]
+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) = 1.0
+laser_emitting_direction_c(2) = 0
+
+emission_angle_rad = 0.1

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
@@ -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

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
@@ -134,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

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

@@ -0,0 +1,91 @@
+/**
+ * @file corner_cube_reflector.hpp
+ * @brief Corner cube reflector
+ */
+
+#ifndef S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_
+#define S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_
+
+#include <dynamics/dynamics.hpp>
+#include <library/initialize/initialize_file_access.hpp>
+#include <library/math/vector.hpp>
+
+#include "../../library/math/translation_first_dual_quaternion.hpp"
+
+/**
+ * @class CornerCubeReflector
+ * @brief Corner Cube Reflector
+ */
+class CornerCubeReflector {
+ public:
+  /**
+   * @fn CornerCubeReflector
+   * @brief Constructor
+   */
+  CornerCubeReflector() : dynamics_(nullptr) {}
+  /**
+   * @fn CornerCubeReflector
+   * @brief Constructor
+   */
+  CornerCubeReflector(const std::string file_name, const Dynamics* dynamics, const size_t id = 0) : dynamics_(dynamics) { Initialize(file_name, id); }
+  /**
+   * @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());
+
+    // 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_ = 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<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");
+  }
+};
+
+#endif  // S2E_COMPONENTS_CORNER_CUBE_REFLECTOR_HPP_

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

@@ -0,0 +1,113 @@
+/**
+ * @file laser_distance_meter.hpp
+ * @brief Laser distance meter
+ */
+
+#include "laser_distance_meter.hpp"
+
+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) {
+  Initialize(file_name, id);
+}
+
+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();
+  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 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;
+    }
+  }
+
+  if (is_reflected_ == true) {
+    // Add noise
+    // TBW
+  } else {
+    observed_distance_m_ = 0.0;
+  }
+}
+
+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();
+}
+
+// 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<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(), "laser_emitting_direction_c", laser_emitting_direction_c_);
+  emission_angle_rad_ = ini_file.ReadDouble(section_name.c_str(), "emission_angle_rad");
+}