diff --git a/Opener.cpp b/Opener.cpp new file mode 100644 index 0000000..1ddf053 --- /dev/null +++ b/Opener.cpp @@ -0,0 +1,272 @@ +/** + * @file Opener/Opener.cpp + * @brief ハイブリッドロケットの開放判定を行うライブラリ + * @details ハイブリッドロケットの開放判定を行うライブラリ + **/ + +#include +#include "Opener.h" + +int arraycmp(const void *p1, const void *p2) +{ + return *(const float *)p1 > *(const float *)p2; +} + +bool is_odd_number(int n) +{ + return n & 0x1; +} + +float get_median(int n, float a[]) +{ + int k = n / 2; + + qsort(a, n, sizeof(float), arraycmp); + + if (is_odd_number(n)) + { + return a[k]; + } + else + { + return (float)(a[k - 1] + a[k]) / 2; + } +} + +uint32_t OPENER::get_time_ms() +{ + // for raspberry pi pico SDK + // return to_ms_since_boot(get_absolute_time()); + + // for Arduino + return millis(); +} + +/*! + * @brief 100Hzで呼び出す必要がある関数 + * @param acceleration_mss 機軸方向の加速度[m/s^2](ランチャ上で重力加速度がかかって正の値をとる方向) + * @param altitude_m 高度[m](基準高度は任意) + * @return 判定が更新されたか場合trueを返す.更新がない場合はfalseを返す. + */ +bool OPENER::opener_100Hz(float acceleration_mss, float altitude_m) +{ + before_100Hz_time = get_time_ms(); + ACC_buf_mss[count_100Hz] = acceleration_mss; + ALT_buf_mss[count_100Hz] = altitude_m; + + // 10Hz + count_100Hz++; + if (count_100Hz >= 10) + { + count_100Hz = 0; + + opener_10Hz(); + return true; + } + return false; +} +/*! + * @brief 加速度・高度の中央値を計算し,離床判定・開放判定を行う + */ +void OPENER::opener_10Hz() +{ + bool normal_datarate = get_time_ms() - before_10Hz_time < period_10Hz_ms; + before_10Hz_time = get_time_ms(); + + float acceleration_median_mss = get_median(10, ACC_buf_mss); + float altitude_median_m = get_median(10, ALT_buf_mss); + + ALT_count++; + if (ALT_count == 1) + { + bool climbing = altitude_median_m > before_altitude_m + lift_off_threshold_altitude_m; + if (climbing && normal_datarate) + { + if (lift_off_altitude_count < ALT_threshold_count){ + lift_off_altitude_count++; + } + } + else + { + lift_off_altitude_count = 0; + } + + bool descending = altitude_median_m < before_altitude_m - open_threshold_altitude_m; + if (descending && normal_datarate) + { + if (apogee_altitude_count < ALT_threshold_count){ + apogee_altitude_count++; + } + } + else + { + apogee_altitude_count = 0; + } + + before_altitude_m = altitude_median_m; + } + if (ALT_count >= ALT_oversampling_count) + { + ALT_count = 0; + } + + bool accelerating = acceleration_median_mss > lift_off_threshold_ac_mss; + if (accelerating) + { + if (lift_off_acc_count <= ACC_threshold_count){ + lift_off_acc_count++; + } + } + else + { + lift_off_acc_count = 0; + } + + bool freefall = acceleration_median_mss < open_threshold_ac_mss; + if (freefall) + { + if (meco_acc_count <= ACC_threshold_count){ + meco_acc_count++; + } + } + else + { + meco_acc_count = 0; + } + + if (mode == READY) + { + if (lift_off_altitude_count >= ALT_threshold_count) + { + goFLIGHT(); + lift_off_judge = ALTSEN; + } + if (lift_off_acc_count >= ACC_threshold_count) + { + goFLIGHT(); + lift_off_judge = ACCSEN; + } + } + + if (mode == FLIGHT) + { + open_judge.meco_time = get_time_ms() - lift_off_time_ms >= meco_threshold_time_ms; + open_judge.meco_acc = meco_acc_count >= ACC_threshold_count; + bool meco = open_judge.meco_time && open_judge.meco_acc; + + open_judge.apogee_time = get_time_ms() - lift_off_time_ms >= open_threshold_time_ms; + open_judge.apogee_descending = apogee_altitude_count >= ALT_threshold_count; + bool apogee = open_judge.apogee_time || open_judge.apogee_descending; + + if (!open_judge.prohibitOpen && meco && apogee) + { + goOPENED(); + } + } +} + +/*! + * @brief ::CHECKモードへ移行する.変数の初期化し,::prohibitOpenを有効化する. + */ +void OPENER::goCHECK() +{ + mode = CHECK; + open_judge.isOpend = false; + open_judge.prohibitOpen = true; + open_judge.meco_time = false; + open_judge.meco_acc = false; + open_judge.apogee_time = false; + open_judge.apogee_descending = false; + lift_off_judge = NONE; + lift_off_time_ms = 0; + + count_100Hz = 0; + + // oversampling + ALT_count = 0; + // altitude->velocity + before_altitude_m = 0; + // sensor_judgement + lift_off_altitude_count = 0; + lift_off_acc_count = 0; + apogee_altitude_count = 0; + meco_acc_count = 0; + close(); +} +/*! + * @brief goCHECK()を実行して変数を初期化してから, prohibitOpen を無効化したうえで READY モードへ移行する. + */ +void OPENER::goREADY() +{ + goCHECK(); + open_judge.prohibitOpen = false; + mode = READY; +} +/*! + * @brief FLIGHT モードに移行し,離床時刻を記録する. + */ +void OPENER::goFLIGHT() +{ + mode = FLIGHT; + lift_off_time_ms = get_time_ms() - flight_judgement_duration_ms; +} +/*! + * @brief OPENED モードに移行し,開放時刻を記録する. + */ +void OPENER::goOPENED() +{ + mode = OPENED; + open_judge.isOpend = true; + open_time_ms = get_time_ms(); + open(); +} +/*! + * @brief prohibitOpen を無効化する. + */ +void OPENER::clear_prohibitOpen() +{ + open_judge.prohibitOpen = false; +} +/*! + * @brief prohibitOpen を有効化する. + */ +void OPENER::prohibitOpen() +{ + open_judge.prohibitOpen = true; +} + +/*! + * @brief prohibitOpen が無効になっていることを確認し,手動で開放機構を開く. + */ +void OPENER::manualOpen() +{ + if (!open_judge.prohibitOpen) + { + open(); + } +} +/*! + * @brief 手動で開放機構を閉じ, prohibitOpen を有効化する. + */ +void OPENER::manualClose() +{ + close(); + open_judge.prohibitOpen = true; +} + +/*! + * @brief 開放までのタイマーの秒数を設定する. + * @param _open_threshold_time_ms 開放機構の動作にかかる時間を引いた,離床から開放までの時間のシム値[ms] + */ +void OPENER::set_open_threshold_time_ms(uint32_t _open_threshold_time_ms) +{ + open_threshold_time_ms = _open_threshold_time_ms; +} + +/*! + * @brief 開放までのタイマーの秒数を取得する. + * @return 開放機構の動作にかかる時間を引いた,離床から開放までの時間のシム値[ms] + */ +uint32_t OPENER::get_open_threshold_time_ms(){ + return open_threshold_time_ms; +} \ No newline at end of file diff --git a/Opener.h b/Opener.h new file mode 100644 index 0000000..5c13dd7 --- /dev/null +++ b/Opener.h @@ -0,0 +1,180 @@ +/** + * @file Opener/Opener.h + * @subpage opener_class + * @brief ハイブリッドロケットの開放機構の制御を行うライブラリ + * @details ハイブリッドロケットの開放機構の制御を行うライブラリ + **/ + +#ifndef OPENER_H +#define OPENER_H + +#include +//#include "pico/stdlib.h" + +int arraycmp(const void *p1, const void *p2); +bool is_odd_number(int n); +float get_median(int n, float a[]); + +/*! + * @brief ハイブリッドロケットの開放機構の制御を行うクラス + */ +class OPENER +{ +private: + /** + * @brief 開放判定の判定結果が格納される + **/ + typedef struct + { + //! 開放機構を開くべきか + bool isOpend = false; + //! 手動で開放禁止されているか + bool prohibitOpen = true; + //! 離床判定からの時間により燃焼終了したと判断したか + bool meco_time = false; + //! 加速度がかかっていないことにより燃焼終了したと判断したか + bool meco_acc = false; + //! 離床判定からの時間により頂点到達したと判断したか + bool apogee_time = false; + //! 下降を検知し頂点到達したと判断したか + bool apogee_descending = false; + } OPEN_JUDGE; + + void goFLIGHT(); + void goOPENED(); + uint32_t get_time_ms(); + + // timer + uint32_t before_10Hz_time = 0; + uint32_t before_100Hz_time = 0; + uint32_t count_100Hz = 0; + void opener_10Hz(); + + // get median + float ALT_buf_mss[10]; + float ACC_buf_mss[10]; + + // oversampling + int ALT_count = 0; + + // altitude->velocity + float before_altitude_m = 0; + + // sensor_judgement + int lift_off_altitude_count = 0; + int lift_off_acc_count = 0; + int apogee_altitude_count = 0; + int meco_acc_count = 0; + + // threshold + float lift_off_threshold_altitude_m = 0; + float lift_off_threshold_ac_mss = 0; + int ALT_oversampling_count = 0; + int ALT_threshold_count = 0; + +protected: + //! 頂点判定に使われる,0.1秒間の高度変化の閾値 + const float open_threshold_altitude_m = 0.5; + //! 燃焼終了に使われる,加速度の閾値 + const float open_threshold_ac_mss = 5; + //! opener_10Hz()が適切なタイミングで呼ばれているかの判定に使われる,10Hzの周期 + const uint32_t period_10Hz_ms = 150; // 10Hz+50ms + //! opener_100Hz()が適切なタイミングで呼ばれているかの判定に使われる,100Hzの周期 + const uint32_t period_100Hz_ms = 20; // 100Hz+10ms + //! 閾値以上の加速度が何回連続したときに離床判定・燃焼終了判定を行うかという回数 + const int ACC_threshold_count = 5; + //! 離床判定にかかる時間 + const int flight_judgement_duration_ms = ACC_threshold_count * 100; + + float fm_lift_off_threshold_altitude_m = 1.0; + float fm_lift_off_threshold_ac_mss = 25.0; + int fm_ALT_oversampling_count = 1; + int fm_ALT_threshold_count = ACC_threshold_count; + + float shinsasyo_lift_off_threshold_altitude_m = 0.5; + float shinsasyo_lift_off_threshold_ac_mss = 9.0; + int shinsasyo_ALT_oversampling_count = 5.0; + int shinsasyo_ALT_threshold_count = 2; + + //! 離床判定後,燃焼中と判断し開放判定を行わない時間[ms] + const uint32_t meco_threshold_time_ms = 10000; + //! 開放機構の動作にかかる時間を引いた,離床から開放までの時間のシム値[ms]の初期値 + uint32_t open_threshold_time_ms = 19000; + + //! 開放機構を開く + virtual void open() = 0; + //! 開放機構を閉じる + virtual void close() = 0; + +public: + enum SETTING + { + //! 閾値をフライト用に設定 + FM, + //! 閾値を審査書用に設定 + SHINSASYO + }; + /*! OPENERのコンストラクタ + * @param _setting 閾値をフライト用に設定するか,審査書用に設定するか + */ + OPENER(SETTING _setting) + { + if (_setting == FM) + { + lift_off_threshold_altitude_m = fm_lift_off_threshold_altitude_m; + lift_off_threshold_ac_mss = fm_lift_off_threshold_ac_mss; + ALT_oversampling_count = fm_ALT_oversampling_count; + ALT_threshold_count = fm_ALT_threshold_count; + } + else if (_setting == SHINSASYO) + { + lift_off_threshold_altitude_m = shinsasyo_lift_off_threshold_altitude_m; + lift_off_threshold_ac_mss = shinsasyo_lift_off_threshold_ac_mss; + ALT_oversampling_count = shinsasyo_ALT_oversampling_count; + ALT_threshold_count = shinsasyo_ALT_threshold_count; + } + goCHECK(); + } + + enum MODE + { + //! 離床判定禁止 + CHECK, + //! 離床判定許可 + READY, + //! 離床判定後 + FLIGHT, + //! 開放判定後 + OPENED + } mode = CHECK; + enum LIFT_OFF_JUDGE + { + //! 離床判定前 + NONE, + //! 加速度により離床判定 + ACCSEN, + //! 高度上昇により離床判定 + ALTSEN + } lift_off_judge = NONE; + + void goCHECK(); + void goREADY(); + void clear_prohibitOpen(); + void prohibitOpen(); + + void manualOpen(); + void manualClose(); + + OPEN_JUDGE open_judge; + bool opener_100Hz(float acceleration_mss, float altitude_m); + + void set_open_threshold_time_ms(uint32_t _open_threshold_time_ms); + uint32_t get_open_threshold_time_ms(); + + //! 離床したと推定される時刻(離床判定が行われた時刻ではない) + uint32_t lift_off_time_ms; + //! 実際に開放した時刻 + uint32_t open_time_ms; +}; + +#endif \ No newline at end of file