Skip to content

Commit

Permalink
Merge pull request #726 from ut-issl/feature/add-earth-surface-pointing
Browse files Browse the repository at this point in the history 
Add Earth surface pointing mode for Controlled attitude
  • Loading branch information
@200km
200km authored Dec 17, 2024
2 parents ed863e6 + 5472af4 commit 0eeb42a
Show file tree
Hide file tree
Showing 4 changed files with 47 additions and 12 deletions.
6 changes: 6 additions & 0 deletions settings/sample_satellite/satellite.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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
Expand Down
12 changes: 11 additions & 1 deletion src/dynamics/attitude/controlled_attitude.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down
22 changes: 13 additions & 9 deletions src/dynamics/attitude/controlled_attitude.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
};

Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down
19 changes: 17 additions & 2 deletions src/dynamics/attitude/initialize_attitude.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 0eeb42a

Please sign in to comment.