fix orbit calc error

ut-issl · May 24, 2024 · 33cd39b · 33cd39b
1 parent f5fb983
commit 33cd39b
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 9 deletions.
diff --git a/src/components/real/mission/telescope.cpp b/src/components/real/mission/telescope.cpp
@@ -79,7 +79,8 @@ Telescope::Telescope(ClockGenerator* clock_generator, const libra::Quaternion& q
   double imaging_duration_day = imaging_duration_sec / 60.0 / 60.0 / 24.0;
   center_imaging_jd = start_imaging_jd_ + imaging_duration_day / 2.0;
   end_imaging_jd = start_imaging_jd_ + imaging_duration_day;
-  assert(end_imaging_jd > current_jd + simulation_time_->GetEndTime_s() / 60.0 / 60.0 / 24.0);
+  assert(end_imaging_jd < (current_jd + simulation_time_->GetEndTime_s() / 60.0 / 60.0 / 24.0));
+  stage_time_day = line_rate_sec_ * stage_mode_ / 60.0 / 60.0 / 24.0;
 }
 
 Telescope::~Telescope() {}
@@ -261,9 +262,8 @@ void Telescope::ObserveGroundPositionDeviation() {
     ground_position_center_y_image_sensor_ = -1;
     return;
   }
-  double current_jd = simulation_time_->GetCurrentTime_jd();
-  double stage_time_sec = line_rate_sec_ * stage_mode_;
-  if (start_imaging_jd_ <= current_jd <= start_imaging_jd_ + stage_time_sec) {
+  double current_jd = simulation_time_->GetCurrentTime_js() / 86400.0;
+  if (start_imaging_jd_ <= current_jd && current_jd <= start_imaging_jd_ + stage_time_day) {
     if (startImagingFlag) {
       CalculateTargetGroundPosition();
       startImagingFlag = false;
@@ -279,7 +279,7 @@ void Telescope::ObserveGroundPositionDeviation() {
     ground_position_y_min_y_image_sensor_ = ground_position_y_min_image_sensor_.second;
   }
 
-  if (center_imaging_jd <= current_jd <= center_imaging_jd + stage_time_sec) {
+  if (center_imaging_jd <= current_jd && current_jd <= center_imaging_jd + stage_time_day) {
     if (centerImagingFlag) {
       CalculateTargetGroundPosition();
       centerImagingFlag = false;
@@ -295,7 +295,7 @@ void Telescope::ObserveGroundPositionDeviation() {
     ground_position_y_min_y_image_sensor_ = ground_position_y_min_image_sensor_.second;
   }
 
-  if (end_imaging_jd <= current_jd <= end_imaging_jd + stage_time_sec) {
+  if (end_imaging_jd <= current_jd && current_jd <= end_imaging_jd + stage_time_day) {
     if (endImagingFlag) {
       CalculateTargetGroundPosition();
       endImagingFlag = false;

diff --git a/src/components/real/mission/telescope.hpp b/src/components/real/mission/telescope.hpp
@@ -114,6 +114,7 @@ class Telescope : public Component, public ILoggable {
   int start_imaging_hour_;    //!< Imaging start hour
   int start_imaging_minute_;  //!< Imaging start minute
   double start_imaging_sec_;  //!< Imaging start seconds
+  double stage_time_day;      //!< Stage time seconds [sec]
 
   double center_imaging_jd;  //!< Imaging center Julian date [day]
   double end_imaging_jd;     //!< Imaging end Julian date [day]

diff --git a/src/dynamics/dynamics.cpp b/src/dynamics/dynamics.cpp
@@ -25,7 +25,7 @@ void Dynamics::Initialize(const SimulationConfiguration* simulation_configuratio
   const LocalCelestialInformation& local_celestial_information = local_environment_->GetCelestialInformation();
   // Initialize
   orbit_ = InitOrbit(&(local_celestial_information.GetGlobalInformation()), simulation_configuration->spacecraft_file_list_[spacecraft_id],
-                     simulation_time->GetOrbitRkStepTime_s(), simulation_time->GetCurrentTime_jd(),
+                     simulation_time->GetOrbitRkStepTime_s(), simulation_time->GetCurrentTime_js(),
                      local_celestial_information.GetGlobalInformation().GetCenterBodyGravityConstant_m3_s2(), "ORBIT", relative_information);
   attitude_ = InitAttitude(simulation_configuration->spacecraft_file_list_[spacecraft_id], orbit_, &local_celestial_information,
                            simulation_time->GetAttitudeRkStepTime_s(), structure->GetKinematicsParameters().GetInertiaTensor_b_kgm2(), spacecraft_id);
@@ -43,7 +43,7 @@ void Dynamics::Update(const SimulationTime* simulation_time, const LocalCelestia
   }
   // Orbit Propagation
   if (simulation_time->GetOrbitPropagateFlag()) {
-    orbit_->Propagate(simulation_time->GetElapsedTime_s(), simulation_time->GetCurrentTime_jd());
+    orbit_->Propagate(simulation_time->GetElapsedTime_s(), simulation_time->GetCurrentTime_js());
   }
   // Attitude dependent update
   orbit_->UpdateByAttitude(attitude_->GetQuaternion_i2b());

diff --git a/src/dynamics/orbit/sgp4_orbit_propagation.cpp b/src/dynamics/orbit/sgp4_orbit_propagation.cpp
@@ -49,7 +49,7 @@ void Sgp4OrbitPropagation::Propagate(const double end_time_s, const double curre
   UNUSED(end_time_s);
 
   if (!is_calc_enabled_) return;
-  double elapse_time_min = (current_time_js / 60.0 - sgp4_data_.jdsatepoch) * (24.0 * 60.0);
+  double elapse_time_min = current_time_js / 60.0 - sgp4_data_.jdsatepoch * 24.0 * 60.0;
 
   double position_i_km[3];
   double velocity_i_km_s[3];