diff --git a/data/sample/initialize_files/sample_satellite.ini b/data/sample/initialize_files/sample_satellite.ini index 11945da77..ff8aa024b 100644 --- a/data/sample/initialize_files/sample_satellite.ini +++ b/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 diff --git a/scripts/Plot/plot_ground_position.py b/scripts/Plot/plot_ground_position.py new file mode 100644 index 000000000..3a6df3f19 --- /dev/null +++ b/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() diff --git a/src/components/real/mission/telescope.cpp b/src/components/real/mission/telescope.cpp index 0b4b87e31..21a8efb71 100644 --- a/src/components/real/mission/telescope.cpp +++ b/src/components/real/mission/telescope.cpp @@ -6,6 +6,7 @@ #include "telescope.hpp" #include +#include #include #include @@ -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; } diff --git a/src/components/real/mission/telescope.hpp b/src/components/real/mission/telescope.hpp index 681a167e5..43dd5997f 100644 --- a/src/components/real/mission/telescope.hpp +++ b/src/components/real/mission/telescope.hpp @@ -7,6 +7,7 @@ #define S2E_COMPONENTS_REAL_MISSION_TELESCOPE_HPP_P_ #include +#include #include #include #include @@ -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_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_ diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index d97cc5a9d..2da0bc5cc 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/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_); } diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index 69f1ae82e..1c882bdb6 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -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