Merge pull request #529 from ut-issl/feature/add-observe-ground-position

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