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_argument.py

@@ -0,0 +1,67 @@
+#
+# Plot Ground Position Argument
+#
+# 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_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
+x_data = read_scalar_from_csv(read_file_name, 'telescope_ground_position_angle_z[rad]')
+y_data = read_scalar_from_csv(read_file_name, 'telescope_ground_position_angle_y[rad]')
+#
+# Plot
+#
+plt.figure(figsize=(10, 7))
+plt.scatter(x_data, y_data, s=2, alpha=0.5)
+plt.title("Scatter plot of telescope ground position angles")
+plt.xlabel("telescope_ground_position_angle_z[rad]")
+plt.ylabel("telescope_ground_position_angle_y[rad]")
+plt.xlim(-0.3,0.05)
+plt.ylim(-0.0025,0.0025)
+plt.grid(True)
+
+# Data save
+if args.no_gui:
+  plt.savefig(read_file_tag + "_ground_position_argument.png") # save last figure only
+else:
+  plt.show()

src/components/real/mission/telescope.cpp

@@ -8,14 +8,16 @@
 #include <cassert>
 #include <library/initialize/initialize_file_access.hpp>
 #include <library/math/constants.hpp>
+#include <environment/global/physical_constants.hpp>
 
 using namespace std;
 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,10 @@ Telescope::Telescope(ClockGenerator* clock_generator, const libra::Quaternion& q
 
     star_list_in_sight.push_back(star);
   }
+  //Get initial spacecraft position in ECEF
+  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 +85,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
+  ObserveGroundPosition();
 }
 
 bool Telescope::JudgeForbiddenAngle(const libra::Vector<3>& target_b, const double forbidden_angle) {
@@ -150,6 +159,27 @@ void Telescope::ObserveStars() {
   }
 }
 
+
+ void Telescope::ObserveGroundPosition() {
+   Quaternion quaternion_i2b = attitude_->GetQuaternion_i2b();
+   libra::Vector<3> direction_ecef;
+   libra::Vector<3> direction_b;
+   libra::Vector<3> direction_i;
+   libra::Vector<3> spacecraft_position_ecef_m = orbit_->GetPosition_ecef_m(); // Get spacecraft position in ECEF
+
+   direction_ecef = initial_ground_position_ecef_m_ - spacecraft_position_ecef_m; // Get the direction vector from spacecraft to ground point in ECEF
+   // Get the Direction Cosine Matrix (DCM) from ECEF to ECI
+   libra::Matrix<3, 3> dcm_ecef_to_i = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose();
+   // Convert the position vector in ECEF to the vector in ECI
+   direction_i = (dcm_ecef_to_i * direction_ecef).CalcNormalizedVector();
+   // Convert the position vector in ECI to the vector in body frame
+   direction_b = quaternion_i2b.FrameConversion(direction_i);
+   libra::Vector<3> target_c = quaternion_b2c_.FrameConversion(direction_b); // Get ground position direction vector in component frame (c)
+
+   ground_arg_z_rad_ = atan2(target_c[2], target_c[0]); // Angle from X-axis on XZ plane in the component frame
+   ground_arg_y_rad_ = atan2(target_c[1], target_c[0]); // Angle from X-axis on XY plane in the component frame
+ }
+
 string Telescope::GetLogHeader() const {
   string str_tmp = "";
 
@@ -161,6 +191,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_angle_z", "rad");
+  str_tmp += WriteScalar(component_name + "ground_position_angle_y", "rad");
   // 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 +218,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_arg_z_rad_);
+  str_tmp += WriteScalar(ground_arg_y_rad_);
   // 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 +238,8 @@ 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);
@@ -240,6 +275,6 @@ Telescope InitTelescope(ClockGenerator* clock_generator, int sensor_id, const st
 
   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);
+                      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);
   /**
    * @fn ~Telescope
    * @brief Destructor
@@ -78,6 +80,8 @@ class Telescope : public Component, public ILoggable {
   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_arg_z_rad_ = 0.0; //!< Ground position argument z
+  double ground_arg_y_rad_ = 0.0; //!< Ground position argument 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,8 @@ 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_spacecraft_position_ecef_m_; //!< Initial spacecraft position
+  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,6 +128,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 Observe
+   * @brief Observe Ground Position
+   */
+   void ObserveGroundPosition();
+
+   const Orbit* orbit_;        //!< Orbit information
 
   // Override ILoggable
   /**
@@ -156,6 +169,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);
 
 #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

@@ -24,6 +24,7 @@
 #include <components/real/communication/antenna.hpp>
 #include <components/real/power/power_control_unit.hpp>
 #include <components/real/propulsion/simple_thruster.hpp>
+#include <components/real/mission/telescope.hpp>
 #include <dynamics/dynamics.hpp>
 #include <library/math/vector.hpp>
 #include <simulation/hils/hils_port_manager.hpp>
@@ -42,6 +43,7 @@ class ReactionWheel;
 class SimpleThruster;
 class ForceGenerator;
 class TorqueGenerator;
+class Telescope;
 
 /**
  * @class SampleComponents
@@ -105,6 +107,8 @@ class SampleComponents : public InstalledComponents {
   ForceGenerator* force_generator_;    //!< Ideal Force Generator
   TorqueGenerator* torque_generator_;  //!< Ideal Torque Generator
 
+  // Mission
+  Telescope* telescope_;  //!< Telescope
   // CommGs
   Antenna* antenna_;  //!< Antenna