diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 817ff500..c7d8ff47 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,8 +1,2 @@ # AOCS team -* @ut-issl/aocs - -# CDH team -# * @chutaro - -# ArkEdge Sapce -* @sksat +* @ut-issl/aocs @sksat diff --git a/.github/ISSUE_TEMPLATE/action_item.md b/.github/ISSUE_TEMPLATE/action_item.md index 5c06b8b5..4443b75f 100644 --- a/.github/ISSUE_TEMPLATE/action_item.md +++ b/.github/ISSUE_TEMPLATE/action_item.md @@ -7,9 +7,6 @@ assignees: '' --- -## 概要 -ひとことで - ## 詳細 詳しく @@ -19,8 +16,18 @@ assignees: '' ## 備考 なにかあれば + diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index 7c03c968..3351b562 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -7,15 +7,21 @@ assignees: '' --- -## 概要 -ひとことで - ## 詳細 ### 詳細症状 -どういうバグか +どういうバグか、バグを再現できるように詳細に記述する ### 発生条件 -発生した環境，症状，パラメタなど +- 発生した環境 + - SILS, HILS, or 実機 + - ビルド、実行環境のバージョン情報など + - C2A-AOBCのバージョン + - S2E-AOBCのバージョン + - Visual Studioなどの情報 +- 症状 + - 可能ならば図などもつけて +- パラメータ + - デフォルトから変更した点があるなら記入する ### 追加資料 あればリンクなどを貼る @@ -30,8 +36,19 @@ tool類が全部死ぬ... みたいな ## 補足 何かれば + diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 4e0b00e2..30d935cd 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -1,11 +1,8 @@ -## 概要 -ひとことで - ## Issue - 関連する issue ## 詳細 -詳しく +詳しくPR内容を記述する ## 検証結果 ### ビルドチェック (どちらもチェック) @@ -20,14 +17,23 @@ ### 試験結果詳細記述場所 or 詳細ログ保存場所へのリンク - 図や表で記述する -## 影響範囲 -XX系の動作がガラッと変わる，とか． - ## 補足 -何かあれば +何かあれば書く。なければ`NA`とする。 + diff --git a/.github/workflows/.python-version b/.github/workflows/.python-version new file mode 100644 index 00000000..2c073331 --- /dev/null +++ b/.github/workflows/.python-version @@ -0,0 +1 @@ +3.11 diff --git a/.github/workflows/actionlint.yml b/.github/workflows/actionlint.yml index 274d11ec..4ef847aa 100644 --- a/.github/workflows/actionlint.yml +++ b/.github/workflows/actionlint.yml @@ -9,7 +9,7 @@ jobs: actionlint: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - uses: reviewdog/action-actionlint@v1 with: github_token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index c20d021a..c32b3a70 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -15,7 +15,7 @@ on: - 'src/**' env: - S2E_AOBC_VERSION: v2.0.1 + S2E_AOBC_VERSION: v3.0.1 jobs: build_c2a_with_s2e_win: @@ -40,7 +40,6 @@ jobs: repository: ut-issl/s2e-aobc ref: '${{ env.S2E_AOBC_VERSION }}' submodules: recursive - ssh-key: ${{ secrets.S2E_AOBC_SSH_KEY }} fetch-depth: 1 - name: Configure build for x86 @@ -92,10 +91,10 @@ jobs: - name: build shell: cmd - working-directory: ./s2e-aobc/s2e-aocs-core + working-directory: ./s2e-aobc run: | cl.exe - cmake -G "Visual Studio 17 2022" -A Win32 -DCMAKE_CONFIGURATION_TYPES:STRING="Debug" -DEXT_LIB_DIR=../s2e-core/ExtLibraries -DFLIGHT_SW_DIR=../../../ -DC2A_NAME=c2a-aobc -DUSE_C2A=ON + cmake -G "Visual Studio 17 2022" -A Win32 -DCMAKE_CONFIGURATION_TYPES:STRING="Debug" -DEXT_LIB_DIR=s2e-core/ExtLibraries -DFLIGHT_SW_DIR=../../ -DC2A_NAME=c2a-aobc -DUSE_C2A=ON cmake --build . --clean-first build_c2a_with_s2e_linux: @@ -121,7 +120,6 @@ jobs: repository: ut-issl/s2e-aobc ref: '${{ env.S2E_AOBC_VERSION }}' submodules: recursive - ssh-key: ${{ secrets.S2E_AOBC_SSH_KEY }} fetch-depth: 1 - name: set compiler @@ -186,10 +184,10 @@ jobs: ls nrlmsise00/src - name: build - working-directory: ./s2e-aobc/s2e-aocs-core + working-directory: ./s2e-aobc env: CC: ${{ steps.compiler.outputs.CC }} CXX: ${{ steps.compiler.outputs.CXX }} run: | - cmake . -DBUILD_64BIT=OFF -DEXT_LIB_DIR=../s2e-core/ExtLibraries -DFLIGHT_SW_DIR=../../../ -DC2A_NAME=c2a-aobc -DUSE_C2A=ON + cmake . -DBUILD_64BIT=OFF -DEXT_LIB_DIR=s2e-core/ExtLibraries -DFLIGHT_SW_DIR=../../ -DC2A_NAME=c2a-aobc -DUSE_C2A=ON cmake --build . --clean-first diff --git a/.github/workflows/c2a.yml b/.github/workflows/c2a.yml new file mode 100644 index 00000000..df166875 --- /dev/null +++ b/.github/workflows/c2a.yml @@ -0,0 +1,13 @@ +name: C2A + +on: + push: + branches: + - main + - develop + pull_request: + +jobs: + c2a: + name: C2A + uses: arkedge/workflows-c2a/.github/workflows/default.yml@v3.2.0 diff --git a/.github/workflows/labeler.yml b/.github/workflows/labeler.yml index e2f01a65..82cb5d16 100644 --- a/.github/workflows/labeler.yml +++ b/.github/workflows/labeler.yml @@ -14,10 +14,10 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: Run Labeler - uses: crazy-max/ghaction-github-labeler@v3 + uses: crazy-max/ghaction-github-labeler@v4 with: github-token: ${{ secrets.GITHUB_TOKEN }} yaml-file: .github/labels.yml diff --git a/CMakeLists.txt b/CMakeLists.txt index fb07e8a6..372adaba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,15 +7,9 @@ option(BUILD_C2A_AS_CXX "Build C2A as C++" ON) option(BUILD_C2A_AS_SILS_FW "Build C2A as SILS firmware" ON) -# SCI COM for connection to WINGS TMTC IF -option(USE_SCI_COM "Use SCI_COM" OFF) - -# SCI COM for connection to PC UART -# ！！！注意！！！ -# これをONにした状態で，SCIの受け口がない場合（TMTC_IFが動いていない状態） -# そちらのバッファが詰まってSILSの動作が止まることがあるので注意すること！ - -option(USE_SCI_COM_UART "Use SCI_COM_UART" OFF) +# UART COM for connection to PC UART +# これをONにした状態でTMTC_IFが動いていない場合，そちらのバッファが詰まってSILSの動作が止まることがあるので注意すること！ +option(USE_UART_COM "Use UART COM" OFF) set(C2A_CORE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/src_core) set(C2A_USER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/src_user) diff --git a/Examples/src_aobc_example/Settings/SatelliteParameters/ina260_parameters.c b/Examples/src_aobc_example/Settings/SatelliteParameters/ina260_parameters.c index 49dce5a4..1539318f 100644 --- a/Examples/src_aobc_example/Settings/SatelliteParameters/ina260_parameters.c +++ b/Examples/src_aobc_example/Settings/SatelliteParameters/ina260_parameters.c @@ -20,7 +20,7 @@ const float INA260_PARAMETERS_stim210_hw_over_current_threshold_mA = 1000.0f; // SAGITTA const INA260_AVERAGING_MODE INA260_PARAMETERS_sagitta_averaging_mode = INA260_AVERAGING_MODE_16; const INA260_CONVERSION_TIME INA260_PARAMETERS_sagitta_voltage_conversion_time = INA260_CONVERSION_TIME_140US; -const INA260_CONVERSION_TIME INA260_PARAMETERS_sagitta_current_conversion_time = INA260_CONVERSION_TIME_140US; +const INA260_CONVERSION_TIME INA260_PARAMETERS_sagitta_current_conversion_time = INA260_CONVERSION_TIME_204US; const float INA260_PARAMETERS_sagitta_hw_over_current_threshold_mA = 500.0f; // OEM7600 diff --git a/README.md b/README.md index c51df8a5..15c5cc7d 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ - [S2E-AOBC](https://github.com/ut-issl/s2e-aobc) v3.0.0 - Telemetry/Command interface - [tlm-cmd-generator](https://github.com/ut-issl/c2a-tlm-cmd-code-generator) - - Version: [ISSL Branch](https://github.com/ut-issl/c2a-tlm-cmd-code-generator/tree/feature/issl) + - Version: [ISSL Branch](https://github.com/ut-issl/c2a-tlm-cmd-code-generator/tree/feature/issl) - [WINGS](https://gitlab.com/ut_issl/wings/wings) - How to use - `The main developers` of the AOCS module directly use this repository to add new features and improve the module. @@ -154,7 +154,7 @@ - Images for markdown document files are allowable when the file size is smaller than 200K Bytes. ### Coding conventions -- Please follow the [Coding rule of c2a-core](https://github.com/ut-issl/c2a-core/blob/042cdfa15b0056880398e857cdd5d5a430562fd1/Docs/General/coding_rule.md)に従う +- Please follow the [Coding rule of c2a-core](https://github.com/ut-issl/c2a-core/blob/042cdfa15b0056880398e857cdd5d5a430562fd1/Docs/General/coding_rule.md) - Please also care about the AOCS-specific rules as follows. - Add unit information in the name of variables and functions - Add frame information in the name of variables and functions diff --git a/src/src_user/Applications/UserDefined/AOCS/AttitudeControl/bdot.c b/src/src_user/Applications/UserDefined/AOCS/AttitudeControl/bdot.c index 39744bda..8f2bf312 100644 --- a/src/src_user/Applications/UserDefined/AOCS/AttitudeControl/bdot.c +++ b/src/src_user/Applications/UserDefined/AOCS/AttitudeControl/bdot.c @@ -146,10 +146,10 @@ AOCS_ERROR APP_BDOT_calculate_mag_vec_time_derivative_(void) if (bdot->time_derivative_variables.num_of_mag_observation == 0) { // 磁気センサの観測が開始していない場合，磁場ベクトルが0となっているので，その場合はすぐにリターンする - if (VECTOR3_norm(aocs_manager->mag_vec_obs_body_nT) < MATH_CONST_NORMAL_CHECK_THRESHOLD) return AOCS_ERROR_OTHERS; + if (VECTOR3_norm(aocs_manager->mag_vec_est_body_nT) < MATH_CONST_NORMAL_CHECK_THRESHOLD) return AOCS_ERROR_OTHERS; bdot_.time_derivative_variables.previous_obc_time = TMGR_get_master_clock(); - VECTOR3_copy(bdot_.time_derivative_variables.mag_vec_before_nT, aocs_manager->mag_vec_obs_body_nT); + VECTOR3_copy(bdot_.time_derivative_variables.mag_vec_before_nT, aocs_manager->mag_vec_est_body_nT); // 1回目の観測が終了した段階では，磁場ベクトルの微分はまだ計算できないので，エラーを出してリターンする． bdot_.time_derivative_variables.num_of_mag_observation = 1; @@ -158,7 +158,7 @@ AOCS_ERROR APP_BDOT_calculate_mag_vec_time_derivative_(void) else // bdot->time_derivative_variables.num_of_mag_observation == 1 { bdot_.time_derivative_variables.current_obc_time = TMGR_get_master_clock(); - VECTOR3_copy(bdot_.time_derivative_variables.mag_vec_after_nT, aocs_manager->mag_vec_obs_body_nT); + VECTOR3_copy(bdot_.time_derivative_variables.mag_vec_after_nT, aocs_manager->mag_vec_est_body_nT); // 時間微分のインターバルが一定以下の場合は，姿勢レートの変化が不十分とみなし，磁場の時間微分を計算しない if (OBCT_diff_in_msec(&(bdot->time_derivative_variables.previous_obc_time), &(bdot->time_derivative_variables.current_obc_time)) diff --git a/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/fine_three_axis_determination.c b/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/fine_three_axis_determination.c index b03d777c..9da78264 100644 --- a/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/fine_three_axis_determination.c +++ b/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/fine_three_axis_determination.c @@ -47,7 +47,8 @@ static void APP_FTAD_exec_(void) ObcTime current_obc_time = TMGR_get_master_clock(); float time_step_s = (float)OBCT_diff_in_sec(&(fine_three_axis_determination_.previous_obc_time), ¤t_obc_time); fine_three_axis_determination_.previous_obc_time = current_obc_time; - if (time_step_s < 0.0f) return; + if (time_step_s < 0.0f) return; // 時間差が負の場合は一旦飛ばす + if (time_step_s > aocs_manager->obct_diff_max_limit_s) return; // 時間差が大きすぎる場合は一旦飛ばす Quaternion q_eci_to_body; switch (fine_three_axis_determination_.method) diff --git a/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/rough_three_axis_determination.c b/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/rough_three_axis_determination.c index c94403db..d088d0c8 100644 --- a/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/rough_three_axis_determination.c +++ b/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/rough_three_axis_determination.c @@ -183,6 +183,8 @@ static void APP_RTAD_exec_(void) ObcTime current_obc_time = TMGR_get_master_clock(); float time_step_s = (float)OBCT_diff_in_sec(&(rough_three_axis_determination_.previous_obc_time), ¤t_obc_time); rough_three_axis_determination_.previous_obc_time = current_obc_time; + if (time_step_s < 0.0f) return; // 時間差が負の場合は一旦飛ばす + if (time_step_s > aocs_manager->obct_diff_max_limit_s) return; // 時間差が大きすぎる場合は一旦飛ばす float sun_ref_vec[PHYSICAL_CONST_THREE_DIM]; //!< 基準の座標系における太陽方向単位ベクトル float mag_ref_vec[PHYSICAL_CONST_THREE_DIM]; //!< 基準の座標系における磁場単位ベクトル diff --git a/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/stt_gyro_ekf.c b/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/stt_gyro_ekf.c index cd4be63b..2642f195 100644 --- a/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/stt_gyro_ekf.c +++ b/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/stt_gyro_ekf.c @@ -295,8 +295,10 @@ static void APP_STT_GYRO_EKF_exec_(void) // 時間刻みを計算する APP_STT_GYRO_EKF_calculation_time_.current = TMGR_get_master_clock(); float time_step_s = (float)OBCT_diff_in_sec(&(APP_STT_GYRO_EKF_calculation_time_.previous), - &(APP_STT_GYRO_EKF_calculation_time_.current)); + &(APP_STT_GYRO_EKF_calculation_time_.current)); APP_STT_GYRO_EKF_calculation_time_.previous = APP_STT_GYRO_EKF_calculation_time_.current; + if (time_step_s < 0.0f) return; // 時間差が負の場合は一旦飛ばす + if (time_step_s > aocs_manager->obct_diff_max_limit_s) return; // 時間差が大きすぎる場合は一旦飛ばす APP_STT_GYRO_EKF_execute_estimation(time_step_s); } @@ -502,7 +504,7 @@ static UpdatedStateVariable APP_STT_GYRO_EKF_update_state_variable_(MATRIX_T(6, delta_quaternion_i2b_ref_to_true.vector_part[i] = updated_state_variable_matrix_form.data[i][0]; delta_rate_bias_ref_to_true_rad_s[i] = updated_state_variable_matrix_form.data[i + 3][0]; } - delta_quaternion_i2b_ref_to_true.scalar_part = sqrtf(1.0f - VECTOR3_norm(delta_quaternion_i2b_ref_to_true.vector_part)); + delta_quaternion_i2b_ref_to_true.scalar_part = C2A_MATH_sqrtf(1.0f - VECTOR3_norm(delta_quaternion_i2b_ref_to_true.vector_part)); UpdatedStateVariable updated_state_variable; updated_state_variable.quaternion_i2b = diff --git a/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/sun_vector_propagator.c b/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/sun_vector_propagator.c index 9929509c..7a27aaee 100644 --- a/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/sun_vector_propagator.c +++ b/src/src_user/Applications/UserDefined/AOCS/AttitudeDetermination/sun_vector_propagator.c @@ -53,7 +53,9 @@ void APP_SUN_VEC_PROPAGATOR_exec_(void) ObcTime current_obc_time = TMGR_get_master_clock(); float time_step_s = (float)OBCT_diff_in_sec(&(sun_vector_propagator_.previous_obc_time), ¤t_obc_time); sun_vector_propagator_.previous_obc_time = current_obc_time; - + if (time_step_s < 0.0f) return; // 時間差が負の場合は一旦飛ばす + if (time_step_s > aocs_manager->obct_diff_max_limit_s) return; // 時間差が大きすぎる場合は一旦飛ばす + // Quaternionライブラリにはスカラー倍や和がないので，q + dqを計算するために一度配列にしてから計算する float quaternion_previous_array[PHYSICAL_CONST_QUATERNION_DIM]; //!< 前回のquaternion_sun_ref_to_body float quaternion_time_derivative_array[PHYSICAL_CONST_QUATERNION_DIM]; //!< quaternion_sun_ref_to_bodyの時間微分 diff --git a/src/src_user/Applications/UserDefined/AOCS/HardwareDependent/ActuatorControllers/mtq_seiren_controller.c b/src/src_user/Applications/UserDefined/AOCS/HardwareDependent/ActuatorControllers/mtq_seiren_controller.c index ccea52a8..0221786d 100644 --- a/src/src_user/Applications/UserDefined/AOCS/HardwareDependent/ActuatorControllers/mtq_seiren_controller.c +++ b/src/src_user/Applications/UserDefined/AOCS/HardwareDependent/ActuatorControllers/mtq_seiren_controller.c @@ -242,6 +242,7 @@ void APP_MTQ_SEIREN_CONTROLLER_maintain_mtq_output_() ObcTime current_obc_time = TMGR_get_master_clock(); uint32_t mtq_driving_time_ms = OBCT_diff_in_msec(&(mtq_seiren_controller->mtq_output_turned_on_obc_time), ¤t_obc_time); // 目標の時間長さだけMTQに磁気モーメントを出力させたら，MTQの出力を切り，消磁に入る + // TODO: 時間アサーションが正しいかどうか検討する if (mtq_driving_time_ms >= mtq_seiren_controller->mtq_output_time_length_ms) { float mag_moment_target_Am2[PHYSICAL_CONST_THREE_DIM]; diff --git a/src/src_user/Applications/UserDefined/AOCS/InertialReference/time_space_calculator.c b/src/src_user/Applications/UserDefined/AOCS/InertialReference/time_space_calculator.c index dfb2ed48..44a6376e 100644 --- a/src/src_user/Applications/UserDefined/AOCS/InertialReference/time_space_calculator.c +++ b/src/src_user/Applications/UserDefined/AOCS/InertialReference/time_space_calculator.c @@ -91,6 +91,7 @@ static double APP_TIME_SPACE_CALC_propagate_current_jday_ref_(void) { ObcTime obct_now = TMGR_get_master_clock(); float elapsed_ti_sec = (float)OBCT_diff_in_sec(&aocs_manager->obct_reference, &obct_now); + // TODO: 時間アサーションを入れるかどうか検討する double reference_jday = (double)(elapsed_ti_sec) / (PHYSICAL_CONST_EARTH_SOLAR_DAY_s) + aocs_manager->reference_jday; 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 db614ecf..7abe52fe 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 @@ -111,6 +111,7 @@ 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, q_target_previous, q_target_current, diff --git a/src/src_user/Applications/UserDefined/AOCS/aocs_manager.c b/src/src_user/Applications/UserDefined/AOCS/aocs_manager.c index 50d15a30..b7146c26 100644 --- a/src/src_user/Applications/UserDefined/AOCS/aocs_manager.c +++ b/src/src_user/Applications/UserDefined/AOCS/aocs_manager.c @@ -117,6 +117,7 @@ static void APP_AOCS_MANAGER_init_(void) aocs_manager_.obct_gps_time_est = OBCT_create(0, 0, 0); aocs_manager_.obct_reference = OBCT_create(0, 0, 0); aocs_manager_.reference_jday = ORBIT_PARAMETERS_reference_jday; + aocs_manager_.obct_diff_max_limit_s = 10.0f; // TODO_L : 可変にする？あまり必要なさそう。 // センサ状態 aocs_manager_.sun_visibility = AOCS_MANAGER_SUN_INVISIBILE; aocs_manager_.gps_visibility = AOCS_MANAGER_GPS_INVISIBILE; diff --git a/src/src_user/Applications/UserDefined/AOCS/aocs_manager.h b/src/src_user/Applications/UserDefined/AOCS/aocs_manager.h index 7bd99fc9..ff66e2db 100644 --- a/src/src_user/Applications/UserDefined/AOCS/aocs_manager.h +++ b/src/src_user/Applications/UserDefined/AOCS/aocs_manager.h @@ -167,6 +167,7 @@ typedef struct ObcTime obct_gps_time_est; //!< 上記を推定したタイミングのC2Aマスタークロック [-] double reference_jday; //!< 慣性系計算の基準時刻として用いるユリウス日 [ユリウス日] ObcTime obct_reference; //!< 慣性系計算の基準時刻として用いるユリウス日を更新した時のOBC時刻 [-] + float obct_diff_max_limit_s; //!< OBC時刻差計算時のアサーションに使う上限 [s] // センサ状態 AOCS_MANAGER_GPS_VISIBILITY gps_visibility; //!< GPS情報の利用可否 AOCS_MANAGER_SUN_VISIBILITY sun_visibility; //!< 太陽がサンセンサから見えているかどうか diff --git a/src/src_user/Applications/UserDefined/AOCS/aocs_mode_manager.c b/src/src_user/Applications/UserDefined/AOCS/aocs_mode_manager.c index e364e211..6debcb4f 100644 --- a/src/src_user/Applications/UserDefined/AOCS/aocs_mode_manager.c +++ b/src/src_user/Applications/UserDefined/AOCS/aocs_mode_manager.c @@ -183,6 +183,7 @@ static void APP_AOCS_MM_bdot_exec_(void) ObcTime current_obc_time = TMGR_get_master_clock(); float diff_time_s = (float)OBCT_diff_in_sec(&aocs_mode_manager_.previous_obc_time, ¤t_obc_time); // (初回など)時間差が大きすぎたら異常なので足さない + // TODO: 時間アサーションが正しいかどうか検討する if (diff_time_s < APP_AOCS_MM_kLimitDiffTime_s_) { APP_AOCS_MM_ang_vel_update_timing_s_ += diff_time_s; @@ -358,6 +359,7 @@ static uint8_t APP_AOCS_MM_judge_condition_(const uint8_t condition_flag, const { float diff_time_s = (float)OBCT_diff_in_sec(&prev_obc_time, ¤t_obc_time); // (初回など)時間差が大きすぎたら異常なので足さない + // TODO: 時間アサーションが正しいかどうか検討する if (diff_time_s < APP_AOCS_MM_kLimitDiffTime_s_) { *counter_s += diff_time_s; diff --git a/src/src_user/IfWrapper/CMakeLists.txt b/src/src_user/IfWrapper/CMakeLists.txt index 8c8f9c76..7aabf273 100644 --- a/src/src_user/IfWrapper/CMakeLists.txt +++ b/src/src_user/IfWrapper/CMakeLists.txt @@ -4,31 +4,20 @@ project(C2A_USER_IF_WRAPPER) set(C2A_SRCS if_list.c - # SILS/CCSDS_SILS.c - # SILS/DC_SILS.c - # SILS/FLASH_SILS.c - SILS/UART_SILS.c - SILS/I2C_SILS.c - SILS/spi_sils.c - SILS/ADC_SILS.c - SILS/WDT_SILS.c - SILS/GPIO_SILS.c + Sils/uart_sils.cpp + Sils/i2c_sils.c + Sils/spi_sils.c + Sils/adc_sils.c + Sils/wdt_sils.c + Sils/gpio_sils.c ) -if(USE_SCI_COM) - add_definitions(-DUSE_SCI_COM) +if(USE_UART_COM) + add_definitions(-DUSE_UART_COM) list(APPEND C2A_SRCS - SILS/CCSDS_SILS_SCI_IF.c + Sils/com_port.cpp ) - message("USE SCI_COM") -endif() - -if(USE_SCI_COM_UART) - add_definitions(-DUSE_SCI_COM_UART) - list(APPEND C2A_SRCS - SILS/uart_sils_sci_if.cpp - ) - message("USE SCI_COM") + message("Use UART COM") endif() add_library(${PROJECT_NAME} OBJECT ${C2A_SRCS}) diff --git a/src/src_user/IfWrapper/SILS/CCSDS_SILS_SCI_IF.c b/src/src_user/IfWrapper/SILS/CCSDS_SILS_SCI_IF.c deleted file mode 100644 index a820d78c..00000000 --- a/src/src_user/IfWrapper/SILS/CCSDS_SILS_SCI_IF.c +++ /dev/null @@ -1,120 +0,0 @@ -#pragma section REPRO -/** - * @file - * @brief CCSDS_SILS_SCI_IF - * @author 鈴本 遼 - * @date 2020/12/28 - * @details WINGS TMTC IFとCCSDSのTransfer FrameをSCI COMでやりとりするIF - Windows上でcom0comを使うことを想定 - SCIComPort classは基本的にEQU ZEUSのコードを流用 - */ - -#include "CCSDS_SILS_SCI_IF.h" - - -// 最初だけ初期化して、プログラム終了時にポートを閉じるようにしたい -static SCIComPort sci_com; - -int SILS_SIC_IF_init() -{ - return 0; -} - -int SILS_SIC_IF_TX(unsigned char* data_v, int count) -{ - sci_com.Send(data_v, 0, count); - return 0; -} - -int SILS_SIC_IF_RX(unsigned char* data_v, int count) -{ - return sci_com.Receive(data_v, 0, count); -} - - -SCIComPort::SCIComPort(void) -{ - // ビルド通らなかったので，ZEUSからちょっと変えた - myHComPort_ = CreateFile("\\\\.\\COM11", GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); - - if ((int)myHComPort_ == -1) - { - // 正常にポートオープンできていない場合は終了 - CloseHandle(myHComPort_); - init_success = false; - return; - } - - // どうやら正常ポートopenにならないっぽく，これが必要 - init_success = true; - - // ポートのボーレート、パリティ等を設定 - config_.BaudRate = 115200; - config_.Parity = PARITY_NONE; - config_.ByteSize = 8; - config_.StopBits = STOPBITS_10; - - // Parity、StopBits、DataBitsも同様に設定 - SetCommState(myHComPort_, &config_); -} - -SCIComPort::~SCIComPort(void) -{ - if (init_success == true) - { - CloseHandle(myHComPort_); - } -} - -int SCIComPort::Send(unsigned char* buffer, size_t offset, size_t count) -{ - DWORD toWriteBytes = count; // 送信したいバイト数 - DWORD writeBytes; // 実際に送信されたバイト数 - - if (init_success == true) - { - WriteFile(myHComPort_, buffer + offset, toWriteBytes, &writeBytes, NULL); - - return writeBytes; - } - else - { - return 0; - } -} - -int SCIComPort::Receive(unsigned char* buffer, size_t offset, size_t count) -{ - DWORD fromReadBytes = count; // 受信したいバイト数 - DWORD dwErrors; - COMSTAT ComStat; - DWORD dwCount; // 受信したバイト数 - - if (init_success == true) - { - ClearCommError(myHComPort_, &dwErrors, &ComStat); - dwCount = ComStat.cbInQue; - - if (dwCount > 0) - { - if (dwCount < count) - { - fromReadBytes = dwCount; - ReadFile(myHComPort_, buffer + offset, fromReadBytes, &dwCount, NULL); - } - else - { - fromReadBytes = count; // 読み込みすぎるとデータが失われるので読み込む量を制御 - ReadFile(myHComPort_, buffer + offset, fromReadBytes, &dwCount, NULL); - } - } - - return dwCount; - } - else - { - return 0; - } -} - -#pragma section diff --git a/src/src_user/IfWrapper/SILS/CCSDS_SILS_SCI_IF.h b/src/src_user/IfWrapper/SILS/CCSDS_SILS_SCI_IF.h deleted file mode 100644 index f0ce7a36..00000000 --- a/src/src_user/IfWrapper/SILS/CCSDS_SILS_SCI_IF.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef CCSDS_SILS_SCI_IF_H_ -#define CCSDS_SILS_SCI_IF_H_ - -#include - -// ZEUS SILSからのコピー -class SCIComPort -{ -public: - SCIComPort(void); - ~SCIComPort(void); - - int Send(unsigned char* buffer, size_t length, size_t offset); - int Receive(unsigned char* buffer, size_t length, size_t offset); - -private: - HANDLE myHComPort_; - DCB config_; - bool init_success; -}; - -int SILS_SIC_IF_init(); -int SILS_SIC_IF_TX(unsigned char* data_v, int count); -int SILS_SIC_IF_RX(unsigned char* data_v, int count); - -#endif diff --git a/src/src_user/IfWrapper/SILS/uart_sils_sci_if.cpp b/src/src_user/IfWrapper/SILS/uart_sils_sci_if.cpp deleted file mode 100644 index 82361be3..00000000 --- a/src/src_user/IfWrapper/SILS/uart_sils_sci_if.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#pragma section REPRO -/** - * @file - * @brief uart_sils_sci_if - * @details SILSでDriverのテストをするように作った - ccsds_sils_sci_if.c/hのほぼコピー - */ - -#include "uart_sils_sci_if.h" - - // 最初だけ初期化して、プログラム終了時にポートを閉じるようにしたい -static SciComPortUart sci_com_uart_; - -int SILS_SCI_UART_IF_init(void) -{ - return 0; -} - -int SILS_SCI_UART_IF_tx(unsigned char* data_v, int count) -{ - sci_com_uart_.send(data_v, 0, count); - return 0; -} - -int SILS_SCI_UART_IF_rx(unsigned char* data_v, int count) -{ - return sci_com_uart_.receive(data_v, 0, count); -} - - -SciComPortUart::SciComPortUart(void) -{ - myHComPort_ = CreateFile("\\\\.\\COM11", GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); - - if ((int)myHComPort_ == -1) - { - // 正常にポートオープンできていない場合は終了 - CloseHandle(myHComPort_); - init_success_ = false; - return; - } - - // どうやら正常ポートopenにならないっぽく，これが必要 - init_success_ = true; - - // SetCommStateを呼び出す前に，DCB構造体に現在の構成を入力する必要がある - GetCommState(myHComPort_, &config_); - - // ポートのボーレート、パリティ等を設定 - config_.BaudRate = CBR_115200; - config_.Parity = NOPARITY; - config_.ByteSize = 8; - config_.StopBits = ONESTOPBIT; - - // Parity、StopBits、DataBitsも同様に設定 - SetCommState(myHComPort_, &config_); -} - -SciComPortUart::~SciComPortUart(void) -{ - if (init_success_ == true) - { - CloseHandle(myHComPort_); - } -} - -int SciComPortUart::send(unsigned char* buffer, size_t offset, size_t count) -{ - DWORD toWriteBytes = count; // 送信したいバイト数 - DWORD writeBytes; // 実際に送信されたバイト数 - - if (init_success_ == true) - { - WriteFile(myHComPort_, buffer + offset, toWriteBytes, &writeBytes, NULL); - - return writeBytes; - } - else - { - return 0; - } -} - -int SciComPortUart::receive(unsigned char* buffer, size_t offset, size_t count) -{ - DWORD fromReadBytes = count; // 受信したいバイト数 - DWORD dwErrors; - COMSTAT ComStat; - DWORD dwCount; // 受信したバイト数 - - if (init_success_ == true) - { - ClearCommError(myHComPort_, &dwErrors, &ComStat); - dwCount = ComStat.cbInQue; - - if (dwCount > 0) - { - if (dwCount < count) - { - fromReadBytes = dwCount; - ReadFile(myHComPort_, buffer + offset, fromReadBytes, &dwCount, NULL); - } - else - { - fromReadBytes = count; // 読み込みすぎるとデータが失われるので読み込む量を制御 - ReadFile(myHComPort_, buffer + offset, fromReadBytes, &dwCount, NULL); - } - } - - return dwCount; - } - else - { - return 0; - } -} - -#pragma section diff --git a/src/src_user/IfWrapper/SILS/uart_sils_sci_if.h b/src/src_user/IfWrapper/SILS/uart_sils_sci_if.h deleted file mode 100644 index efa888e6..00000000 --- a/src/src_user/IfWrapper/SILS/uart_sils_sci_if.h +++ /dev/null @@ -1,31 +0,0 @@ -/** - * @file - * @brief uart_sils_sci_if - * @details SILSでDriverのテストをするように作った - ccsds_sils_sci_if.c/hのほぼコピー - */ -#ifndef UART_SILS_SCI_IF_H_ -#define UART_SILS_SCI_IF_H_ - -#include - -class SciComPortUart -{ -public: - SciComPortUart(void); - ~SciComPortUart(void); - - int send(unsigned char* buffer, size_t length, size_t offset); - int receive(unsigned char* buffer, size_t length, size_t offset); - -private: - HANDLE myHComPort_; - DCB config_; - bool init_success_; -}; - -int SILS_SCI_UART_IF_init(); -int SILS_SCI_UART_IF_tx(unsigned char* data_v, int count); -int SILS_SCI_UART_IF_rx(unsigned char* data_v, int count); - -#endif diff --git a/src/src_user/IfWrapper/SILS/ADC_SILS.c b/src/src_user/IfWrapper/Sils/adc_sils.c similarity index 100% rename from src/src_user/IfWrapper/SILS/ADC_SILS.c rename to src/src_user/IfWrapper/Sils/adc_sils.c diff --git a/src/src_user/IfWrapper/Sils/com_port.cpp b/src/src_user/IfWrapper/Sils/com_port.cpp new file mode 100644 index 00000000..1ccd69eb --- /dev/null +++ b/src/src_user/IfWrapper/Sils/com_port.cpp @@ -0,0 +1,148 @@ +#pragma section REPRO + +#include "com_port.h" + +#ifdef WIN32 +ComPort::ComPort(int port) + :init_success_{true} +{ + char port_settings[15]; + snprintf(port_settings, 15, "%s%d", "\\\\.\\COM", port); + com_port_handle_ = CreateFile(port_settings, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); + + if ((int)com_port_handle_ == -1) + { + // 正常にポートオープンできていない場合は終了 + CloseHandle(com_port_handle_); + init_success_ = false; + return; + } + + // ポートのボーレート、パリティ等を設定 + // TODO: 外部ファイルから設定できるようにする + config_.BaudRate = 115200; + config_.Parity = PARITY_NONE; + config_.ByteSize = 8; + config_.StopBits = STOPBITS_10; + + // Parity、StopBits、DataBitsも同様に設定 + SetCommState(com_port_handle_, &config_); +} + +ComPort::~ComPort(void) +{ + if (init_success_ == true) + { + CloseHandle(com_port_handle_); + } +} + +int ComPort::send(unsigned char* buffer, size_t offset, size_t count) +{ + DWORD bytes_to_send = count; // 送信したいバイト数 + DWORD bytes_sent; // 実際に送信されたバイト数 + + if (init_success_ == true) + { + WriteFile(com_port_handle_, buffer + offset, bytes_to_send, &bytes_sent, NULL); + return bytes_sent; + } + else + { + return 0; + } +} + +int ComPort::receive(unsigned char* buffer, size_t offset, size_t count) +{ + DWORD bytes_to_receive = count; // 受信したいバイト数 + DWORD bytes_received; // 受信したバイト数 + DWORD dw_errors; + COMSTAT com_stat; + + if (init_success_ == true) + { + ClearCommError(com_port_handle_, &dw_errors, &com_stat); + bytes_received = com_stat.cbInQue; + + if (bytes_received > 0) + { + if (bytes_received < count) + { + bytes_to_receive = bytes_received; + ReadFile(com_port_handle_, buffer + offset, bytes_to_receive, &bytes_received, NULL); + } + else + { + bytes_to_receive = count; // 読み込みすぎるとデータが失われるので読み込む量を制御 + ReadFile(com_port_handle_, buffer + offset, bytes_to_receive, &bytes_received, NULL); + } + } + + return bytes_received; + } + else + { + return 0; + } +} + +#else + +ComPort::ComPort(int port) + :init_success_{true} +{ + char port_settings[13]; + snprintf(port_settings, 13, "%s%d", "/dev/tnt", port); + if ((com_port_handle_ = open(port_settings, O_RDWR | O_NOCTTY | O_NONBLOCK)) < 0) + { + close(com_port_handle_); + init_success_ = false; + return; + } + + // TODO: 外部ファイルから設定できるようにする + cfsetispeed(&config_, 115200); + cfsetospeed(&config_, 115200); + config_.c_cflag &= ~PARENB; // No Parity + config_.c_cflag &= ~CSTOPB; // 1 Stop Bit + config_.c_cflag &= ~CSIZE; + config_.c_cflag |= CS8; // 8 Bits + tcsetattr(com_port_handle_, TCSANOW, &config_); +} + +ComPort::~ComPort(void) +{ + if (init_success_ == true) + { + close(com_port_handle_); + } +} + +int ComPort::send(unsigned char* buffer, size_t offset, size_t count) +{ + if (init_success_ == true) + { + return write(com_port_handle_, buffer + offset, count); + } + else + { + return 0; + } +} + +int ComPort::receive(unsigned char* buffer, size_t offset, size_t count) +{ + if (init_success_ == true) + { + return read(com_port_handle_, buffer + offset, count); + } + else + { + return 0; + } +} + +#endif + +#pragma section diff --git a/src/src_user/IfWrapper/Sils/com_port.h b/src/src_user/IfWrapper/Sils/com_port.h new file mode 100644 index 00000000..b2d8a904 --- /dev/null +++ b/src/src_user/IfWrapper/Sils/com_port.h @@ -0,0 +1,40 @@ +/** + * @file com_port.h + * @brief SILSにおけるポート操作 + */ + +#ifndef COM_PORT_H_ +#define COM_PORT_H_ + +#ifdef WIN32 +#include +#else +#include +#include +#include +#endif + +#include +#include + +class ComPort +{ +public: + ComPort(int port); + ~ComPort(void); + + int send(unsigned char* buffer, size_t length, size_t offset); + int receive(unsigned char* buffer, size_t length, size_t offset); + +private: +#ifdef WIN32 + HANDLE com_port_handle_; + DCB config_; +#else + int com_port_handle_; + struct termios config_; +#endif + bool init_success_; +}; + +#endif diff --git a/src/src_user/IfWrapper/SILS/DC_SILS.c b/src/src_user/IfWrapper/Sils/dc_sils.c similarity index 100% rename from src/src_user/IfWrapper/SILS/DC_SILS.c rename to src/src_user/IfWrapper/Sils/dc_sils.c diff --git a/src/src_user/IfWrapper/SILS/FLASH_DB/.gitkeep b/src/src_user/IfWrapper/Sils/flash_db/.gitkeep similarity index 100% rename from src/src_user/IfWrapper/SILS/FLASH_DB/.gitkeep rename to src/src_user/IfWrapper/Sils/flash_db/.gitkeep diff --git a/src/src_user/IfWrapper/SILS/GPIO_SILS.c b/src/src_user/IfWrapper/Sils/gpio_sils.c similarity index 95% rename from src/src_user/IfWrapper/SILS/GPIO_SILS.c rename to src/src_user/IfWrapper/Sils/gpio_sils.c index 746d3360..ef2db06f 100644 --- a/src/src_user/IfWrapper/SILS/GPIO_SILS.c +++ b/src/src_user/IfWrapper/Sils/gpio_sils.c @@ -1,4 +1,4 @@ - +#include #include "../GPIO.h" int OBC_C2A_GpioWrite(int port_id, const bool is_high); diff --git a/src/src_user/IfWrapper/SILS/I2C_SILS.c b/src/src_user/IfWrapper/Sils/i2c_sils.c similarity index 100% rename from src/src_user/IfWrapper/SILS/I2C_SILS.c rename to src/src_user/IfWrapper/Sils/i2c_sils.c diff --git a/src/src_user/IfWrapper/SILS/spi_sils.c b/src/src_user/IfWrapper/Sils/spi_sils.c similarity index 100% rename from src/src_user/IfWrapper/SILS/spi_sils.c rename to src/src_user/IfWrapper/Sils/spi_sils.c diff --git a/src/src_user/IfWrapper/SILS/UART_SILS.c b/src/src_user/IfWrapper/Sils/uart_sils.cpp similarity index 65% rename from src/src_user/IfWrapper/SILS/UART_SILS.c rename to src/src_user/IfWrapper/Sils/uart_sils.cpp index e18e53a9..68e3cf9a 100644 --- a/src/src_user/IfWrapper/SILS/UART_SILS.c +++ b/src/src_user/IfWrapper/Sils/uart_sils.cpp @@ -1,15 +1,18 @@ - #pragma section REPRO + +#include "uart_sils.h" + #include + + +#ifdef USE_UART_COM +#include "com_port.h" #include "../../Settings/port_config.h" -// #include "../../../../../../s2e_core_oss/src/Component/CDH/OBC_C2A.h" //It is difficut to include S2E file because of the difference of character codes +#include "../../Settings/sils_port_config.h" -#ifdef USE_SCI_COM_UART -#include "uart_sils_sci_if.h" +static ComPort uart_sils_com_port(COM_PORT_SILS); #endif -int OBC_C2A_SendFromObc(int port_id, unsigned char* buffer, int offset, int count); -int OBC_C2A_ReceivedByObc(int port_id, unsigned char* buffer, int offset, int count); int UART_init(void* my_uart_v) { @@ -20,10 +23,10 @@ int UART_rx(void* my_uart_v, void* data_v, int buffer_size) { UART_Config* my_uart = (UART_Config*)my_uart_v; -#ifdef USE_SCI_COM_UART +#ifdef USE_UART_COM if (my_uart->ch == PORT_CH_UART_MOBC) { - return SILS_SCI_UART_IF_rx((unsigned char*)data_v, buffer_size); + return uart_sils_com_port.receive((unsigned char*)data_v, 0, buffer_size); } else { @@ -38,10 +41,10 @@ int UART_tx(void* my_uart_v, void* data_v, int data_size) { UART_Config* my_uart = (UART_Config*)my_uart_v; -#ifdef USE_SCI_COM_UART +#ifdef USE_UART_COM if (my_uart->ch == PORT_CH_UART_MOBC) { - SILS_SCI_UART_IF_tx((unsigned char*)data_v, data_size); + uart_sils_com_port.send((unsigned char*)data_v, 0, data_size); } else { diff --git a/src/src_user/IfWrapper/Sils/uart_sils.h b/src/src_user/IfWrapper/Sils/uart_sils.h new file mode 100644 index 00000000..34de1aff --- /dev/null +++ b/src/src_user/IfWrapper/Sils/uart_sils.h @@ -0,0 +1,18 @@ +/** + * @file uart_sils.h + * @brief SILS用UART Interface Wrapper + */ + +#ifndef UART_SILS_H_ +#define UART_SILS_H_ + + +int OBC_C2A_SendFromObc(int port_id, unsigned char* buffer, int offset, int count); +int OBC_C2A_ReceivedByObc(int port_id, unsigned char* buffer, int offset, int count); + +int UART_init(void* my_uart_v); +int UART_rx(void* my_uart_v, void* data_v, int buffer_size); +int UART_tx(void* my_uart_v, void* data_v, int data_size); +int UART_reopen(void* my_uart_v, int reason); + +#endif diff --git a/src/src_user/IfWrapper/SILS/WDT_SILS.c b/src/src_user/IfWrapper/Sils/wdt_sils.c similarity index 100% rename from src/src_user/IfWrapper/SILS/WDT_SILS.c rename to src/src_user/IfWrapper/Sils/wdt_sils.c diff --git a/src/src_user/Library/Orbit/kepler_orbit.c b/src/src_user/Library/Orbit/kepler_orbit.c index 1eb08527..6d7278f3 100644 --- a/src/src_user/Library/Orbit/kepler_orbit.c +++ b/src/src_user/Library/Orbit/kepler_orbit.c @@ -24,7 +24,7 @@ void KEPLER_ORBIT_init_constants(KeplerOrbitConstants* kepler_orbit_constants, // 平均運動を求める TODO_L:計算精度を考えると平均運動をアップリンクするほうが良いかも float earth_gravity_const_km3_s2 = PHYSICAL_CONST_EARTH_GRAVITY_CONST_m3_s2 / 1.0e9f; float semi_major_axis_km3 = powf(orbital_elements.semi_major_axis_km, 3.0f); - kepler_orbit_constants->mean_motion_rad_s = sqrtf(earth_gravity_const_km3_s2 / semi_major_axis_km3); + kepler_orbit_constants->mean_motion_rad_s = C2A_MATH_sqrtf(earth_gravity_const_km3_s2 / semi_major_axis_km3); // 座標変換を求める float dcm_arg_perigee[PHYSICAL_CONST_THREE_DIM][PHYSICAL_CONST_THREE_DIM]; @@ -69,7 +69,7 @@ C2A_MATH_ERROR KEPLER_ORBIT_calc_position(float position_eci_km[PHYSICAL_CONST_T // 軌道面内の位置速度を求める float cos_u = cosf(u_rad); float sin_u = sinf(u_rad); - float a_sqrt_e_km = a_km * sqrtf(1.0f - e * e); + float a_sqrt_e_km = a_km * C2A_MATH_sqrtf(1.0f - e * e); float e_cos_u = 1.0f - e * cos_u; float pos_inplane_km[PHYSICAL_CONST_THREE_DIM]; @@ -181,9 +181,9 @@ C2A_MATH_ERROR KEPLER_ORBIT_calc_oe_from_pos_vel(KeplerOrbitalElements* orbital_ if (eccentricity > KEPLER_ORBIT_ECCENTRICITY_THRESHOLD) { float e_cosE = 1.0f - r_km / semi_major_axis_km; - float e_sinE = VECTOR3_inner_product(position_eci_km, velocity_eci_km_s) / sqrtf(mu_km3_s2 * semi_major_axis_km); + float e_sinE = VECTOR3_inner_product(position_eci_km, velocity_eci_km_s) / C2A_MATH_sqrtf(mu_km3_s2 * semi_major_axis_km); float E_rad = atan2f(e_sinE, e_cosE); - float n = sqrtf(mu_km3_s2 / semi_major_axis_km / semi_major_axis_km / semi_major_axis_km); + float n = C2A_MATH_sqrtf(mu_km3_s2 / semi_major_axis_km / semi_major_axis_km / semi_major_axis_km); epoch_jday = current_time_jday - (double)((E_rad - e_sinE) / n / PHYSICAL_CONST_EARTH_SOLAR_DAY_s); } else // e->0のとき近点引数の代わりに緯度引数を用いるため、現在時刻がそのまま近心点通過時刻となる diff --git a/src/src_user/Library/Orbit/sgp4.c b/src/src_user/Library/Orbit/sgp4.c index f1145d59..a1c88ca2 100644 --- a/src/src_user/Library/Orbit/sgp4.c +++ b/src/src_user/Library/Orbit/sgp4.c @@ -306,7 +306,7 @@ static C2A_MATH_ERROR SGP4_calc_secular_constants_(Sgp4SecularConstants* secular if (fabsf(ao - SGP4_kDensityFunc) < MATH_CONST_PROHIBIT_DIVISION_VALUE) return C2A_MATH_ERROR_SINGULAR; float xi = 1.0f / (ao - SGP4_kDensityFunc); - float beta = sqrtf(1.0f - e * e); + float beta = C2A_MATH_sqrtf(1.0f - e * e); if (fabsf(beta) < MATH_CONST_PROHIBIT_DIVISION_VALUE) return C2A_MATH_ERROR_SINGULAR; float eta = ao * e * xi; @@ -598,10 +598,10 @@ static C2A_MATH_ERROR SGP4_calc_short_period_term_(Sgp4DerivedElement* derived_e float position = a * (1.0f - e_cos); if (fabs(position) < MATH_CONST_PROHIBIT_DIVISION_VALUE) return C2A_MATH_ERROR_SINGULAR; - float velocity = sqrtf(a) / position * e_sin; - float velocity_f = sqrtf(p_l) / position; + float velocity = C2A_MATH_sqrtf(a) / position * e_sin; + float velocity_f = C2A_MATH_sqrtf(p_l) / position; - float coeff_temp = e_sin / (1.0f + sqrtf(1.0f - e_l2)); + float coeff_temp = e_sin / (1.0f + C2A_MATH_sqrtf(1.0f - e_l2)); float cos_u = cosf(eo) - a_x_n + a_y_n * coeff_temp; // sin_uと割り算取るのでどちらもa/rはかけない float sin_u = sinf(eo) - a_y_n - a_x_n * coeff_temp; float u = atan2f(sin_u, cos_u); @@ -622,7 +622,7 @@ static C2A_MATH_ERROR SGP4_calc_short_period_term_(Sgp4DerivedElement* derived_e // 補正後の値 derived_element->position_ = d_position + position * (1.0f - 0.75f * j2 * (3.0f * cos_i2 - 1.0f) * - sqrtf(1.0f - e_l2) / (p_l * p_l)); + C2A_MATH_sqrtf(1.0f - e_l2) / (p_l * p_l)); derived_element->true_anomaly_rad_ = u - d_true_anomaly_rad; derived_element->raan_rad_ = raan_rad + d_raan_rad; derived_element->inclination_rad_ = inclination_rad + d_inclination_rad; diff --git a/src/src_user/Library/c2a_math.c b/src/src_user/Library/c2a_math.c index 08e28141..4c9bc027 100644 --- a/src/src_user/Library/c2a_math.c +++ b/src/src_user/Library/c2a_math.c @@ -165,4 +165,13 @@ float C2A_MATH_acos_rad(const float input_cos) return acosf(limited_input); } +float C2A_MATH_sqrtf(const float input) +{ + if (input <= 0.0f) + { + return 0.0f; + } + return sqrtf(input); +} + #pragma section diff --git a/src/src_user/Library/c2a_math.h b/src/src_user/Library/c2a_math.h index e27d0125..5a03626e 100644 --- a/src/src_user/Library/c2a_math.h +++ b/src/src_user/Library/c2a_math.h @@ -151,4 +151,11 @@ C2A_MATH_ERROR C2A_MATH_calculate_time_derivative(float out, */ float C2A_MATH_acos_rad(const float input_cos); +/** + * @brief 負値が入力されたら0を返す様に拡張したsqrtf関数 + * @param input : 入力値 + * @return 平方根を取った値 + */ +float C2A_MATH_sqrtf(const float input); + #endif diff --git a/src/src_user/Library/geomagnetism.c b/src/src_user/Library/geomagnetism.c index ee7d1d76..d7df2e41 100644 --- a/src/src_user/Library/geomagnetism.c +++ b/src/src_user/Library/geomagnetism.c @@ -423,7 +423,7 @@ static void GEOMAGNETISM_calc_gauss_coeffs_(float gnm[GEOMAGNETISM_IGRF_ORDER_MA gnm[0] = geomag_coeff_.igrf_gnm_nT[n - 1][0] + decimal_year * geomag_coeff_.igrf_gtnm_nT[n - 1][0]; hnm[0] = geomag_coeff_.igrf_hnm_nT[n - 1][0] + decimal_year * geomag_coeff_.igrf_htnm_nT[n - 1][0]; - float factorial_coeff = sqrtf(2.0f); //!< factorial_coeff(n,m) = [2(n-m)!/(n+m)!]^(1/2) (for m >0) + float factorial_coeff = C2A_MATH_sqrtf(2.0f); //!< factorial_coeff(n,m) = [2(n-m)!/(n+m)!]^(1/2) (for m >0) float devide_coeff = 0.0; for (uint8_t m = 1; m <= n; m++) { @@ -431,11 +431,11 @@ static void GEOMAGNETISM_calc_gauss_coeffs_(float gnm[GEOMAGNETISM_IGRF_ORDER_MA // factorial_coeff(m) = factorial_coeff(m-1)/[(n+m)]^(1/2) (for m = n) if (m == n) { - devide_coeff = sqrtf((float)((n + m))); + devide_coeff = C2A_MATH_sqrtf((float)((n + m))); } else { - devide_coeff = sqrtf((float)((n + m)*(n - m + 1))); + devide_coeff = C2A_MATH_sqrtf((float)((n + m)*(n - m + 1))); } factorial_coeff = factorial_coeff / devide_coeff; diff --git a/src/src_user/Library/quaternion.c b/src/src_user/Library/quaternion.c index 1fa5f92d..5815650c 100644 --- a/src/src_user/Library/quaternion.c +++ b/src/src_user/Library/quaternion.c @@ -101,7 +101,7 @@ C2A_MATH_ERROR QUATERNION_make_from_matrix(Quaternion* q_out, float trace = MATRIX33_trace(matrix_n); if (trace > 0.0f) { - float q_scalar_seed = sqrtf(trace + 1.0f); + float q_scalar_seed = C2A_MATH_sqrtf(trace + 1.0f); q_before_check.scalar_part = 0.5f * q_scalar_seed; q_before_check.vector_part[0] = (matrix_n[1][2] - matrix_n[2][1]) / (2.0f * q_scalar_seed); q_before_check.vector_part[1] = (matrix_n[2][0] - matrix_n[0][2]) / (2.0f * q_scalar_seed); @@ -110,10 +110,10 @@ C2A_MATH_ERROR QUATERNION_make_from_matrix(Quaternion* q_out, else { float q_candidate[PHYSICAL_CONST_QUATERNION_DIM]; - q_candidate[0] = 0.5f * sqrtf(1.0f + matrix_n[0][0] - matrix_n[1][1] - matrix_n[2][2]); - q_candidate[1] = 0.5f * sqrtf(1.0f - matrix_n[0][0] + matrix_n[1][1] - matrix_n[2][2]); - q_candidate[2] = 0.5f * sqrtf(1.0f - matrix_n[0][0] - matrix_n[1][1] + matrix_n[2][2]); - q_candidate[3] = 0.5f * sqrtf(1.0f + matrix_n[0][0] + matrix_n[1][1] + matrix_n[2][2]); + q_candidate[0] = 0.5f * C2A_MATH_sqrtf(1.0f + matrix_n[0][0] - matrix_n[1][1] - matrix_n[2][2]); + q_candidate[1] = 0.5f * C2A_MATH_sqrtf(1.0f - matrix_n[0][0] + matrix_n[1][1] - matrix_n[2][2]); + q_candidate[2] = 0.5f * C2A_MATH_sqrtf(1.0f - matrix_n[0][0] - matrix_n[1][1] + matrix_n[2][2]); + q_candidate[3] = 0.5f * C2A_MATH_sqrtf(1.0f + matrix_n[0][0] + matrix_n[1][1] + matrix_n[2][2]); uint8_t max_value_index = 0; float max_value_in_candidate = fabsf(q_candidate[0]); @@ -427,7 +427,7 @@ float QUATERNION_calc_norm(const Quaternion q_in) { float pow2_norm = q_in.scalar_part * q_in.scalar_part + VECTOR3_inner_product(q_in.vector_part, q_in.vector_part); - return (sqrtf(pow2_norm)); + return (C2A_MATH_sqrtf(pow2_norm)); } diff --git a/src/src_user/Library/time_space.c b/src/src_user/Library/time_space.c index 708c0c7e..de206e1e 100644 --- a/src/src_user/Library/time_space.c +++ b/src/src_user/Library/time_space.c @@ -176,7 +176,7 @@ C2A_MATH_ERROR TIME_SPACE_convert_geodetic_to_geocentric(float* colat_rad, float float rp_earth_sin_lat = PHYSICAL_CONST_EARTH_POLAR_RADIUS_m * sinf(lat_rad); //!< Rm^2 = (Re*cos_lat)^2 + (Rp*sin_lat)^2 float pow2_radius_mean = re_earth_cos_lat * re_earth_cos_lat + rp_earth_sin_lat * rp_earth_sin_lat; - float radius_mean = sqrtf(pow2_radius_mean); //!< mean radius at the interest point + float radius_mean = C2A_MATH_sqrtf(pow2_radius_mean); //!< mean radius at the interest point //!< Re^2 float pow2_earth_re = PHYSICAL_CONST_EARTH_RADIUS_m * PHYSICAL_CONST_EARTH_RADIUS_m; @@ -193,7 +193,7 @@ C2A_MATH_ERROR TIME_SPACE_convert_geodetic_to_geocentric(float* colat_rad, float //!< r^2 = 1st_term of r^2 + 2h*Rm + h^2 float pow2_radius_m = pow2_radious_m_1st_term + 2.0f * height_m * radius_mean + height_m * height_m; - *radious_m = sqrtf(pow2_radius_m); + *radious_m = C2A_MATH_sqrtf(pow2_radius_m); //!< cos(colat) = (sin_lat/r)*(h + (Rp^2)/Rm) float cos_colat = (sinf(lat_rad) / *radious_m) * (height_m + pow2_earth_rp / radius_mean); diff --git a/src/src_user/Library/vector3.c b/src/src_user/Library/vector3.c index f75f2f91..470733ff 100644 --- a/src/src_user/Library/vector3.c +++ b/src/src_user/Library/vector3.c @@ -192,7 +192,7 @@ void VECTOR3_copy_double(double out[PHYSICAL_CONST_THREE_DIM], float VECTOR3_norm(const float vec[PHYSICAL_CONST_THREE_DIM]) { - return sqrtf(VECTOR3_inner_product(vec, vec)); + return C2A_MATH_sqrtf(VECTOR3_inner_product(vec, vec)); } C2A_MATH_ERROR VECTOR3_normalize(float out[PHYSICAL_CONST_THREE_DIM], diff --git a/src/src_user/Settings/SatelliteParameters/Sample/ina260_parameters.c b/src/src_user/Settings/SatelliteParameters/Sample/ina260_parameters.c index 49dce5a4..1539318f 100644 --- a/src/src_user/Settings/SatelliteParameters/Sample/ina260_parameters.c +++ b/src/src_user/Settings/SatelliteParameters/Sample/ina260_parameters.c @@ -20,7 +20,7 @@ const float INA260_PARAMETERS_stim210_hw_over_current_threshold_mA = 1000.0f; // SAGITTA const INA260_AVERAGING_MODE INA260_PARAMETERS_sagitta_averaging_mode = INA260_AVERAGING_MODE_16; const INA260_CONVERSION_TIME INA260_PARAMETERS_sagitta_voltage_conversion_time = INA260_CONVERSION_TIME_140US; -const INA260_CONVERSION_TIME INA260_PARAMETERS_sagitta_current_conversion_time = INA260_CONVERSION_TIME_140US; +const INA260_CONVERSION_TIME INA260_PARAMETERS_sagitta_current_conversion_time = INA260_CONVERSION_TIME_204US; const float INA260_PARAMETERS_sagitta_hw_over_current_threshold_mA = 500.0f; // OEM7600 diff --git a/src/src_user/Settings/sils_port_config.h b/src/src_user/Settings/sils_port_config.h new file mode 100644 index 00000000..e10c9cab --- /dev/null +++ b/src/src_user/Settings/sils_port_config.h @@ -0,0 +1,15 @@ +#ifndef SILS_PORT_CONFIG_H_ +#define SILS_PORT_CONFIG_H_ + +// ============================================ // +// SILS関連のポート設定 // +// ============================================ // +#ifdef USE_UART_COM +#ifdef WIN32 +#define COM_PORT_SILS (11) //!< COM11 +#else +#define COM_PORT_SILS (1) //!< /dev/tnt1 +#endif +#endif + +#endif