diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index f30c7692..bfe4b052 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -86,6 +86,8 @@ set(SOURCE_FILES src/Components/AOCS/InitializeRelativePositionSensor.cpp src/Components/IdealComponents/ForceGenerator.cpp src/Components/IdealComponents/InitializeForceGenerator.cpp + src/Components/IdealComponents/RelativeAttitudeController.cpp + src/Components/IdealComponents/InitializeRelativeAttitudeController.cpp src/Simulation/Case/FfCase.cpp src/Simulation/Spacecraft/FfSat.cpp src/Simulation/Spacecraft/FfComponents.cpp diff --git a/s2e-ff/data/ini/Components/RelativeAttitudeController.ini b/s2e-ff/data/ini/Components/RelativeAttitudeController.ini new file mode 100644 index 00000000..aced840c --- /dev/null +++ b/s2e-ff/data/ini/Components/RelativeAttitudeController.ini @@ -0,0 +1,26 @@ +[RelativeAttitudeController] +// Attitude controle mode settings. Select from the following modes. +// - TARGET_SATELLITE_POINTING +// - SUN_POINTING +// - EARTH_CENTER_POINTING +// - VELOCITY_DIRECTION_POINTING +// - ORBIT_NORMAL_POINTING +main_mode = TARGET_SATELLITE_POINTING +sub_mode = ORBIT_NORMAL_POINTING + +// Target direction of body frame +main_target_direction_b(0) = 1.0 +main_target_direction_b(1) = 0.0 +main_target_direction_b(2) = 0.0 + +sub_target_direction_b(0) = 0.0 +sub_target_direction_b(1) = 1.0 +sub_target_direction_b(2) = 0.0 + +// Satellite ID settings +target_sat_id = 1 +reference_sat_id = 0 // Reccomend to set as the mounted satellite ID + +[ComponentBase] +// Prescaler with respect to the component update period +prescaler = 1 diff --git a/s2e-ff/data/ini/Components/RelativePositionSensor.ini b/s2e-ff/data/ini/Components/RelativePositionSensor.ini index 62bd9243..fc73fa94 100644 --- a/s2e-ff/data/ini/Components/RelativePositionSensor.ini +++ b/s2e-ff/data/ini/Components/RelativePositionSensor.ini @@ -27,14 +27,14 @@ scale_factor_c(7) = 0; scale_factor_c(8) = 1; // Constant bias noise [m] -constant_bias_c(0) = 10.0 -constant_bias_c(1) = 10.0 -constant_bias_c(2) = 10.0 +constant_bias_c(0) = 0.0 +constant_bias_c(1) = 0.0 +constant_bias_c(2) = 0.0 // Standard deviation of normal random noise [m] -normal_random_standard_deviation_c(0) = 1.0 -normal_random_standard_deviation_c(1) = 1.0 -normal_random_standard_deviation_c(2) = 1.0 +normal_random_standard_deviation_c(0) = 0.0 +normal_random_standard_deviation_c(1) = 0.0 +normal_random_standard_deviation_c(2) = 0.0 // Standard deviation for random walk noise [m] random_walk_standard_deviation_c(0) = 0.0 diff --git a/s2e-ff/data/ini/FfSat.ini b/s2e-ff/data/ini/FfSat.ini index a21373eb..c1c54337 100644 --- a/s2e-ff/data/ini/FfSat.ini +++ b/s2e-ff/data/ini/FfSat.ini @@ -2,7 +2,7 @@ // Attitude propagation mode // RK4 : Attitude Propagation with RK4 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 // Initial angular velocity at body frameļ¼Œ[rad/s] Omega_b(0) = 0.0 @@ -141,3 +141,4 @@ structure_file = ../../data/ini/FfSatStructure.ini relative_distance_sensor_file = ../../data/ini/Components/RelativeDistanceSensor.ini relative_position_sensor_file = ../../data/ini/Components/RelativePositionSensor.ini force_generator_file = ../../data/ini/Components/ForceGenerator.ini +relative_attitude_controller_file = ../../data/ini/Components/RelativeAttitudeController.ini diff --git a/s2e-ff/src/Components/IdealComponents/InitializeRelativeAttitudeController.cpp b/s2e-ff/src/Components/IdealComponents/InitializeRelativeAttitudeController.cpp new file mode 100644 index 00000000..f7a23e0d --- /dev/null +++ b/s2e-ff/src/Components/IdealComponents/InitializeRelativeAttitudeController.cpp @@ -0,0 +1,35 @@ +#include "InitializeRelativeAttitudeController.hpp" + +#include + +RelativeAttitudeController InitializeRelativeAttitudeController(ClockGenerator* clock_gen, const std::string file_name, + const RelativeInformation& rel_info, + const LocalCelestialInformation& local_celes_info, const Dynamics& dynamics) { + // General + IniAccess ini_file(file_name); + + // CompoBase + int prescaler = ini_file.ReadInt("ComponentBase", "prescaler"); + if (prescaler <= 1) prescaler = 1; + + // RelativeAttitudeController + char section[30] = "RelativeAttitudeController"; + + std::string main_mode_name = ini_file.ReadString(section, "main_mode"); + RelativeAttitudeControlMode main_mode = ConvertStringToRelativeAttitudeControlMode(main_mode_name); + std::string sub_mode_name = ini_file.ReadString(section, "sub_mode"); + RelativeAttitudeControlMode sub_mode = ConvertStringToRelativeAttitudeControlMode(sub_mode_name); + + libra::Vector<3> main_target_direction_b; + ini_file.ReadVector(section, "main_target_direction_b", main_target_direction_b); + libra::Vector<3> sub_target_direction_b; + ini_file.ReadVector(section, "sub_target_direction_b", sub_target_direction_b); + + int target_sat_id = ini_file.ReadInt(section, "target_sat_id"); + int reference_sat_id = ini_file.ReadInt(section, "reference_sat_id"); + + RelativeAttitudeController relative_attitude_controller(prescaler, clock_gen, main_mode, sub_mode, main_target_direction_b, sub_target_direction_b, + target_sat_id, reference_sat_id, rel_info, local_celes_info, dynamics); + + return relative_attitude_controller; +} diff --git a/s2e-ff/src/Components/IdealComponents/InitializeRelativeAttitudeController.hpp b/s2e-ff/src/Components/IdealComponents/InitializeRelativeAttitudeController.hpp new file mode 100644 index 00000000..18c01b63 --- /dev/null +++ b/s2e-ff/src/Components/IdealComponents/InitializeRelativeAttitudeController.hpp @@ -0,0 +1,7 @@ +#pragma once + +#include "RelativeAttitudeController.hpp" + +RelativeAttitudeController InitializeRelativeAttitudeController(ClockGenerator* clock_gen, const std::string file_name, + const RelativeInformation& rel_info, + const LocalCelestialInformation& local_celes_info, const Dynamics& dynamics); diff --git a/s2e-ff/src/Components/IdealComponents/RelativeAttitudeController.cpp b/s2e-ff/src/Components/IdealComponents/RelativeAttitudeController.cpp new file mode 100644 index 00000000..2a5c4c23 --- /dev/null +++ b/s2e-ff/src/Components/IdealComponents/RelativeAttitudeController.cpp @@ -0,0 +1,166 @@ +#include "RelativeAttitudeController.hpp" + +#include + +#define THRESHOLD_CA cos(30.0 / 180.0 * libra::pi) // fix me + +// Constructor +RelativeAttitudeController::RelativeAttitudeController(const int prescaler, ClockGenerator* clock_gen, const RelativeAttitudeControlMode main_mode, + const RelativeAttitudeControlMode sub_mode, const libra::Vector<3> main_target_direction_b, + const libra::Vector<3> sub_target_direction_b, const int target_sat_id, + const int reference_sat_id, const RelativeInformation& rel_info, + const LocalCelestialInformation& local_celes_info, const Dynamics& dynamics) + : ComponentBase(prescaler, clock_gen), + 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_sat_id_(target_sat_id), + my_sat_id_(reference_sat_id), + rel_info_(rel_info), + local_celes_info_(local_celes_info), + dynamics_(dynamics) {} + +RelativeAttitudeController::~RelativeAttitudeController() {} + +void RelativeAttitudeController::MainRoutine(int count) { + UNUSED(count); + if (!is_calc_enabled_) return; + + libra::Vector<3> main_direction_i, sub_direction_i; + // Calc main target direction + main_direction_i = CalcTargetDirection_i(main_mode_); + // Calc sub target direction + sub_direction_i = CalcTargetDirection_i(sub_mode_); + + // Calc attitude + libra::Quaternion q_i2b = CalcTargetQuaternion(main_direction_i, sub_direction_i); + dynamics_.SetAttitude().SetQuaternion_i2b(q_i2b); +} + +void RelativeAttitudeController::PowerOffRoutine() {} + +std::string RelativeAttitudeController::GetLogHeader() const { + std::string str_tmp = ""; + + std::string head = "RelativeAttitudeController_"; + str_tmp += WriteScalar(head + "main_mode", "-"); + + return str_tmp; +} + +std::string RelativeAttitudeController::GetLogValue() const { + std::string str_tmp = ""; + str_tmp += WriteScalar((int)main_mode_); + + return str_tmp; +} + +// Internal functions +void RelativeAttitudeController::Initialize(void) { + if (main_mode_ >= RelativeAttitudeControlMode::NO_CONTROL) is_calc_enabled_ = false; + if (sub_mode_ >= RelativeAttitudeControlMode::NO_CONTROL) is_calc_enabled_ = false; + + // sub mode check + if (main_mode_ == sub_mode_) { + std::cout << "sub mode should not equal to main mode. \n"; + is_calc_enabled_ = false; + return; + } + // pointing direction check + normalize(main_target_direction_b_); + normalize(sub_target_direction_b_); + double tmp = inner_product(main_target_direction_b_, sub_target_direction_b_); + tmp = std::abs(tmp); + if (tmp > THRESHOLD_CA) { + std::cout << "sub target direction should separate from the main target direction. \n"; + is_calc_enabled_ = false; + return; + } + + return; +} + +libra::Vector<3> RelativeAttitudeController::CalcTargetDirection_i(RelativeAttitudeControlMode mode) { + libra::Vector<3> direction_i; + switch (mode) { + case RelativeAttitudeControlMode::TARGET_SATELLITE_POINTING: + direction_i = rel_info_.GetRelativePosition_i_m(target_sat_id_, my_sat_id_); + break; + case RelativeAttitudeControlMode::SUN_POINTING: + direction_i = local_celes_info_.GetPosFromSC_i("SUN"); + break; + case RelativeAttitudeControlMode::EARTH_CENTER_POINTING: + direction_i = local_celes_info_.GetPosFromSC_i("EARTH"); + break; + case RelativeAttitudeControlMode::VELOCITY_DIRECTION_POINTING: + direction_i = dynamics_.GetOrbit().GetSatVelocity_i(); + break; + case RelativeAttitudeControlMode::ORBIT_NORMAL_POINTING: + direction_i = outer_product(dynamics_.GetOrbit().GetSatPosition_i(), dynamics_.GetOrbit().GetSatVelocity_i()); + break; + default: + // Not Reached + break; + } + + normalize(direction_i); + return direction_i; +} + +libra::Quaternion RelativeAttitudeController::CalcTargetQuaternion(const libra::Vector<3> main_direction_i, const libra::Vector<3> sub_direction_i) { + // Calc DCM ECI->Target + libra::Matrix<3, 3> DCM_t2i = CalcDcmFromVectors(main_direction_i, sub_direction_i); + // Calc DCM Target->body + libra::Matrix<3, 3> DCM_t2b = CalcDcmFromVectors(main_target_direction_b_, sub_target_direction_b_); + // Calc DCM ECI->body + libra::Matrix<3, 3> DCM_i2b = DCM_t2b * transpose(DCM_t2i); + // Convert to Quaternion + return libra::Quaternion::fromDCM(DCM_i2b); +} + +libra::Matrix<3, 3> RelativeAttitudeController::CalcDcmFromVectors(const libra::Vector<3> main_direction, const libra::Vector<3> sub_direction) { + libra::Matrix<3, 3> dcm = libra::eye<3>(); + + // Check vector + double tmp = inner_product(main_direction, sub_direction); + tmp = std::abs(tmp); + if (tmp > 0.99999) { + return dcm; + } + + // Calc basis vectors + libra::Vector<3> ex, ey, ez; + ex = main_direction; + libra::Vector<3> tmp1 = outer_product(ex, sub_direction); + libra::Vector<3> tmp2 = outer_product(tmp1, ex); + ey = normalize(tmp2); + libra::Vector<3> tmp3 = outer_product(ex, ey); + ez = normalize(tmp3); + + // Generate DCM + for (int i = 0; i < 3; i++) { + dcm[i][0] = ex[i]; + dcm[i][1] = ey[i]; + dcm[i][2] = ez[i]; + } + return dcm; +} + +RelativeAttitudeControlMode ConvertStringToRelativeAttitudeControlMode(const std::string mode_name) { + if (mode_name == "TARGET_SATELLITE_POINTING") { + return RelativeAttitudeControlMode::TARGET_SATELLITE_POINTING; + } else if (mode_name == "SUN_POINTING") { + return RelativeAttitudeControlMode::SUN_POINTING; + } else if (mode_name == "EARTH_CENTER_POINTING") { + return RelativeAttitudeControlMode::EARTH_CENTER_POINTING; + } else if (mode_name == "VELOCITY_DIRECTION_POINTING") { + return RelativeAttitudeControlMode::VELOCITY_DIRECTION_POINTING; + } else if (mode_name == "ORBIT_NORMAL_POINTING") { + return RelativeAttitudeControlMode::ORBIT_NORMAL_POINTING; + } else { + // Error + std::cerr << "RelativeAttitudeControlMode error!" << std::endl; + return RelativeAttitudeControlMode::TARGET_SATELLITE_POINTING; + } +} diff --git a/s2e-ff/src/Components/IdealComponents/RelativeAttitudeController.hpp b/s2e-ff/src/Components/IdealComponents/RelativeAttitudeController.hpp new file mode 100644 index 00000000..8b663384 --- /dev/null +++ b/s2e-ff/src/Components/IdealComponents/RelativeAttitudeController.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include + +enum class RelativeAttitudeControlMode { + TARGET_SATELLITE_POINTING, + SUN_POINTING, + EARTH_CENTER_POINTING, + VELOCITY_DIRECTION_POINTING, + ORBIT_NORMAL_POINTING, + NO_CONTROL, +}; + +RelativeAttitudeControlMode ConvertStringToRelativeAttitudeControlMode(const std::string mode_name); + +class RelativeAttitudeController : public ComponentBase, public ILoggable { + public: + RelativeAttitudeController(const int prescaler, ClockGenerator* clock_gen, const RelativeAttitudeControlMode main_mode, + const RelativeAttitudeControlMode sub_mode, const libra::Vector<3> main_target_direction_b, + const libra::Vector<3> sub_target_direction_b, const int target_sat_id, const int reference_sat_id, + const RelativeInformation& rel_info, const LocalCelestialInformation& local_celes_info, const Dynamics& dynamics); + ~RelativeAttitudeController(); + + // ComponentBase override function + void MainRoutine(int count); + void PowerOffRoutine(); + + // ILogabble override function + virtual std::string GetLogHeader() const; + virtual std::string GetLogValue() const; + + // Getter + + // Setter + inline void SetIsCalcEnabled(const bool is_enabled) { is_calc_enabled_ = is_enabled; } + inline void SetTargetSatId(const int target_sat_id) { target_sat_id_ = target_sat_id; } + + protected: + bool is_calc_enabled_ = true; + RelativeAttitudeControlMode main_mode_; + RelativeAttitudeControlMode sub_mode_; + int target_sat_id_; + int my_sat_id_; + + libra::Vector<3> main_target_direction_b_; //!< Pointing main target on body frame + libra::Vector<3> sub_target_direction_b_; //!< Pointing main target on body frame + + const RelativeInformation& rel_info_; + const LocalCelestialInformation& local_celes_info_; + const Dynamics& dynamics_; + + // Internal functions + void Initialize(void); + libra::Vector<3> CalcTargetDirection_i(RelativeAttitudeControlMode mode); + libra::Quaternion CalcTargetQuaternion(const libra::Vector<3> main_direction_i, const libra::Vector<3> sub_direction_i); + libra::Matrix<3, 3> CalcDcmFromVectors(const libra::Vector<3> main_direction, + const libra::Vector<3> sub_direction); // TODO: move to core's library +}; diff --git a/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp b/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp index 71094b59..7503d7d1 100644 --- a/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp +++ b/s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp @@ -5,6 +5,7 @@ #include "../../Components/AOCS/InitializeRelativeDistanceSensor.hpp" #include "../../Components/AOCS/InitializeRelativePositionSensor.hpp" #include "../../Components/IdealComponents/InitializeForceGenerator.hpp" +#include "../../Components/IdealComponents/InitializeRelativeAttitudeController.hpp" FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, const LocalEnvironment* local_env, const GlobalEnvironment* glo_env, const SimulationConfig* config, ClockGenerator* clock_gen, const RelativeInformation* rel_info) @@ -26,6 +27,10 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, const std::string force_generator_file = sat_file.ReadString("COMPONENTS_FILE", "force_generator_file"); force_generator_ = new ForceGenerator(InitializeForceGenerator(clock_gen, force_generator_file, dynamics_)); + const std::string relative_attitude_controller_file = sat_file.ReadString("COMPONENTS_FILE", "relative_attitude_controller_file"); + relative_attitude_controller_ = new RelativeAttitudeController( + InitializeRelativeAttitudeController(clock_gen, relative_attitude_controller_file, *rel_info_, local_env_->GetCelesInfo(), *dynamics_)); + // Debug for actuator output libra::Vector<3> force_N; force_N[0] = 1.0; @@ -38,6 +43,7 @@ FfComponents::~FfComponents() { delete relative_distance_sensor_; delete relative_position_sensor_; delete force_generator_; + delete relative_attitude_controller_; // OBC must be deleted the last since it has com ports delete obc_; } diff --git a/s2e-ff/src/Simulation/Spacecraft/FfComponents.hpp b/s2e-ff/src/Simulation/Spacecraft/FfComponents.hpp index aa107498..4b64b158 100644 --- a/s2e-ff/src/Simulation/Spacecraft/FfComponents.hpp +++ b/s2e-ff/src/Simulation/Spacecraft/FfComponents.hpp @@ -11,6 +11,7 @@ #include "../../Components/AOCS/RelativeDistanceSensor.hpp" #include "../../Components/AOCS/RelativePositionSensor.hpp" #include "../../Components/IdealComponents/ForceGenerator.hpp" +#include "../../Components/IdealComponents/RelativeAttitudeController.hpp" #include "OBC.h" class FfComponents : public InstalledComponents { @@ -28,6 +29,7 @@ class FfComponents : public InstalledComponents { RelativeDistanceSensor* relative_distance_sensor_; RelativePositionSensor* relative_position_sensor_; ForceGenerator* force_generator_; + RelativeAttitudeController* relative_attitude_controller_; // References const Dynamics* dynamics_;