Merge branch 'develop' into feature/remove-library-directory

CMakeLists.txt

@@ -4,12 +4,13 @@ cmake_minimum_required(VERSION 3.13)
 project(S2E
   LANGUAGES CXX
   DESCRIPTION "S2E: Spacecraft Simulation Environment"
-  VERSION 7.2.4
+  VERSION 7.2.5
 )
 
 # build config
 option(USE_HILS "Use HILS" OFF)
 option(USE_C2A "Use C2A" OFF)
+option(USE_C2A_COMMAND_SENDER "Use command sender to C2A" OFF)
 option(BUILD_64BIT "Build 64bit" OFF)
 option(GOOGLE_TEST "Execute GoogleTest" OFF)
 

data/sample/initialize_files/components/orbit_observer.ini

@@ -0,0 +1,19 @@
+[ORBIT_OBSERVER]
+// Noise definition frame
+// INERTIAL: Inertial frame
+// RTN: RTN frame
+noise_frame = INERTIAL
+
+// Standard deviation of position and velocity noise [m, m/s]
+// The frame definition can be selected above
+noise_standard_deviation(0) = 1000 // Position-X
+noise_standard_deviation(1) = 2000 // Position-Y
+noise_standard_deviation(2) = 3000 // Position-Z
+noise_standard_deviation(3) = 30  // Velocity-X
+noise_standard_deviation(4) = 20  // Velocity-Y
+noise_standard_deviation(5) = 10  // Velocity-Z
+
+
+[COMPONENT_BASE]
+// Prescaler with respect to the component update period
+prescaler = 1

data/sample/initialize_files/sample_satellite.ini

@@ -146,6 +146,7 @@ force_generator_file = INI_FILE_DIR_FROM_EXE/components/force_generator.ini
 torque_generator_file = INI_FILE_DIR_FROM_EXE/components/torque_generator.ini
 angular_velocity_observer_file = INI_FILE_DIR_FROM_EXE/components/angular_velocity_observer.ini
 attitude_observer_file = INI_FILE_DIR_FROM_EXE/components/attitude_observer.ini
+orbit_observer_file = INI_FILE_DIR_FROM_EXE/components/orbit_observer.ini
 antenna_file = INI_FILE_DIR_FROM_EXE/components/spacecraft_antenna.ini
 component_interference_file = INI_FILE_DIR_FROM_EXE/components/component_interference.ini
 telescope_file = INI_FILE_DIR_FROM_EXE/components/telescope.ini

scripts/Plot/Pipfile

@@ -4,12 +4,12 @@ verify_ssl = true
 name = "pypi"
 
 [packages]
-basemap = "==1.4.0"
+basemap = "==1.4.1"
 matplotlib = "==3.7.2"
 numpy = "==1.24.3"
 pandas = "==2.0.3"
 numpy-stl = "==3.0.1"
-numpy-quaternion = "==2023.0.2"
+numpy-quaternion = "==2023.0.3"
 python-utils = "==3.5.2"
 
 [dev-packages]

scripts/Plot/plot_orbit_observer.py

@@ -0,0 +1,126 @@
+#
+# Plot orbit observer
+#
+# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946
+#
+
+#
+# Import
+#
+# plots
+import matplotlib.pyplot as plt
+# numpy
+import numpy as np
+# local function
+from common import find_latest_log_tag
+from common import add_log_file_arguments
+from common import read_3d_vector_from_csv
+from common import read_scalar_from_csv
+# arguments
+import argparse
+
+# Arguments
+aparser = argparse.ArgumentParser()
+aparser = add_log_file_arguments(aparser)
+aparser.add_argument('--no-gui', action='store_true')
+args = aparser.parse_args()
+
+#
+# Read Arguments
+#
+# log file path
+path_to_logs = args.logs_dir
+
+read_file_tag = args.file_tag
+if read_file_tag == None:
+  print("file tag does not found. use latest.")
+  read_file_tag = find_latest_log_tag(path_to_logs)
+
+print("log: " + read_file_tag)
+
+#
+# CSV file name
+#
+read_file_name  = path_to_logs + '/' + 'logs_' + read_file_tag + '/' + read_file_tag + '_default.csv'
+
+#
+# Data read and edit
+#
+# Read S2E CSV
+time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]')
+
+measured_position_eci_m = read_3d_vector_from_csv(read_file_name, 'orbit_observer_position_i', 'm')
+true_position_eci_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_i', 'm')
+
+measured_velocity_eci_m_s = read_3d_vector_from_csv(read_file_name, 'orbit_observer_velocity_i', 'm/s')
+true_velocity_eci_m_s = read_3d_vector_from_csv(read_file_name, 'spacecraft_velocity_i', 'm/s')
+
+# Statistics
+position_error_m = measured_position_eci_m[:, 1:] - true_position_eci_m[:, 1:]
+position_average = [0.0, 0.0, 0.0]
+position_standard_deviation = [0.0, 0.0, 0.0]
+velocity_error_m = measured_velocity_eci_m_s[:, 1:] - true_velocity_eci_m_s[:, 1:]
+velocity_average = [0.0, 0.0, 0.0]
+velocity_standard_deviation = [0.0, 0.0, 0.0]
+for i in range(3):
+  position_average[i] = position_error_m[i].mean()
+  position_standard_deviation[i] = position_error_m[i].std()
+  velocity_average[i] = velocity_error_m[i].mean()
+  velocity_standard_deviation[i] = velocity_error_m[i].std()
+
+#
+# Plot
+#
+unit = ' m'
+fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True)
+axis[0, 0].plot(time[0], measured_position_eci_m[0], marker=".", c="red",    label="MEASURED-X")
+axis[0, 0].plot(time[0], true_position_eci_m[0], marker=".", c="orange",  label="TRUE-X")
+axis[0, 0].text(0.01, 0.99, "Error average:" + format(position_average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
+axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
+axis[0, 0].legend(loc = 'upper right')
+
+axis[1, 0].plot(time[0], measured_position_eci_m[1], marker=".", c="green",  label="MEASURED-Y")
+axis[1, 0].plot(time[0], true_position_eci_m[1], marker=".", c="yellow",  label="TRUE-Y")
+axis[1, 0].text(0.01, 0.99, "Error average:" + format(position_average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
+axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
+axis[1, 0].legend(loc = 'upper right')
+
+axis[2, 0].plot(time[0], measured_position_eci_m[2], marker=".", c="blue",   label="MEASURED-Z")
+axis[2, 0].plot(time[0], true_position_eci_m[2], marker=".", c="purple",  label="TRUE-Z")
+axis[2, 0].text(0.01, 0.99, "Error average:" + format(position_average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
+axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
+axis[2, 0].legend(loc = 'upper right')
+
+fig.suptitle("Orbit observer position results @ ECI")
+fig.supylabel("Position [m]")
+fig.supxlabel("Time [s]")
+
+unit = ' m/s'
+fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True)
+axis[0, 0].plot(time[0], measured_velocity_eci_m_s[0], marker=".", c="red",    label="MEASURED-X")
+axis[0, 0].plot(time[0], true_velocity_eci_m_s[0], marker=".", c="orange",  label="TRUE-X")
+axis[0, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
+axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
+axis[0, 0].legend(loc = 'upper right')
+
+axis[1, 0].plot(time[0], measured_velocity_eci_m_s[1], marker=".", c="green",  label="MEASURED-Y")
+axis[1, 0].plot(time[0], true_velocity_eci_m_s[1], marker=".", c="yellow",  label="TRUE-Y")
+axis[1, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
+axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
+axis[1, 0].legend(loc = 'upper right')
+
+axis[2, 0].plot(time[0], measured_velocity_eci_m_s[2], marker=".", c="blue",   label="MEASURED-Z")
+axis[2, 0].plot(time[0], true_velocity_eci_m_s[2], marker=".", c="purple",  label="TRUE-Z")
+axis[2, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
+axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
+axis[2, 0].legend(loc = 'upper right')
+
+fig.suptitle("Orbit observer velocity results @ ECI")
+fig.supylabel("Velocity [m/s]")
+fig.supxlabel("Time [s]")
+
+# Data save
+if args.no_gui:
+  plt.savefig(read_file_tag + "_orbit_observer.png") # save last figure only
+else:
+  plt.show()

src/components/CMakeLists.txt

@@ -25,7 +25,6 @@ real/cdh/on_board_computer_with_c2a.cpp
 real/communication/antenna.cpp
 real/communication/antenna_radiation_pattern.cpp
 real/communication/ground_station_calculator.cpp
-real/communication/wings_command_sender_to_c2a.cpp
 
 examples/example_change_structure.cpp
 examples/example_serial_communication_with_obc.cpp
@@ -37,6 +36,7 @@ ideal/force_generator.cpp
 ideal/torque_generator.cpp
 ideal/angular_velocity_observer.cpp
 ideal/attitude_observer.cpp
+ideal/orbit_observer.cpp
 
 real/mission/telescope.cpp
 
@@ -62,6 +62,13 @@ if(USE_HILS)
   )
 endif()
 
+if(USE_C2A_COMMAND_SENDER)
+  set(SOURCE_FILES
+    ${SOURCE_FILES}
+    real/communication/wings_command_sender.cpp
+  )
+endif()
+
 add_library(${PROJECT_NAME} STATIC
   ${SOURCE_FILES}
 )

src/components/ideal/orbit_observer.cpp

@@ -0,0 +1,102 @@
+/*
+ * @file orbit_observer.cpp
+ * @brief Ideal component which can observe orbit
+ */
+
+#include "orbit_observer.hpp"
+
+#include <library/initialize/initialize_file_access.hpp>
+#include <library/randomization/global_randomization.hpp>
+
+OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame,
+                             const libra::Vector<6> error_standard_deviation, const Orbit& orbit)
+    : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) {
+  for (size_t i = 0; i < 6; i++) {
+    normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed());
+  }
+}
+
+void OrbitObserver::MainRoutine(const int time_count) {
+  UNUSED(time_count);
+
+  // Calc noise
+  libra::Vector<3> position_error_i_m{0.0};
+  libra::Vector<3> position_error_rtn_m{0.0};
+  libra::Vector<3> velocity_error_i_m_s{0.0};
+  libra::Vector<3> velocity_error_rtn_m_s{0.0};
+  libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh();
+  switch (noise_frame_) {
+    case NoiseFrame::kInertial:
+      for (size_t axis = 0; axis < 3; axis++) {
+        position_error_i_m[axis] = normal_random_noise_[axis];
+        velocity_error_i_m_s[axis] = normal_random_noise_[axis + 3];
+      }
+      break;
+    case NoiseFrame::kRtn:
+      for (size_t axis = 0; axis < 3; axis++) {
+        position_error_rtn_m[axis] = normal_random_noise_[axis];
+        velocity_error_rtn_m_s[axis] = normal_random_noise_[axis + 3];
+      }
+      // Frame conversion
+      position_error_i_m = q_i2rtn.InverseFrameConversion(position_error_rtn_m);
+      // For zero bias noise, we do not need to care the frame rotation effect.
+      velocity_error_i_m_s = q_i2rtn.InverseFrameConversion(velocity_error_rtn_m_s);
+
+      break;
+    default:
+      break;
+  }
+
+  // Get observed value
+  observed_position_i_m_ = orbit_.GetPosition_i_m() + position_error_i_m;
+  observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s() + velocity_error_i_m_s;
+}
+
+std::string OrbitObserver::GetLogHeader() const {
+  std::string str_tmp = "";
+
+  std::string head = "orbit_observer_";
+  str_tmp += WriteVector(head + "position", "i", "m", 3);
+  str_tmp += WriteVector(head + "velocity", "i", "m/s", 3);
+
+  return str_tmp;
+}
+
+std::string OrbitObserver::GetLogValue() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteVector(observed_position_i_m_, 16);
+  str_tmp += WriteVector(observed_velocity_i_m_s_, 16);
+
+  return str_tmp;
+}
+
+NoiseFrame SetNoiseFrame(const std::string noise_frame) {
+  if (noise_frame == "INERTIAL") {
+    return NoiseFrame::kInertial;
+  } else if (noise_frame == "RTN") {
+    return NoiseFrame::kRtn;
+  } else {
+    std::cerr << "[WARNINGS] Orbit observer noise frame is not defined!" << std::endl;
+    std::cerr << "The noise frame is automatically initialized as INERTIAL" << std::endl;
+    return NoiseFrame::kInertial;
+  }
+}
+
+OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) {
+  // General
+  IniAccess ini_file(file_name);
+
+  // CompoBase
+  int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler");
+  if (prescaler <= 1) prescaler = 1;
+
+  // Noise
+  const NoiseFrame noise_frame = SetNoiseFrame(ini_file.ReadString("ORBIT_OBSERVER", "noise_frame"));
+  libra::Vector<6> noise_standard_deviation;
+  ini_file.ReadVector("ORBIT_OBSERVER", "noise_standard_deviation", noise_standard_deviation);
+
+  OrbitObserver orbit_observer(prescaler, clock_generator, noise_frame, noise_standard_deviation, orbit);
+
+  return orbit_observer;
+}