Merge pull request #531 from ut-issl/feature/add-gnss-satellite-log

Add GNSS satellite log output

scripts/Plot/plot_gnss_satellites.py

@@ -0,0 +1,69 @@
+#
+# Plot GNSS Satellites
+#
+# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946
+#
+
+#
+# Import
+#
+# plots
+import matplotlib.pyplot as plt
+# numpy
+import numpy as np
+# local function
+from common import find_latest_log_tag
+from common import add_log_file_arguments
+from common import read_3d_vector_from_csv
+from common import read_scalar_from_csv
+# arguments
+import argparse
+
+# Arguments
+aparser = argparse.ArgumentParser()
+aparser = add_log_file_arguments(aparser)
+aparser.add_argument('--no-gui', action='store_true')
+args = aparser.parse_args()
+
+#
+# Read Arguments
+#
+# log file path
+path_to_logs = args.logs_dir
+
+read_file_tag = args.file_tag
+if read_file_tag == None:
+  print("file tag does not found. use latest.")
+  read_file_tag = find_latest_log_tag(path_to_logs)
+
+print("log: " + read_file_tag)
+
+#
+# CSV file name
+#
+read_file_name  = path_to_logs + '/' + 'logs_' + read_file_tag + '/' + read_file_tag + '_default.csv'
+
+#
+# Data read and edit
+#
+# Read S2E CSV
+time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]')
+
+plt.figure(0)
+for gps_idx in range(32):
+  gps_str = 'GPS' + str(gps_idx)
+  position = read_3d_vector_from_csv(read_file_name, gps_str + '_position_ecef', 'm')
+  plt.plot(time[0], position[0], marker=".", label=gps_str + "x")
+  plt.plot(time[0], position[1], marker=".", label=gps_str + "y")
+  plt.plot(time[0], position[2], marker=".", label=gps_str + "z")
+
+plt.title("GPS Position")
+plt.xlabel("Time [s]")
+plt.ylabel("Position @ ECEF [m]")
+plt.legend(fontsize=7, loc="upper right")
+
+# Data save
+if args.no_gui:
+  plt.savefig(read_file_tag + "_gnss_satellites.png") # save last figure only
+else:
+  plt.show()

src/environment/global/global_environment.cpp

@@ -42,6 +42,7 @@ void GlobalEnvironment::Update() {
 void GlobalEnvironment::LogSetup(Logger& logger) {
   logger.AddLogList(simulation_time_);
   logger.AddLogList(celestial_information_);
+  logger.AddLogList(gnss_satellites_);
 }
 
 void GlobalEnvironment::Reset(void) { simulation_time_->ResetClock(); }

src/environment/global/gnss_satellites.cpp

@@ -854,7 +854,13 @@ GnssSatellites::GnssSatellites(bool is_calc_enabled)
       ofs_sa("sa.csv")
 #endif
 {
+  // TODO: Add log enable flag in ini file
   is_calc_enabled_ = is_calc_enabled;
+  if (is_calc_enabled_) {
+    is_log_enabled_ = true;
+  } else {
+    is_log_enabled_ = false;
+  }
 }
 
 bool GnssSatellites::IsCalcEnabled() const { return is_calc_enabled_; }
@@ -1100,12 +1106,21 @@ double GnssSatellites::AddIonosphericDelay(const int gnss_satellite_id, const li
 std::string GnssSatellites::GetLogHeader() const {
   std::string str_tmp = "";
 
+  // TODO: Add log output for other navigation systems
+  for (size_t gps_index = 0; gps_index < gps_sat_num_; gps_index++) {
+    str_tmp += WriteVector("GPS" + std::to_string(gps_index) + "_position", "ecef", "m", 3);
+  }
+
   return str_tmp;
 }
 
 std::string GnssSatellites::GetLogValue() const {
   std::string str_tmp = "";
 
+  for (size_t gps_index = 0; gps_index < gps_sat_num_; gps_index++) {
+    str_tmp += WriteVector(true_info_.GetSatellitePositionEcef(gps_index), 16);
+  }
+
   return str_tmp;
 }