diff --git a/s2e-core b/s2e-core index a533ca33..3aab710d 160000 --- a/s2e-core +++ b/s2e-core @@ -1 +1 @@ -Subproject commit a533ca33f11465777a5aacab38f4db1ae920af80 +Subproject commit 3aab710d9d46225f736ccf53c65e4b8012360a61 diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index ecffd6c3..f42f8d0a 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -82,6 +82,8 @@ set(SOURCE_FILES src/S2eFf.cpp src/Components/AOCS/RelativeDistanceSensor.cpp src/Components/AOCS/InitializeRelativeDistanceSensor.cpp + src/Components/IdealComponents/ForceGenerator.cpp + src/Components/IdealComponents/InitializeForceGenerator.cpp src/Simulation/Case/FfCase.cpp src/Simulation/Spacecraft/FfSat.cpp src/Simulation/Spacecraft/FfComponents.cpp diff --git a/s2e-ff/data/ini/Components/ForceGenerator.ini b/s2e-ff/data/ini/Components/ForceGenerator.ini new file mode 100644 index 00000000..f942e9d7 --- /dev/null +++ b/s2e-ff/data/ini/Components/ForceGenerator.ini @@ -0,0 +1,10 @@ +[ForceGenerator] +// Standard deviation of force magnitude error [N] +force_magnitude_standard_deviation_N = 0.01 + +// Standard deviation of force direction error [deg] +force_direction_standard_deviation_deg = 1 + +[ComponentBase] +// Prescaler with respect to the component update period +prescaler = 1 diff --git a/s2e-ff/data/ini/FfSat.ini b/s2e-ff/data/ini/FfSat.ini index b8b6f4f7..ff86d876 100644 --- a/s2e-ff/data/ini/FfSat.ini +++ b/s2e-ff/data/ini/FfSat.ini @@ -139,3 +139,4 @@ structure_file = ../../data/ini/FfSatStructure.ini [COMPONENTS_FILE] // Users can add the path for component initialize files here. relative_distance_sensor_file = ../../data/ini/Components/RelativeDistanceSensor.ini +force_generator_file = ../../data/ini/Components/ForceGenerator.ini diff --git a/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp new file mode 100644 index 00000000..a85bc980 --- /dev/null +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp @@ -0,0 +1,100 @@ +#include "ForceGenerator.hpp" + +#include + +// Constructor +ForceGenerator::ForceGenerator(const int prescaler, ClockGenerator* clock_gen, const double magnitude_error_standard_deviation_N, + const double direction_error_standard_deviation_rad, const Dynamics* dynamics) + : ComponentBase(prescaler, clock_gen), + magnitude_noise_(0.0, magnitude_error_standard_deviation_N), + direction_error_standard_deviation_rad_(direction_error_standard_deviation_rad), + dynamics_(dynamics) { + direction_noise_.set_param(0.0, 1.0); +} + +ForceGenerator::~ForceGenerator() {} + +void ForceGenerator::MainRoutine(int count) { + UNUSED(count); + + generated_force_b_N_ = ordered_force_b_N_; + + // Add noise + double norm_ordered_force = norm(ordered_force_b_N_); + if (norm_ordered_force > 0.0 + DBL_EPSILON) { + // Add noise only when the force is generated + libra::Vector<3> true_direction = normalize(generated_force_b_N_); + libra::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_); + libra::Vector<3> converted_direction = error_quaternion.frame_conv(generated_force_b_N_); + double force_norm_with_error = norm_ordered_force + magnitude_noise_; + generated_force_b_N_ = force_norm_with_error * converted_direction; + } + + // Convert frame + libra::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + libra::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternionI2LVLH(); + generated_force_i_N_ = q_i2b.frame_conv_inv(generated_force_b_N_); + generated_force_rtn_N_ = q_i2rtn.frame_conv(generated_force_i_N_); +} + +void ForceGenerator::PowerOffRoutine() { + generated_force_b_N_ *= 0.0; + generated_force_i_N_ *= 0.0; + generated_force_rtn_N_ *= 0.0; +} + +void ForceGenerator::SetForce_i_N(const libra::Vector<3> force_i_N) { + libra::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + ordered_force_b_N_ = q_i2b.frame_conv(force_i_N); +} + +void ForceGenerator::SetForce_rtn_N(const libra::Vector<3> force_rtn_N) { + libra::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); + libra::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternionI2LVLH(); + + libra::Vector<3> force_i_N = q_i2rtn.frame_conv_inv(force_rtn_N); + ordered_force_b_N_ = q_i2b.frame_conv(force_i_N); +} + +std::string ForceGenerator::GetLogHeader() const { + std::string str_tmp = ""; + + std::string head = "IdealForceGenerator_"; + str_tmp += WriteVector(head + "ordered_force", "b", "N", 3); + str_tmp += WriteVector(head + "generated_force", "b", "N", 3); + str_tmp += WriteVector(head + "generated_force", "i", "N", 3); + str_tmp += WriteVector(head + "generated_force", "rtn", "N", 3); + + return str_tmp; +} + +std::string ForceGenerator::GetLogValue() const { + std::string str_tmp = ""; + + str_tmp += WriteVector(ordered_force_b_N_); + str_tmp += WriteVector(generated_force_b_N_); + str_tmp += WriteVector(generated_force_i_N_); + str_tmp += WriteVector(generated_force_rtn_N_); + + return str_tmp; +} + +libra::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(libra::Vector<3> true_direction, const double error_standard_deviation_rad) { + libra::Vector<3> random_direction; + random_direction[0] = direction_noise_; + random_direction[1] = direction_noise_; + random_direction[2] = direction_noise_; + random_direction = normalize(random_direction); + + libra::Vector<3> rotation_axis; + rotation_axis = outer_product(true_direction, random_direction); + double norm_rotation_axis = norm(rotation_axis); + if (norm_rotation_axis < 0.0 + DBL_EPSILON) { + // No rotation error if the randomized direction is parallel to the true direction + rotation_axis = true_direction; + } + + double error_angle_rad = direction_noise_ * error_standard_deviation_rad; + libra::Quaternion error_quaternion(rotation_axis, error_angle_rad); + return error_quaternion; +} diff --git a/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp b/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp new file mode 100644 index 00000000..d5976ef6 --- /dev/null +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include + +#include +#include + +class ForceGenerator : public ComponentBase, public ILoggable { + public: + ForceGenerator(const int prescaler, ClockGenerator* clock_gen, const double magnitude_error_standard_deviation_N, + const double direction_error_standard_deviation_rad, const Dynamics* dynamics); + ~ForceGenerator(); + + // ComponentBase override function + void MainRoutine(int count); + void PowerOffRoutine(); + + // ILogabble override function + virtual std::string GetLogHeader() const; + virtual std::string GetLogValue() const; + + // Getter + inline const Vector<3> GetGeneratedForce_b_N() const { return generated_force_b_N_; }; + inline const Vector<3> GetGeneratedForce_i_N() const { return generated_force_i_N_; }; + inline const Vector<3> GetGeneratedForce_rtn_N() const { return generated_force_rtn_N_; }; + + // Setter + inline void SetForce_b_N(const libra::Vector<3> force_b_N) { ordered_force_b_N_ = force_b_N; }; + void SetForce_i_N(const libra::Vector<3> force_i_N); + void SetForce_rtn_N(const libra::Vector<3> force_rtn_N); + + protected: + libra::Vector<3> ordered_force_b_N_{0.0}; + libra::Vector<3> generated_force_b_N_{0.0}; + libra::Vector<3> generated_force_i_N_{0.0}; + libra::Vector<3> generated_force_rtn_N_{0.0}; + + // Noise + libra::NormalRand magnitude_noise_; + libra::NormalRand direction_noise_; + double direction_error_standard_deviation_rad_; + libra::Quaternion GenerateDirectionNoiseQuaternion(libra::Vector<3> true_direction, const double error_standard_deviation_rad); + + const Dynamics* dynamics_; +}; diff --git a/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp b/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp new file mode 100644 index 00000000..a901ff53 --- /dev/null +++ b/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp @@ -0,0 +1,21 @@ +#include "InitializeForceGenerator.hpp" + +#include + +ForceGenerator InitializeForceGenerator(ClockGenerator* clock_gen, const std::string file_name, const Dynamics* dynamics) { + // General + IniAccess ini_file(file_name); + + // CompoBase + int prescaler = ini_file.ReadInt("ComponentBase", "prescaler"); + if (prescaler <= 1) prescaler = 1; + + // ForceGenerator + char section[30] = "ForceGenerator"; + double force_magnitude_standard_deviation_N = ini_file.ReadDouble(section, "force_magnitude_standard_deviation_N"); + double force_direction_standard_deviation_deg = ini_file.ReadDouble(section, "force_direction_standard_deviation_deg"); + double force_direction_standard_deviation_rad = libra::deg_to_rad * force_direction_standard_deviation_deg; + ForceGenerator force_generator(prescaler, clock_gen, force_magnitude_standard_deviation_N, force_direction_standard_deviation_rad, dynamics); + + return force_generator; +} \ No newline at end of file diff --git a/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.hpp b/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.hpp new file mode 100644 index 00000000..c4936cbe --- /dev/null +++ b/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include "ForceGenerator.hpp" + +ForceGenerator InitializeForceGenerator(ClockGenerator* clock_gen, const std::string file_name, const Dynamics* dynamics); diff --git a/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp b/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp index cc6d23b9..77d7cfb9 100644 --- a/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp +++ b/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp @@ -3,6 +3,7 @@ #include #include "../../Components/AOCS/InitializeRelativeDistanceSensor.hpp" +#include "../../Components/IdealComponents/InitializeForceGenerator.hpp" FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, const LocalEnvironment* local_env, const GlobalEnvironment* glo_env, const SimulationConfig* config, ClockGenerator* clock_gen, const RelativeInformation* rel_info) @@ -16,17 +17,28 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, const std::string rel_dist_file = sat_file.ReadString("COMPONENTS_FILE", "relative_distance_sensor_file"); relative_distance_sensor_ = new RelativeDistanceSensor(InitializeRelativeDistanceSensor(clock_gen, rel_dist_file, compo_step_sec, *rel_info_)); + + const std::string force_generator_file = sat_file.ReadString("COMPONENTS_FILE", "force_generator_file"); + force_generator_ = new ForceGenerator(InitializeForceGenerator(clock_gen, force_generator_file, dynamics_)); + + // Debug for actuator output + libra::Vector<3> force_N; + force_N[0] = 1.0; + force_N[1] = 0.0; + force_N[2] = 0.0; + // force_generator_->SetForce_b_N(force_N); } FfComponents::~FfComponents() { delete relative_distance_sensor_; + delete force_generator_; // OBC must be deleted the last since it has com ports delete obc_; } Vector<3> FfComponents::GenerateForce_N_b() { - // There is no orbit control component, so it remains 0 Vector<3> force_N_b_(0.0); + force_N_b_ += force_generator_->GetGeneratedForce_b_N(); return force_N_b_; } @@ -36,4 +48,7 @@ Vector<3> FfComponents::GenerateTorque_Nm_b() { return torque_Nm_b_; } -void FfComponents::LogSetup(Logger& logger) { logger.AddLoggable(relative_distance_sensor_); } +void FfComponents::LogSetup(Logger& logger) { + logger.AddLoggable(relative_distance_sensor_); + logger.AddLoggable(force_generator_); +} diff --git a/s2e-ff/src/Simulation/Spacecraft/FfComponents.hpp b/s2e-ff/src/Simulation/Spacecraft/FfComponents.hpp index 39c0c791..4f4a6cf5 100644 --- a/s2e-ff/src/Simulation/Spacecraft/FfComponents.hpp +++ b/s2e-ff/src/Simulation/Spacecraft/FfComponents.hpp @@ -9,6 +9,7 @@ // include for components #include "../../Components/AOCS/RelativeDistanceSensor.hpp" +#include "../../Components/IdealComponents/ForceGenerator.hpp" #include "OBC.h" class FfComponents : public InstalledComponents { @@ -24,6 +25,7 @@ class FfComponents : public InstalledComponents { // Components OBC* obc_; RelativeDistanceSensor* relative_distance_sensor_; + ForceGenerator* force_generator_; // References const Dynamics* dynamics_;