diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 930a95fb..30abad3a 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -49,6 +49,7 @@ set(SOURCE_FILES src/components/aocs/initialize_relative_distance_sensor.cpp src/components/aocs/relative_position_sensor.cpp src/components/aocs/initialize_relative_position_sensor.cpp + src/components/aocs/relative_attitude_sensor.cpp src/components/aocs/relative_velocity_sensor.cpp src/components/aocs/initialize_relative_velocity_sensor.cpp src/components/aocs/laser_distance_meter.cpp diff --git a/s2e-ff/data/initialize_files/components/relative_attitude_sensor.ini b/s2e-ff/data/initialize_files/components/relative_attitude_sensor.ini new file mode 100644 index 00000000..d457df0d --- /dev/null +++ b/s2e-ff/data/initialize_files/components/relative_attitude_sensor.ini @@ -0,0 +1,12 @@ +[RELATIVE_ATTITUDE_SENSOR] +// Prescaler with respect to the component update period +prescaler = 1 + +// Target satellite ID +target_sat_id = 1 + +// When this value is negative, reference_sat_id is automatically set as the mounting satellite ID +reference_sat_id = -1 + +// Standard deviation of force direction error [deg] +error_angle_standard_deviation_deg = 1.0 diff --git a/s2e-ff/data/initialize_files/ff_satellite.ini b/s2e-ff/data/initialize_files/ff_satellite.ini index 9aa01319..d6ab6d94 100644 --- a/s2e-ff/data/initialize_files/ff_satellite.ini +++ b/s2e-ff/data/initialize_files/ff_satellite.ini @@ -135,6 +135,7 @@ structure_file = ../../data/initialize_files/ff_satellite_structure.ini // Users can add the path for component initialize files here. relative_distance_sensor_file = ../../data/initialize_files/components/relative_distance_sensor.ini relative_position_sensor_file = ../../data/initialize_files/components/relative_position_sensor.ini +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 force_generator_file = ../../data/initialize_files/components/force_generator.ini diff --git a/s2e-ff/src/components/aocs/relative_attitude_sensor.cpp b/s2e-ff/src/components/aocs/relative_attitude_sensor.cpp new file mode 100644 index 00000000..7727f3b4 --- /dev/null +++ b/s2e-ff/src/components/aocs/relative_attitude_sensor.cpp @@ -0,0 +1,85 @@ +/** + * @file relative_attitude_sensor.cpp + * @brief Relative attitude sensor + */ + +#include "relative_attitude_sensor.hpp" + +#include +#include + +RelativeAttitudeSensor::RelativeAttitudeSensor(const int prescaler, ClockGenerator* clock_gen, const int target_sat_id, const int reference_sat_id, + const RelativeInformation& rel_info, const double standard_deviation_rad) + : Component(prescaler, clock_gen), + target_sat_id_(target_sat_id), + reference_sat_id_(reference_sat_id), + rel_info_(rel_info), + angle_noise_(0.0, standard_deviation_rad) { + direction_noise_.SetParameters(0.0, 1.0); +} + +RelativeAttitudeSensor::~RelativeAttitudeSensor() {} + +void RelativeAttitudeSensor::MainRoutine(int count) { + UNUSED(count); + // Error calculation + libra::Vector<3> random_direction; + random_direction[0] = direction_noise_; + random_direction[1] = direction_noise_; + random_direction[2] = direction_noise_; + random_direction = random_direction.CalcNormalizedVector(); + + double error_angle_rad = angle_noise_; + libra::Quaternion error_quaternion(random_direction, error_angle_rad); + + // Get true value + measured_quaternion_rb2tb_ = rel_info_.GetRelativeAttitudeQuaternion(target_sat_id_, reference_sat_id_); + measured_quaternion_rb2tb_ = error_quaternion * measured_quaternion_rb2tb_; + measured_euler_angle_rb2tb_rad_ = measured_quaternion_rb2tb_.ConvertToEuler(); +} + +std::string RelativeAttitudeSensor::GetLogHeader() const { + std::string str_tmp = ""; + std::string head = "RelativeAttitudeSensor_"; + + const std::string frame_name = std::to_string(reference_sat_id_) + "to" + std::to_string(target_sat_id_); + str_tmp += WriteQuaternion(head + "quaternion", frame_name); + str_tmp += WriteVector(head + "euler_angle", frame_name, "rad", 3); + + return str_tmp; +} + +std::string RelativeAttitudeSensor::GetLogValue() const { + std::string str_tmp = ""; + + str_tmp += WriteQuaternion(measured_quaternion_rb2tb_); + str_tmp += WriteVector(measured_euler_angle_rb2tb_rad_); + + return str_tmp; +} + +RelativeAttitudeSensor InitializeRelativeAttitudeSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s, + const RelativeInformation& rel_info, const int reference_sat_id_input) { + // General + IniAccess ini_file(file_name); + char section[30] = "RELATIVE_ATTITUDE_SENSOR"; + + // CompoBase + int prescaler = ini_file.ReadInt(section, "prescaler"); + if (prescaler <= 1) prescaler = 1; + + double error_angle_standard_deviation_deg = ini_file.ReadDouble(section, "error_angle_standard_deviation_deg"); + double error_angle_standard_deviation_rad = libra::deg_to_rad * error_angle_standard_deviation_deg; + + // RelativeAttitudeSensor + int target_sat_id = ini_file.ReadInt(section, "target_sat_id"); + int reference_sat_id = ini_file.ReadInt(section, "reference_sat_id"); + if (reference_sat_id < 0) { + reference_sat_id = reference_sat_id_input; + } + + RelativeAttitudeSensor relative_attitude_sensor(prescaler, clock_gen, target_sat_id, reference_sat_id, rel_info, + error_angle_standard_deviation_rad); + + return relative_attitude_sensor; +} diff --git a/s2e-ff/src/components/aocs/relative_attitude_sensor.hpp b/s2e-ff/src/components/aocs/relative_attitude_sensor.hpp new file mode 100644 index 00000000..0a2f4ce0 --- /dev/null +++ b/s2e-ff/src/components/aocs/relative_attitude_sensor.hpp @@ -0,0 +1,78 @@ +/** + * @file relative_attitude_sensor.hpp + * @brief Relative attitude sensor + */ + +#ifndef S2E_COMPONENTS_AOCS_RELATIVE_ATTITUDE_SENSOR_HPP_ +#define S2E_COMPONENTS_AOCS_RELATIVE_ATTITUDE_SENSOR_HPP_ + +#include +#include +#include +#include + +/** + * @class RelativeAttitudeSensor + * @brief Relative attitude sensor + */ +class RelativeAttitudeSensor : public Component, public ILoggable { + public: + /** + * @fn RelativeAttitudeSensor + * @brief Constructor + */ + RelativeAttitudeSensor(const int prescaler, ClockGenerator* clock_gen, const int target_sat_id, const int reference_sat_id, + const RelativeInformation& rel_info, const double standard_deviation_rad); + /** + * @fn ~RelativeAttitudeSensor + * @brief Destructor + */ + ~RelativeAttitudeSensor(); + + // ComponentBase override function + /** + * @fn MainRoutine + * @brief Main routine + */ + void MainRoutine(int count); + + // 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; + + // Getter + inline libra::Quaternion GetMeasuredQuaternion_rb2tb() const { return measured_quaternion_rb2tb_; } + inline libra::Vector<3> GetMeasuredEulerAngle_rb2tb_rad() const { return measured_euler_angle_rb2tb_rad_; } + + // Setter + void SetTargetSatId(const int target_sat_id) { target_sat_id_ = target_sat_id; } + + protected: + int target_sat_id_; //!< Target satellite ID + const int reference_sat_id_; //!< Reference satellite ID + + // Measured value + libra::Quaternion measured_quaternion_rb2tb_ = {0.0, 0.0, 0.0, 1.0}; //!< Measured quaternion of target body from reference body + libra::Vector<3> measured_euler_angle_rb2tb_rad_{ + 0.0}; //!< Measured Euler angle of target body in BODY frame [rad], 3-2-1 Euler angle (1: roll, 2: pitch, 3: yaw order) + + // Noises + libra::NormalRand angle_noise_; //!< Normal random for magnitude noise + libra::NormalRand direction_noise_; //!< Normal random for direction noise + + // References + const RelativeInformation& rel_info_; //!< Relative information +}; + +RelativeAttitudeSensor InitializeRelativeAttitudeSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s, + const RelativeInformation& rel_info, const int reference_sat_id_input = -1); + +#endif // S2E_COMPONENTS_AOCS_RELATIVE_ATTITUDE_SENSOR_HPP_ diff --git a/s2e-ff/src/simulation/spacecraft/ff_components.cpp b/s2e-ff/src/simulation/spacecraft/ff_components.cpp index ffd653d0..9589f533 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components.cpp @@ -30,6 +30,10 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, relative_position_sensor_ = new RelativePositionSensor(InitializeRelativePositionSensor(clock_gen, rel_pos_file, compo_step_sec, *rel_info_, *dynamics_, sat_id)); + const std::string rel_att_file = sat_file.ReadString(section_name.c_str(), "relative_attitude_sensor_file"); + relative_attitude_sensor_ = + new RelativeAttitudeSensor(InitializeRelativeAttitudeSensor(clock_gen, rel_att_file, compo_step_sec, *rel_info_, sat_id)); + const std::string rel_vel_file = sat_file.ReadString(section_name.c_str(), "relative_velocity_sensor_file"); relative_velocity_sensor_ = new RelativeVelocitySensor(InitializeRelativeVelocitySensor(clock_gen, rel_vel_file, compo_step_sec, *rel_info_, *dynamics_, sat_id)); @@ -55,6 +59,7 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, FfComponents::~FfComponents() { delete relative_distance_sensor_; delete relative_position_sensor_; + delete relative_attitude_sensor_; delete relative_velocity_sensor_; delete force_generator_; delete relative_attitude_controller_; @@ -78,6 +83,7 @@ Vector<3> FfComponents::GenerateTorque_b_Nm() { void FfComponents::LogSetup(Logger& logger) { logger.AddLogList(relative_distance_sensor_); logger.AddLogList(relative_position_sensor_); + logger.AddLogList(relative_attitude_sensor_); logger.AddLogList(relative_velocity_sensor_); logger.AddLogList(force_generator_); logger.AddLogList(laser_distance_meter_); diff --git a/s2e-ff/src/simulation/spacecraft/ff_components.hpp b/s2e-ff/src/simulation/spacecraft/ff_components.hpp index 3dd499ef..5f463e33 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components.hpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components.hpp @@ -20,6 +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/relative_attitude_sensor.hpp" #include "../../components/ideal/initialize_relative_attitude_controller.hpp" /** @@ -65,6 +66,7 @@ class FfComponents : public InstalledComponents { // Sensors RelativeDistanceSensor* relative_distance_sensor_; //!< Example of Relative distance sensor RelativePositionSensor* relative_position_sensor_; //!< Example of Relative position sensor + RelativeAttitudeSensor* relative_attitude_sensor_; //!< Example of Relative attitude sensor RelativeVelocitySensor* relative_velocity_sensor_; //!< Example of Relative velocity sensor LaserDistanceMeter* laser_distance_meter_; // Actuators diff --git a/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp b/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp index ade388ce..1afdc0dc 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp @@ -19,6 +19,7 @@ #include "../../components/aocs/corner_cube_reflector.hpp" #include "../../components/aocs/initialize_relative_distance_sensor.hpp" #include "../../components/aocs/initialize_relative_position_sensor.hpp" +#include "../../components/aocs/relative_attitude_sensor.hpp" #include "../../components/ideal/initialize_relative_attitude_controller.hpp" /**