From 396a99ce1b890553a45479eeee6cd4ee7c7642fa Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 13:23:11 +0100 Subject: [PATCH 01/61] Fix to use size_t for component ID --- src/components/real/aocs/gnss_receiver.cpp | 8 ++++---- src/components/real/aocs/gnss_receiver.hpp | 10 +++++----- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index c379daa26..412f5b605 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -11,8 +11,8 @@ #include #include -GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const int component_id, const std::string gnss_id, - const int max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, +GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_id, + const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_rad, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator), @@ -29,8 +29,8 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, dynamics_(dynamics), gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) {} -GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, - const std::string gnss_id, const int max_channel, const AntennaModel antenna_model, +GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, + const std::string gnss_id, const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_rad, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 1a5ac4e35..1d00f9555 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -59,7 +59,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] gnss_satellites: GNSS Satellites information * @param [in] simulation_time: Simulation time information */ - GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const int component_id, const std::string gnss_id, const int max_channel, + GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_id, const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_rad, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); @@ -80,8 +80,8 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] gnss_satellites: GNSS Satellites information * @param [in] simulation_time: Simulation time information */ - GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const int component_id, std::string gnss_id, - const int max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, + GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, std::string gnss_id, + const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_rad, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); @@ -142,8 +142,8 @@ class GnssReceiver : public Component, public ILoggable { protected: // Parameters for receiver - const int component_id_; //!< Receiver ID - const int max_channel_; //!< Maximum number of channels + const size_t component_id_; //!< Receiver ID + const size_t max_channel_; //!< Maximum number of channels libra::Vector<3> antenna_position_b_m_; //!< GNSS antenna position at the body-fixed frame [m] libra::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (antenna frame) From e08a4e7814b4f25b07ef9a8859601afe4ba7767e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 13:25:38 +0100 Subject: [PATCH 02/61] Fix to use size_t for visible sat number --- src/components/real/aocs/gnss_receiver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 1d00f9555..d583580ae 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -163,7 +163,7 @@ class GnssReceiver : public Component, public ILoggable { unsigned int gps_time_week_ = 0; //!< Observed GPS time week part double gps_time_s_ = 0.0; //!< Observed GPS time second part int is_gnss_visible_ = 0; //!< Flag for GNSS satellite is visible or not - int visible_satellite_number_ = 0; //!< Number of visible GNSS satellites + size_t visible_satellite_number_ = 0; //!< Number of visible GNSS satellites std::vector gnss_information_list_; //!< Information List of visible GNSS satellites // References From c1f2cc2dc635670e7257c818600d4513e27194b4 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 13:30:16 +0100 Subject: [PATCH 03/61] Fix to use size_t instead of int --- src/components/real/aocs/gnss_receiver.cpp | 10 +++++----- src/components/real/aocs/gnss_receiver.hpp | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 412f5b605..ccf2bdc83 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -113,9 +113,9 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> pos_true_eci_, libra: // initialize visible_satellite_number_ = 0; - int gnss_num = kTotalNumberOfGnssSatellite; + size_t gnss_num = kTotalNumberOfGnssSatellite; - for (int i = 0; i < gnss_num; i++) { + for (size_t i = 0; i < gnss_num; i++) { // check if gnss ID is compatible with the receiver std::string id_tmp = ConvertIndexToGnssSatelliteNumber(i); if (gnss_id_.find(id_tmp[0]) == std::string::npos) continue; @@ -245,7 +245,7 @@ typedef struct _gnssrecever_param { libra::Quaternion quaternion_b2c; double half_width_rad; std::string gnss_id; - int max_channel; + size_t max_channel; libra::Vector<3> noise_standard_deviation_m; } GnssReceiverParam; @@ -279,7 +279,7 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat return gnss_receiver_param; } -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, int component_id, const std::string file_name, const Dynamics* dynamics, +GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); @@ -289,7 +289,7 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, int component_id, return gnss_r; } -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, int component_id, const std::string file_name, +GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index d583580ae..f37b4baea 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -98,7 +98,7 @@ class GnssReceiver : public Component, public ILoggable { * @brief Return GNSS satellite information * @param [in] channel: Channel number */ - inline const GnssInfo GetGnssInfo(int channel) const { return gnss_information_list_[channel]; }; + inline const GnssInfo GetGnssInfo(size_t channel) const { return gnss_information_list_[channel]; }; /** * @fn GetMeasuredPosition_eci_m * @brief Return Observed position in the ECI frame [m] @@ -229,7 +229,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] gnss_satellites: GNSS satellites information * @param [in] simulation_time: Simulation time information */ -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, int component_id, const std::string file_name, const Dynamics* dynamics, +GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn InitGnssReceiver @@ -242,7 +242,7 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, int component_id, * @param [in] gnss_satellites: GNSS satellites information * @param [in] simulation_time: Simulation time information */ -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, int component_id, const std::string file_name, +GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); #endif // S2E_COMPONENTS_REAL_AOCS_GNSS_RECEIVER_HPP_ From 5876eb0f69d5562e7974326270e102c8c07243a5 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 13:34:41 +0100 Subject: [PATCH 04/61] Fix to use bool instead of int --- src/components/real/aocs/gnss_receiver.cpp | 33 ++++++++++++---------- src/components/real/aocs/gnss_receiver.hpp | 2 +- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index ccf2bdc83..752a408b5 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -57,7 +57,7 @@ void GnssReceiver::MainRoutine(const int time_count) { CheckAntenna(pos_true_eci_, quaternion_i2b); - if (is_gnss_visible_ == 1) { // Antenna of GNSS-R can detect GNSS signal + if (is_gnss_visible_ == true) { // Antenna of GNSS-R can detect GNSS signal position_ecef_m_ = dynamics_->GetOrbit().GetPosition_ecef_m(); position_llh_ = dynamics_->GetOrbit().GetLatLonAlt(); velocity_ecef_m_s_ = dynamics_->GetOrbit().GetVelocity_ecef_m_s(); @@ -90,10 +90,11 @@ void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> pos_true_eci_, libr libra::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); double inner = InnerProduct(pos_true_eci_, antenna_direction_i); - if (inner <= 0) - is_gnss_visible_ = 0; - else - is_gnss_visible_ = 1; + if (inner <= 0) { + is_gnss_visible_ = false; + } else { + is_gnss_visible_ = true; + } } void GnssReceiver::CheckAntennaCone(const libra::Vector<3> pos_true_eci_, libra::Quaternion quaternion_i2b) { @@ -129,17 +130,18 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> pos_true_eci_, libra: // check gnss sats are visible from antenna double Re = environment::earth_equatorial_radius_m; double inner1 = InnerProduct(ant_pos_i, gnss_sat_pos_i); - int is_visible_ant2gnss = 0; + bool is_visible_ant2gnss = false; if (inner1 > 0) - is_visible_ant2gnss = 1; + is_visible_ant2gnss = true; else { Vector<3> tmp = ant_pos_i + InnerProduct(-ant_pos_i, ant2gnss_i_n) * antenna_to_satellite_i_m; - if (tmp.CalcNorm() < Re) + if (tmp.CalcNorm() < Re) { // There is earth between antenna and gnss - is_visible_ant2gnss = 0; - else + is_visible_ant2gnss = false; + } else { // There is not earth between antenna and gnss - is_visible_ant2gnss = 1; + is_visible_ant2gnss = true; + } } double inner2 = InnerProduct(antenna_direction_i, ant2gnss_i_n); @@ -150,10 +152,11 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> pos_true_eci_, libra: } } - if (visible_satellite_number_ > 0) - is_gnss_visible_ = 1; - else - is_gnss_visible_ = 0; + if (visible_satellite_number_ > 0) { + is_gnss_visible_ = true; + } else { + is_gnss_visible_ = false; + } } void GnssReceiver::SetGnssInfo(libra::Vector<3> antenna_to_satellite_i_m, libra::Quaternion quaternion_i2b, std::string gnss_id) { diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index f37b4baea..9dbdcd45d 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -162,7 +162,7 @@ class GnssReceiver : public Component, public ILoggable { UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] unsigned int gps_time_week_ = 0; //!< Observed GPS time week part double gps_time_s_ = 0.0; //!< Observed GPS time second part - int is_gnss_visible_ = 0; //!< Flag for GNSS satellite is visible or not + bool is_gnss_visible_ = 0; //!< Flag for GNSS satellite is visible or not size_t visible_satellite_number_ = 0; //!< Number of visible GNSS satellites std::vector gnss_information_list_; //!< Information List of visible GNSS satellites From e8f81b61d078ca0496132b1029ac177b3029caea Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 13:39:34 +0100 Subject: [PATCH 05/61] Add const to function arguments --- src/components/real/aocs/gnss_receiver.cpp | 14 +++++++------- src/components/real/aocs/gnss_receiver.hpp | 18 +++++++++--------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 752a408b5..1aa7f9445 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -73,14 +73,14 @@ void GnssReceiver::MainRoutine(const int time_count) { } } -void GnssReceiver::CheckAntenna(const libra::Vector<3> pos_true_eci_, libra::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntenna(const libra::Vector<3> pos_true_eci_, const libra::Quaternion quaternion_i2b) { if (antenna_model_ == AntennaModel::kSimple) CheckAntennaSimple(pos_true_eci_, quaternion_i2b); else if (antenna_model_ == AntennaModel::kCone) CheckAntennaCone(pos_true_eci_, quaternion_i2b); } -void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> pos_true_eci_, libra::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> pos_true_eci_, const libra::Quaternion quaternion_i2b) { // Simplest model // GNSS sats are visible when antenna directs anti-earth direction // antenna normal vector at inertial frame @@ -97,7 +97,7 @@ void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> pos_true_eci_, libr } } -void GnssReceiver::CheckAntennaCone(const libra::Vector<3> pos_true_eci_, libra::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntennaCone(const libra::Vector<3> pos_true_eci_, const libra::Quaternion quaternion_i2b) { // Cone model libra::Vector<3> gnss_sat_pos_i, ant_pos_i, antenna_to_satellite_i_m, ant2gnss_i_n, sat2ant_i; gnss_information_list_.clear(); @@ -159,7 +159,7 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> pos_true_eci_, libra: } } -void GnssReceiver::SetGnssInfo(libra::Vector<3> antenna_to_satellite_i_m, libra::Quaternion quaternion_i2b, std::string gnss_id) { +void GnssReceiver::SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, const libra::Quaternion quaternion_i2b, const std::string gnss_id) { libra::Vector<3> ant2gnss_b, ant2gnss_c; ant2gnss_b = quaternion_i2b.FrameConversion(antenna_to_satellite_i_m); @@ -173,7 +173,7 @@ void GnssReceiver::SetGnssInfo(libra::Vector<3> antenna_to_satellite_i_m, libra: gnss_information_list_.push_back(gnss_info_new); } -void GnssReceiver::AddNoise(libra::Vector<3> position_true_i_m, libra::Vector<3> position_true_ecef_m) { +void GnssReceiver::AddNoise(const libra::Vector<3> position_true_i_m, const libra::Vector<3> position_true_ecef_m) { // Simplest noise model position_eci_m_[0] = position_true_i_m[0] + random_noise_i_x_; position_eci_m_[1] = position_true_i_m[1] + random_noise_i_y_; @@ -282,7 +282,7 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat return gnss_receiver_param; } -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, size_t component_id, const std::string file_name, const Dynamics* dynamics, +GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); @@ -292,7 +292,7 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, size_t component_ return gnss_r; } -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, size_t component_id, const std::string file_name, +GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 9dbdcd45d..280bceffa 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -80,7 +80,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] gnss_satellites: GNSS Satellites information * @param [in] simulation_time: Simulation time information */ - GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, std::string gnss_id, + GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string gnss_id, const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_rad, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); @@ -98,7 +98,7 @@ class GnssReceiver : public Component, public ILoggable { * @brief Return GNSS satellite information * @param [in] channel: Channel number */ - inline const GnssInfo GetGnssInfo(size_t channel) const { return gnss_information_list_[channel]; }; + inline const GnssInfo GetGnssInfo(const size_t channel) const { return gnss_information_list_[channel]; }; /** * @fn GetMeasuredPosition_eci_m * @brief Return Observed position in the ECI frame [m] @@ -179,7 +179,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame */ - void CheckAntenna(libra::Vector<3> position_true_i_m, libra::Quaternion quaternion_i2b); + void CheckAntenna(const libra::Vector<3> position_true_i_m, const libra::Quaternion quaternion_i2b); /** * @fn CheckAntennaSimple * @brief Check the antenna can detect GNSS signal with Simple mode @@ -187,7 +187,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame */ - void CheckAntennaSimple(libra::Vector<3> position_true_i_m, libra::Quaternion quaternion_i2b); + void CheckAntennaSimple(const libra::Vector<3> position_true_i_m, const libra::Quaternion quaternion_i2b); /** * @fn CheckAntennaCone * @brief Check the antenna can detect GNSS signal with Cone mode @@ -195,7 +195,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame */ - void CheckAntennaCone(libra::Vector<3> position_true_i_m, libra::Quaternion quaternion_i2b); + void CheckAntennaCone(const libra::Vector<3> position_true_i_m, const libra::Quaternion quaternion_i2b); /** * @fn SetGnssInfo * @brief Calculate and set the GnssInfo values of target GNSS satellite @@ -203,14 +203,14 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame * @param [in] gnss_id: ID of target GNSS satellite */ - void SetGnssInfo(libra::Vector<3> antenna_to_satellite_i_m, libra::Quaternion quaternion_i2b, std::string gnss_id); + void SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, const libra::Quaternion quaternion_i2b, const std::string gnss_id); /** * @fn AddNoise * @brief Substitutional method for "Measure" in other sensor models inherited Sensor class * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] position_true_ecef_m: True position of the spacecraft in the ECEF frame [m] */ - void AddNoise(libra::Vector<3> position_true_i_m, libra::Vector<3> position_true_ecef_m); + void AddNoise(const libra::Vector<3> position_true_i_m, const libra::Vector<3> position_true_ecef_m); /** * @fn ConvertJulianDayToGPSTime * @brief Convert Julian day to GPS time @@ -229,7 +229,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] gnss_satellites: GNSS satellites information * @param [in] simulation_time: Simulation time information */ -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, size_t component_id, const std::string file_name, const Dynamics* dynamics, +GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn InitGnssReceiver @@ -242,7 +242,7 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, size_t component_ * @param [in] gnss_satellites: GNSS satellites information * @param [in] simulation_time: Simulation time information */ -GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, size_t component_id, const std::string file_name, +GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string file_name, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); #endif // S2E_COMPONENTS_REAL_AOCS_GNSS_RECEIVER_HPP_ From 73360edb65fc3c550bbb3b9c9b1236fda2fefb6a Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 13:56:10 +0100 Subject: [PATCH 06/61] Fix typos --- src/components/real/aocs/gnss_receiver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 1aa7f9445..392e3fe7a 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -82,7 +82,7 @@ void GnssReceiver::CheckAntenna(const libra::Vector<3> pos_true_eci_, const libr void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> pos_true_eci_, const libra::Quaternion quaternion_i2b) { // Simplest model - // GNSS sats are visible when antenna directs anti-earth direction + // GNSS satellites are visible when antenna directs anti-earth direction // antenna normal vector at inertial frame libra::Vector<3> antenna_direction_c(0.0); antenna_direction_c[2] = 1.0; @@ -127,7 +127,7 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> pos_true_eci_, const double normalizer = 1 / antenna_to_satellite_i_m.CalcNorm(); ant2gnss_i_n = normalizer * antenna_to_satellite_i_m; - // check gnss sats are visible from antenna + // check gnss satellites are visible from antenna double Re = environment::earth_equatorial_radius_m; double inner1 = InnerProduct(ant_pos_i, gnss_sat_pos_i); bool is_visible_ant2gnss = false; @@ -241,7 +241,7 @@ std::string GnssReceiver::GetLogValue() const // For logs return str_tmp; } -typedef struct _gnssrecever_param { +typedef struct _gnss_receiver_param { int prescaler; AntennaModel antenna_model; libra::Vector<3> antenna_pos_b; From 068a3749ea4c8f8a677fd9662d90d961bfa2b857 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 13:56:46 +0100 Subject: [PATCH 07/61] Fix initial value of bool --- src/components/real/aocs/gnss_receiver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 280bceffa..9ef4241e2 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -162,7 +162,7 @@ class GnssReceiver : public Component, public ILoggable { UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] unsigned int gps_time_week_ = 0; //!< Observed GPS time week part double gps_time_s_ = 0.0; //!< Observed GPS time second part - bool is_gnss_visible_ = 0; //!< Flag for GNSS satellite is visible or not + bool is_gnss_visible_ = false; //!< Flag for GNSS satellite is visible or not size_t visible_satellite_number_ = 0; //!< Number of visible GNSS satellites std::vector gnss_information_list_; //!< Information List of visible GNSS satellites From 3c9db6f668d34fb5e6d67dfcc1a4819063ed12bb Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 14:09:11 +0100 Subject: [PATCH 08/61] Small refactors --- src/components/real/aocs/gnss_receiver.cpp | 37 ++++++++++++---------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 392e3fe7a..4d2e9bbe1 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -52,35 +52,38 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, void GnssReceiver::MainRoutine(const int time_count) { UNUSED(time_count); - libra::Vector<3> pos_true_eci_ = dynamics_->GetOrbit().GetPosition_i_m(); + libra::Vector<3> position_true_eci_ = dynamics_->GetOrbit().GetPosition_i_m(); libra::Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); - CheckAntenna(pos_true_eci_, quaternion_i2b); + CheckAntenna(position_true_eci_, quaternion_i2b); - if (is_gnss_visible_ == true) { // Antenna of GNSS-R can detect GNSS signal + if (is_gnss_visible_) { + // Antenna of GNSS-R can detect GNSS signal position_ecef_m_ = dynamics_->GetOrbit().GetPosition_ecef_m(); position_llh_ = dynamics_->GetOrbit().GetLatLonAlt(); velocity_ecef_m_s_ = dynamics_->GetOrbit().GetVelocity_ecef_m_s(); - AddNoise(pos_true_eci_, position_ecef_m_); - - utc_ = simulation_time_->GetCurrentUtc(); - ConvertJulianDayToGPSTime(simulation_time_->GetCurrentTime_jd()); + AddNoise(position_true_eci_, position_ecef_m_); } else { // position information will not be updated in this case - // only time information will be updated in this case (according to the receiver's internal clock) utc_ = simulation_time_->GetCurrentUtc(); ConvertJulianDayToGPSTime(simulation_time_->GetCurrentTime_jd()); } + // Time is updated with internal clock + utc_ = simulation_time_->GetCurrentUtc(); + ConvertJulianDayToGPSTime(simulation_time_->GetCurrentTime_jd()); } -void GnssReceiver::CheckAntenna(const libra::Vector<3> pos_true_eci_, const libra::Quaternion quaternion_i2b) { - if (antenna_model_ == AntennaModel::kSimple) - CheckAntennaSimple(pos_true_eci_, quaternion_i2b); - else if (antenna_model_ == AntennaModel::kCone) - CheckAntennaCone(pos_true_eci_, quaternion_i2b); +void GnssReceiver::CheckAntenna(const libra::Vector<3> position_true_eci_, const libra::Quaternion quaternion_i2b) { + if (antenna_model_ == AntennaModel::kSimple) { + CheckAntennaSimple(position_true_eci_, quaternion_i2b); + } else if (antenna_model_ == AntennaModel::kCone) { + CheckAntennaCone(position_true_eci_, quaternion_i2b); + } else { + std::cout << "[Error] GNSS Receiver: Undefined antenna model." << std::endl; + } } -void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> pos_true_eci_, const libra::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> position_true_eci_, const libra::Quaternion quaternion_i2b) { // Simplest model // GNSS satellites are visible when antenna directs anti-earth direction // antenna normal vector at inertial frame @@ -89,7 +92,7 @@ void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> pos_true_eci_, cons libra::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); libra::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); - double inner = InnerProduct(pos_true_eci_, antenna_direction_i); + double inner = InnerProduct(position_true_eci_, antenna_direction_i); if (inner <= 0) { is_gnss_visible_ = false; } else { @@ -97,7 +100,7 @@ void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> pos_true_eci_, cons } } -void GnssReceiver::CheckAntennaCone(const libra::Vector<3> pos_true_eci_, const libra::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_, const libra::Quaternion quaternion_i2b) { // Cone model libra::Vector<3> gnss_sat_pos_i, ant_pos_i, antenna_to_satellite_i_m, ant2gnss_i_n, sat2ant_i; gnss_information_list_.clear(); @@ -109,7 +112,7 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> pos_true_eci_, const libra::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); sat2ant_i = quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); - ant_pos_i = pos_true_eci_ + sat2ant_i; + ant_pos_i = position_true_eci_ + sat2ant_i; // initialize visible_satellite_number_ = 0; From d9e3841583c704d71e7ff633388efb6d80305d2f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 14:11:42 +0100 Subject: [PATCH 09/61] Fix function name to suit with the coding rule --- src/components/real/aocs/gnss_receiver.cpp | 10 +++++----- src/components/real/aocs/gnss_receiver.hpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 4d2e9bbe1..f83191bf3 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -66,11 +66,11 @@ void GnssReceiver::MainRoutine(const int time_count) { } else { // position information will not be updated in this case utc_ = simulation_time_->GetCurrentUtc(); - ConvertJulianDayToGPSTime(simulation_time_->GetCurrentTime_jd()); + ConvertJulianDayToGpsTime(simulation_time_->GetCurrentTime_jd()); } // Time is updated with internal clock utc_ = simulation_time_->GetCurrentUtc(); - ConvertJulianDayToGPSTime(simulation_time_->GetCurrentTime_jd()); + ConvertJulianDayToGpsTime(simulation_time_->GetCurrentTime_jd()); } void GnssReceiver::CheckAntenna(const libra::Vector<3> position_true_eci_, const libra::Quaternion quaternion_i2b) { @@ -188,15 +188,15 @@ void GnssReceiver::AddNoise(const libra::Vector<3> position_true_i_m, const libr position_ecef_m_[2] = position_true_ecef_m[2] + random_noise_i_z_; } -void GnssReceiver::ConvertJulianDayToGPSTime(const double julian_day) { - const double kJulianDayAtGPSTimeZero = 2444244.5; // corresponds to 1980/1/5 midnight +void GnssReceiver::ConvertJulianDayToGpsTime(const double julian_day) { + const double kJulianDayAtGpsTimeZero = 2444244.5; // corresponds to 1980/1/5 midnight const double kDayInWeek = 7.0; // const double kSecInWeek = 604800.0; const double kSecInDay = 86400.0; // compute ToW from current julian_day // note:"gps_time_week_ " computed in this method is larger than 1024 - double elapsed_day = julian_day - kJulianDayAtGPSTimeZero; + double elapsed_day = julian_day - kJulianDayAtGpsTimeZero; gps_time_week_ = (unsigned int)(elapsed_day / kDayInWeek); gps_time_s_ = (elapsed_day - (double)(gps_time_week_)*kDayInWeek) * kSecInDay; } diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 9ef4241e2..0b3380823 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -216,7 +216,7 @@ class GnssReceiver : public Component, public ILoggable { * @brief Convert Julian day to GPS time * @param [in] julian_day: Julian day */ - void ConvertJulianDayToGPSTime(const double julian_day); + void ConvertJulianDayToGpsTime(const double julian_day); }; /** From 30eaeeff50ee774a71d3d3071a891799f4e48aa9 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 14:38:44 +0100 Subject: [PATCH 10/61] Simplify the internal function --- src/components/real/aocs/gnss_receiver.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index f83191bf3..fd679548c 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -93,7 +93,7 @@ void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> position_true_eci_, libra::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); double inner = InnerProduct(position_true_eci_, antenna_direction_i); - if (inner <= 0) { + if (inner <= 0.0) { is_gnss_visible_ = false; } else { is_gnss_visible_ = true; @@ -102,7 +102,6 @@ void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> position_true_eci_, void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_, const libra::Quaternion quaternion_i2b) { // Cone model - libra::Vector<3> gnss_sat_pos_i, ant_pos_i, antenna_to_satellite_i_m, ant2gnss_i_n, sat2ant_i; gnss_information_list_.clear(); // antenna normal vector at inertial frame @@ -111,24 +110,22 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_, c libra::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); libra::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); - sat2ant_i = quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); - ant_pos_i = position_true_eci_ + sat2ant_i; + libra::Vector<3> sat2ant_i = quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); + libra::Vector<3> ant_pos_i = position_true_eci_ + sat2ant_i; // initialize visible_satellite_number_ = 0; - size_t gnss_num = kTotalNumberOfGnssSatellite; - - for (size_t i = 0; i < gnss_num; i++) { + for (size_t i = 0; i < kTotalNumberOfGnssSatellite; i++) { // check if gnss ID is compatible with the receiver std::string id_tmp = ConvertIndexToGnssSatelliteNumber(i); if (gnss_id_.find(id_tmp[0]) == std::string::npos) continue; // compute direction from sat to gnss in body-fixed frame - gnss_sat_pos_i = gnss_satellites_->GetPosition_eci_m(i); - antenna_to_satellite_i_m = gnss_sat_pos_i - ant_pos_i; + libra::Vector<3> gnss_sat_pos_i = gnss_satellites_->GetPosition_eci_m(i); + libra::Vector<3> antenna_to_satellite_i_m = gnss_sat_pos_i - ant_pos_i; double normalizer = 1 / antenna_to_satellite_i_m.CalcNorm(); - ant2gnss_i_n = normalizer * antenna_to_satellite_i_m; + libra::Vector<3> ant2gnss_i_n = normalizer * antenna_to_satellite_i_m; // check gnss satellites are visible from antenna double Re = environment::earth_equatorial_radius_m; From f17ebf1c203868f4c1b12af760a7de34843b25a7 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 14:41:57 +0100 Subject: [PATCH 11/61] Fix argument unit --- src/components/real/aocs/gnss_receiver.cpp | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index fd679548c..fe3d1d6e7 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -73,17 +73,17 @@ void GnssReceiver::MainRoutine(const int time_count) { ConvertJulianDayToGpsTime(simulation_time_->GetCurrentTime_jd()); } -void GnssReceiver::CheckAntenna(const libra::Vector<3> position_true_eci_, const libra::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntenna(const libra::Vector<3> position_true_eci_m, const libra::Quaternion quaternion_i2b) { if (antenna_model_ == AntennaModel::kSimple) { - CheckAntennaSimple(position_true_eci_, quaternion_i2b); + CheckAntennaSimple(position_true_eci_m, quaternion_i2b); } else if (antenna_model_ == AntennaModel::kCone) { - CheckAntennaCone(position_true_eci_, quaternion_i2b); + CheckAntennaCone(position_true_eci_m, quaternion_i2b); } else { std::cout << "[Error] GNSS Receiver: Undefined antenna model." << std::endl; } } -void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> position_true_eci_, const libra::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> position_true_eci_m, const libra::Quaternion quaternion_i2b) { // Simplest model // GNSS satellites are visible when antenna directs anti-earth direction // antenna normal vector at inertial frame @@ -92,7 +92,7 @@ void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> position_true_eci_, libra::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); libra::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); - double inner = InnerProduct(position_true_eci_, antenna_direction_i); + double inner = InnerProduct(position_true_eci_m, antenna_direction_i); if (inner <= 0.0) { is_gnss_visible_ = false; } else { @@ -100,7 +100,7 @@ void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> position_true_eci_, } } -void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_, const libra::Quaternion quaternion_i2b) { +void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, const libra::Quaternion quaternion_i2b) { // Cone model gnss_information_list_.clear(); @@ -110,8 +110,7 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_, c libra::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); libra::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); - libra::Vector<3> sat2ant_i = quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); - libra::Vector<3> ant_pos_i = position_true_eci_ + sat2ant_i; + libra::Vector<3> antenna_position_i = position_true_eci_m + quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); // initialize visible_satellite_number_ = 0; @@ -123,18 +122,18 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_, c // compute direction from sat to gnss in body-fixed frame libra::Vector<3> gnss_sat_pos_i = gnss_satellites_->GetPosition_eci_m(i); - libra::Vector<3> antenna_to_satellite_i_m = gnss_sat_pos_i - ant_pos_i; + libra::Vector<3> antenna_to_satellite_i_m = gnss_sat_pos_i - antenna_position_i; double normalizer = 1 / antenna_to_satellite_i_m.CalcNorm(); libra::Vector<3> ant2gnss_i_n = normalizer * antenna_to_satellite_i_m; // check gnss satellites are visible from antenna double Re = environment::earth_equatorial_radius_m; - double inner1 = InnerProduct(ant_pos_i, gnss_sat_pos_i); + double inner1 = InnerProduct(antenna_position_i, gnss_sat_pos_i); bool is_visible_ant2gnss = false; if (inner1 > 0) is_visible_ant2gnss = true; else { - Vector<3> tmp = ant_pos_i + InnerProduct(-ant_pos_i, ant2gnss_i_n) * antenna_to_satellite_i_m; + Vector<3> tmp = antenna_position_i + InnerProduct(-antenna_position_i, ant2gnss_i_n) * antenna_to_satellite_i_m; if (tmp.CalcNorm() < Re) { // There is earth between antenna and gnss is_visible_ant2gnss = false; From 85dc2f37050b5e6f5a226e1fed3549727227fa09 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 14:53:42 +0100 Subject: [PATCH 12/61] Fix internal variables name suit with the naming rule --- src/components/real/aocs/gnss_receiver.cpp | 40 +++++++++++----------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index fe3d1d6e7..555bc83c5 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -86,7 +86,8 @@ void GnssReceiver::CheckAntenna(const libra::Vector<3> position_true_eci_m, cons void GnssReceiver::CheckAntennaSimple(const libra::Vector<3> position_true_eci_m, const libra::Quaternion quaternion_i2b) { // Simplest model // GNSS satellites are visible when antenna directs anti-earth direction - // antenna normal vector at inertial frame + + // Antenna normal vector at inertial frame libra::Vector<3> antenna_direction_c(0.0); antenna_direction_c[2] = 1.0; libra::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); @@ -110,44 +111,43 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, libra::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); libra::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); - libra::Vector<3> antenna_position_i = position_true_eci_m + quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); + libra::Vector<3> antenna_position_i_m = position_true_eci_m + quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); // initialize visible_satellite_number_ = 0; for (size_t i = 0; i < kTotalNumberOfGnssSatellite; i++) { // check if gnss ID is compatible with the receiver - std::string id_tmp = ConvertIndexToGnssSatelliteNumber(i); - if (gnss_id_.find(id_tmp[0]) == std::string::npos) continue; + std::string gnss_id_string = ConvertIndexToGnssSatelliteNumber(i); + if (gnss_id_.find(gnss_id_string[0]) == std::string::npos) continue; // compute direction from sat to gnss in body-fixed frame - libra::Vector<3> gnss_sat_pos_i = gnss_satellites_->GetPosition_eci_m(i); - libra::Vector<3> antenna_to_satellite_i_m = gnss_sat_pos_i - antenna_position_i; + libra::Vector<3> gnss_satellite_position_i_m = gnss_satellites_->GetPosition_eci_m(i); + libra::Vector<3> antenna_to_satellite_i_m = gnss_satellite_position_i_m - antenna_position_i_m; double normalizer = 1 / antenna_to_satellite_i_m.CalcNorm(); - libra::Vector<3> ant2gnss_i_n = normalizer * antenna_to_satellite_i_m; + libra::Vector<3> antenna_to_gnss_satellite_direction_i = normalizer * antenna_to_satellite_i_m; // check gnss satellites are visible from antenna - double Re = environment::earth_equatorial_radius_m; - double inner1 = InnerProduct(antenna_position_i, gnss_sat_pos_i); - bool is_visible_ant2gnss = false; - if (inner1 > 0) - is_visible_ant2gnss = true; - else { - Vector<3> tmp = antenna_position_i + InnerProduct(-antenna_position_i, ant2gnss_i_n) * antenna_to_satellite_i_m; - if (tmp.CalcNorm() < Re) { + double inner1 = InnerProduct(antenna_position_i_m, gnss_satellite_position_i_m); + bool is_satellite_visible = false; + if (inner1 > 0) { + is_satellite_visible = true; + } else { + Vector<3> tmp = antenna_position_i_m + InnerProduct(-antenna_position_i_m, antenna_to_gnss_satellite_direction_i) * antenna_to_satellite_i_m; + if (tmp.CalcNorm() < environment::earth_equatorial_radius_m) { // There is earth between antenna and gnss - is_visible_ant2gnss = false; + is_satellite_visible = false; } else { // There is not earth between antenna and gnss - is_visible_ant2gnss = true; + is_satellite_visible = true; } } - double inner2 = InnerProduct(antenna_direction_i, ant2gnss_i_n); - if (inner2 > cos(half_width_rad_ * libra::deg_to_rad) && is_visible_ant2gnss) { + double inner2 = InnerProduct(antenna_direction_i, antenna_to_gnss_satellite_direction_i); + if (inner2 > cos(half_width_rad_ * libra::deg_to_rad) && is_satellite_visible) { // is visible visible_satellite_number_++; - SetGnssInfo(antenna_to_satellite_i_m, quaternion_i2b, id_tmp); + SetGnssInfo(antenna_to_satellite_i_m, quaternion_i2b, gnss_id_string); } } From de05d4d5cd1953d4b5aee61319c47e3578fad787 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 14:56:08 +0100 Subject: [PATCH 13/61] Fix internal variables name suit with the naming rule --- src/components/real/aocs/gnss_receiver.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 555bc83c5..d20511288 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -159,16 +159,15 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, } void GnssReceiver::SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, const libra::Quaternion quaternion_i2b, const std::string gnss_id) { - libra::Vector<3> ant2gnss_b, ant2gnss_c; + libra::Vector<3> antenna_to_satellite_direction_b = quaternion_i2b.FrameConversion(antenna_to_satellite_i_m); + libra::Vector<3> antenna_to_satellite_direction_c = quaternion_b2c_.FrameConversion(antenna_to_satellite_direction_b); - ant2gnss_b = quaternion_i2b.FrameConversion(antenna_to_satellite_i_m); - ant2gnss_c = quaternion_b2c_.FrameConversion(ant2gnss_b); + double distance_m = antenna_to_satellite_i_m.CalcNorm(); + double longitude_rad = AcTan(antenna_to_satellite_direction_c[1], antenna_to_satellite_direction_c[0]); + double latitude_rad = + AcTan(antenna_to_satellite_direction_c[2], sqrt(pow(antenna_to_satellite_direction_c[0], 2.0) + pow(antenna_to_satellite_direction_c[1], 2.0))); - double dist = ant2gnss_c.CalcNorm(); - double lon = AcTan(ant2gnss_c[1], ant2gnss_c[0]); - double lat = AcTan(ant2gnss_c[2], sqrt(pow(ant2gnss_c[0], 2.0) + pow(ant2gnss_c[1], 2.0))); - - GnssInfo gnss_info_new = {gnss_id, lat, lon, dist}; + GnssInfo gnss_info_new = {gnss_id, latitude_rad, longitude_rad, distance_m}; gnss_information_list_.push_back(gnss_info_new); } From 6e0c63da9e09d6bc563f3f4af2073dfc11cd4904 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 14:57:59 +0100 Subject: [PATCH 14/61] Fix comment --- src/components/real/aocs/gnss_receiver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index d20511288..346f1183c 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -186,10 +186,9 @@ void GnssReceiver::AddNoise(const libra::Vector<3> position_true_i_m, const libr void GnssReceiver::ConvertJulianDayToGpsTime(const double julian_day) { const double kJulianDayAtGpsTimeZero = 2444244.5; // corresponds to 1980/1/5 midnight const double kDayInWeek = 7.0; - // const double kSecInWeek = 604800.0; const double kSecInDay = 86400.0; - // compute ToW from current julian_day + // compute time of week from current julian_day // note:"gps_time_week_ " computed in this method is larger than 1024 double elapsed_day = julian_day - kJulianDayAtGpsTimeZero; gps_time_week_ = (unsigned int)(elapsed_day / kDayInWeek); From c79ae7affd2aba6fb58bb22cb504f716b15646fc Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 15:08:08 +0100 Subject: [PATCH 15/61] Fix unit of harf width angle --- src/components/real/aocs/gnss_receiver.cpp | 18 +++++++++--------- src/components/real/aocs/gnss_receiver.hpp | 10 +++++----- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 346f1183c..002877bb5 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -13,7 +13,7 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_id, const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, - const libra::Quaternion quaternion_b2c, const double half_width_rad, const libra::Vector<3> noise_standard_deviation_m, + const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator), component_id_(component_id), @@ -23,7 +23,7 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, random_noise_i_x_(0.0, noise_standard_deviation_m[0], global_randomization.MakeSeed()), random_noise_i_y_(0.0, noise_standard_deviation_m[1], global_randomization.MakeSeed()), random_noise_i_z_(0.0, noise_standard_deviation_m[2], global_randomization.MakeSeed()), - half_width_rad_(half_width_rad), + half_width_deg_(half_width_deg), gnss_id_(gnss_id), antenna_model_(antenna_model), dynamics_(dynamics), @@ -31,7 +31,7 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, simulation_time_(simulation_time) {} GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string gnss_id, const size_t max_channel, const AntennaModel antenna_model, - const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_rad, + const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator, power_port), @@ -42,7 +42,7 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, random_noise_i_x_(0.0, noise_standard_deviation_m[0], global_randomization.MakeSeed()), random_noise_i_y_(0.0, noise_standard_deviation_m[1], global_randomization.MakeSeed()), random_noise_i_z_(0.0, noise_standard_deviation_m[2], global_randomization.MakeSeed()), - half_width_rad_(half_width_rad), + half_width_deg_(half_width_deg), gnss_id_(gnss_id), antenna_model_(antenna_model), dynamics_(dynamics), @@ -144,7 +144,7 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, } double inner2 = InnerProduct(antenna_direction_i, antenna_to_gnss_satellite_direction_i); - if (inner2 > cos(half_width_rad_ * libra::deg_to_rad) && is_satellite_visible) { + if (inner2 > cos(half_width_deg_ * libra::deg_to_rad) && is_satellite_visible) { // is visible visible_satellite_number_++; SetGnssInfo(antenna_to_satellite_i_m, quaternion_i2b, gnss_id_string); @@ -243,7 +243,7 @@ typedef struct _gnss_receiver_param { AntennaModel antenna_model; libra::Vector<3> antenna_pos_b; libra::Quaternion quaternion_b2c; - double half_width_rad; + double half_width_deg; std::string gnss_id; size_t max_channel; libra::Vector<3> noise_standard_deviation_m; @@ -271,7 +271,7 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat gnssr_conf.ReadVector(GSection, "antenna_position_b_m", gnss_receiver_param.antenna_pos_b); gnssr_conf.ReadQuaternion(GSection, "quaternion_b2c", gnss_receiver_param.quaternion_b2c); - gnss_receiver_param.half_width_rad = gnssr_conf.ReadDouble(GSection, "antenna_half_width_deg"); + gnss_receiver_param.half_width_deg = gnssr_conf.ReadDouble(GSection, "antenna_half_width_deg"); gnss_receiver_param.gnss_id = gnssr_conf.ReadString(GSection, "gnss_id"); gnss_receiver_param.max_channel = gnssr_conf.ReadInt(GSection, "maximum_channel"); gnssr_conf.ReadVector(GSection, "white_noise_standard_deviation_eci_m", gnss_receiver_param.noise_standard_deviation_m); @@ -284,7 +284,7 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t comp GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.gnss_id, gr_param.max_channel, gr_param.antenna_model, - gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_rad, gr_param.noise_standard_deviation_m, dynamics, + gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.noise_standard_deviation_m, dynamics, gnss_satellites, simulation_time); return gnss_r; } @@ -297,7 +297,7 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_ power_port->InitializeWithInitializeFile(file_name); GnssReceiver gnss_r(gr_param.prescaler, clock_generator, power_port, component_id, gr_param.gnss_id, gr_param.max_channel, gr_param.antenna_model, - gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_rad, gr_param.noise_standard_deviation_m, dynamics, + gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.noise_standard_deviation_m, dynamics, gnss_satellites, simulation_time); return gnss_r; } diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 0b3380823..23b9ba7ab 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -53,7 +53,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] antenna_model: Antenna model * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] * @param [in] quaternion_b2c: Quaternion from body frame to component frame (antenna frame) - * @param [in] half_width_rad: Half width of the antenna cone model [rad] + * @param [in] half_width_deg: Half width of the antenna cone model [deg] * @param [in] noise_standard_deviation_m: Standard deviation of normal random noise in the ECI frame [m] * @param [in] dynamics: Dynamics information * @param [in] gnss_satellites: GNSS Satellites information @@ -61,7 +61,7 @@ class GnssReceiver : public Component, public ILoggable { */ GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_id, const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, - const double half_width_rad, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, + const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn GnssReceiver @@ -74,7 +74,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] antenna_model: Antenna model * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] * @param [in] quaternion_b2c: Quaternion from body frame to component frame (antenna frame) - * @param [in] half_width_rad: Half width of the antenna cone model [rad] + * @param [in] half_width_deg: Half width of the antenna cone model [rad] * @param [in] noise_standard_deviation_m: Standard deviation of normal random noise in the ECI frame [m] * @param [in] dynamics: Dynamics information * @param [in] gnss_satellites: GNSS Satellites information @@ -82,7 +82,7 @@ class GnssReceiver : public Component, public ILoggable { */ GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string gnss_id, const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, - const libra::Quaternion quaternion_b2c, const double half_width_rad, const libra::Vector<3> noise_standard_deviation_m, + const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); // Override functions for Component @@ -149,7 +149,7 @@ class GnssReceiver : public Component, public ILoggable { libra::NormalRand random_noise_i_x_, random_noise_i_y_, random_noise_i_z_; //!< Random noise for each axis - double half_width_rad_ = 0.0; //!< Half width of the antenna cone model [rad] + double half_width_deg_ = 0.0; //!< Half width of the antenna cone model [deg] std::string gnss_id_; //!< GNSS satellite number defined by GNSS system AntennaModel antenna_model_; //!< Antenna model From 92e53742ef8efdc1b8e9d8ca84c42d1363df56ea Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 15:09:59 +0100 Subject: [PATCH 16/61] Fix to use normalize function --- src/components/real/aocs/gnss_receiver.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 002877bb5..f957e5370 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -123,9 +123,8 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, // compute direction from sat to gnss in body-fixed frame libra::Vector<3> gnss_satellite_position_i_m = gnss_satellites_->GetPosition_eci_m(i); - libra::Vector<3> antenna_to_satellite_i_m = gnss_satellite_position_i_m - antenna_position_i_m; - double normalizer = 1 / antenna_to_satellite_i_m.CalcNorm(); - libra::Vector<3> antenna_to_gnss_satellite_direction_i = normalizer * antenna_to_satellite_i_m; + libra::Vector<3> antenna_to_gnss_satellite_i_m = gnss_satellite_position_i_m - antenna_position_i_m; + libra::Vector<3> antenna_to_gnss_satellite_direction_i = antenna_to_gnss_satellite_i_m.CalcNormalizedVector(); // check gnss satellites are visible from antenna double inner1 = InnerProduct(antenna_position_i_m, gnss_satellite_position_i_m); @@ -133,7 +132,8 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, if (inner1 > 0) { is_satellite_visible = true; } else { - Vector<3> tmp = antenna_position_i_m + InnerProduct(-antenna_position_i_m, antenna_to_gnss_satellite_direction_i) * antenna_to_satellite_i_m; + Vector<3> tmp = + antenna_position_i_m + InnerProduct(-antenna_position_i_m, antenna_to_gnss_satellite_direction_i) * antenna_to_gnss_satellite_i_m; if (tmp.CalcNorm() < environment::earth_equatorial_radius_m) { // There is earth between antenna and gnss is_satellite_visible = false; @@ -147,7 +147,7 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, if (inner2 > cos(half_width_deg_ * libra::deg_to_rad) && is_satellite_visible) { // is visible visible_satellite_number_++; - SetGnssInfo(antenna_to_satellite_i_m, quaternion_i2b, gnss_id_string); + SetGnssInfo(antenna_to_gnss_satellite_i_m, quaternion_i2b, gnss_id_string); } } From d9a89e80b1b4f4b072e95422023aa746f0aae58a Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 15:14:54 +0100 Subject: [PATCH 17/61] Fix variable name to suit with coding rule --- src/components/real/aocs/gnss_receiver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 23b9ba7ab..eb6c32c6c 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -30,7 +30,7 @@ enum class AntennaModel { * @brief Information of GNSS satellites */ typedef struct _gnss_info { - std::string ID; //!< ID of GNSS satellites + std::string id; //!< ID of GNSS satellites double latitude_rad; //!< Latitude on the antenna frame [rad] double longitude_rad; //!< Longitude on the antenna frame [rad] double distance_m; //!< Distance between the GNSS satellite and the GNSS receiver antenna [m] From e2f8d3d6adbb7689c9b8247ccc424dd3f9b7b279 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 16:18:19 +0100 Subject: [PATCH 18/61] Fix to use geodetic position --- src/components/real/aocs/gnss_receiver.cpp | 8 ++++---- src/components/real/aocs/gnss_receiver.hpp | 7 ++----- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index f957e5370..98057e257 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -60,7 +60,7 @@ void GnssReceiver::MainRoutine(const int time_count) { if (is_gnss_visible_) { // Antenna of GNSS-R can detect GNSS signal position_ecef_m_ = dynamics_->GetOrbit().GetPosition_ecef_m(); - position_llh_ = dynamics_->GetOrbit().GetLatLonAlt(); + geodetic_position_ = dynamics_->GetOrbit().GetGeodeticPosition(); velocity_ecef_m_s_ = dynamics_->GetOrbit().GetVelocity_ecef_m_s(); AddNoise(position_true_eci_, position_ecef_m_); } else { @@ -229,9 +229,9 @@ std::string GnssReceiver::GetLogValue() const // For logs str_tmp += WriteScalar(utc_.second); str_tmp += WriteVector(position_eci_m_, 10); str_tmp += WriteVector(velocity_ecef_m_s_, 10); - str_tmp += WriteScalar(position_llh_[0], 10); - str_tmp += WriteScalar(position_llh_[1], 10); - str_tmp += WriteScalar(position_llh_[2], 10); + str_tmp += WriteScalar(geodetic_position_.GetLatitude_rad(), 10); + str_tmp += WriteScalar(geodetic_position_.GetLongitude_rad(), 10); + str_tmp += WriteScalar(geodetic_position_.GetAltitude_m(), 10); str_tmp += WriteScalar(is_gnss_visible_); str_tmp += WriteScalar(visible_satellite_number_); diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index eb6c32c6c..8b4f8cf07 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -113,10 +113,7 @@ class GnssReceiver : public Component, public ILoggable { * @fn GetMeasuredGeodeticPosition * @brief Return Observed position in the LLH frame [m] */ - inline const GeodeticPosition GetMeasuredGeodeticPosition(void) const { - GeodeticPosition geodetic_position(position_llh_[0], position_llh_[1], position_llh_[2]); - return geodetic_position; - } + inline const GeodeticPosition GetMeasuredGeodeticPosition(void) const { return geodetic_position_; } /** * @fn GetMeasuredVelocity_eci_m_s * @brief Return Observed velocity in the ECI frame [m/s] @@ -158,7 +155,7 @@ class GnssReceiver : public Component, public ILoggable { libra::Vector<3> velocity_eci_m_s_{0.0}; //!< Observed velocity in the ECI frame [m/s] libra::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] libra::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] - libra::Vector<3> position_llh_{0.0}; //!< Observed position in the geodetic frame [rad,rad,m] TODO: use GeodeticPosition class + GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] unsigned int gps_time_week_ = 0; //!< Observed GPS time week part double gps_time_s_ = 0.0; //!< Observed GPS time second part From 1204b9488bf2273ca53e6ac56131fdeadebdc962 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 16:21:06 +0100 Subject: [PATCH 19/61] Fix to add noise for the geodetic position --- src/components/real/aocs/gnss_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 98057e257..e499a4169 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -60,9 +60,9 @@ void GnssReceiver::MainRoutine(const int time_count) { if (is_gnss_visible_) { // Antenna of GNSS-R can detect GNSS signal position_ecef_m_ = dynamics_->GetOrbit().GetPosition_ecef_m(); - geodetic_position_ = dynamics_->GetOrbit().GetGeodeticPosition(); velocity_ecef_m_s_ = dynamics_->GetOrbit().GetVelocity_ecef_m_s(); AddNoise(position_true_eci_, position_ecef_m_); + geodetic_position_.UpdateFromEcef(position_ecef_m_); } else { // position information will not be updated in this case utc_ = simulation_time_->GetCurrentUtc(); From 564d9562782b37ebee7e7960636ce1a915d749c6 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 16:27:25 +0100 Subject: [PATCH 20/61] Fix comments in ini file --- data/sample/initialize_files/components/gnss_receiver.ini | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/data/sample/initialize_files/components/gnss_receiver.ini b/data/sample/initialize_files/components/gnss_receiver.ini index a17ed683e..8a0120c8c 100644 --- a/data/sample/initialize_files/components/gnss_receiver.ini +++ b/data/sample/initialize_files/components/gnss_receiver.ini @@ -6,7 +6,8 @@ antenna_position_b_m(0) = 0.0125 antenna_position_b_m(1) = 0.0000 antenna_position_b_m(2) = 0.1815 -// Quaternion from body frame to component frame +// Quaternion from body frame to component frame of the antenna +// Note: The antenna boresight direction is +Z direction at the component frame quaternion_b2c(0) = 0.0 quaternion_b2c(1) = 0.0 quaternion_b2c(2) = 0.0 @@ -23,7 +24,7 @@ antenna_half_width_deg = 60 // Number of channels maximum_channel = 8 -// GNSS ID +// Target GNSS system ID // G...GPS // R...GLONASS // E...Galileo @@ -33,7 +34,7 @@ maximum_channel = 8 // if your receiver is compatible with GPS and QZSS : GJ gnss_id = G -//Random noise [m] +// Random noise [m] white_noise_standard_deviation_eci_m(0) = 10000.0 white_noise_standard_deviation_eci_m(1) = 1000.0 white_noise_standard_deviation_eci_m(2) = 1000.0 From 9df6917487bf6371b25c6b38428577149592045d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 16:31:43 +0100 Subject: [PATCH 21/61] Rename gnss_id to gnss_system_id --- .../components/gnss_receiver.ini | 2 +- src/components/real/aocs/gnss_receiver.cpp | 31 ++++++++++--------- src/components/real/aocs/gnss_receiver.hpp | 27 ++++++++-------- 3 files changed, 31 insertions(+), 29 deletions(-) diff --git a/data/sample/initialize_files/components/gnss_receiver.ini b/data/sample/initialize_files/components/gnss_receiver.ini index 8a0120c8c..cd6e7b5e1 100644 --- a/data/sample/initialize_files/components/gnss_receiver.ini +++ b/data/sample/initialize_files/components/gnss_receiver.ini @@ -32,7 +32,7 @@ maximum_channel = 8 // J...QZSS // if your receiver is compatible with all kind of gnss satellites : GRECJ // if your receiver is compatible with GPS and QZSS : GJ -gnss_id = G +gnss_system_id = G // Random noise [m] white_noise_standard_deviation_eci_m(0) = 10000.0 diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index e499a4169..907f3b8e5 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -11,7 +11,7 @@ #include #include -GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_id, +GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_system_id, const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) @@ -24,13 +24,13 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, random_noise_i_y_(0.0, noise_standard_deviation_m[1], global_randomization.MakeSeed()), random_noise_i_z_(0.0, noise_standard_deviation_m[2], global_randomization.MakeSeed()), half_width_deg_(half_width_deg), - gnss_id_(gnss_id), + gnss_system_id_(gnss_system_id), antenna_model_(antenna_model), dynamics_(dynamics), gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) {} GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const std::string gnss_id, const size_t max_channel, const AntennaModel antenna_model, + const std::string gnss_system_id, const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) @@ -43,7 +43,7 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, random_noise_i_y_(0.0, noise_standard_deviation_m[1], global_randomization.MakeSeed()), random_noise_i_z_(0.0, noise_standard_deviation_m[2], global_randomization.MakeSeed()), half_width_deg_(half_width_deg), - gnss_id_(gnss_id), + gnss_system_id_(gnss_system_id), antenna_model_(antenna_model), dynamics_(dynamics), gnss_satellites_(gnss_satellites), @@ -118,8 +118,8 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, for (size_t i = 0; i < kTotalNumberOfGnssSatellite; i++) { // check if gnss ID is compatible with the receiver - std::string gnss_id_string = ConvertIndexToGnssSatelliteNumber(i); - if (gnss_id_.find(gnss_id_string[0]) == std::string::npos) continue; + std::string gnss_system_id_string = ConvertIndexToGnssSatelliteNumber(i); + if (gnss_system_id_.find(gnss_system_id_string[0]) == std::string::npos) continue; // compute direction from sat to gnss in body-fixed frame libra::Vector<3> gnss_satellite_position_i_m = gnss_satellites_->GetPosition_eci_m(i); @@ -147,7 +147,7 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, if (inner2 > cos(half_width_deg_ * libra::deg_to_rad) && is_satellite_visible) { // is visible visible_satellite_number_++; - SetGnssInfo(antenna_to_gnss_satellite_i_m, quaternion_i2b, gnss_id_string); + SetGnssInfo(antenna_to_gnss_satellite_i_m, quaternion_i2b, gnss_system_id_string); } } @@ -158,7 +158,8 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, } } -void GnssReceiver::SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, const libra::Quaternion quaternion_i2b, const std::string gnss_id) { +void GnssReceiver::SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, const libra::Quaternion quaternion_i2b, + const std::string gnss_system_id) { libra::Vector<3> antenna_to_satellite_direction_b = quaternion_i2b.FrameConversion(antenna_to_satellite_i_m); libra::Vector<3> antenna_to_satellite_direction_c = quaternion_b2c_.FrameConversion(antenna_to_satellite_direction_b); @@ -167,7 +168,7 @@ void GnssReceiver::SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, double latitude_rad = AcTan(antenna_to_satellite_direction_c[2], sqrt(pow(antenna_to_satellite_direction_c[0], 2.0) + pow(antenna_to_satellite_direction_c[1], 2.0))); - GnssInfo gnss_info_new = {gnss_id, latitude_rad, longitude_rad, distance_m}; + GnssInfo gnss_info_new = {gnss_system_id, latitude_rad, longitude_rad, distance_m}; gnss_information_list_.push_back(gnss_info_new); } @@ -244,7 +245,7 @@ typedef struct _gnss_receiver_param { libra::Vector<3> antenna_pos_b; libra::Quaternion quaternion_b2c; double half_width_deg; - std::string gnss_id; + std::string gnss_system_id; size_t max_channel; libra::Vector<3> noise_standard_deviation_m; } GnssReceiverParam; @@ -272,7 +273,7 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat gnssr_conf.ReadVector(GSection, "antenna_position_b_m", gnss_receiver_param.antenna_pos_b); gnssr_conf.ReadQuaternion(GSection, "quaternion_b2c", gnss_receiver_param.quaternion_b2c); gnss_receiver_param.half_width_deg = gnssr_conf.ReadDouble(GSection, "antenna_half_width_deg"); - gnss_receiver_param.gnss_id = gnssr_conf.ReadString(GSection, "gnss_id"); + gnss_receiver_param.gnss_system_id = gnssr_conf.ReadString(GSection, "gnss_system_id"); gnss_receiver_param.max_channel = gnssr_conf.ReadInt(GSection, "maximum_channel"); gnssr_conf.ReadVector(GSection, "white_noise_standard_deviation_eci_m", gnss_receiver_param.noise_standard_deviation_m); @@ -283,7 +284,7 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t comp const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); - GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.gnss_id, gr_param.max_channel, gr_param.antenna_model, + GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.gnss_system_id, gr_param.max_channel, gr_param.antenna_model, gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.noise_standard_deviation_m, dynamics, gnss_satellites, simulation_time); return gnss_r; @@ -296,8 +297,8 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_ // PowerPort power_port->InitializeWithInitializeFile(file_name); - GnssReceiver gnss_r(gr_param.prescaler, clock_generator, power_port, component_id, gr_param.gnss_id, gr_param.max_channel, gr_param.antenna_model, - gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.noise_standard_deviation_m, dynamics, - gnss_satellites, simulation_time); + GnssReceiver gnss_r(gr_param.prescaler, clock_generator, power_port, component_id, gr_param.gnss_system_id, gr_param.max_channel, + gr_param.antenna_model, gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, + gr_param.noise_standard_deviation_m, dynamics, gnss_satellites, simulation_time); return gnss_r; } diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 8b4f8cf07..6cd9d1819 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -48,7 +48,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] prescaler: Frequency scale factor for update * @param [in] clock_generator: Clock generator * @param [in] component_id: Component ID - * @param [in] gnss_id: GNSS satellite number defined by GNSS system + * @param [in] gnss_system_id: GNSS satellite number defined by GNSS system * @param [in] max_channel: Maximum number of channels * @param [in] antenna_model: Antenna model * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] @@ -59,17 +59,17 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] gnss_satellites: GNSS Satellites information * @param [in] simulation_time: Simulation time information */ - GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_id, const size_t max_channel, - const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, - const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, - const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); + GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_system_id, + const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, + const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, + const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn GnssReceiver * @brief Constructor with power port * @param [in] prescaler: Frequency scale factor for update * @param [in] clock_generator: Clock generator * @param [in] power_port: Power port - * @param [in] gnss_id: GNSS satellite number defined by GNSS system + * @param [in] gnss_system_id: GNSS satellite number defined by GNSS system * @param [in] max_channel: Maximum number of channels * @param [in] antenna_model: Antenna model * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] @@ -80,10 +80,11 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] gnss_satellites: GNSS Satellites information * @param [in] simulation_time: Simulation time information */ - GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string gnss_id, - const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, - const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, - const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); + GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, + const std::string gnss_system_id, const size_t max_channel, const AntennaModel antenna_model, + const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_deg, + const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, + const SimulationTime* simulation_time); // Override functions for Component /** @@ -147,7 +148,7 @@ class GnssReceiver : public Component, public ILoggable { libra::NormalRand random_noise_i_x_, random_noise_i_y_, random_noise_i_z_; //!< Random noise for each axis double half_width_deg_ = 0.0; //!< Half width of the antenna cone model [deg] - std::string gnss_id_; //!< GNSS satellite number defined by GNSS system + std::string gnss_system_id_; //!< GNSS satellite number defined by GNSS system AntennaModel antenna_model_; //!< Antenna model // Calculated values @@ -198,9 +199,9 @@ class GnssReceiver : public Component, public ILoggable { * @brief Calculate and set the GnssInfo values of target GNSS satellite * @param [in] antenna_to_satellite_i_m: Position vector from the antenna to the GNSS satellites in the ECI frame * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame - * @param [in] gnss_id: ID of target GNSS satellite + * @param [in] gnss_system_id: ID of target GNSS satellite */ - void SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, const libra::Quaternion quaternion_i2b, const std::string gnss_id); + void SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, const libra::Quaternion quaternion_i2b, const std::string gnss_system_id); /** * @fn AddNoise * @brief Substitutional method for "Measure" in other sensor models inherited Sensor class From 0ad7095759341bbe52adb6cf515b209e76472021 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 16:33:07 +0100 Subject: [PATCH 22/61] Fix comment --- src/components/real/aocs/gnss_receiver.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 6cd9d1819..d4d8d940c 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -48,7 +48,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] prescaler: Frequency scale factor for update * @param [in] clock_generator: Clock generator * @param [in] component_id: Component ID - * @param [in] gnss_system_id: GNSS satellite number defined by GNSS system + * @param [in] gnss_system_id: Target GNSS system IDs * @param [in] max_channel: Maximum number of channels * @param [in] antenna_model: Antenna model * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] @@ -69,7 +69,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] prescaler: Frequency scale factor for update * @param [in] clock_generator: Clock generator * @param [in] power_port: Power port - * @param [in] gnss_system_id: GNSS satellite number defined by GNSS system + * @param [in] gnss_system_id: Target GNSS system IDs * @param [in] max_channel: Maximum number of channels * @param [in] antenna_model: Antenna model * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] From 538acf8d62290951d17a93ecb9171166fcfc571f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 16:35:13 +0100 Subject: [PATCH 23/61] Remove max channel because it is not used --- .../components/gnss_receiver.ini | 3 -- src/components/real/aocs/gnss_receiver.cpp | 30 ++++++++----------- src/components/real/aocs/gnss_receiver.hpp | 16 ++++------ 3 files changed, 19 insertions(+), 30 deletions(-) diff --git a/data/sample/initialize_files/components/gnss_receiver.ini b/data/sample/initialize_files/components/gnss_receiver.ini index cd6e7b5e1..7be86ea4b 100644 --- a/data/sample/initialize_files/components/gnss_receiver.ini +++ b/data/sample/initialize_files/components/gnss_receiver.ini @@ -21,9 +21,6 @@ antenna_model = 0 // Antenna half width [deg] antenna_half_width_deg = 60 -// Number of channels -maximum_channel = 8 - // Target GNSS system ID // G...GPS // R...GLONASS diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 907f3b8e5..9bcf5666b 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -12,12 +12,11 @@ #include GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_system_id, - const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, - const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, - const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) + const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, + const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, + const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator), component_id_(component_id), - max_channel_(max_channel), antenna_position_b_m_(antenna_position_b_m), quaternion_b2c_(quaternion_b2c), random_noise_i_x_(0.0, noise_standard_deviation_m[0], global_randomization.MakeSeed()), @@ -29,14 +28,13 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, dynamics_(dynamics), gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) {} + GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const std::string gnss_system_id, const size_t max_channel, const AntennaModel antenna_model, - const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_deg, - const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, - const SimulationTime* simulation_time) + const std::string gnss_system_id, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, + const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, + const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator, power_port), component_id_(component_id), - max_channel_(max_channel), antenna_position_b_m_(antenna_position_b_m), quaternion_b2c_(quaternion_b2c), random_noise_i_x_(0.0, noise_standard_deviation_m[0], global_randomization.MakeSeed()), @@ -246,7 +244,6 @@ typedef struct _gnss_receiver_param { libra::Quaternion quaternion_b2c; double half_width_deg; std::string gnss_system_id; - size_t max_channel; libra::Vector<3> noise_standard_deviation_m; } GnssReceiverParam; @@ -274,7 +271,6 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat gnssr_conf.ReadQuaternion(GSection, "quaternion_b2c", gnss_receiver_param.quaternion_b2c); gnss_receiver_param.half_width_deg = gnssr_conf.ReadDouble(GSection, "antenna_half_width_deg"); gnss_receiver_param.gnss_system_id = gnssr_conf.ReadString(GSection, "gnss_system_id"); - gnss_receiver_param.max_channel = gnssr_conf.ReadInt(GSection, "maximum_channel"); gnssr_conf.ReadVector(GSection, "white_noise_standard_deviation_eci_m", gnss_receiver_param.noise_standard_deviation_m); return gnss_receiver_param; @@ -284,9 +280,9 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t comp const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); - GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.gnss_system_id, gr_param.max_channel, gr_param.antenna_model, - gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.noise_standard_deviation_m, dynamics, - gnss_satellites, simulation_time); + GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.gnss_system_id, gr_param.antenna_model, gr_param.antenna_pos_b, + gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.noise_standard_deviation_m, dynamics, gnss_satellites, + simulation_time); return gnss_r; } @@ -297,8 +293,8 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_ // PowerPort power_port->InitializeWithInitializeFile(file_name); - GnssReceiver gnss_r(gr_param.prescaler, clock_generator, power_port, component_id, gr_param.gnss_system_id, gr_param.max_channel, - gr_param.antenna_model, gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, - gr_param.noise_standard_deviation_m, dynamics, gnss_satellites, simulation_time); + GnssReceiver gnss_r(gr_param.prescaler, clock_generator, power_port, component_id, gr_param.gnss_system_id, gr_param.antenna_model, + gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.noise_standard_deviation_m, dynamics, + gnss_satellites, simulation_time); return gnss_r; } diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index d4d8d940c..006d8ec97 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -49,7 +49,6 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] clock_generator: Clock generator * @param [in] component_id: Component ID * @param [in] gnss_system_id: Target GNSS system IDs - * @param [in] max_channel: Maximum number of channels * @param [in] antenna_model: Antenna model * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] * @param [in] quaternion_b2c: Quaternion from body frame to component frame (antenna frame) @@ -60,9 +59,9 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] simulation_time: Simulation time information */ GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_system_id, - const size_t max_channel, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, - const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, - const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); + const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, + const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, + const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn GnssReceiver * @brief Constructor with power port @@ -70,7 +69,6 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] clock_generator: Clock generator * @param [in] power_port: Power port * @param [in] gnss_system_id: Target GNSS system IDs - * @param [in] max_channel: Maximum number of channels * @param [in] antenna_model: Antenna model * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] * @param [in] quaternion_b2c: Quaternion from body frame to component frame (antenna frame) @@ -81,10 +79,9 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] simulation_time: Simulation time information */ GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const std::string gnss_system_id, const size_t max_channel, const AntennaModel antenna_model, - const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_deg, - const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, - const SimulationTime* simulation_time); + const std::string gnss_system_id, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, + const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, + const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); // Override functions for Component /** @@ -141,7 +138,6 @@ class GnssReceiver : public Component, public ILoggable { protected: // Parameters for receiver const size_t component_id_; //!< Receiver ID - const size_t max_channel_; //!< Maximum number of channels libra::Vector<3> antenna_position_b_m_; //!< GNSS antenna position at the body-fixed frame [m] libra::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (antenna frame) From c53946d41703b3078172f68508f11ba8a0d07e07 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 16:38:04 +0100 Subject: [PATCH 24/61] Clean up local variables definition --- src/components/real/aocs/gnss_receiver.cpp | 12 ++++---- src/components/real/aocs/gnss_receiver.hpp | 36 +++++++++++----------- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 9bcf5666b..6eddfbd55 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -19,12 +19,12 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, component_id_(component_id), antenna_position_b_m_(antenna_position_b_m), quaternion_b2c_(quaternion_b2c), - random_noise_i_x_(0.0, noise_standard_deviation_m[0], global_randomization.MakeSeed()), - random_noise_i_y_(0.0, noise_standard_deviation_m[1], global_randomization.MakeSeed()), - random_noise_i_z_(0.0, noise_standard_deviation_m[2], global_randomization.MakeSeed()), half_width_deg_(half_width_deg), gnss_system_id_(gnss_system_id), antenna_model_(antenna_model), + random_noise_i_x_(0.0, noise_standard_deviation_m[0], global_randomization.MakeSeed()), + random_noise_i_y_(0.0, noise_standard_deviation_m[1], global_randomization.MakeSeed()), + random_noise_i_z_(0.0, noise_standard_deviation_m[2], global_randomization.MakeSeed()), dynamics_(dynamics), gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) {} @@ -37,12 +37,12 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, component_id_(component_id), antenna_position_b_m_(antenna_position_b_m), quaternion_b2c_(quaternion_b2c), - random_noise_i_x_(0.0, noise_standard_deviation_m[0], global_randomization.MakeSeed()), - random_noise_i_y_(0.0, noise_standard_deviation_m[1], global_randomization.MakeSeed()), - random_noise_i_z_(0.0, noise_standard_deviation_m[2], global_randomization.MakeSeed()), half_width_deg_(half_width_deg), gnss_system_id_(gnss_system_id), antenna_model_(antenna_model), + random_noise_i_x_(0.0, noise_standard_deviation_m[0], global_randomization.MakeSeed()), + random_noise_i_y_(0.0, noise_standard_deviation_m[1], global_randomization.MakeSeed()), + random_noise_i_z_(0.0, noise_standard_deviation_m[2], global_randomization.MakeSeed()), dynamics_(dynamics), gnss_satellites_(gnss_satellites), simulation_time_(simulation_time) {} diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 006d8ec97..55c0b5dc7 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -137,28 +137,28 @@ class GnssReceiver : public Component, public ILoggable { protected: // Parameters for receiver - const size_t component_id_; //!< Receiver ID + const size_t component_id_; //!< Receiver ID + + // Antenna libra::Vector<3> antenna_position_b_m_; //!< GNSS antenna position at the body-fixed frame [m] libra::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (antenna frame) - - libra::NormalRand random_noise_i_x_, random_noise_i_y_, random_noise_i_z_; //!< Random noise for each axis - - double half_width_deg_ = 0.0; //!< Half width of the antenna cone model [deg] - std::string gnss_system_id_; //!< GNSS satellite number defined by GNSS system - AntennaModel antenna_model_; //!< Antenna model + double half_width_deg_ = 0.0; //!< Half width of the antenna cone model [deg] + std::string gnss_system_id_; //!< GNSS satellite number defined by GNSS system + AntennaModel antenna_model_; //!< Antenna model // Calculated values - libra::Vector<3> position_eci_m_{0.0}; //!< Observed position in the ECI frame [m] - libra::Vector<3> velocity_eci_m_s_{0.0}; //!< Observed velocity in the ECI frame [m/s] - libra::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] - libra::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] - GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame - UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] - unsigned int gps_time_week_ = 0; //!< Observed GPS time week part - double gps_time_s_ = 0.0; //!< Observed GPS time second part - bool is_gnss_visible_ = false; //!< Flag for GNSS satellite is visible or not - size_t visible_satellite_number_ = 0; //!< Number of visible GNSS satellites - std::vector gnss_information_list_; //!< Information List of visible GNSS satellites + libra::NormalRand random_noise_i_x_, random_noise_i_y_, random_noise_i_z_; //!< Random noise for each axis + libra::Vector<3> position_eci_m_{0.0}; //!< Observed position in the ECI frame [m] + libra::Vector<3> velocity_eci_m_s_{0.0}; //!< Observed velocity in the ECI frame [m/s] + libra::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] + libra::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] + GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame + UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] + unsigned int gps_time_week_ = 0; //!< Observed GPS time week part + double gps_time_s_ = 0.0; //!< Observed GPS time second part + bool is_gnss_visible_ = false; //!< Flag for GNSS satellite is visible or not + size_t visible_satellite_number_ = 0; //!< Number of visible GNSS satellites + std::vector gnss_information_list_; //!< Information List of visible GNSS satellites // References const Dynamics* dynamics_; //!< Dynamics of spacecraft From 598c40b01228c781e09e72810bd23495ad8106d5 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 16:57:01 +0100 Subject: [PATCH 25/61] Modify antenna model interface --- .../components/gnss_receiver.ini | 8 +++++--- src/components/real/aocs/gnss_receiver.cpp | 19 ++++++++++++++++--- src/components/real/aocs/gnss_receiver.hpp | 8 ++++++++ 3 files changed, 29 insertions(+), 6 deletions(-) diff --git a/data/sample/initialize_files/components/gnss_receiver.ini b/data/sample/initialize_files/components/gnss_receiver.ini index 7be86ea4b..378345817 100644 --- a/data/sample/initialize_files/components/gnss_receiver.ini +++ b/data/sample/initialize_files/components/gnss_receiver.ini @@ -14,9 +14,11 @@ quaternion_b2c(2) = 0.0 quaternion_b2c(3) = 1.0 // Antenna model -// 0... simple model : GNSS sats are visible when antenna directs anti-earth direction -// 1... cone model : GNSS sats visible when a sat is in a cone -antenna_model = 0 +// SIMPLE: We assume that GNSS satellites are visible when antenna directs anti-earth direction +// CONE: We calculate the number of GNSS satellites in the antenna, +// and the position is observable when more than 4 satellites are in the antenna. +// Note : We need to enable the GNSS satellite calculation when we use this mode. +antenna_model = SIMPLE // Antenna half width [deg] antenna_half_width_deg = 60 diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 6eddfbd55..c5d9502b8 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -237,6 +237,18 @@ std::string GnssReceiver::GetLogValue() const // For logs return str_tmp; } +AntennaModel SetAntennaModel(const std::string antenna_model) { + if (antenna_model == "SIMPLE") { + return AntennaModel ::kSimple; + } else if (antenna_model == "CONE") { + return AntennaModel ::kCone; + } else { + std::cerr << "[WARNINGS] GNSS receiver antenna model is not defined!" << std::endl; + std::cerr << "The antenna model is automatically initialized as SIMPLE mode" << std::endl; + return AntennaModel ::kSimple; + } +} + typedef struct _gnss_receiver_param { int prescaler; AntennaModel antenna_model; @@ -259,10 +271,11 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat if (prescaler <= 1) prescaler = 1; gnss_receiver_param.prescaler = prescaler; - gnss_receiver_param.antenna_model = static_cast(gnssr_conf.ReadInt(GSection, "antenna_model")); + std::string antenna_model_name = gnssr_conf.ReadString(GSection, "antenna_model"); + gnss_receiver_param.antenna_model = SetAntennaModel(antenna_model_name); if (!gnss_satellites->IsCalcEnabled() && gnss_receiver_param.antenna_model == AntennaModel::kCone) { - std::cout << "Calculation of GNSS SATELLITES is DISABLED, so the antenna " - "model of GNSS Receiver is automatically set to SIMPLE model." + std::cout << "[WARNINGS] Calculation of GNSS SATELLITES is DISABLED, " + "so the antenna model of GnssReceiver is automatically set to SIMPLE model." << std::endl; gnss_receiver_param.antenna_model = AntennaModel::kSimple; } diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 55c0b5dc7..fd008e9bc 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -213,6 +213,14 @@ class GnssReceiver : public Component, public ILoggable { void ConvertJulianDayToGpsTime(const double julian_day); }; +/** + * @fn SetAntennaModel + * @brief Set AntennaModel by string + * @param [in] antenna_model: Antenna model name + * @return antenna model + */ +AntennaModel SetAntennaModel(const std::string antenna_model); + /** * @fn InitGnssReceiver * @brief Initialize functions for GNSS Receiver without power port From 3a666c0ab642e76a3a851485a9639e8074467be0 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 09:11:39 +0100 Subject: [PATCH 26/61] Add description comment --- data/sample/initialize_files/components/gnss_receiver.ini | 1 + 1 file changed, 1 insertion(+) diff --git a/data/sample/initialize_files/components/gnss_receiver.ini b/data/sample/initialize_files/components/gnss_receiver.ini index 378345817..4c6b38372 100644 --- a/data/sample/initialize_files/components/gnss_receiver.ini +++ b/data/sample/initialize_files/components/gnss_receiver.ini @@ -31,6 +31,7 @@ antenna_half_width_deg = 60 // J...QZSS // if your receiver is compatible with all kind of gnss satellites : GRECJ // if your receiver is compatible with GPS and QZSS : GJ +// Note: GNSS satellite calculation must includes the target GNSS systems gnss_system_id = G // Random noise [m] From 827ec0c07e1d685e5bcc9467ce30c70b64a7215f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 09:33:26 +0100 Subject: [PATCH 27/61] Modify to use ECEF frame for noise definition and Add velocity noise --- .../components/gnss_receiver.ini | 10 +++- src/components/real/aocs/gnss_receiver.cpp | 60 ++++++++++--------- src/components/real/aocs/gnss_receiver.hpp | 45 +++++++------- 3 files changed, 64 insertions(+), 51 deletions(-) diff --git a/data/sample/initialize_files/components/gnss_receiver.ini b/data/sample/initialize_files/components/gnss_receiver.ini index 4c6b38372..2b7ce54bd 100644 --- a/data/sample/initialize_files/components/gnss_receiver.ini +++ b/data/sample/initialize_files/components/gnss_receiver.ini @@ -35,9 +35,13 @@ antenna_half_width_deg = 60 gnss_system_id = G // Random noise [m] -white_noise_standard_deviation_eci_m(0) = 10000.0 -white_noise_standard_deviation_eci_m(1) = 1000.0 -white_noise_standard_deviation_eci_m(2) = 1000.0 +white_noise_standard_deviation_position_ecef_m(0) = 2000.0 +white_noise_standard_deviation_position_ecef_m(1) = 1000.0 +white_noise_standard_deviation_position_ecef_m(2) = 1500.0 + +white_noise_standard_deviation_velocity_ecef_m_s(0) = 1.0 +white_noise_standard_deviation_velocity_ecef_m_s(1) = 1.5 +white_noise_standard_deviation_velocity_ecef_m_s(2) = 2.0 [POWER_PORT] minimum_voltage_V = 3.3 diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index c5d9502b8..f390121e2 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -13,7 +13,8 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_system_id, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, - const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, + const double half_width_deg, const libra::Vector<3> position_noise_standard_deviation_ecef_m, + const libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator), component_id_(component_id), @@ -22,17 +23,21 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, half_width_deg_(half_width_deg), gnss_system_id_(gnss_system_id), antenna_model_(antenna_model), - random_noise_i_x_(0.0, noise_standard_deviation_m[0], global_randomization.MakeSeed()), - random_noise_i_y_(0.0, noise_standard_deviation_m[1], global_randomization.MakeSeed()), - random_noise_i_z_(0.0, noise_standard_deviation_m[2], global_randomization.MakeSeed()), dynamics_(dynamics), gnss_satellites_(gnss_satellites), - simulation_time_(simulation_time) {} + simulation_time_(simulation_time) { + for (size_t i = 0; i < 3; i++) { + position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], global_randomization.MakeSeed()); + velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], global_randomization.MakeSeed()); + } +} GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string gnss_system_id, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, - const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, - const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) + const libra::Quaternion quaternion_b2c, const double half_width_deg, + const libra::Vector<3> position_noise_standard_deviation_ecef_m, + const libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, + const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator, power_port), component_id_(component_id), antenna_position_b_m_(antenna_position_b_m), @@ -40,12 +45,14 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, half_width_deg_(half_width_deg), gnss_system_id_(gnss_system_id), antenna_model_(antenna_model), - random_noise_i_x_(0.0, noise_standard_deviation_m[0], global_randomization.MakeSeed()), - random_noise_i_y_(0.0, noise_standard_deviation_m[1], global_randomization.MakeSeed()), - random_noise_i_z_(0.0, noise_standard_deviation_m[2], global_randomization.MakeSeed()), dynamics_(dynamics), gnss_satellites_(gnss_satellites), - simulation_time_(simulation_time) {} + simulation_time_(simulation_time) { + for (size_t i = 0; i < 3; i++) { + position_random_noise_ecef_m_[i].SetParameters(0.0, position_noise_standard_deviation_ecef_m[i], global_randomization.MakeSeed()); + velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i], global_randomization.MakeSeed()); + } +} void GnssReceiver::MainRoutine(const int time_count) { UNUSED(time_count); @@ -59,7 +66,7 @@ void GnssReceiver::MainRoutine(const int time_count) { // Antenna of GNSS-R can detect GNSS signal position_ecef_m_ = dynamics_->GetOrbit().GetPosition_ecef_m(); velocity_ecef_m_s_ = dynamics_->GetOrbit().GetVelocity_ecef_m_s(); - AddNoise(position_true_eci_, position_ecef_m_); + AddNoise(position_ecef_m_, velocity_ecef_m_s_); geodetic_position_.UpdateFromEcef(position_ecef_m_); } else { // position information will not be updated in this case @@ -170,16 +177,11 @@ void GnssReceiver::SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, gnss_information_list_.push_back(gnss_info_new); } -void GnssReceiver::AddNoise(const libra::Vector<3> position_true_i_m, const libra::Vector<3> position_true_ecef_m) { - // Simplest noise model - position_eci_m_[0] = position_true_i_m[0] + random_noise_i_x_; - position_eci_m_[1] = position_true_i_m[1] + random_noise_i_y_; - position_eci_m_[2] = position_true_i_m[2] + random_noise_i_z_; - - // FIXME: noise in ECI frame is added to ECEF frame value - position_ecef_m_[0] = position_true_ecef_m[0] + random_noise_i_x_; - position_ecef_m_[1] = position_true_ecef_m[1] + random_noise_i_y_; - position_ecef_m_[2] = position_true_ecef_m[2] + random_noise_i_z_; +void GnssReceiver::AddNoise(const libra::Vector<3> position_true_ecef_m, const libra::Vector<3> velocity_true_ecef_m_s) { + for (size_t i = 0; i < 3; i++) { + position_ecef_m_[i] = position_true_ecef_m[i] + position_random_noise_ecef_m_[i]; + velocity_ecef_m_s_[i] = velocity_true_ecef_m_s[i] + velocity_random_noise_ecef_m_s_[i]; + } } void GnssReceiver::ConvertJulianDayToGpsTime(const double julian_day) { @@ -256,7 +258,8 @@ typedef struct _gnss_receiver_param { libra::Quaternion quaternion_b2c; double half_width_deg; std::string gnss_system_id; - libra::Vector<3> noise_standard_deviation_m; + libra::Vector<3> position_noise_standard_deviation_ecef_m; + libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s; } GnssReceiverParam; GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const int component_id) { @@ -284,7 +287,8 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat gnssr_conf.ReadQuaternion(GSection, "quaternion_b2c", gnss_receiver_param.quaternion_b2c); gnss_receiver_param.half_width_deg = gnssr_conf.ReadDouble(GSection, "antenna_half_width_deg"); gnss_receiver_param.gnss_system_id = gnssr_conf.ReadString(GSection, "gnss_system_id"); - gnssr_conf.ReadVector(GSection, "white_noise_standard_deviation_eci_m", gnss_receiver_param.noise_standard_deviation_m); + gnssr_conf.ReadVector(GSection, "white_noise_standard_deviation_position_ecef_m", gnss_receiver_param.position_noise_standard_deviation_ecef_m); + gnssr_conf.ReadVector(GSection, "white_noise_standard_deviation_velocity_ecef_m_s", gnss_receiver_param.velocity_noise_standard_deviation_ecef_m_s); return gnss_receiver_param; } @@ -294,8 +298,8 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t comp GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.gnss_system_id, gr_param.antenna_model, gr_param.antenna_pos_b, - gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.noise_standard_deviation_m, dynamics, gnss_satellites, - simulation_time); + gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.position_noise_standard_deviation_ecef_m, + gr_param.velocity_noise_standard_deviation_ecef_m_s, dynamics, gnss_satellites, simulation_time); return gnss_r; } @@ -307,7 +311,7 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_ power_port->InitializeWithInitializeFile(file_name); GnssReceiver gnss_r(gr_param.prescaler, clock_generator, power_port, component_id, gr_param.gnss_system_id, gr_param.antenna_model, - gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.noise_standard_deviation_m, dynamics, - gnss_satellites, simulation_time); + gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.position_noise_standard_deviation_ecef_m, + gr_param.velocity_noise_standard_deviation_ecef_m_s, dynamics, gnss_satellites, simulation_time); return gnss_r; } diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index fd008e9bc..c8ebba3fb 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -53,15 +53,17 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] * @param [in] quaternion_b2c: Quaternion from body frame to component frame (antenna frame) * @param [in] half_width_deg: Half width of the antenna cone model [deg] - * @param [in] noise_standard_deviation_m: Standard deviation of normal random noise in the ECI frame [m] + * @param [in] position_noise_standard_deviation_ecef_m: Standard deviation of normal random noise for position in the ECEF frame [m] + * @param [in] velocity_noise_standard_deviation_ecef_m_s: Standard deviation of normal random noise for velocity in the ECEF frame [m/s] * @param [in] dynamics: Dynamics information * @param [in] gnss_satellites: GNSS Satellites information * @param [in] simulation_time: Simulation time information */ GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_system_id, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, - const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, const Dynamics* dynamics, - const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); + const double half_width_deg, const libra::Vector<3> position_noise_standard_deviation_ecef_m, + const libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, + const SimulationTime* simulation_time); /** * @fn GnssReceiver * @brief Constructor with power port @@ -73,15 +75,17 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] * @param [in] quaternion_b2c: Quaternion from body frame to component frame (antenna frame) * @param [in] half_width_deg: Half width of the antenna cone model [rad] - * @param [in] noise_standard_deviation_m: Standard deviation of normal random noise in the ECI frame [m] + * @param [in] position_noise_standard_deviation_ecef_m: Standard deviation of normal random noise for position in the ECEF frame [m] + * @param [in] velocity_noise_standard_deviation_ecef_m_s: Standard deviation of normal random noise for velocity in the ECEF frame [m/s] * @param [in] dynamics: Dynamics information * @param [in] gnss_satellites: GNSS Satellites information * @param [in] simulation_time: Simulation time information */ GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, const std::string gnss_system_id, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, - const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> noise_standard_deviation_m, - const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); + const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> position_noise_standard_deviation_ecef_m, + const libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, + const SimulationTime* simulation_time); // Override functions for Component /** @@ -147,18 +151,20 @@ class GnssReceiver : public Component, public ILoggable { AntennaModel antenna_model_; //!< Antenna model // Calculated values - libra::NormalRand random_noise_i_x_, random_noise_i_y_, random_noise_i_z_; //!< Random noise for each axis - libra::Vector<3> position_eci_m_{0.0}; //!< Observed position in the ECI frame [m] - libra::Vector<3> velocity_eci_m_s_{0.0}; //!< Observed velocity in the ECI frame [m/s] - libra::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] - libra::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] - GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame - UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] - unsigned int gps_time_week_ = 0; //!< Observed GPS time week part - double gps_time_s_ = 0.0; //!< Observed GPS time second part - bool is_gnss_visible_ = false; //!< Flag for GNSS satellite is visible or not - size_t visible_satellite_number_ = 0; //!< Number of visible GNSS satellites - std::vector gnss_information_list_; //!< Information List of visible GNSS satellites + libra::NormalRand position_random_noise_ecef_m_[3]; + libra::NormalRand velocity_random_noise_ecef_m_s_[3]; + + libra::Vector<3> position_eci_m_{0.0}; //!< Observed position in the ECI frame [m] + libra::Vector<3> velocity_eci_m_s_{0.0}; //!< Observed velocity in the ECI frame [m/s] + libra::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] + libra::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] + GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame + UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] + unsigned int gps_time_week_ = 0; //!< Observed GPS time week part + double gps_time_s_ = 0.0; //!< Observed GPS time second part + bool is_gnss_visible_ = false; //!< Flag for GNSS satellite is visible or not + size_t visible_satellite_number_ = 0; //!< Number of visible GNSS satellites + std::vector gnss_information_list_; //!< Information List of visible GNSS satellites // References const Dynamics* dynamics_; //!< Dynamics of spacecraft @@ -201,10 +207,9 @@ class GnssReceiver : public Component, public ILoggable { /** * @fn AddNoise * @brief Substitutional method for "Measure" in other sensor models inherited Sensor class - * @param [in] position_true_i_m: True position of the spacecraft in the ECI frame [m] * @param [in] position_true_ecef_m: True position of the spacecraft in the ECEF frame [m] */ - void AddNoise(const libra::Vector<3> position_true_i_m, const libra::Vector<3> position_true_ecef_m); + void AddNoise(const libra::Vector<3> position_true_ecef_m, const libra::Vector<3> velocity_true_ecef_m_s); /** * @fn ConvertJulianDayToGPSTime * @brief Convert Julian day to GPS time From 45e63fd05e748354e08359d809710b7239671ae4 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 09:34:27 +0100 Subject: [PATCH 28/61] Fix small --- src/components/real/aocs/gnss_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index f390121e2..d69847c90 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -57,10 +57,10 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, void GnssReceiver::MainRoutine(const int time_count) { UNUSED(time_count); - libra::Vector<3> position_true_eci_ = dynamics_->GetOrbit().GetPosition_i_m(); + libra::Vector<3> position_true_eci = dynamics_->GetOrbit().GetPosition_i_m(); libra::Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); - CheckAntenna(position_true_eci_, quaternion_i2b); + CheckAntenna(position_true_eci, quaternion_i2b); if (is_gnss_visible_) { // Antenna of GNSS-R can detect GNSS signal From 075b8d223bb7c333851edcab2ce161c25f8a6a5f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 09:36:37 +0100 Subject: [PATCH 29/61] Fix comments --- src/components/real/aocs/gnss_receiver.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index d69847c90..31ab576c2 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -57,9 +57,10 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, void GnssReceiver::MainRoutine(const int time_count) { UNUSED(time_count); + // Antenna checking + // TODO: Use ECEF position only libra::Vector<3> position_true_eci = dynamics_->GetOrbit().GetPosition_i_m(); libra::Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b(); - CheckAntenna(position_true_eci, quaternion_i2b); if (is_gnss_visible_) { @@ -67,12 +68,10 @@ void GnssReceiver::MainRoutine(const int time_count) { position_ecef_m_ = dynamics_->GetOrbit().GetPosition_ecef_m(); velocity_ecef_m_s_ = dynamics_->GetOrbit().GetVelocity_ecef_m_s(); AddNoise(position_ecef_m_, velocity_ecef_m_s_); + // Convert observed value to other frames geodetic_position_.UpdateFromEcef(position_ecef_m_); - } else { - // position information will not be updated in this case - utc_ = simulation_time_->GetCurrentUtc(); - ConvertJulianDayToGpsTime(simulation_time_->GetCurrentTime_jd()); } + // Time is updated with internal clock utc_ = simulation_time_->GetCurrentUtc(); ConvertJulianDayToGpsTime(simulation_time_->GetCurrentTime_jd()); From c7eeacbd33a9f253c6274a2a5b50038f92261340 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 09:43:55 +0100 Subject: [PATCH 30/61] Remove ECI observation information --- src/components/real/aocs/gnss_receiver.cpp | 4 ++-- src/components/real/aocs/gnss_receiver.hpp | 15 ++------------- 2 files changed, 4 insertions(+), 15 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 31ab576c2..56155a0ad 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -207,7 +207,7 @@ std::string GnssReceiver::GetLogHeader() const // For logs str_tmp += WriteScalar(sensor_name + "measured_utc_time_hour"); str_tmp += WriteScalar(sensor_name + "measured_utc_time_min"); str_tmp += WriteScalar(sensor_name + "measured_utc_time_sec"); - str_tmp += WriteVector(sensor_name + "measured_position", "eci", "m", 3); + str_tmp += WriteVector(sensor_name + "measured_position", "ecef", "m", 3); str_tmp += WriteVector(sensor_name + "measured_velocity", "ecef", "m/s", 3); str_tmp += WriteScalar(sensor_name + "measured_latitude", "rad"); str_tmp += WriteScalar(sensor_name + "measured_longitude", "rad"); @@ -227,7 +227,7 @@ std::string GnssReceiver::GetLogValue() const // For logs str_tmp += WriteScalar(utc_.hour); str_tmp += WriteScalar(utc_.minute); str_tmp += WriteScalar(utc_.second); - str_tmp += WriteVector(position_eci_m_, 10); + str_tmp += WriteVector(position_ecef_m_, 10); str_tmp += WriteVector(velocity_ecef_m_s_, 10); str_tmp += WriteScalar(geodetic_position_.GetLatitude_rad(), 10); str_tmp += WriteScalar(geodetic_position_.GetLongitude_rad(), 10); diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index c8ebba3fb..6cd9b5db9 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -101,11 +101,6 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] channel: Channel number */ inline const GnssInfo GetGnssInfo(const size_t channel) const { return gnss_information_list_[channel]; }; - /** - * @fn GetMeasuredPosition_eci_m - * @brief Return Observed position in the ECI frame [m] - */ - inline const libra::Vector<3> GetMeasuredPosition_eci_m(void) const { return position_eci_m_; } /** * @fn GetMeasuredPosition_ecef_m * @brief Return Observed position in the ECEF frame [m] @@ -116,11 +111,6 @@ class GnssReceiver : public Component, public ILoggable { * @brief Return Observed position in the LLH frame [m] */ inline const GeodeticPosition GetMeasuredGeodeticPosition(void) const { return geodetic_position_; } - /** - * @fn GetMeasuredVelocity_eci_m_s - * @brief Return Observed velocity in the ECI frame [m/s] - */ - inline const libra::Vector<3> GetMeasuredVelocity_eci_m_s(void) const { return velocity_eci_m_s_; } /** * @fn GetMeasuredVelocity_ecef_m_s * @brief Return Observed velocity in the ECEF frame [m/s] @@ -154,8 +144,6 @@ class GnssReceiver : public Component, public ILoggable { libra::NormalRand position_random_noise_ecef_m_[3]; libra::NormalRand velocity_random_noise_ecef_m_s_[3]; - libra::Vector<3> position_eci_m_{0.0}; //!< Observed position in the ECI frame [m] - libra::Vector<3> velocity_eci_m_s_{0.0}; //!< Observed velocity in the ECI frame [m/s] libra::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] libra::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame @@ -208,10 +196,11 @@ class GnssReceiver : public Component, public ILoggable { * @fn AddNoise * @brief Substitutional method for "Measure" in other sensor models inherited Sensor class * @param [in] position_true_ecef_m: True position of the spacecraft in the ECEF frame [m] + * @param [in] velocity_true_ecef_m_s: True velocity of the spacecraft in the ECEF frame [m/s] */ void AddNoise(const libra::Vector<3> position_true_ecef_m, const libra::Vector<3> velocity_true_ecef_m_s); /** - * @fn ConvertJulianDayToGPSTime + * @fn ConvertJulianDayToGpsTime * @brief Convert Julian day to GPS time * @param [in] julian_day: Julian day */ From 2bee223fc784a52fec57500f423c31c9288e1ca1 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 09:49:24 +0100 Subject: [PATCH 31/61] Add comment --- src/components/real/aocs/gnss_receiver.hpp | 23 ++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 6cd9b5db9..a9dac1963 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -140,16 +140,19 @@ class GnssReceiver : public Component, public ILoggable { std::string gnss_system_id_; //!< GNSS satellite number defined by GNSS system AntennaModel antenna_model_; //!< Antenna model - // Calculated values - libra::NormalRand position_random_noise_ecef_m_[3]; - libra::NormalRand velocity_random_noise_ecef_m_s_[3]; - - libra::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] - libra::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] - GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame - UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] - unsigned int gps_time_week_ = 0; //!< Observed GPS time week part - double gps_time_s_ = 0.0; //!< Observed GPS time second part + // Simple position observation + libra::NormalRand position_random_noise_ecef_m_[3]; //!< Random noise for position at the ECEF frame [m] + libra::NormalRand velocity_random_noise_ecef_m_s_[3]; //!< Random noise for velocity at the ECEF frame [m] + libra::Vector<3> position_ecef_m_{0.0}; //!< Observed position in the ECEF frame [m] + libra::Vector<3> velocity_ecef_m_s_{0.0}; //!< Observed velocity in the ECEF frame [m/s] + GeodeticPosition geodetic_position_; //!< Observed position in the geodetic frame + + // Time observation + UTC utc_ = {2000, 1, 1, 0, 0, 0.0}; //!< Observed time in UTC [year, month, day, hour, min, sec] + unsigned int gps_time_week_ = 0; //!< Observed GPS time week part + double gps_time_s_ = 0.0; //!< Observed GPS time second part + + // Satellite visibility bool is_gnss_visible_ = false; //!< Flag for GNSS satellite is visible or not size_t visible_satellite_number_ = 0; //!< Number of visible GNSS satellites std::vector gnss_information_list_; //!< Information List of visible GNSS satellites From 9e4f68dab64d1eb1e8379a21bbaa5704b3753956 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 10:04:15 +0100 Subject: [PATCH 32/61] Fix comment --- data/sample/initialize_files/components/gnss_receiver.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/data/sample/initialize_files/components/gnss_receiver.ini b/data/sample/initialize_files/components/gnss_receiver.ini index 2b7ce54bd..dc15ce622 100644 --- a/data/sample/initialize_files/components/gnss_receiver.ini +++ b/data/sample/initialize_files/components/gnss_receiver.ini @@ -34,7 +34,7 @@ antenna_half_width_deg = 60 // Note: GNSS satellite calculation must includes the target GNSS systems gnss_system_id = G -// Random noise [m] +// Random noise for simple position observation white_noise_standard_deviation_position_ecef_m(0) = 2000.0 white_noise_standard_deviation_position_ecef_m(1) = 1000.0 white_noise_standard_deviation_position_ecef_m(2) = 1500.0 From d45cf2c5f69def0f3f2bfee79b3273704a3c66a2 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 10:21:30 +0100 Subject: [PATCH 33/61] Fix GNSS visibility threhold to 4 --- src/components/real/aocs/gnss_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 56155a0ad..e49d85368 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -155,7 +155,7 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, } } - if (visible_satellite_number_ > 0) { + if (visible_satellite_number_ >= 4) { is_gnss_visible_ = true; } else { is_gnss_visible_ = false; From 53bcd76643e2acaab7c7b7cbd8f8ee71f16062bc Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 11:03:07 +0100 Subject: [PATCH 34/61] Modify to use GNSS satellite calculated number --- src/components/real/aocs/gnss_receiver.cpp | 4 +++- src/environment/global/gnss_satellites.hpp | 6 ++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index e49d85368..fafd98610 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -120,7 +120,9 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, // initialize visible_satellite_number_ = 0; - for (size_t i = 0; i < kTotalNumberOfGnssSatellite; i++) { + size_t number_of_calculated_gnss_satellites = gnss_satellites_->GetNumberOfCalculatedSatellite(); + + for (size_t i = 0; i < number_of_calculated_gnss_satellites; i++) { // check if gnss ID is compatible with the receiver std::string gnss_system_id_string = ConvertIndexToGnssSatelliteNumber(i); if (gnss_system_id_.find(gnss_system_id_string[0]) == std::string::npos) continue; diff --git a/src/environment/global/gnss_satellites.hpp b/src/environment/global/gnss_satellites.hpp index 1d0dce6e1..643afe52f 100644 --- a/src/environment/global/gnss_satellites.hpp +++ b/src/environment/global/gnss_satellites.hpp @@ -62,6 +62,12 @@ class GnssSatellites : public ILoggable { */ inline bool IsCalcEnabled() const { return is_calc_enabled_; } + /** + * @fn GetNumberOfCalculatedSatellite + * @brief Return number of calculated satellite + */ + inline size_t GetNumberOfCalculatedSatellite() const { return number_of_calculated_gnss_satellites_; } + /** * @fn Update * @brief Update both GNSS satellite information From 5357837e7671435a86021aa90a276589034814ed Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 11:13:44 +0100 Subject: [PATCH 35/61] Delete GNSS system ID because it should be defined in GnssSatellites --- .../components/gnss_receiver.ini | 11 ------- src/components/real/aocs/gnss_receiver.cpp | 33 +++++++------------ src/components/real/aocs/gnss_receiver.hpp | 20 +++++------ 3 files changed, 20 insertions(+), 44 deletions(-) diff --git a/data/sample/initialize_files/components/gnss_receiver.ini b/data/sample/initialize_files/components/gnss_receiver.ini index dc15ce622..b277b8e93 100644 --- a/data/sample/initialize_files/components/gnss_receiver.ini +++ b/data/sample/initialize_files/components/gnss_receiver.ini @@ -23,17 +23,6 @@ antenna_model = SIMPLE // Antenna half width [deg] antenna_half_width_deg = 60 -// Target GNSS system ID -// G...GPS -// R...GLONASS -// E...Galileo -// C...Beidou -// J...QZSS -// if your receiver is compatible with all kind of gnss satellites : GRECJ -// if your receiver is compatible with GPS and QZSS : GJ -// Note: GNSS satellite calculation must includes the target GNSS systems -gnss_system_id = G - // Random noise for simple position observation white_noise_standard_deviation_position_ecef_m(0) = 2000.0 white_noise_standard_deviation_position_ecef_m(1) = 1000.0 diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index fafd98610..08eeb78f6 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -11,9 +11,9 @@ #include #include -GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_system_id, - const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, - const double half_width_deg, const libra::Vector<3> position_noise_standard_deviation_ecef_m, +GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, + const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_deg, + const libra::Vector<3> position_noise_standard_deviation_ecef_m, const libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator), @@ -21,7 +21,6 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, antenna_position_b_m_(antenna_position_b_m), quaternion_b2c_(quaternion_b2c), half_width_deg_(half_width_deg), - gnss_system_id_(gnss_system_id), antenna_model_(antenna_model), dynamics_(dynamics), gnss_satellites_(gnss_satellites), @@ -33,9 +32,8 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, } GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const std::string gnss_system_id, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, - const libra::Quaternion quaternion_b2c, const double half_width_deg, - const libra::Vector<3> position_noise_standard_deviation_ecef_m, + const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, + const double half_width_deg, const libra::Vector<3> position_noise_standard_deviation_ecef_m, const libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) : Component(prescaler, clock_generator, power_port), @@ -43,7 +41,6 @@ GnssReceiver::GnssReceiver(const int prescaler, ClockGenerator* clock_generator, antenna_position_b_m_(antenna_position_b_m), quaternion_b2c_(quaternion_b2c), half_width_deg_(half_width_deg), - gnss_system_id_(gnss_system_id), antenna_model_(antenna_model), dynamics_(dynamics), gnss_satellites_(gnss_satellites), @@ -123,10 +120,6 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, size_t number_of_calculated_gnss_satellites = gnss_satellites_->GetNumberOfCalculatedSatellite(); for (size_t i = 0; i < number_of_calculated_gnss_satellites; i++) { - // check if gnss ID is compatible with the receiver - std::string gnss_system_id_string = ConvertIndexToGnssSatelliteNumber(i); - if (gnss_system_id_.find(gnss_system_id_string[0]) == std::string::npos) continue; - // compute direction from sat to gnss in body-fixed frame libra::Vector<3> gnss_satellite_position_i_m = gnss_satellites_->GetPosition_eci_m(i); libra::Vector<3> antenna_to_gnss_satellite_i_m = gnss_satellite_position_i_m - antenna_position_i_m; @@ -153,7 +146,7 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, if (inner2 > cos(half_width_deg_ * libra::deg_to_rad) && is_satellite_visible) { // is visible visible_satellite_number_++; - SetGnssInfo(antenna_to_gnss_satellite_i_m, quaternion_i2b, gnss_system_id_string); + SetGnssInfo(antenna_to_gnss_satellite_i_m, quaternion_i2b, i); } } @@ -165,7 +158,7 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, } void GnssReceiver::SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, const libra::Quaternion quaternion_i2b, - const std::string gnss_system_id) { + const std::size_t gnss_system_id) { libra::Vector<3> antenna_to_satellite_direction_b = quaternion_i2b.FrameConversion(antenna_to_satellite_i_m); libra::Vector<3> antenna_to_satellite_direction_c = quaternion_b2c_.FrameConversion(antenna_to_satellite_direction_b); @@ -258,7 +251,6 @@ typedef struct _gnss_receiver_param { libra::Vector<3> antenna_pos_b; libra::Quaternion quaternion_b2c; double half_width_deg; - std::string gnss_system_id; libra::Vector<3> position_noise_standard_deviation_ecef_m; libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s; } GnssReceiverParam; @@ -287,7 +279,6 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSat gnssr_conf.ReadVector(GSection, "antenna_position_b_m", gnss_receiver_param.antenna_pos_b); gnssr_conf.ReadQuaternion(GSection, "quaternion_b2c", gnss_receiver_param.quaternion_b2c); gnss_receiver_param.half_width_deg = gnssr_conf.ReadDouble(GSection, "antenna_half_width_deg"); - gnss_receiver_param.gnss_system_id = gnssr_conf.ReadString(GSection, "gnss_system_id"); gnssr_conf.ReadVector(GSection, "white_noise_standard_deviation_position_ecef_m", gnss_receiver_param.position_noise_standard_deviation_ecef_m); gnssr_conf.ReadVector(GSection, "white_noise_standard_deviation_velocity_ecef_m_s", gnss_receiver_param.velocity_noise_standard_deviation_ecef_m_s); @@ -298,9 +289,9 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, const size_t comp const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time) { GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id); - GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.gnss_system_id, gr_param.antenna_model, gr_param.antenna_pos_b, - gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.position_noise_standard_deviation_ecef_m, - gr_param.velocity_noise_standard_deviation_ecef_m_s, dynamics, gnss_satellites, simulation_time); + GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.antenna_model, gr_param.antenna_pos_b, gr_param.quaternion_b2c, + gr_param.half_width_deg, gr_param.position_noise_standard_deviation_ecef_m, gr_param.velocity_noise_standard_deviation_ecef_m_s, + dynamics, gnss_satellites, simulation_time); return gnss_r; } @@ -311,8 +302,8 @@ GnssReceiver InitGnssReceiver(ClockGenerator* clock_generator, PowerPort* power_ // PowerPort power_port->InitializeWithInitializeFile(file_name); - GnssReceiver gnss_r(gr_param.prescaler, clock_generator, power_port, component_id, gr_param.gnss_system_id, gr_param.antenna_model, - gr_param.antenna_pos_b, gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.position_noise_standard_deviation_ecef_m, + GnssReceiver gnss_r(gr_param.prescaler, clock_generator, power_port, component_id, gr_param.antenna_model, gr_param.antenna_pos_b, + gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.position_noise_standard_deviation_ecef_m, gr_param.velocity_noise_standard_deviation_ecef_m_s, dynamics, gnss_satellites, simulation_time); return gnss_r; } diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index a9dac1963..04026e89f 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/src/components/real/aocs/gnss_receiver.hpp @@ -30,7 +30,7 @@ enum class AntennaModel { * @brief Information of GNSS satellites */ typedef struct _gnss_info { - std::string id; //!< ID of GNSS satellites + size_t gnss_id; //!< ID of GNSS satellites double latitude_rad; //!< Latitude on the antenna frame [rad] double longitude_rad; //!< Longitude on the antenna frame [rad] double distance_m; //!< Distance between the GNSS satellite and the GNSS receiver antenna [m] @@ -48,7 +48,6 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] prescaler: Frequency scale factor for update * @param [in] clock_generator: Clock generator * @param [in] component_id: Component ID - * @param [in] gnss_system_id: Target GNSS system IDs * @param [in] antenna_model: Antenna model * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] * @param [in] quaternion_b2c: Quaternion from body frame to component frame (antenna frame) @@ -59,18 +58,16 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] gnss_satellites: GNSS Satellites information * @param [in] simulation_time: Simulation time information */ - GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const std::string gnss_system_id, - const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, - const double half_width_deg, const libra::Vector<3> position_noise_standard_deviation_ecef_m, - const libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, - const SimulationTime* simulation_time); + GnssReceiver(const int prescaler, ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model, + const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, const double half_width_deg, + const libra::Vector<3> position_noise_standard_deviation_ecef_m, const libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s, + const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); /** * @fn GnssReceiver * @brief Constructor with power port * @param [in] prescaler: Frequency scale factor for update * @param [in] clock_generator: Clock generator * @param [in] power_port: Power port - * @param [in] gnss_system_id: Target GNSS system IDs * @param [in] antenna_model: Antenna model * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m] * @param [in] quaternion_b2c: Quaternion from body frame to component frame (antenna frame) @@ -82,8 +79,8 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] simulation_time: Simulation time information */ GnssReceiver(const int prescaler, ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id, - const std::string gnss_system_id, const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, - const libra::Quaternion quaternion_b2c, const double half_width_deg, const libra::Vector<3> position_noise_standard_deviation_ecef_m, + const AntennaModel antenna_model, const libra::Vector<3> antenna_position_b_m, const libra::Quaternion quaternion_b2c, + const double half_width_deg, const libra::Vector<3> position_noise_standard_deviation_ecef_m, const libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time); @@ -137,7 +134,6 @@ class GnssReceiver : public Component, public ILoggable { libra::Vector<3> antenna_position_b_m_; //!< GNSS antenna position at the body-fixed frame [m] libra::Quaternion quaternion_b2c_; //!< Quaternion from body frame to component frame (antenna frame) double half_width_deg_ = 0.0; //!< Half width of the antenna cone model [deg] - std::string gnss_system_id_; //!< GNSS satellite number defined by GNSS system AntennaModel antenna_model_; //!< Antenna model // Simple position observation @@ -194,7 +190,7 @@ class GnssReceiver : public Component, public ILoggable { * @param [in] quaternion_i2b: True attitude of the spacecraft expressed by quaternion from the inertial frame to the body-fixed frame * @param [in] gnss_system_id: ID of target GNSS satellite */ - void SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, const libra::Quaternion quaternion_i2b, const std::string gnss_system_id); + void SetGnssInfo(const libra::Vector<3> antenna_to_satellite_i_m, const libra::Quaternion quaternion_i2b, const size_t gnss_system_id); /** * @fn AddNoise * @brief Substitutional method for "Measure" in other sensor models inherited Sensor class From 7d470890211b0efca36863845c2c2e98d6caedb0 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 11:17:23 +0100 Subject: [PATCH 36/61] Add comment --- data/sample/initialize_files/components/gnss_receiver.ini | 1 + 1 file changed, 1 insertion(+) diff --git a/data/sample/initialize_files/components/gnss_receiver.ini b/data/sample/initialize_files/components/gnss_receiver.ini index b277b8e93..a558caaa0 100644 --- a/data/sample/initialize_files/components/gnss_receiver.ini +++ b/data/sample/initialize_files/components/gnss_receiver.ini @@ -18,6 +18,7 @@ quaternion_b2c(3) = 1.0 // CONE: We calculate the number of GNSS satellites in the antenna, // and the position is observable when more than 4 satellites are in the antenna. // Note : We need to enable the GNSS satellite calculation when we use this mode. +// All satellites managed in the GnssSatellite class are used in this model. antenna_model = SIMPLE // Antenna half width [deg] From 7249e7afc06b9a3a976f3d24b7a03f30eafd7a57 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 11:29:40 +0100 Subject: [PATCH 37/61] Fix python plot for GnssReceiver --- scripts/Plot/plot_gnss_receiver.py | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/scripts/Plot/plot_gnss_receiver.py b/scripts/Plot/plot_gnss_receiver.py index d650c18b3..aa489015a 100644 --- a/scripts/Plot/plot_gnss_receiver.py +++ b/scripts/Plot/plot_gnss_receiver.py @@ -49,39 +49,39 @@ # Read S2E CSV time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]') -measured_position_eci_m = read_3d_vector_from_csv(read_file_name, 'gnss_receiver1_measured_position_eci', 'm') -true_position_eci_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_i', 'm') +measured_position_ecef_m = read_3d_vector_from_csv(read_file_name, 'gnss_receiver1_measured_position_ecef', 'm') +# true_position_ecef_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_ecef', 'm') number_of_visible_satellites = read_scalar_from_csv(read_file_name, 'gnss_receiver1_number_of_visible_satellites') satellite_visible_flag = read_scalar_from_csv(read_file_name, 'gnss_receiver1_satellite_visible_flag') # Statistics -error_m = measured_position_eci_m[:, 1:] - true_position_eci_m[:, 1:] +#error_m = measured_position_ecef_m[:, 1:] - true_position_ecef_m[:, 1:] average = [0.0, 0.0, 0.0] standard_deviation = [0.0, 0.0, 0.0] -for i in range(3): - average[i] = error_m[i].mean() - standard_deviation[i] = error_m[i].std() +#for i in range(3): +# average[i] = error_m[i].mean() +# standard_deviation[i] = error_m[i].std() # # Plot # unit = ' m' fig, axis = plt.subplots(5, 1, squeeze = False, tight_layout = True, sharex = True) -axis[0, 0].plot(time[0], measured_position_eci_m[0], marker=".", c="red", label="MEASURED-X") -axis[0, 0].plot(time[0], true_position_eci_m[0], marker=".", c="orange", label="TRUE-X") +axis[0, 0].plot(time[0], measured_position_ecef_m[0], marker=".", c="red", label="MEASURED-X") +#axis[0, 0].plot(time[0], true_position_ecef_m[0], marker=".", c="orange", label="TRUE-X") axis[0, 0].text(0.01, 0.99, "Error average:" + format(average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) axis[0, 0].legend(loc = 'upper right') -axis[1, 0].plot(time[0], measured_position_eci_m[1], marker=".", c="green", label="MEASURED-Y") -axis[1, 0].plot(time[0], true_position_eci_m[1], marker=".", c="yellow", label="TRUE-Y") +axis[1, 0].plot(time[0], measured_position_ecef_m[1], marker=".", c="green", label="MEASURED-Y") +#axis[1, 0].plot(time[0], true_position_ecef_m[1], marker=".", c="yellow", label="TRUE-Y") axis[1, 0].text(0.01, 0.99, "Error average:" + format(average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) axis[1, 0].legend(loc = 'upper right') -axis[2, 0].plot(time[0], measured_position_eci_m[2], marker=".", c="blue", label="MEASURED-Z") -axis[2, 0].plot(time[0], true_position_eci_m[2], marker=".", c="purple", label="TRUE-Z") +axis[2, 0].plot(time[0], measured_position_ecef_m[2], marker=".", c="blue", label="MEASURED-Z") +#axis[2, 0].plot(time[0], true_position_ecef_m[2], marker=".", c="purple", label="TRUE-Z") axis[2, 0].text(0.01, 0.99, "Error average:" + format(average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) axis[2, 0].legend(loc = 'upper right') @@ -94,7 +94,7 @@ axis[4, 0].legend(loc = 'upper right') axis[4, 0].set_ylim(0, max(number_of_visible_satellites[0]) + 2) -fig.suptitle("GNSS Receiver Spacecraft position @ ECI") +fig.suptitle("GNSS Receiver Spacecraft position @ ECEF") fig.supylabel("Position [m]") fig.supxlabel("Time [s]") From e285179ca83a90a174075dcc5a977808850a6a2a Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:07:12 +0100 Subject: [PATCH 38/61] Fix visible satellite detection method for cone antenna model --- src/components/real/aocs/gnss_receiver.cpp | 42 ++++++++++++---------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 08eeb78f6..23cc3c099 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -106,12 +106,13 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, // Cone model gnss_information_list_.clear(); - // antenna normal vector at inertial frame - libra::Vector<3> antenna_direction_c(0.0); - antenna_direction_c[2] = 1.0; - libra::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c); - libra::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b); + // Antenna pointing direction vector at inertial frame + libra::Vector<3> antenna_pointing_direction_c(0.0); + antenna_pointing_direction_c[2] = 1.0; + libra::Vector<3> antenna_pointing_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_pointing_direction_c); + libra::Vector<3> antenna_pointing_direction_i = quaternion_i2b.InverseFrameConversion(antenna_pointing_direction_b); + // Antenna position vector at inertial frame libra::Vector<3> antenna_position_i_m = position_true_eci_m + quaternion_i2b.InverseFrameConversion(antenna_position_b_m_); // initialize @@ -125,25 +126,28 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m, libra::Vector<3> antenna_to_gnss_satellite_i_m = gnss_satellite_position_i_m - antenna_position_i_m; libra::Vector<3> antenna_to_gnss_satellite_direction_i = antenna_to_gnss_satellite_i_m.CalcNormalizedVector(); - // check gnss satellites are visible from antenna + // Check GNSS satellites are visible from the receiver(not care antenna direction) + bool is_gnss_satellite_visible_from_receiver = false; double inner1 = InnerProduct(antenna_position_i_m, gnss_satellite_position_i_m); - bool is_satellite_visible = false; - if (inner1 > 0) { - is_satellite_visible = true; - } else { - Vector<3> tmp = - antenna_position_i_m + InnerProduct(-antenna_position_i_m, antenna_to_gnss_satellite_direction_i) * antenna_to_gnss_satellite_i_m; - if (tmp.CalcNorm() < environment::earth_equatorial_radius_m) { - // There is earth between antenna and gnss - is_satellite_visible = false; + if (inner1 > 0.0) { // GNSS satellite and receiver are in the same hemisphere + is_gnss_satellite_visible_from_receiver = true; + } else { // GNSS satellite is in the another hemisphere + double angle_bw_earth_center_and_edge_rad = asin(environment::earth_equatorial_radius_m / antenna_position_i_m.CalcNorm()); + double angle_bw_earth_center_and_gnss_rad = + acos(InnerProduct(-antenna_position_i_m.CalcNormalizedVector(), antenna_to_gnss_satellite_direction_i)); + + if (angle_bw_earth_center_and_edge_rad < angle_bw_earth_center_and_gnss_rad) { + // There is no Earth between receiver and GNSS satellite + is_gnss_satellite_visible_from_receiver = true; } else { - // There is not earth between antenna and gnss - is_satellite_visible = true; + // There is Earth between receiver and GNSS satellite + is_gnss_satellite_visible_from_receiver = false; } } - double inner2 = InnerProduct(antenna_direction_i, antenna_to_gnss_satellite_direction_i); - if (inner2 > cos(half_width_deg_ * libra::deg_to_rad) && is_satellite_visible) { + // Check GNSS satellites are in the antenna half width angle + double inner2 = InnerProduct(antenna_pointing_direction_i, antenna_to_gnss_satellite_direction_i); + if (inner2 > cos(half_width_deg_ * libra::deg_to_rad) && is_gnss_satellite_visible_from_receiver) { // is visible visible_satellite_number_++; SetGnssInfo(antenna_to_gnss_satellite_i_m, quaternion_i2b, i); From df0fe71903935d8779cb4c7140a29a06b0b18ed9 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:09:37 +0100 Subject: [PATCH 39/61] Add ECEF position log --- src/dynamics/orbit/orbit.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/dynamics/orbit/orbit.cpp b/src/dynamics/orbit/orbit.cpp index c73223fd8..80a03d6a0 100644 --- a/src/dynamics/orbit/orbit.cpp +++ b/src/dynamics/orbit/orbit.cpp @@ -68,6 +68,7 @@ std::string Orbit::GetLogHeader() const { std::string str_tmp = ""; str_tmp += WriteVector("spacecraft_position", "i", "m", 3); + str_tmp += WriteVector("spacecraft_position", "ecef", "m", 3); str_tmp += WriteVector("spacecraft_velocity", "i", "m/s", 3); str_tmp += WriteVector("spacecraft_velocity", "b", "m/s", 3); str_tmp += WriteVector("spacecraft_acceleration", "i", "m/s2", 3); @@ -82,6 +83,7 @@ std::string Orbit::GetLogValue() const { std::string str_tmp = ""; str_tmp += WriteVector(spacecraft_position_i_m_, 16); + str_tmp += WriteVector(spacecraft_position_ecef_m_, 16); str_tmp += WriteVector(spacecraft_velocity_i_m_s_, 10); str_tmp += WriteVector(spacecraft_velocity_b_m_s_, 10); str_tmp += WriteVector(spacecraft_acceleration_i_m_s2_, 10); From 463c032b9f8b10f246ed9ba0261fdd05545d5f0d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:09:57 +0100 Subject: [PATCH 40/61] Uncomment ECEF position log read in plot gnss receiver --- scripts/Plot/plot_gnss_receiver.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/scripts/Plot/plot_gnss_receiver.py b/scripts/Plot/plot_gnss_receiver.py index aa489015a..430a17f3c 100644 --- a/scripts/Plot/plot_gnss_receiver.py +++ b/scripts/Plot/plot_gnss_receiver.py @@ -50,18 +50,18 @@ time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]') measured_position_ecef_m = read_3d_vector_from_csv(read_file_name, 'gnss_receiver1_measured_position_ecef', 'm') -# true_position_ecef_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_ecef', 'm') +true_position_ecef_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_ecef', 'm') number_of_visible_satellites = read_scalar_from_csv(read_file_name, 'gnss_receiver1_number_of_visible_satellites') satellite_visible_flag = read_scalar_from_csv(read_file_name, 'gnss_receiver1_satellite_visible_flag') # Statistics -#error_m = measured_position_ecef_m[:, 1:] - true_position_ecef_m[:, 1:] +error_m = measured_position_ecef_m[:, 1:] - true_position_ecef_m[:, 1:] average = [0.0, 0.0, 0.0] standard_deviation = [0.0, 0.0, 0.0] -#for i in range(3): -# average[i] = error_m[i].mean() -# standard_deviation[i] = error_m[i].std() +for i in range(3): + average[i] = error_m[i].mean() + standard_deviation[i] = error_m[i].std() # # Plot @@ -69,19 +69,19 @@ unit = ' m' fig, axis = plt.subplots(5, 1, squeeze = False, tight_layout = True, sharex = True) axis[0, 0].plot(time[0], measured_position_ecef_m[0], marker=".", c="red", label="MEASURED-X") -#axis[0, 0].plot(time[0], true_position_ecef_m[0], marker=".", c="orange", label="TRUE-X") +axis[0, 0].plot(time[0], true_position_ecef_m[0], marker=".", c="orange", label="TRUE-X") axis[0, 0].text(0.01, 0.99, "Error average:" + format(average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) axis[0, 0].legend(loc = 'upper right') axis[1, 0].plot(time[0], measured_position_ecef_m[1], marker=".", c="green", label="MEASURED-Y") -#axis[1, 0].plot(time[0], true_position_ecef_m[1], marker=".", c="yellow", label="TRUE-Y") +axis[1, 0].plot(time[0], true_position_ecef_m[1], marker=".", c="yellow", label="TRUE-Y") axis[1, 0].text(0.01, 0.99, "Error average:" + format(average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) axis[1, 0].legend(loc = 'upper right') axis[2, 0].plot(time[0], measured_position_ecef_m[2], marker=".", c="blue", label="MEASURED-Z") -#axis[2, 0].plot(time[0], true_position_ecef_m[2], marker=".", c="purple", label="TRUE-Z") +axis[2, 0].plot(time[0], true_position_ecef_m[2], marker=".", c="purple", label="TRUE-Z") axis[2, 0].text(0.01, 0.99, "Error average:" + format(average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) axis[2, 0].legend(loc = 'upper right') From f08f45bb5d19ccacb47cddb47d801e5b0bea8a68 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:13:37 +0100 Subject: [PATCH 41/61] Fix warning --- src/components/real/aocs/gnss_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index 23cc3c099..f182764f4 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/src/components/real/aocs/gnss_receiver.cpp @@ -259,7 +259,7 @@ typedef struct _gnss_receiver_param { libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s; } GnssReceiverParam; -GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const int component_id) { +GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const size_t component_id) { GnssReceiverParam gnss_receiver_param; IniAccess gnssr_conf(file_name); From 310493986e692b5529d9dc7a8dc91a1fc703240e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 3 Nov 2023 20:39:39 +0100 Subject: [PATCH 42/61] Add orbit observer --- src/components/ideal/orbit_observer.hpp | 78 +++++++++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 src/components/ideal/orbit_observer.hpp diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp new file mode 100644 index 000000000..97b9216e0 --- /dev/null +++ b/src/components/ideal/orbit_observer.hpp @@ -0,0 +1,78 @@ +/* + * @file orbit_observer.hpp + * @brief Ideal component which can observe orbit + */ + +#ifndef S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ +#define S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ + +#include +#include +#include +#include + +#include "../base/component.hpp" + +/* + * @class OrbitObserver + * @brief Ideal component which can observe orbit + */ +class OrbitObserver : public Component, public ILoggable { + public: + /** + * @fn OrbitObserver + * @brief Constructor without power port + * @param [in] prescaler: Frequency scale factor for update + * @param [in] clock_generator: Clock generator + * @param [in] orbit: Orbit information + */ + OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit* orbit); + + /** + * @fn ~AttitudeObserver + * @brief Destructor + */ + ~OrbitObserver() {} + + // Override functions for Component + /** + * @fn MainRoutine + * @brief Main routine for sensor observation + */ + void MainRoutine(const int time_count) override; + + // Override ILoggable + /** + * @fn GetLogHeader + * @brief Override GetLogHeader function of ILoggable + */ + virtual std::string GetLogHeader() const override; + /** + * @fn GetLogValue + * @brief Override GetLogValue function of ILoggable + */ + virtual std::string GetLogValue() const override; + + /** + * @fn GetPosition_i_m + * @brief Return observed quaternion from the inertial frame to the component frame + */ + inline const libra::Vector<3> GetPosition() const { return observed_position_i_m_; }; + + protected: + libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] + + // Observed variables + const Orbit* orbit_; //!< Orbit information +}; + +/** + * @fn InitStarSensor + * @brief Initialize functions for StarSensor without power port + * @param [in] clock_generator: Clock generator + * @param [in] file_name: Path to the initialize file + * @param [in] orbit: Orbit information + */ +OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit* orbit); + +#endif // S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ From d174ef74b4635d5f3c5090e450eb5728dd12a818 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 4 Nov 2023 08:25:25 +0100 Subject: [PATCH 43/61] Add cpp file --- src/components/ideal/orbit_observer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 src/components/ideal/orbit_observer.cpp diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp new file mode 100644 index 000000000..ec6b4bec7 --- /dev/null +++ b/src/components/ideal/orbit_observer.cpp @@ -0,0 +1,8 @@ +/* + * @file orbit_observer.cpp + * @brief Ideal component which can observe orbit + */ + +#include "orbit_observer.hpp" + +OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit* orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} From 3fc4c0abfb5acc6d3b47a289521a0d93cc72ecf1 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 4 Nov 2023 14:22:58 +0100 Subject: [PATCH 44/61] Implement main routine without noise --- src/components/ideal/orbit_observer.cpp | 38 ++++++++++++++++++++++++- src/components/ideal/orbit_observer.hpp | 4 +-- 2 files changed, 39 insertions(+), 3 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index ec6b4bec7..6d1171b89 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -5,4 +5,40 @@ #include "orbit_observer.hpp" -OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit* orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} +OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} + +void OrbitObserver::MainRoutine(const int time_count) { + UNUSED(time_count); + + observed_position_i_m_ = orbit_.GetPosition_i_m(); +} + +std::string OrbitObserver::GetLogHeader() const { + std::string str_tmp = ""; + + std::string head = "orbit_observer_"; + str_tmp += WriteVector(head + "position", "i", "m", 3); + + return str_tmp; +} + +std::string OrbitObserver::GetLogValue() const { + std::string str_tmp = ""; + + str_tmp += WriteVector(observed_position_i_m_, 16); + + return str_tmp; +} + +OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { + // General + IniAccess ini_file(file_name); + + // CompoBase + int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); + if (prescaler <= 1) prescaler = 1; + + OrbitObserver orbit_observer(prescaler, clock_generator, orbit); + + return orbit_observer; +} diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 97b9216e0..c495a06d1 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -26,7 +26,7 @@ class OrbitObserver : public Component, public ILoggable { * @param [in] clock_generator: Clock generator * @param [in] orbit: Orbit information */ - OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit* orbit); + OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit); /** * @fn ~AttitudeObserver @@ -73,6 +73,6 @@ class OrbitObserver : public Component, public ILoggable { * @param [in] file_name: Path to the initialize file * @param [in] orbit: Orbit information */ -OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit* orbit); +OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit); #endif // S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ From 4b38b432ba779117adb7531648955ce18363553c Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 4 Nov 2023 14:25:57 +0100 Subject: [PATCH 45/61] dd to CMake --- src/components/CMakeLists.txt | 1 + src/components/ideal/orbit_observer.cpp | 4 +++- src/components/ideal/orbit_observer.hpp | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/components/CMakeLists.txt b/src/components/CMakeLists.txt index 697b2322f..2cc69c93d 100644 --- a/src/components/CMakeLists.txt +++ b/src/components/CMakeLists.txt @@ -37,6 +37,7 @@ ideal/force_generator.cpp ideal/torque_generator.cpp ideal/angular_velocity_observer.cpp ideal/attitude_observer.cpp +ideal/orbit_observer.cpp real/mission/telescope.cpp diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 6d1171b89..f66e9802f 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -5,7 +5,9 @@ #include "orbit_observer.hpp" -OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} +#include + +OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index c495a06d1..5990e5f89 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -63,7 +63,7 @@ class OrbitObserver : public Component, public ILoggable { libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] // Observed variables - const Orbit* orbit_; //!< Orbit information + const Orbit& orbit_; //!< Orbit information }; /** From 6975859ba4dbbf40aa8d950cce9e6be2da7a2289 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 4 Nov 2023 14:36:02 +0100 Subject: [PATCH 46/61] Add velocity --- src/components/ideal/orbit_observer.cpp | 6 +++++- src/components/ideal/orbit_observer.hpp | 3 ++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index f66e9802f..38cdeed6c 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -7,12 +7,14 @@ #include -OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} +OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit) + : Component(prescaler, clock_generator), orbit_(orbit) {} void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); observed_position_i_m_ = orbit_.GetPosition_i_m(); + observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s(); } std::string OrbitObserver::GetLogHeader() const { @@ -20,6 +22,7 @@ std::string OrbitObserver::GetLogHeader() const { std::string head = "orbit_observer_"; str_tmp += WriteVector(head + "position", "i", "m", 3); + str_tmp += WriteVector(head + "velocity", "i", "m/s", 3); return str_tmp; } @@ -28,6 +31,7 @@ std::string OrbitObserver::GetLogValue() const { std::string str_tmp = ""; str_tmp += WriteVector(observed_position_i_m_, 16); + str_tmp += WriteVector(observed_velocity_i_m_s_, 16); return str_tmp; } diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 5990e5f89..78bfcfdd3 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -60,7 +60,8 @@ class OrbitObserver : public Component, public ILoggable { inline const libra::Vector<3> GetPosition() const { return observed_position_i_m_; }; protected: - libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] + libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] + libra::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] // Observed variables const Orbit& orbit_; //!< Orbit information From fa55700acae12e78d464371ef5bdae5da22e94fe Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 4 Nov 2023 14:36:46 +0100 Subject: [PATCH 47/61] Add velocity --- src/components/ideal/orbit_observer.hpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 78bfcfdd3..e2bbba1ef 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -55,9 +55,15 @@ class OrbitObserver : public Component, public ILoggable { /** * @fn GetPosition_i_m - * @brief Return observed quaternion from the inertial frame to the component frame + * @brief Return observed position */ - inline const libra::Vector<3> GetPosition() const { return observed_position_i_m_; }; + inline const libra::Vector<3> GetPosition_i_m() const { return observed_position_i_m_; }; + + /** + * @fn GetVelocity_i_m_s + * @brief Return observed velocity + */ + inline const libra::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; }; protected: libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] From b8019e5d3ad93902a2d3e93208a377195d7ef522 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 7 Nov 2023 08:09:20 +0100 Subject: [PATCH 48/61] Add noise --- src/components/ideal/orbit_observer.cpp | 26 +++++++++++++++++++++++-- src/components/ideal/orbit_observer.hpp | 26 ++++++++++++++++++++++--- 2 files changed, 47 insertions(+), 5 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 38cdeed6c..d065ebac4 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -7,12 +7,32 @@ #include -OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit) - : Component(prescaler, clock_generator), orbit_(orbit) {} +OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, + const libra::Vector<6> error_standard_deviation, const const Orbit& orbit) + : Component(prescaler, clock_generator), orbit_(orbit) { + for (size_t i = 0; i < 6; i++) { + normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i]); + } +} void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); + switch (error_frame_) { + case ErrorFrame::kInertial: + observed_position_i_m_ = Measure(observed_position_i_m_); + // Frame conversion + observed_position_rtn_m_ = q_i2rtn.FrameConversion(observed_position_i_m_); + break; + case ErrorFrame::kRtn: + observed_position_rtn_m_ = Measure(observed_position_rtn_m_); + // Frame conversion + observed_position_i_m_ = q_i2rtn.InverseFrameConversion(observed_position_rtn_m_); + break; + default: + break; + } + observed_position_i_m_ = orbit_.GetPosition_i_m(); observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s(); } @@ -36,6 +56,8 @@ std::string OrbitObserver::GetLogValue() const { return str_tmp; } +void AddNoise(){} + OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { // General IniAccess ini_file(file_name); diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index e2bbba1ef..2271ae271 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -13,6 +13,15 @@ #include "../base/component.hpp" +/** + * @enum ErrorFrame + * @brief Error definition frame + */ +enum class ErrorFrame { + kInertial, //!< Inertial frame + kRtn, //!< RTN frame +}; + /* * @class OrbitObserver * @brief Ideal component which can observe orbit @@ -24,9 +33,12 @@ class OrbitObserver : public Component, public ILoggable { * @brief Constructor without power port * @param [in] prescaler: Frequency scale factor for update * @param [in] clock_generator: Clock generator + * @param [in] error_frame: Error frame definition + * @param [in] error_standard_deviation: Position and Velocity standard deviation noise [m, m/s] * @param [in] orbit: Orbit information */ - OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit); + OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation, + const const Orbit& orbit); /** * @fn ~AttitudeObserver @@ -66,11 +78,19 @@ class OrbitObserver : public Component, public ILoggable { inline const libra::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; }; protected: - libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] - libra::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] + libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] + libra::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] + libra::Vector<3> observed_position_rtn_m_{0.0}; //!< Observed position @ RTN frame [m] + libra::Vector<3> observed_velocity_rtn_m_s_{0.0}; //!< Observed velocity @ RTN frame [m/s] + + ErrorFrame error_frame_; //!< Error definition frame + libra::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] // Observed variables const Orbit& orbit_; //!< Orbit information + + libra::Vector<3> AddPositionNoise(const libra::Vector<3> position_m); + libra::Vector<3> AddVelocityNoise(const libra::Vector<3> velocity_m_s); }; /** From 5f00035ca7b64eb168f01b2f46630384af1b505c Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:23:34 +0100 Subject: [PATCH 49/61] Fix format --- src/components/ideal/orbit_observer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index d065ebac4..ba12a9b2f 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -56,7 +56,7 @@ std::string OrbitObserver::GetLogValue() const { return str_tmp; } -void AddNoise(){} +void AddNoise() {} OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { // General From 475d4c42a9cfc8f461eecc25dce9654d90e31003 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:33:36 +0100 Subject: [PATCH 50/61] Fix for build success --- src/components/ideal/orbit_observer.cpp | 18 ++++++++++-------- src/components/ideal/orbit_observer.hpp | 2 +- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index ba12a9b2f..053941c49 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -8,7 +8,7 @@ #include OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, - const libra::Vector<6> error_standard_deviation, const const Orbit& orbit) + const libra::Vector<6> error_standard_deviation, const Orbit& orbit) : Component(prescaler, clock_generator), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i]); @@ -18,23 +18,21 @@ OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generato void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); + libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); switch (error_frame_) { case ErrorFrame::kInertial: - observed_position_i_m_ = Measure(observed_position_i_m_); + observed_position_i_m_ = AddPositionNoise(orbit_.GetPosition_i_m()); // Frame conversion observed_position_rtn_m_ = q_i2rtn.FrameConversion(observed_position_i_m_); break; case ErrorFrame::kRtn: - observed_position_rtn_m_ = Measure(observed_position_rtn_m_); - // Frame conversion + // observed_position_rtn_m_ = AddPositionNoise(orbit_.GetPosition_); + // Frame conversion observed_position_i_m_ = q_i2rtn.InverseFrameConversion(observed_position_rtn_m_); break; default: break; } - - observed_position_i_m_ = orbit_.GetPosition_i_m(); - observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s(); } std::string OrbitObserver::GetLogHeader() const { @@ -66,7 +64,11 @@ OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); if (prescaler <= 1) prescaler = 1; - OrbitObserver orbit_observer(prescaler, clock_generator, orbit); + // Noise + const ErrorFrame error_frame = ErrorFrame::kInertial; + const libra::Vector<6> error_standard_deviation; + + OrbitObserver orbit_observer(prescaler, clock_generator, error_frame, error_standard_deviation, orbit); return orbit_observer; } diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 2271ae271..9f0d79314 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -38,7 +38,7 @@ class OrbitObserver : public Component, public ILoggable { * @param [in] orbit: Orbit information */ OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation, - const const Orbit& orbit); + const Orbit& orbit); /** * @fn ~AttitudeObserver From d9e76b007c807aa96b417ebcf77351f926af43ac Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:46:10 +0100 Subject: [PATCH 51/61] Fix noise calculation --- src/components/ideal/orbit_observer.cpp | 23 +++++++++++++++-------- src/components/ideal/orbit_observer.hpp | 10 ++++------ 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 053941c49..4ee50551b 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -6,33 +6,42 @@ #include "orbit_observer.hpp" #include +#include OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation, const Orbit& orbit) : Component(prescaler, clock_generator), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { - normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i]); + normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed()); } } void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); + // Calc noise + libra::Vector<3> position_error_i_m{0.0}; + libra::Vector<3> position_error_rtn_m{0.0}; libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); switch (error_frame_) { case ErrorFrame::kInertial: - observed_position_i_m_ = AddPositionNoise(orbit_.GetPosition_i_m()); - // Frame conversion - observed_position_rtn_m_ = q_i2rtn.FrameConversion(observed_position_i_m_); + for (size_t axis = 0; axis < 3; axis++) { + position_error_i_m[axis] = normal_random_noise_[axis]; + } break; case ErrorFrame::kRtn: - // observed_position_rtn_m_ = AddPositionNoise(orbit_.GetPosition_); + for (size_t axis = 0; axis < 3; axis++) { + position_error_rtn_m[axis] = normal_random_noise_[axis]; + } // Frame conversion - observed_position_i_m_ = q_i2rtn.InverseFrameConversion(observed_position_rtn_m_); + position_error_i_m = q_i2rtn.InverseFrameConversion(position_error_rtn_m); break; default: break; } + + // Get observed value + observed_position_i_m_ = orbit_.GetPosition_i_m() + position_error_i_m; } std::string OrbitObserver::GetLogHeader() const { @@ -54,8 +63,6 @@ std::string OrbitObserver::GetLogValue() const { return str_tmp; } -void AddNoise() {} - OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { // General IniAccess ini_file(file_name); diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 9f0d79314..0481263e2 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -78,10 +78,8 @@ class OrbitObserver : public Component, public ILoggable { inline const libra::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; }; protected: - libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] - libra::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] - libra::Vector<3> observed_position_rtn_m_{0.0}; //!< Observed position @ RTN frame [m] - libra::Vector<3> observed_velocity_rtn_m_s_{0.0}; //!< Observed velocity @ RTN frame [m/s] + libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] + libra::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] ErrorFrame error_frame_; //!< Error definition frame libra::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] @@ -89,8 +87,8 @@ class OrbitObserver : public Component, public ILoggable { // Observed variables const Orbit& orbit_; //!< Orbit information - libra::Vector<3> AddPositionNoise(const libra::Vector<3> position_m); - libra::Vector<3> AddVelocityNoise(const libra::Vector<3> velocity_m_s); + libra::Vector<3> CalcPositionNoise(); + libra::Vector<3> CalcVelocityNoise(); }; /** From 000c5c1260fc45591246f47c27f6d2875f3a3cf0 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:48:09 +0100 Subject: [PATCH 52/61] Fix small --- src/components/ideal/orbit_observer.cpp | 2 +- src/components/ideal/orbit_observer.hpp | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 4ee50551b..1b7283eca 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -10,7 +10,7 @@ OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation, const Orbit& orbit) - : Component(prescaler, clock_generator), orbit_(orbit) { + : Component(prescaler, clock_generator), error_frame_(error_frame), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed()); } diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 0481263e2..03963afe9 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -86,9 +86,6 @@ class OrbitObserver : public Component, public ILoggable { // Observed variables const Orbit& orbit_; //!< Orbit information - - libra::Vector<3> CalcPositionNoise(); - libra::Vector<3> CalcVelocityNoise(); }; /** From fa9d18d406e4a77d37049779cd60b76f3946fbe3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 16:00:38 +0100 Subject: [PATCH 53/61] Add initialize file --- .../components/orbit_observer.ini | 19 ++++++++++++ src/components/ideal/orbit_observer.cpp | 29 ++++++++++++++----- src/components/ideal/orbit_observer.hpp | 20 +++++++++---- 3 files changed, 54 insertions(+), 14 deletions(-) create mode 100644 data/sample/initialize_files/components/orbit_observer.ini diff --git a/data/sample/initialize_files/components/orbit_observer.ini b/data/sample/initialize_files/components/orbit_observer.ini new file mode 100644 index 000000000..8d527e618 --- /dev/null +++ b/data/sample/initialize_files/components/orbit_observer.ini @@ -0,0 +1,19 @@ +[ORBIT_OBSERVER] +// Noise definition frame +// INERTIAL: Inertial frame +// RTN: RTN frame +noise_frame = INERTIAL + +// Standard deviation of position and velocity noise [m, m/s] +// The frame definition can be selected above +noise_standard_deviation[0] = 1000 // Position-X +noise_standard_deviation[1] = 2000 // Position-Y +noise_standard_deviation[2] = 3000 // Position-Z +noise_standard_deviation[3] = 30 // Velocity-X +noise_standard_deviation[4] = 20 // Velocity-Y +noise_standard_deviation[5] = 10 // Velocity-Z + + +[COMPONENT_BASE] +// Prescaler with respect to the component update period +prescaler = 1 diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 1b7283eca..26d816933 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -8,9 +8,9 @@ #include #include -OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, +OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, const libra::Vector<6> error_standard_deviation, const Orbit& orbit) - : Component(prescaler, clock_generator), error_frame_(error_frame), orbit_(orbit) { + : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed()); } @@ -23,13 +23,13 @@ void OrbitObserver::MainRoutine(const int time_count) { libra::Vector<3> position_error_i_m{0.0}; libra::Vector<3> position_error_rtn_m{0.0}; libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); - switch (error_frame_) { - case ErrorFrame::kInertial: + switch (noise_frame_) { + case NoiseFrame::kInertial: for (size_t axis = 0; axis < 3; axis++) { position_error_i_m[axis] = normal_random_noise_[axis]; } break; - case ErrorFrame::kRtn: + case NoiseFrame::kRtn: for (size_t axis = 0; axis < 3; axis++) { position_error_rtn_m[axis] = normal_random_noise_[axis]; } @@ -63,6 +63,18 @@ std::string OrbitObserver::GetLogValue() const { return str_tmp; } +NoiseFrame SetNoiseFrame(const std::string noise_frame) { + if (noise_frame == "INERTIAL") { + return NoiseFrame::kInertial; + } else if (noise_frame == "RTN") { + return NoiseFrame::kRtn; + } else { + std::cerr << "[WARNINGS] Orbit observer noise frame is not defined!" << std::endl; + std::cerr << "The noise frame is automatically initialized as INERTIAL" << std::endl; + return NoiseFrame::kInertial; + } +} + OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { // General IniAccess ini_file(file_name); @@ -72,10 +84,11 @@ OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std if (prescaler <= 1) prescaler = 1; // Noise - const ErrorFrame error_frame = ErrorFrame::kInertial; - const libra::Vector<6> error_standard_deviation; + const NoiseFrame noise_frame = SetNoiseFrame(ini_file.ReadString("ORBIT_OBSERVER", "noise_frame")); + libra::Vector<6> noise_standard_deviation; + ini_file.ReadVector("ORBIT_OBSERVER", "noise_standard_deviation", noise_standard_deviation); - OrbitObserver orbit_observer(prescaler, clock_generator, error_frame, error_standard_deviation, orbit); + OrbitObserver orbit_observer(prescaler, clock_generator, noise_frame, noise_standard_deviation, orbit); return orbit_observer; } diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 03963afe9..df22c807d 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -14,10 +14,10 @@ #include "../base/component.hpp" /** - * @enum ErrorFrame - * @brief Error definition frame + * @enum NoiseFrame + * @brief Noide definition frame */ -enum class ErrorFrame { +enum class NoiseFrame { kInertial, //!< Inertial frame kRtn, //!< RTN frame }; @@ -33,11 +33,11 @@ class OrbitObserver : public Component, public ILoggable { * @brief Constructor without power port * @param [in] prescaler: Frequency scale factor for update * @param [in] clock_generator: Clock generator - * @param [in] error_frame: Error frame definition + * @param [in] noise_frame: Error frame definition * @param [in] error_standard_deviation: Position and Velocity standard deviation noise [m, m/s] * @param [in] orbit: Orbit information */ - OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation, + OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, const libra::Vector<6> error_standard_deviation, const Orbit& orbit); /** @@ -81,13 +81,21 @@ class OrbitObserver : public Component, public ILoggable { libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] libra::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] - ErrorFrame error_frame_; //!< Error definition frame + NoiseFrame noise_frame_; //!< Noise definition frame libra::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] // Observed variables const Orbit& orbit_; //!< Orbit information }; +/** + * @fn SetNoiseFrame + * @brief Set NoiseFrame by string + * @param [in] noise_frame: Noise frame name + * @return noise frame + */ +NoiseFrame SetNoiseFrame(const std::string noise_frame); + /** * @fn InitStarSensor * @brief Initialize functions for StarSensor without power port From 3c6a1fec8820df7f2d7b92ca7b684da7780fcd05 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 16:20:23 +0100 Subject: [PATCH 54/61] Add velocity observation --- src/components/ideal/orbit_observer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 26d816933..07416e891 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -22,19 +22,26 @@ void OrbitObserver::MainRoutine(const int time_count) { // Calc noise libra::Vector<3> position_error_i_m{0.0}; libra::Vector<3> position_error_rtn_m{0.0}; + libra::Vector<3> velocity_error_i_m_s{0.0}; + libra::Vector<3> velocity_error_rtn_m_s{0.0}; libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); switch (noise_frame_) { case NoiseFrame::kInertial: for (size_t axis = 0; axis < 3; axis++) { position_error_i_m[axis] = normal_random_noise_[axis]; + velocity_error_i_m_s[axis] = normal_random_noise_[axis + 3]; } break; case NoiseFrame::kRtn: for (size_t axis = 0; axis < 3; axis++) { position_error_rtn_m[axis] = normal_random_noise_[axis]; + velocity_error_rtn_m_s[axis] = normal_random_noise_[axis + 3]; } // Frame conversion position_error_i_m = q_i2rtn.InverseFrameConversion(position_error_rtn_m); + // For zero bias noise, we do not need to care frame rotation effect. + velocity_error_i_m_s = q_i2rtn.InverseFrameConversion(velocity_error_rtn_m_s); + break; default: break; @@ -42,6 +49,7 @@ void OrbitObserver::MainRoutine(const int time_count) { // Get observed value observed_position_i_m_ = orbit_.GetPosition_i_m() + position_error_i_m; + observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s() + velocity_error_i_m_s; } std::string OrbitObserver::GetLogHeader() const { From ae87d688acdaebce336648f6c1acd906aebfbbc2 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 9 Feb 2024 10:31:22 +0100 Subject: [PATCH 55/61] Fix typo --- src/components/ideal/orbit_observer.cpp | 4 ++-- src/components/ideal/orbit_observer.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 07416e891..61fe2b002 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -37,9 +37,9 @@ void OrbitObserver::MainRoutine(const int time_count) { position_error_rtn_m[axis] = normal_random_noise_[axis]; velocity_error_rtn_m_s[axis] = normal_random_noise_[axis + 3]; } - // Frame conversion + // Frame conversion position_error_i_m = q_i2rtn.InverseFrameConversion(position_error_rtn_m); - // For zero bias noise, we do not need to care frame rotation effect. + // For zero bias noise, we do not need to care the frame rotation effect. velocity_error_i_m_s = q_i2rtn.InverseFrameConversion(velocity_error_rtn_m_s); break; diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index df22c807d..490489957 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -15,7 +15,7 @@ /** * @enum NoiseFrame - * @brief Noide definition frame + * @brief Noise definition frame */ enum class NoiseFrame { kInertial, //!< Inertial frame From 9180f53eb11f5694cfac21ccceb8a825dea8ed45 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 9 Feb 2024 10:53:26 +0100 Subject: [PATCH 56/61] Add instance to sample codes --- .../initialize_files/components/orbit_observer.ini | 12 ++++++------ data/sample/initialize_files/sample_satellite.ini | 1 + .../spacecraft/sample_components.cpp | 6 ++++++ .../spacecraft/sample_components.hpp | 2 ++ 4 files changed, 15 insertions(+), 6 deletions(-) diff --git a/data/sample/initialize_files/components/orbit_observer.ini b/data/sample/initialize_files/components/orbit_observer.ini index 8d527e618..1c27ecf57 100644 --- a/data/sample/initialize_files/components/orbit_observer.ini +++ b/data/sample/initialize_files/components/orbit_observer.ini @@ -6,12 +6,12 @@ noise_frame = INERTIAL // Standard deviation of position and velocity noise [m, m/s] // The frame definition can be selected above -noise_standard_deviation[0] = 1000 // Position-X -noise_standard_deviation[1] = 2000 // Position-Y -noise_standard_deviation[2] = 3000 // Position-Z -noise_standard_deviation[3] = 30 // Velocity-X -noise_standard_deviation[4] = 20 // Velocity-Y -noise_standard_deviation[5] = 10 // Velocity-Z +noise_standard_deviation(0) = 1000 // Position-X +noise_standard_deviation(1) = 2000 // Position-Y +noise_standard_deviation(2) = 3000 // Position-Z +noise_standard_deviation(3) = 30 // Velocity-X +noise_standard_deviation(4) = 20 // Velocity-Y +noise_standard_deviation(5) = 10 // Velocity-Z [COMPONENT_BASE] diff --git a/data/sample/initialize_files/sample_satellite.ini b/data/sample/initialize_files/sample_satellite.ini index 69dcd5210..d0fa9c23a 100644 --- a/data/sample/initialize_files/sample_satellite.ini +++ b/data/sample/initialize_files/sample_satellite.ini @@ -146,6 +146,7 @@ force_generator_file = INI_FILE_DIR_FROM_EXE/components/force_generator.ini torque_generator_file = INI_FILE_DIR_FROM_EXE/components/torque_generator.ini angular_velocity_observer_file = INI_FILE_DIR_FROM_EXE/components/angular_velocity_observer.ini attitude_observer_file = INI_FILE_DIR_FROM_EXE/components/attitude_observer.ini +orbit_observer_file = INI_FILE_DIR_FROM_EXE/components/orbit_observer.ini antenna_file = INI_FILE_DIR_FROM_EXE/components/spacecraft_antenna.ini component_interference_file = INI_FILE_DIR_FROM_EXE/components/component_interference.ini telescope_file = INI_FILE_DIR_FROM_EXE/components/telescope.ini diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index d529d040a..1d997625f 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -106,6 +106,11 @@ SampleComponents::SampleComponents(const Dynamics* dynamics, Structure* structur configuration_->main_logger_->CopyFileToLogDirectory(file_name); attitude_observer_ = new AttitudeObserver(InitializeAttitudeObserver(clock_generator, file_name, dynamics_->GetAttitude())); + // Orbit Observer + file_name = iniAccess.ReadString("COMPONENT_FILES", "orbit_observer_file"); + configuration_->main_logger_->CopyFileToLogDirectory(file_name); + orbit_observer_ = new OrbitObserver(InitializeOrbitObserver(clock_generator, file_name, dynamics_->GetOrbit())); + // Antenna file_name = iniAccess.ReadString("COMPONENT_FILES", "antenna_file"); configuration_->main_logger_->CopyFileToLogDirectory(file_name); @@ -171,6 +176,7 @@ SampleComponents::~SampleComponents() { delete torque_generator_; delete angular_velocity_observer_; delete attitude_observer_; + delete orbit_observer_; delete antenna_; delete mtq_magnetometer_interference_; // delete change_structure_; diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index 2b7574f27..abc355bef 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -114,6 +115,7 @@ class SampleComponents : public InstalledComponents { TorqueGenerator* torque_generator_; //!< Ideal Torque Generator AngularVelocityObserver* angular_velocity_observer_; //!< Ideal Angular velocity observer AttitudeObserver* attitude_observer_; //!< Ideal Attitude observer + OrbitObserver* orbit_observer_; //!< Ideal Orbit observer // Mission Telescope* telescope_; //!< Telescope From cc4e1cfe499b9f9bfa65a3fdf07a34cab8fcae8d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 9 Feb 2024 11:00:24 +0100 Subject: [PATCH 57/61] Add plot script for orbit observer --- scripts/Plot/plot_orbit_observer.py | 126 ++++++++++++++++++++++++++++ 1 file changed, 126 insertions(+) create mode 100644 scripts/Plot/plot_orbit_observer.py diff --git a/scripts/Plot/plot_orbit_observer.py b/scripts/Plot/plot_orbit_observer.py new file mode 100644 index 000000000..b84c02830 --- /dev/null +++ b/scripts/Plot/plot_orbit_observer.py @@ -0,0 +1,126 @@ +# +# Plot orbit observer +# +# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946 +# + +# +# Import +# +# plots +import matplotlib.pyplot as plt +# numpy +import numpy as np +# local function +from common import find_latest_log_tag +from common import add_log_file_arguments +from common import read_3d_vector_from_csv +from common import read_scalar_from_csv +# arguments +import argparse + +# Arguments +aparser = argparse.ArgumentParser() +aparser = add_log_file_arguments(aparser) +aparser.add_argument('--no-gui', action='store_true') +args = aparser.parse_args() + +# +# Read Arguments +# +# log file path +path_to_logs = args.logs_dir + +read_file_tag = args.file_tag +if read_file_tag == None: + print("file tag does not found. use latest.") + read_file_tag = find_latest_log_tag(path_to_logs) + +print("log: " + read_file_tag) + +# +# CSV file name +# +read_file_name = path_to_logs + '/' + 'logs_' + read_file_tag + '/' + read_file_tag + '_default.csv' + +# +# Data read and edit +# +# Read S2E CSV +time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]') + +measured_position_eci_m = read_3d_vector_from_csv(read_file_name, 'orbit_observer_position_i', 'm') +true_position_eci_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_i', 'm') + +measured_velocity_eci_m_s = read_3d_vector_from_csv(read_file_name, 'orbit_observer_velocity_i', 'm/s') +true_velocity_eci_m_s = read_3d_vector_from_csv(read_file_name, 'spacecraft_velocity_i', 'm/s') + +# Statistics +position_error_m = measured_position_eci_m[:, 1:] - true_position_eci_m[:, 1:] +position_average = [0.0, 0.0, 0.0] +position_standard_deviation = [0.0, 0.0, 0.0] +velocity_error_m = measured_velocity_eci_m_s[:, 1:] - true_velocity_eci_m_s[:, 1:] +velocity_average = [0.0, 0.0, 0.0] +velocity_standard_deviation = [0.0, 0.0, 0.0] +for i in range(3): + position_average[i] = position_error_m[i].mean() + position_standard_deviation[i] = position_error_m[i].std() + velocity_average[i] = velocity_error_m[i].mean() + velocity_standard_deviation[i] = velocity_error_m[i].std() + +# +# Plot +# +unit = ' m' +fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True) +axis[0, 0].plot(time[0], measured_position_eci_m[0], marker=".", c="red", label="MEASURED-X") +axis[0, 0].plot(time[0], true_position_eci_m[0], marker=".", c="orange", label="TRUE-X") +axis[0, 0].text(0.01, 0.99, "Error average:" + format(position_average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].legend(loc = 'upper right') + +axis[1, 0].plot(time[0], measured_position_eci_m[1], marker=".", c="green", label="MEASURED-Y") +axis[1, 0].plot(time[0], true_position_eci_m[1], marker=".", c="yellow", label="TRUE-Y") +axis[1, 0].text(0.01, 0.99, "Error average:" + format(position_average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].legend(loc = 'upper right') + +axis[2, 0].plot(time[0], measured_position_eci_m[2], marker=".", c="blue", label="MEASURED-Z") +axis[2, 0].plot(time[0], true_position_eci_m[2], marker=".", c="purple", label="TRUE-Z") +axis[2, 0].text(0.01, 0.99, "Error average:" + format(position_average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].legend(loc = 'upper right') + +fig.suptitle("Orbit observer position results @ ECI") +fig.supylabel("Position [m]") +fig.supxlabel("Time [s]") + +unit = ' m/s' +fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True) +axis[0, 0].plot(time[0], measured_velocity_eci_m_s[0], marker=".", c="red", label="MEASURED-X") +axis[0, 0].plot(time[0], true_velocity_eci_m_s[0], marker=".", c="orange", label="TRUE-X") +axis[0, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].legend(loc = 'upper right') + +axis[1, 0].plot(time[0], measured_velocity_eci_m_s[1], marker=".", c="green", label="MEASURED-Y") +axis[1, 0].plot(time[0], true_velocity_eci_m_s[1], marker=".", c="yellow", label="TRUE-Y") +axis[1, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].legend(loc = 'upper right') + +axis[2, 0].plot(time[0], measured_velocity_eci_m_s[2], marker=".", c="blue", label="MEASURED-Z") +axis[2, 0].plot(time[0], true_velocity_eci_m_s[2], marker=".", c="purple", label="TRUE-Z") +axis[2, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].legend(loc = 'upper right') + +fig.suptitle("Orbit observer velocity results @ ECI") +fig.supylabel("Velocity [m/s]") +fig.supxlabel("Time [s]") + +# Data save +if args.no_gui: + plt.savefig(read_file_tag + "_orbit_observer.png") # save last figure only +else: + plt.show() From e9dcc23fc192d9a794043216eb435d117b2c4912 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 9 Feb 2024 11:01:08 +0100 Subject: [PATCH 58/61] Add log output for orbit observer --- src/simulation_sample/spacecraft/sample_components.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index 1d997625f..ffe4023a5 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -221,4 +221,5 @@ void SampleComponents::LogSetup(Logger& logger) { logger.AddLogList(torque_generator_); logger.AddLogList(angular_velocity_observer_); logger.AddLogList(attitude_observer_); + logger.AddLogList(orbit_observer_); } From 44c9351ae9bce0e02c8bf5c6076ad420ba31e992 Mon Sep 17 00:00:00 2001 From: sksat Date: Tue, 20 Feb 2024 17:21:59 +0900 Subject: [PATCH 59/61] make C2A command sender optional --- CMakeLists.txt | 1 + src/components/CMakeLists.txt | 8 +++++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 76b16fcea..3439fd0be 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,7 @@ project(S2E # build config option(USE_HILS "Use HILS" OFF) option(USE_C2A "Use C2A" OFF) +option(USE_C2A_COMMAND_SENDER "Use command sender to C2A" OFF) option(BUILD_64BIT "Build 64bit" OFF) option(GOOGLE_TEST "Execute GoogleTest" OFF) diff --git a/src/components/CMakeLists.txt b/src/components/CMakeLists.txt index 697b2322f..d52c72293 100644 --- a/src/components/CMakeLists.txt +++ b/src/components/CMakeLists.txt @@ -25,7 +25,6 @@ real/cdh/on_board_computer_with_c2a.cpp real/communication/antenna.cpp real/communication/antenna_radiation_pattern.cpp real/communication/ground_station_calculator.cpp -real/communication/wings_command_sender_to_c2a.cpp examples/example_change_structure.cpp examples/example_serial_communication_with_obc.cpp @@ -62,6 +61,13 @@ if(USE_HILS) ) endif() +if(USE_C2A_COMMAND_SENDER) + set(SOURCE_FILES + ${SOURCE_FILES} + real/communication/wings_command_sender.cpp + ) +endif() + add_library(${PROJECT_NAME} STATIC ${SOURCE_FILES} ) From 79d1cb322d68eca580c51c7088603154b7df7e53 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 7 Mar 2024 23:02:44 +0900 Subject: [PATCH 60/61] Patch update v7.2.5 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3439fd0be..823808347 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 3.13) project(S2E LANGUAGES CXX DESCRIPTION "S2E: Spacecraft Simulation Environment" - VERSION 7.2.4 + VERSION 7.2.5 ) # build config From 3c142b2174011e5b464e018254adb3104ee17cf7 Mon Sep 17 00:00:00 2001 From: "renovate[bot]" <29139614+renovate[bot]@users.noreply.github.com> Date: Tue, 12 Mar 2024 07:43:13 +0000 Subject: [PATCH 61/61] Update dependency numpy-quaternion to v2023.0.3 --- scripts/Plot/Pipfile | 2 +- scripts/Plot/Pipfile.lock | 208 ++++++++++++++++++++------------------ 2 files changed, 109 insertions(+), 101 deletions(-) diff --git a/scripts/Plot/Pipfile b/scripts/Plot/Pipfile index 0b9e89182..6ac2b122e 100644 --- a/scripts/Plot/Pipfile +++ b/scripts/Plot/Pipfile @@ -9,7 +9,7 @@ matplotlib = "==3.7.2" numpy = "==1.24.3" pandas = "==2.0.3" numpy-stl = "==3.0.1" -numpy-quaternion = "==2023.0.2" +numpy-quaternion = "==2023.0.3" python-utils = "==3.5.2" [dev-packages] diff --git a/scripts/Plot/Pipfile.lock b/scripts/Plot/Pipfile.lock index ecc9b0a59..e34e59e44 100644 --- a/scripts/Plot/Pipfile.lock +++ b/scripts/Plot/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "df7f65ba3ec6206a2c239ec2a9ee9fffc46478ac37e3cd2a6a8e05cf83dcf3fc" + "sha256": "f1e9adad28dde1efbdd7ba4126b803234663495b72d241fef74b5c6e54627fd7" }, "pipfile-spec": 6, "requires": { @@ -20,23 +20,30 @@ "hashes": [ "sha256:1cb70fe82ea9f49ee6df2ecafeade1e9ef4d5654a3e5c2b6f1b50bbfcfada33a", "sha256:20644986a63d57c9d94afb8aba180d5362c1bd08d61d4537aa6221a70e3df762", + "sha256:22793cf731d6cc06e11cf2760fd2875cd18b2980ce1c5ba15f6ebd9e224b1a1d", "sha256:2985d8d937427a20868dc18cd677bd6a6b4b94efc1b33ab29706d4e0dac418cf", "sha256:2e198e442bae7ad0d25f529788fcb5802192d844d856fa966f6018a46648a375", "sha256:353b2d02bd70fd48b3d0e146a028862b7d9e09ffb7995c70151fb80607a2df25", "sha256:355c984cbde3f098cac49f41e2ab0bbbd49091770eeb58486a001f9ee865f725", + "sha256:3752eec899129a6645170fb1e7a16d0263f8ed2691f9f37916ecb19a2947ab72", "sha256:427a2050dcfbbde5bb4421f87b3f594852160029f8bbdf8d0fff81ad554de2f4", "sha256:4fb6763f4f2bb904fea0afbd8194f14af0b01fb52d719be35dd4b423e8d8dca2", "sha256:500a3d314166057d0b3c065e63147042c996ead1af5d59e319f813b83d4ea220", + "sha256:509bb451ce26f1f7651cc19d27ef049935304c7795a141cfbe05582802554226", + "sha256:51a5e8f2183e7505f4dfbb965048348c2f1e501283b13ad9950409c3da9655f6", "sha256:5c50fa14a71d115a7614b3a659f14d1e65dbf43dbcbedae0c901058860806f5b", "sha256:650e9b2170579193cb89bea2c328740fc13c0327b8ea55173490f760d0f04f8c", "sha256:669d89621f8c7899c061fcf37bc32df24ebeaf5eb0a02b9d9b9896ca6698f6de", "sha256:6822d3d08c11cdc443e3ed01d61d512e7cf62d4b40bbc1d235f0a898f2c54a7a", "sha256:6e0a13ba17ca011c55db44fab9210363154bc67482776d3b149a3ef1b4b01bad", "sha256:7b58e500413dbe655127bbe9da82c9d175eadd130ed66b6a9efea8d46b65881b", + "sha256:7f99f3da41fc3e6bfb726f0fbd3302939a3b03e353ccd2be5ebd7bc09b84c386", "sha256:84918a6e030adc9aedee92fe622adde8b54cc81051a6ce6723c04987081d92f4", "sha256:87091beba0e5f6cfc416d1847a582645b2447cd6bf1e8c4ceaabd98d3493ef41", "sha256:8818fbb5e1133f0955a62ea159a3792124fc06f9b37ea4427612f6def923b711", "sha256:91d03458343d73f6a5e4d42e79df59cf57eba699094b71ce7c57ec40c8de9f59", + "sha256:a6860ab68a461cd36b6eda5d3973bfa1e3a4fdeaf608e8dd4b324b08e8561eed", + "sha256:a8786f23e3f456ab402b8160efb7b8b1c3780d6b84154dfcfdcd3f0401ef3285", "sha256:ad169a9ca573752867b136b62ac70cc12ab0ce86e3c69d57e924eaa21b5f5ec7", "sha256:b196ae9446183deaaedfbd2e757d3edf23225584ce1a47b795cba96febd6acdf", "sha256:b64f000374a41bf19ac8f8b39cbd2a3b244c79673cd686c9eb507c19daef9e78", @@ -44,6 +51,7 @@ "sha256:be103ef8c2cb1e7363e3ab48ae633ec2643f0b8a9226d119ec8681096b81335c", "sha256:c4d80330f00728acfd88be78e0485fd688984db873853f9c75647ec78e653203", "sha256:c76d77ac21b6fb5008ca0283c6f86d24bf7ed6d0d18867edcfc534748cf348b5", + "sha256:c93beda915b0b68e74a8eec986313e4758a0caa1bd57fee8660362c82f78b26f", "sha256:d1600576b21a37883992d5bff0c4485e5468a14fe2df7616e08824103b21465d", "sha256:d273b628adb276200eb9b97f6175f9145b8a280d55d995869ec4bb3c561021c2", "sha256:d56d7ed588a5ca5d364b42c73c933258d394c10b3c4dc224e7d31bb41a6fbef5", @@ -144,59 +152,59 @@ }, "fonttools": { "hashes": [ - "sha256:0452fcbfbce752ba596737a7c5ec5cf76bc5f83847ce1781f4f90eab14ece252", - "sha256:0a2417547462e468edf35b32e3dd06a6215ac26aa6316b41e03b8eeaf9f079ea", - "sha256:0d2b01428f7da26f229a5656defc824427b741e454b4e210ad2b25ed6ea2aed4", - "sha256:0d533f89819f9b3ee2dbedf0fed3825c425850e32bdda24c558563c71be0064e", - "sha256:12ee86abca46193359ea69216b3a724e90c66ab05ab220d39e3fc068c1eb72ac", - "sha256:18b35fd1a850ed7233a99bbd6774485271756f717dac8b594958224b54118b61", - "sha256:292922dc356d7f11f5063b4111a8b719efb8faea92a2a88ed296408d449d8c2e", - "sha256:2eb4167bde04e172a93cf22c875d8b0cff76a2491f67f5eb069566215302d45d", - "sha256:3cdb9a92521b81bf717ebccf592bd0292e853244d84115bfb4db0c426de58348", - "sha256:4108b1d247953dd7c90ec8f457a2dec5fceb373485973cc852b14200118a51ee", - "sha256:4709c5bf123ba10eac210d2d5c9027d3f472591d9f1a04262122710fa3d23199", - "sha256:5057ade278e67923000041e2b195c9ea53e87f227690d499b6a4edd3702f7f01", - "sha256:56339ec557f0c342bddd7c175f5e41c45fc21282bee58a86bd9aa322bec715f2", - "sha256:578c00f93868f64a4102ecc5aa600a03b49162c654676c3fadc33de2ddb88a81", - "sha256:594206b31c95fcfa65f484385171fabb4ec69f7d2d7f56d27f17db26b7a31814", - "sha256:63c73b9dd56a94a3cbd2f90544b5fca83666948a9e03370888994143b8d7c070", - "sha256:63dc592a16cd08388d8c4c7502b59ac74190b23e16dfc863c69fe1ea74605b68", - "sha256:6978bade7b6c0335095bdd0bd97f8f3d590d2877b370f17e03e0865241694eb5", - "sha256:6f30e605c7565d0da6f0aec75a30ec372072d016957cd8fc4469721a36ea59b7", - "sha256:702ae93058c81f46461dc4b2c79f11d3c3d8fd7296eaf8f75b4ba5bbf813cd5f", - "sha256:8b8a45254218679c7f1127812761e7854ed5c8e34349aebf581e8c9204e7495a", - "sha256:902e9c4e9928301912f34a6638741b8ae0b64824112b42aaf240e06b735774b1", - "sha256:97f0a49fa6aa2d6205c6f72f4f98b74ef4b9bfdcb06fd78e6fe6c7af4989b63e", - "sha256:9b4ec6d42a7555f5ae35f3b805482f0aad0f1baeeef54859492ea3b782959d4a", - "sha256:9b58638d8a85e3a1b32ec0a91d9f8171a877b4b81c408d4cb3257d0dee63e092", - "sha256:a8c8b54bd1420c184a995f980f1a8076f87363e2bb24239ef8c171a369d85a31", - "sha256:aee76fd81a8571c68841d6ef0da750d5ff08ff2c5f025576473016f16ac3bcf7", - "sha256:b10633aafc5932995a391ec07eba5e79f52af0003a1735b2306b3dab8a056d48", - "sha256:bcd77f89fc1a6b18428e7a55dde8ef56dae95640293bfb8f4e929929eba5e2a2", - "sha256:bff5b38d0e76eb18e0b8abbf35d384e60b3371be92f7be36128ee3e67483b3ec", - "sha256:c900508c46274d32d308ae8e82335117f11aaee1f7d369ac16502c9a78930b0a", - "sha256:cad5cfd044ea2e306fda44482b3dd32ee47830fa82dfa4679374b41baa294f5f", - "sha256:cdfd7557d1bd294a200bd211aa665ca3b02998dcc18f8211a5532da5b8fad5c5", - "sha256:cf5a0cd974f85a80b74785db2d5c3c1fd6cc09a2ba3c837359b2b5da629ee1b0", - "sha256:d10979ef14a8beaaa32f613bb698743f7241d92f437a3b5e32356dfb9769c65d", - "sha256:d20588466367f05025bb1efdf4e5d498ca6d14bde07b6928b79199c588800f0a", - "sha256:d3260db55f1843e57115256e91247ad9f68cb02a434b51262fe0019e95a98738", - "sha256:df48798f9a4fc4c315ab46e17873436c8746f5df6eddd02fad91299b2af7af95", - "sha256:e3e33862fc5261d46d9aae3544acb36203b1a337d00bdb5d3753aae50dac860e", - "sha256:e740a7602c2bb71e1091269b5dbe89549749a8817dc294b34628ffd8b2bf7124", - "sha256:f40441437b039930428e04fb05ac3a132e77458fb57666c808d74a556779e784", - "sha256:f7449493886da6a17472004d3818cc050ba3f4a0aa03fb47972e4fa5578e6703" + "sha256:0404faea044577a01bb82d47a8fa4bc7a54067fa7e324785dd65d200d6dd1133", + "sha256:07bc5ea02bb7bc3aa40a1eb0481ce20e8d9b9642a9536cde0218290dd6085828", + "sha256:08877e355d3dde1c11973bb58d4acad1981e6d1140711230a4bfb40b2b937ccc", + "sha256:0af65c720520710cc01c293f9c70bd69684365c6015cc3671db2b7d807fe51f2", + "sha256:0ba0e00620ca28d4ca11fc700806fd69144b463aa3275e1b36e56c7c09915559", + "sha256:1f255ce8ed7556658f6d23f6afd22a6d9bbc3edb9b96c96682124dc487e1bf42", + "sha256:1fac1b7eebfce75ea663e860e7c5b4a8831b858c17acd68263bc156125201abf", + "sha256:263832fae27481d48dfafcc43174644b6706639661e242902ceb30553557e16c", + "sha256:29e89d0e1a7f18bc30f197cfadcbef5a13d99806447c7e245f5667579a808036", + "sha256:33037d9e56e2562c710c8954d0f20d25b8386b397250d65581e544edc9d6b942", + "sha256:33c584c0ef7dc54f5dd4f84082eabd8d09d1871a3d8ca2986b0c0c98165f8e86", + "sha256:36c8865bdb5cfeec88f5028e7e592370a0657b676c6f1d84a2108e0564f90e22", + "sha256:4145f91531fd43c50f9eb893faa08399816bb0b13c425667c48475c9f3a2b9b5", + "sha256:4d418b1fee41a1d14931f7ab4b92dc0bc323b490e41d7a333eec82c9f1780c75", + "sha256:768947008b4dc552d02772e5ebd49e71430a466e2373008ce905f953afea755a", + "sha256:7c7125068e04a70739dad11857a4d47626f2b0bd54de39e8622e89701836eabd", + "sha256:83a0d9336de2cba86d886507dd6e0153df333ac787377325a39a2797ec529814", + "sha256:86eef6aab7fd7c6c8545f3ebd00fd1d6729ca1f63b0cb4d621bccb7d1d1c852b", + "sha256:8fb022d799b96df3eaa27263e9eea306bd3d437cc9aa981820850281a02b6c9a", + "sha256:9d95fa0d22bf4f12d2fb7b07a46070cdfc19ef5a7b1c98bc172bfab5bf0d6844", + "sha256:a974c49a981e187381b9cc2c07c6b902d0079b88ff01aed34695ec5360767034", + "sha256:ac9a745b7609f489faa65e1dc842168c18530874a5f5b742ac3dd79e26bca8bc", + "sha256:af20acbe198a8a790618ee42db192eb128afcdcc4e96d99993aca0b60d1faeb4", + "sha256:af281525e5dd7fa0b39fb1667b8d5ca0e2a9079967e14c4bfe90fd1cd13e0f18", + "sha256:b050d362df50fc6e38ae3954d8c29bf2da52be384649ee8245fdb5186b620836", + "sha256:b44a52b8e6244b6548851b03b2b377a9702b88ddc21dcaf56a15a0393d425cb9", + "sha256:b607ea1e96768d13be26d2b400d10d3ebd1456343eb5eaddd2f47d1c4bd00880", + "sha256:b85ec0bdd7bdaa5c1946398cbb541e90a6dfc51df76dfa88e0aaa41b335940cb", + "sha256:bebd91041dda0d511b0d303180ed36e31f4f54b106b1259b69fade68413aa7ff", + "sha256:c076a9e548521ecc13d944b1d261ff3d7825048c338722a4bd126d22316087b7", + "sha256:cbe61b158deb09cffdd8540dc4a948d6e8f4d5b4f3bf5cd7db09bd6a61fee64e", + "sha256:cdee3ab220283057e7840d5fb768ad4c2ebe65bdba6f75d5d7bf47f4e0ed7d29", + "sha256:ce7033cb61f2bb65d8849658d3786188afd80f53dad8366a7232654804529532", + "sha256:d00af0884c0e65f60dfaf9340e26658836b935052fdd0439952ae42e44fdd2be", + "sha256:d647a0e697e5daa98c87993726da8281c7233d9d4ffe410812a4896c7c57c075", + "sha256:d970ecca0aac90d399e458f0b7a8a597e08f95de021f17785fb68e2dc0b99717", + "sha256:ea329dafb9670ffbdf4dbc3b0e5c264104abcd8441d56de77f06967f032943cb", + "sha256:ebf46e7f01b7af7861310417d7c49591a85d99146fc23a5ba82fdb28af156321", + "sha256:edc0cce355984bb3c1d1e89d6a661934d39586bb32191ebff98c600f8957c63e", + "sha256:f3bbe672df03563d1f3a691ae531f2e31f84061724c319652039e5a70927167e", + "sha256:fc11e5114f3f978d0cea7e9853627935b30d451742eeb4239a81a677bdee6bf6", + "sha256:fdb54b076f25d6b0f0298dc706acee5052de20c83530fa165b60d1f2e9cbe3cb" ], "markers": "python_version >= '3.8'", - "version": "==4.48.1" + "version": "==4.49.0" }, "importlib-resources": { "hashes": [ - "sha256:3893a00122eafde6894c59914446a512f728a0c1a45f9bb9b63721b6bacf0b4a", - "sha256:e8bf90d8213b486f428c9c39714b920041cb02c184686a3dee24905aaa8105d6" + "sha256:4c0269e3580fe2634d364b39b38b961540a7738c02cb984e98add8b4221d793d", + "sha256:56fb4525197b78544a3354ea27793952ab93f935bb4bf746b846bb1015020f2b" ], "markers": "python_version < '3.10'", - "version": "==6.1.1" + "version": "==6.1.3" }, "kiwisolver": { "hashes": [ @@ -393,58 +401,58 @@ }, "numpy-quaternion": { "hashes": [ - "sha256:02b93874723c38ad1e684d0862899d9266bf9855fd5a5bdcba8793169e672c31", - "sha256:04671ea098c0fe879eb07a24ec80dc09efc674e178f9b58a427f9d2368b2c009", - "sha256:04c4536fdb7f22733631b7953e2db82b27964d96f97423901e749c971cb7f6f2", - "sha256:0560b12235aaab7aee56e94c2df2f7879e0c965b8aea3c6bccaad7f2b4fb031a", - "sha256:0c058ee103024dc15b3232e57204934a53be080d5c75246cdec9eb92e9f56c5f", - "sha256:13958c8628b17f9bc725bb54e910c384e211e54b057cbe069f1615aebae8735d", - "sha256:2fdbc31fdac812ed2ff0287a2d51e1b87d5ec6d2aeea4a667adb14f4b6198bc5", - "sha256:32e34d2ebeeed25b238df22eba0030ba8db4a4e82a7eb6f5e32fda45768990ee", - "sha256:333dea61c9628707223dc062e4a6e0a72bbb4fffd58a84231ea24b959e694bde", - "sha256:37f73d7f84c645bd9be95cb4862bd900b7f99bd2f801232006dde00641bf2fd7", - "sha256:3f89e11f89ded410fb34e6f997d4c7f4cf7c31c3eb9537c035756a5d2a6cc4e3", - "sha256:41968027811fa81157c9bc9f2bf00cc22dc8865d7fb5834f9f83bafc5995b6ec", - "sha256:449ba07ec505dd757aa4ba6df8ef086bdd06c85f4681529ddaecd4ce7d62e792", - "sha256:48bb1fc03b580a9bb89da9d4f8916f87101bc75682611c423bafa031b6d96176", - "sha256:4b9421d46d56fbec0dd625c9909550c66bb81265a76efaecc5621166f18069bf", - "sha256:57d99cee91c7356c62d70817d32432db3da58f4d5f3bd29757c5696f56fa2e86", - "sha256:5c5b7dfb7412b582101ae4e576f15bc6af904f66b24b832aa1fafa3a846c71da", - "sha256:5dd15141aecbf32cdb6bf96bdc13df7dd2f31833011a7f0ef51ecc86872cf8f0", - "sha256:5f994628b10bf29461fb50cf3ce022d0a610e173068414942a9efd746b35b38b", - "sha256:60c1e9f9997205949c770702307451eeffd96f3a2824f4dc49ed42336bd698e2", - "sha256:681aaa2cf4d59fc412ee00188dcdc551c8ff91ea63d54d06f37ec66dd383633d", - "sha256:6c7e82014a51c93fe76322654d9c59f03b2e5cd19d0d6535d606bf7a119d4394", - "sha256:6dd56641fddad6c35d86a6d9f3cee4a786d0a4c6b41ed74d60dad97741835280", - "sha256:713e4357868ebd8e4f3500435fcb49a997a8a9a5f8514e3a79d51f46abcdf2ae", - "sha256:86d46c5f220ed2289d7d53c918b0e2432d6ddeae20c5ca232f3dab6fafe6c340", - "sha256:86f931da5893db57c4da4142045b605cc99d469fb3e6238ae487e080dcd7227e", - "sha256:8e218a5207be1a983d3fd54d710067a6638d324015ba695c0509082a29086284", - "sha256:98bfb77597ea56462be3f94e002640ebc6ecf9d2eeea140f5d1c13145af56a31", - "sha256:9b26f4961fef053d552f5dcea0957b1eb34c99fea92efe1544044013d04e1407", - "sha256:ac5e37ed57c0e2ff938c88d4462a126b16c98581dde0c003eba05741188b7f38", - "sha256:b0f8517c268d748cbfe686214bd53ac7064e85106c90e22bd7cf04940a17323e", - "sha256:b144be3dca3330f8ad5866c561cebbfe3273a5b228ece058c014cdbf8916630d", - "sha256:b343649600eb9f30275380b47ee4430f4393ed3370e5fa3fbb1db0ebbd908228", - "sha256:b4df8ffdcab6f773eec518ed09abb81e233afd9a38534e3a1db0cb0bfc54b370", - "sha256:bb3ab05505ccb5c835a6f0401811d64f23c843e622751956ba77734f7dc20493", - "sha256:bca80ceef24364eb4dc07026e3d5c7cc9932b844888a3a15f27941f0ee6ba5c3", - "sha256:bf6a99191d1d0b3289eb256c1eaf7e290d80d4a306bb31d04121bf9a7eb88701", - "sha256:c2ddf7e16a611f1c07a170d9464d69291eeb734ade2ce50b7f4eb38d9620f007", - "sha256:c829f58ebc908f07487d3351a13ba99c3e39eb5e04aea389ca5175642cfdab15", - "sha256:cab8b1626c6d719639360a6af920c25df3f0248ab04635b72919aa1a05cb575f", - "sha256:cf487d6b56883895ddf22307a0cf8e9949604465154d0cd9b78250d800d07a0d", - "sha256:d11f6f030d1cc7b58afe83fa849422a1c8c3a742b7af30232b98acbe32cd2be6", - "sha256:d725796e9f21eb703ae19448ceea0ab34e850c903ab01fef3de06f7217ae17f5", - "sha256:dec65adaac6bf15f31951e25bf5fe908135db6e223cf2df0112c93afe432d5de", - "sha256:e033eef943a904b9c34c1d9e66570a07fa2c3d4a311a357d1aeb305493092c08", - "sha256:e6b4dd4797e6e77fcdd8b3487893f8af3fe934f1f26839d1605f771f700dded6", - "sha256:e6dcfec4c7f615e6c46411c2034631e0a1934ffc3509e7bd61c3aacce4ecb181", - "sha256:eeeb8a6004a649b4a411fb25fb94a6da8e937de25b7c409c62528c937d1bb47d", - "sha256:eeffe622c5cec8396e61c266f65c75ec54fa4c21688a9633e8737276dc7fcc4b" + "sha256:00462f35b7668e45ec626578cbb7f67c6a258b915d77de606e4f5058c882ed4d", + "sha256:05c7b17fea1304e3f3aad8478148c671940abe6303c904c12da472ebb4e3c201", + "sha256:0ba2e4f266a94650d45407ff8507c546951f30a445f3a3f1701e517c6c456dda", + "sha256:13cbd13e3119bbbc2e9c45cae27d5723f94ab758256eaafe75f7bf4deea6537f", + "sha256:151f238ad5bfe51e18a8fad6afc75bec060f9ec3ebd01ad0565fb33ab6714818", + "sha256:1a99125c750ad5c7d17bf4c69b03fb9c86e418c582c9991ef9c119fd97a8a640", + "sha256:1b8fc0d8cdee31cee21d8564726dab3ad8206a4119d60e0732bcd9b8b7d079d2", + "sha256:23f429263e0e6f290ca6f6187a2739567f7fcd5e066259f0098007fa06068d44", + "sha256:241d635791b5aadb798a56baba0d754b8f5b11ac93e6e490de52324dd06ee608", + "sha256:324d19204d7ce7b60cfd9b1a351eb2e6bbfda307e5ea8326a2978f5dc082f334", + "sha256:33fa45a2a7e52173ecc10862377dcccc97b6f97c84aabb70e922d29411836694", + "sha256:3489f172485bb86c7f1875f35a885ebdc671e3b5d45eac0e227d141b36a5bf02", + "sha256:392bf3cb4eee36c0e9271534e93e39e46cdb4f7e2062b08cb38bd0872061ff6c", + "sha256:4285e8e4df7216f9adfe15d2802472c89758d92cbf3c7fde31798ca0e22eb7e0", + "sha256:481d386e4863e2aef88b618ac73e6dec9635737d01c9ccddfd53a855edcfea84", + "sha256:48693acbbb5868d56a370bafd1051d1c7c7ab39f1b852a29e67ac1258938cf4d", + "sha256:55ff80e4a2d6ca5d0e0e00e2ead59757e2816b50e97a19307ea09b56124bf5b1", + "sha256:6175a0555b9e6cf8fb22f88113b55e29dbec26622938e61a79ea4fcfcd8ad8e3", + "sha256:6699dbba41286da391b1ba40d0bebab8c2417cbce799d70c5745a254630bd0cd", + "sha256:68acbb54ea114b258259a588a3de2b190f58855ff6289a71efb26a22f659e863", + "sha256:6992f33b7f0b94a25ff31685eb5ab13d173ccdb996a40b28757da4120e852930", + "sha256:734259bfa8da467812a46ed79fcd9e3429e9aaa26f4449f47e65a709abd8b000", + "sha256:73e10823b0b0fb0f5f1a5fb6e36db3e09d5ed1b547dd5e6a31ac2f126916dfe5", + "sha256:77d98ea3a5c6d005dbda7cf38c3d8bf5de49fa3b5809046083349439930248d0", + "sha256:7c67e35c3f128f8c32b76be448d8b8f102d4fd8d19086afc816fa8908abd91f0", + "sha256:7fb35558e572d17ede74bd295bd39b7b4e430e2454394162a419ec13e398e16f", + "sha256:82158827e53fe84b79bb0e1fa993151dd4b25381c148d0486fe0518f96937c55", + "sha256:85811e0dbf7bb5e0d1d589331ecf452f972f555c1d6be31f3383c5e826184735", + "sha256:8ebc517a59487e2424b98a0d6291d1e7ad77ca36d2702015357f62097953de1f", + "sha256:92be260eccb718f07c67e314601a3cca22bb5cdcba8e6fd6c70febd24f947d4a", + "sha256:a05f00ce120089315d67471e01542e9aeffa3069be05e41690d6de5ac36a1da0", + "sha256:a16cc15587c47fd5ba512c621b2f72e9f7c26b829ef869395f9d16029639849e", + "sha256:a4e22488ce2a5f4d8b93aafa50d2d659719b46fb358648853d52db12d000f13c", + "sha256:ab8912509ff8b6de23c39fa6ae2e6b055893687939382ba2289078f9c8d0f164", + "sha256:b3aec1175b788b74d788fdef7c221707a2f68cbc20653ddc2a3c15e5332c1fb4", + "sha256:c2eef9417c0c7d339846537ce539417aff0e546f05c9c52ae4494966322e0ce7", + "sha256:c2f300ef729e3f0755e5518ef97770c27f29ebd5a1e58699adc2f045aad6958d", + "sha256:cd6bed4d09c141062d6e3745e0fcaae80536a36ffa5ba3248771a7ca8d447734", + "sha256:d0dba8b77df485ca9fdc9b1e8bf57fcbc34e9f08362b73a29df2d5bd72c50fe7", + "sha256:d4e452a95a55bb58016ff5b555cde3ffcf95a4a4ad4d6c92b8f05119324f3229", + "sha256:d65d06b1a8c4f153f3b830601e8c1a1c3d75a92b289c67f5394684de082b36ef", + "sha256:db804fed5bc128f4ff143bedf5ae987efc1f8c75b53956cc3f1194ed8732b4b8", + "sha256:e3579c10c63a40fc466fa051419222fe17010c7ddabcd6300d7ae8ecdf4f68cc", + "sha256:e56939aae3f493acfa7c09dc5476893a61b3a7843e7c22d7cf65a14f9fe5eba3", + "sha256:ed5667e9dd4c16f4d88407f041009374fd53c034b3d9b61a7496a515fb235a9b", + "sha256:f24aee35a72195c3f4002859c48f9a77660385153ea72df7ef3a9dc9c5e6e533", + "sha256:f54a63b34f4207f7bf56dda94aecb7926019ba99d860e305048cb4ec126dcd14", + "sha256:fadc1caa87ac0d7e898250fe30d92e83aa3e0f441edba106d1bffa54b684054e", + "sha256:fcac49e60f26be6db809fec0ce07b36af48cea159c7d5ac4944955814846b3b4" ], "index": "pypi", - "version": "==2023.0.2" + "version": "==2023.0.3" }, "numpy-stl": { "hashes": [ @@ -628,11 +636,11 @@ }, "python-dateutil": { "hashes": [ - "sha256:0123cacc1627ae19ddf3c27a5de5bd67ee4586fbdd6440d9748f8abb483d3e86", - "sha256:961d03dc3453ebbc59dbdea9e4e11c5651520a876d0f4db161e8674aae935da9" + "sha256:37dd54208da7e1cd875388217d5e00ebd4179249f90fb72437e91a35459a0ad3", + "sha256:a8b2bc7bffae282281c8140a97d3aa9c14da0b136dfe83f850eea9a5f7470427" ], "markers": "python_version >= '2.7' and python_version not in '3.0, 3.1, 3.2, 3.3'", - "version": "==2.8.2" + "version": "==2.9.0.post0" }, "python-utils": { "hashes": [