Skip to content

Commit 0eeb42a

Browse files
200km200km
authored
Merge pull request #726 from ut-issl/feature/add-earth-surface-pointing
Add Earth surface pointing mode for Controlled attitude
2 parents ed863e6 + 5472af4 commit 0eeb42a

File tree

4 files changed

+47
-12
lines changed

4 files changed

+47
-12
lines changed

‎settings/sample_satellite/satellite.ini

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@ initial_torque_b_Nm(2) = 0.000
3636
// VELOCITY_DIRECTION_POINTING
3737
// GROUND_SPEED_DIRECTION_POINTING
3838
// ORBIT_NORMAL_POINTING
39+
// EARTH_SURFACE_POINTING
3940
main_mode = INERTIAL_STABILIZE
4041
sub_mode = SUN_POINTING
4142

@@ -50,6 +51,11 @@ sub_pointing_direction_b(0) = 0.0
5051
sub_pointing_direction_b(1) = 0.0
5152
sub_pointing_direction_b(2) = 1.0
5253

54+
// Position of Target Earth Surface
55+
target_position_latitude_deg = 35.0
56+
target_position_longitude_deg = 139.0
57+
target_position_altitude_m = 0.0
58+
5359
[ORBIT]
5460
calculation = ENABLE
5561
logging = ENABLE

‎src/dynamics/attitude/controlled_attitude.cpp

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,14 +11,15 @@ namespace s2e::dynamics::attitude {
1111

1212
ControlledAttitude::ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const math::Quaternion quaternion_i2b,
1313
const math::Vector<3> main_target_direction_b, const math::Vector<3> sub_target_direction_b,
14-
const math::Matrix<3, 3>& inertia_tensor_kgm2,
14+
const math::Matrix<3, 3>& inertia_tensor_kgm2, const geodesy::GeodeticPosition target_earth_surface_position,
1515
const environment::LocalCelestialInformation* local_celestial_information, const orbit::Orbit* orbit,
1616
const std::string& simulation_object_name)
1717
: Attitude(inertia_tensor_kgm2, simulation_object_name),
1818
main_mode_(main_mode),
1919
sub_mode_(sub_mode),
2020
main_target_direction_b_(main_target_direction_b),
2121
sub_target_direction_b_(sub_target_direction_b),
22+
target_earth_surface_position_(target_earth_surface_position),
2223
local_celestial_information_(local_celestial_information),
2324
orbit_(orbit) {
2425
quaternion_i2b_ = quaternion_i2b;
@@ -100,6 +101,13 @@ math::Vector<3> ControlledAttitude::CalcTargetDirection_i(AttitudeControlMode mo
100101
direction = dcm_ecef2eci * orbit_->GetVelocity_ecef_m_s();
101102
} else if (mode == AttitudeControlMode::kOrbitNormalPointing) {
102103
direction = OuterProduct(orbit_->GetPosition_i_m(), orbit_->GetVelocity_i_m_s());
104+
} else if (mode == AttitudeControlMode::kEarthSurfacePointing) {
105+
math::Vector<3> earth_surface_target_position_ecef_m = target_earth_surface_position_.CalcEcefPosition();
106+
math::Vector<3> earth_surface_target_position_i_m =
107+
local_celestial_information_->GetGlobalInformation().GetEarthRotation().GetDcmJ2000ToEcef().Transpose() *
108+
earth_surface_target_position_ecef_m;
109+
110+
direction = earth_surface_target_position_i_m - orbit_->GetPosition_i_m();
103111
}
104112
direction = direction.CalcNormalizedVector();
105113
return direction;
@@ -149,6 +157,8 @@ AttitudeControlMode ConvertStringToCtrlMode(const std::string mode) {
149157
return AttitudeControlMode::kGroundSpeedDirectionPointing;
150158
} else if (mode == "ORBIT_NORMAL_POINTING") {
151159
return AttitudeControlMode::kOrbitNormalPointing;
160+
} else if (mode == "EARTH_SURFACE_POINTING") {
161+
return AttitudeControlMode::kEarthSurfacePointing;
152162
} else {
153163
return AttitudeControlMode::kNoControl;
154164
}

‎src/dynamics/attitude/controlled_attitude.hpp

Lines changed: 13 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ enum class AttitudeControlMode {
2626
kVelocityDirectionPointing, //!< Spacecraft velocity direction pointing
2727
kGroundSpeedDirectionPointing, //!< Ground speed direction pointing
2828
kOrbitNormalPointing, //!< Orbit normal direction pointing
29+
kEarthSurfacePointing, //!< Earth surface pointing
2930
kNoControl, // No Control
3031
};
3132

@@ -52,14 +53,16 @@ class ControlledAttitude : public Attitude {
5253
* @param [in] main_target_direction_b: Main target direction on the body fixed frame
5354
* @param [in] sub_target_direction_b: Sun target direction on the body fixed frame
5455
* @param [in] inertia_tensor_kgm2: Inertia tensor of the spacecraft [kg m^2]
56+
* @param [in] target_earth_surface_position: Target position on the Earth surface for Earth surface pointing mode
5557
* @param [in] local_celestial_information: Local celestial information
5658
* @param [in] orbit: Orbit
5759
* @param [in] simulation_object_name: Simulation object name for Monte-Carlo simulation
5860
*/
5961
ControlledAttitude(const AttitudeControlMode main_mode, const AttitudeControlMode sub_mode, const math::Quaternion quaternion_i2b,
6062
const math::Vector<3> main_target_direction_b, const math::Vector<3> sub_target_direction_b,
61-
const math::Matrix<3, 3>& inertia_tensor_kgm2, const environment::LocalCelestialInformation* local_celestial_information,
62-
const orbit::Orbit* orbit, const std::string& simulation_object_name = "attitude");
63+
const math::Matrix<3, 3>& inertia_tensor_kgm2, const geodesy::GeodeticPosition target_earth_surface_position,
64+
const environment::LocalCelestialInformation* local_celestial_information, const orbit::Orbit* orbit,
65+
const std::string& simulation_object_name = "attitude");
6366
/**
6467
* @fn ~ControlledAttitude
6568
* @brief Destructor
@@ -101,13 +104,14 @@ class ControlledAttitude : public Attitude {
101104
virtual void Propagate(const double end_time_s);
102105

103106
private:
104-
AttitudeControlMode main_mode_; //!< Main control mode
105-
AttitudeControlMode sub_mode_; //!< Sub control mode
106-
math::Vector<3> main_target_direction_b_; //!< Main target direction on the body fixed frame
107-
math::Vector<3> sub_target_direction_b_; //!< Sub target direction on tge body fixed frame
108-
double previous_calc_time_s_ = -1.0; //!< Previous time of velocity calculation [sec]
109-
math::Quaternion previous_quaternion_i2b_; //!< Previous quaternion
110-
math::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s]
107+
AttitudeControlMode main_mode_; //!< Main control mode
108+
AttitudeControlMode sub_mode_; //!< Sub control mode
109+
math::Vector<3> main_target_direction_b_; //!< Main target direction on the body fixed frame
110+
math::Vector<3> sub_target_direction_b_; //!< Sub target direction on tge body fixed frame
111+
double previous_calc_time_s_ = -1.0; //!< Previous time of velocity calculation [sec]
112+
math::Quaternion previous_quaternion_i2b_; //!< Previous quaternion
113+
math::Vector<3> previous_omega_b_rad_s_; //!< Previous angular velocity [rad/s]
114+
geodesy::GeodeticPosition target_earth_surface_position_; //!< Target position on the Earth surface
111115

112116
const double kMinDirectionAngle_rad = 30.0 * math::deg_to_rad; //!< Minimum angle b/w main and sub direction
113117
// TODO Change with ini file

‎src/dynamics/attitude/initialize_attitude.cpp

Lines changed: 17 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -36,8 +36,16 @@ Attitude* InitAttitude(std::string file_name, const orbit::Orbit* orbit, const e
3636
ini_file_ca.ReadVector(section_ca_, "main_pointing_direction_b", main_target_direction_b);
3737
ini_file_ca.ReadVector(section_ca_, "sub_pointing_direction_b", sub_target_direction_b);
3838
std::string mc_name_temp = section_ + std::to_string(spacecraft_id) + "_TEMP";
39+
40+
// Earth surface pointing mode
41+
double latitude_deg, longitude_deg, altitude_m;
42+
latitude_deg = ini_file_ca.ReadDouble(section_ca_, "target_position_latitude_deg");
43+
longitude_deg = ini_file_ca.ReadDouble(section_ca_, "target_position_longitude_deg");
44+
altitude_m = ini_file_ca.ReadDouble(section_ca_, "altitude_m");
45+
geodesy::GeodeticPosition target_position(latitude_deg * math::deg_to_rad, longitude_deg * math::deg_to_rad, altitude_m);
46+
3947
Attitude* attitude_temp = new ControlledAttitude(main_mode, sub_mode, quaternion_i2b, main_target_direction_b, sub_target_direction_b,
40-
inertia_tensor_kgm2, local_celestial_information, orbit, mc_name_temp);
48+
inertia_tensor_kgm2, target_position, local_celestial_information, orbit, mc_name_temp);
4149
attitude_temp->Propagate(step_width_s);
4250
quaternion_i2b = attitude_temp->GetQuaternion_i2b();
4351
omega_b = math::Vector<3>(0.0);
@@ -84,8 +92,15 @@ Attitude* InitAttitude(std::string file_name, const orbit::Orbit* orbit, const e
8492
ini_file_ca.ReadVector(section_ca_, "main_pointing_direction_b", main_target_direction_b);
8593
ini_file_ca.ReadVector(section_ca_, "sub_pointing_direction_b", sub_target_direction_b);
8694

95+
// Earth surface pointing mode
96+
double latitude_deg, longitude_deg, altitude_m;
97+
latitude_deg = ini_file_ca.ReadDouble(section_ca_, "target_position_latitude_deg");
98+
longitude_deg = ini_file_ca.ReadDouble(section_ca_, "target_position_longitude_deg");
99+
altitude_m = ini_file_ca.ReadDouble(section_ca_, "altitude_m");
100+
geodesy::GeodeticPosition target_position(latitude_deg * math::deg_to_rad, longitude_deg * math::deg_to_rad, altitude_m);
101+
87102
attitude = new ControlledAttitude(main_mode, sub_mode, quaternion_i2b, main_target_direction_b, sub_target_direction_b, inertia_tensor_kgm2,
88-
local_celestial_information, orbit, mc_name);
103+
target_position, local_celestial_information, orbit, mc_name);
89104
} else {
90105
std::cerr << "ERROR: attitude propagation mode: " << propagate_mode << " is not defined!" << std::endl;
91106
std::cerr << "The attitude mode is automatically set as RK4" << std::endl;

0 commit comments

Comments
 (0)