add relative attitude sensor

s2e-ff/CMakeLists.txt

@@ -49,6 +49,8 @@ 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/initialize_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,46 @@
+[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
+
+[SENSOR_BASE_RELATIVE_ATTITUDE_SENSOR]
+// 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 [rad]
+constant_bias_c_rad(0) = 0.0
+constant_bias_c_rad(1) = 0.0
+constant_bias_c_rad(2) = 0.0
+
+// Standard deviation of normal random noise [rad]
+normal_random_standard_deviation_c_rad(0) = 0.0
+normal_random_standard_deviation_c_rad(1) = 0.0
+normal_random_standard_deviation_c_rad(2) = 0.0
+
+// Standard deviation for random walk noise [rad]
+random_walk_standard_deviation_c_rad(0) = 0.0
+random_walk_standard_deviation_c_rad(1) = 0.0
+random_walk_standard_deviation_c_rad(2) = 0.0
+
+// Limit of random walk noise [rad]
+random_walk_limit_c_rad(0) = 0.0
+random_walk_limit_c_rad(1) = 0.0
+random_walk_limit_c_rad(2) = 0.0
+
+// Range [rad]
+range_to_constant_rad = 6.283186  // smaller than range_to_zero_rad
+range_to_zero_rad = 7.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/initialize_relative_attitude_sensor.cpp

@@ -0,0 +1,35 @@
+/**
+ * @file initialize_relative_attitude_sensor.cpp
+ * @brief Initialize function for RelativeAttitudeSensor
+ */
+
+#include "initialize_relative_attitude_sensor.hpp"
+
+#include <components/base/initialize_sensor.hpp>
+#include <library/initialize/initialize_file_access.hpp>
+
+RelativeAttitudeSensor InitializeRelativeAttitudeSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s,
+                                                        const RelativeInformation& rel_info, const Dynamics& dynamics,
+                                                        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;
+
+  // 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;
+  }
+
+  // SensorBase
+  Sensor<3> sensor_base = ReadSensorInformation<3>(file_name, compo_step_time_s * (double)(prescaler), section, "rad");
+
+  RelativeAttitudeSensor relative_attitude_sensor(prescaler, clock_gen, sensor_base, target_sat_id, reference_sat_id, rel_info, dynamics);
+
+  return relative_attitude_sensor;
+}

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

@@ -0,0 +1,19 @@
+/**
+ * @file initialize_relative_attitude_sensor.hpp
+ * @brief Initialize function for RelativeAttitudeSensor
+ */
+
+#ifndef S2E_COMPONENTS_AOCS_INITIALIZE_RELATIVE_ATTITUDE_SENSOR_HPP_
+#define S2E_COMPONENTS_AOCS_INITIALIZE_RELATIVE_ATTITUDE_SENSOR_HPP_
+
+#include "relative_attitude_sensor.hpp"
+
+/**
+ * @fn InitializeRelativeAttitudeSensor
+ * @brief Initialize function
+ */
+RelativeAttitudeSensor InitializeRelativeAttitudeSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s,
+                                                        const RelativeInformation& rel_info, const Dynamics& dynamics,
+                                                        const int reference_sat_id_input = -1);
+
+#endif  // S2E_COMPONENTS_AOCS_INITIALIZE_RELATIVE_ATTITUDE_SENSOR_HPP_

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

@@ -0,0 +1,47 @@
+/**
+ * @file relative_attitude_sensor.cpp
+ * @brief Relative attitude sensor
+ */
+
+#include "relative_attitude_sensor.hpp"
+
+RelativeAttitudeSensor::RelativeAttitudeSensor(const int prescaler, ClockGenerator* clock_gen, Sensor& sensor_base, const int target_sat_id,
+                                               const int reference_sat_id, const RelativeInformation& rel_info, const Dynamics& dynamics)
+    : Component(prescaler, clock_gen),
+      Sensor(sensor_base),
+      target_sat_id_(target_sat_id),
+      reference_sat_id_(reference_sat_id),
+      rel_info_(rel_info),
+      dynamics_(dynamics) {}
+
+RelativeAttitudeSensor::~RelativeAttitudeSensor() {}
+
+void RelativeAttitudeSensor::MainRoutine(int count) {
+  UNUSED(count);
+
+  // Get true value
+  measured_target_attitude_b_quaternion_ = rel_info_.GetRelativeAttitudeQuaternion(target_sat_id_, reference_sat_id_);
+  measured_target_attitude_b_rad_ = measured_target_attitude_b_quaternion_.ConvertToEuler();
+  measured_target_attitude_b_rad_ = Measure(measured_target_attitude_b_rad_);
+  measured_target_attitude_b_quaternion_ = libra::Quaternion::ConvertFromEuler(measured_target_attitude_b_rad_);
+}
+
+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 + "attitude", frame_name, "rad", 3);
+
+  return str_tmp;
+}
+
+std::string RelativeAttitudeSensor::GetLogValue() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteQuaternion(measured_target_attitude_b_quaternion_);
+  str_tmp += WriteVector(measured_target_attitude_b_rad_);
+
+  return str_tmp;
+}

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

@@ -0,0 +1,71 @@
+/**
+ * @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 <components/base/sensor.hpp>
+#include <library/logger/logger.hpp>
+#include <simulation/multiple_spacecraft/relative_information.hpp>
+
+/**
+ * @class RelativeAttitudeSensor
+ * @brief Relative position sensor
+ */
+class RelativeAttitudeSensor : public Component, public Sensor<3>, public ILoggable {
+ public:
+  /**
+   * @fn RelativeAttitudeSensor
+   * @brief Constructor
+   */
+  RelativeAttitudeSensor(const int prescaler, ClockGenerator* clock_gen, Sensor& sensor_base, const int target_sat_id, const int reference_sat_id,
+                         const RelativeInformation& rel_info, const Dynamics& dynamics);
+  /**
+   * @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 GetMeasuredTargetQuaternion_b_m() const { return measured_target_attitude_b_quaternion_; }
+  inline libra::Vector<3> GetMeasuredTargetAttitude_b_m() const { return measured_target_attitude_b_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_target_attitude_b_quaternion_;
+  libra::Vector<3> measured_target_attitude_b_rad_{0.0};  //!< Measured attitude in BODY frame [m]
+
+  // References
+  const RelativeInformation& rel_info_;  //!< Relative information
+  const Dynamics& dynamics_;             //!< Dynamics
+};
+
+#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_, *dynamics_, 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

@@ -16,6 +16,7 @@
 #include <components/ideal/force_generator.hpp>
 #include <components/real/cdh/on_board_computer.hpp>
 
+#include "../../components/aocs/initialize_relative_attitude_sensor.hpp"
 #include "../../components/aocs/initialize_relative_distance_sensor.hpp"
 #include "../../components/aocs/initialize_relative_position_sensor.hpp"
 #include "../../components/aocs/initialize_relative_velocity_sensor.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

@@ -17,6 +17,7 @@
 #include <components/real/cdh/on_board_computer.hpp>
 
 #include "../../components/aocs/corner_cube_reflector.hpp"
+#include "../../components/aocs/initialize_relative_attitude_sensor.hpp"
 #include "../../components/aocs/initialize_relative_distance_sensor.hpp"
 #include "../../components/aocs/initialize_relative_position_sensor.hpp"
 #include "../../components/ideal/initialize_relative_attitude_controller.hpp"