Merge pull request #308 from ut-issl/feature/add-lpf-for-ang-vel

Add LPF for angular velocity calculation

database/CMD_DB/AOBC_CMD_DB_CMD_DB.csv

@@ -303,8 +303,9 @@ Comment,,,,,Type,Description,Type,Description,Type,Description,Type,Description,
 ,APP_QI_SET_NEXT_TARGET_QUATERNION_JDAY,AOBC,0x00DF,5,double,attitude_changed_jday,float,q_i2t_vec_0,float,q_i2t_vec_1,float,q_i2t_vec_2,float,q_i2t_scalar,,,,,目標Quaternionをユリウス日で設定する,
 ,APP_QI_RESET_TARGET_QUATERNION,AOBC,0x00E0,0,,,,,,,,,,,,,,,目標Quaternionをリセットする,
 *,APP_QI_ENABLE,AOBC,,1,uint8_t,DISABLE=0; ENABLE=1,,,,,,,,,,,,,内挿計算した目標Quaternionをaocs_managerに反映するか設定する,
+,APP_TARGET_ATT_CALC_SET_LPF_ANGULAR_VELOCITY,AOBC,0x00E1,2,float,sampling freq Hz,float,cut off freq Hz,,,,,,,,,,,目標姿勢計算時に計算される角速度に対するLPF設定,
 **,環境演算,,,,,,,,,,,,,,,,,,,
-,APP_TIME_SPACE_CALC_SET_OFFSET_TIME,AOBC,0x00E1,1,float,offset_time [sec],,,,,,,,,,,,,計算結果を使用する時刻と実行時刻の間のオフセット設定,
+,APP_TIME_SPACE_CALC_SET_OFFSET_TIME,AOBC,0x00E2,1,float,offset_time [sec],,,,,,,,,,,,,計算結果を使用する時刻と実行時刻の間のオフセット設定,
 * MANAGER,AOCS MANAGER関連,,,,,,,,,,,,,,,,,,,
 **,衛星構造,,,,,,,,,,,,,,,,,,,
 ,APP_AOCS_MANAGER_SET_MASS,AOBC,0x00E3,1,float,mass_kg,,,,,,,,,,,,,衛星質量を設定する,
@@ -1001,4 +1002,3 @@ Comment,,,,,Type,Description,Type,Description,Type,Description,Type,Description,
 ,,,,,,,,,,,,,,,,,,,,
 ,,,,,,,,,,,,,,,,,,,,
 ,,,,,,,,,,,,,,,,,,,,
-,,,,,,,,,,,,,,,,,,,,

database/TLM_DB/AOBC_TLM_DB_AOBC_CONTROL.csv

@@ -83,8 +83,8 @@ Comment,TLM Entry,Onboard Software Info.,,Extraction Info.,,,,Conversion Info.,,
 ,TARGET.INTERPOLATION.PREVIOUS_QUATERNION_TARGET_I2T_Y,float,(float)(quaternion_interpolator->previous_quaternion_target_i2t.vector_part[1]),PACKET,=R[-1]C+INT((R[-1]C[1]+R[-1]C[2])/8),=MOD((R[-1]C+R[-1]C[1])@@8),=IF(OR(EXACT(RC[-5]@@"uint8_t")@@EXACT(RC[-5]@@"int8_t"))@@8@@IF(OR(EXACT(RC[-5]@@"uint16_t")@@EXACT(RC[-5]@@"int16_t"))@@16@@IF(OR(EXACT(RC[-5]@@"uint32_t")@@EXACT(RC[-5]@@"int32_t")@@EXACT(RC[-5]@@"float"))@@32@@IF(EXACT(RC[-5]@@"double")@@64)))),NONE,,,,,,,,前回の目標Quaternion I2T Y,
 ,TARGET.INTERPOLATION.PREVIOUS_QUATERNION_TARGET_I2T_Z,float,(float)(quaternion_interpolator->previous_quaternion_target_i2t.vector_part[2]),PACKET,=R[-1]C+INT((R[-1]C[1]+R[-1]C[2])/8),=MOD((R[-1]C+R[-1]C[1])@@8),=IF(OR(EXACT(RC[-5]@@"uint8_t")@@EXACT(RC[-5]@@"int8_t"))@@8@@IF(OR(EXACT(RC[-5]@@"uint16_t")@@EXACT(RC[-5]@@"int16_t"))@@16@@IF(OR(EXACT(RC[-5]@@"uint32_t")@@EXACT(RC[-5]@@"int32_t")@@EXACT(RC[-5]@@"float"))@@32@@IF(EXACT(RC[-5]@@"double")@@64)))),NONE,,,,,,,,前回の目標Quaternion I2T Z,
 ,TARGET.INTERPOLATION.PREVIOUS_QUATERNION_TARGET_I2T_W,float,(float)(quaternion_interpolator->previous_quaternion_target_i2t.scalar_part),PACKET,=R[-1]C+INT((R[-1]C[1]+R[-1]C[2])/8),=MOD((R[-1]C+R[-1]C[1])@@8),=IF(OR(EXACT(RC[-5]@@"uint8_t")@@EXACT(RC[-5]@@"int8_t"))@@8@@IF(OR(EXACT(RC[-5]@@"uint16_t")@@EXACT(RC[-5]@@"int16_t"))@@16@@IF(OR(EXACT(RC[-5]@@"uint32_t")@@EXACT(RC[-5]@@"int32_t")@@EXACT(RC[-5]@@"float"))@@32@@IF(EXACT(RC[-5]@@"double")@@64)))),NONE,,,,,,,,前回の目標Quaternion I2T W,
-,,,,,,,,,,,,,,,,,
-,,,,,,,,,,,,,,,,,
+,TARGET.LPF_ANGULAR_VELOCITY.SAMPLING_Hz,float,(float)(target_attitude_calculator->sampling_freq_Hz),PACKET,=R[-1]C+INT((R[-1]C[1]+R[-1]C[2])/8),=MOD((R[-1]C+R[-1]C[1])@@8),=IF(OR(EXACT(RC[-5]@@"uint8_t")@@EXACT(RC[-5]@@"int8_t"))@@8@@IF(OR(EXACT(RC[-5]@@"uint16_t")@@EXACT(RC[-5]@@"int16_t"))@@16@@IF(OR(EXACT(RC[-5]@@"uint32_t")@@EXACT(RC[-5]@@"int32_t")@@EXACT(RC[-5]@@"float"))@@32@@IF(EXACT(RC[-5]@@"double")@@64)))),NONE,,,,,,,,Angular velocity LPF sampling frequency Hz,
+,TARGET.LPF_ANGULAR_VELOCITY.CUTOFF_Hz,float,(float)(target_attitude_calculator->cut_off_freq_lpf_1st_Hz),PACKET,=R[-1]C+INT((R[-1]C[1]+R[-1]C[2])/8),=MOD((R[-1]C+R[-1]C[1])@@8),=IF(OR(EXACT(RC[-5]@@"uint8_t")@@EXACT(RC[-5]@@"int8_t"))@@8@@IF(OR(EXACT(RC[-5]@@"uint16_t")@@EXACT(RC[-5]@@"int16_t"))@@16@@IF(OR(EXACT(RC[-5]@@"uint32_t")@@EXACT(RC[-5]@@"int32_t")@@EXACT(RC[-5]@@"float"))@@32@@IF(EXACT(RC[-5]@@"double")@@64)))),NONE,,,,,,,,Angular velocity LPF cutoff frequency Hz,
 ,,,,,,,,,,,,,,,,,
 ,,,,,,,,,,,,,,,,,
 ,,,,,,,,,,,,,,,,,

database/TLM_DB/calced_data/AOBC_TLM_DB_AOBC_CONTROL.csv

@@ -83,8 +83,8 @@ Comment,TLM Entry,Onboard Software Info.,,Extraction Info.,,,,Conversion Info.,,
 ,TARGET.INTERPOLATION.PREVIOUS_QUATERNION_TARGET_I2T_Y,float,(float)(quaternion_interpolator->previous_quaternion_target_i2t.vector_part[1]),PACKET,221,0,32,NONE,,,,,,,,前回の目標Quaternion I2T Y,
 ,TARGET.INTERPOLATION.PREVIOUS_QUATERNION_TARGET_I2T_Z,float,(float)(quaternion_interpolator->previous_quaternion_target_i2t.vector_part[2]),PACKET,225,0,32,NONE,,,,,,,,前回の目標Quaternion I2T Z,
 ,TARGET.INTERPOLATION.PREVIOUS_QUATERNION_TARGET_I2T_W,float,(float)(quaternion_interpolator->previous_quaternion_target_i2t.scalar_part),PACKET,229,0,32,NONE,,,,,,,,前回の目標Quaternion I2T W,
-,,,,,,,,,,,,,,,,,
-,,,,,,,,,,,,,,,,,
+,TARGET.LPF_ANGULAR_VELOCITY.SAMPLING_Hz,float,(float)(target_attitude_calculator->sampling_freq_Hz),PACKET,233,0,32,NONE,,,,,,,,Angular velocity LPF sampling frequency Hz,
+,TARGET.LPF_ANGULAR_VELOCITY.CUTOFF_Hz,float,(float)(target_attitude_calculator->cut_off_freq_lpf_1st_Hz),PACKET,237,0,32,NONE,,,,,,,,Angular velocity LPF cutoff frequency Hz,
 ,,,,,,,,,,,,,,,,,
 ,,,,,,,,,,,,,,,,,
 ,,,,,,,,,,,,,,,,,

src/src_user/Applications/UserDefined/AOCS/TargetAttitude/target_attitude_calculator.c

@@ -15,12 +15,14 @@
 #include <src_user/Applications/UserDefined/AOCS/aocs_manager.h>
 #include <src_user/Applications/app_registry.h>
 #include <src_user/Library/vector3.h>
+#include <src_user/Library/SignalProcess/z_filter.h>
 
 // SatelliteParameters
 #include <src_user/Settings/SatelliteParameters/attitude_target_parameters.h>
 
 static TargetAttitudeCalculator        target_attitude_calculator_;
 const  TargetAttitudeCalculator* const target_attitude_calculator = &target_attitude_calculator_;
+static ZFilter APP_TARGET_ATT_CALC_lpf_1st_ang_vel_[PHYSICAL_CONST_THREE_DIM];     //!< 角速度LPF
 
 static void APP_TARGET_ATT_CALC_init_(void);
 static void APP_TARGET_ATT_CALC_exec_(void);
@@ -52,6 +54,16 @@ static void APP_TARGET_ATT_CALC_init_(void)
   target_attitude_calculator_.mode = ATTITUDE_TARGET_PARAMETERS_mode;
   target_attitude_calculator_.quaternion_target_i2t = ATTITUDE_TARGET_PARAMETERS_quaternion_target_i2t;
   target_attitude_calculator_.is_enabled = 0;
+
+  // 角速度計算LPF
+  target_attitude_calculator_.sampling_freq_Hz = 10.0f;
+  target_attitude_calculator_.cut_off_freq_lpf_1st_Hz = 0.05f;
+  for (uint8_t axis_id = 0; axis_id < PHYSICAL_CONST_THREE_DIM; axis_id++)
+  {
+    Z_FILTER_init_as_lpf_1st(&APP_TARGET_ATT_CALC_lpf_1st_ang_vel_[axis_id],
+                             target_attitude_calculator_.sampling_freq_Hz,
+                             target_attitude_calculator_.cut_off_freq_lpf_1st_Hz);
+  }
 }
 
 static void APP_TARGET_ATT_CALC_exec_(void)
@@ -112,7 +124,8 @@ static C2A_MATH_ERROR APP_TARGET_ATT_CALC_calc_target_angular_velocity_(void)
   // Quaternion差より角速度計算
   float time_diff_sec = (float)OBCT_diff_in_sec(&target_attitude_calculator_.obctime, &obctime_current);
   // TODO: 時間アサーションを入れるかどうか検討する
-  C2A_MATH_ERROR ret = QUATERNION_calc_angular_velocity(target_attitude_calculator_.ang_vel_target_body_rad_s,
+  float ang_vel_target_body_rad_s[PHYSICAL_CONST_THREE_DIM];
+  C2A_MATH_ERROR ret = QUATERNION_calc_angular_velocity(ang_vel_target_body_rad_s,
                                                         q_target_previous,
                                                         q_target_current,
                                                         time_diff_sec);
@@ -121,7 +134,12 @@ static C2A_MATH_ERROR APP_TARGET_ATT_CALC_calc_target_angular_velocity_(void)
     EL_record_event(EL_GROUP_CALCULATION_ERROR, (uint32_t)AR_APP_TARGET_ATTITUDE_CALCULATOR, EL_ERROR_LEVEL_LOW, (uint32_t)ret);
     return ret;
   }
-
+  // LPF
+  for (uint8_t axis_id = 0; axis_id < PHYSICAL_CONST_THREE_DIM; axis_id++)
+  {
+    target_attitude_calculator_.ang_vel_target_body_rad_s[axis_id] =
+      Z_FILTER_calc_output(&APP_TARGET_ATT_CALC_lpf_1st_ang_vel_[axis_id], ang_vel_target_body_rad_s[axis_id]);
+  }
   // 次回計算のために現在の値を代入
   target_attitude_calculator_.quaternion_target_i2t_previous = target_attitude_calculator_.quaternion_target_i2t;
   target_attitude_calculator_.obctime = obctime_current;
@@ -162,4 +180,23 @@ CCP_CmdRet Cmd_APP_TARGET_ATT_CALC_ENABLE(const CommonCmdPacket* packet)
   return CCP_make_cmd_ret_without_err_code(CCP_EXEC_SUCCESS);
 }
 
+CCP_CmdRet Cmd_APP_TARGET_ATT_CALC_SET_LPF_ANGULAR_VELOCITY(const CommonCmdPacket* packet)
+{
+  float sampling_Hz = CCP_get_param_from_packet(packet, 0, float);
+  if (sampling_Hz <= 0.0f)
+  {
+    return CCP_make_cmd_ret_without_err_code(CCP_EXEC_ILLEGAL_PARAMETER);
+  }
+  float cutoff_Hz = CCP_get_param_from_packet(packet, 1, float);
+  if (cutoff_Hz <= 0.0f)
+  {
+    return CCP_make_cmd_ret_without_err_code(CCP_EXEC_ILLEGAL_PARAMETER);
+  }
+
+  target_attitude_calculator_.sampling_freq_Hz = sampling_Hz;
+  target_attitude_calculator_.cut_off_freq_lpf_1st_Hz = cutoff_Hz;
+
+  return CCP_make_cmd_ret_without_err_code(CCP_EXEC_SUCCESS);
+}
+
 #pragma section

src/src_user/Applications/UserDefined/AOCS/TargetAttitude/target_attitude_calculator.h

@@ -34,6 +34,8 @@ typedef struct
   float ang_vel_target_body_rad_s[PHYSICAL_CONST_THREE_DIM]; //!< 目標角速度
   ObcTime obctime;                                           //!< 目標角速度計算用ObcTime
   uint8_t is_enabled;                                        //!< 目標Quaternionをaocs_managerにセットするか否か
+  float sampling_freq_Hz;                                    //!< 角速度LPFサンプリング周期 [Hz]
+  float cut_off_freq_lpf_1st_Hz;                             //!< 角速度LPFカットオフ周波数 [Hz]
 } TargetAttitudeCalculator;
 
 extern const TargetAttitudeCalculator* const target_attitude_calculator;
@@ -47,5 +49,6 @@ AppInfo APP_TARGET_ATT_CALC_create_app(void);
 
 CCP_CmdRet Cmd_APP_TARGET_ATT_CALC_SET_MODE(const CommonCmdPacket* packet);
 CCP_CmdRet Cmd_APP_TARGET_ATT_CALC_ENABLE(const CommonCmdPacket* packet);
+CCP_CmdRet Cmd_APP_TARGET_ATT_CALC_SET_LPF_ANGULAR_VELOCITY(const CommonCmdPacket* packet);
 
 #endif

src/src_user/TlmCmd/command_definitions.c

@@ -228,6 +228,7 @@ void CA_load_cmd_table(CA_CmdInfo cmd_table[CA_MAX_CMDS])
   cmd_table[Cmd_CODE_APP_QI_SET_PREV_TARGET_QUATERNION].cmd_func = Cmd_APP_QI_SET_PREV_TARGET_QUATERNION;
   cmd_table[Cmd_CODE_APP_QI_SET_NEXT_TARGET_QUATERNION_JDAY].cmd_func = Cmd_APP_QI_SET_NEXT_TARGET_QUATERNION_JDAY;
   cmd_table[Cmd_CODE_APP_QI_RESET_TARGET_QUATERNION].cmd_func = Cmd_APP_QI_RESET_TARGET_QUATERNION;
+  cmd_table[Cmd_CODE_APP_TARGET_ATT_CALC_SET_LPF_ANGULAR_VELOCITY].cmd_func = Cmd_APP_TARGET_ATT_CALC_SET_LPF_ANGULAR_VELOCITY;
   cmd_table[Cmd_CODE_APP_TIME_SPACE_CALC_SET_OFFSET_TIME].cmd_func = Cmd_APP_TIME_SPACE_CALC_SET_OFFSET_TIME;
   cmd_table[Cmd_CODE_APP_AOCS_MANAGER_SET_MASS].cmd_func = Cmd_APP_AOCS_MANAGER_SET_MASS;
   cmd_table[Cmd_CODE_APP_AOCS_MANAGER_SET_INERTIA_TENSOR].cmd_func = Cmd_APP_AOCS_MANAGER_SET_INERTIA_TENSOR;
@@ -699,6 +700,8 @@ void CA_load_cmd_table(CA_CmdInfo cmd_table[CA_MAX_CMDS])
   cmd_table[Cmd_CODE_APP_QI_SET_NEXT_TARGET_QUATERNION_JDAY].param_size_infos[1].packed_info.bit.first = CA_PARAM_SIZE_TYPE_4BYTE;
   cmd_table[Cmd_CODE_APP_QI_SET_NEXT_TARGET_QUATERNION_JDAY].param_size_infos[1].packed_info.bit.second = CA_PARAM_SIZE_TYPE_4BYTE;
   cmd_table[Cmd_CODE_APP_QI_SET_NEXT_TARGET_QUATERNION_JDAY].param_size_infos[2].packed_info.bit.first = CA_PARAM_SIZE_TYPE_4BYTE;
+  cmd_table[Cmd_CODE_APP_TARGET_ATT_CALC_SET_LPF_ANGULAR_VELOCITY].param_size_infos[0].packed_info.bit.first = CA_PARAM_SIZE_TYPE_4BYTE;
+  cmd_table[Cmd_CODE_APP_TARGET_ATT_CALC_SET_LPF_ANGULAR_VELOCITY].param_size_infos[0].packed_info.bit.second = CA_PARAM_SIZE_TYPE_4BYTE;
   cmd_table[Cmd_CODE_APP_TIME_SPACE_CALC_SET_OFFSET_TIME].param_size_infos[0].packed_info.bit.first = CA_PARAM_SIZE_TYPE_4BYTE;
   cmd_table[Cmd_CODE_APP_AOCS_MANAGER_SET_MASS].param_size_infos[0].packed_info.bit.first = CA_PARAM_SIZE_TYPE_4BYTE;
   cmd_table[Cmd_CODE_APP_AOCS_MANAGER_SET_INERTIA_TENSOR].param_size_infos[0].packed_info.bit.first = CA_PARAM_SIZE_TYPE_4BYTE;

src/src_user/TlmCmd/command_definitions.h

@@ -226,7 +226,8 @@ typedef enum
   Cmd_CODE_APP_QI_SET_PREV_TARGET_QUATERNION = 0x00DE,
   Cmd_CODE_APP_QI_SET_NEXT_TARGET_QUATERNION_JDAY = 0x00DF,
   Cmd_CODE_APP_QI_RESET_TARGET_QUATERNION = 0x00E0,
-  Cmd_CODE_APP_TIME_SPACE_CALC_SET_OFFSET_TIME = 0x00E1,
+  Cmd_CODE_APP_TARGET_ATT_CALC_SET_LPF_ANGULAR_VELOCITY = 0x00E1,
+  Cmd_CODE_APP_TIME_SPACE_CALC_SET_OFFSET_TIME = 0x00E2,
   Cmd_CODE_APP_AOCS_MANAGER_SET_MASS = 0x00E3,
   Cmd_CODE_APP_AOCS_MANAGER_SET_INERTIA_TENSOR = 0x00E4,
   Cmd_CODE_APP_AOCS_MANAGER_SET_RMM = 0x00E5,

src/src_user/TlmCmd/telemetry_definitions.c

@@ -2120,7 +2120,7 @@ static TF_TLM_FUNC_ACK Tlm_AOBC_FRAME_TRANSFORMATION_(uint8_t* packet, uint16_t*
 
 static TF_TLM_FUNC_ACK Tlm_AOBC_CONTROL_(uint8_t* packet, uint16_t* len, uint16_t max_len)
 {
-  if (233 > max_len) return TF_TLM_FUNC_ACK_TOO_SHORT_LEN;
+  if (241 > max_len) return TF_TLM_FUNC_ACK_TOO_SHORT_LEN;
 
 #ifndef BUILD_SETTINGS_FAST_BUILD
   TF_copy_float(&packet[26], (float)(bdot->control_gain[0]));
@@ -2186,9 +2186,11 @@ static TF_TLM_FUNC_ACK Tlm_AOBC_CONTROL_(uint8_t* packet, uint16_t* len, uint16_
   TF_copy_float(&packet[221], (float)(quaternion_interpolator->previous_quaternion_target_i2t.vector_part[1]));
   TF_copy_float(&packet[225], (float)(quaternion_interpolator->previous_quaternion_target_i2t.vector_part[2]));
   TF_copy_float(&packet[229], (float)(quaternion_interpolator->previous_quaternion_target_i2t.scalar_part));
+  TF_copy_float(&packet[233], (float)(target_attitude_calculator->sampling_freq_Hz));
+  TF_copy_float(&packet[237], (float)(target_attitude_calculator->cut_off_freq_lpf_1st_Hz));
 #endif
 
-  *len = 233;
+  *len = 241;
   return TF_TLM_FUNC_ACK_SUCCESS;
 }