fix based on the comment

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

@@ -13,7 +13,7 @@ quaternion_b2c(2) = 1.0
 quaternion_b2c(3) = 0.0
 
 position_b_m(0) = -0.5
-position_b_m(1) = 0
+position_b_m(1) = 0.0
 position_b_m(2) = 0.012
 
 // It is required to have a position calculation converter based on the output values of the quadrant sensors.
@@ -22,8 +22,18 @@ position_b_m(2) = 0.012
 // reference_ratio_z_axis_list : List of `qpd_sensor_output_z_axis_V / qpd_sensor_output_sum_V` at each point on the z-axis.
 qpd_sensor_file_directory = ../../data/initialize_files/components/qpd_sensor_csv_files/
 
+// Radius of light-receiving area of quadrant photodiode sensor [m]
 qpd_sensor_radius_m = 3.9e-3
+
+// Integral interval to calculate the output values of the quadrant photodiode sensor [m]
 qpd_sensor_integral_step_m = 5.0e-5
+
+// Positioning threshold of the quadrant photodiode sensor [m]
 qpd_positioning_threshold_m = 5.0e-3
+
+// Laser receivable angle of the quadrant photodiode sensor [rad]
 qpd_laser_receivable_angle_rad = 0.785
+
+// Voltage threshold ot the quadrant photodiode sensor [V]
+// Less than this value, the QPD positioning sensor cannot determine the position displacement.
 qpd_sensor_output_voltage_threshold_V = 0.4

s2e-ff/data/initialize_files/components/qpd_sensor_csv_files/qpd_sensor_reference.csv → s2e-ff/data/initialize_files/components/qpd_sensor_csv_files/qpd_sensor_voltage_ratio.csv

@@ -1,4 +1,4 @@
-displacement[m],reference_ratio_y_axis_list,reference_ratio_z_axis_list
+displacement[m],voltage_ratio_y_axis_list,voltage_ratio_z_axis_list
 -4.00e-3,1,-1
 -3.99e-3,1,-1
 -3.98e-3,1,-1

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

@@ -25,19 +25,13 @@ void QpdPositioningSensor::MainRoutine(int count) {
   // Laser emitter info
   size_t number_of_laser_emitters = inter_spacecraft_communication_.GetNumberOfLasers();
   distance_true_m_ = 1e30;
-  y_axis_displacement_true_m_ = 1e30;
-  z_axis_displacement_true_m_ = 1e30;
+  y_axis_displacement_true_m_ = 1.0e3;
+  z_axis_displacement_true_m_ = 1.0e3;
   observed_y_axis_displacement_m_ = 1.0e3;
   observed_z_axis_displacement_m_ = 1.0e3;
   qpd_sensor_output_y_axis_V_ = 0.0;
   qpd_sensor_output_z_axis_V_ = 0.0;
   qpd_sensor_output_sum_V_ = 0.0;
-  double qpd_laser_distance_m = 0.0;
-  double qpd_y_axis_displacement_m = 0.0;
-  double qpd_z_axis_displacement_m = 0.0;
-
-  double qpd_received_laser_beam_radius_m = 0.0;
-  double qpd_received_laser_power_W = 0.0;
 
   is_received_laser_ = false;
   for (size_t laser_id = 0; laser_id < number_of_laser_emitters; laser_id++) {
@@ -60,9 +54,9 @@ void QpdPositioningSensor::MainRoutine(int count) {
     }
     libra::Vector<3> laser_received_position_c_m =
         CalcLaserReceivedPosition(laser_position_c_m, libra::Vector<3>{0.0}, x_axis_direction_c_, laser_emitting_direction_c);
-    qpd_laser_distance_m = laser_position_c_m.CalcNorm();
-    qpd_y_axis_displacement_m = CalcDisplacement(laser_received_position_c_m, libra::Vector<3>{0.0}, y_axis_direction_c_);
-    qpd_z_axis_displacement_m = CalcDisplacement(laser_received_position_c_m, libra::Vector<3>{0.0}, z_axis_direction_c_);
+    double qpd_laser_distance_m = laser_position_c_m.CalcNorm();
+    double qpd_y_axis_displacement_m = CalcDisplacement(laser_received_position_c_m, libra::Vector<3>{0.0}, y_axis_direction_c_);
+    double qpd_z_axis_displacement_m = CalcDisplacement(laser_received_position_c_m, libra::Vector<3>{0.0}, z_axis_direction_c_);
 
     if (qpd_laser_distance_m < distance_true_m_) {
       distance_true_m_ = qpd_laser_distance_m;
@@ -86,9 +80,9 @@ void QpdPositioningSensor::MainRoutine(int count) {
   }
   if (is_received_laser_) {
     observed_y_axis_displacement_m_ =
-        ObservePositionDisplacement(kYAxisDirection, qpd_sensor_output_y_axis_V_, qpd_sensor_output_sum_V_, qpd_ratio_y_reference_list_);
+        ObservePositionDisplacement(kYAxisDirection, qpd_sensor_output_y_axis_V_, qpd_sensor_output_sum_V_, qpd_sensor_voltage_ratio_y_list_);
     observed_z_axis_displacement_m_ =
-        ObservePositionDisplacement(kZAxisDirection, qpd_sensor_output_z_axis_V_, qpd_sensor_output_sum_V_, qpd_ratio_z_reference_list_);
+        ObservePositionDisplacement(kZAxisDirection, qpd_sensor_output_z_axis_V_, qpd_sensor_output_sum_V_, qpd_sensor_voltage_ratio_z_list_);
   }
 }
 
@@ -175,16 +169,16 @@ double QpdPositioningSensor::CalcSign(const double input_value, const double thr
 }
 
 double QpdPositioningSensor::ObservePositionDisplacement(const QpdObservedPositionDirection observation_direction, const double qpd_sensor_output_V,
-                                                         const double qpd_sensor_output_sum_V, const std::vector<double>& qpd_ratio_reference_list) {
+                                                         const double qpd_sensor_output_sum_V, const std::vector<double>& qpd_voltage_ratio_list) {
   double qpd_sensor_output_polarization = (observation_direction == kYAxisDirection) ? -1.0 : 1.0;
-  double observed_displacement_m = qpd_sensor_output_polarization * CalcSign(qpd_sensor_output_y_axis_V_, 0.0) * qpd_positioning_threshold_m_;
+  double observed_displacement_m = qpd_sensor_output_polarization * CalcSign(qpd_sensor_output_sum_V, 0.0) * qpd_positioning_threshold_m_;
   double sensor_value_ratio = qpd_sensor_output_V / qpd_sensor_output_sum_V;
-  for (size_t i = 0; i < qpd_ratio_reference_list.size() - 1; ++i) {
-    if ((qpd_sensor_output_polarization * sensor_value_ratio >= qpd_sensor_output_polarization * qpd_ratio_reference_list[i]) &&
-        (qpd_sensor_output_polarization * sensor_value_ratio <= qpd_sensor_output_polarization * qpd_ratio_reference_list[i + 1])) {
+  for (size_t i = 0; i < qpd_voltage_ratio_list.size() - 1; ++i) {
+    if ((qpd_sensor_output_polarization * sensor_value_ratio >= qpd_sensor_output_polarization * qpd_voltage_ratio_list[i]) &&
+        (qpd_sensor_output_polarization * sensor_value_ratio <= qpd_sensor_output_polarization * qpd_voltage_ratio_list[i + 1])) {
       observed_displacement_m = qpd_displacement_reference_list_m_[i];
       observed_displacement_m += (qpd_displacement_reference_list_m_[i + 1] - qpd_displacement_reference_list_m_[i]) *
-                                 (sensor_value_ratio - qpd_ratio_reference_list[i]) / (qpd_ratio_reference_list[i + 1] - qpd_ratio_reference_list[i]);
+                                 (sensor_value_ratio - qpd_voltage_ratio_list[i]) / (qpd_voltage_ratio_list[i + 1] - qpd_voltage_ratio_list[i]);
     }
   }
   return observed_displacement_m;
@@ -197,15 +191,15 @@ void QpdPositioningSensor::Initialize(const std::string file_name, const size_t
   const std::string section_name = name + std::to_string(static_cast<long long>(id));
 
   std::string file_path = ini_file.ReadString(section_name.c_str(), "qpd_sensor_file_directory");
-  std::string filepath_qpd_sensor_reference = file_path + "qpd_sensor_reference.csv";
-  IniAccess conf_qpd_sensor_reference(filepath_qpd_sensor_reference);
-  std::vector<std::vector<std::string>> qpd_sensor_reference_str_list;
-  conf_qpd_sensor_reference.ReadCsvString(qpd_sensor_reference_str_list, 1000);
-
-  for (size_t index = 1; index < qpd_sensor_reference_str_list.size(); ++index) {  // first row is for labels
-    qpd_displacement_reference_list_m_.push_back(stod(qpd_sensor_reference_str_list[index][0]));
-    qpd_ratio_y_reference_list_.push_back(stod(qpd_sensor_reference_str_list[index][1]));
-    qpd_ratio_z_reference_list_.push_back(stod(qpd_sensor_reference_str_list[index][2]));
+  std::string filepath_qpd_sensor_voltage_ratio = file_path + "qpd_sensor_voltage_ratio.csv";
+  IniAccess conf_qpd_sensor_voltage_ratio(filepath_qpd_sensor_voltage_ratio);
+  std::vector<std::vector<std::string>> qpd_sensor_voltage_ratio_str_list;
+  conf_qpd_sensor_voltage_ratio.ReadCsvString(qpd_sensor_voltage_ratio_str_list, 1000);
+
+  for (size_t index = 1; index < qpd_sensor_voltage_ratio_str_list.size(); ++index) {  // first row is for labels
+    qpd_displacement_reference_list_m_.push_back(stod(qpd_sensor_voltage_ratio_str_list[index][0]));
+    qpd_sensor_voltage_ratio_y_list_.push_back(stod(qpd_sensor_voltage_ratio_str_list[index][1]));
+    qpd_sensor_voltage_ratio_z_list_.push_back(stod(qpd_sensor_voltage_ratio_str_list[index][2]));
   }
 
   libra::Quaternion quaternion_b2c;

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

@@ -91,8 +91,8 @@ class QpdPositioningSensor : public Component, public ILoggable {
   double observed_z_axis_displacement_m_ = 0.0;  //!< Observed displacement in the z-axis direction [m]
 
   std::vector<double> qpd_displacement_reference_list_m_;
-  std::vector<double> qpd_ratio_y_reference_list_;
-  std::vector<double> qpd_ratio_z_reference_list_;
+  std::vector<double> qpd_sensor_voltage_ratio_y_list_;
+  std::vector<double> qpd_sensor_voltage_ratio_z_list_;
 
   // Reference
   const Dynamics& dynamics_;