Merge pull request #309 from ut-issl/hotfix/fix_integrated_torque_cle…

…ar_timing

fix reset timing for torque integration in MTQ controller

src/src_user/Applications/UserDefined/AOCS/HardwareDependent/ActuatorControllers/mtq_seiren_controller.c

@@ -70,6 +70,7 @@ void APP_MTQ_SEIREN_CONTROLLER_init_(void)
   mtq_seiren_controller_.previous_torque_integration_obc_time = TMGR_get_master_clock();
   mtq_seiren_controller_.cross_product_error = CROSS_PRODUCT_CONTROL_ERROR_OK;
   mtq_seiren_controller_.cross_product_error_ratio = 0.0f;
+  mtq_seiren_controller_.integrator_status = MTQ_SEIREN_CONTROLLER_TORQUE_IS_INTEGRATING;
 }
 
 void APP_MTQ_SEIREN_CONTROLLER_exec_(void)
@@ -81,10 +82,16 @@ void APP_MTQ_SEIREN_CONTROLLER_exec_(void)
   case APP_AOCS_MANAGER_MAGNETIC_EXCLUSIVE_CONTROL_STATE_OBSERVE:
     // 次にMTQを出力するとき、何秒間電流を流すかを決める
     APP_MTQ_SEIREN_CONTROLLER_convert_mag_moment_to_output_duration_();
-    // 次回の出力を決めたら、トルク積分値をリセットする
-    APP_MTQ_SEIREN_CONTROLLER_reset_integrated_torque_();
+    // 指令トルクの積分は完了
+    mtq_seiren_controller_.integrator_status = MTQ_SEIREN_CONTROLLER_TORQUE_INTEGRATION_COMPLETED;
     break;
   case APP_AOCS_MANAGER_MAGNETIC_EXCLUSIVE_CONTROL_STATE_CONTROL:
+    if (mtq_seiren_controller_.integrator_status == MTQ_SEIREN_CONTROLLER_TORQUE_INTEGRATION_COMPLETED)
+    {
+      // 指令トルク積分値をリセットし、次のトルク指令に備えて積分をリスタートする
+      APP_MTQ_SEIREN_CONTROLLER_reset_integrated_torque_();
+      mtq_seiren_controller_.integrator_status = MTQ_SEIREN_CONTROLLER_TORQUE_IS_INTEGRATING;
+    }
     // APP_AOCS_MANAGER_MAGNETIC_EXCLUSIVE_CONTROL_STATE_OBSERVEのケースで計算していた出力時間の分だけMTQを出力する
     for (size_t idx = 0; idx < MTQ_SEIREN_IDX_MAX; idx++)
     {
@@ -104,6 +111,8 @@ void APP_MTQ_SEIREN_CONTROLLER_exec_(void)
     {
       MTQ_SEIREN_output(mtq_seiren_driver[idx], MTQ_SEIREN_NO_OUTPUT);
     }
+    // 消磁中も積分を続ける
+    mtq_seiren_controller_.integrator_status = MTQ_SEIREN_CONTROLLER_TORQUE_IS_INTEGRATING;
     break;
   default:
     // NOT REACHED

src/src_user/Applications/UserDefined/AOCS/HardwareDependent/ActuatorControllers/mtq_seiren_controller.h

@@ -12,6 +12,17 @@
 #include <src_user/Library/ControlUtility/cross_product_control.h>
 #include <src_user/Applications/DriverInstances/di_mtq_seiren.h>
 
+/**
+ * @struct MTQ_SEIREN_CONTROLLER_TORQUE_INTEGRATOR_STATUS
+ * @brief  指令トルクの積分状態
+ * @note   int8_tを想定
+ */
+typedef enum
+{
+  MTQ_SEIREN_CONTROLLER_TORQUE_IS_INTEGRATING = 0,   //!< 積分中
+  MTQ_SEIREN_CONTROLLER_TORQUE_INTEGRATION_COMPLETED //!< 積分完了
+} MTQ_SEIREN_CONTROLLER_TORQUE_INTEGRATOR_STATUS;
+
 /**
  * @struct MtqSeirenController
  * @brief  MTQ_SEIRENの制御に必要な情報を管理する構造体
@@ -23,8 +34,9 @@ typedef struct
   int8_t mtq_output_duty[MTQ_SEIREN_IDX_MAX];                  //!< 極性と出力時間から計算され、テレメに出力するためのMTQ出力duty比 [-100, +100] （単位：％）
 
   // 消磁中に更新された指令トルクに関する積分関連パラメータ
-  float integrated_torque_Nms[PHYSICAL_CONST_THREE_DIM]; //!< 消磁中に更新された指令トルクを積分して角運動量指令に換算するためのバッファ [Nms]
-  ObcTime  previous_torque_integration_obc_time;         //!< 前回積分した時間
+  float integrated_torque_Nms[PHYSICAL_CONST_THREE_DIM];             //!< 消磁中に更新された指令トルクを積分して角運動量指令に換算するためのバッファ [Nms]
+  MTQ_SEIREN_CONTROLLER_TORQUE_INTEGRATOR_STATUS integrator_status;  //!< 指令トルクの積分状態
+  ObcTime  previous_torque_integration_obc_time;                     //!< 前回積分した時間
 
   CROSS_PRODUCT_CONTROL_ERROR cross_product_error;    //!< CrossProductの実行エラー記録 (TODO_L: 残すか要検討，デバッグ用)
   float cross_product_error_ratio;