diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 3fda80f..7780350 100644 --- a/s2e-ff/CMakeLists.txt +++ b/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 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 0000000..79d6d46 --- /dev/null +++ b/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(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"); +} \ No newline at end of file diff --git a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp b/s2e-ff/src/components/aocs/corner_cube_reflector.hpp index 47bc799..8188955 100644 --- a/s2e-ff/src/components/aocs/corner_cube_reflector.hpp +++ b/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 #include #include #include @@ -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(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_ diff --git a/s2e-ff/src/components/aocs/laser_distance_meter.cpp b/s2e-ff/src/components/aocs/laser_distance_meter.cpp index 3e39d13..b61590e 100644 --- a/s2e-ff/src/components/aocs/laser_distance_meter.cpp +++ b/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(); diff --git a/s2e-ff/src/components/aocs/laser_emitter.cpp b/s2e-ff/src/components/aocs/laser_emitter.cpp index ccaca01..1c3d8fd 100644 --- a/s2e-ff/src/components/aocs/laser_emitter.cpp +++ b/s2e-ff/src/components/aocs/laser_emitter.cpp @@ -8,10 +8,11 @@ * @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), @@ -19,7 +20,39 @@ LaserEmitter::LaserEmitter(const Dynamics& dynamics, libra::Vector<3> emitting_d 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(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; }; diff --git a/s2e-ff/src/components/aocs/laser_emitter.hpp b/s2e-ff/src/components/aocs/laser_emitter.hpp index 7a418d7..7280a58 100644 --- a/s2e-ff/src/components/aocs/laser_emitter.hpp +++ b/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 #include #include #include @@ -17,15 +18,15 @@ * @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 @@ -33,32 +34,29 @@ class LaserEmitter : public GaussianBeamBase { */ ~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_ diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index f630394..a7f239a 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -38,6 +38,7 @@ void QpdPositioningSensor::MainRoutine(int count) { is_received_laser_ = false; for (size_t laser_id = 0; laser_id < number_of_laser_emitters; laser_id++) { // Get laser information + inter_spacecraft_communication_.GetLaserEmitter(laser_id).Update(count); libra::Vector<3> laser_position_i_m = inter_spacecraft_communication_.GetLaserEmitter(laser_id).GetLaserPosition_i_m(); libra::Vector<3> laser_emitting_direction_i = inter_spacecraft_communication_.GetLaserEmitter(laser_id).GetEmittingDirection_i(); @@ -90,7 +91,7 @@ void QpdPositioningSensor::MainRoutine(int count) { std::string QpdPositioningSensor::GetLogHeader() const { std::string str_tmp = ""; - std::string head = "qpd_positioning_sensor_"; + std::string head = "qpd_positioning_sensor_" + std::to_string(qpd_positioning_sensor_id_) + "_"; str_tmp += WriteScalar(head + "is_received_laser"); str_tmp += WriteScalar(head + "distance_true[m]"); str_tmp += WriteScalar(head + "y_axis_displacement_true[m]"); @@ -144,6 +145,9 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d const double qpd_z_axis_displacement_m) { qpd_sensor_radius_m_ = (double)(((int32_t)(qpd_sensor_radius_m_ / qpd_sensor_integral_step_m_)) * qpd_sensor_integral_step_m_); const double distance_from_beam_waist_m = qpd_laser_distance_m - laser_rayleigh_length_offset_m; + double qpd_sensor_output_y_axis_V = 0.0; + double qpd_sensor_output_z_axis_V = 0.0; + double qpd_sensor_output_sum_V = 0.0; double qpd_sensor_output_derivative_y_axis_V_m = 0.0; double qpd_sensor_output_derivative_z_axis_V_m = 0.0; double qpd_sensor_output_derivative_sum_V_m = 0.0; @@ -166,16 +170,19 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d pow(laser_emitter->CalcBeamWidthRadius_m(distance_from_beam_waist_m), 2.0) * photovoltage_at_each_point; - qpd_sensor_output_y_axis_V_ += CalcSign(-y_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * photovoltage_at_each_point; - qpd_sensor_output_z_axis_V_ += CalcSign(z_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * photovoltage_at_each_point; - qpd_sensor_output_sum_V_ += photovoltage_at_each_point; + qpd_sensor_output_y_axis_V += CalcSign(-y_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * photovoltage_at_each_point; + qpd_sensor_output_z_axis_V += CalcSign(z_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * photovoltage_at_each_point; + qpd_sensor_output_sum_V += photovoltage_at_each_point; qpd_sensor_output_derivative_y_axis_V_m += CalcSign(-y_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * photovoltage_variation_at_each_point; qpd_sensor_output_derivative_z_axis_V_m += CalcSign(z_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * photovoltage_variation_at_each_point; qpd_sensor_output_derivative_sum_V_m += photovoltage_variation_at_each_point; } } - if (qpd_sensor_output_sum_V_ < qpd_sensor_output_voltage_threshold_V_) return; + qpd_sensor_output_y_axis_V_ += qpd_sensor_output_y_axis_V; + qpd_sensor_output_z_axis_V_ += qpd_sensor_output_z_axis_V; + qpd_sensor_output_sum_V_ += qpd_sensor_output_sum_V; + if (qpd_sensor_output_sum_V < qpd_sensor_output_voltage_threshold_V_) return; const double qpd_standard_deviation_y_axis_V = CalcStandardDeviation(qpd_sensor_output_derivative_y_axis_V_m, qpd_laser_distance_m); const double qpd_standard_deviation_z_axis_V = CalcStandardDeviation(qpd_sensor_output_derivative_z_axis_V_m, qpd_laser_distance_m); @@ -225,14 +232,29 @@ double QpdPositioningSensor::ObservePositionDisplacement(const double qpd_sensor const double qpd_sensor_output_sum_V, const std::vector& qpd_voltage_ratio_list) { 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_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_sensor_displacement_list_m_[i]; - observed_displacement_m += (qpd_sensor_displacement_list_m_[i + 1] - qpd_sensor_displacement_list_m_[i]) * - (sensor_value_ratio - qpd_voltage_ratio_list[i]) / (qpd_voltage_ratio_list[i + 1] - qpd_voltage_ratio_list[i]); + double sensor_value_ratio_with_polarization = qpd_sensor_output_polarization * sensor_value_ratio; + + size_t left = 0; + size_t right = qpd_voltage_ratio_list.size() - 1; + + if (sensor_value_ratio_with_polarization < qpd_sensor_output_polarization * qpd_voltage_ratio_list[left]) return observed_displacement_m; + if (sensor_value_ratio_with_polarization > qpd_sensor_output_polarization * qpd_voltage_ratio_list[right]) return observed_displacement_m; + + while (left <= right) { + size_t mid = left + (right - left) / 2; + if ((sensor_value_ratio_with_polarization == qpd_sensor_output_polarization * qpd_voltage_ratio_list[mid])) { + return qpd_sensor_displacement_list_m_[mid]; + } else if (sensor_value_ratio_with_polarization > qpd_sensor_output_polarization * qpd_voltage_ratio_list[mid]) { + left = mid + 1; + } else { + right = mid - 1; } } + + observed_displacement_m = qpd_sensor_displacement_list_m_[right]; + observed_displacement_m += (qpd_sensor_displacement_list_m_[left] - qpd_sensor_displacement_list_m_[right]) * + (sensor_value_ratio - qpd_voltage_ratio_list[right]) / (qpd_voltage_ratio_list[left] - qpd_voltage_ratio_list[right]); + return observed_displacement_m; } @@ -254,6 +276,8 @@ void QpdPositioningSensor::Initialize(const std::string file_name, const size_t qpd_sensor_voltage_ratio_z_list_.push_back(stod(qpd_sensor_voltage_ratio_str_list[index][2])); } + qpd_positioning_sensor_id_ = id; + libra::Quaternion quaternion_b2c; ini_file.ReadQuaternion(section_name.c_str(), "quaternion_b2c", quaternion_b2c); libra::Vector<3> position_b_m; diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp index 129b04f..560cff2 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp @@ -98,6 +98,8 @@ class QpdPositioningSensor : public Component, public ILoggable { std::vector qpd_sensor_voltage_ratio_z_list_; //!< List of `qpd_sensor_output_z_axis_V / qpd_sensor_output_sum_V` at each point on the z-axis. + size_t qpd_positioning_sensor_id_ = 0; + // Reference const Dynamics& dynamics_; const FfInterSpacecraftCommunication& inter_spacecraft_communication_; diff --git a/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp b/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp index 9d4cb75..8774c53 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components_2.cpp @@ -24,7 +24,7 @@ FfComponents2::FfComponents2(const Dynamics* dynamics, const Structure* structur 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)); + corner_cube_reflectors_.push_back(new CornerCubeReflector(1, clock_gen, file_name, dynamics_, id)); } inter_spacecraft_communication.SetCornerCubeReflector(corner_cube_reflectors_); @@ -33,7 +33,7 @@ FfComponents2::FfComponents2(const Dynamics* dynamics, const Structure* structur IniAccess laser_emitter_file(file_name); size_t number_of_laser_emitters = laser_emitter_file.ReadInt("GENERAL", "number_of_laser_emitters"); for (size_t id = 0; id < number_of_laser_emitters; id++) { - laser_emitters_.push_back(new LaserEmitter(InitializeLaserEmitter(file_name, *dynamics_, id))); + laser_emitters_.push_back(new LaserEmitter(InitializeLaserEmitter(1, clock_gen, file_name, *dynamics_, id))); } inter_spacecraft_communication.SetLaserEmitter(laser_emitters_);