From 388fdd4a85da48b16dea9b3869daecf9184f4c7d Mon Sep 17 00:00:00 2001 From: Tomoki Date: Sat, 6 Jan 2024 10:34:15 +0900 Subject: [PATCH] rename sensor --- s2e-ff/CMakeLists.txt | 2 +- ..._sensor.ini => qpd_positioning_sensor.ini} | 4 +- s2e-ff/data/initialize_files/ff_satellite.ini | 2 +- ..._sensor.cpp => qpd_positioning_sensor.cpp} | 49 +++++++++---------- ..._sensor.hpp => qpd_positioning_sensor.hpp} | 32 ++++++------ .../simulation/spacecraft/ff_components.cpp | 8 +-- .../simulation/spacecraft/ff_components.hpp | 4 +- 7 files changed, 49 insertions(+), 52 deletions(-) rename s2e-ff/data/initialize_files/components/{quadrant_photodiode_sensor.ini => qpd_positioning_sensor.ini} (94%) rename s2e-ff/src/components/aocs/{quadrant_photodiode_sensor.cpp => qpd_positioning_sensor.cpp} (81%) rename s2e-ff/src/components/aocs/{quadrant_photodiode_sensor.hpp => qpd_positioning_sensor.hpp} (82%) diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 220423a7..a9a4a5ec 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -54,7 +54,7 @@ set(SOURCE_FILES src/components/aocs/initialize_relative_velocity_sensor.cpp src/components/aocs/laser_distance_meter.cpp src/components/aocs/laser_emitter.cpp - src/components/aocs/quadrant_photodiode_sensor.cpp + src/components/aocs/qpd_positioning_sensor.cpp src/components/ideal/relative_attitude_controller.cpp src/components/ideal/initialize_relative_attitude_controller.cpp src/library/math/dual_quaternion.cpp diff --git a/s2e-ff/data/initialize_files/components/quadrant_photodiode_sensor.ini b/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini similarity index 94% rename from s2e-ff/data/initialize_files/components/quadrant_photodiode_sensor.ini rename to s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini index ba091733..cf127a00 100644 --- a/s2e-ff/data/initialize_files/components/quadrant_photodiode_sensor.ini +++ b/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini @@ -1,4 +1,4 @@ -[QUADRANT_PHOTODIODE_SENSOR_0] +[QPD_POSITIONING_SENSOR_0] // This quadrant photodiode sensor is a sensor that observes position displacement on // the y-z plane in the component coordinate system. // The x-axis is the direction normal to the plane of sensitivity of the quadrant photodiode sensor. @@ -24,6 +24,6 @@ qpd_sensor_file_directory = ../../data/initialize_files/components/qpd_sensor_cs qpd_sensor_radius_m = 3.9e-3 qpd_sensor_integral_step_m = 5.0e-5 -qpd_sensor_position_determination_threshold_m = 5.0e-3 +qpd_positioning_threshold_m = 5.0e-3 qpd_laser_receivable_angle_rad = 0.785 qpd_sensor_output_voltage_threshold_V = 0.4 diff --git a/s2e-ff/data/initialize_files/ff_satellite.ini b/s2e-ff/data/initialize_files/ff_satellite.ini index d0189d4e..76ed623e 100644 --- a/s2e-ff/data/initialize_files/ff_satellite.ini +++ b/s2e-ff/data/initialize_files/ff_satellite.ini @@ -138,6 +138,6 @@ relative_position_sensor_file = ../../data/initialize_files/components/relative_ relative_attitude_sensor_file = ../../data/initialize_files/components/relative_attitude_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 -quadrant_photodiode_sensor_file = ../../data/initialize_files/components/quadrant_photodiode_sensor.ini +qpd_positioning_sensor_file = ../../data/initialize_files/components/qpd_positioning_sensor.ini force_generator_file = ../../data/initialize_files/components/force_generator.ini relative_attitude_controller_file = ../../data/initialize_files/components/relative_attitude_controller.ini diff --git a/s2e-ff/src/components/aocs/quadrant_photodiode_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp similarity index 81% rename from s2e-ff/src/components/aocs/quadrant_photodiode_sensor.cpp rename to s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index 690d7e30..9c5c03c1 100644 --- a/s2e-ff/src/components/aocs/quadrant_photodiode_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -1,18 +1,17 @@ /** - * @file quadrant_photodiode_sensor.cpp + * @file qpd_positioning_sensor.cpp * @brief Quadrant photodiode (QPD) sensor */ -#include "quadrant_photodiode_sensor.hpp" +#include "./qpd_positioning_sensor.hpp" -QuadrantPhotodiodeSensor::QuadrantPhotodiodeSensor(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, - const Dynamics& dynamics, const FfInterSpacecraftCommunication& inter_spacecraft_communication, - const size_t id) +QpdPositioningSensor::QpdPositioningSensor(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 QuadrantPhotodiodeSensor::MainRoutine(int count) { +void QpdPositioningSensor::MainRoutine(int count) { if (count < 10) return; // Body -> Inertial frame @@ -93,9 +92,9 @@ void QuadrantPhotodiodeSensor::MainRoutine(int count) { } } -std::string QuadrantPhotodiodeSensor::GetLogHeader() const { +std::string QpdPositioningSensor::GetLogHeader() const { std::string str_tmp = ""; - std::string head = "quadrant_photodiode_sensor_"; + std::string head = "qpd_positioning_sensor_"; str_tmp += WriteScalar(head + "is_received_laser"); str_tmp += WriteScalar(head + "distance_true[m]"); str_tmp += WriteScalar(head + "y_axis_displacement_true[m]"); @@ -109,7 +108,7 @@ std::string QuadrantPhotodiodeSensor::GetLogHeader() const { return str_tmp; } -std::string QuadrantPhotodiodeSensor::GetLogValue() const { +std::string QpdPositioningSensor::GetLogValue() const { std::string str_tmp = ""; str_tmp += WriteScalar(is_received_laser_); @@ -125,9 +124,9 @@ std::string QuadrantPhotodiodeSensor::GetLogValue() const { return str_tmp; } -libra::Vector<3> QuadrantPhotodiodeSensor::CalcLaserReceivedPosition(const libra::Vector<3> point_position, const libra::Vector<3> origin_position, - const libra::Vector<3> plane_normal_direction, - const libra::Vector<3> point_line_direction) { +libra::Vector<3> QpdPositioningSensor::CalcLaserReceivedPosition(const libra::Vector<3> point_position, const libra::Vector<3> origin_position, + const libra::Vector<3> plane_normal_direction, + const libra::Vector<3> point_line_direction) { libra::Vector<3> q_p = point_position - origin_position; double temp1 = libra::InnerProduct(q_p, plane_normal_direction) / pow(plane_normal_direction.CalcNorm(), 2.0); libra::Vector<3> position_temp1 = q_p - temp1 * plane_normal_direction; @@ -137,15 +136,15 @@ libra::Vector<3> QuadrantPhotodiodeSensor::CalcLaserReceivedPosition(const libra return position; } -double QuadrantPhotodiodeSensor::CalcDisplacement(const libra::Vector<3> point_position, const libra::Vector<3> origin_position, - const libra::Vector<3> displacement_direction) { +double QpdPositioningSensor::CalcDisplacement(const libra::Vector<3> point_position, const libra::Vector<3> origin_position, + const libra::Vector<3> displacement_direction) { libra::Vector<3> q_p = point_position - origin_position; double displacement_m = libra::InnerProduct(q_p, displacement_direction) / pow(displacement_direction.CalcNorm(), 2.0); return displacement_m; }; -void QuadrantPhotodiodeSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const double distance_from_beam_waist_m, - const double qpd_y_axis_displacement_m, const double qpd_z_axis_displacement_m) { +void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const double distance_from_beam_waist_m, + const double qpd_y_axis_displacement_m, 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_); for (size_t horizontal_step = 0; horizontal_step <= (size_t)(qpd_sensor_radius_m_ / qpd_sensor_integral_step_m_) * 2; horizontal_step++) { double horizontal_pos_m = qpd_sensor_integral_step_m_ * horizontal_step - qpd_sensor_radius_m_; @@ -166,7 +165,7 @@ void QuadrantPhotodiodeSensor::CalcSensorOutput(LaserEmitter* laser_emitter, con } } -double QuadrantPhotodiodeSensor::CalcSign(const double input_value, const double threshold) { +double QpdPositioningSensor::CalcSign(const double input_value, const double threshold) { if (input_value < -threshold) { return -1; } else if (input_value > threshold) { @@ -175,12 +174,10 @@ double QuadrantPhotodiodeSensor::CalcSign(const double input_value, const double return 0.0; } -double QuadrantPhotodiodeSensor::ObservePositionDisplacement(const QpdPositionDeterminationDirection determination_direction, - const double qpd_sensor_output_V, const double qpd_sensor_output_sum_V, - const std::vector& qpd_ratio_reference_list) { - double qpd_sensor_output_polarization = (determination_direction == kYAxisDirection) ? -1.0 : 1.0; - double observed_displacement_m = - qpd_sensor_output_polarization * CalcSign(qpd_sensor_output_y_axis_V_, 0.0) * qpd_sensor_position_determination_threshold_m_; +double QpdPositioningSensor::ObservePositionDisplacement(const QpdObservedPositionDirection observation_direction, const double qpd_sensor_output_V, + const double qpd_sensor_output_sum_V, const std::vector& qpd_ratio_reference_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 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]) && @@ -194,9 +191,9 @@ double QuadrantPhotodiodeSensor::ObservePositionDisplacement(const QpdPositionDe } // Functions -void QuadrantPhotodiodeSensor::Initialize(const std::string file_name, const size_t id) { +void QpdPositioningSensor::Initialize(const std::string file_name, const size_t id) { IniAccess ini_file(file_name); - std::string name = "QUADRANT_PHOTODIODE_SENSOR_"; + std::string name = "QPD_POSITIONING_SENSOR_"; const std::string section_name = name + std::to_string(static_cast(id)); std::string file_path = ini_file.ReadString(section_name.c_str(), "qpd_sensor_file_directory"); @@ -219,7 +216,7 @@ void QuadrantPhotodiodeSensor::Initialize(const std::string file_name, const siz qpd_sensor_radius_m_ = ini_file.ReadDouble(section_name.c_str(), "qpd_sensor_radius_m"); qpd_sensor_integral_step_m_ = ini_file.ReadDouble(section_name.c_str(), "qpd_sensor_integral_step_m"); - qpd_sensor_position_determination_threshold_m_ = ini_file.ReadDouble(section_name.c_str(), "qpd_sensor_position_determination_threshold_m"); + qpd_positioning_threshold_m_ = ini_file.ReadDouble(section_name.c_str(), "qpd_positioning_threshold_m"); qpd_laser_receivable_angle_rad_ = ini_file.ReadDouble(section_name.c_str(), "qpd_laser_receivable_angle_rad"); qpd_sensor_output_voltage_threshold_V_ = ini_file.ReadDouble(section_name.c_str(), "qpd_sensor_output_voltage_threshold_V"); diff --git a/s2e-ff/src/components/aocs/quadrant_photodiode_sensor.hpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp similarity index 82% rename from s2e-ff/src/components/aocs/quadrant_photodiode_sensor.hpp rename to s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp index 25445a4f..d7d40d45 100644 --- a/s2e-ff/src/components/aocs/quadrant_photodiode_sensor.hpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp @@ -1,10 +1,10 @@ /** - * @file quadrant_photodiode_sensor.hpp - * @brief Quadrant photodiode (QPD) sensor + * @file qpd_positioning_sensor.hpp + * @brief Quadrant photodiode (QPD) positioning system */ -#ifndef S2E_COMPONENTS_QUADRANT_PHOTODIODE_SENSOR_HPP_ -#define S2E_COMPONENTS_QUADRANT_PHOTODIODE_SENSOR_HPP_ +#ifndef S2E_COMPONENTS_QPD_POSITIONING_SENSOR_HPP_ +#define S2E_COMPONENTS_QPD_POSITIONING_SENSOR_HPP_ #include #include @@ -15,23 +15,23 @@ #include "../../simulation/case/ff_inter_spacecraft_communication.hpp" /** - * @class QuadrantPhotodiodeSensor + * @class QpdPositioningSensor * @brief Quadrant photodiode sensor * @note This component not only calculate the QPD sensor output values, but also calculate position displacements. */ -class QuadrantPhotodiodeSensor : public Component, public ILoggable { +class QpdPositioningSensor : public Component, public ILoggable { public: /** - * @fn QuadrantPhotodiodeSensor + * @fn QpdPositioningSensor * @brief Constructor */ - QuadrantPhotodiodeSensor(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics& dynamics, - const FfInterSpacecraftCommunication& inter_spacecraft_communication, const size_t id = 0); + QpdPositioningSensor(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics& dynamics, + const FfInterSpacecraftCommunication& inter_spacecraft_communication, const size_t id = 0); /** - * @fn ~QuadrantPhotodiodeSensor + * @fn ~QpdPositioningSensor * @brief Destructor */ - ~QuadrantPhotodiodeSensor() {} + ~QpdPositioningSensor() {} // ComponentBase override function /** @@ -59,20 +59,20 @@ class QuadrantPhotodiodeSensor : public Component, public ILoggable { inline bool GetIsReceivedLaser() const { return is_received_laser_; } /** - * @enum QpdPositionDeterminationDirection + * @enum QpdObservedPositionDirection * @brief Type of the quadrant photodiode sensor output value */ typedef enum { kYAxisDirection = 0, //!< y-axis direction kZAxisDirection, //!< z-axis direction - } QpdPositionDeterminationDirection; + } QpdObservedPositionDirection; protected: double qpd_laser_receivable_angle_rad_; //!< laser receivable half angle from the normal direction [rad] double qpd_sensor_output_voltage_threshold_V_; //!< Quadrant photodiode sensor output voltage threshold [V] double qpd_sensor_radius_m_; //!< Quadrant photodiode sensor radius [m] double qpd_sensor_integral_step_m_; //!< Integral step to calculate the output value of the quadrant photodiode sensor [m] - double qpd_sensor_position_determination_threshold_m_; //!< Position determination threshold using the quadrant photodiode sensor [m] + double qpd_positioning_threshold_m_; //!< Position determination threshold using the quadrant photodiode sensor [m] libra::TranslationFirstDualQuaternion dual_quaternion_c2b_; //!< Dual quaternion from component to body frame bool is_received_laser_ = false; //!< Flag to detect laser received @@ -105,11 +105,11 @@ class QuadrantPhotodiodeSensor : public Component, public ILoggable { void CalcSensorOutput(LaserEmitter* laser_emitter, const double distance_from_beam_waist_m, const double qpd_horizontal_displacement_m, const double qpd_vertical_displacement_m); - double ObservePositionDisplacement(const QpdPositionDeterminationDirection determination_direction, const double qpd_sensor_output_V, + double ObservePositionDisplacement(const QpdObservedPositionDirection observation_direction, const double qpd_sensor_output_V, const double qpd_sensor_output_sum_V, const std::vector& qpd_ratio_reference_list); double CalcSign(const double input_value, const double threshold); void Initialize(const std::string file_name, const size_t id = 0); }; -#endif // S2E_COMPONENTS_QUADRANT_PHOTODIODE_SENSOR_HPP_ +#endif // S2E_COMPONENTS_QPD_POSITIONING_SENSOR_HPP_ diff --git a/s2e-ff/src/simulation/spacecraft/ff_components.cpp b/s2e-ff/src/simulation/spacecraft/ff_components.cpp index 757316a3..6fdbf55c 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components.cpp @@ -41,8 +41,8 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, const std::string ldm_file = sat_file.ReadString(section_name.c_str(), "Laser_distance_meter_file"); laser_distance_meter_ = new LaserDistanceMeter(1, clock_gen, ldm_file, *dynamics_, inter_spacecraft_communication_); - const std::string qpd_file = sat_file.ReadString(section_name.c_str(), "quadrant_photodiode_sensor_file"); - quadrant_photodiode_sensor_ = new QuadrantPhotodiodeSensor(1, clock_gen, qpd_file, *dynamics_, inter_spacecraft_communication_); + const std::string qpd_file = sat_file.ReadString(section_name.c_str(), "qpd_positioning_sensor_file"); + qpd_positioning_sensor_ = new QpdPositioningSensor(1, clock_gen, qpd_file, *dynamics_, inter_spacecraft_communication_); const std::string force_generator_file = sat_file.ReadString(section_name.c_str(), "force_generator_file"); force_generator_ = new ForceGenerator(InitializeForceGenerator(clock_gen, force_generator_file, dynamics_)); @@ -67,7 +67,7 @@ FfComponents::~FfComponents() { delete force_generator_; delete relative_attitude_controller_; delete laser_distance_meter_; - delete quadrant_photodiode_sensor_; + delete qpd_positioning_sensor_; // OBC must be deleted the last since it has com ports delete obc_; } @@ -91,5 +91,5 @@ void FfComponents::LogSetup(Logger& logger) { logger.AddLogList(relative_velocity_sensor_); logger.AddLogList(force_generator_); logger.AddLogList(laser_distance_meter_); - logger.AddLogList(quadrant_photodiode_sensor_); + logger.AddLogList(qpd_positioning_sensor_); } diff --git a/s2e-ff/src/simulation/spacecraft/ff_components.hpp b/s2e-ff/src/simulation/spacecraft/ff_components.hpp index 9892392c..cb0a5b8d 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components.hpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components.hpp @@ -20,7 +20,7 @@ #include "../../components/aocs/initialize_relative_position_sensor.hpp" #include "../../components/aocs/initialize_relative_velocity_sensor.hpp" #include "../../components/aocs/laser_distance_meter.hpp" -#include "../../components/aocs/quadrant_photodiode_sensor.hpp" +#include "../../components/aocs/qpd_positioning_sensor.hpp" #include "../../components/aocs/relative_attitude_sensor.hpp" #include "../../components/ideal/initialize_relative_attitude_controller.hpp" @@ -70,7 +70,7 @@ class FfComponents : public InstalledComponents { RelativeAttitudeSensor* relative_attitude_sensor_; //!< Example of Relative attitude sensor RelativeVelocitySensor* relative_velocity_sensor_; //!< Example of Relative velocity sensor LaserDistanceMeter* laser_distance_meter_; - QuadrantPhotodiodeSensor* quadrant_photodiode_sensor_; + QpdPositioningSensor* qpd_positioning_sensor_; // Actuators ForceGenerator* force_generator_; //!< Example of force generator RelativeAttitudeController* relative_attitude_controller_; //!< Example of attitude controller