Merge pull request #15 from ut-issl/feature/add-distance-sensor

Add distance sensor

s2e-ff/CMakeLists.txt

@@ -77,6 +77,8 @@ add_subdirectory(${S2E_CORE_DIR}/src/Simulation S2E_CORE/Simulation)
 
 set(SOURCE_FILES
   src/S2eFf.cpp
+  src/Components/AOCS/RelativeDistanceSensor.cpp
+  src/Components/AOCS/InitializeRelativeDistanceSensor.cpp
   src/Simulation/Case/FfCase.cpp
   src/Simulation/Spacecraft/FfSat.cpp
   src/Simulation/Spacecraft/FfComponents.cpp

s2e-ff/data/ini/Components/RelativeDistanceSensor.ini

@@ -0,0 +1,26 @@
+[RelativeDistanceSensor]
+target_sat_id = 1
+reference_sat_id = 0
+
+[ComponentBase]
+// Prescaler with respect to the component update period
+prescaler = 1
+
+[SensorBase]
+scale_factor_c(0) = 1;
+
+// Constant bias noise [m]
+constant_bias_c(0) = 10.0
+
+// Standard deviation of normal random noise [m]
+normal_random_standard_deviation_c(0) = 1.0
+
+// Standard deviation for random walk noise [m]
+random_walk_standard_deviation_c(0) = 0.0
+
+// Limit of random walk noise [m]
+random_walk_limit_c(0) = 0.0
+
+// Range [m]
+range_to_const = 1000000.0  // smaller than range_to_zero_m
+range_to_zero = 10000000.0

s2e-ff/data/ini/FfSat.ini

@@ -138,3 +138,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

s2e-ff/src/Components/AOCS/InitializeRelativeDistanceSensor.cpp

@@ -0,0 +1,27 @@
+#include "InitializeRelativeDistanceSensor.hpp"
+
+#include <Interface/InitInput/IniAccess.h>
+
+#include "../Abstract/InitializeSensorBase.hpp"
+
+RelativeDistanceSensor InitializeRelativeDistanceSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s,
+                                                        const RelativeInformation& rel_info) {
+  // General
+  IniAccess ini_file(file_name);
+
+  // CompoBase
+  int prescaler = ini_file.ReadInt("ComponentBase", "prescaler");
+  if (prescaler <= 1) prescaler = 1;
+
+  // SensorBase
+  SensorBase<1> sensor_base = ReadSensorBaseInformation<1>(file_name, compo_step_time_s * (double)(prescaler));
+
+  // RelativeDistanceSensor
+  char section[30] = "RelativeDistanceSensor";
+  int target_sat_id = ini_file.ReadInt(section, "target_sat_id");
+  int reference_sat_id = ini_file.ReadInt(section, "reference_sat_id");
+
+  RelativeDistanceSensor relative_distance_sensor(prescaler, clock_gen, sensor_base, target_sat_id, reference_sat_id, rel_info);
+
+  return relative_distance_sensor;
+}

s2e-ff/src/Components/AOCS/InitializeRelativeDistanceSensor.hpp

@@ -0,0 +1,6 @@
+#pragma once
+
+#include "RelativeDistanceSensor.hpp"
+
+RelativeDistanceSensor InitializeRelativeDistanceSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s,
+                                                        const RelativeInformation& rel_info);

s2e-ff/src/Components/AOCS/RelativeDistanceSensor.cpp

@@ -0,0 +1,31 @@
+#include "RelativeDistanceSensor.hpp"
+
+RelativeDistanceSensor::RelativeDistanceSensor(const int prescaler, ClockGenerator* clock_gen, SensorBase& sensor_base,
+                                               const int target_sat_id, const int reference_sat_id,
+                                               const RelativeInformation& rel_info)
+    : ComponentBase(prescaler, clock_gen), SensorBase(sensor_base),
+      target_sat_id_(target_sat_id), reference_sat_id_(reference_sat_id), rel_info_(rel_info) {}
+
+RelativeDistanceSensor::~RelativeDistanceSensor() {}
+
+void RelativeDistanceSensor::MainRoutine(int count) {
+  UNUSED(count);
+
+  measured_distance_bw_ref_target_m_[0] = rel_info_.GetRelativeDistance_m(target_sat_id_, reference_sat_id_);
+  measured_distance_bw_ref_target_m_ = Measure(measured_distance_bw_ref_target_m_);  // Add Noise
+}
+
+std::string RelativeDistanceSensor::GetLogHeader() const {
+  std::string str_tmp = "";
+  str_tmp += WriteScalar("relative_distance_sensor_ref_to_target", "m");
+
+  return str_tmp;
+}
+
+std::string RelativeDistanceSensor::GetLogValue() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteScalar(measured_distance_bw_ref_target_m_[0]);
+
+  return str_tmp;
+}

s2e-ff/src/Components/AOCS/RelativeDistanceSensor.hpp

@@ -0,0 +1,38 @@
+#ifndef RELATIVE_DISTANCE_SENSOR_H_
+#define RELATIVE_DISTANCE_SENSOR_H_
+
+#include <Interface/LogOutput/ILoggable.h>
+#include <RelativeInformation/RelativeInformation.h>
+
+#include <Component/Abstract/ComponentBase.h>
+#include <Component/Abstract/SensorBase.h>
+
+class RelativeDistanceSensor : public ComponentBase, public SensorBase<1>, public ILoggable {
+ public:
+  RelativeDistanceSensor(const int prescaler, ClockGenerator* clock_gen, SensorBase& sensor_base,
+                         const int target_sat_id, const int reference_sat_id,
+                         const RelativeInformation& rel_info);
+  ~RelativeDistanceSensor();
+  // ComponentBase
+  void MainRoutine(int count) override;
+
+  // ILoggable
+  virtual std::string GetLogHeader() const;
+  virtual std::string GetLogValue() const;
+
+  // Getter
+  inline double GetMeasuredDistance_m() const { return measured_distance_bw_ref_target_m_[0]; }
+
+  // Setter
+  void SetTargetSatId(const int target_sat_id) { target_sat_id_ = target_sat_id; }
+
+ protected:
+  int target_sat_id_;
+  const int reference_sat_id_;
+
+  libra::Vector<1> measured_distance_bw_ref_target_m_{0.0};
+
+  const RelativeInformation& rel_info_;
+};
+
+#endif

s2e-ff/src/Components/Abstract/InitializeSensorBase.hpp

@@ -0,0 +1,9 @@
+#pragma once
+
+#include <Component/Abstract/SensorBase.h>
+
+// TODO:これをコアに移して全体で共有したい
+template <size_t N>
+SensorBase<N> ReadSensorBaseInformation(const std::string file_name, const double step_width_s);
+
+#include "InitializeSensorBase_tfs.hpp"

s2e-ff/src/Components/Abstract/InitializeSensorBase_tfs.hpp

@@ -0,0 +1,37 @@
+
+#pragma once
+#include <Interface/InitInput/IniAccess.h>
+
+template <size_t N>
+SensorBase<N> ReadSensorBaseInformation(const std::string file_name, const double step_width_s) {
+  IniAccess ini_file(file_name);
+  char section[30] = "SensorBase";
+
+  libra::Vector<N * N> scale_factor_vector;
+  ini_file.ReadVector(section, "scale_factor_c", scale_factor_vector);
+  libra::Matrix<N, N> scale_factor_c;
+  for (size_t i = 0; i < N; i++) {
+    for (size_t j = 0; j < N; j++) {
+      scale_factor_c[i][j] = scale_factor_vector[i * N + j];
+    }
+  }
+
+  libra::Vector<N> constant_bias_c;
+  ini_file.ReadVector(section, "constant_bias_c", constant_bias_c);
+  libra::Vector<N> normal_random_standard_deviation_c;
+  ini_file.ReadVector(section, "normal_random_standard_deviation_c", normal_random_standard_deviation_c);
+  libra::Vector<N> random_walk_standard_deviation_c;
+  ini_file.ReadVector(section, "random_walk_standard_deviation_c", random_walk_standard_deviation_c);
+  libra::Vector<N> random_walk_limit_c;
+  ini_file.ReadVector(section, "random_walk_limit_c", random_walk_limit_c);
+
+  double range_to_const = ini_file.ReadDouble(section, "range_to_const");
+  libra::Vector<N> range_to_const_c{range_to_const};
+  double range_to_zero = ini_file.ReadDouble(section, "range_to_zero");
+  libra::Vector<N> range_to_zero_c{range_to_zero};
+
+  SensorBase<N> sensor_base(scale_factor_c, range_to_const_c, range_to_zero_c, constant_bias_c, normal_random_standard_deviation_c, step_width_s,
+                            random_walk_standard_deviation_c, random_walk_limit_c);
+
+  return sensor_base;
+}

s2e-ff/src/Simulation/Case/FfCase.cpp

@@ -19,6 +19,8 @@ void FfCase::Initialize() {
   for (auto& sc : satellites_) {
     sc->LogSetup(*(sim_config_.main_logger_));
   }
+  relative_information_.LogSetup(*(sim_config_.main_logger_));
+
   // Write headers to the log
   sim_config_.main_logger_->WriteHeaders();
 
@@ -40,6 +42,8 @@ void FfCase::Main() {
     for (auto& sc : satellites_) {
       sc->Update(&(glo_env_->GetSimTime()));
     }
+    // Relative Information
+    relative_information_.Update();
     // Debug output
     if (glo_env_->GetSimTime().GetState().disp_output) {
       std::cout << "Progresss: " << glo_env_->GetSimTime().GetProgressionRate() << "%\r";

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

@@ -2,13 +2,24 @@
 
 #include <Interface/InitInput/IniAccess.h>
 
+#include "../../Components/AOCS/InitializeRelativeDistanceSensor.hpp"
+
 FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, const LocalEnvironment* local_env, const GlobalEnvironment* glo_env,
-                           const SimulationConfig* config, ClockGenerator* clock_gen)
-    : dynamics_(dynamics), structure_(structure), local_env_(local_env), glo_env_(glo_env), config_(config) {
+                           const SimulationConfig* config, ClockGenerator* clock_gen, const RelativeInformation* rel_info)
+    : dynamics_(dynamics), structure_(structure), local_env_(local_env), glo_env_(glo_env), config_(config), rel_info_(rel_info) {
+  // General
+  IniAccess sat_file = IniAccess(config->sat_file_[0]);
+  double compo_step_sec = glo_env_->GetSimTime().GetCompoStepSec();
+
+  // Component Instantiation
   obc_ = new OBC(clock_gen);
+
+  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_));
 }
 
 FfComponents::~FfComponents() {
+  delete relative_distance_sensor_;
   // OBC must be deleted the last since it has com ports
   delete obc_;
 }
@@ -25,7 +36,4 @@ Vector<3> FfComponents::GenerateTorque_Nm_b() {
   return torque_Nm_b_;
 }
 
-void FfComponents::LogSetup(Logger& logger) {
-  // Users can set log output when they need component log
-  UNUSED(logger);
-}
+void FfComponents::LogSetup(Logger& logger) { logger.AddLoggable(relative_distance_sensor_); }

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

@@ -8,12 +8,13 @@
 #include "Vector.hpp"
 
 // include for components
+#include "../../Components/AOCS/RelativeDistanceSensor.hpp"
 #include "OBC.h"
 
 class FfComponents : public InstalledComponents {
  public:
   FfComponents(const Dynamics* dynamics, const Structure* structure, const LocalEnvironment* local_env, const GlobalEnvironment* glo_env,
-               const SimulationConfig* config, ClockGenerator* clock_gen);
+               const SimulationConfig* config, ClockGenerator* clock_gen, const RelativeInformation* rel_info);
   ~FfComponents();
   libra::Vector<3> GenerateForce_N_b();
   libra::Vector<3> GenerateTorque_Nm_b();
@@ -22,11 +23,13 @@ class FfComponents : public InstalledComponents {
  private:
   // Components
   OBC* obc_;
+  RelativeDistanceSensor* relative_distance_sensor_;
 
   // References
   const Dynamics* dynamics_;
   const Structure* structure_;
   const LocalEnvironment* local_env_;
   const GlobalEnvironment* glo_env_;
   const SimulationConfig* config_;
+  const RelativeInformation* rel_info_;
 };

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

@@ -2,5 +2,5 @@
 
 FfSat::FfSat(SimulationConfig* sim_config, const GlobalEnvironment* glo_env, RelativeInformation* relative_information, const int sat_id)
     : Spacecraft(sim_config, glo_env, relative_information, sat_id) {
-  components_ = new FfComponents(dynamics_, structure_, local_env_, glo_env, sim_config, &clock_gen_);
+  components_ = new FfComponents(dynamics_, structure_, local_env_, glo_env, sim_config, &clock_gen_, relative_information);
 }