From 7a2a4e2db68b1bc0b7767a1af42aecde109918e0 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Sun, 31 Dec 2023 13:56:57 +0900 Subject: [PATCH] add relative attitude sensor --- s2e-ff/CMakeLists.txt | 2 + .../components/relative_attitude_sensor.ini | 46 ++++++++++++ s2e-ff/data/initialize_files/ff_satellite.ini | 1 + .../initialize_relative_attitude_sensor.cpp | 35 +++++++++ .../initialize_relative_attitude_sensor.hpp | 19 +++++ .../aocs/relative_attitude_sensor.cpp | 47 ++++++++++++ .../aocs/relative_attitude_sensor.hpp | 71 +++++++++++++++++++ .../simulation/spacecraft/ff_components.cpp | 6 ++ .../simulation/spacecraft/ff_components.hpp | 2 + .../simulation/spacecraft/ff_components_2.hpp | 1 + 10 files changed, 230 insertions(+) create mode 100644 s2e-ff/data/initialize_files/components/relative_attitude_sensor.ini create mode 100644 s2e-ff/src/components/aocs/initialize_relative_attitude_sensor.cpp create mode 100644 s2e-ff/src/components/aocs/initialize_relative_attitude_sensor.hpp create mode 100644 s2e-ff/src/components/aocs/relative_attitude_sensor.cpp create mode 100644 s2e-ff/src/components/aocs/relative_attitude_sensor.hpp diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 930a95fb..f34412ae 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -49,6 +49,8 @@ set(SOURCE_FILES src/components/aocs/initialize_relative_distance_sensor.cpp src/components/aocs/relative_position_sensor.cpp src/components/aocs/initialize_relative_position_sensor.cpp + src/components/aocs/relative_attitude_sensor.cpp + src/components/aocs/initialize_relative_attitude_sensor.cpp src/components/aocs/relative_velocity_sensor.cpp src/components/aocs/initialize_relative_velocity_sensor.cpp src/components/aocs/laser_distance_meter.cpp diff --git a/s2e-ff/data/initialize_files/components/relative_attitude_sensor.ini b/s2e-ff/data/initialize_files/components/relative_attitude_sensor.ini new file mode 100644 index 00000000..69f2a2a4 --- /dev/null +++ b/s2e-ff/data/initialize_files/components/relative_attitude_sensor.ini @@ -0,0 +1,46 @@ +[RELATIVE_ATTITUDE_SENSOR] +// Prescaler with respect to the component update period +prescaler = 1 + +// Target satellite ID +target_sat_id = 1 + +// When this value is negative, reference_sat_id is automatically set as the mounting satellite ID +reference_sat_id = -1 + +[SENSOR_BASE_RELATIVE_ATTITUDE_SENSOR] +// The coordinate of the error is selected by the error_frame +// Scale factor [-] +scale_factor_c(0) = 1; +scale_factor_c(1) = 0; +scale_factor_c(2) = 0; +scale_factor_c(3) = 0; +scale_factor_c(4) = 1; +scale_factor_c(5) = 0; +scale_factor_c(6) = 0; +scale_factor_c(7) = 0; +scale_factor_c(8) = 1; + +// Constant bias noise [rad] +constant_bias_c_rad(0) = 0.0 +constant_bias_c_rad(1) = 0.0 +constant_bias_c_rad(2) = 0.0 + +// Standard deviation of normal random noise [rad] +normal_random_standard_deviation_c_rad(0) = 0.0 +normal_random_standard_deviation_c_rad(1) = 0.0 +normal_random_standard_deviation_c_rad(2) = 0.0 + +// Standard deviation for random walk noise [rad] +random_walk_standard_deviation_c_rad(0) = 0.0 +random_walk_standard_deviation_c_rad(1) = 0.0 +random_walk_standard_deviation_c_rad(2) = 0.0 + +// Limit of random walk noise [rad] +random_walk_limit_c_rad(0) = 0.0 +random_walk_limit_c_rad(1) = 0.0 +random_walk_limit_c_rad(2) = 0.0 + +// Range [rad] +range_to_constant_rad = 6.283186 // smaller than range_to_zero_rad +range_to_zero_rad = 7.0 diff --git a/s2e-ff/data/initialize_files/ff_satellite.ini b/s2e-ff/data/initialize_files/ff_satellite.ini index 9aa01319..d6ab6d94 100644 --- a/s2e-ff/data/initialize_files/ff_satellite.ini +++ b/s2e-ff/data/initialize_files/ff_satellite.ini @@ -135,6 +135,7 @@ structure_file = ../../data/initialize_files/ff_satellite_structure.ini // Users can add the path for component initialize files here. relative_distance_sensor_file = ../../data/initialize_files/components/relative_distance_sensor.ini relative_position_sensor_file = ../../data/initialize_files/components/relative_position_sensor.ini +relative_attitude_sensor_file = ../../data/initialize_files/components/relative_attitude_sensor.ini relative_velocity_sensor_file = ../../data/initialize_files/components/relative_velocity_sensor.ini laser_distance_meter_file = ../../data/initialize_files/components/laser_distance_meter.ini force_generator_file = ../../data/initialize_files/components/force_generator.ini diff --git a/s2e-ff/src/components/aocs/initialize_relative_attitude_sensor.cpp b/s2e-ff/src/components/aocs/initialize_relative_attitude_sensor.cpp new file mode 100644 index 00000000..4e5d8032 --- /dev/null +++ b/s2e-ff/src/components/aocs/initialize_relative_attitude_sensor.cpp @@ -0,0 +1,35 @@ +/** + * @file initialize_relative_attitude_sensor.cpp + * @brief Initialize function for RelativeAttitudeSensor + */ + +#include "initialize_relative_attitude_sensor.hpp" + +#include +#include + +RelativeAttitudeSensor InitializeRelativeAttitudeSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s, + const RelativeInformation& rel_info, const Dynamics& dynamics, + const int reference_sat_id_input) { + // General + IniAccess ini_file(file_name); + char section[30] = "RELATIVE_ATTITUDE_SENSOR"; + + // CompoBase + int prescaler = ini_file.ReadInt(section, "prescaler"); + if (prescaler <= 1) prescaler = 1; + + // RelativeAttitudeSensor + int target_sat_id = ini_file.ReadInt(section, "target_sat_id"); + int reference_sat_id = ini_file.ReadInt(section, "reference_sat_id"); + if (reference_sat_id < 0) { + reference_sat_id = reference_sat_id_input; + } + + // SensorBase + Sensor<3> sensor_base = ReadSensorInformation<3>(file_name, compo_step_time_s * (double)(prescaler), section, "rad"); + + RelativeAttitudeSensor relative_attitude_sensor(prescaler, clock_gen, sensor_base, target_sat_id, reference_sat_id, rel_info, dynamics); + + return relative_attitude_sensor; +} diff --git a/s2e-ff/src/components/aocs/initialize_relative_attitude_sensor.hpp b/s2e-ff/src/components/aocs/initialize_relative_attitude_sensor.hpp new file mode 100644 index 00000000..25cf7ee7 --- /dev/null +++ b/s2e-ff/src/components/aocs/initialize_relative_attitude_sensor.hpp @@ -0,0 +1,19 @@ +/** + * @file initialize_relative_attitude_sensor.hpp + * @brief Initialize function for RelativeAttitudeSensor + */ + +#ifndef S2E_COMPONENTS_AOCS_INITIALIZE_RELATIVE_ATTITUDE_SENSOR_HPP_ +#define S2E_COMPONENTS_AOCS_INITIALIZE_RELATIVE_ATTITUDE_SENSOR_HPP_ + +#include "relative_attitude_sensor.hpp" + +/** + * @fn InitializeRelativeAttitudeSensor + * @brief Initialize function + */ +RelativeAttitudeSensor InitializeRelativeAttitudeSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s, + const RelativeInformation& rel_info, const Dynamics& dynamics, + const int reference_sat_id_input = -1); + +#endif // S2E_COMPONENTS_AOCS_INITIALIZE_RELATIVE_ATTITUDE_SENSOR_HPP_ diff --git a/s2e-ff/src/components/aocs/relative_attitude_sensor.cpp b/s2e-ff/src/components/aocs/relative_attitude_sensor.cpp new file mode 100644 index 00000000..16d756a7 --- /dev/null +++ b/s2e-ff/src/components/aocs/relative_attitude_sensor.cpp @@ -0,0 +1,47 @@ +/** + * @file relative_attitude_sensor.cpp + * @brief Relative attitude sensor + */ + +#include "relative_attitude_sensor.hpp" + +RelativeAttitudeSensor::RelativeAttitudeSensor(const int prescaler, ClockGenerator* clock_gen, Sensor& sensor_base, const int target_sat_id, + const int reference_sat_id, const RelativeInformation& rel_info, const Dynamics& dynamics) + : Component(prescaler, clock_gen), + Sensor(sensor_base), + target_sat_id_(target_sat_id), + reference_sat_id_(reference_sat_id), + rel_info_(rel_info), + dynamics_(dynamics) {} + +RelativeAttitudeSensor::~RelativeAttitudeSensor() {} + +void RelativeAttitudeSensor::MainRoutine(int count) { + UNUSED(count); + + // Get true value + measured_target_attitude_b_quaternion_ = rel_info_.GetRelativeAttitudeQuaternion(target_sat_id_, reference_sat_id_); + measured_target_attitude_b_rad_ = measured_target_attitude_b_quaternion_.ConvertToEuler(); + measured_target_attitude_b_rad_ = Measure(measured_target_attitude_b_rad_); + measured_target_attitude_b_quaternion_ = libra::Quaternion::ConvertFromEuler(measured_target_attitude_b_rad_); +} + +std::string RelativeAttitudeSensor::GetLogHeader() const { + std::string str_tmp = ""; + std::string head = "RelativeAttitudeSensor_"; + + const std::string frame_name = std::to_string(reference_sat_id_) + "to" + std::to_string(target_sat_id_); + str_tmp += WriteQuaternion(head + "quaternion", frame_name); + str_tmp += WriteVector(head + "attitude", frame_name, "rad", 3); + + return str_tmp; +} + +std::string RelativeAttitudeSensor::GetLogValue() const { + std::string str_tmp = ""; + + str_tmp += WriteQuaternion(measured_target_attitude_b_quaternion_); + str_tmp += WriteVector(measured_target_attitude_b_rad_); + + return str_tmp; +} diff --git a/s2e-ff/src/components/aocs/relative_attitude_sensor.hpp b/s2e-ff/src/components/aocs/relative_attitude_sensor.hpp new file mode 100644 index 00000000..8bd26bb0 --- /dev/null +++ b/s2e-ff/src/components/aocs/relative_attitude_sensor.hpp @@ -0,0 +1,71 @@ +/** + * @file relative_attitude_sensor.hpp + * @brief Relative attitude sensor + */ + +#ifndef S2E_COMPONENTS_AOCS_RELATIVE_ATTITUDE_SENSOR_HPP_ +#define S2E_COMPONENTS_AOCS_RELATIVE_ATTITUDE_SENSOR_HPP_ + +#include +#include +#include +#include + +/** + * @class RelativeAttitudeSensor + * @brief Relative position sensor + */ +class RelativeAttitudeSensor : public Component, public Sensor<3>, public ILoggable { + public: + /** + * @fn RelativeAttitudeSensor + * @brief Constructor + */ + RelativeAttitudeSensor(const int prescaler, ClockGenerator* clock_gen, Sensor& sensor_base, const int target_sat_id, const int reference_sat_id, + const RelativeInformation& rel_info, const Dynamics& dynamics); + /** + * @fn ~RelativeAttitudeSensor + * @brief Destructor + */ + ~RelativeAttitudeSensor(); + + // ComponentBase override function + /** + * @fn MainRoutine + * @brief Main routine + */ + void MainRoutine(int count); + + // Override ILoggable + /** + * @fn GetLogHeader + * @brief Override GetLogHeader function of ILoggable + */ + virtual std::string GetLogHeader() const; + /** + * @fn GetLogValue + * @brief Override GetLogValue function of ILoggable + */ + virtual std::string GetLogValue() const; + + // Getter + inline libra::Quaternion GetMeasuredTargetQuaternion_b_m() const { return measured_target_attitude_b_quaternion_; } + inline libra::Vector<3> GetMeasuredTargetAttitude_b_m() const { return measured_target_attitude_b_rad_; } + + // Setter + void SetTargetSatId(const int target_sat_id) { target_sat_id_ = target_sat_id; } + + protected: + int target_sat_id_; //!< Target satellite ID + const int reference_sat_id_; //!< Reference satellite ID + + // Measured value + libra::Quaternion measured_target_attitude_b_quaternion_; + libra::Vector<3> measured_target_attitude_b_rad_{0.0}; //!< Measured attitude in BODY frame [m] + + // References + const RelativeInformation& rel_info_; //!< Relative information + const Dynamics& dynamics_; //!< Dynamics +}; + +#endif // S2E_COMPONENTS_AOCS_RELATIVE_ATTITUDE_SENSOR_HPP_ diff --git a/s2e-ff/src/simulation/spacecraft/ff_components.cpp b/s2e-ff/src/simulation/spacecraft/ff_components.cpp index ffd653d0..1b089013 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components.cpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components.cpp @@ -30,6 +30,10 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, relative_position_sensor_ = new RelativePositionSensor(InitializeRelativePositionSensor(clock_gen, rel_pos_file, compo_step_sec, *rel_info_, *dynamics_, sat_id)); + const std::string rel_att_file = sat_file.ReadString(section_name.c_str(), "relative_attitude_sensor_file"); + relative_attitude_sensor_ = + new RelativeAttitudeSensor(InitializeRelativeAttitudeSensor(clock_gen, rel_att_file, compo_step_sec, *rel_info_, *dynamics_, sat_id)); + const std::string rel_vel_file = sat_file.ReadString(section_name.c_str(), "relative_velocity_sensor_file"); relative_velocity_sensor_ = new RelativeVelocitySensor(InitializeRelativeVelocitySensor(clock_gen, rel_vel_file, compo_step_sec, *rel_info_, *dynamics_, sat_id)); @@ -55,6 +59,7 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, FfComponents::~FfComponents() { delete relative_distance_sensor_; delete relative_position_sensor_; + delete relative_attitude_sensor_; delete relative_velocity_sensor_; delete force_generator_; delete relative_attitude_controller_; @@ -78,6 +83,7 @@ Vector<3> FfComponents::GenerateTorque_b_Nm() { void FfComponents::LogSetup(Logger& logger) { logger.AddLogList(relative_distance_sensor_); logger.AddLogList(relative_position_sensor_); + logger.AddLogList(relative_attitude_sensor_); logger.AddLogList(relative_velocity_sensor_); logger.AddLogList(force_generator_); logger.AddLogList(laser_distance_meter_); diff --git a/s2e-ff/src/simulation/spacecraft/ff_components.hpp b/s2e-ff/src/simulation/spacecraft/ff_components.hpp index 3dd499ef..5b256493 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components.hpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components.hpp @@ -16,6 +16,7 @@ #include #include +#include "../../components/aocs/initialize_relative_attitude_sensor.hpp" #include "../../components/aocs/initialize_relative_distance_sensor.hpp" #include "../../components/aocs/initialize_relative_position_sensor.hpp" #include "../../components/aocs/initialize_relative_velocity_sensor.hpp" @@ -65,6 +66,7 @@ class FfComponents : public InstalledComponents { // Sensors RelativeDistanceSensor* relative_distance_sensor_; //!< Example of Relative distance sensor RelativePositionSensor* relative_position_sensor_; //!< Example of Relative position sensor + RelativeAttitudeSensor* relative_attitude_sensor_; //!< Example of Relative attitude sensor RelativeVelocitySensor* relative_velocity_sensor_; //!< Example of Relative velocity sensor LaserDistanceMeter* laser_distance_meter_; // Actuators diff --git a/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp b/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp index ade388ce..aaedeb7f 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp +++ b/s2e-ff/src/simulation/spacecraft/ff_components_2.hpp @@ -17,6 +17,7 @@ #include #include "../../components/aocs/corner_cube_reflector.hpp" +#include "../../components/aocs/initialize_relative_attitude_sensor.hpp" #include "../../components/aocs/initialize_relative_distance_sensor.hpp" #include "../../components/aocs/initialize_relative_position_sensor.hpp" #include "../../components/ideal/initialize_relative_attitude_controller.hpp"