From d1f50ecbe20dfd89935174f22f4314870e96108e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 22 Jul 2022 16:15:19 +0200 Subject: [PATCH 1/8] Add force generator without algorithm --- s2e-ff/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 771db2fd..246902ca 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -77,6 +77,7 @@ add_subdirectory(${S2E_CORE_DIR}/src/Simulation S2E_CORE/Simulation) set(SOURCE_FILES src/S2eFf.cpp + src/Components/IdealComponents/ForceGenerator.cpp src/Simulation/Case/FfCase.cpp src/Simulation/Spacecraft/FfSat.cpp src/Simulation/Spacecraft/FfComponents.cpp From 1bd6ba314e01e6407a1642a8a3fc555cf5bfe978 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 22 Jul 2022 16:24:49 +0200 Subject: [PATCH 2/8] Add force generator --- .../IdealComponents/ForceGenerator.cpp | 49 +++++++++++++++++++ .../IdealComponents/ForceGenerator.hpp | 39 +++++++++++++++ 2 files changed, 88 insertions(+) create mode 100644 s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp create mode 100644 s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp diff --git a/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp new file mode 100644 index 00000000..98f4aa1c --- /dev/null +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp @@ -0,0 +1,49 @@ +#include "ForceGenerator.hpp" + +// Constructor +ForceGenerator::ForceGenerator(const int prescaler, ClockGenerator* clock_gen, const Dynamics* dynamics) + : ComponentBase(prescaler, clock_gen), dynamics_(dynamics) {} + +ForceGenerator::~ForceGenerator() {} + +void ForceGenerator::MainRoutine(int count) { + UNUSED(count); + + generated_force_b_N_ = ordered_force_b_N_; + // TODO: Add noise + + 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::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 + "generated_force", "b", "N", 3); + + return str_tmp; +} + +std::string ForceGenerator::GetLogValue() const { + std::string str_tmp = ""; + + str_tmp += WriteVector(generated_force_b_N_); + + return str_tmp; +} diff --git a/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp b/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp new file mode 100644 index 00000000..4c4dc6dd --- /dev/null +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include + +#include + +class ForceGenerator : public ComponentBase, public ILoggable { + public: + ForceGenerator(const int prescaler, ClockGenerator* clock_gen, const Dynamics* dynamics); + ~ForceGenerator(); + + // ComponentBase override function + void MainRoutine(int count); + // TODO: add power off routines + + // 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}; + + const Dynamics* dynamics_; +}; From 3c0dc33607b8e3431bd5b083553b47f44b700c4b Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Jul 2022 10:33:59 +0200 Subject: [PATCH 3/8] update core --- s2e-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 7a051b5e6c11528f75ff259f17e164299990050d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Jul 2022 10:37:30 +0200 Subject: [PATCH 4/8] Add PowerOffRoutine --- s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp | 6 ++++++ s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp index 98f4aa1c..47f5c50c 100644 --- a/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp @@ -18,6 +18,12 @@ void ForceGenerator::MainRoutine(int count) { 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); diff --git a/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp b/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp index 4c4dc6dd..5eda7762 100644 --- a/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp @@ -13,7 +13,7 @@ class ForceGenerator : public ComponentBase, public ILoggable { // ComponentBase override function void MainRoutine(int count); - // TODO: add power off routines + void PowerOffRoutine(); // ILogabble override function virtual std::string GetLogHeader() const; From 566190207116b4412a0bb2e3a8ebe9e6c36e881e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Jul 2022 13:44:27 +0200 Subject: [PATCH 5/8] Add initialize file --- s2e-ff/data/ini/Components/ForceGenerator.ini | 10 ++++++ .../IdealComponents/ForceGenerator.cpp | 31 +++++++++++++++++-- .../IdealComponents/ForceGenerator.hpp | 9 +++++- 3 files changed, 47 insertions(+), 3 deletions(-) create mode 100644 s2e-ff/data/ini/Components/ForceGenerator.ini 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/src/Components/IdealComponents/ForceGenerator.cpp b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp index 47f5c50c..63ca88c0 100644 --- a/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp @@ -1,8 +1,14 @@ #include "ForceGenerator.hpp" +#include + // Constructor -ForceGenerator::ForceGenerator(const int prescaler, ClockGenerator* clock_gen, const Dynamics* dynamics) - : ComponentBase(prescaler, clock_gen), dynamics_(dynamics) {} +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_noise_(0.0, direction_error_standard_deviation_rad), + dynamics_(dynamics) {} ForceGenerator::~ForceGenerator() {} @@ -53,3 +59,24 @@ std::string ForceGenerator::GetLogValue() const { return str_tmp; } + +libra::Quaternion GenerateDirectionNoiseQuaternion(libra::Vector<3> true_direction, const double error_standard_deviation_rad) { + libra::NormalRand normal_rand; + + libra::Vector<3> random_direction; + random_direction[0] = normal_rand; + random_direction[1] = normal_rand; + random_direction[2] = normal_rand; + 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) { + // TODO: add error handling + } + + double error_angle_rad = normal_rand * 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 index 5eda7762..5240c8f8 100644 --- a/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp @@ -4,11 +4,13 @@ #include #include +#include #include class ForceGenerator : public ComponentBase, public ILoggable { public: - ForceGenerator(const int prescaler, ClockGenerator* clock_gen, const Dynamics* dynamics); + 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 @@ -35,5 +37,10 @@ class ForceGenerator : public ComponentBase, public ILoggable { 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_; + libra::Quaternion GenerateDirectionNoiseQuaternion(); + const Dynamics* dynamics_; }; From 4ebbfa7da858a56996635c62b4c828a41bc85bb0 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Jul 2022 14:21:33 +0200 Subject: [PATCH 6/8] Add initialize files --- s2e-ff/CMakeLists.txt | 1 + .../IdealComponents/ForceGenerator.cpp | 14 ++++++++++--- .../IdealComponents/ForceGenerator.hpp | 6 ++++-- .../InitializeForceGenerator.cpp | 21 +++++++++++++++++++ .../InitializeForceGenerator.hpp | 5 +++++ 5 files changed, 42 insertions(+), 5 deletions(-) create mode 100644 s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp create mode 100644 s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.hpp diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 6814f7be..f42f8d0a 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -83,6 +83,7 @@ set(SOURCE_FILES 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/src/Components/IdealComponents/ForceGenerator.cpp b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp index 63ca88c0..875ff661 100644 --- a/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp @@ -7,7 +7,7 @@ ForceGenerator::ForceGenerator(const int prescaler, ClockGenerator* clock_gen, c const double direction_error_standard_deviation_rad, const Dynamics* dynamics) : ComponentBase(prescaler, clock_gen), magnitude_noise_(0.0, magnitude_error_standard_deviation_N), - direction_noise_(0.0, direction_error_standard_deviation_rad), + direction_error_standard_deviation_rad_(direction_error_standard_deviation_rad), dynamics_(dynamics) {} ForceGenerator::~ForceGenerator() {} @@ -16,8 +16,15 @@ void ForceGenerator::MainRoutine(int count) { UNUSED(count); generated_force_b_N_ = ordered_force_b_N_; - // TODO: Add noise + // Add noise + 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(generated_force_b_N_) + 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_); @@ -73,7 +80,8 @@ libra::Quaternion GenerateDirectionNoiseQuaternion(libra::Vector<3> true_directi rotation_axis = outer_product(true_direction, random_direction); double norm_rotation_axis = norm(rotation_axis); if (norm_rotation_axis < 0.0 + DBL_EPSILON) { - // TODO: add error handling + // No rotation error if the randomized direction is parallel to the true direction + rotation_axis = true_direction; } double error_angle_rad = normal_rand * error_standard_deviation_rad; diff --git a/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp b/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp index 5240c8f8..64094fc0 100644 --- a/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp @@ -39,8 +39,10 @@ class ForceGenerator : public ComponentBase, public ILoggable { // Noise libra::NormalRand magnitude_noise_; - libra::NormalRand direction_noise_; - libra::Quaternion GenerateDirectionNoiseQuaternion(); + double direction_error_standard_deviation_rad_; const Dynamics* dynamics_; }; + +// TODO: move to core +libra::Quaternion GenerateDirectionNoiseQuaternion(libra::Vector<3> true_direction, const double error_standard_deviation_rad); diff --git a/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp b/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp new file mode 100644 index 00000000..3caea87e --- /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.ReadInt(section, "force_magnitude_standard_deviation_N"); + double force_direction_standard_deviation_deg = ini_file.ReadInt(section, "force_direction_standard_deviation_deg"); + + ForceGenerator force_generator(prescaler, clock_gen, force_magnitude_standard_deviation_N, force_direction_standard_deviation_deg, 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); From b1000037b45374a49873cbc13eea0bb4183ed54c Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Jul 2022 14:28:40 +0200 Subject: [PATCH 7/8] Add component to use --- s2e-ff/data/ini/FfSat.ini | 1 + .../Components/IdealComponents/ForceGenerator.cpp | 6 ++++++ s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp | 12 ++++++++++-- s2e-ff/src/Simulation/Spacecraft/FfComponents.hpp | 2 ++ 4 files changed, 19 insertions(+), 2 deletions(-) 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 index 875ff661..8292cbf0 100644 --- a/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp @@ -54,7 +54,10 @@ 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; } @@ -62,7 +65,10 @@ std::string ForceGenerator::GetLogHeader() const { 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; } diff --git a/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp b/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp index cc6d23b9..29def817 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,21 @@ 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, rel_dist_file, dynamics_)); } 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 +41,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_; From 340dbcd79601f2d4e8fcac2305a24d4f0b69d162 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 25 Jul 2022 15:19:33 +0200 Subject: [PATCH 8/8] Debug noise --- .../IdealComponents/ForceGenerator.cpp | 32 +++++++++++-------- .../IdealComponents/ForceGenerator.hpp | 5 ++- .../InitializeForceGenerator.cpp | 8 ++--- .../Simulation/Spacecraft/FfComponents.cpp | 9 +++++- 4 files changed, 32 insertions(+), 22 deletions(-) diff --git a/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp index 8292cbf0..a85bc980 100644 --- a/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp @@ -8,7 +8,9 @@ ForceGenerator::ForceGenerator(const int prescaler, ClockGenerator* clock_gen, c : 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) {} + dynamics_(dynamics) { + direction_noise_.set_param(0.0, 1.0); +} ForceGenerator::~ForceGenerator() {} @@ -18,11 +20,15 @@ void ForceGenerator::MainRoutine(int count) { generated_force_b_N_ = ordered_force_b_N_; // Add noise - 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(generated_force_b_N_) + magnitude_noise_; - generated_force_b_N_ = force_norm_with_error * converted_direction; + 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(); @@ -53,7 +59,7 @@ void ForceGenerator::SetForce_rtn_N(const libra::Vector<3> force_rtn_N) { std::string ForceGenerator::GetLogHeader() const { std::string str_tmp = ""; - std::string head = "IdealForceGenerator"; + 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); @@ -73,13 +79,11 @@ std::string ForceGenerator::GetLogValue() const { return str_tmp; } -libra::Quaternion GenerateDirectionNoiseQuaternion(libra::Vector<3> true_direction, const double error_standard_deviation_rad) { - libra::NormalRand normal_rand; - +libra::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(libra::Vector<3> true_direction, const double error_standard_deviation_rad) { libra::Vector<3> random_direction; - random_direction[0] = normal_rand; - random_direction[1] = normal_rand; - random_direction[2] = normal_rand; + 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; @@ -90,7 +94,7 @@ libra::Quaternion GenerateDirectionNoiseQuaternion(libra::Vector<3> true_directi rotation_axis = true_direction; } - double error_angle_rad = normal_rand * error_standard_deviation_rad; + 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 index 64094fc0..d5976ef6 100644 --- a/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp +++ b/s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp @@ -39,10 +39,9 @@ class ForceGenerator : public ComponentBase, public ILoggable { // 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_; }; - -// TODO: move to core -libra::Quaternion GenerateDirectionNoiseQuaternion(libra::Vector<3> true_direction, const double error_standard_deviation_rad); diff --git a/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp b/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp index 3caea87e..a901ff53 100644 --- a/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp +++ b/s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp @@ -12,10 +12,10 @@ ForceGenerator InitializeForceGenerator(ClockGenerator* clock_gen, const std::st // ForceGenerator char section[30] = "ForceGenerator"; - double force_magnitude_standard_deviation_N = ini_file.ReadInt(section, "force_magnitude_standard_deviation_N"); - double force_direction_standard_deviation_deg = ini_file.ReadInt(section, "force_direction_standard_deviation_deg"); - - ForceGenerator force_generator(prescaler, clock_gen, force_magnitude_standard_deviation_N, force_direction_standard_deviation_deg, dynamics); + 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/Simulation/Spacecraft/FfComponents.cpp b/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp index 29def817..77d7cfb9 100644 --- a/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp +++ b/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp @@ -19,7 +19,14 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, 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, rel_dist_file, dynamics_)); + 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() {