From 564d9562782b37ebee7e7960636ce1a915d749c6 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 6 Feb 2024 16:27:25 +0100 Subject: [PATCH 01/13] 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 02/13] 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 03/13] 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 04/13] 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 05/13] 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 06/13] 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 07/13] 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 08/13] 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 09/13] 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 10/13] 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 11/13] 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 12/13] 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 13/13] 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