Merge pull request #44 from ut-issl/feature/add-relative-velocity-rtn

Add relative velocity sensor

s2e-ff/CMakeLists.txt

@@ -88,6 +88,8 @@ set(SOURCE_FILES
   src/Components/AOCS/InitializeRelativeDistanceSensor.cpp
   src/Components/AOCS/RelativePositionSensor.cpp
   src/Components/AOCS/InitializeRelativePositionSensor.cpp
+  src/Components/AOCS/RelativeVelocitySensor.cpp
+  src/Components/AOCS/InitializeRelativeVelocitySensor.cpp
   src/Components/IdealComponents/RelativeAttitudeController.cpp
   src/Components/IdealComponents/InitializeRelativeAttitudeController.cpp
   src/Library/math/DualQuaternion.cpp

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

@@ -0,0 +1,51 @@
+[RelativeVelocitySensor]
+// Target satellite ID
+target_sat_id = 1
+
+// Normally, reference_sat_id is set the id of mounted satellite
+reference_sat_id = 0
+
+// Users can choose the frame for error settings
+// INERTIAL, RTN
+error_frame = RTN
+
+[ComponentBase]
+// Prescaler with respect to the component update period
+prescaler = 1
+
+[SensorBase]
+// The coordinate of the error is selected by the error_frame
+// Scale factor [-]
+scale_factor_c(0) = 1;
+scale_factor_c(1) = 0;
+scale_factor_c(2) = 0;
+scale_factor_c(3) = 0;
+scale_factor_c(4) = 1;
+scale_factor_c(5) = 0;
+scale_factor_c(6) = 0;
+scale_factor_c(7) = 0;
+scale_factor_c(8) = 1;
+
+// Constant bias noise [m/s]
+constant_bias_c(0) = 0.0
+constant_bias_c(1) = 0.0
+constant_bias_c(2) = 0.0
+
+// Standard deviation of normal random noise [m/s]
+normal_random_standard_deviation_c(0) = 0.0
+normal_random_standard_deviation_c(1) = 0.0
+normal_random_standard_deviation_c(2) = 0.0
+
+// Standard deviation for random walk noise [m/s]
+random_walk_standard_deviation_c(0) = 0.0
+random_walk_standard_deviation_c(1) = 0.0
+random_walk_standard_deviation_c(2) = 0.0
+
+// Limit of random walk noise [m/s]
+random_walk_limit_c(0) = 0.0
+random_walk_limit_c(1) = 0.0
+random_walk_limit_c(2) = 0.0
+
+// Range [m/s]
+range_to_const = 1000000.0  // smaller than range_to_zero_m
+range_to_zero = 10000000.0

s2e-ff/data/ini/FfSat.ini

@@ -140,5 +140,6 @@ structure_file = ../../data/ini/FfSatStructure.ini
 // Users can add the path for component initialize files here.
 relative_distance_sensor_file = ../../data/ini/Components/RelativeDistanceSensor.ini
 relative_position_sensor_file = ../../data/ini/Components/RelativePositionSensor.ini
+relative_velocity_sensor_file = ../../data/ini/Components/RelativeVelocitySensor.ini
 force_generator_file = ../../data/ini/Components/ForceGenerator.ini
 relative_attitude_controller_file = ../../data/ini/Components/RelativeAttitudeController.ini

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

@@ -0,0 +1,39 @@
+#include "InitializeRelativeVelocitySensor.hpp"
+
+#include <Interface/InitInput/IniAccess.h>
+
+#include "../Abstract/InitializeSensorBase.hpp"
+
+RelativeVelocitySensor InitializeRelativeVelocitySensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s,
+                                                        const RelativeInformation& rel_info, const Dynamics& dynamics) {
+  // General
+  IniAccess ini_file(file_name);
+
+  // CompoBase
+  int prescaler = ini_file.ReadInt("ComponentBase", "prescaler");
+  if (prescaler <= 1) prescaler = 1;
+
+  // SensorBase
+  SensorBase<3> sensor_base = ReadSensorBaseInformation<3>(file_name, compo_step_time_s * (double)(prescaler));
+
+  // RelativeVelocitySensor
+  char section[30] = "RelativeVelocitySensor";
+  int target_sat_id = ini_file.ReadInt(section, "target_sat_id");
+  int reference_sat_id = ini_file.ReadInt(section, "reference_sat_id");
+  std::string error_frame_string = ini_file.ReadString(section, "error_frame");
+  RelativeVelocitySensorErrorFrame error_frame;
+
+  if (error_frame_string == "INERTIAL") {
+    error_frame = RelativeVelocitySensorErrorFrame::INERTIAL;
+  } else if (error_frame_string == "RTN") {
+    error_frame = RelativeVelocitySensorErrorFrame::RTN;
+  } else {
+    std::cerr << "Warnings: InitializeRelativeVelocitySensor: The error frame setting was failed. It is automatically set as RTN frame." << std::endl;
+    error_frame = RelativeVelocitySensorErrorFrame::RTN;
+  }
+
+  RelativeVelocitySensor relative_velocity_sensor(prescaler, clock_gen, sensor_base, target_sat_id, reference_sat_id, error_frame, rel_info,
+                                                  dynamics);
+
+  return relative_velocity_sensor;
+}

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

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

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

@@ -0,0 +1,63 @@
+#include "RelativeVelocitySensor.hpp"
+
+RelativeVelocitySensor::RelativeVelocitySensor(const int prescaler, ClockGenerator* clock_gen, SensorBase& sensor_base, const int target_sat_id,
+                                               const int reference_sat_id, const RelativeVelocitySensorErrorFrame error_frame,
+                                               const RelativeInformation& rel_info, const Dynamics& dynamics)
+    : ComponentBase(prescaler, clock_gen),
+      SensorBase(sensor_base),
+      target_sat_id_(target_sat_id),
+      reference_sat_id_(reference_sat_id),
+      error_frame_(error_frame),
+      rel_info_(rel_info),
+      dynamics_(dynamics) {}
+
+RelativeVelocitySensor::~RelativeVelocitySensor() {}
+
+void RelativeVelocitySensor::MainRoutine(int count) {
+  UNUSED(count);
+
+  // Get true value
+  libra::Vector<3> true_target_velocity_i_m_s = rel_info_.GetRelativeVelocity_i_m_s(target_sat_id_, reference_sat_id_);
+  libra::Vector<3> true_target_velocity_rtn_m_s = rel_info_.GetRelativeVelocity_rtn_m_s(target_sat_id_, reference_sat_id_);
+
+  // Add noise at body frame and frame conversion
+  libra::Quaternion q_i2rtn = dynamics_.GetOrbit().CalcQuaternionI2LVLH();
+  switch (error_frame_) {
+    case RelativeVelocitySensorErrorFrame::INERTIAL: {
+      measured_target_velocity_i_m_s_ = Measure(true_target_velocity_i_m_s);
+      libra::Vector<3> d_vel_i_m_s = measured_target_velocity_i_m_s_ - true_target_velocity_i_m_s;
+      // Frame conversion
+      libra::Vector<3> d_vel_rtn_m_s = q_i2rtn.frame_conv(d_vel_i_m_s);
+      measured_target_velocity_rtn_m_s_ = true_target_velocity_rtn_m_s + d_vel_rtn_m_s;
+      break;
+    }
+    case RelativeVelocitySensorErrorFrame::RTN: {
+      measured_target_velocity_rtn_m_s_ = Measure(true_target_velocity_rtn_m_s);
+      libra::Vector<3> d_vel_rtn_m_s = measured_target_velocity_rtn_m_s_ - true_target_velocity_rtn_m_s;
+      // Frame conversion
+      libra::Vector<3> d_vel_i_m_s = q_i2rtn.frame_conv_inv(d_vel_rtn_m_s);
+      measured_target_velocity_i_m_s_ = true_target_velocity_i_m_s + d_vel_i_m_s;
+      break;
+    }
+    default:
+      break;
+  }
+}
+
+std::string RelativeVelocitySensor::GetLogHeader() const {
+  std::string str_tmp = "";
+  std::string head = "RelativeVelocitySensor_";
+  str_tmp += WriteVector(head + "velocity", "i", "m", 3);
+  str_tmp += WriteVector(head + "velocity", "rtn", "m", 3);
+
+  return str_tmp;
+}
+
+std::string RelativeVelocitySensor::GetLogValue() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteVector(measured_target_velocity_i_m_s_);
+  str_tmp += WriteVector(measured_target_velocity_rtn_m_s_);
+
+  return str_tmp;
+}

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

@@ -0,0 +1,42 @@
+#ifndef RELATIVE_VELOCITY_SENSOR_H_
+#define RELATIVE_VELOCITY_SENSOR_H_
+
+#include <Component/Abstract/ComponentBase.h>
+#include <Component/Abstract/SensorBase.h>
+#include <Interface/LogOutput/ILoggable.h>
+#include <RelativeInformation/RelativeInformation.h>
+
+enum class RelativeVelocitySensorErrorFrame { INERTIAL, RTN };
+
+class RelativeVelocitySensor : public ComponentBase, public SensorBase<3>, public ILoggable {
+ public:
+  RelativeVelocitySensor(const int prescaler, ClockGenerator* clock_gen, SensorBase& sensor_base, const int target_sat_id, const int reference_sat_id,
+                         const RelativeVelocitySensorErrorFrame error_frame, const RelativeInformation& rel_info, const Dynamics& dynamics);
+  ~RelativeVelocitySensor();
+  // ComponentBase
+  void MainRoutine(int count) override;
+
+  // ILoggable
+  virtual std::string GetLogHeader() const;
+  virtual std::string GetLogValue() const;
+
+  // Getter
+  inline libra::Vector<3> GetMeasuredTargetVelocity_i_m_s() const { return measured_target_velocity_i_m_s_; }
+  inline libra::Vector<3> GetMeasuredTargetVelocity_rtn_m_s() const { return measured_target_velocity_rtn_m_s_; }
+
+  // Setter
+  void SetTargetSatId(const int target_sat_id) { target_sat_id_ = target_sat_id; }
+
+ protected:
+  int target_sat_id_;
+  const int reference_sat_id_;
+  RelativeVelocitySensorErrorFrame error_frame_;
+
+  libra::Vector<3> measured_target_velocity_i_m_s_{0.0};
+  libra::Vector<3> measured_target_velocity_rtn_m_s_{0.0};
+
+  const RelativeInformation& rel_info_;
+  const Dynamics& dynamics_;
+};
+
+#endif

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

@@ -6,6 +6,7 @@
 
 #include "../../Components/AOCS/InitializeRelativeDistanceSensor.hpp"
 #include "../../Components/AOCS/InitializeRelativePositionSensor.hpp"
+#include "../../Components/AOCS/InitializeRelativeVelocitySensor.hpp"
 #include "../../Components/IdealComponents/InitializeRelativeAttitudeController.hpp"
 
 FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, const LocalEnvironment* local_env, const GlobalEnvironment* glo_env,
@@ -25,6 +26,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_));
 
+  const std::string rel_vel_file = sat_file.ReadString("COMPONENTS_FILE", "relative_velocity_sensor_file");
+  relative_velocity_sensor_ =
+      new RelativeVelocitySensor(InitializeRelativeVelocitySensor(clock_gen, rel_vel_file, compo_step_sec, *rel_info_, *dynamics_));
+
   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_));
 
@@ -43,6 +48,7 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure,
 FfComponents::~FfComponents() {
   delete relative_distance_sensor_;
   delete relative_position_sensor_;
+  delete relative_velocity_sensor_;
   delete force_generator_;
   delete relative_attitude_controller_;
   // OBC must be deleted the last since it has com ports
@@ -64,5 +70,6 @@ Vector<3> FfComponents::GenerateTorque_Nm_b() {
 void FfComponents::LogSetup(Logger& logger) {
   logger.AddLoggable(relative_distance_sensor_);
   logger.AddLoggable(relative_position_sensor_);
+  logger.AddLoggable(relative_velocity_sensor_);
   logger.AddLoggable(force_generator_);
 }

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

@@ -12,6 +12,7 @@
 
 #include "../../Components/AOCS/RelativeDistanceSensor.hpp"
 #include "../../Components/AOCS/RelativePositionSensor.hpp"
+#include "../../Components/AOCS/RelativeVelocitySensor.hpp"
 #include "../../Components/IdealComponents/RelativeAttitudeController.hpp"
 #include "OBC.h"
 
@@ -29,6 +30,7 @@ class FfComponents : public InstalledComponents {
   OBC* obc_;
   RelativeDistanceSensor* relative_distance_sensor_;
   RelativePositionSensor* relative_position_sensor_;
+  RelativeVelocitySensor* relative_velocity_sensor_;
   ForceGenerator* force_generator_;
   RelativeAttitudeController* relative_attitude_controller_;