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.

Sign up for GitHub

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

Jump to bottom

Refactor simulation #338

Merged
merged 62 commits into from
Mar 12, 2023
Merged

Refactor simulation #338

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
62 commits
Select commit Hold shift + click to select a range
c71aa22
Fix simulation config
200km Feb 28, 2023
a0db89e
Fix private variables name
200km Feb 28, 2023
7ac10f7
Fix function variables
200km Feb 28, 2023
43598bf
Fix public function name
200km Feb 28, 2023
c26abb8
Fix public function name
200km Feb 28, 2023
1d7e842
Remove using
200km Feb 28, 2023
4abd820
Fix private variables
200km Feb 28, 2023
5ae9302
Fix public function name
200km Feb 28, 2023
7e09244
Fix function argument names
200km Feb 28, 2023
c5389b7
Fix class name
200km Feb 28, 2023
ced126a
Fix private variables name
200km Feb 28, 2023
b773f5f
Fix private variables name
200km Feb 28, 2023
6d3c0c1
Fix public function name
200km Feb 28, 2023
f127ab9
Rename class
200km Feb 28, 2023
8a3d743
Fix private variables
200km Feb 28, 2023
a76a985
Fix function argument names
200km Feb 28, 2023
0425767
Remove using
200km Feb 28, 2023
921774c
Fix public function name
200km Feb 28, 2023
bd5fa16
Fix private variables name
200km Feb 28, 2023
bded6d2
Fix public function name
200km Feb 28, 2023
937bece
Remove using
200km Feb 28, 2023
c08147a
Fix initialize function name
200km Feb 28, 2023
64912a7
Fix sample components
200km Feb 28, 2023
3a7790f
Add const for simulation config
200km Feb 28, 2023
7b4bb5e
Add unsigned
200km Feb 28, 2023
1e6ce36
Fix private and function argument name
200km Feb 28, 2023
6e53a2c
Fix class name
200km Feb 28, 2023
6d57676
Remove using
200km Feb 28, 2023
406d085
Fix function argument names
200km Feb 28, 2023
cc9b4c4
Remove using
200km Feb 28, 2023
b655cae
Fix private variables name
200km Feb 28, 2023
8785c12
Fix private function name
200km Feb 28, 2023
eaf3992
Fix private function name
200km Feb 28, 2023
11eff8a
Fix public function name
200km Feb 28, 2023
a0b6e17
Fix class name
200km Feb 28, 2023
5e47992
Fix function argument names
200km Feb 28, 2023
9173d04
Fix private variables and inline function
200km Feb 28, 2023
09093dc
Rename class
200km Feb 28, 2023
a96cd85
Fix public function name
200km Feb 28, 2023
9d47536
Fix function argument name
200km Feb 28, 2023
d97d12a
Fix InitializedMonteCarloParameter
200km Feb 28, 2023
651a39f
Fix function argument names
200km Feb 28, 2023
0cc51a0
Fix public function name
200km Feb 28, 2023
fa4ce35
Rename class
200km Feb 28, 2023
b6b1093
Fix function argument name
200km Feb 28, 2023
bc442ab
Rename class
200km Feb 28, 2023
6c04e1c
Fix simulation case
200km Feb 28, 2023
df9f46d
Fix sample case
200km Feb 28, 2023
4f509c2
Rename Simulationconfig
200km Feb 28, 2023
5f96d0b
Fix main
200km Feb 28, 2023
7c91fb3
Fix format
200km Feb 28, 2023
fa0903f
Fix gnss satellite id
200km Feb 28, 2023
1de39ef
Fix Initialize name
200km Feb 28, 2023
fa2c19e
Fix typo
200km Feb 28, 2023
705dcab
Fix public function name
200km Feb 28, 2023
f1ac87e
Fix format
200km Feb 28, 2023
c10d351
Fix typo
200km Feb 28, 2023
ed1da9e
Merge branch 'feature/refactor-components' into feature/refactor-simu…
200km Mar 1, 2023
a0f5fa7
Merge branch 'feature/major-update-v6.0.0' into feature/refactor-simu…
200km Mar 12, 2023
2c15097
Fix typos
200km Mar 12, 2023
b1f7a59
Merge branch 'feature/refactor-simulation' of github.com:ut-issl/s2e-…
200km Mar 12, 2023
36def60
Fix typo
200km Mar 12, 2023
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
3 changes: 2 additions & 1 deletion data/sample/initialize_files/sample_simulation_base.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,8 @@ save_initialize_files = ENABLE
// File name must not over 256 characters (defined in initialize.h as MAX_CHAR_NUM)
// If you want to add a spacecraft, create the corresponding Sat.ini, and specify it as sat_file(1), sat_file(2)... .
number_of_simulated_spacecraft = 1
number_of_simulated_ground_station = 1
spacecraft_file(0) = ../../data/sample/initialize_files/sample_satellite.ini
ground_station_file = ../../data/sample/initialize_files/sample_ground_station.ini
ground_station_file(0) = ../../data/sample/initialize_files/sample_ground_station.ini
gnss_file = ../../data/sample/initialize_files/sample_gnss.ini
log_file_save_directory = ../../data/sample/logs/
8 changes: 4 additions & 4 deletions src/components/examples/example_change_structure.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,21 +13,21 @@ ExampleChangeStructure::~ExampleChangeStructure() {}
void ExampleChangeStructure::MainRoutine(const int time_count) {
if (time_count > 1000) {
// Mass
structure_->GetToSetKinematicsParams().SetMass_kg(100.0);
structure_->GetToSetKinematicsParameters().SetMass_kg(100.0);
// Center of gravity
Vector<3> cg(0.0);
cg[0] = 0.01;
cg[1] = -0.01;
cg[2] = 0.02;
structure_->GetToSetKinematicsParams().SetCenterOfGravityVector_b_m(cg);
structure_->GetToSetKinematicsParameters().SetCenterOfGravityVector_b_m(cg);
// RMM
Vector<3> rmm(0.0);
rmm[0] = 0.1;
rmm[1] = -0.1;
rmm[2] = 0.2;
structure_->GetToSetRMMParams().SetRmmConstant_b_Am2(rmm);
structure_->GetToSetResidualMagneticMoment().SetRmmConstant_b_Am2(rmm);
// Surface
structure_->GetToSetSurfaces()[0].SetArea(0.5);
structure_->GetToSetSurfaces()[0].SetArea_m2(0.5);
}
}

Expand Down
2 changes: 1 addition & 1 deletion src/components/real/aocs/gnss_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ void GnssReceiver::MainRoutine(const int time_count) {
UNUSED(time_count);

libra::Vector<3> pos_true_eci_ = dynamics_->GetOrbit().GetPosition_i_m();
Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b();
libra::Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b();

CheckAntenna(pos_true_eci_, quaternion_i2b);

Expand Down
4 changes: 2 additions & 2 deletions src/components/real/aocs/gyro_sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
#include "gyro_sensor.hpp"

GyroSensor::GyroSensor(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id,
const Quaternion& quaternion_b2c, const Dynamics* dynamics)
const libra::Quaternion& quaternion_b2c, const Dynamics* dynamics)
: Component(prescaler, clock_generator), Sensor(sensor_base), sensor_id_(sensor_id), quaternion_b2c_(quaternion_b2c), dynamics_(dynamics) {}

GyroSensor::GyroSensor(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id,
Expand All @@ -22,7 +22,7 @@ GyroSensor::~GyroSensor() {}
void GyroSensor::MainRoutine(const int time_count) {
UNUSED(time_count);

angular_velocity_c_rad_s_ = quaternion_b2c_.FrameConversion(dynamics_->GetAttitude().GetOmega_b()); // Convert frame
angular_velocity_c_rad_s_ = quaternion_b2c_.FrameConversion(dynamics_->GetAttitude().GetAngularVelocity_b_rad_s()); // Convert frame
angular_velocity_c_rad_s_ = Measure(angular_velocity_c_rad_s_); // Add noises
}

Expand Down
6 changes: 3 additions & 3 deletions src/components/real/aocs/initialize_gnss_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,12 +11,12 @@
typedef struct _gnssrecever_param {
int prescaler;
AntennaModel antenna_model;
Vector<3> antenna_pos_b;
Quaternion quaternion_b2c;
libra::Vector<3> antenna_pos_b;
libra::Quaternion quaternion_b2c;
double half_width_rad;
std::string gnss_id;
int max_channel;
Vector<3> noise_standard_deviation_m;
libra::Vector<3> noise_standard_deviation_m;
} GnssReceiverParam;

GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const int component_id) {
Expand Down
4 changes: 2 additions & 2 deletions src/components/real/aocs/initialize_gyro_sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ GyroSensor InitGyroSensor(ClockGenerator* clock_generator, int sensor_id, const
const std::string section_name = sensor_name + std::to_string(static_cast<long long>(sensor_id));
const char* GSection = section_name.c_str();

Quaternion quaternion_b2c;
libra::Quaternion quaternion_b2c;
gyro_conf.ReadQuaternion(GSection, "quaternion_b2c", quaternion_b2c);
int prescaler = gyro_conf.ReadInt(GSection, "prescaler");
if (prescaler <= 1) prescaler = 1;
Expand All @@ -36,7 +36,7 @@ GyroSensor InitGyroSensor(ClockGenerator* clock_generator, PowerPort* power_port
const std::string section_name = sensor_name + std::to_string(static_cast<long long>(sensor_id));
const char* GSection = section_name.c_str();

Quaternion quaternion_b2c;
libra::Quaternion quaternion_b2c;
gyro_conf.ReadQuaternion(GSection, "quaternion_b2c", quaternion_b2c);
int prescaler = gyro_conf.ReadInt(GSection, "prescaler");
if (prescaler <= 1) prescaler = 1;
Expand Down
4 changes: 2 additions & 2 deletions src/components/real/aocs/initialize_magnetometer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ Magnetometer InitMagetometer(ClockGenerator* clock_generator, int sensor_id, con
int prescaler = magsensor_conf.ReadInt(MSSection, "prescaler");
if (prescaler <= 1) prescaler = 1;

Quaternion quaternion_b2c;
libra::Quaternion quaternion_b2c;
magsensor_conf.ReadQuaternion(MSSection, "quaternion_b2c", quaternion_b2c);

// Sensor
Expand All @@ -38,7 +38,7 @@ Magnetometer InitMagetometer(ClockGenerator* clock_generator, PowerPort* power_p
int prescaler = magsensor_conf.ReadInt(MSSection, "prescaler");
if (prescaler <= 1) prescaler = 1;

Quaternion quaternion_b2c;
libra::Quaternion quaternion_b2c;
magsensor_conf.ReadQuaternion(MSSection, "quaternion_b2c", quaternion_b2c);

// Sensor
Expand Down
44 changes: 22 additions & 22 deletions src/components/real/aocs/initialize_magnetorquer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,37 +16,37 @@ Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, int actuator_id,
int prescaler = magtorquer_conf.ReadInt(MTSection, "prescaler");
if (prescaler <= 1) prescaler = 1;

Vector<kMtqDimension * kMtqDimension> sf_vec;
libra::Vector<kMtqDimension * kMtqDimension> sf_vec;
magtorquer_conf.ReadVector(MTSection, "scale_factor_c", sf_vec);
Matrix<kMtqDimension, kMtqDimension> scale_factor;
libra::Matrix<kMtqDimension, kMtqDimension> scale_factor;
for (size_t i = 0; i < kMtqDimension; i++) {
for (size_t j = 0; j < kMtqDimension; j++) {
scale_factor[i][j] = sf_vec[i * kMtqDimension + j];
}
}

Quaternion quaternion_b2c;
libra::Quaternion quaternion_b2c;
magtorquer_conf.ReadQuaternion(MTSection, "quaternion_b2c", quaternion_b2c);

Vector<kMtqDimension> max_magnetic_moment_c_Am2;
libra::Vector<kMtqDimension> max_magnetic_moment_c_Am2;
magtorquer_conf.ReadVector(MTSection, "max_output_magnetic_moment_c_Am2", max_magnetic_moment_c_Am2);

Vector<kMtqDimension> min_magnetic_moment_c_Am2;
libra::Vector<kMtqDimension> min_magnetic_moment_c_Am2;
magtorquer_conf.ReadVector(MTSection, "min_output_magnetic_moment_c_Am2", min_magnetic_moment_c_Am2);

Vector<kMtqDimension> bias_noise_c_Am2_;
magtorquer_conf.ReadVector(MTSection, "constant_bias_noise_c_Am2", bias_noise_c_Am2_);
libra::Vector<kMtqDimension> bias_noise_c_Am2;
magtorquer_conf.ReadVector(MTSection, "constant_bias_noise_c_Am2", bias_noise_c_Am2);

double random_walk_step_width_s = component_step_time_s * (double)prescaler;
Vector<kMtqDimension> random_walk_standard_deviation_c_Am2;
libra::Vector<kMtqDimension> random_walk_standard_deviation_c_Am2;
magtorquer_conf.ReadVector(MTSection, "random_walk_standard_deviation_c_Am2", random_walk_standard_deviation_c_Am2);
Vector<kMtqDimension> random_walk_limit_c_Am2;
libra::Vector<kMtqDimension> random_walk_limit_c_Am2;
magtorquer_conf.ReadVector(MTSection, "random_walk_limit_c_Am2", random_walk_limit_c_Am2);
Vector<kMtqDimension> normal_random_standard_deviation_c_Am2;
libra::Vector<kMtqDimension> normal_random_standard_deviation_c_Am2;
magtorquer_conf.ReadVector(MTSection, "white_noise_standard_deviation_c_Am2", normal_random_standard_deviation_c_Am2);

Magnetorquer magtorquer(prescaler, clock_generator, actuator_id, quaternion_b2c, scale_factor, max_magnetic_moment_c_Am2, min_magnetic_moment_c_Am2,
bias_noise_c_Am2_, random_walk_step_width_s, random_walk_standard_deviation_c_Am2, random_walk_limit_c_Am2,
bias_noise_c_Am2, random_walk_step_width_s, random_walk_standard_deviation_c_Am2, random_walk_limit_c_Am2,
normal_random_standard_deviation_c_Am2, geomagnetic_field);
return magtorquer;
}
Expand All @@ -61,40 +61,40 @@ Magnetorquer InitMagnetorquer(ClockGenerator* clock_generator, PowerPort* power_
int prescaler = magtorquer_conf.ReadInt(MTSection, "prescaler");
if (prescaler <= 1) prescaler = 1;

Vector<kMtqDimension * kMtqDimension> sf_vec;
libra::Vector<kMtqDimension * kMtqDimension> sf_vec;
magtorquer_conf.ReadVector(MTSection, "scale_factor_c", sf_vec);
Matrix<kMtqDimension, kMtqDimension> scale_factor;
libra::Matrix<kMtqDimension, kMtqDimension> scale_factor;
for (size_t i = 0; i < kMtqDimension; i++) {
for (size_t j = 0; j < kMtqDimension; j++) {
scale_factor[i][j] = sf_vec[i * kMtqDimension + j];
}
}

Quaternion quaternion_b2c;
libra::Quaternion quaternion_b2c;
magtorquer_conf.ReadQuaternion(MTSection, "quaternion_b2c", quaternion_b2c);

Vector<kMtqDimension> max_magnetic_moment_c_Am2;
libra::Vector<kMtqDimension> max_magnetic_moment_c_Am2;
magtorquer_conf.ReadVector(MTSection, "max_output_magnetic_moment_c_Am2", max_magnetic_moment_c_Am2);

Vector<kMtqDimension> min_magnetic_moment_c_Am2;
libra::Vector<kMtqDimension> min_magnetic_moment_c_Am2;
magtorquer_conf.ReadVector(MTSection, "min_output_magnetic_moment_c_Am2", min_magnetic_moment_c_Am2);

Vector<kMtqDimension> bias_noise_c_Am2_;
magtorquer_conf.ReadVector(MTSection, "constant_bias_noise_c_Am2", bias_noise_c_Am2_);
libra::Vector<kMtqDimension> bias_noise_c_Am2;
magtorquer_conf.ReadVector(MTSection, "constant_bias_noise_c_Am2", bias_noise_c_Am2);

double random_walk_step_width_s = component_step_time_s * (double)prescaler;
Vector<kMtqDimension> random_walk_standard_deviation_c_Am2;
libra::Vector<kMtqDimension> random_walk_standard_deviation_c_Am2;
magtorquer_conf.ReadVector(MTSection, "random_walk_standard_deviation_c_Am2", random_walk_standard_deviation_c_Am2);
Vector<kMtqDimension> random_walk_limit_c_Am2;
libra::Vector<kMtqDimension> random_walk_limit_c_Am2;
magtorquer_conf.ReadVector(MTSection, "random_walk_limit_c_Am2", random_walk_limit_c_Am2);
Vector<kMtqDimension> normal_random_standard_deviation_c_Am2;
libra::Vector<kMtqDimension> normal_random_standard_deviation_c_Am2;
magtorquer_conf.ReadVector(MTSection, "white_noise_standard_deviation_c_Am2", normal_random_standard_deviation_c_Am2);

// PowerPort
power_port->InitializeWithInitializeFile(file_name);

Magnetorquer magtorquer(prescaler, clock_generator, power_port, actuator_id, quaternion_b2c, scale_factor, max_magnetic_moment_c_Am2,
min_magnetic_moment_c_Am2, bias_noise_c_Am2_, random_walk_step_width_s, random_walk_standard_deviation_c_Am2,
min_magnetic_moment_c_Am2, bias_noise_c_Am2, random_walk_step_width_s, random_walk_standard_deviation_c_Am2,
random_walk_limit_c_Am2, normal_random_standard_deviation_c_Am2, geomagnetic_field);
return magtorquer;
}
4 changes: 2 additions & 2 deletions src/components/real/aocs/initialize_star_sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ StarSensor InitStarSensor(ClockGenerator* clock_generator, int sensor_id, const
int prescaler = STT_conf.ReadInt(STTSection, "prescaler");
if (prescaler <= 1) prescaler = 1;
double step_time_s = component_step_time_s * prescaler;
Quaternion quaternion_b2c;
libra::Quaternion quaternion_b2c;
STT_conf.ReadQuaternion(STTSection, "quaternion_b2c", quaternion_b2c);
double standard_deviation_orthogonal_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_orthogonal_direction_rad");
double standard_deviation_sight_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_sight_direction_rad");
Expand Down Expand Up @@ -54,7 +54,7 @@ StarSensor InitStarSensor(ClockGenerator* clock_generator, PowerPort* power_port
if (prescaler <= 1) prescaler = 1;
double step_time_s = component_step_time_s * prescaler;

Quaternion quaternion_b2c;
libra::Quaternion quaternion_b2c;
STT_conf.ReadQuaternion(STTSection, "quaternion_b2c", quaternion_b2c);
double standard_deviation_orthogonal_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_orthogonal_direction_rad");
double standard_deviation_sight_direction = STT_conf.ReadDouble(STTSection, "standard_deviation_sight_direction_rad");
Expand Down
4 changes: 2 additions & 2 deletions src/components/real/aocs/magnetometer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,14 @@
#include <library/math/quaternion.hpp>

Magnetometer::Magnetometer(int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const unsigned int sensor_id,
const Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field)
const libra::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field)
: Component(prescaler, clock_generator),
Sensor(sensor_base),
sensor_id_(sensor_id),
quaternion_b2c_(quaternion_b2c),
geomagnetic_field_(geomagnetic_field) {}
Magnetometer::Magnetometer(int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, Sensor& sensor_base, const unsigned int sensor_id,
const Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field)
const libra::Quaternion& quaternion_b2c, const GeomagneticField* geomagnetic_field)
: Component(prescaler, clock_generator, power_port),
Sensor(sensor_base),
sensor_id_(sensor_id),
Expand Down
4 changes: 2 additions & 2 deletions src/components/real/aocs/magnetorquer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
#include <library/math/quaternion.hpp>
#include <library/randomization/global_randomization.hpp>

Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, const int component_id, const Quaternion& quaternion_b2c,
Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, const int component_id, const libra::Quaternion& quaternion_b2c,
const libra::Matrix<kMtqDimension, kMtqDimension>& scale_factor,
const libra::Vector<kMtqDimension>& max_magnetic_moment_c_Am2,
const libra::Vector<kMtqDimension>& min_magnetic_moment_c_Am2, const libra::Vector<kMtqDimension>& bias_noise_c_Am2_,
Expand All @@ -33,7 +33,7 @@ Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator,
}

Magnetorquer::Magnetorquer(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id,
const Quaternion& quaternion_b2c, const libra::Matrix<kMtqDimension, kMtqDimension>& scale_factor,
const libra::Quaternion& quaternion_b2c, const libra::Matrix<kMtqDimension, kMtqDimension>& scale_factor,
const libra::Vector<kMtqDimension>& max_magnetic_moment_c_Am2,
const libra::Vector<kMtqDimension>& min_magnetic_moment_c_Am2, const libra::Vector<kMtqDimension>& bias_noise_c_Am2_,
double random_walk_step_width_s, const libra::Vector<kMtqDimension>& random_walk_standard_deviation_c_Am2,
Expand Down
32 changes: 16 additions & 16 deletions src/components/real/aocs/star_sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,21 +66,21 @@ StarSensor::StarSensor(const int prescaler, ClockGenerator* clock_generator, Pow
}

void StarSensor::Initialize() {
measured_quaternion_i2c_ = Quaternion(0.0, 0.0, 0.0, 1.0);
measured_quaternion_i2c_ = libra::Quaternion(0.0, 0.0, 0.0, 1.0);

// Decide delay buffer size
max_delay_ = int(output_delay_ * 2 / step_time_s_);
if (max_delay_ <= 0) max_delay_ = 1;
vector<Quaternion> temp(max_delay_);
vector<libra::Quaternion> temp(max_delay_);
delay_buffer_ = temp;
// Initialize delay buffer
for (int i = 0; i < max_delay_; ++i) {
delay_buffer_[i] = measured_quaternion_i2c_;
}

sight_direction_c_ = Vector<3>(0.0);
first_orthogonal_direction_c = Vector<3>(0.0);
second_orthogonal_direction_c = Vector<3>(0.0);
sight_direction_c_ = libra::Vector<3>(0.0);
first_orthogonal_direction_c = libra::Vector<3>(0.0);
second_orthogonal_direction_c = libra::Vector<3>(0.0);
sight_direction_c_[0] = 1.0; //(1,0,0)@Component coordinates, viewing direction
first_orthogonal_direction_c[1] = 1.0; //(0,1,0)@Component coordinates, line-of-sight orthogonal direction
second_orthogonal_direction_c[2] = 1.0; //(0,0,1)@Component coordinates, line-of-sight orthogonal direction
Expand Down Expand Up @@ -111,8 +111,8 @@ void StarSensor::update(const LocalCelestialInformation* local_celestial_informa
// Random noise on orthogonal direction of sight. Range [0:2pi]
double rot = libra::tau * double(rotation_noise_);
// Calc observation error on orthogonal direction of sight
Vector<3> rot_axis = cos(rot) * first_orthogonal_direction_c + sin(rot) * second_orthogonal_direction_c;
Quaternion q_ortho(rot_axis, orthogonal_direction_noise_);
libra::Vector<3> rot_axis = cos(rot) * first_orthogonal_direction_c + sin(rot) * second_orthogonal_direction_c;
libra::Quaternion q_ortho(rot_axis, orthogonal_direction_noise_);
// Judge errors
AllJudgement(local_celestial_information, attitude);

Expand All @@ -129,16 +129,16 @@ void StarSensor::AllJudgement(const LocalCelestialInformation* local_celestial_i
judgement = SunJudgement(local_celestial_information->GetPositionFromSpacecraft_b_m("SUN"));
judgement += EarthJudgement(local_celestial_information->GetPositionFromSpacecraft_b_m("EARTH"));
judgement += MoonJudgement(local_celestial_information->GetPositionFromSpacecraft_b_m("MOON"));
judgement += CaptureRateJudgement(attitude->GetOmega_b());
judgement += CaptureRateJudgement(attitude->GetAngularVelocity_b_rad_s());
if (judgement > 0)
error_flag_ = true;
else
error_flag_ = false;
}

int StarSensor::SunJudgement(const libra::Vector<3>& sun_b) {
Quaternion q_c2b = quaternion_b2c_.Conjugate();
Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_);
libra::Quaternion q_c2b = quaternion_b2c_.Conjugate();
libra::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_);
double sun_angle_rad = CalAngleVector_rad(sun_b, sight_b);
if (sun_angle_rad < sun_forbidden_angle_rad_)
return 1;
Expand All @@ -147,8 +147,8 @@ int StarSensor::SunJudgement(const libra::Vector<3>& sun_b) {
}

int StarSensor::EarthJudgement(const libra::Vector<3>& earth_b) {
Quaternion q_c2b = quaternion_b2c_.Conjugate();
Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_);
libra::Quaternion q_c2b = quaternion_b2c_.Conjugate();
libra::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_);
double earth_size_rad = atan2(environment::earth_equatorial_radius_m,
CalcNorm(earth_b)); // angles between sat<->earth_center & sat<->earth_edge
double earth_center_angle_rad = CalAngleVector_rad(earth_b, sight_b); // angles between sat<->earth_center & sat_sight
Expand All @@ -160,8 +160,8 @@ int StarSensor::EarthJudgement(const libra::Vector<3>& earth_b) {
}

int StarSensor::MoonJudgement(const libra::Vector<3>& moon_b) {
Quaternion q_c2b = quaternion_b2c_.Conjugate();
Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_);
libra::Quaternion q_c2b = quaternion_b2c_.Conjugate();
libra::Vector<3> sight_b = q_c2b.FrameConversion(sight_direction_c_);
double moon_angle_rad = CalAngleVector_rad(moon_b, sight_b);
if (moon_angle_rad < moon_forbidden_angle_rad_)
return 1;
Expand Down Expand Up @@ -198,9 +198,9 @@ std::string StarSensor::GetLogValue() const {
}

double StarSensor::CalAngleVector_rad(const Vector<3>& vector1, const Vector<3>& vector2) {
Vector<3> vect1_normal(vector1);
libra::Vector<3> vect1_normal(vector1);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[NITS] vect1 -> vector1 などに修正したいです．

Normalize(vect1_normal); // Normalize Vector1
Vector<3> vect2_normal(vector2);
libra::Vector<3> vect2_normal(vector2);
Normalize(vect2_normal); // Normalize Vector2
double cosTheta = InnerProduct(vect1_normal, vect2_normal); // Calc cos value
double theta_rad = acos(cosTheta);
Expand Down
Loading