Add ObserveGroundPositionDeviation Function

data/sample/initialize_files/sample_satellite.ini

@@ -146,3 +146,4 @@ force_generator_file = INI_FILE_DIR_FROM_EXE/components/force_generator.ini
 torque_generator_file = INI_FILE_DIR_FROM_EXE/components/torque_generator.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/plot_ground_position.py

@@ -0,0 +1,68 @@
+#
+# Plot Ground Position in the image sensor
+#
+# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946
+#
+
+#
+# Import
+#
+# plots
+import matplotlib.pyplot as plt
+# local function
+from common import find_latest_log_tag
+from common import add_log_file_arguments
+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
+x_data = read_scalar_from_csv(
+    read_file_name, 'telescope_ground_position_x[pix]')
+y_data = read_scalar_from_csv(
+    read_file_name, 'telescope_ground_position_y[pix]')
+#
+# Plot
+#
+plt.figure(figsize=(10, 7))
+plt.scatter(x_data, y_data, s=2, alpha=1.0)
+plt.title("Scatter plot of ground position in the image sensor")
+plt.xlabel("X [pix]")
+plt.ylabel("Y [pix]")
+plt.grid(True)
+
+# Data save
+if args.no_gui:
+    # save last figure only
+    plt.savefig(read_file_tag + "_ground_position.png")
+else:
+    plt.show()

src/components/real/mission/telescope.cpp

@@ -6,6 +6,7 @@
 #include "telescope.hpp"
 
 #include <cassert>
+#include <environment/global/physical_constants.hpp>
 #include <library/initialize/initialize_file_access.hpp>
 #include <library/math/constants.hpp>
 
@@ -15,7 +16,8 @@ using namespace libra;
 Telescope::Telescope(ClockGenerator* clock_generator, const libra::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad,
                      const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix,
                      const int y_number_of_pix, const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars,
-                     const Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information)
+                     const Attitude* attitude, const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information,
+                     const Orbit* orbit)
     : Component(1, clock_generator),
       quaternion_b2c_(quaternion_b2c),
       sun_forbidden_angle_rad_(sun_forbidden_angle_rad),
@@ -28,7 +30,8 @@ Telescope::Telescope(ClockGenerator* clock_generator, const libra::Quaternion& q
       number_of_logged_stars_(number_of_logged_stars),
       attitude_(attitude),
       hipparcos_(hipparcos),
-      local_celestial_information_(local_celestial_information) {
+      local_celestial_information_(local_celestial_information),
+      orbit_(orbit) {
   is_sun_in_forbidden_angle = true;
   is_earth_in_forbidden_angle = true;
   is_moon_in_forbidden_angle = true;
@@ -53,6 +56,12 @@ Telescope::Telescope(ClockGenerator* clock_generator, const libra::Quaternion& q
 
     star_list_in_sight.push_back(star);
   }
+  // Get initial spacecraft position in ECEF
+  if (orbit_ != nullptr) {
+    libra::Vector<3> initial_spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m();
+    initial_ground_position_ecef_m_ = environment::earth_equatorial_radius_m * initial_spacecraft_position_ecef_m;
+    initial_ground_position_ecef_m_ /= (orbit_->GetGeodeticPosition().GetAltitude_m() + environment::earth_equatorial_radius_m);
+  }
 }
 
 Telescope::~Telescope() {}
@@ -78,6 +87,8 @@ void Telescope::MainRoutine(const int time_count) {
   // angle_earth = CalcAngleTwoVectors_rad(sight_direction_c_, earth_pos_c) * 180 / libra::pi; angle_moon =
   // CalcAngleTwoVectors_rad(sight_direction_c_, moon_pos_c) * 180 / libra::pi;
   //******************************************************************************
+  // Direction calculation of ground point
+  ObserveGroundPositionDeviation();
 }
 
 bool Telescope::JudgeForbiddenAngle(const libra::Vector<3>& target_b, const double forbidden_angle) {
@@ -150,6 +161,33 @@ void Telescope::ObserveStars() {
   }
 }
 
+void Telescope::ObserveGroundPositionDeviation() {
+  // Orbit information is not available, so skip the ground position calculation
+  if (orbit_ == nullptr) {
+    return;
+  }
+  // Check if the ground point is in the image sensor
+  if (ground_position_x_image_sensor_ > x_number_of_pix_ || ground_position_y_image_sensor_ > y_number_of_pix_) {
+    ground_position_x_image_sensor_ = -1;
+    ground_position_y_image_sensor_ = -1;
+    return;
+  }
+
+  Quaternion quaternion_i2b = attitude_->GetQuaternion_i2b();
+  libra::Vector<3> spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m();
+  libra::Vector<3> direction_ecef = (initial_ground_position_ecef_m_ - spacecraft_position_ecef_m).CalcNormalizedVector();
+  libra::Matrix<3, 3> dcm_ecef_to_i = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose();
+  libra::Vector<3> direction_i = (dcm_ecef_to_i * direction_ecef).CalcNormalizedVector();
+  libra::Vector<3> direction_b = quaternion_i2b.FrameConversion(direction_i);
+  libra::Vector<3> target_c = quaternion_b2c_.FrameConversion(direction_b);
+
+  // Ground position in the image sensor in the satellite frame
+  double ground_angle_z_rad = atan2(target_c[2], target_c[0]);
+  ground_position_x_image_sensor_ = ground_angle_z_rad / x_fov_per_pix_;
+  double ground_angle_y_rad = atan2(target_c[1], target_c[0]);
+  ground_position_y_image_sensor_ = ground_angle_y_rad / y_fov_per_pix_;
+}
+
 string Telescope::GetLogHeader() const {
   string str_tmp = "";
 
@@ -161,6 +199,8 @@ string Telescope::GetLogHeader() const {
   str_tmp += WriteVector(component_name + "sun_position", "img", "pix", 2);
   str_tmp += WriteVector(component_name + "earth_position", "img", "pix", 2);
   str_tmp += WriteVector(component_name + "moon_position", "img", "pix", 2);
+  str_tmp += WriteScalar(component_name + "ground_position_x", "pix");
+  str_tmp += WriteScalar(component_name + "ground_position_y", "pix");
   // When Hipparcos Catalogue was not read, no output of ObserveStars
   if (hipparcos_->IsCalcEnabled) {
     for (size_t i = 0; i < number_of_logged_stars_; i++) {
@@ -186,6 +226,8 @@ string Telescope::GetLogValue() const {
   str_tmp += WriteVector(sun_position_image_sensor);
   str_tmp += WriteVector(earth_position_image_sensor);
   str_tmp += WriteVector(moon_position_image_sensor);
+  str_tmp += WriteScalar(ground_position_x_image_sensor_);
+  str_tmp += WriteScalar(ground_position_y_image_sensor_);
   // When Hipparcos Catalogue was not read, no output of ObserveStars
   if (hipparcos_->IsCalcEnabled) {
     for (size_t i = 0; i < number_of_logged_stars_; i++) {
@@ -204,7 +246,7 @@ string Telescope::GetLogValue() const {
 }
 
 Telescope InitTelescope(ClockGenerator* clock_generator, int sensor_id, const string file_name, const Attitude* attitude,
-                        const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information) {
+                        const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit) {
   using libra::pi;
 
   IniAccess Telescope_conf(file_name);
@@ -239,7 +281,7 @@ Telescope InitTelescope(ClockGenerator* clock_generator, int sensor_id, const st
   int number_of_logged_stars = Telescope_conf.ReadInt(TelescopeSection, "number_of_stars_for_log");
 
   Telescope telescope(clock_generator, quaternion_b2c, sun_forbidden_angle_rad, earth_forbidden_angle_rad, moon_forbidden_angle_rad, x_number_of_pix,
-                      y_number_of_pix, x_fov_per_pix_rad, y_fov_per_pix_rad, number_of_logged_stars, attitude, hipparcos,
-                      local_celestial_information);
+                      y_number_of_pix, x_fov_per_pix_rad, y_fov_per_pix_rad, number_of_logged_stars, attitude, hipparcos, local_celestial_information,
+                      orbit);
   return telescope;
 }

src/components/real/mission/telescope.hpp

@@ -7,6 +7,7 @@
 #define S2E_COMPONENTS_REAL_MISSION_TELESCOPE_HPP_P_
 
 #include <dynamics/attitude/attitude.hpp>
+#include <dynamics/orbit/orbit.hpp>
 #include <environment/global/hipparcos_catalogue.hpp>
 #include <environment/local/local_celestial_information.hpp>
 #include <library/logger/loggable.hpp>
@@ -47,11 +48,12 @@ class Telescope : public Component, public ILoggable {
    * @param [in] attitude: Attitude Information
    * @param [in] hipparcos: Hipparcos catalogue information
    * @param [in] local_celestial_information: Local celestial information
+   * @param [in] orbit: Orbit information
    */
   Telescope(ClockGenerator* clock_generator, const libra::Quaternion& quaternion_b2c, const double sun_forbidden_angle_rad,
             const double earth_forbidden_angle_rad, const double moon_forbidden_angle_rad, const int x_number_of_pix, const int y_number_of_pix,
             const double x_fov_per_pix, const double y_fov_per_pix, size_t number_of_logged_stars, const Attitude* attitude,
-            const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information);
+            const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information, const Orbit* orbit = nullptr);
   /**
    * @fn ~Telescope
    * @brief Destructor
@@ -72,12 +74,14 @@ class Telescope : public Component, public ILoggable {
   double earth_forbidden_angle_rad_;  //!< Earth forbidden angle [rad]
   double moon_forbidden_angle_rad_;   //!< Moon forbidden angle [rad]
 
-  int x_number_of_pix_;        //!< Number of pixel on X-axis in the image plane
-  int y_number_of_pix_;        //!< Number of pixel on Y-axis in the image plane
-  double x_fov_per_pix_;       //!< Field of view per pixel of X-axis in the image plane [rad/pix]
-  double y_fov_per_pix_;       //!< Field of view per pixel of Y-axis in the image plane [rad/pix]
-  double x_field_of_view_rad;  //!< Field of view of X-axis in the image plane [rad/pix]
-  double y_field_of_view_rad;  //!< Field of view of Y-axis in the image plane [rad/pix]
+  int x_number_of_pix_;                          //!< Number of pixel on X-axis in the image plane
+  int y_number_of_pix_;                          //!< Number of pixel on Y-axis in the image plane
+  double x_fov_per_pix_;                         //!< Field of view per pixel of X-axis in the image plane [rad/pix]
+  double y_fov_per_pix_;                         //!< Field of view per pixel of Y-axis in the image plane [rad/pix]
+  double x_field_of_view_rad;                    //!< Field of view of X-axis in the image plane [rad/pix]
+  double y_field_of_view_rad;                    //!< Field of view of Y-axis in the image plane [rad/pix]
+  double ground_position_x_image_sensor_ = 0.0;  //!< Ground position x
+  double ground_position_y_image_sensor_ = 0.0;  //!< Ground position y
 
   bool is_sun_in_forbidden_angle = false;    //!< Is the sun in the forbidden angle
   bool is_earth_in_forbidden_angle = false;  //!< Is the earth in the forbidden angle
@@ -88,6 +92,7 @@ class Telescope : public Component, public ILoggable {
   libra::Vector<2> sun_position_image_sensor{-1};    //!< Position of the sun on the image plane
   libra::Vector<2> earth_position_image_sensor{-1};  //!< Position of the earth on the image plane
   libra::Vector<2> moon_position_image_sensor{-1};   //!< Position of the moon on the image plane
+  libra::Vector<3> initial_ground_position_ecef_m_;  //!< Initial spacecraft position
 
   std::vector<Star> star_list_in_sight;  //!< Star information in the field of view
 
@@ -122,7 +127,13 @@ class Telescope : public Component, public ILoggable {
   const Attitude* attitude_;                                      //!< Attitude information
   const HipparcosCatalogue* hipparcos_;                           //!< Star information
   const LocalCelestialInformation* local_celestial_information_;  //!< Local celestial information
+  /**
+   * @fn ObserveGroundPositionDeviation
+   * @brief Calculate the deviation of the ground position from its initial value in the image sensor
+   */
+  void ObserveGroundPositionDeviation();
 
+  const Orbit* orbit_;  //!< Orbit information
   // Override ILoggable
   /**
    * @fn GetLogHeader
@@ -156,6 +167,7 @@ class Telescope : public Component, public ILoggable {
  * @param [in] local_celestial_information: Local celestial information
  */
 Telescope InitTelescope(ClockGenerator* clock_generator, int sensor_id, const std::string file_name, const Attitude* attitude,
-                        const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information);
+                        const HipparcosCatalogue* hipparcos, const LocalCelestialInformation* local_celestial_information,
+                        const Orbit* orbit = nullptr);
 
 #endif  // S2E_COMPONENTS_REAL_MISSION_TELESCOPE_HPP_P_

src/simulation_sample/spacecraft/sample_components.cpp

@@ -83,6 +83,13 @@ SampleComponents::SampleComponents(const Dynamics* dynamics, Structure* structur
   configuration_->main_logger_->CopyFileToLogDirectory(file_name);
   thruster_ = new SimpleThruster(InitSimpleThruster(clock_generator, pcu_->GetPowerPort(2), 1, file_name, structure_, dynamics));
 
+  // Mission
+  const std::string telescope_ini_path = iniAccess.ReadString("COMPONENT_FILES", "telescope_file");
+  configuration_->main_logger_->CopyFileToLogDirectory(file_name);
+  telescope_ =
+      new Telescope(InitTelescope(clock_generator, 1, telescope_ini_path, &(dynamics_->GetAttitude()), &(global_environment_->GetHipparcosCatalog()),
+                                  &(local_environment_->GetCelestialInformation()), &(dynamics_->GetOrbit())));
+
   // Force Generator
   file_name = iniAccess.ReadString("COMPONENT_FILES", "force_generator_file");
   configuration_->main_logger_->CopyFileToLogDirectory(file_name);
@@ -190,6 +197,7 @@ void SampleComponents::LogSetup(Logger& logger) {
   logger.AddLogList(magnetorquer_);
   logger.AddLogList(reaction_wheel_);
   logger.AddLogList(thruster_);
+  logger.AddLogList(telescope_);
   logger.AddLogList(force_generator_);
   logger.AddLogList(torque_generator_);
 }

src/simulation_sample/spacecraft/sample_components.hpp

@@ -22,6 +22,7 @@
 #include <components/real/aocs/sun_sensor.hpp>
 #include <components/real/cdh/on_board_computer.hpp>
 #include <components/real/communication/antenna.hpp>
+#include <components/real/mission/telescope.hpp>
 #include <components/real/power/power_control_unit.hpp>
 #include <components/real/propulsion/simple_thruster.hpp>
 #include <dynamics/dynamics.hpp>
@@ -42,6 +43,7 @@ class ReactionWheel;
 class SimpleThruster;
 class ForceGenerator;
 class TorqueGenerator;
+class Telescope;
 
 /**
  * @class SampleComponents
@@ -105,6 +107,9 @@ class SampleComponents : public InstalledComponents {
   ForceGenerator* force_generator_;    //!< Ideal Force Generator
   TorqueGenerator* torque_generator_;  //!< Ideal Torque Generator
 
+  // Mission
+  Telescope* telescope_;  //!< Telescope
+
   // CommGs
   Antenna* antenna_;  //!< Antenna