Merge pull request #79 from ut-issl/feature/add_relative_attitude_sensor

Add relative attitude sensor

s2e-ff/CMakeLists.txt

@@ -49,6 +49,7 @@ set(SOURCE_FILES
   src/components/aocs/initialize_relative_distance_sensor.cpp
   src/components/aocs/relative_position_sensor.cpp
   src/components/aocs/initialize_relative_position_sensor.cpp
+  src/components/aocs/relative_attitude_sensor.cpp
   src/components/aocs/relative_velocity_sensor.cpp
   src/components/aocs/initialize_relative_velocity_sensor.cpp
   src/components/aocs/laser_distance_meter.cpp

s2e-ff/data/initialize_files/components/relative_attitude_sensor.ini

@@ -0,0 +1,12 @@
+[RELATIVE_ATTITUDE_SENSOR]
+// Prescaler with respect to the component update period
+prescaler = 1
+
+// Target satellite ID
+target_sat_id = 1
+
+// When this value is negative, reference_sat_id is automatically set as the mounting satellite ID
+reference_sat_id = -1
+
+// Standard deviation of force direction error [deg]
+error_angle_standard_deviation_deg = 1.0

s2e-ff/data/initialize_files/ff_satellite.ini

@@ -135,6 +135,7 @@ structure_file = ../../data/initialize_files/ff_satellite_structure.ini
 // Users can add the path for component initialize files here.
 relative_distance_sensor_file = ../../data/initialize_files/components/relative_distance_sensor.ini
 relative_position_sensor_file = ../../data/initialize_files/components/relative_position_sensor.ini
+relative_attitude_sensor_file = ../../data/initialize_files/components/relative_attitude_sensor.ini
 relative_velocity_sensor_file = ../../data/initialize_files/components/relative_velocity_sensor.ini
 laser_distance_meter_file = ../../data/initialize_files/components/laser_distance_meter.ini
 force_generator_file = ../../data/initialize_files/components/force_generator.ini

s2e-ff/src/components/aocs/relative_attitude_sensor.cpp

@@ -0,0 +1,85 @@
+/**
+ * @file relative_attitude_sensor.cpp
+ * @brief Relative attitude sensor
+ */
+
+#include "relative_attitude_sensor.hpp"
+
+#include <components/base/initialize_sensor.hpp>
+#include <library/initialize/initialize_file_access.hpp>
+
+RelativeAttitudeSensor::RelativeAttitudeSensor(const int prescaler, ClockGenerator* clock_gen, const int target_sat_id, const int reference_sat_id,
+                                               const RelativeInformation& rel_info, const double standard_deviation_rad)
+    : Component(prescaler, clock_gen),
+      target_sat_id_(target_sat_id),
+      reference_sat_id_(reference_sat_id),
+      rel_info_(rel_info),
+      angle_noise_(0.0, standard_deviation_rad) {
+  direction_noise_.SetParameters(0.0, 1.0);
+}
+
+RelativeAttitudeSensor::~RelativeAttitudeSensor() {}
+
+void RelativeAttitudeSensor::MainRoutine(int count) {
+  UNUSED(count);
+  // Error calculation
+  libra::Vector<3> random_direction;
+  random_direction[0] = direction_noise_;
+  random_direction[1] = direction_noise_;
+  random_direction[2] = direction_noise_;
+  random_direction = random_direction.CalcNormalizedVector();
+
+  double error_angle_rad = angle_noise_;
+  libra::Quaternion error_quaternion(random_direction, error_angle_rad);
+
+  // Get true value
+  measured_quaternion_rb2tb_ = rel_info_.GetRelativeAttitudeQuaternion(target_sat_id_, reference_sat_id_);
+  measured_quaternion_rb2tb_ = error_quaternion * measured_quaternion_rb2tb_;
+  measured_euler_angle_rb2tb_rad_ = measured_quaternion_rb2tb_.ConvertToEuler();
+}
+
+std::string RelativeAttitudeSensor::GetLogHeader() const {
+  std::string str_tmp = "";
+  std::string head = "RelativeAttitudeSensor_";
+
+  const std::string frame_name = std::to_string(reference_sat_id_) + "to" + std::to_string(target_sat_id_);
+  str_tmp += WriteQuaternion(head + "quaternion", frame_name);
+  str_tmp += WriteVector(head + "euler_angle", frame_name, "rad", 3);
+
+  return str_tmp;
+}
+
+std::string RelativeAttitudeSensor::GetLogValue() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteQuaternion(measured_quaternion_rb2tb_);
+  str_tmp += WriteVector(measured_euler_angle_rb2tb_rad_);
+
+  return str_tmp;
+}
+
+RelativeAttitudeSensor InitializeRelativeAttitudeSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s,
+                                                        const RelativeInformation& rel_info, const int reference_sat_id_input) {
+  // General
+  IniAccess ini_file(file_name);
+  char section[30] = "RELATIVE_ATTITUDE_SENSOR";
+
+  // CompoBase
+  int prescaler = ini_file.ReadInt(section, "prescaler");
+  if (prescaler <= 1) prescaler = 1;
+
+  double error_angle_standard_deviation_deg = ini_file.ReadDouble(section, "error_angle_standard_deviation_deg");
+  double error_angle_standard_deviation_rad = libra::deg_to_rad * error_angle_standard_deviation_deg;
+
+  // RelativeAttitudeSensor
+  int target_sat_id = ini_file.ReadInt(section, "target_sat_id");
+  int reference_sat_id = ini_file.ReadInt(section, "reference_sat_id");
+  if (reference_sat_id < 0) {
+    reference_sat_id = reference_sat_id_input;
+  }
+
+  RelativeAttitudeSensor relative_attitude_sensor(prescaler, clock_gen, target_sat_id, reference_sat_id, rel_info,
+                                                  error_angle_standard_deviation_rad);
+
+  return relative_attitude_sensor;
+}

s2e-ff/src/components/aocs/relative_attitude_sensor.hpp

@@ -0,0 +1,78 @@
+/**
+ * @file relative_attitude_sensor.hpp
+ * @brief Relative attitude sensor
+ */
+
+#ifndef S2E_COMPONENTS_AOCS_RELATIVE_ATTITUDE_SENSOR_HPP_
+#define S2E_COMPONENTS_AOCS_RELATIVE_ATTITUDE_SENSOR_HPP_
+
+#include <components/base/component.hpp>
+#include <library/logger/logger.hpp>
+#include <library/randomization/normal_randomization.hpp>
+#include <simulation/multiple_spacecraft/relative_information.hpp>
+
+/**
+ * @class RelativeAttitudeSensor
+ * @brief Relative attitude sensor
+ */
+class RelativeAttitudeSensor : public Component, public ILoggable {
+ public:
+  /**
+   * @fn RelativeAttitudeSensor
+   * @brief Constructor
+   */
+  RelativeAttitudeSensor(const int prescaler, ClockGenerator* clock_gen, const int target_sat_id, const int reference_sat_id,
+                         const RelativeInformation& rel_info, const double standard_deviation_rad);
+  /**
+   * @fn ~RelativeAttitudeSensor
+   * @brief Destructor
+   */
+  ~RelativeAttitudeSensor();
+
+  // ComponentBase override function
+  /**
+   * @fn MainRoutine
+   * @brief Main routine
+   */
+  void MainRoutine(int count);
+
+  // Override ILoggable
+  /**
+   * @fn GetLogHeader
+   * @brief Override GetLogHeader function of ILoggable
+   */
+  virtual std::string GetLogHeader() const;
+  /**
+   * @fn GetLogValue
+   * @brief Override GetLogValue function of ILoggable
+   */
+  virtual std::string GetLogValue() const;
+
+  // Getter
+  inline libra::Quaternion GetMeasuredQuaternion_rb2tb() const { return measured_quaternion_rb2tb_; }
+  inline libra::Vector<3> GetMeasuredEulerAngle_rb2tb_rad() const { return measured_euler_angle_rb2tb_rad_; }
+
+  // Setter
+  void SetTargetSatId(const int target_sat_id) { target_sat_id_ = target_sat_id; }
+
+ protected:
+  int target_sat_id_;           //!< Target satellite ID
+  const int reference_sat_id_;  //!< Reference satellite ID
+
+  // Measured value
+  libra::Quaternion measured_quaternion_rb2tb_ = {0.0, 0.0, 0.0, 1.0};  //!< Measured quaternion of target body from reference body
+  libra::Vector<3> measured_euler_angle_rb2tb_rad_{
+      0.0};  //!< Measured Euler angle of target body in BODY frame [rad], 3-2-1 Euler angle (1: roll, 2: pitch, 3: yaw order)
+
+  // Noises
+  libra::NormalRand angle_noise_;      //!< Normal random for magnitude noise
+  libra::NormalRand direction_noise_;  //!< Normal random for direction noise
+
+  // References
+  const RelativeInformation& rel_info_;  //!< Relative information
+};
+
+RelativeAttitudeSensor InitializeRelativeAttitudeSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s,
+                                                        const RelativeInformation& rel_info, const int reference_sat_id_input = -1);
+
+#endif  // S2E_COMPONENTS_AOCS_RELATIVE_ATTITUDE_SENSOR_HPP_

s2e-ff/src/simulation/spacecraft/ff_components.cpp

@@ -30,6 +30,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_, sat_id));
 
+  const std::string rel_att_file = sat_file.ReadString(section_name.c_str(), "relative_attitude_sensor_file");
+  relative_attitude_sensor_ =
+      new RelativeAttitudeSensor(InitializeRelativeAttitudeSensor(clock_gen, rel_att_file, compo_step_sec, *rel_info_, sat_id));
+
   const std::string rel_vel_file = sat_file.ReadString(section_name.c_str(), "relative_velocity_sensor_file");
   relative_velocity_sensor_ =
       new RelativeVelocitySensor(InitializeRelativeVelocitySensor(clock_gen, rel_vel_file, compo_step_sec, *rel_info_, *dynamics_, sat_id));
@@ -55,6 +59,7 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure,
 FfComponents::~FfComponents() {
   delete relative_distance_sensor_;
   delete relative_position_sensor_;
+  delete relative_attitude_sensor_;
   delete relative_velocity_sensor_;
   delete force_generator_;
   delete relative_attitude_controller_;
@@ -78,6 +83,7 @@ Vector<3> FfComponents::GenerateTorque_b_Nm() {
 void FfComponents::LogSetup(Logger& logger) {
   logger.AddLogList(relative_distance_sensor_);
   logger.AddLogList(relative_position_sensor_);
+  logger.AddLogList(relative_attitude_sensor_);
   logger.AddLogList(relative_velocity_sensor_);
   logger.AddLogList(force_generator_);
   logger.AddLogList(laser_distance_meter_);

s2e-ff/src/simulation/spacecraft/ff_components.hpp

@@ -20,6 +20,7 @@
 #include "../../components/aocs/initialize_relative_position_sensor.hpp"
 #include "../../components/aocs/initialize_relative_velocity_sensor.hpp"
 #include "../../components/aocs/laser_distance_meter.hpp"
+#include "../../components/aocs/relative_attitude_sensor.hpp"
 #include "../../components/ideal/initialize_relative_attitude_controller.hpp"
 
 /**
@@ -65,6 +66,7 @@ class FfComponents : public InstalledComponents {
   // Sensors
   RelativeDistanceSensor* relative_distance_sensor_;  //!< Example of Relative distance sensor
   RelativePositionSensor* relative_position_sensor_;  //!< Example of Relative position sensor
+  RelativeAttitudeSensor* relative_attitude_sensor_;  //!< Example of Relative attitude sensor
   RelativeVelocitySensor* relative_velocity_sensor_;  //!< Example of Relative velocity sensor
   LaserDistanceMeter* laser_distance_meter_;
   // Actuators

s2e-ff/src/simulation/spacecraft/ff_components_2.hpp

@@ -19,6 +19,7 @@
 #include "../../components/aocs/corner_cube_reflector.hpp"
 #include "../../components/aocs/initialize_relative_distance_sensor.hpp"
 #include "../../components/aocs/initialize_relative_position_sensor.hpp"
+#include "../../components/aocs/relative_attitude_sensor.hpp"
 #include "../../components/ideal/initialize_relative_attitude_controller.hpp"
 
 /**