Merge pull request #81 from ut-issl/feature/satellite-parameters

SatelliteParametersの追加
ut-issl · Jul 10, 2023 · 8566e00 · 8566e00
2 parents 66c2ae1 + 06b571f
commit 8566e00
Show file tree

Hide file tree

Showing 229 changed files with 3,266 additions and 1,177 deletions.
diff --git a/C2A_AOBC.vcxproj b/C2A_AOBC.vcxproj
@@ -309,7 +309,20 @@
     <ClCompile Include="src\src_user\Settings\Modes\Transitions\sl_rough_sun_pointing.c" />
     <ClCompile Include="src\src_user\Settings\Modes\Transitions\sl_rough_three_axis.c" />
     <ClCompile Include="src\src_user\Settings\Modes\Transitions\sl_rough_three_axis_rw.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\attitude_control_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\attitude_determination_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\attitude_target_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\component_selector_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\fdir_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\ina260_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\mpu9250_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\nanossoc_d60_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\oem7600_parameters.c" />
     <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\orbit_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\rm3100_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\rw0003_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\sagitta_parameters.c" />
+    <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\stim210_parameters.c" />
     <ClCompile Include="src\src_user\Settings\SatelliteParameters\Sample\structure_parameters.c" />
     <ClCompile Include="src\src_user\Settings\System\EventHandlerRules\event_handler_rules.c" />
     <ClCompile Include="src\src_user\Settings\System\EventHandlerRules\event_handler_rule_hw_oc.c" />

diff --git a/Examples/C2A_AOBC_EXAMPLE.vcxproj b/Examples/C2A_AOBC_EXAMPLE.vcxproj
@@ -316,6 +316,20 @@
     <ClCompile Include="src_aobc\src\src_user\Settings\System\EventHandlerRules\event_handler_rule_sw_oc.c" />
     <ClCompile Include="src_aobc\src\src_user\Settings\System\EventHandlerRules\event_handler_rule_tlmcmd.c" />
     <ClCompile Include="src_aobc\src\src_user\Settings\System\event_logger_settings.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\attitude_control_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\attitude_determination_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\attitude_target_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\component_selector_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\fdir_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\ina260_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\mpu9250_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\nanossoc_d60_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\oem7600_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\orbit_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\rm3100_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\rw0003_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\sagitta_parameters.c" />
+    <ClCompile Include="src_aobc_example\Settings\SatelliteParameters\Sample\stim210_parameters.c" />
   </ItemGroup>
   <ItemGroup>
     <None Include="c2a_aobc_example.ino" />

diff --git a/Examples/CMakeLists.txt b/Examples/CMakeLists.txt
@@ -19,6 +19,8 @@ option(USE_SCI_COM_UART "Use SCI_COM_UART" OFF)
 
 set(C2A_CORE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src_aobc/src/src_core)
 set(C2A_USER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src_aobc/src/src_user)
+set(C2A_SATELLITE_PARAMETERS_DIR ../../../../src_aobc_example/Settings/SatelliteParameters/) # relative path from Settings
+add_definitions(-DUSER_DEFINED_I2C_ADDRESS)
 
 set(USE_ALL_C2A_CORE_APPS OFF)
 set(USE_ALL_C2A_CORE_TEST_APPS OFF)

diff --git a/Examples/src_aobc_example/Settings/SatelliteParameters/attitude_control_parameters.c b/Examples/src_aobc_example/Settings/SatelliteParameters/attitude_control_parameters.c
@@ -0,0 +1,93 @@
+/**
+ * @file  attitude_control_parameters.c
+ * @brief 姿勢制御関連の衛星固有パラメータを管理する
+ */
+
+#include <src_user/Settings/SatelliteParameters/attitude_control_parameters.h>
+#include <src_user/Settings/SatelliteParameters/parameter_setting_macro.h>
+
+// Bdot
+const float ATTITUDE_CONTROL_PARAMETERS_bdot_control_gain[PHYSICAL_CONST_THREE_DIM] = { -0.1f, -0.1f, -0.1f };
+const uint32_t ATTITUDE_CONTROL_PARAMETERS_bdot_minimum_time_derivative_step_ms = 100;
+const uint32_t ATTITUDE_CONTROL_PARAMETERS_bdot_mtq_output_time_length_ms = 1000;
+
+// Sun Pointing
+const SUN_POINTING_AXIS_ID ATTITUDE_CONTROL_PARAMETERS_sun_pointing_axis_id = SUN_POINTING_AXIS_ID_Z;
+// Gain
+const PidGains ATTITUDE_CONTROL_PARAMETERS_sun_pointing_attitude_gains_body_x = { 5.0e-5f, 0.0f, 0.0f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_sun_pointing_attitude_gains_body_y = { 10.0e-5f, 0.0f, 0.0f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_sun_pointing_attitude_gains_body_z = { 0.0f, 0.0f, 0.0f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_sun_pointing_attitude_rate_gains_body_x = { 1.0e-2f, 0.0f, 0.0f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_sun_pointing_attitude_rate_gains_body_y = { 2.0e-2f, 0.0f, 0.0f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_sun_pointing_attitude_rate_gains_body_z = { 7.0e-3f, 0.0f, 2.0e-2f };
+// MTQ control settings
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_max_direct_feedback_angle_rad = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(20.0f);
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_max_direct_feedback_rate_rad_s = 1.6e-3f;
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_mtq_allowable_error_ratio_transient = 0.6f;
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_correction_gain_transient = 0.0f;
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_mtq_allowable_error_ratio_stable = 0.8f;
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_correction_gain_stable = 0.25f;
+// Integral control setting
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_max_integral_angle_rad = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(20.0f);
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_max_angle_to_run_integral_control_rad = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(30.0f);
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_integral_control_permission_angle_rad = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(40.0f);
+// Spin control setting
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_acceptable_angle_error_to_spin_up_rad = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(30.0f);
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_spin_rate_around_sun_rad_s = 1.4e-2f;
+// Low Pass Filter setting
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_lpf_sample_freq_Hz = 10.0f;
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_lpf_trq_cutoff_freq_Hz = 0.10f;
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_lpf_trq_damping_factor = 1.0f;
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_lpf_trq_cutoff_freq_spin_axis_Hz = 0.03f;
+const float ATTITUDE_CONTROL_PARAMETERS_sun_pointing_lpf_spin_rate_cutoff_freq_Hz = 5e-4f;
+
+// Three Axis Control with MTQ
+// Gain
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_mtq_attitude_gains_body_x = { 1.2e-4f, 0.0f, 0.0f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_mtq_attitude_gains_body_y = { 2.0f * 1.2e-4f, 0.0f, 0.0f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_mtq_attitude_gains_body_z = { 1.5f * 1.2e-4f, 0.0f, 0.0f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_mtq_attitude_rate_gains_body_x = { 2.5e-2f, 0.0f, 0.0f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_mtq_attitude_rate_gains_body_y = { 2.0f * 2.5e-2f, 0.0f, 0.0f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_mtq_attitude_rate_gains_body_z = { 1.5f * 2.5e-2f, 0.0f, 0.0f };
+// MTQ control settings
+// 下記の値はISS軌道を想定した値, 変更時は，HW側の出力最大値にかからない様に留意しながら，ゲインと組み合わせて調整すること．
+const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_max_direct_feedback_angle_rad = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(18.0f);
+const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_max_direct_feedback_rate_rad_s = 5.0e-3f;
+const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_mtq_allowable_error_ratio_transient = 0.6f;
+const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_correction_gain_transient = 0.0f;
+const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_mtq_allowable_error_ratio_stable = 0.6f;
+const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_correction_gain_stable = 0.1f;
+const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_acceptable_angle_error_as_stable_rad = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(20.0f);
+// Integral control setting
+extern const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_max_integral_angle_rad = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(40.0f);
+extern const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_max_angle_to_run_integral_control_rad = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(30.0f);
+// Output torque Low Pass Filter(LPF) setting
+extern const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_lpf_sample_freq_Hz = 10.0f;
+extern const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_lpf_trq_cutoff_freq_Hz = 0.3f;
+extern const float ATTITUDE_CONTROL_PARAMETERS_tac_mtq_lpf_trq_damping_factor = 1.0f;
+
+// Three Axis Control with MTQ
+// Gain
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_rw_attitude_gains_body_x = { 1.0e-3f, 0.0f, 1.0e-3f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_rw_attitude_gains_body_y = { 2.0f * 1.0e-3f, 0.0f, 2.0f * 1.0e-3f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_rw_attitude_gains_body_z = { 2.0f * 1.0e-3f, 0.0f, 2.0f * 1.0e-3f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_rw_attitude_rate_gains_body_x = { 7.0e-2f, 0.0f, 2.0e-3f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_rw_attitude_rate_gains_body_y = { 2.0f * 7.0e-2f, 0.0f, 2.0f * 2.0e-3f };
+const PidGains ATTITUDE_CONTROL_PARAMETERS_tac_rw_attitude_rate_gains_body_z = { 2.0f * 7.0e-2f, 0.0f, 2.0f * 2.0e-3f };
+
+// Unloading
+const float ATTITUDE_CONTROL_PARAMETERS_unloading_angular_velocity_upper_threshold_rad_s = PARAMETER_SETTING_MACRO_RPM_TO_RADIAN_SEC(7000.0f);
+const float ATTITUDE_CONTROL_PARAMETERS_unloading_angular_velocity_lower_threshold_rad_s = PARAMETER_SETTING_MACRO_RPM_TO_RADIAN_SEC(-7000.0f);
+const float ATTITUDE_CONTROL_PARAMETERS_unloading_angular_velocity_target_rad_s = PARAMETER_SETTING_MACRO_RPM_TO_RADIAN_SEC(0.0f);
+const float ATTITUDE_CONTROL_PARAMETERS_unloading_control_gain = -1.0e-7f;
+const APP_UNLOADING_EXEC ATTITUDE_CONTROL_PARAMETERS_unloading_exec_is_enable = APP_UNLOADING_EXEC_DISABLE;
+
+// Control Torques
+const AOCS_MANAGER_CONSTANT_TORQUE_PERMISSION ATTITUDE_CONTROL_PARAMETERS_constant_torque_permission = AOCS_MANAGER_CONSTANT_TORQUE_DISABLE;
+const float ATTITUDE_CONTROL_PARAMETERS_constant_torque_body_Nm[PHYSICAL_CONST_THREE_DIM] = { 0.0f, 0.0f, 0.0f };
+const float ATTITUDE_CONTROL_PARAMETERS_internal_torque_max_body_Nm[PHYSICAL_CONST_THREE_DIM] = { 5.0e-3f, 5.0e-3f, 5.0e-3f };
+const float ATTITUDE_CONTROL_PARAMETERS_external_torque_max_body_Nm[PHYSICAL_CONST_THREE_DIM] = { 3.0e-4f, 3.0e-4f, 3.0e-4f };
+
+// Target Setting parameters
+const float ATTITUDE_CONTROL_PARAMETERS_limit_angular_velocity_rad_s = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(0.8f);
+const float ATTITUDE_CONTROL_PARAMETERS_limit_maneuver_angle_rad = PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(30.0f);
diff --git a/Examples/src_aobc_example/Settings/SatelliteParameters/attitude_determination_parameters.c b/Examples/src_aobc_example/Settings/SatelliteParameters/attitude_determination_parameters.c
@@ -0,0 +1,42 @@
+/**
+ * @file  attitude_determination_parameters.c
+ * @brief 姿勢決定系に関する衛星固有パラメータを管理する
+ */
+
+#include <src_user/Settings/SatelliteParameters/attitude_determination_parameters.h>
+#include <math.h>
+#include <src_user/Library/math_constants.h>
+
+// Rough Three Axis Determination
+const APP_RTAD_METHOD ATTITUDE_DETERMINATION_PARAMETERS_rtad_method = APP_RTAD_METHOD_TRIAD;
+const float ATTITUDE_DETERMINATION_PARAMETERS_q_method_sun_vec_weight = 0.5f;
+
+// Fine Three Axis Determination
+const APP_FTAD_METHOD ATTITUDE_DETERMINATION_PARAMETERS_ftad_method = APP_FTAD_METHOD_STT;
+
+// STT-Gyro Extended Kalman Filter
+// STIM210 random noise N = 15deg/sq(h)
+const float ATTITUDE_DETERMINATION_PARAMETERS_ekf_gyro_random_noise_standard_deviation_compo_rad_s[PHYSICAL_CONST_THREE_DIM] = { 4.363323e-5f,
+                                                                                                                                 4.363323e-5f,
+                                                                                                                                 4.363323e-5f };
+// STIM210 bias stability B = 0.3deg/h, K=3/2*B^2/N
+const float ATTITUDE_DETERMINATION_PARAMETERS_ekf_gyro_random_walk_standard_deviation_compo_rad_s2[PHYSICAL_CONST_THREE_DIM] = { 7.272205e-8f,
+                                                                                                                                 7.272205e-8f,
+                                                                                                                                 7.272205e-8f };
+// As result of sensor feature measurement, STIM210 noise looks a simple random walk model.
+// When ECRV time constant is infinite, the model is same as a simple random walk, so we set very large value here.
+const float ATTITUDE_DETERMINATION_PARAMETERS_ekf_gyro_random_walk_time_constant_s = 1e9f;
+
+// STT noise roll direction 10 arcsec, cross direction 2 arcsec
+const float ATTITUDE_DETERMINATION_PARAMETERS_ekf_stt_standard_deviation_compo_rad[PHYSICAL_CONST_THREE_DIM] = { 4.8481e-5f,
+                                                                                                                 9.6963e-6f,
+                                                                                                                 9.6963e-6f };
+// Process noise model
+// We assume the attitude target is operated to synchronize with orbit.
+#define ATTITUDE_PARAMETER_ORBIT_PERIOD_SEC (5700.0f)
+#define ATTITUDE_PARAMETER_COMPUTATION_CYCLE_SEC (0.1f)
+const float ATTITUDE_DETERMINATION_PARAMETERS_ekf_process_noise_covariance_attitude = ((MATH_CONST_2PI / ATTITUDE_PARAMETER_ORBIT_PERIOD_SEC * ATTITUDE_PARAMETER_COMPUTATION_CYCLE_SEC))
+                                                                                    * ((MATH_CONST_2PI / ATTITUDE_PARAMETER_ORBIT_PERIOD_SEC * ATTITUDE_PARAMETER_COMPUTATION_CYCLE_SEC));
+// The attitude rate process noise is not well tuned at this moment
+const float ATTITUDE_DETERMINATION_PARAMETERS_ekf_process_noise_covariance_attitude_rate = ((MATH_CONST_2PI / ATTITUDE_PARAMETER_ORBIT_PERIOD_SEC * ATTITUDE_PARAMETER_COMPUTATION_CYCLE_SEC))
+                                                                                    * ((MATH_CONST_2PI / ATTITUDE_PARAMETER_ORBIT_PERIOD_SEC * ATTITUDE_PARAMETER_COMPUTATION_CYCLE_SEC));
diff --git a/Examples/src_aobc_example/Settings/SatelliteParameters/attitude_target_parameters.c b/Examples/src_aobc_example/Settings/SatelliteParameters/attitude_target_parameters.c
@@ -0,0 +1,28 @@
+/**
+ * @file  attitude_target_parameters.c
+ * @brief 姿勢目標の衛星固有パラメータを管理する
+ */
+
+#include <src_user/Settings/SatelliteParameters/attitude_target_parameters.h>
+#include <src_user/Settings/SatelliteParameters/parameter_setting_macro.h>
+
+// Target Calculation Mode
+const APP_TARGET_ATT_CALC_MODE ATTITUDE_TARGET_PARAMETERS_mode = APP_TARGET_ATT_CALC_MODE_MANUAL;
+
+// Manual mode target
+const Quaternion ATTITUDE_TARGET_PARAMETERS_quaternion_target_i2t = { 0.5f, 0.5f, 0.5f, 0.5f };
+
+// Target calculation from orbit
+// Main target
+extern const APP_TAFO_TARGET_DIRECITON ATTITUDE_TARGET_PARAMETERS_main_target_direction = APP_TAFO_TARGET_DIRECITON_SUN;
+extern const float ATTITUDE_TARGET_PARAMETERS_vec_to_main_target_body[PHYSICAL_CONST_THREE_DIM] = { 1.0f, 0.0f, 0.0f };
+// Sub target
+extern const APP_TAFO_TARGET_DIRECITON ATTITUDE_TARGET_PARAMETERS_sub_target_direction = APP_TAFO_TARGET_DIRECITON_EARTH_CENTER;
+extern const float ATTITUDE_TARGET_PARAMETERS_vec_to_sub_target_body[PHYSICAL_CONST_THREE_DIM] = { 0.0f, 1.0f, 0.0f };
+// Offset angle
+extern const MATRIX33_ROTATION_AXIS ATTITUDE_TARGET_PARAMETERS_offset_angle_axis = MATRIX33_ROTATION_AXIS_X;
+extern const float ATTITUDE_TARGET_PARAMETERS_offset_angle_rad = 0.0f;
+// Target on earth surface
+extern const float ATTITUDE_TARGET_PARAMETERS_target_lla_rad_m[PHYSICAL_CONST_THREE_DIM] = { PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(35.7130f),
+                                                                                             PARAMETER_SETTING_MACRO_DEGREE_TO_RADIAN(139.7596f),
+                                                                                             23.0f };
diff --git a/Examples/src_aobc_example/Settings/SatelliteParameters/component_selector_parameters.c b/Examples/src_aobc_example/Settings/SatelliteParameters/component_selector_parameters.c
@@ -0,0 +1,8 @@
+/**
+ * @file  component_selector_parameters.c
+ * @brief コンポセレクタに関する衛星固有パラメータを管理する
+ */
+
+#include <src_user/Settings/SatelliteParameters/component_selector_parameters.h>
+
+const APP_MAG_SELECTOR_STATE COMPONENT_SELECTOR_PARAMETERS_initial_selected_magnetometer = APP_MAG_SELECTOR_STATE_RM_EXT;