From 9421a39a60eea8711ebaa8484e5565bcc6042cc0 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 17 Dec 2024 16:36:25 +0900 Subject: [PATCH] Add target direction calculation --- settings/sample_satellite/satellite.ini | 4 ++-- src/dynamics/attitude/controlled_attitude.cpp | 10 ++++++++-- src/dynamics/attitude/controlled_attitude.hpp | 4 +++- src/dynamics/attitude/initialize_attitude.cpp | 12 ++++++++++-- 4 files changed, 23 insertions(+), 7 deletions(-) diff --git a/settings/sample_satellite/satellite.ini b/settings/sample_satellite/satellite.ini index 715ea908f..d52ffd5e4 100644 --- a/settings/sample_satellite/satellite.ini +++ b/settings/sample_satellite/satellite.ini @@ -3,7 +3,7 @@ // RK4 : Attitude Propagation with RK4 including disturbances and control torque // CANTILEVER_VIBRATION : Attitude Propagation with the consideration of the cantilever vibration (flexible structure) including disturbances and control torque. // CONTROLLED : Attitude Calculation with Controlled Attitude mode. All disturbances and control torque are ignored. -propagate_mode = RK4 +propagate_mode = CONTROLLED // Initialize Attitude mode // MANUAL : Initialize Quaternion_i2b manually below @@ -37,7 +37,7 @@ initial_torque_b_Nm(2) = 0.000 // GROUND_SPEED_DIRECTION_POINTING // ORBIT_NORMAL_POINTING // EARTH_SURFACE_POINTING -main_mode = INERTIAL_STABILIZE +main_mode = EARTH_SURFACE_POINTING sub_mode = SUN_POINTING // Pointing direction @ body frame for main pointing mode diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index 29b12fa44..e910f916c 100644 --- a/src/dynamics/attitude/controlled_attitude.cpp +++ b/src/dynamics/attitude/controlled_attitude.cpp @@ -11,7 +11,7 @@ namespace s2e::dynamics::attitude { ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const math::Quaternion quaternion_i2b, const math::Vector<3> main_target_direction_b, const math::Vector<3> sub_target_direction_b, - const math::Matrix<3, 3>& inertia_tensor_kgm2, + const math::Matrix<3, 3>& inertia_tensor_kgm2, const geodesy::GeodeticPosition target_earth_surface_position, const environment::LocalCelestialInformation* local_celestial_information, const orbit::Orbit* orbit, const std::string& simulation_object_name) : Attitude(inertia_tensor_kgm2, simulation_object_name), @@ -19,6 +19,7 @@ ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, cons sub_mode_(sub_mode), main_target_direction_b_(main_target_direction_b), sub_target_direction_b_(sub_target_direction_b), + target_earth_surface_position_(target_earth_surface_position), local_celestial_information_(local_celestial_information), orbit_(orbit) { quaternion_i2b_ = quaternion_i2b; @@ -100,6 +101,11 @@ math::Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMode mo direction = dcm_ecef2eci * orbit_->GetVelocity_ecef_m_s(); } else if (mode == AttitudeControlMode::kOrbitNormalPointing) { direction = OuterProduct(orbit_->GetPosition_i_m(), orbit_->GetVelocity_i_m_s()); + } else if (mode == AttitudeControlMode::kEarthSurfacePointing) { + math::Vector<3> earth_surface_target_position_ecef_m = target_earth_surface_position_.CalcEcefPosition(); + math::Vector<3> earth_surface_target_position_i_m = local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose()*earth_surface_target_position_ecef_m; + + direction = earth_surface_target_position_i_m - orbit_->GetPosition_i_m(); } direction = direction.CalcNormalizedVector(); return direction; @@ -149,7 +155,7 @@ AttitudeControlMode ConvertStringToCtrlMode(const std::string mode) { return AttitudeControlMode::kGroundSpeedDirectionPointing; } else if (mode == "ORBIT_NORMAL_POINTING") { return AttitudeControlMode::kOrbitNormalPointing; - } else if (mode == "Earth_SURFACE_POINTING") { + } else if (mode == "EARTH_SURFACE_POINTING") { return AttitudeControlMode::kEarthSurfacePointing; } else { return AttitudeControlMode::kNoControl; diff --git a/src/dynamics/attitude/controlled_attitude.hpp b/src/dynamics/attitude/controlled_attitude.hpp index 430403096..b0cfa1d15 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -53,13 +53,15 @@ class ControlledAttitude : public Attitude { * @param [in] main_target_direction_b: Main target direction on the body fixed frame * @param [in] sub_target_direction_b: Sun target direction on the body fixed frame * @param [in] inertia_tensor_kgm2: Inertia tensor of the spacecraft [kg m^2] + * @param [in] target_earth_surface_position: Target position on the Earth surface for Earth surface pointing mode * @param [in] local_celestial_information: Local celestial information * @param [in] orbit: Orbit * @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation */ ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const math::Quaternion quaternion_i2b, const math::Vector<3> main_target_direction_b, const math::Vector<3> sub_target_direction_b, - const math::Matrix<3, 3>& inertia_tensor_kgm2, const environment::LocalCelestialInformation* local_celestial_information, + const math::Matrix<3, 3>& inertia_tensor_kgm2, const geodesy::GeodeticPosition target_earth_surface_position, + const environment::LocalCelestialInformation* local_celestial_information, const orbit::Orbit* orbit, const std::string& simulation_object_name = "attitude"); /** * @fn ~ControlledAttitude diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index 5f73b16c1..2119bae88 100644 --- a/src/dynamics/attitude/initialize_attitude.cpp +++ b/src/dynamics/attitude/initialize_attitude.cpp @@ -36,8 +36,16 @@ Attitude* InitAttitude(std::string file_name, const orbit::Orbit* orbit, const e ini_file_ca.ReadVector(section_ca_, "main_pointing_direction_b", main_target_direction_b); ini_file_ca.ReadVector(section_ca_, "sub_pointing_direction_b", sub_target_direction_b); std::string mc_name_temp = section_ + std::to_string(spacecraft_id) + "_TEMP"; + + // Earth surface pointing mode + double latitude_deg, longitude_deg, altitude_m; + latitude_deg = ini_file_ca.ReadDouble(section_ca_, "target_position_latitude_deg"); + longitude_deg = ini_file_ca.ReadDouble(section_ca_, "target_position_longitude_deg"); + altitude_m = ini_file_ca.ReadDouble(section_ca_, "altitude_m"); + geodesy::GeodeticPosition target_position(latitude_deg * math::deg_to_rad, longitude_deg * math::deg_to_rad, altitude_m); + Attitude* attitude_temp = new ControlledAttitude(main_mode, sub_mode, quaternion_i2b, main_target_direction_b, sub_target_direction_b, - inertia_tensor_kgm2, local_celestial_information, orbit, mc_name_temp); + inertia_tensor_kgm2, target_position, local_celestial_information, orbit, mc_name_temp); attitude_temp->Propagate(step_width_s); quaternion_i2b = attitude_temp->GetQuaternion_i2b(); omega_b = math::Vector<3>(0.0); @@ -92,7 +100,7 @@ Attitude* InitAttitude(std::string file_name, const orbit::Orbit* orbit, const e geodesy::GeodeticPosition target_position(latitude_deg * math::deg_to_rad, longitude_deg * math::deg_to_rad, altitude_m); attitude = new ControlledAttitude(main_mode, sub_mode, quaternion_i2b, main_target_direction_b, sub_target_direction_b, inertia_tensor_kgm2, - local_celestial_information, orbit, mc_name); + target_position, local_celestial_information, orbit, mc_name); } else { std::cerr << "ERROR: attitude propagation mode: " << propagate_mode << " is not defined!" << std::endl; std::cerr << "The attitude mode is automatically set as RK4" << std::endl;