From 4ddda323c9d184f5de2de2670b1aa862f32ac946 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Dec 2023 23:26:25 +0100 Subject: [PATCH] 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_;