Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add relative attitude sensor #79

Merged
merged 6 commits into from
Jan 5, 2024
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions s2e-ff/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
200km marked this conversation as resolved.
Show resolved Hide resolved
src/components/aocs/relative_velocity_sensor.cpp
src/components/aocs/initialize_relative_velocity_sensor.cpp
src/components/aocs/laser_distance_meter.cpp
Expand Down
Original file line number Diff line number Diff line change
@@ -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;
200km marked this conversation as resolved.
Show resolved Hide resolved
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
200km marked this conversation as resolved.
Show resolved Hide resolved
constant_bias_c_rad(1) = 0.0
200km marked this conversation as resolved.
Show resolved Hide resolved
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
200km marked this conversation as resolved.
Show resolved Hide resolved
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
1 change: 1 addition & 0 deletions s2e-ff/data/initialize_files/ff_satellite.ini
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
35 changes: 35 additions & 0 deletions s2e-ff/src/components/aocs/initialize_relative_attitude_sensor.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
/**
* @file initialize_relative_attitude_sensor.cpp
* @brief Initialize function for RelativeAttitudeSensor
*/

#include "initialize_relative_attitude_sensor.hpp"

#include <components/base/initialize_sensor.hpp>
#include <library/initialize/initialize_file_access.hpp>

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;
}
19 changes: 19 additions & 0 deletions s2e-ff/src/components/aocs/initialize_relative_attitude_sensor.hpp
Original file line number Diff line number Diff line change
@@ -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_
47 changes: 47 additions & 0 deletions s2e-ff/src/components/aocs/relative_attitude_sensor.cpp
Original file line number Diff line number Diff line change
@@ -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) {}
200km marked this conversation as resolved.
Show resolved Hide resolved

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();
200km marked this conversation as resolved.
Show resolved Hide resolved
measured_target_attitude_b_rad_ = Measure(measured_target_attitude_b_rad_);
measured_target_attitude_b_quaternion_ = libra::Quaternion::ConvertFromEuler(measured_target_attitude_b_rad_);
200km marked this conversation as resolved.
Show resolved Hide resolved
}

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);
200km marked this conversation as resolved.
Show resolved Hide resolved

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;
}
71 changes: 71 additions & 0 deletions s2e-ff/src/components/aocs/relative_attitude_sensor.hpp
Original file line number Diff line number Diff line change
@@ -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 <components/base/component.hpp>
#include <components/base/sensor.hpp>
#include <library/logger/logger.hpp>
#include <simulation/multiple_spacecraft/relative_information.hpp>

/**
* @class RelativeAttitudeSensor
* @brief Relative position sensor
200km marked this conversation as resolved.
Show resolved Hide resolved
*/
class RelativeAttitudeSensor : public Component, public Sensor<3>, public ILoggable {
200km marked this conversation as resolved.
Show resolved Hide resolved
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_; }
200km marked this conversation as resolved.
Show resolved Hide resolved

// 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_;
200km marked this conversation as resolved.
Show resolved Hide resolved
libra::Vector<3> measured_target_attitude_b_rad_{0.0}; //!< Measured attitude in BODY frame [m]
200km marked this conversation as resolved.
Show resolved Hide resolved

// References
const RelativeInformation& rel_info_; //!< Relative information
const Dynamics& dynamics_; //!< Dynamics
};

#endif // S2E_COMPONENTS_AOCS_RELATIVE_ATTITUDE_SENSOR_HPP_
6 changes: 6 additions & 0 deletions s2e-ff/src/simulation/spacecraft/ff_components.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand All @@ -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_;
Expand All @@ -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_);
Expand Down
2 changes: 2 additions & 0 deletions s2e-ff/src/simulation/spacecraft/ff_components.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include <components/ideal/force_generator.hpp>
#include <components/real/cdh/on_board_computer.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/aocs/initialize_relative_velocity_sensor.hpp"
Expand Down Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions s2e-ff/src/simulation/spacecraft/ff_components_2.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include <components/real/cdh/on_board_computer.hpp>

#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"
Expand Down