diff --git a/database/CMD_DB/AOBC_CMD_DB_CMD_DB.csv b/database/CMD_DB/AOBC_CMD_DB_CMD_DB.csv index 3e3af3a8..56b49a65 100644 --- a/database/CMD_DB/AOBC_CMD_DB_CMD_DB.csv +++ b/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, ,,,,,,,,,,,,,,,,,,,, ,,,,,,,,,,,,,,,,,,,, ,,,,,,,,,,,,,,,,,,,, -,,,,,,,,,,,,,,,,,,,, diff --git a/database/TLM_DB/AOBC_TLM_DB_AOBC_CONTROL.csv b/database/TLM_DB/AOBC_TLM_DB_AOBC_CONTROL.csv index b003b156..1f0001fb 100644 --- a/database/TLM_DB/AOBC_TLM_DB_AOBC_CONTROL.csv +++ b/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, ,,,,,,,,,,,,,,,,, ,,,,,,,,,,,,,,,,, ,,,,,,,,,,,,,,,,, diff --git a/database/TLM_DB/calced_data/AOBC_TLM_DB_AOBC_CONTROL.csv b/database/TLM_DB/calced_data/AOBC_TLM_DB_AOBC_CONTROL.csv index 2131e962..d04a4d81 100644 --- a/database/TLM_DB/calced_data/AOBC_TLM_DB_AOBC_CONTROL.csv +++ b/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, ,,,,,,,,,,,,,,,,, ,,,,,,,,,,,,,,,,, ,,,,,,,,,,,,,,,,, diff --git a/src/src_user/Applications/UserDefined/AOCS/TargetAttitude/target_attitude_calculator.c b/src/src_user/Applications/UserDefined/AOCS/TargetAttitude/target_attitude_calculator.c index 7abe52fe..665fbd9d 100644 --- a/src/src_user/Applications/UserDefined/AOCS/TargetAttitude/target_attitude_calculator.c +++ b/src/src_user/Applications/UserDefined/AOCS/TargetAttitude/target_attitude_calculator.c @@ -15,12 +15,14 @@ #include #include #include +#include // SatelliteParameters #include 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 diff --git a/src/src_user/Applications/UserDefined/AOCS/TargetAttitude/target_attitude_calculator.h b/src/src_user/Applications/UserDefined/AOCS/TargetAttitude/target_attitude_calculator.h index ee05e19a..ba6f6d4a 100644 --- a/src/src_user/Applications/UserDefined/AOCS/TargetAttitude/target_attitude_calculator.h +++ b/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 diff --git a/src/src_user/TlmCmd/command_definitions.c b/src/src_user/TlmCmd/command_definitions.c index 053bbe2c..6176109f 100644 --- a/src/src_user/TlmCmd/command_definitions.c +++ b/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; diff --git a/src/src_user/TlmCmd/command_definitions.h b/src/src_user/TlmCmd/command_definitions.h index 51dea42b..7945c8fe 100644 --- a/src/src_user/TlmCmd/command_definitions.h +++ b/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, diff --git a/src/src_user/TlmCmd/telemetry_definitions.c b/src/src_user/TlmCmd/telemetry_definitions.c index c1b450ae..6236b4e2 100644 --- a/src/src_user/TlmCmd/telemetry_definitions.c +++ b/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; }