Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Fix GNSS receiver ini file #597

Merged
merged 13 commits into from
Mar 12, 2024
Merged

Fix GNSS receiver ini file #597

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
564d956
Fix comments in ini file
200km Feb 6, 2024
9df6917
Rename gnss_id to gnss_system_id
200km Feb 6, 2024
0ad7095
Fix comment
200km Feb 6, 2024
538acf8
Remove max channel because it is not used
200km Feb 6, 2024
c53946d
Clean up local variables definition
200km Feb 6, 2024
598c40b
Modify antenna model interface
200km Feb 6, 2024
3a666c0
Add description comment
200km Feb 7, 2024
827ec0c
Modify to use ECEF frame for noise definition and Add velocity noise
200km Feb 7, 2024
45e63fd
Fix small
200km Feb 7, 2024
075b8d2
Fix comments
200km Feb 7, 2024
c7eeacb
Remove ECI observation information
200km Feb 7, 2024
2bee223
Add comment
200km Feb 7, 2024
9e4f68d
Fix comment
200km Feb 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 18 additions & 13 deletions data/sample/initialize_files/components/gnss_receiver.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,37 +6,42 @@ 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
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

// Number of channels
maximum_channel = 8

// GNSS ID
// 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
gnss_id = G
// 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
white_noise_standard_deviation_position_ecef_m(2) = 1500.0

//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_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
Expand Down
133 changes: 73 additions & 60 deletions src/components/real/aocs/gnss_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,63 +11,67 @@
#include <library/randomization/global_randomization.hpp>
#include <string>

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_deg, const libra::Vector<3> noise_standard_deviation_m,
const Dynamics* dynamics, const GnssSatellites* gnss_satellites, const SimulationTime* simulation_time)
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,
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),
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()),
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) {}
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_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> 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),
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()),
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) {}
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);

libra::Vector<3> position_true_eci_ = dynamics_->GetOrbit().GetPosition_i_m();
// 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);
CheckAntenna(position_true_eci, quaternion_i2b);

if (is_gnss_visible_) {
// 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_);
// 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());
Expand Down Expand Up @@ -118,8 +122,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);
Expand Down Expand Up @@ -147,7 +151,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);
}
}

Expand All @@ -158,7 +162,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);

Expand All @@ -167,20 +172,15 @@ 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);
}

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) {
Expand All @@ -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");
Expand All @@ -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);
Expand All @@ -238,15 +238,27 @@ 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;
libra::Vector<3> antenna_pos_b;
libra::Quaternion quaternion_b2c;
double half_width_deg;
std::string gnss_id;
size_t max_channel;
libra::Vector<3> noise_standard_deviation_m;
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;

GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const int component_id) {
Expand All @@ -261,20 +273,21 @@ 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<AntennaModel>(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;
}

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.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);
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);

return gnss_receiver_param;
}
Expand All @@ -283,9 +296,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_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.position_noise_standard_deviation_ecef_m,
gr_param.velocity_noise_standard_deviation_ecef_m_s, dynamics, gnss_satellites, simulation_time);
return gnss_r;
}

Expand All @@ -296,8 +309,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.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;
}
Loading
Loading