Add RelativeAttitudeController

s2e-ff/CMakeLists.txt

@@ -86,6 +86,8 @@ set(SOURCE_FILES
   src/Components/AOCS/InitializeRelativePositionSensor.cpp
   src/Components/IdealComponents/ForceGenerator.cpp
   src/Components/IdealComponents/InitializeForceGenerator.cpp
+  src/Components/IdealComponents/RelativeAttitudeController.cpp
+  src/Components/IdealComponents/InitializeRelativeAttitudeController.cpp
   src/Simulation/Case/FfCase.cpp
   src/Simulation/Spacecraft/FfSat.cpp
   src/Simulation/Spacecraft/FfComponents.cpp

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

@@ -0,0 +1,26 @@
+[RelativeAttitudeController]
+// Attitude controle mode settings. Select from the following modes.
+// - TARGET_SATELLITE_POINTING
+// - SUN_POINTING
+// - EARTH_CENTER_POINTING
+// - VELOCITY_DIRECTION_POINTING
+// - ORBIT_NORMAL_POINTING
+main_mode = TARGET_SATELLITE_POINTING
+sub_mode = ORBIT_NORMAL_POINTING
+
+// Target direction of body frame
+main_target_direction_b(0) = 1.0
+main_target_direction_b(1) = 0.0
+main_target_direction_b(2) = 0.0
+
+sub_target_direction_b(0) = 0.0
+sub_target_direction_b(1) = 1.0
+sub_target_direction_b(2) = 0.0
+
+// Satellite ID settings
+target_sat_id = 1
+reference_sat_id = 0 // Reccomend to set as the mounted satellite ID
+
+[ComponentBase]
+// Prescaler with respect to the component update period
+prescaler = 1

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

@@ -27,14 +27,14 @@ scale_factor_c(7) = 0;
 scale_factor_c(8) = 1;
 
 // Constant bias noise [m]
-constant_bias_c(0) = 10.0
-constant_bias_c(1) = 10.0
-constant_bias_c(2) = 10.0
+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]
-normal_random_standard_deviation_c(0) = 1.0
-normal_random_standard_deviation_c(1) = 1.0
-normal_random_standard_deviation_c(2) = 1.0
+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]
 random_walk_standard_deviation_c(0) = 0.0

s2e-ff/data/ini/FfSat.ini

@@ -2,7 +2,7 @@
 // Attitude propagation mode
 // RK4 : Attitude Propagation with RK4 including disturbances and control torque
 // CONTROLLED : Attitude Calculation with Controlled Attitude mode. All disturbances and control torque are ignored.
-propagate_mode = RK4
+propagate_mode = CONTROLLED
 
 // Initial angular velocity at body frame，[rad/s]
 Omega_b(0) = 0.0
@@ -141,3 +141,4 @@ structure_file = ../../data/ini/FfSatStructure.ini
 relative_distance_sensor_file = ../../data/ini/Components/RelativeDistanceSensor.ini
 relative_position_sensor_file = ../../data/ini/Components/RelativePositionSensor.ini
 force_generator_file = ../../data/ini/Components/ForceGenerator.ini
+relative_attitude_controller_file = ../../data/ini/Components/RelativeAttitudeController.ini

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

@@ -0,0 +1,35 @@
+#include "InitializeRelativeAttitudeController.hpp"
+
+#include <Interface/InitInput/IniAccess.h>
+
+RelativeAttitudeController InitializeRelativeAttitudeController(ClockGenerator* clock_gen, const std::string file_name,
+                                                                const RelativeInformation& rel_info,
+                                                                const LocalCelestialInformation& local_celes_info, const Dynamics& dynamics) {
+  // General
+  IniAccess ini_file(file_name);
+
+  // CompoBase
+  int prescaler = ini_file.ReadInt("ComponentBase", "prescaler");
+  if (prescaler <= 1) prescaler = 1;
+
+  // RelativeAttitudeController
+  char section[30] = "RelativeAttitudeController";
+
+  std::string main_mode_name = ini_file.ReadString(section, "main_mode");
+  RelativeAttitudeControlMode main_mode = ConvertStringToRelativeAttitudeControlMode(main_mode_name);
+  std::string sub_mode_name = ini_file.ReadString(section, "sub_mode");
+  RelativeAttitudeControlMode sub_mode = ConvertStringToRelativeAttitudeControlMode(sub_mode_name);
+
+  libra::Vector<3> main_target_direction_b;
+  ini_file.ReadVector(section, "main_target_direction_b", main_target_direction_b);
+  libra::Vector<3> sub_target_direction_b;
+  ini_file.ReadVector(section, "sub_target_direction_b", sub_target_direction_b);
+
+  int target_sat_id = ini_file.ReadInt(section, "target_sat_id");
+  int reference_sat_id = ini_file.ReadInt(section, "reference_sat_id");
+
+  RelativeAttitudeController relative_attitude_controller(prescaler, clock_gen, main_mode, sub_mode, main_target_direction_b, sub_target_direction_b,
+                                                          target_sat_id, reference_sat_id, rel_info, local_celes_info, dynamics);
+
+  return relative_attitude_controller;
+}

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

@@ -0,0 +1,7 @@
+#pragma once
+
+#include "RelativeAttitudeController.hpp"
+
+RelativeAttitudeController InitializeRelativeAttitudeController(ClockGenerator* clock_gen, const std::string file_name,
+                                                                const RelativeInformation& rel_info,
+                                                                const LocalCelestialInformation& local_celes_info, const Dynamics& dynamics);

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

@@ -0,0 +1,166 @@
+#include "RelativeAttitudeController.hpp"
+
+#include <Library/math/Matrix.hpp>
+
+#define THRESHOLD_CA cos(30.0 / 180.0 * libra::pi)  // fix me
+
+// Constructor
+RelativeAttitudeController::RelativeAttitudeController(const int prescaler, ClockGenerator* clock_gen, const RelativeAttitudeControlMode main_mode,
+                                                       const RelativeAttitudeControlMode sub_mode, const libra::Vector<3> main_target_direction_b,
+                                                       const libra::Vector<3> sub_target_direction_b, const int target_sat_id,
+                                                       const int reference_sat_id, const RelativeInformation& rel_info,
+                                                       const LocalCelestialInformation& local_celes_info, const Dynamics& dynamics)
+    : ComponentBase(prescaler, clock_gen),
+      main_mode_(main_mode),
+      sub_mode_(sub_mode),
+      main_target_direction_b_(main_target_direction_b),
+      sub_target_direction_b_(sub_target_direction_b),
+      target_sat_id_(target_sat_id),
+      my_sat_id_(reference_sat_id),
+      rel_info_(rel_info),
+      local_celes_info_(local_celes_info),
+      dynamics_(dynamics) {}
+
+RelativeAttitudeController::~RelativeAttitudeController() {}
+
+void RelativeAttitudeController::MainRoutine(int count) {
+  UNUSED(count);
+  if (!is_calc_enabled_) return;
+
+  libra::Vector<3> main_direction_i, sub_direction_i;
+  // Calc main target direction
+  main_direction_i = CalcTargetDirection_i(main_mode_);
+  // Calc sub target direction
+  sub_direction_i = CalcTargetDirection_i(sub_mode_);
+
+  // Calc attitude
+  libra::Quaternion q_i2b = CalcTargetQuaternion(main_direction_i, sub_direction_i);
+  dynamics_.SetAttitude().SetQuaternion_i2b(q_i2b);
+}
+
+void RelativeAttitudeController::PowerOffRoutine() {}
+
+std::string RelativeAttitudeController::GetLogHeader() const {
+  std::string str_tmp = "";
+
+  std::string head = "RelativeAttitudeController_";
+  str_tmp += WriteScalar(head + "main_mode", "-");
+
+  return str_tmp;
+}
+
+std::string RelativeAttitudeController::GetLogValue() const {
+  std::string str_tmp = "";
+  str_tmp += WriteScalar((int)main_mode_);
+
+  return str_tmp;
+}
+
+// Internal functions
+void RelativeAttitudeController::Initialize(void) {
+  if (main_mode_ >= RelativeAttitudeControlMode::NO_CONTROL) is_calc_enabled_ = false;
+  if (sub_mode_ >= RelativeAttitudeControlMode::NO_CONTROL) is_calc_enabled_ = false;
+
+  // sub mode check
+  if (main_mode_ == sub_mode_) {
+    std::cout << "sub mode should not equal to main mode. \n";
+    is_calc_enabled_ = false;
+    return;
+  }
+  // pointing direction check
+  normalize(main_target_direction_b_);
+  normalize(sub_target_direction_b_);
+  double tmp = inner_product(main_target_direction_b_, sub_target_direction_b_);
+  tmp = std::abs(tmp);
+  if (tmp > THRESHOLD_CA) {
+    std::cout << "sub target direction should separate from the main target direction. \n";
+    is_calc_enabled_ = false;
+    return;
+  }
+
+  return;
+}
+
+libra::Vector<3> RelativeAttitudeController::CalcTargetDirection_i(RelativeAttitudeControlMode mode) {
+  libra::Vector<3> direction_i;
+  switch (mode) {
+    case RelativeAttitudeControlMode::TARGET_SATELLITE_POINTING:
+      direction_i = rel_info_.GetRelativePosition_i_m(target_sat_id_, my_sat_id_);
+      break;
+    case RelativeAttitudeControlMode::SUN_POINTING:
+      direction_i = local_celes_info_.GetPosFromSC_i("SUN");
+      break;
+    case RelativeAttitudeControlMode::EARTH_CENTER_POINTING:
+      direction_i = local_celes_info_.GetPosFromSC_i("EARTH");
+      break;
+    case RelativeAttitudeControlMode::VELOCITY_DIRECTION_POINTING:
+      direction_i = dynamics_.GetOrbit().GetSatVelocity_i();
+      break;
+    case RelativeAttitudeControlMode::ORBIT_NORMAL_POINTING:
+      direction_i = outer_product(dynamics_.GetOrbit().GetSatPosition_i(), dynamics_.GetOrbit().GetSatVelocity_i());
+      break;
+    default:
+      // Not Reached
+      break;
+  }
+
+  normalize(direction_i);
+  return direction_i;
+}
+
+libra::Quaternion RelativeAttitudeController::CalcTargetQuaternion(const libra::Vector<3> main_direction_i, const libra::Vector<3> sub_direction_i) {
+  // Calc DCM ECI->Target
+  libra::Matrix<3, 3> DCM_t2i = CalcDcmFromVectors(main_direction_i, sub_direction_i);
+  // Calc DCM Target->body
+  libra::Matrix<3, 3> DCM_t2b = CalcDcmFromVectors(main_target_direction_b_, sub_target_direction_b_);
+  // Calc DCM ECI->body
+  libra::Matrix<3, 3> DCM_i2b = DCM_t2b * transpose(DCM_t2i);
+  // Convert to Quaternion
+  return libra::Quaternion::fromDCM(DCM_i2b);
+}
+
+libra::Matrix<3, 3> RelativeAttitudeController::CalcDcmFromVectors(const libra::Vector<3> main_direction, const libra::Vector<3> sub_direction) {
+  libra::Matrix<3, 3> dcm = libra::eye<3>();
+
+  // Check vector
+  double tmp = inner_product(main_direction, sub_direction);
+  tmp = std::abs(tmp);
+  if (tmp > 0.99999) {
+    return dcm;
+  }
+
+  // Calc basis vectors
+  libra::Vector<3> ex, ey, ez;
+  ex = main_direction;
+  libra::Vector<3> tmp1 = outer_product(ex, sub_direction);
+  libra::Vector<3> tmp2 = outer_product(tmp1, ex);
+  ey = normalize(tmp2);
+  libra::Vector<3> tmp3 = outer_product(ex, ey);
+  ez = normalize(tmp3);
+
+  // Generate DCM
+  for (int i = 0; i < 3; i++) {
+    dcm[i][0] = ex[i];
+    dcm[i][1] = ey[i];
+    dcm[i][2] = ez[i];
+  }
+  return dcm;
+}
+
+RelativeAttitudeControlMode ConvertStringToRelativeAttitudeControlMode(const std::string mode_name) {
+  if (mode_name == "TARGET_SATELLITE_POINTING") {
+    return RelativeAttitudeControlMode::TARGET_SATELLITE_POINTING;
+  } else if (mode_name == "SUN_POINTING") {
+    return RelativeAttitudeControlMode::SUN_POINTING;
+  } else if (mode_name == "EARTH_CENTER_POINTING") {
+    return RelativeAttitudeControlMode::EARTH_CENTER_POINTING;
+  } else if (mode_name == "VELOCITY_DIRECTION_POINTING") {
+    return RelativeAttitudeControlMode::VELOCITY_DIRECTION_POINTING;
+  } else if (mode_name == "ORBIT_NORMAL_POINTING") {
+    return RelativeAttitudeControlMode::ORBIT_NORMAL_POINTING;
+  } else {
+    // Error
+    std::cerr << "RelativeAttitudeControlMode error!" << std::endl;
+    return RelativeAttitudeControlMode::TARGET_SATELLITE_POINTING;
+  }
+}

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

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <Component/Abstract/ComponentBase.h>
+#include <Interface/LogOutput/Logger.h>
+#include <RelativeInformation/RelativeInformation.h>
+
+enum class RelativeAttitudeControlMode {
+  TARGET_SATELLITE_POINTING,
+  SUN_POINTING,
+  EARTH_CENTER_POINTING,
+  VELOCITY_DIRECTION_POINTING,
+  ORBIT_NORMAL_POINTING,
+  NO_CONTROL,
+};
+
+RelativeAttitudeControlMode ConvertStringToRelativeAttitudeControlMode(const std::string mode_name);
+
+class RelativeAttitudeController : public ComponentBase, public ILoggable {
+ public:
+  RelativeAttitudeController(const int prescaler, ClockGenerator* clock_gen, const RelativeAttitudeControlMode main_mode,
+                             const RelativeAttitudeControlMode sub_mode, const libra::Vector<3> main_target_direction_b,
+                             const libra::Vector<3> sub_target_direction_b, const int target_sat_id, const int reference_sat_id,
+                             const RelativeInformation& rel_info, const LocalCelestialInformation& local_celes_info, const Dynamics& dynamics);
+  ~RelativeAttitudeController();
+
+  // ComponentBase override function
+  void MainRoutine(int count);
+  void PowerOffRoutine();
+
+  // ILogabble override function
+  virtual std::string GetLogHeader() const;
+  virtual std::string GetLogValue() const;
+
+  // Getter
+
+  // Setter
+  inline void SetIsCalcEnabled(const bool is_enabled) { is_calc_enabled_ = is_enabled; }
+  inline void SetTargetSatId(const int target_sat_id) { target_sat_id_ = target_sat_id; }
+
+ protected:
+  bool is_calc_enabled_ = true;
+  RelativeAttitudeControlMode main_mode_;
+  RelativeAttitudeControlMode sub_mode_;
+  int target_sat_id_;
+  int my_sat_id_;
+
+  libra::Vector<3> main_target_direction_b_;  //!< Pointing main target on body frame
+  libra::Vector<3> sub_target_direction_b_;   //!< Pointing main target on body frame
+
+  const RelativeInformation& rel_info_;
+  const LocalCelestialInformation& local_celes_info_;
+  const Dynamics& dynamics_;
+
+  // Internal functions
+  void Initialize(void);
+  libra::Vector<3> CalcTargetDirection_i(RelativeAttitudeControlMode mode);
+  libra::Quaternion CalcTargetQuaternion(const libra::Vector<3> main_direction_i, const libra::Vector<3> sub_direction_i);
+  libra::Matrix<3, 3> CalcDcmFromVectors(const libra::Vector<3> main_direction,
+                                         const libra::Vector<3> sub_direction);  // TODO: move to core's library
+};

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

@@ -5,6 +5,7 @@
 #include "../../Components/AOCS/InitializeRelativeDistanceSensor.hpp"
 #include "../../Components/AOCS/InitializeRelativePositionSensor.hpp"
 #include "../../Components/IdealComponents/InitializeForceGenerator.hpp"
+#include "../../Components/IdealComponents/InitializeRelativeAttitudeController.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)
@@ -26,6 +27,10 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure,
   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_));
 
+  const std::string relative_attitude_controller_file = sat_file.ReadString("COMPONENTS_FILE", "relative_attitude_controller_file");
+  relative_attitude_controller_ = new RelativeAttitudeController(
+      InitializeRelativeAttitudeController(clock_gen, relative_attitude_controller_file, *rel_info_, local_env_->GetCelesInfo(), *dynamics_));
+
   // Debug for actuator output
   libra::Vector<3> force_N;
   force_N[0] = 1.0;
@@ -38,6 +43,7 @@ FfComponents::~FfComponents() {
   delete relative_distance_sensor_;
   delete relative_position_sensor_;
   delete force_generator_;
+  delete relative_attitude_controller_;
   // OBC must be deleted the last since it has com ports
   delete obc_;
 }

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

@@ -11,6 +11,7 @@
 #include "../../Components/AOCS/RelativeDistanceSensor.hpp"
 #include "../../Components/AOCS/RelativePositionSensor.hpp"
 #include "../../Components/IdealComponents/ForceGenerator.hpp"
+#include "../../Components/IdealComponents/RelativeAttitudeController.hpp"
 #include "OBC.h"
 
 class FfComponents : public InstalledComponents {
@@ -28,6 +29,7 @@ class FfComponents : public InstalledComponents {
   RelativeDistanceSensor* relative_distance_sensor_;
   RelativePositionSensor* relative_position_sensor_;
   ForceGenerator* force_generator_;
+  RelativeAttitudeController* relative_attitude_controller_;
 
   // References
   const Dynamics* dynamics_;