Skip to content

Commit

Permalink
Add time_system namespace
Browse files Browse the repository at this point in the history
  • Loading branch information
200km committed Jul 20, 2024
1 parent 216744c commit 5873393
Show file tree
Hide file tree
Showing 15 changed files with 84 additions and 53 deletions.
31 changes: 16 additions & 15 deletions src/environment/global/gnss_satellites.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ using namespace gnss;

const size_t kNumberOfInterpolation = 9;

void GnssSatellites::Initialize(const std::vector<Sp3FileReader>& sp3_files, const EpochTime start_time) {
void GnssSatellites::Initialize(const std::vector<Sp3FileReader>& sp3_files, const time_system::EpochTime start_time) {
sp3_files_ = sp3_files;
current_epoch_time_ = start_time;

Expand All @@ -37,7 +37,7 @@ void GnssSatellites::Initialize(const std::vector<Sp3FileReader>& sp3_files, con
if (nearest_epoch_id >= half_interpolation_number) {
reference_interpolation_id_ = nearest_epoch_id - half_interpolation_number;
}
reference_time_ = EpochTime(initial_sp3_file.GetEpochData(reference_interpolation_id_));
reference_time_ = time_system::EpochTime(initial_sp3_file.GetEpochData(reference_interpolation_id_));

// Initialize orbit
orbit_.assign(number_of_calculated_gnss_satellites_, orbit::InterpolationOrbit(kNumberOfInterpolation));
Expand All @@ -60,9 +60,9 @@ void GnssSatellites::Update(const SimulationTime& simulation_time) {

// Get time
UTC current_utc = simulation_time.GetCurrentUtc();
DateTime current_date_time((size_t)current_utc.year, (size_t)current_utc.month, (size_t)current_utc.day, (size_t)current_utc.hour,
(size_t)current_utc.minute, current_utc.second);
current_epoch_time_ = EpochTime(current_date_time);
time_system::DateTime current_date_time((size_t)current_utc.year, (size_t)current_utc.month, (size_t)current_utc.day, (size_t)current_utc.hour,
(size_t)current_utc.minute, current_utc.second);
current_epoch_time_ = time_system::EpochTime(current_date_time);

// Check interpolation update
double diff_s = current_epoch_time_.GetTimeWithFraction_s() - reference_time_.GetTimeWithFraction_s();
Expand All @@ -74,10 +74,10 @@ void GnssSatellites::Update(const SimulationTime& simulation_time) {
return;
}

math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_id, const EpochTime time) const {
math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_id, const time_system::EpochTime time) const {
if (gnss_satellite_id > number_of_calculated_gnss_satellites_) return math::Vector<3>(0.0);

EpochTime target_time;
time_system::EpochTime target_time;

if (time.GetTime_s() == 0) {
target_time = current_epoch_time_;
Expand All @@ -92,10 +92,10 @@ math::Vector<3> GnssSatellites::GetPosition_ecef_m(const size_t gnss_satellite_i
return orbit_[gnss_satellite_id].CalcPositionWithTrigonometric(diff_s, math::tau / kOrbitalPeriodCorrection_s);
}

double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const EpochTime time) const {
double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const time_system::EpochTime time) const {
if (gnss_satellite_id > number_of_calculated_gnss_satellites_) return 0.0;

EpochTime target_time;
time_system::EpochTime target_time;

if (time.GetTime_s() == 0) {
target_time = current_epoch_time_;
Expand All @@ -109,9 +109,9 @@ double GnssSatellites::GetClock_s(const size_t gnss_satellite_id, const EpochTim
return clock_[gnss_satellite_id].CalcPolynomial(diff_s) * 1e-6;
}

bool GnssSatellites::GetCurrentSp3File(Sp3FileReader& current_sp3_file, const EpochTime current_time) {
bool GnssSatellites::GetCurrentSp3File(Sp3FileReader& current_sp3_file, const time_system::EpochTime current_time) {
for (size_t i = 0; i < sp3_files_.size(); i++) {
EpochTime sp3_start_time(sp3_files_[i].GetStartEpochDateTime());
time_system::EpochTime sp3_start_time(sp3_files_[i].GetStartEpochDateTime());
double diff_s = current_time.GetTimeWithFraction_s() - sp3_start_time.GetTimeWithFraction_s();
if (diff_s < 0.0) {
// Error
Expand All @@ -129,7 +129,7 @@ bool GnssSatellites::UpdateInterpolationInformation() {
Sp3FileReader sp3_file = sp3_files_[sp3_file_id_];

for (size_t gnss_id = 0; gnss_id < number_of_calculated_gnss_satellites_; gnss_id++) {
EpochTime sp3_time = EpochTime(sp3_file.GetEpochData(reference_interpolation_id_));
time_system::EpochTime sp3_time = time_system::EpochTime(sp3_file.GetEpochData(reference_interpolation_id_));
double time_diff_s = sp3_time.GetTimeWithFraction_s() - reference_time_.GetTimeWithFraction_s();
math::Vector<3> sp3_position_m = 1000.0 * sp3_file.GetSatellitePosition_km(reference_interpolation_id_, gnss_id);

Expand Down Expand Up @@ -226,9 +226,10 @@ GnssSatellites* InitGnssSatellites(const std::string file_name, const EarthRotat
}

//
DateTime start_date_time((size_t)simulation_time.GetStartYear(), (size_t)simulation_time.GetStartMonth(), (size_t)simulation_time.GetStartDay(),
(size_t)simulation_time.GetStartHour(), (size_t)simulation_time.GetStartMinute(), simulation_time.GetStartSecond());
EpochTime start_epoch_time(start_date_time);
time_system::DateTime start_date_time((size_t)simulation_time.GetStartYear(), (size_t)simulation_time.GetStartMonth(),
(size_t)simulation_time.GetStartDay(), (size_t)simulation_time.GetStartHour(),
(size_t)simulation_time.GetStartMinute(), simulation_time.GetStartSecond());
time_system::EpochTime start_epoch_time(start_date_time);
gnss_satellites->Initialize(sp3_file_readers, start_epoch_time);

return gnss_satellites;
Expand Down
12 changes: 6 additions & 6 deletions src/environment/global/gnss_satellites.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ class GnssSatellites : public ILoggable {
* @param [in] sp3_files: List of SP3 files
* @param [in] start_time: The simulation start time
*/
void Initialize(const std::vector<gnss::Sp3FileReader>& sp3_files, const EpochTime start_time);
void Initialize(const std::vector<gnss::Sp3FileReader>& sp3_files, const time_system::EpochTime start_time);

/**
* @fn IsCalcEnabled
Expand Down Expand Up @@ -87,7 +87,7 @@ class GnssSatellites : public ILoggable {
* @param [in] time: Target time to get the GNSS satellite. When the argument is not set, the last updated time is used for the calculation.
* @return GNSS satellite position at ECEF frame at the time. Or return zero vector when the arguments are out of range.
*/
math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, const EpochTime time = EpochTime(0, 0.0)) const;
math::Vector<3> GetPosition_ecef_m(const size_t gnss_satellite_id, const time_system::EpochTime time = time_system::EpochTime(0, 0.0)) const;

/**
* @fn GetGetClock_s
Expand All @@ -96,7 +96,7 @@ class GnssSatellites : public ILoggable {
* @param [in] time: Target time to get the GNSS satellite. When the argument is not set, the last updated time is used for the calculation.
* @return GNSS satellite clock offset at the time. Or return zero when the arguments are out of range.
*/
double GetClock_s(const size_t gnss_satellite_id, const EpochTime time = EpochTime(0, 0.0)) const;
double GetClock_s(const size_t gnss_satellite_id, const time_system::EpochTime time = time_system::EpochTime(0, 0.0)) const;

// Override ILoggable
/**
Expand All @@ -116,9 +116,9 @@ class GnssSatellites : public ILoggable {
std::vector<gnss::Sp3FileReader> sp3_files_; //!< List of SP3 files
size_t number_of_calculated_gnss_satellites_; //!< Number of calculated GNSS satellites
size_t sp3_file_id_; //!< Current SP3 file ID
EpochTime reference_time_; //!< Reference start time of the SP3 handling
time_system::EpochTime reference_time_; //!< Reference start time of the SP3 handling
size_t reference_interpolation_id_ = 0; //!< Reference epoch ID of the interpolation
EpochTime current_epoch_time_; //!< The last updated time
time_system::EpochTime current_epoch_time_; //!< The last updated time

std::vector<orbit::InterpolationOrbit> orbit_; //!< GNSS satellite orbit with interpolation
std::vector<math::Interpolation> clock_; //!< GNSS satellite clock offset with interpolation
Expand All @@ -133,7 +133,7 @@ class GnssSatellites : public ILoggable {
* @param [in] current_time: Target time
* @return true means no error, false means the time argument is out of range
*/
bool GetCurrentSp3File(gnss::Sp3FileReader& current_sp3_file, const EpochTime current_time);
bool GetCurrentSp3File(gnss::Sp3FileReader& current_sp3_file, const time_system::EpochTime current_time);

/**
* @fn UpdateInterpolationInformation
Expand Down
4 changes: 2 additions & 2 deletions src/math_physics/gnss/antex_file_reader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -192,11 +192,11 @@ AntexPhaseCenterData AntexFileReader::ReadPhaseCenterData(std::ifstream& antex_f
return phase_center_data;
}

DateTime AntexFileReader::ReadDateTime(std::string line) {
time_system::DateTime AntexFileReader::ReadDateTime(std::string line) {
size_t year, month, day, hour, minute;
double second;
sscanf(line.c_str(), "%zu %2zu %2zu %2zu %2zu %10lf", &year, &month, &day, &hour, &minute, &second);
return DateTime(year, month, day, hour, minute, second);
return time_system::DateTime(year, month, day, hour, minute, second);
}

} // namespace gnss
14 changes: 7 additions & 7 deletions src/math_physics/gnss/antex_file_reader.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -203,12 +203,12 @@ class AntexSatelliteData {
* @fn SetValidStartTime
* @param[in] valid_start_time: Valid start time
*/
inline void SetValidStartTime(const DateTime valid_start_time) { valid_start_time_ = valid_start_time; };
inline void SetValidStartTime(const time_system::DateTime valid_start_time) { valid_start_time_ = valid_start_time; };
/**
* @fn SetValidEndTime
* @param[in] valid_end_time: Valid end time
*/
inline void SetValidEndTime(const DateTime valid_end_time) { valid_end_time_ = valid_end_time; };
inline void SetValidEndTime(const time_system::DateTime valid_end_time) { valid_end_time_ = valid_end_time; };
/**
* @fn SetNumberOfFrequency
* @param[in] number_of_frequency: Number of frequency
Expand All @@ -235,12 +235,12 @@ class AntexSatelliteData {
* @fn GetValidStartTime
* @return Valid start time
*/
inline DateTime GetValidStartTime() const { return valid_start_time_; };
inline time_system::DateTime GetValidStartTime() const { return valid_start_time_; };
/**
* @fn GetValidEndTime
* @return Valid end time
*/
inline DateTime GetValidEndTime() const { return valid_end_time_; };
inline time_system::DateTime GetValidEndTime() const { return valid_end_time_; };
/**
* @fn GetNumberOfFrequency
* @return Number of frequency
Expand All @@ -256,8 +256,8 @@ class AntexSatelliteData {
private:
std::string antenna_type_; //!< Antenna type
std::string serial_number_; //!< Serial number or satellite code
DateTime valid_start_time_; //!< Valid start time
DateTime valid_end_time_; //!< Valid end time (The latest data does not have the end time)
time_system::DateTime valid_start_time_; //!< Valid start time
time_system::DateTime valid_end_time_; //!< Valid end time (The latest data does not have the end time)
size_t number_of_frequency_ = 1; //!< Number of frequency
std::vector<AntexPhaseCenterData> phase_center_data_; //!< Phase center data for each frequency
};
Expand Down Expand Up @@ -342,7 +342,7 @@ class AntexFileReader {
* @param[in] line: A single line in ANTEX file
* @return Read date time
*/
DateTime ReadDateTime(std::string line);
time_system::DateTime ReadDateTime(std::string line);
};

} // namespace gnss
Expand Down
18 changes: 9 additions & 9 deletions src/math_physics/gnss/sp3_file_reader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,9 @@ namespace gnss {

Sp3FileReader::Sp3FileReader(const std::string file_name) { ReadFile(file_name); }

DateTime Sp3FileReader::GetEpochData(const size_t epoch_id) const {
time_system::DateTime Sp3FileReader::GetEpochData(const size_t epoch_id) const {
if (epoch_id > epoch_.size()) {
DateTime zero;
time_system::DateTime zero;
return zero;
}
return epoch_[epoch_id];
Expand Down Expand Up @@ -69,7 +69,7 @@ bool Sp3FileReader::ReadFile(const std::string file_name) {
size_t year, month, day, hour, minute;
double second;
sscanf(line.substr(3, 28).c_str(), "%zu %2zu %2zu %2zu %2zu %12lf", &year, &month, &day, &hour, &minute, &second);
epoch_.push_back(DateTime(year, month, day, hour, minute, second));
epoch_.push_back(time_system::DateTime(year, month, day, hour, minute, second));

// Orbit and Clock information
for (size_t satellite_id = 0; satellite_id < header_.number_of_satellites_; satellite_id++) {
Expand Down Expand Up @@ -117,7 +117,7 @@ bool Sp3FileReader::ReadFile(const std::string file_name) {
}

// Test
DateTime test = epoch_[0];
time_system::DateTime test = epoch_[0];
test = epoch_[1];
std::vector<Sp3PositionClock> test_p = position_clock_[0];
test_p = position_clock_[1];
Expand All @@ -126,18 +126,18 @@ bool Sp3FileReader::ReadFile(const std::string file_name) {
return true;
}

size_t Sp3FileReader::SearchNearestEpochId(const EpochTime time) {
size_t Sp3FileReader::SearchNearestEpochId(const time_system::EpochTime time) {
size_t nearest_epoch_id = 0;

// Get header info
const size_t num_epoch = header_.number_of_epoch_;
const double interval_s = header_.epoch_interval_s_;

// Check range
EpochTime start_epoch(epoch_[0]);
time_system::EpochTime start_epoch(epoch_[0]);
if (start_epoch > time) {
nearest_epoch_id = 0;
} else if ((EpochTime)(epoch_[num_epoch - 1]) < time) {
} else if ((time_system::EpochTime)(epoch_[num_epoch - 1]) < time) {
nearest_epoch_id = num_epoch - 1;
} else { // Calc nearest point
double diff_s = time.GetTimeWithFraction_s() - start_epoch.GetTimeWithFraction_s();
Expand Down Expand Up @@ -171,7 +171,7 @@ size_t Sp3FileReader::ReadHeader(std::ifstream& sp3_file) {
size_t year, month, day, hour, minute;
double second;
sscanf(line.substr(3, 28).c_str(), "%zu %2zu %2zu %2zu %2zu %12lf", &year, &month, &day, &hour, &minute, &second);
header_.start_epoch_ = DateTime(year, month, day, hour, minute, second);
header_.start_epoch_ = time_system::DateTime(year, month, day, hour, minute, second);
header_.number_of_epoch_ = std::stoi(line.substr(32, 7));
header_.used_data_ = line.substr(40, 5);
header_.coordinate_system_ = line.substr(46, 5);
Expand Down Expand Up @@ -199,7 +199,7 @@ size_t Sp3FileReader::ReadHeader(std::ifstream& sp3_file) {
return 0;
}
// Read contents
header_.start_gps_time_ = GpsTime(std::stoi(line.substr(3, 4)), std::stod(line.substr(8, 15)));
header_.start_gps_time_ = time_system::GpsTime(std::stoi(line.substr(3, 4)), std::stod(line.substr(8, 15)));
header_.epoch_interval_s_ = std::stod(line.substr(24, 14));
header_.start_time_mjday_ = std::stoi(line.substr(39, 5));
header_.start_time_mjday_fractional_day_ = std::stod(line.substr(45, 15));
Expand Down
28 changes: 14 additions & 14 deletions src/math_physics/gnss/sp3_file_reader.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,16 +51,16 @@ enum class Sp3OrbitType {
struct Sp3Header {
// 1st line information
// version -> not implemented yet
Sp3Mode mode_; //!< position or velocity
DateTime start_epoch_; //!< Time of start epoch
size_t number_of_epoch_ = 0; //!< Number of epoch in the SP3 file
std::string used_data_; //!< Used data to generate the SP3 file
std::string coordinate_system_; //!< Coordinate system for the position and velocity data
Sp3OrbitType orbit_type_; //!< Orbit type
std::string agency_name_; //!< Agency name who generates the SP3 file
Sp3Mode mode_; //!< position or velocity
time_system::DateTime start_epoch_; //!< Time of start epoch
size_t number_of_epoch_ = 0; //!< Number of epoch in the SP3 file
std::string used_data_; //!< Used data to generate the SP3 file
std::string coordinate_system_; //!< Coordinate system for the position and velocity data
Sp3OrbitType orbit_type_; //!< Orbit type
std::string agency_name_; //!< Agency name who generates the SP3 file

// 2nd line information
GpsTime start_gps_time_; //!< Start time of orbit
time_system::GpsTime start_gps_time_; //!< Start time of orbit
double epoch_interval_s_ = 1.0; //!< Epoch interval (0.0, 100000.0)
size_t start_time_mjday_; //!< Start time of the orbit data (44244 = 6th Jan. 1980) [Modified Julian day]
double start_time_mjday_fractional_day_ = 0.0; //!< Fractional part of the start time [0.0, 1.0) [day]
Expand Down Expand Up @@ -172,19 +172,19 @@ class Sp3FileReader {
inline Sp3Header GetHeader() const { return header_; }
inline size_t GetNumberOfEpoch() const { return header_.number_of_epoch_; }
inline size_t GetNumberOfSatellites() const { return header_.number_of_satellites_; }
inline DateTime GetStartEpochDateTime() const { return header_.start_epoch_; }
inline GpsTime GetStartEpochGpsTime() const { return header_.start_gps_time_; }
inline time_system::DateTime GetStartEpochDateTime() const { return header_.start_epoch_; }
inline time_system::GpsTime GetStartEpochGpsTime() const { return header_.start_gps_time_; }
// Data
DateTime GetEpochData(const size_t epoch_id) const;
time_system::DateTime GetEpochData(const size_t epoch_id) const;
Sp3PositionClock GetPositionClock(const size_t epoch_id, const size_t satellite_id);
double GetSatelliteClockOffset(const size_t epoch_id, const size_t satellite_id);
math::Vector<3> GetSatellitePosition_km(const size_t epoch_id, const size_t satellite_id);

size_t SearchNearestEpochId(const EpochTime time);
size_t SearchNearestEpochId(const time_system::EpochTime time);

private:
Sp3Header header_; //!< SP3 header information
std::vector<DateTime> epoch_; //!< Epoch data list
Sp3Header header_; //!< SP3 header information
std::vector<time_system::DateTime> epoch_; //!< Epoch data list

// Orbit and clock data (Use as position_clock_[satellite_id][epoch_id])
std::map<size_t, std::vector<Sp3PositionClock>> position_clock_; //!< Position and Clock data
Expand Down
4 changes: 4 additions & 0 deletions src/math_physics/time_system/date_time_format.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@
#include <iomanip>
#include <sstream>

namespace time_system {

DateTime::DateTime(const std::string date_time) {
sscanf(date_time.c_str(), "%zu/%zu/%zu %zu:%zu:%lf", &year_, &month_, &day_, &hour_, &minute_, &second_);
}
Expand Down Expand Up @@ -52,3 +54,5 @@ std::string DateTime::GetAsString() const {
std::string output = stream.str();
return output;
}

} // namespace time_system
4 changes: 4 additions & 0 deletions src/math_physics/time_system/date_time_format.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@

#include "epoch_time.hpp"

namespace time_system {

class EpochTime;

/**
Expand Down Expand Up @@ -60,4 +62,6 @@ class DateTime {
double second_; //!< Second [0.0, 60.0)
};

} // namespace time_system

#endif // S2E_LIBRARY_TIME_SYSTEM_DATE_TIME_FORMAT_HPP_
4 changes: 4 additions & 0 deletions src/math_physics/time_system/epoch_time.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@

#include <cmath>

namespace time_system {

EpochTime::EpochTime(const DateTime date_time) {
// No leap second calculation
const int doy[] = {1, 32, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335}; //!< Day of Year for the 1st day of each month
Expand Down Expand Up @@ -73,3 +75,5 @@ EpochTime EpochTime::operator-(const EpochTime& right_side) const {
EpochTime result(time_s, fraction_s);
return result;
}

} // namespace time_system
4 changes: 4 additions & 0 deletions src/math_physics/time_system/epoch_time.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@

#include "date_time_format.hpp"

namespace time_system {

class DateTime;

/**
Expand Down Expand Up @@ -67,4 +69,6 @@ class EpochTime {
double fraction_s_; //!< Fraction of second under 1 sec [0, 1)
};

} // namespace time_system

#endif // S2E_LIBRARY_TIME_SYSTEM_EPOCH_TIME_HPP_
Loading

0 comments on commit 5873393

Please sign in to comment.