GPS時刻関連の更新

C2A_ISSL6U_AOBC.vcxproj

@@ -282,6 +282,7 @@
     <ClCompile Include="src\src_user\Library\SignalProcess\spike_filter.c" />
     <ClCompile Include="src\src_user\Library\xxhash.c" />
     <ClCompile Include="src\src_user\Library\crc8.c" />
+    <ClCompile Include="src\src_user\Library\time_system\gps_time.c" />
     <ClCompile Include="src\src_user\Settings\Applications\NvmParams\non_volatile_memory_normal_parameters.c" />
     <ClCompile Include="src\src_user\Settings\Applications\NvmParams\non_volatile_memory_redundant_parameters.c" />
     <ClCompile Include="src\src_user\Settings\Modes\mode_definitions.c" />

src/src_user/Applications/UserDefined/AOCS/HardwareDependent/SensorFilters/oem7600_filter.c

@@ -93,8 +93,7 @@ static void APP_OEM7600_FILTER_exec_(void)
 
   // EKF用の座標変換
   // ここではセンサ取得時のECEFをECIに戻すので，obsの時刻で座標変換行列を計算する
-  double reference_jday = TIME_SPACE_convert_gpstime_to_julian_day(aocs_manager->current_gps_time_obs_week,
-                                                                   aocs_manager->current_gps_time_obs_msec);
+  double reference_jday = TIME_SPACE_convert_gpstime_to_julian_day(aocs_manager->current_gps_time_obs);
   double ref_gmst_rad = TIME_SPACE_calc_gmst_rad(reference_jday);
 
   double dcm_eci_to_ecef[PHYSICAL_CONST_THREE_DIM][PHYSICAL_CONST_THREE_DIM];

src/src_user/Applications/UserDefined/AOCS/HardwareDependent/SensorSelectors/gpsr_selector.c

@@ -25,6 +25,7 @@ AppInfo APP_GPSR_SELECTOR_create_app(void)
 static void APP_GPSR_SELECTOR_init_(void)
 {
   gpsr_selector_.state = APP_GPSR_SELECTOR_STATE_OEM_IN_UNIT;
+
   return;
 }
 
@@ -38,9 +39,9 @@ static void APP_GPSR_SELECTOR_exec_(void)
   case APP_GPSR_SELECTOR_STATE_OEM_IN_UNIT:
     if (oem7600_driver[OEM7600_IDX_IN_UNIT]->info.num_of_visible_sats >= kEnoughNumberOfVisibleGps)
     {
-      AOCS_MANAGER_set_current_gps_time_obs(oem7600_driver[OEM7600_IDX_IN_UNIT]->info.times.gps_time_ms,
-                                            oem7600_driver[OEM7600_IDX_IN_UNIT]->info.times.gps_time_week,
-                                            oem7600_driver[OEM7600_IDX_IN_UNIT]->info.times.obct_gps_time_obs);
+      GPS_TIME_OF_WEEK obs_gps_time = GPS_TIME_OF_WEEK_create_gps_time(oem7600_driver[OEM7600_IDX_IN_UNIT]->info.times.gps_time_week,
+                                                                       oem7600_driver[OEM7600_IDX_IN_UNIT]->info.times.gps_time_ms);
+      AOCS_MANAGER_set_current_gps_time_obs(obs_gps_time, oem7600_driver[OEM7600_IDX_IN_UNIT]->info.times.obct_gps_time_obs);
 
       AOCS_MANAGER_set_sat_pos_vel_obs_ecef_m(oem7600_driver[OEM7600_IDX_IN_UNIT]->info.pos_antenna_ecef_m,
                                               oem7600_driver[OEM7600_IDX_IN_UNIT]->info.vel_antenna_ecef_m_s);

src/src_user/Applications/UserDefined/AOCS/InertialReference/time_space_calculator.c

@@ -43,10 +43,16 @@ static void APP_TIME_SPACE_CALC_init_(void)
 
 static void APP_TIME_SPACE_CALC_exec_(void)
 {
+  static ObcTime obct_tag_for_last_gps_tlm = OBCT_create(0, 0, 0); //!< 最後にJday更新に用いたGPSテレメの取得時刻
+  ObcTime obct_tag_for_current_gps_tlm     = aocs_manager->obct_gps_time_obs;
+  uint32_t time_from_last_gps_tlm_update   = OBCT_diff_in_msec(&obct_tag_for_last_gps_tlm, &obct_tag_for_current_gps_tlm);
+
   double reference_jday;
-  if (aocs_manager->gps_visibility == AOCS_MANAGER_GPS_VISIBLE)
+  // GPSテレメの更新頻度よりも早いインターバルで更新される場合，AOCS_MANAGER_GPS_VISIBLEとテレメ時刻両方での判定が必要
+  if ((time_from_last_gps_tlm_update > 0) && (aocs_manager->gps_visibility == AOCS_MANAGER_GPS_VISIBLE))
   {
     reference_jday = APP_TIME_SPACE_CALC_update_current_jday_ref_();
+    obct_tag_for_last_gps_tlm = obct_tag_for_current_gps_tlm;
   }
   else
   {
@@ -61,27 +67,25 @@ static void APP_TIME_SPACE_CALC_exec_(void)
 
 static double APP_TIME_SPACE_CALC_update_current_jday_ref_(void)
 {
-  uint32_t offset_time_msec  = (uint32_t)(time_space_calculator_.offset_sec * 1e3f);
   // TODO_L: 位置情報をobsからestに置き換えるタイミングで，時刻もobsからestに置き換えて，ここではestを用いる方が他との統一性の観点でベターだが，
   // 現状では，GPSR情報は全てobsに入れる流れとなっており，estに値が入らないことから，ここではobsのままにする
-  uint32_t ref_gps_time_msec = aocs_manager->current_gps_time_obs_msec + offset_time_msec;
-  uint16_t ref_gps_time_week = aocs_manager->current_gps_time_obs_week;
+  GPS_TIME_OF_WEEK ref_gps_time = aocs_manager->current_gps_time_obs;
 
   double week_of_msec = PHYSICAL_CONST_EARTH_SOLAR_DAY_s * 7.0 * 1e3;
 
   // check rollover
-  if ((double)(ref_gps_time_msec) > week_of_msec)
+  if ((double)(ref_gps_time.msec_of_week) > week_of_msec)
   {
-    ref_gps_time_msec = 0;
-    ref_gps_time_week += 1;
+    ref_gps_time.msec_of_week = 0;
+    ref_gps_time.week_number += 1;
   }
 
-  double reference_jday = TIME_SPACE_convert_gpstime_to_julian_day(ref_gps_time_week, ref_gps_time_msec);
+  ObcTime current_obct  = TMGR_get_master_clock();
+  double reference_jday = TIME_SPACE_convert_gpstime_to_julian_day(ref_gps_time);
+  reference_jday += (double)(time_space_calculator_.offset_sec) / (PHYSICAL_CONST_EARTH_SOLAR_DAY_s);
 
   // TODO_L: 位置情報をobsからestに置き換えるタイミングで，時刻もobsからestに置き換えて，ここではestを用いる
-  // 上の話と同様
-  ObcTime obct_now = aocs_manager->obct_gps_time_obs;
-  AOCS_MANAGER_set_reference_jday(reference_jday, obct_now);
+  AOCS_MANAGER_set_reference_jday(reference_jday, current_obct);
 
   return reference_jday;
 }

src/src_user/Applications/UserDefined/AOCS/OrbitEstimation/gpsr_orbit_propagator.c

@@ -113,8 +113,7 @@ C2A_MATH_ERROR GPSR_ORBIT_PROPAGATOR_calc_position(float position_eci_km[PHYSICA
     // gpsrテレメをもとに接触軌道要素を計算
     float position_eci_km_tmp[PHYSICAL_CONST_THREE_DIM];
     float velocity_eci_km_s_tmp[PHYSICAL_CONST_THREE_DIM];
-    double reference_jday = TIME_SPACE_convert_gpstime_to_julian_day(aocs_manager->current_gps_time_obs_week,
-                                                                     aocs_manager->current_gps_time_obs_msec);
+    double reference_jday = TIME_SPACE_convert_gpstime_to_julian_day(aocs_manager->current_gps_time_obs);
     for (int idx = 0; idx < PHYSICAL_CONST_THREE_DIM; idx++)
     {
       position_eci_km_tmp[idx]   = (float)aocs_manager->sat_pos_obs_eci_m[idx]   / 1000.0f;

src/src_user/Applications/UserDefined/AOCS/aocs_manager.c

@@ -109,8 +109,8 @@ static void APP_AOCS_MANAGER_init_(void)
   VECTOR3_initialize(rw_angular_velocity_rad_s, 0.0f);
   AOCS_MANAGER_set_rw_angular_velocity_rad_s(rw_angular_velocity_rad_s);
   // 時刻
-  aocs_manager_.current_gps_time_obs_msec = 0;
-  aocs_manager_.current_gps_time_obs_week = 0;
+  aocs_manager_.current_gps_time_obs.week_number  = 0;
+  aocs_manager_.current_gps_time_obs.msec_of_week = 0;
   aocs_manager_.obct_gps_time_obs = OBCT_create(0, 0, 0);
   // aocs_manager_.current_gps_time_est_msec = 0;
   // aocs_manager_.current_gps_time_est_week = 0;
@@ -465,13 +465,11 @@ AOCS_MANAGER_ERROR AOCS_MANAGER_set_rw_rotation_direction_matrix(
 }
 
 // 時刻
-AOCS_MANAGER_ERROR AOCS_MANAGER_set_current_gps_time_obs(const uint32_t current_gps_time_obs_msec,
-                                                         const uint16_t current_gps_time_obs_week,
+AOCS_MANAGER_ERROR AOCS_MANAGER_set_current_gps_time_obs(const GPS_TIME_OF_WEEK current_gps_time,
                                                          const ObcTime  obct_gps_time_obs)
 {
-  aocs_manager_.current_gps_time_obs_msec = current_gps_time_obs_msec;
-  aocs_manager_.current_gps_time_obs_week = current_gps_time_obs_week;
-  aocs_manager_.obct_gps_time_obs = obct_gps_time_obs;
+  aocs_manager_.current_gps_time_obs = current_gps_time;
+  aocs_manager_.obct_gps_time_obs    = obct_gps_time_obs;
   return AOCS_MANAGER_ERROR_OK;
 }
 

src/src_user/Applications/UserDefined/AOCS/aocs_manager.h

@@ -10,6 +10,7 @@
 #include "../../../Library/physical_constants.h"
 #include "../../../Library/quaternion.h"
 #include "../../../Library/stdint.h"
+#include "../../../Library/time_system/gps_time.h"
 #include "../../DriverInstances/di_rw0003.h"
 #include "../../DriverInstances/di_mtq_seiren.h"
 
@@ -159,14 +160,13 @@ typedef struct
   float rw_rotation_direction_matrix[AOCS_MANAGER_NUM_OF_RW][PHYSICAL_CONST_THREE_DIM]; //!< RWの回転向きをRWの個数分まとめたもの
   float rw_distribution_matrix[PHYSICAL_CONST_THREE_DIM][AOCS_MANAGER_NUM_OF_RW];       //!< RW分配行列
   // 時刻
-  uint32_t current_gps_time_obs_msec;  //!< センサで観測されたGPS時刻の秒成分 [msec]
-  uint16_t current_gps_time_obs_week;  //!< センサで観測されたGPS時刻の週成分 [week]
-  ObcTime  obct_gps_time_obs;          //!< 上記をセンサで観測したタイミングのC2Aマスタークロック [-]
+  GPS_TIME_OF_WEEK current_gps_time_obs;  //!< センサで観測されたGPS時刻
+  ObcTime  obct_gps_time_obs;             //!< 上記をセンサで観測したタイミングのC2Aマスタークロック [-]
   // uint32_t current_gps_time_est_msec;  //!< 推定系で推定されたGPS時刻の秒成分 [msec] : 22_08_24時点で使用箇所がないため，一旦コメントアウト
   // uint16_t current_gps_time_est_week;  //!< 推定系で推定されたGPS時刻の週成分 [week] : 22_08_24時点で使用箇所がないため，一旦コメントアウト
-  ObcTime  obct_gps_time_est;          //!< 上記を推定したタイミングのC2Aマスタークロック [-]
-  double   reference_jday;             //!< 慣性系計算の基準時刻として用いるユリウス日 [ユリウス日]
-  ObcTime  obct_reference;             //!< 慣性系計算の基準時刻として用いるユリウス日を更新した時のOBC時刻 [-]
+  ObcTime  obct_gps_time_est;             //!< 上記を推定したタイミングのC2Aマスタークロック [-]
+  double   reference_jday;                //!< 慣性系計算の基準時刻として用いるユリウス日 [ユリウス日]
+  ObcTime  obct_reference;                //!< 慣性系計算の基準時刻として用いるユリウス日を更新した時のOBC時刻 [-]
   // センサ状態
   AOCS_MANAGER_GPS_VISIBILITY gps_visibility;     //!< GPS情報の利用可否
   AOCS_MANAGER_SUN_VISIBILITY sun_visibility;     //!< 太陽がサンセンサから見えているかどうか
@@ -238,8 +238,7 @@ AOCS_MANAGER_ERROR AOCS_MANAGER_set_rw_angular_velocity_rad_s(const float rw_ang
 AOCS_MANAGER_ERROR AOCS_MANAGER_set_rw_rotation_direction_matrix(
   const float rw_rotation_direction_body[AOCS_MANAGER_NUM_OF_RW][PHYSICAL_CONST_THREE_DIM]);
 // 時刻
-AOCS_MANAGER_ERROR AOCS_MANAGER_set_current_gps_time_obs(const uint32_t current_gps_time_obs_msec,
-                                                         const uint16_t current_gps_time_obs_week,
+AOCS_MANAGER_ERROR AOCS_MANAGER_set_current_gps_time_obs(const GPS_TIME_OF_WEEK current_gps_time,
                                                          const ObcTime  obct_gps_time_obs);
 // 22_08_24時点で使用箇所がないため，一旦コメントアウト
 // AOCS_MANAGER_ERROR AOCS_MANAGER_set_current_gps_time_est(const uint32_t current_gps_time_est_msec,

src/src_user/Library/CMakeLists.txt

@@ -20,6 +20,7 @@ set(C2A_SRCS
   c2a_math.c
   quaternion.c
   crc8.c
+  time_system/gps_time.c
   Orbit/kepler_orbit.c
   Orbit/sgp4.c
   SignalProcess/z_filter.c

src/src_user/Library/time_space.c

@@ -14,6 +14,13 @@
 #include "matrix33.h"
 #include "vector3.h"
 
+
+//!< JulainDay at Origin of GPS Time of Week (1980/1/6 00:00)
+static const double gps_time_of_week_jday_origin_ = 2444244.5;
+
+//!< leap seconds (GPSTime - UTC)
+static double leap_seconds_ = 18.0;
+
 /**
  * @brief      日周運動を表すR行列の計算
  * @param[in]  gast_rad       : Greenwitch Apparent Sidereal Time [rad]
@@ -51,22 +58,23 @@ static C2A_MATH_ERROR TIME_SPACE_nutation_(const double julian_century_terrestri
                                                  double nutation[PHYSICAL_CONST_THREE_DIM][PHYSICAL_CONST_THREE_DIM],
                                                  double* epsilon_rad, double* delta_epsilon_rad, double* delta_psi_rad);
 
-double TIME_SPACE_convert_gpstime_to_julian_day(const uint16_t gps_time_week, const uint32_t gps_time_msec)
+
+double TIME_SPACE_convert_gpstime_to_julian_day(const GPS_TIME_OF_WEEK gps_time)
 {
-  const double kJulianDayAtGpsTimeEpoch_day = 2444244.5;  //!< JulainDay at Origin of GPSTime (1980/1/6 00:00)
-  const double kMsecToSec     = 1.0e-3;                   //!< conversion from msec to sec
+  const double kMsecToSec = 1.0e-3;                       //!< conversion from msec to sec
   const double kDayOfWeek_day = 7.0;                      //!< days of a week
   const double kSecOfWeek_sec = kDayOfWeek_day * (double)(PHYSICAL_CONST_EARTH_SOLAR_DAY_s);
 
-  double gps_time_sec = (double)(gps_time_msec) * kMsecToSec;
+  double gps_time_sec = (double)(gps_time.msec_of_week)*kMsecToSec;
 
-  if (gps_time_sec > kSecOfWeek_sec) return kJulianDayAtGpsTimeEpoch_day;
+  if (gps_time_sec > kSecOfWeek_sec) return (gps_time_of_week_jday_origin_);
 
-  // Since both GPSTime and JulianDay have no leap seconds, they are directly converted to each other.
-  double elapsed_julian_day = (double)(gps_time_week) * kDayOfWeek_day +
-                               gps_time_sec / PHYSICAL_CONST_EARTH_SOLAR_DAY_s;
+  // leap_secondsの分だけgps時刻の方が進んでいるため，leap_seconds分を差し引く．負値になり得るが，doubleにしているので許容可
+  double elapsed_julian_day_sec_part = gps_time_sec - (double)(leap_seconds_);
+  double elapsed_julian_day = (double)(gps_time.week_number)*kDayOfWeek_day +
+                               elapsed_julian_day_sec_part / (double)(PHYSICAL_CONST_EARTH_SOLAR_DAY_s);
 
-  return (kJulianDayAtGpsTimeEpoch_day + elapsed_julian_day);
+  return (gps_time_of_week_jday_origin_ + elapsed_julian_day);
 }
 
 
@@ -163,6 +171,12 @@ void TIME_SPACE_calc_sun_direction_eci(const double julian_century, float sun_di
   VECTOR3_normalize(sun_direction_eci, sun_direction_eci_unnormalize);
 }
 
+// ToDo: どこかのアプリでこれをCMDとして呼ぶ
+void TIME_SPACE_update_leap_seconds(const float leap_seconds_updated)
+{
+  leap_seconds_ = (double)(leap_seconds_updated);
+  return;
+}
 
 C2A_MATH_ERROR TIME_SPACE_convert_geodetic_to_geocentric(float* colat_rad, float* radious_m,
                                                    const double lla_rad_m[PHYSICAL_CONST_THREE_DIM])

src/src_user/Library/time_space.h

@@ -10,15 +10,15 @@
 #include "stdint.h"
 #include "physical_constants.h"
 #include "c2a_math.h"
+#include "time_system/gps_time.h"
 
 // 時刻換算関数
 /**
  * @brief  GPSTimeからJulian Dayへの変換
  * @param  gps_time_week : GPSTime週番号 [week] (should be larger than zero)
- * @param  gps_time_sec  : GPSTime週秒 [msec]   (should be in range of 0 ~ 7*24*60*60)
  * @return Julian Day [day] (returns Julian day at the origin of GPSTime when the input is out-of-range )
  */
-double TIME_SPACE_convert_gpstime_to_julian_day(const uint16_t gps_time_week, const uint32_t gps_time_msec);
+double TIME_SPACE_convert_gpstime_to_julian_day(const GPS_TIME_OF_WEEK gps_time);
 
 /**
  * @brief  Julian DayからJulian Centuryへの変換
@@ -62,6 +62,12 @@ void TIME_SPACE_calc_sun_direction_eci(const double julian_century, float sun_di
 void TIME_SPACE_trans_ned_to_ecef(float dcm_ned_to_ecef[][PHYSICAL_CONST_THREE_DIM],
                             const float lat_rad, const float lon_rad);
 
+/**
+ * @brief      うるう秒 (GPSTime - UTC) の更新
+ * @param[in]  leap_seconds_updated   : 更新後のうるう秒 [s]
+ */
+void TIME_SPACE_update_leap_seconds(const float leap_seconds_updated);
+
 /**
  * @brief      ECIからECEFへの変換 R*N*P
  *

src/src_user/Library/time_system/gps_time.c

@@ -0,0 +1,16 @@
+/**
+ * @file   gps_time.c
+ * @brief  GPS時刻構造体と定数の定義
+ */
+
+#include "gps_time.h"
+
+
+GPS_TIME_OF_WEEK GPS_TIME_OF_WEEK_create_gps_time(const uint16_t week_number, const uint32_t msec_of_week)
+{
+  GPS_TIME_OF_WEEK gps_time;
+  gps_time.week_number  = week_number;
+  gps_time.msec_of_week = msec_of_week;
+
+  return gps_time;
+}

src/src_user/Library/time_system/gps_time.h

@@ -0,0 +1,29 @@
+/**
+ * @file   gps_time.h
+ * @brief  GPS時刻構造体と定数の定義
+ */
+#ifndef GPS_TIME_H_
+#define GPS_TIME_H_
+
+#include "stdint.h"
+
+ /**
+  * @struct   GPS_TIME_OF_WEEK
+  * @brief    GPS時刻構造体
+  */
+typedef struct
+{
+  uint16_t week_number;   //!< 週番号
+  uint32_t msec_of_week;  //!< 週内の経過秒 [ミリ秒]
+} GPS_TIME_OF_WEEK;
+
+
+/**
+ * @brief      uint変数からのGPS時刻構造体生成
+ * @param[in]  week_number    : 週番号
+ * @param[in]  msec_of_week   : 週内の経過秒 [ミリ秒]
+ * @return     GPS_TIME_OF_WEEK
+ */
+GPS_TIME_OF_WEEK GPS_TIME_OF_WEEK_create_gps_time(const uint16_t week_number, const uint32_t msec_of_week);
+
+#endif