Merge pull request #18 from ut-issl/feature/add-force-generator

 Add force generator

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

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

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

s2e-ff/src/Components/IdealComponents/ForceGenerator.cpp

@@ -0,0 +1,100 @@
+#include "ForceGenerator.hpp"
+
+#include <cfloat>
+
+// 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;
+}

s2e-ff/src/Components/IdealComponents/ForceGenerator.hpp

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <Component/Abstract/ComponentBase.h>
+#include <Dynamics/Dynamics.h>
+#include <Interface/LogOutput/Logger.h>
+
+#include <Library/math/NormalRand.hpp>
+#include <Library/math/Vector.hpp>
+
+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_;
+};

s2e-ff/src/Components/IdealComponents/InitializeForceGenerator.cpp

@@ -0,0 +1,21 @@
+#include "InitializeForceGenerator.hpp"
+
+#include <Interface/InitInput/IniAccess.h>
+
+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;
+}

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);

s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp

@@ -3,6 +3,7 @@
 #include <Interface/InitInput/IniAccess.h>
 
 #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_);
+}

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_;