diff --git a/s2e-ff/data/ini/Components/RelativePositionSensor.ini b/s2e-ff/data/ini/Components/RelativePositionSensor.ini index a857f599..62bd9243 100644 --- a/s2e-ff/data/ini/Components/RelativePositionSensor.ini +++ b/s2e-ff/data/ini/Components/RelativePositionSensor.ini @@ -1,12 +1,21 @@ [RelativePositionSensor] +// Target satellite ID target_sat_id = 1 + +// Normally, reference_sat_id is set the id of mounted satellite reference_sat_id = 0 +// Users can choose the frame for error settings +// INERTIAL, RTN, or BODY +error_frame = BODY + [ComponentBase] // Prescaler with respect to the component update period prescaler = 1 [SensorBase] +// The coordinate of the error is selected by the error_frame +// Scale factor [-] scale_factor_c(0) = 1; scale_factor_c(1) = 0; scale_factor_c(2) = 0; diff --git a/s2e-ff/src/Components/AOCS/InitializeRelativePositionSensor.cpp b/s2e-ff/src/Components/AOCS/InitializeRelativePositionSensor.cpp index a0738270..e7161868 100644 --- a/s2e-ff/src/Components/AOCS/InitializeRelativePositionSensor.cpp +++ b/s2e-ff/src/Components/AOCS/InitializeRelativePositionSensor.cpp @@ -20,8 +20,21 @@ RelativePositionSensor InitializeRelativePositionSensor(ClockGenerator* clock_ge char section[30] = "RelativePositionSensor"; int target_sat_id = ini_file.ReadInt(section, "target_sat_id"); int reference_sat_id = ini_file.ReadInt(section, "reference_sat_id"); - - RelativePositionSensor relative_position_sensor(prescaler, clock_gen, sensor_base, target_sat_id, reference_sat_id, rel_info, dynamics); + std::string error_frame_string = ini_file.ReadString(section, "error_frame"); + RelativePositionSensorErrorFrame error_frame; + + if (error_frame_string == "INERTIAL") { + error_frame = RelativePositionSensorErrorFrame::INERTIAL; + } else if (error_frame_string == "RTN") { + error_frame = RelativePositionSensorErrorFrame::RTN; + } else if (error_frame_string == "BODY") { + error_frame = RelativePositionSensorErrorFrame::BODY; + } else { + std::cerr << "Warnings: InitializeRelativePositionSensor: The error frame setting was failed. It is automatically set as BODY frame." << std::endl; + error_frame = RelativePositionSensorErrorFrame::BODY; + } + + RelativePositionSensor relative_position_sensor(prescaler, clock_gen, sensor_base, target_sat_id, reference_sat_id, error_frame, rel_info, dynamics); return relative_position_sensor; } diff --git a/s2e-ff/src/Components/AOCS/RelativePositionSensor.cpp b/s2e-ff/src/Components/AOCS/RelativePositionSensor.cpp index 5b00ed0a..9b7ceb76 100644 --- a/s2e-ff/src/Components/AOCS/RelativePositionSensor.cpp +++ b/s2e-ff/src/Components/AOCS/RelativePositionSensor.cpp @@ -1,11 +1,13 @@ #include "RelativePositionSensor.hpp" RelativePositionSensor::RelativePositionSensor(const int prescaler, ClockGenerator* clock_gen, SensorBase& sensor_base, const int target_sat_id, - const int reference_sat_id, const RelativeInformation& rel_info, const Dynamics& dynamics) + const int reference_sat_id, const RelativePositionSensorErrorFrame error_frame, + const RelativeInformation& rel_info, const Dynamics& dynamics) : ComponentBase(prescaler, clock_gen), SensorBase(sensor_base), target_sat_id_(target_sat_id), reference_sat_id_(reference_sat_id), + error_frame_(error_frame), rel_info_(rel_info), dynamics_(dynamics) {} @@ -14,11 +16,37 @@ RelativePositionSensor::~RelativePositionSensor() {} void RelativePositionSensor::MainRoutine(int count) { UNUSED(count); + // Get true value measured_target_position_i_m_ = rel_info_.GetRelativePosition_i_m(target_sat_id_, reference_sat_id_); measured_target_position_rtn_m_ = rel_info_.GetRelativePosition_rtn_m(target_sat_id_, reference_sat_id_); - libra::Quaternion q_i2b = dynamics_.GetAttitude().GetQuaternion_i2b(); measured_target_position_body_m_ = q_i2b.frame_conv(measured_target_position_i_m_); + + // Add noise at body frame and frame conversion + libra::Quaternion q_i2rtn = dynamics_.GetOrbit().CalcQuaternionI2LVLH(); + switch (error_frame_) + { + case RelativePositionSensorErrorFrame::INERTIAL : + measured_target_position_i_m_ = Measure(measured_target_position_i_m_); + // Frame conversion + measured_target_position_rtn_m_ = q_i2rtn.frame_conv(measured_target_position_i_m_); + measured_target_position_body_m_ = q_i2b.frame_conv(measured_target_position_i_m_); + break; + case RelativePositionSensorErrorFrame::RTN : + measured_target_position_rtn_m_ = Measure(measured_target_position_rtn_m_); + // Frame conversion + measured_target_position_i_m_ = q_i2rtn.frame_conv_inv(measured_target_position_rtn_m_); + measured_target_position_body_m_ = q_i2b.frame_conv(measured_target_position_i_m_); + break; + case RelativePositionSensorErrorFrame::BODY : + measured_target_position_body_m_ = Measure(measured_target_position_body_m_); + // Frame conversion + measured_target_position_i_m_ = q_i2b.frame_conv_inv(measured_target_position_body_m_); + measured_target_position_rtn_m_ = q_i2rtn.frame_conv(measured_target_position_i_m_); + break; + default: + break; + } } std::string RelativePositionSensor::GetLogHeader() const { diff --git a/s2e-ff/src/Components/AOCS/RelativePositionSensor.hpp b/s2e-ff/src/Components/AOCS/RelativePositionSensor.hpp index 1cbc51f8..dbcf21f3 100644 --- a/s2e-ff/src/Components/AOCS/RelativePositionSensor.hpp +++ b/s2e-ff/src/Components/AOCS/RelativePositionSensor.hpp @@ -6,10 +6,16 @@ #include #include +enum class RelativePositionSensorErrorFrame { + INERTIAL, + RTN, + BODY +}; + class RelativePositionSensor : public ComponentBase, public SensorBase<3>, public ILoggable { public: RelativePositionSensor(const int prescaler, ClockGenerator* clock_gen, SensorBase& sensor_base, const int target_sat_id, const int reference_sat_id, - const RelativeInformation& rel_info, const Dynamics& dynamics); + const RelativePositionSensorErrorFrame error_frame, const RelativeInformation& rel_info, const Dynamics& dynamics); ~RelativePositionSensor(); // ComponentBase void MainRoutine(int count) override; @@ -29,6 +35,7 @@ class RelativePositionSensor : public ComponentBase, public SensorBase<3>, publi protected: int target_sat_id_; const int reference_sat_id_; + RelativePositionSensorErrorFrame error_frame_; libra::Vector<3> measured_target_position_i_m_{0.0}; libra::Vector<3> measured_target_position_rtn_m_{0.0};