Add target direction calculation

ut-issl · Dec 17, 2024 · 9421a39 · 9421a39
1 parent adbc79b
commit 9421a39
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 7 deletions.
diff --git 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
@@ -11,14 +11,15 @@ 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),
       main_mode_(main_mode),
       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
@@ -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
@@ -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;