diff --git a/settings/sample_satellite/satellite.ini b/settings/sample_satellite/satellite.ini index 3a89dc56f..715ea908f 100644 --- a/settings/sample_satellite/satellite.ini +++ b/settings/sample_satellite/satellite.ini @@ -36,6 +36,7 @@ initial_torque_b_Nm(2) = 0.000 // VELOCITY_DIRECTION_POINTING // GROUND_SPEED_DIRECTION_POINTING // ORBIT_NORMAL_POINTING +// EARTH_SURFACE_POINTING main_mode = INERTIAL_STABILIZE sub_mode = SUN_POINTING @@ -50,6 +51,11 @@ sub_pointing_direction_b(0) = 0.0 sub_pointing_direction_b(1) = 0.0 sub_pointing_direction_b(2) = 1.0 +// Position of Target Earth Surface +target_position_latitude_deg = 35.0 +target_position_longitude_deg = 139.0 +target_position_altitude_m = 0.0 + [ORBIT] calculation = ENABLE logging = ENABLE diff --git a/src/dynamics/attitude/controlled_attitude.cpp b/src/dynamics/attitude/controlled_attitude.cpp index d852dc9e3..e92cc1341 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,13 @@ 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,6 +157,8 @@ AttitudeControlMode ConvertStringToCtrlMode(const std::string mode) { return AttitudeControlMode::kGroundSpeedDirectionPointing; } else if (mode == "ORBIT_NORMAL_POINTING") { return AttitudeControlMode::kOrbitNormalPointing; + } 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 c2853871e..bd3db1410 100644 --- a/src/dynamics/attitude/controlled_attitude.hpp +++ b/src/dynamics/attitude/controlled_attitude.hpp @@ -26,6 +26,7 @@ enum class AttitudeControlMode { kVelocityDirectionPointing, //!< Spacecraft velocity direction pointing kGroundSpeedDirectionPointing, //!< Ground speed direction pointing kOrbitNormalPointing, //!< Orbit normal direction pointing + kEarthSurfacePointing, //!< Earth surface pointing kNoControl, // No Control }; @@ -52,14 +53,16 @@ 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 orbit::Orbit* orbit, const std::string& simulation_object_name = "attitude"); + 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 * @brief Destructor @@ -101,13 +104,14 @@ class ControlledAttitude : public Attitude { virtual void Propagate(const double end_time_s); private: - AttitudeControlMode main_mode_; //!< Main control mode - AttitudeControlMode sub_mode_; //!< Sub control mode - math::Vector<3> main_target_direction_b_; //!< Main target direction on the body fixed frame - math::Vector<3> sub_target_direction_b_; //!< Sub target direction on tge body fixed frame - double previous_calc_time_s_ = -1.0; //!< Previous time of velocity calculation [sec] - math::Quaternion previous_quaternion_i2b_; //!< Previous quaternion - math::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s] + AttitudeControlMode main_mode_; //!< Main control mode + AttitudeControlMode sub_mode_; //!< Sub control mode + math::Vector<3> main_target_direction_b_; //!< Main target direction on the body fixed frame + math::Vector<3> sub_target_direction_b_; //!< Sub target direction on tge body fixed frame + double previous_calc_time_s_ = -1.0; //!< Previous time of velocity calculation [sec] + math::Quaternion previous_quaternion_i2b_; //!< Previous quaternion + math::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s] + geodesy::GeodeticPosition target_earth_surface_position_; //!< Target position on the Earth surface const double kMinDirectionAngle_rad = 30.0 * math::deg_to_rad; //!< Minimum angle b/w main and sub direction // TODO Change with ini file diff --git a/src/dynamics/attitude/initialize_attitude.cpp b/src/dynamics/attitude/initialize_attitude.cpp index f5dbb7b82..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); @@ -84,8 +92,15 @@ 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); + // 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 = 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;