Skip to content

Commit

Permalink
Merge pull request #597 from ut-issl/feature/fix-gnss-receiver-ini-file
Browse files Browse the repository at this point in the history 
Fix GNSS receiver ini file
  • Loading branch information
@200km
200km authored Mar 12, 2024
2 parents 093afc7 + 9e4f68d commit 5505198
Show file tree
Hide file tree
Showing 3 changed files with 139 additions and 119 deletions.
31 changes: 18 additions & 13 deletions data/sample/initialize_files/components/gnss_receiver.ini
View file
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
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

0 comments on commit 5505198

Please sign in to comment.