diff --git a/Main/Main.ino b/Main/Main.ino index 9c29fe3..f4d0206 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -10,6 +10,9 @@ uint32_t data_gnss_longitude_udeg = 0; float data_bat_v = 0; float data_ext_v = 0; bool data_key_sw_active = false; +int emst = 0; +bool accelopen = false, altitudeopen = false; + // pinout const pin_size_t MIF_TX = 2; @@ -18,8 +21,6 @@ const pin_size_t BNO_SDA = 4; const pin_size_t BNO_SCL = 5; const pin_size_t VALVE_TX = 6; const pin_size_t VALVE_RX = 7; -const pin_size_t ES920_TX = 8; -const pin_size_t ES920_RX = 9; const pin_size_t BME_SDA = 10; const pin_size_t BME_SCL = 11; const pin_size_t E220_TX = 12; @@ -27,18 +28,22 @@ const pin_size_t E220_RX = 13; const pin_size_t GNSS_TX = 14; const pin_size_t GNSS_RX = 15; const pin_size_t KEY_SW = 20; -// const pin_size_t SERVO_1 = 21; -// const pin_size_t SERVO_2 = 22; const pin_size_t EXT_V = 26; const pin_size_t BAT_V = 27; // Servo - +#include +const pin_size_t SERVO_1 = 21; +const pin_size_t SERVO_2 = 22; +Servo servo1; +Servo servo2; +int servo_close_time = 0; // KeySW // BNO055 +#include // 必要なヘッダーファイルをインクルード #include #include #include "Adafruit_BNO055.h" @@ -46,10 +51,38 @@ const pin_size_t BAT_V = 27; Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire); sensors_event_t accelerometerData; +sensors_event_t* event; +bool acceljudge_gettingout = false; //加速度による離床判定 +bool acceljudge_open = false; //加速度による開放判定 +int accelcount = 0; //閾値を5回満たすカウントをする +int phase_now = 0; //現在のフェーズを区別(0=CHEAK,1=READY,2=FLIGHT,3=OPENED) +int Openjudge = 1; //開放禁止コマンド用状態表示用(0=NOT_OPEN,1=OPEN) +double BNO[10]; //BNOデータ取得、格納に用いる +unsigned long time_data_ms = 0; //離床判定タイマー(燃焼終了検知) +bool mecotime_data_judge_ms = false; //燃焼終了検知 +bool apogeetime_data_judge_ms = false; //頂点到達検知 + +//BNO設定 +/* Set the delay between fresh samples */ +uint16_t BNO055_SAMPLERATE_DELAY_MS = 100; + +// Check I2C device address and correct line below (by default address is 0x29 or 0x28) +// id, address + // BME280 +//BME280のライブラリーを取り込みます。 #include +//1013.25は地球の海面上の大気圧の平均値(1気圧)です。 +#define SEALEVELPRESSURE_HPA (1013.25) + Adafruit_BME280 bme; -const float SEALEVELPRESSURE_HPA = 1013.25; +unsigned long delayTime; +float temperature; //気温 +float altitude; //高度 +bool altitudejudge_gettingout = false; //高度による離床判定 +bool altitudejudge_open = false; //高度による開放判定 +int altitudecount = 0; +double BME[10]; //BMEデータ取得、格納に用いる // SAM-M8Q #include @@ -61,15 +94,11 @@ SerialPIO Serial_GNSS(GNSS_TX, GNSS_RX, 512); #define Serial_ES920 Serial2 // https://moyoi-memo.hatenablog.com/entry/2022/02/15/112100 String downlink = ""; -String response = ""; -bool need_response_usb = false; -bool need_response_es920 = false; +String uplink = ""; +String uplink_string = ""; // W25Q128 -// Opener -#include "myOpener.h" -MY_OPENER opener(OPENER::SHINSASYO); // Valve char valve_mode = '/'; @@ -92,11 +121,12 @@ void setup() { Serial.print("BNO055 ERR"); delay(100); } + bno.setMode(OPERATION_MODE_CONFIG); delay(25); uint8_t savePageID = bno.read8(Adafruit_BNO055::BNO055_PAGE_ID_ADDR); bno.write8(Adafruit_BNO055::BNO055_PAGE_ID_ADDR, 0X01); - bno.write8(Adafruit_BNO055::BNO055_ACCEL_DATA_X_LSB_ADDR, 0X17); // page2なのでホントはACC_DATA_X_LSBではなくACC_DATA_X_LSBにアクセス + bno.write8(Adafruit_BNO055::BNO055_ACCEL_DATA_X_LSB_ADDR, 0X17); // page2なのでホントはACC_DATA_X_LSBではなくACC_DATA_X_LSBにアクセス delay(10); bno.write8(Adafruit_BNO055::BNO055_PAGE_ID_ADDR, savePageID); bno.setMode(OPERATION_MODE_ACCONLY); @@ -117,10 +147,10 @@ void setup() { Adafruit_BME280::STANDBY_MS_0_5); analogReadResolution(12); - pinMode(KEY_SW, INPUT); + // pinMode(KEY_SW, INPUT); - Serial_ES920.setTX(ES920_TX); - Serial_ES920.setRX(ES920_RX); + Serial_ES920.setTX(8); + Serial_ES920.setRX(9); Serial_ES920.setFIFOSize(64); Serial_ES920.begin(115200); while (Serial_ES920.available()) { @@ -131,7 +161,9 @@ void setup() { Serial_Valve.begin(115200); Serial_MIF.begin(115200); - opener.init(); + //servo + servo1.attach(SERVO_1); + servo2.attach(SERVO_2); } // loop()と,ここから呼び出される関数ではdelay()使用禁止 @@ -146,83 +178,187 @@ void loop() { count_10Hz = 0; // 10Hzで実行する処理 - + getevent(); data_bat_v = analogRead(BAT_V) * 3.3 * 11 / (1 << 12); data_ext_v = analogRead(EXT_V) * 3.3 * 11 / (1 << 12); // フライト = 回路としてOPEN = LOW // バッテリー駆動でなくUSB駆動の場合常にLOWなので除外 + data_key_sw_active = (digitalRead(KEY_SW) == LOW) && data_bat_v > 1; static bool last_data_key_sw_active = false; - if (data_key_sw_active && !last_data_key_sw_active && opener.mode == OPENER::CHECK) { - opener.goREADY(); - } - if (!data_key_sw_active && last_data_key_sw_active) { - if (opener.open_judge.prohibitOpen) { - opener.goCHECK(); - } else { - opener.goCHECK(); - opener.clear_prohibitOpen(); - } + if (data_key_sw_active && !last_data_key_sw_active && phase_now == 0) { + phase_now = 1; } + // if (!data_key_sw_active && last_data_key_sw_active) { + // if (opener.open_judge.prohibitOpen) { + // gocheck(); + // } else { + // gocheck(); + // opener.clear_prohibitOpen(); + // } + // } last_data_key_sw_active = data_key_sw_active; // デバッグ出力 - if (need_response_usb) { - Serial.println("response:" + response); - need_response_usb = false; + /* + Serial.print("BNO055, "); + Serial.print(data_bno_accel_x_mss); + Serial.print(", "); + Serial.print(data_bno_accel_y_mss); + Serial.print(", "); + Serial.print(data_bno_accel_z_mss); + Serial.print(", "); + + Serial.print("BNO055, "); + Serial.print(data_bme_pressure_hPa); + Serial.print(", "); + Serial.print(data_bme_temperature_degC); + Serial.print(", "); + Serial.print(data_bme_altitude_m); + Serial.print(", "); + + Serial.print("GNSS, "); + Serial.print(data_gnss_latitude_udeg); + Serial.print(", "); + Serial.print(data_gnss_latitude_udeg); + Serial.print(", "); + */ + // if (need_response_usb) { + // Serial.println("response:" + response); + // need_response_usb = false; + // } else { + // Serial.println(downlink); + // } + } + + //100Hz + // BME280から100Hzで測定 + if (altitudejudge_gettingout == false) { //READYフェーズでのみ離床判定可能 + if (altitudecount > 2) { //2回連続で閾値を超えたときの処理 + altitudejudge_gettingout = true; //離床判定・フェーズ移行 + altitudecount = 0; //開放判定に向けた初期化 + } else { + if (calculateMediumBME() < -360) { + altitudecount++; + Serial.print(1); + } else { + altitudecount = 0; //初期化 + } + } + } + if ((altitudejudge_open == false) && (phase_now == 2)) { //READYフェーズのみ開放判定可能 + if (altitudecount > 2) { //2回連続で閾値を下回ったときの処理 + altitudejudge_open = true; //開放判定・フェーズ移行 + } + if (calculateMediumBME() > -350) { //開放判定閾値 + altitudecount++; } else { - Serial.println(downlink); + altitudecount = 0; //初期化 } - // Serial.print("BNO055, "); - // Serial.print(data_bno_accel_x_mss); - // Serial.print(", "); - // Serial.print(data_bno_accel_y_mss); - // Serial.print(", "); - // Serial.print(data_bno_accel_z_mss); - // Serial.print(", "); - - // Serial.print("BME280, "); - // Serial.print(data_bme_pressure_hPa); - // Serial.print(", "); - // Serial.print(data_bme_temperature_degC); - // Serial.print(", "); - // Serial.print(data_bme_altitude_m); - // Serial.print(", "); - - // Serial.print("GNSS, "); - // Serial.print(data_gnss_latitude_udeg); - // Serial.print(", "); - // Serial.print(data_gnss_longitude_udeg); - // Serial.print(", "); - - // Serial.print("voltage, "); - // Serial.print(data_bat_v); - // Serial.print(", "); - // Serial.print(data_ext_v); - // Serial.print(", "); - // Serial.print("\n"); } + } + + // 常に実行する処理 + + // コマンドアップリンク + //ES920LR設定用 + // if (Serial.available()) { + // downlink = Serial.readStringUntil('\n'); + // Serial.println(downlink); + // } + if (Serial.available()) { + char c = (char)Serial.read(); + Serial_ES920.write(c); + } + + // if (Serial_ES920.available()) { //動作検証用(使わない) + // uplink_string = Serial_ES920.read(); + // uplink_string.trim(); + // Serial.println(uplink_string); + // } + + uplink = ""; + if (Serial_ES920.available()) { + uplink = Serial_ES920.readStringUntil('\n'); + } + // if (uplink != "") { //動作検証用(使わない) + // Serial.print("uplink:"); + // Serial.println(uplink); + // } + //CHEACK・NOTOPEN・READYコマンド受信 + //READYコマンド(CHECKphaseであることが条件) + if (phase_now == 0) { + if (uplink.substring(0, uplink.length() - 1) == "READY") { //READYコマンド + phase_now = 1; // "READY"が入力された場合、phase_nowに1を代入する + Serial.println("READY_transitioned"); //動作確認用 + Serial.println(phase_now); + } + } + if (uplink.substring(0, uplink.length() - 1) == "emst") { + emst = 1; + Serial.println("emergency"); //動作確認用 + Serial.println("NOTopened"); + } + if (uplink.substring(0, uplink.length() - 1) == "CHECK") { //CHECKコマンド + gocheck(); // "CHECK"が入力された場合、phaseに0を代入 + Serial.println("CHECK_transitioned"); //動作確認用 + } - // 100Hzで実行する処理 + //離床判定 + if (phase_now == 1 && (altitudejudge_gettingout || acceljudge_gettingout)) { //READYフェーズに移行かつ、離床判定後(加速度or気圧で判定)である場合 + //高度による離床判定 + //10Hz処理に記述 - // BNO055から100Hzで測定 - bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER); - data_bno_accel_x_mss = accelerometerData.acceleration.x; - data_bno_accel_y_mss = accelerometerData.acceleration.y; - data_bno_accel_z_mss = accelerometerData.acceleration.z; + //加速度による離床判定 + //10Hz処理に記述 - // BME280から100Hzで測定 - data_bme_pressure_hPa = bme.readPressure() / 100.0F; - data_bme_temperature_degC = bme.readTemperature(); - data_bme_altitude_m = bme.readAltitude(SEALEVELPRESSURE_HPA); - bool new_judge = opener.opener_100Hz(-data_bno_accel_z_mss, data_bme_altitude_m); + //高度・加速度による離床判定 + if (acceljudge_gettingout) { //加速度による離床判定 + phase_now = 2; //フェーズ移行 + Serial.println("FLIGHT_accel_transitioned"); //シリアルモニタに状態移行を表示 + time_data_ms = millis(); //離床判定タイマー + } else if (altitudejudge_gettingout /*高度による離床判定条件*/) { //高度による離床判定 + phase_now = 2; //フェーズ移行 + Serial.println("FLIGHT_altitude_transitioned"); //シリアルモニタに状態移行を表示 + time_data_ms = millis(); //離床判定タイマー + } } + //開放判定 + if (phase_now == 2) { + //高度による開放判定 + //100Hz処理に記述する - // 常に実行する処理 + //加速度による開放判定 + //100Hz処理に記述 + + //高度・加速度による開放判定 + //加速度での開放判定 + if (millis() - time_data_ms > 5000 /*とりあえず燃焼時間5秒設定*/) { //時間による燃焼終了検知 + mecotime_data_judge_ms = true; + } + + if ((acceljudge_open) && (phase_now == 2) && (mecotime_data_judge_ms)) { + accelopen = true; + } + //高度による開放判定 + if (millis() - time_data_ms > 12000 /*とりあえず燃焼時間12秒設定*/) { //時間による頂点到達検知 + apogeetime_data_judge_ms = true; + } + if ((altitudejudge_open) || (apogeetime_data_judge_ms)) { + altitudeopen = true; + } + if (((accelopen) && (altitudeopen) && (emst == 0))) { //高度と加速度の閾値超過連続回数がともに5回以上かつ開放禁止コマンドが入力されていないとき + phase_now = 3; //フェーズ移行 + servo1.write(93); + servo2.write(93); + servo_close_time = millis(); + } + Serial.println("OPENED_transitioned"); + } // SAM-M8QのUARTを常に読み出し while (Serial_GNSS.available()) { @@ -232,43 +368,73 @@ void loop() { data_gnss_longitude_udeg = gps.location.lng() * 1000000; } } - - // バルブ電装状態受信 - while (Serial_Valve.available()) { - valve_mode = Serial_Valve.read(); + if (millis() - servo_close_time >= 5000) { + servo1.write(30); + servo2.write(30); } + // テレメトリ生成 downlink = ""; - switch (opener.mode) { - case OPENER::CHECK: - downlink += 'C'; - break; - case OPENER::READY: - downlink += 'R'; - break; - case OPENER::FLIGHT: - downlink += 'F'; - break; - case OPENER::OPENED: - downlink += 'O'; - break; + if (phase_now == 0) { + downlink += 'C'; + } else if (phase_now == 1) { + downlink += 'R'; + } else if (phase_now == 2) { + downlink += 'F'; + } else if (phase_now == 3) { + downlink += 'O'; + } + + if (emst == 0) { + downlink += '/'; // Emergency + } else { + downlink += 'E'; // Emergency + } + //時間による頂点到達 + if (phase_now >= 2) { + if (apogeetime_data_judge_ms) { + downlink += 'T'; // apogeeTime + } else { + downlink += '/'; //apogeeTime + } } - downlink += opener.open_judge.prohibitOpen ? 'E' : '/'; // Emergency - downlink += opener.open_judge.apogee_time ? 'T' : '/'; // Time - downlink += opener.open_judge.apogee_descending ? 'P' : '/'; // Pressure - downlink += opener.open_judge.meco_time ? 'T' : '/'; // Time - downlink += opener.open_judge.meco_acc ? 'A' : '/'; // Acceleration - if (opener.lift_off_judge == OPENER::NONE) { + //高度による頂点到達 + if (altitudejudge_open) { + downlink += 'P'; // Pressure + } else { downlink += '/'; } - if (opener.lift_off_judge == OPENER::ACCSEN) { + + //時間による燃焼終了 + if (phase_now >= 2) { + if (mecotime_data_judge_ms) { + downlink += 'T'; // mecoTime + } else { + downlink += '/'; // mecoTime + } + } else { + downlink += '/'; + } + //加速度による燃焼終了 + if (acceljudge_open) { downlink += 'A'; + } else { + downlink += '/'; } - if (opener.lift_off_judge == OPENER::ALTSEN) { + + //離床条件 + if (acceljudge_gettingout) { //高度と加速度のどっちで離床したかの判定 + downlink += 'A'; + } else if (altitudejudge_gettingout) { downlink += 'P'; + } else { + downlink += '/'; } + + + data_bme_altitude_m = bme.readAltitude(SEALEVELPRESSURE_HPA); downlink += valve_mode; downlink += String(data_key_sw_active ? 'K' : '/') + ','; // Key downlink += String(data_bno_accel_z_mss, 1) + ','; @@ -278,86 +444,105 @@ void loop() { downlink += String(data_gnss_longitude_udeg % 1000000) + ','; downlink += String(data_bat_v, 1) + ','; downlink += String(static_cast(data_ext_v)); + Serial.println(data_bno_accel_z_mss); // テレメトリダウンリンク const uint32_t downlink_rate_ms = 2500; static uint32_t last_downlink_ms = 0; if (millis() - last_downlink_ms > downlink_rate_ms) { last_downlink_ms = millis(); - if (need_response_es920) { - Serial_ES920.print("response:" + response + "\r\n"); - need_response_es920 = false; - } else { - Serial_ES920.print(downlink + "\r\n"); - } + Serial_ES920.println(downlink); + //Serial.println(downlink); } +} - // コマンドアップリンク - String uplink = ""; - if (Serial.available()) { - uplink = Serial.readStringUntil('\n'); +//BNO055の100Hzで測定する際に用いる関数 +//BNOからセンサ値が0でない値のみを返す関数 +double getBNO(sensors_event_t* event) { + bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER); + if (event->type == SENSOR_TYPE_ACCELEROMETER) { + data_bno_accel_x_mss = event->acceleration.x; + data_bno_accel_y_mss = event->acceleration.y; + data_bno_accel_z_mss = event->acceleration.z; + + } else { + Serial.print("Unk:"); } - if (Serial_ES920.available()) { - uplink = Serial_ES920.readStringUntil('\n'); + // Serial.print(data_bno_accel_z_mss ); + // Serial.print(","); + return data_bno_accel_z_mss; +} +//BNOの中央値算出 +double calculateMedianBNO(sensors_event_t* event) { + double BNO[10]; + for (int i = 0; i < 10; i++) { + BNO[i] = getBNO(event); } - uplink.trim(); - // if (uplink != "") { - // Serial.print("uplink:"); - // Serial.println(uplink); - // } + // 中央値を計算するために配列をソートする + std::sort(BNO, BNO + 10); + double median = BNO[4]; // 中央値 + return median; +} - if (uplink.indexOf("NG") == 0) { - Serial.println(uplink); +//BMEデータ中央値計算(気圧値) +double calculateMediumBME() { + double BME[4]; + double tmp = 0, medium = 0, Medium = 0, MEDIUM; + for (int i = 0; i < 4; i++) { + data_bme_temperature_degC = bme.readTemperature(); + BME[i] = bme.readAltitude(SEALEVELPRESSURE_HPA); } + // 中央値を計算するために配列をソートする + std::sort(BME, BME + 4); + medium = BME[2]; // 中央値 + Medium = MEDIUM; + MEDIUM = (Medium - medium) / 0.5; + Serial.print(data_bme_altitude_m); + Serial.print(","); + return MEDIUM; +} - if (uplink == "emst") { - opener.prohibitOpen(); - } - if (uplink == "clr") { - opener.clear_prohibitOpen(); - } - if (uplink == "open") { - opener.goCHECK(); - opener.clear_prohibitOpen(); - opener.manualOpen(); - } - if (uplink == "close") { - opener.goCHECK(); - opener.manualClose(); - } - if (uplink == "check") { - opener.goCHECK(); - } - if (uplink == "ready") { - opener.goREADY(); - } - if (uplink == "drain-start") { - Serial_Valve.print("drain-start\n"); - } - if (uplink == "drain-stop") { - Serial_Valve.print("drain-stop\n"); - } - if (uplink == "valve") { - Serial_Valve.print("valve\n"); - } - if (uplink == "valve-check") { - Serial_Valve.print("valve-check\n"); - } +void gocheck() { + acceljudge_gettingout = false; //加速度による離床判定 + acceljudge_open = false; //加速度による開放判定 + accelcount = 0; //閾値を5回満たすカウントをする + altitudejudge_gettingout = false; //高度による離床判定 + altitudejudge_open = false; //高度による開放判定 + altitudecount = 0; //閾値を5回満たすカウントをする + phase_now = 0; //現在のフェーズを区別(0=CHEAK,1=READY,2=FLIGHT,3=OPENED) + emst = 0; //開放禁止コマンド用状態表示用(0=NOT_OPEN,1=OPEN) + apogeetime_data_judge_ms = false; + mecotime_data_judge_ms = false; + time_data_ms = 100000000; + return; +} +void getevent() { + // 10Hzで実行する処理 + // BNO055から100Hzで測定 + if (acceljudge_gettingout == false) { //READYフェーズでのみ離床判定可能 + if (calculateMedianBNO(&accelerometerData) <= -9) { //中央値が閾値を超えた回数カウント + accelcount++; + } else { + accelcount = 0; //初期化 + } - if (uplink == "mif-on") { - Serial_MIF.print("mif-on\n"); - } - if (uplink == "mif-off") { - Serial_MIF.print("mif-off\n"); + if (accelcount > 5) { //5回連続で閾値を超えたときの処理 + acceljudge_gettingout = true; //離床判定・フェーズ移行 + accelcount = 0; //開放判定に向けた初期化 + } } - - float uplink_float = uplink.toFloat(); - if (uplink_float != 0) { - opener.set_open_threshold_time_ms(uplink_float * 1000); - response = "open:" + String(static_cast(opener.get_open_threshold_time_ms()) / 1000.0, 2); - need_response_usb = true; - need_response_es920 = true; + if ((acceljudge_open == false) && (phase_now == 2)) { //READYフェーズのみ開放判定可能 + if (calculateMedianBNO(&accelerometerData) > -7) { //中央値が閾値を下回った回数カウント + accelcount++; + } else { + accelcount = 0; //初期化 + } + if (accelcount > 5) { //5回連続で閾値を下回ったときの処理 + acceljudge_open = true; //開放判定・フェーズ移行 + } } + return; } +