PQ_Hybrid_Electrical_Equipment_Team
IZU2019 Hybrid main program
Dependencies: Nichrome_lib mbed mpu9250_i2c MadgwickFilter SHT35_lib ADXL375_i2c ES920LR SDFileSystem pqLPS22HB_lib INA226_lib GPS_interrupt TSL_2561_lib
Diff: main.cpp
- Revision:
- 0:ca455457108f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Feb 26 12:11:36 2019 +0000
@@ -0,0 +1,1229 @@
+#include "mbed.h"
+#include "math.h"
+
+#include "ES920LR.hpp"
+#include "Nichrome_lib.h"
+#include "SDFileSystem.h"
+
+#include "ADXL375_i2c.h"
+#include "INA226.h"
+#include "MadgwickFilter.hpp"
+#include "mpu9250_i2c.h"
+#include "pqLPS22HB_lib.h"
+#include "SHT35.h"
+#include "TSL2561.h"
+#include "GPS_interrupt.h"
+
+
+/*******************************************************************************
+設定値
+打ち上げ前に必ず確認！！
+*******************************************************************************/
+const float TIME_SEP1 = 16.0f; //1段目分離までの時間
+const float TIME_SEP2 = 75.0f; //2段目分離までの時間
+
+const float TIME_RECOVERY = 120.0f; //回収モード移行までの時間
+
+const float SEP1_ALT_UNDER = 500.0f;//1段目分離条件（高度）
+const float SEP1_SPEED = 5.0f; //1段目分離条件（速度）
+
+const float SEP2_ALT_UP = 300.0f; //2段目分離条件（高度）
+
+const float OK_UP_ALT = 500.0f;
+const float OK_DOWN_ALT = 30.0f;
+const float STOP_SPEED_UP = 2.0f;
+const float STOP_SPEED_UNDER = -2.0f;
+
+bool wait_GPS = true; //GPS受信待ちするか
+bool ok_PC = false; //PCを使用するか
+
+
+/*******************************************************************************
+コンストラクタ
+*******************************************************************************/
+RawSerial pc(USBTX, USBRX, 115200);
+
+RawSerial serial_es920(p9, p10);
+ES920LR es920(serial_es920, pc, 115200);
+
+Serial GPS_serial(p13, p14, 38400);
+GPS_interrupt GPS(&GPS_serial);
+float GPS_freq = 4;
+
+SDFileSystem sd(p5, p6, p7, p8, "sd");
+const char file_name[64] = "/sd/Felix_Luminous_LOG.csv";
+
+I2C i2c_bus(p28, p27);
+ADXL375_i2c ADXL375(i2c_bus, ADXL375_i2c::ALT_ADDRESS_HIGH);
+myINA226 INA226_in(i2c_bus, myINA226::A1_GND, myINA226::A0_GND);
+myINA226 INA226_ex(i2c_bus, myINA226::A1_VDD, myINA226::A0_GND);
+mpu9250 nine(i2c_bus, AD0_HIGH);
+pqLPS22HB_lib LPS22HB(pqLPS22HB_lib::AD0_HIGH, i2c_bus);
+pqLPS22HB_lib LPS33HW(pqLPS22HB_lib::AD0_LOW, i2c_bus);
+mySHT35 SHT35(i2c_bus, mySHT35::AD0_LOW);
+myTSL2561 TSL2561(i2c_bus, myTSL2561::AD0_OPEN);
+
+//InterruptIn FlightPin(p26);
+DigitalIn FlightPin(p26);
+
+Nichrome_lib Separate1(p24);
+Nichrome_lib Separate2(p25);
+
+DigitalOut Buzzer(p22);
+//PwmOut Buzzer_music(p21);
+
+//AnalogIn Pitot(p20);
+
+Timeout timeout_stop;
+Timeout timeout_sep;
+Timeout timeout_recovery;
+
+Timer time_main;
+Timer time_flight;
+
+Ticker tick_gps;
+Ticker tick_print;
+Ticker tick_header_data;
+
+
+/*******************************************************************************
+関数のプロトタイプ宣言
+*******************************************************************************/
+void setup(); //無線あり
+
+void readSensor();
+void readGPS();
+
+void printData();
+void readPC();
+void printHelp();
+
+void sendDownLink();//無線あり
+void readUpLink(); //無線あり
+
+void startRec();
+void stopRec();
+void recData();
+
+void modeChange(); //無線あり
+void changePhase_SEP1();
+void changePhase_SEP2();
+void changePhase_RECOVERY();
+void buzzerChange();
+
+void separate1();
+void separate2();
+
+
+/*******************************************************************************
+変数の宣言
+*******************************************************************************/
+char rocket_phase;
+bool do_first = false;
+
+bool es920_using = false;
+char es920_uplink_command = '-';
+
+float adxl_acc[3];
+
+float ina_in_v, ina_in_i, ina_ex_v, ina_ex_i;
+bool ex_power;
+
+MadgwickFilter attitude(1.0);
+float euler[3];
+Quaternion quart(1.0, 0.0, 0.0, 0.0);
+
+float gyro[3], gyro_rad[3], acc[3], mag[3];
+
+float pres22, temp22, alt22, pres22_0, temp22_0;
+
+float pres33, temp33, alt33, pres33_0, temp33_0;
+float alt33_buf[10], alt33_ave, alt33_ave_old, speed33, speed33_buf[10], speed33_ave;
+int alt33_count = 0, speed33_count = 0;
+
+bool ok_up = false;
+bool ok_down = false;
+bool ok_top = false;
+
+float temp35, hum35;
+
+int lux;
+
+float pitot_raw, pitot_err, pitot;
+float pitot_buf[100];
+int pitot_count = 0;
+float pitot_speed;
+
+float gps_lat, gps_lon, gps_alt, gps_utc[6], gps_knot, gps_deg;
+int gps_sat;
+bool gps_yn;
+
+bool flight_pin;
+
+FILE *fp;
+bool save = false;
+int save_c = 0;
+
+int time_read, time_reset = 0;
+int time_min, time_sec, time_ms;
+
+int flight_time_read, flight_time_read_old, flight_time_reset = 0;
+int flight_time_min, flight_time_sec, flight_time_ms;
+
+
+/*******************************************************************************
+定数
+*******************************************************************************/
+const float ES920_MAX_20 = 0.000305176;
+const float ES920_MAX_100 = 0.001525879;
+const float ES920_MAX_200 = 0.003051758;
+const float ES920_MAX_500 = 0.007629395;
+const float ES920_MAX_1500 = 0.022888184;
+const float ES920_MAX_3000 = 0.045776367;
+
+
+/*******************************************************************************
+無線のヘッダまとめ(ROCKET -> GND)
+*******************************************************************************/
+const char HEADER_SENSOR_SETUP = 0x01;
+// 0x01, ADXL, INA_in, INA_ex, MPU, MPU_m, 22HB, 33HW, SHT, TSL, SD
+// 0 1 1 1 1 1 1 1 1 1 1
+// 0 1 2 3 4 5 6 7 8 9
+
+const char HEADER_GPS_SETUP = 0x02;
+// 0x02, readable, wait_count
+// 0 1 4
+// 0 1
+// 0x00 : NG
+// 0x01 : OK
+// 0xAA : Finish
+// 0xFF : Ignore
+
+const char HEADER_DATA = 0x10;
+//起動時間, フライト時間, フェーズ, ex_pow&Sep1&Sep2&ok_up&ok_down&ok_top&gps_yn, lat, lon, alt, knot, deg, sat, in_v, in_i, ex_v, ex_i, pres33, alt33, speed33, temp35, hum35, pitot_speed
+//4(2,2) 4(2,2) 2 1 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2
+//0 2 4 6 8 10 11 13 15 17 19 21 23 25 27 29 31 33 35 37 39 41
+// 0x01 : ex_pow
+// 0x02 : Sep1
+// 0x04 : Sep2
+// 0x08 : ok_up
+// 0x10 : ok_down
+// 0x20 : ok_top
+// 0x40 : save
+// 0x80 : flight_pin
+
+const char HEADER_START = 0x11;
+// 0x11, What
+// 0 1
+// 0
+// 'W' : 安全モード->離床検知モード
+// 'S' : 離床検知モード->安全モード
+// 'F' : 離床検知モード->フライトモード
+// '1' : 1段目分離
+// '2' : 2段目分離
+
+
+/*******************************************************************************
+無線のヘッダまとめ(GND -> ROCKET)
+*******************************************************************************/
+const char HEADER_COMMAND = 0xcd;
+// 0xcd,コマンド
+// 0 1
+// 0 1
+
+
+/*******************************************************************************
+ロケットのフェーズ
+*******************************************************************************/
+const char ROCKET_SETUP = 0x00; //セットアップ中
+const char ROCKET_SAFETY = 0x01; //安全モード
+const char ROCKET_WAIT = 0x02; //待機モード
+const char ROCKET_FLIGHT = 0x03; //フライトモード
+const char ROCKET_SEP1 = 0x04; //分離1モード
+const char ROCKET_SEP2 = 0x05; //分離2モード
+const char ROCKET_RECOVERY = 0x06; //回収モード
+
+
+/*******************************************************************************
+メイン関数
+*******************************************************************************/
+int main() {
+ rocket_phase = ROCKET_SETUP;
+ setup();
+
+ pc.attach(&readPC, Serial::RxIrq);
+
+ tick_gps.attach(&readGPS, 1/GPS_freq);
+ tick_print.attach(&printData, 1.0f);
+ tick_header_data.attach(&sendDownLink, 1.0f);
+
+ es920.attach(&readUpLink, HEADER_COMMAND);
+
+ FlightPin.mode(PullUp);
+
+ time_main.reset();
+ time_main.start();
+ startRec();
+
+ rocket_phase = ROCKET_SAFETY;
+ while(1) {
+ readSensor();
+ recData();
+ modeChange(); //状態遷移の判断
+ }
+}
+
+
+/*******************************************************************************
+センサ読み取り
+*******************************************************************************/
+void readSensor(){
+ time_read = time_main.read_ms();
+ if(time_read >= 30*60*1000){
+ time_main.reset();
+ time_reset ++;
+ }
+ time_min = time_reset * 30 + floor((double)time_read / (60 * 1000));
+ time_sec = time_read % (60 * 1000);
+ time_sec = floor((double)time_sec / 1000);
+ time_ms = time_read % 1000;
+
+ if(rocket_phase >= ROCKET_FLIGHT){
+ flight_time_read_old = flight_time_read;
+ flight_time_read = time_flight.read_ms();
+ }
+ else{
+ flight_time_read = 0;
+ }
+ if(flight_time_read >= 30*60*1000){
+ time_flight.reset();
+ flight_time_reset ++;
+ }
+ flight_time_min = flight_time_reset * 30 + floor((double)flight_time_read / (60 * 1000));
+ flight_time_sec = flight_time_read % (60 * 1000);
+ flight_time_sec = floor((double)flight_time_sec / 1000);
+ flight_time_ms = flight_time_read % 1000;
+
+ ADXL375.getOutput(adxl_acc);
+
+ INA226_in.get_Voltage_current(&ina_in_v, &ina_in_i);
+ ina_in_v = ina_in_v / 1000;
+ ina_in_i = ina_in_i / 1000;
+ if(ina_in_i > 64){
+ ina_in_i = 0.0f;
+ }
+ INA226_ex.get_Voltage_current(&ina_ex_v, &ina_ex_i);
+ ina_ex_v = ina_ex_v / 1000;
+ ina_ex_i = ina_ex_i / 1000;
+ if(ina_ex_i > 64){
+ ina_ex_i = 0.0f;
+ }
+ if(ina_ex_i > 0.1f){
+ ex_power = true;
+ }
+ else{
+ ex_power = false;
+ }
+
+ nine.getGyro(gyro);
+ nine.getAcc(acc);
+ nine.getMag(mag);
+ for(int i = 0; i < 3; i ++){
+ gyro_rad[i] = gyro[i] * 3.1415926535 / 180;
+ }
+ attitude.MadgwickAHRSupdate(gyro_rad, acc, mag);
+ attitude.getAttitude(&quart);
+ attitude.getEulerAngle(euler);
+ for(int i = 0; i < 3; i ++){
+ euler[i] = euler[i] * 180 / 3.145926535;
+ }
+
+ pres22 = LPS22HB.getPres();
+ temp22 = LPS22HB.getTemp();
+ alt22 = LPS22HB.getAlt(pres22_0, temp22_0);
+
+ pres33 = LPS33HW.getPres();
+ temp33 = LPS33HW.getTemp();
+ alt33 = LPS33HW.getAlt(pres33_0, temp33_0);
+
+ alt33_buf[alt33_count] = alt33;
+ alt33_count ++;
+ if(alt33_count > 10){
+ alt33_count = 0;
+ }
+ float alt33_sum = 0;
+ for(int i = 0; i < 10; i ++){
+ alt33_sum += alt33_buf[i];
+ }
+ alt33_ave_old = alt33_ave;
+ alt33_ave = alt33_sum / 10;
+
+ if(alt33_ave - alt33_ave_old > 0.01 || alt33_ave - alt33_ave_old < -0.01){
+ speed33 = (alt33_ave - alt33_ave_old) / (((float)flight_time_read - (float)flight_time_read_old) / 1000);
+ }
+ if(rocket_phase <= ROCKET_WAIT){
+ speed33 = 0.0f;
+ }
+
+ speed33_buf[speed33_count] = speed33;
+ speed33_count ++;
+ if(speed33_count > 10){
+ speed33_count = 0;
+ }
+ float speed33_sum = 0;
+ for(int i = 0; i < 10; i ++){
+ speed33_sum += speed33_buf[i];
+ }
+ speed33_ave = speed33_sum / 10;
+
+
+ if(rocket_phase <= ROCKET_WAIT){
+ pres22_0 = pres22;
+ temp22_0 = temp22;
+ pres33_0 = pres33;
+ temp33_0 = temp33;
+ }
+
+ SHT35.getTempHum(&temp35, &hum35);
+
+ lux = TSL2561.getLuminous();
+
+ /*pitot_raw = Pitot.read() * 3.3;
+ pitot = (pitot_raw - pitot_err - 0.2) / (5 * 0.0012858);
+ if(pitot < 0.0f){
+ pitot = 0.0f;
+ }
+ pitot_speed = sqrt(2 * pitot * 1000 / 1.3);
+ if(rocket_phase <= ROCKET_WAIT){
+ pitot_buf[pitot_count] = pitot;
+ pitot_count ++;
+ if(pitot_count > 100){
+ pitot_err = 0;
+ for(int i = 0; i < 100; i ++){
+ pitot_err += pitot_buf[pitot_count];
+ }
+ pitot_err = pitot_err / 100;
+ pitot_count = 0;
+ }
+ }*/
+
+ flight_pin = FlightPin;
+}
+
+
+/*******************************************************************************
+GPS読み取り
+*******************************************************************************/
+void readGPS(){
+ gps_yn = GPS.gps_readable;
+ if(gps_yn){
+ gps_lat = GPS.Latitude();
+ gps_lon = GPS.Longitude();
+ gps_alt = GPS.Height();
+ GPS.getUTC(gps_utc);
+ gps_utc[3] += 9;
+ if(gps_utc[3] >= 24){
+ gps_utc[3] -= 24;
+ gps_utc[2] += 1;
+ }
+ gps_knot = GPS.Knot();
+ gps_deg = GPS.Degree();
+ gps_sat = GPS.Number();
+ }
+}
+
+
+/*******************************************************************************
+PCでデータ表示
+*******************************************************************************/
+void printData(){
+ if(ok_PC){
+ pc.printf("Time : %d:%02d\r\n", time_min, time_sec);
+ pc.printf("FlightTime : %d:%02d\r\n", flight_time_min, flight_time_sec);
+
+ pc.printf("Phase : ");
+ switch(rocket_phase){
+ case ROCKET_SETUP:
+ pc.printf("SETUP\r\n");
+ break;
+ case ROCKET_SAFETY:
+ pc.printf("SAFETY\r\n");
+ break;
+ case ROCKET_WAIT:
+ pc.printf("WAIT\r\n");
+ break;
+ case ROCKET_FLIGHT:
+ pc.printf("FLIGHT\r\n");
+ break;
+ case ROCKET_SEP1:
+ pc.printf("SEPARATE 1\r\n");
+ break;
+ case ROCKET_SEP2:
+ pc.printf("SEPARATE 2\r\n");
+ break;
+ case ROCKET_RECOVERY:
+ pc.printf("RECOVERY\r\n");
+ break;
+ }
+
+ pc.printf("Power : ");
+ if(ex_power){
+ pc.printf("External\r\n");
+ }
+ else{
+ pc.printf("Battery\r\n");
+ }
+
+ pc.printf("Separate : %d, %d\r\n", Separate1.status, Separate2.status);
+ pc.printf("UP DOWN TOP : %d, %d, %d\r\n", ok_up, ok_down, ok_top);
+ pc.printf("SAVE : %d\r\n", save);
+ pc.printf("Flight Pin : %d\r\n", flight_pin);
+
+ pc.printf("GPS : %3.7f, %3.7f, %.2f\r\n", gps_lat, gps_lon, gps_alt);
+ pc.printf("Move : %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg);
+ pc.printf("Sats : %d\r\n", gps_sat);
+
+ pc.printf("INA_in : %.2f[V], %.2f[A]\r\n", ina_in_v, ina_in_i);
+ pc.printf("INA_ex : %.2f[V], %.2f[A]\r\n", ina_ex_v, ina_ex_i);
+
+ pc.printf("LPS33HW : %.4f[hPa], %.2f[m], %.2f[m/s]\r\n", pres33, alt33, speed33);
+ pc.printf("SHT35 : %.2f[degC], %.2f[%%]\r\n", temp35, hum35);
+ // pc.printf("Pitot : %.2f[m/s]\r\n", pitot_speed);
+
+ pc.printf("\n\n\n");
+ }
+}
+
+
+/*******************************************************************************
+PCからコマンド読み取り
+*******************************************************************************/
+void readPC(){
+ if(ok_PC){
+ char command = pc.getc();
+ pc.printf("PC Command = %c\r\n", command);
+
+ switch(command){
+ case '?':
+ printHelp();
+ break;
+
+ case 'W': //離床検知モードへ移行
+ if(rocket_phase == ROCKET_SAFETY){
+ rocket_phase = ROCKET_WAIT;
+ do_first = false;
+ }
+ break;
+
+ case 'S': //安全モードへ移行
+ if(rocket_phase == ROCKET_WAIT){
+ rocket_phase = ROCKET_SAFETY;
+ do_first = false;
+ }
+ if(rocket_phase == ROCKET_FLIGHT){
+ rocket_phase = ROCKET_SAFETY;
+ do_first = false;
+ timeout_sep.detach();
+ }
+ break;
+
+ case 'F': //フライトモードへ移行（手動）
+ if(rocket_phase == ROCKET_WAIT){
+ rocket_phase = ROCKET_FLIGHT;
+ do_first = false;
+ }
+ break;
+
+ case '1': //1段目強制分離
+ if(rocket_phase == ROCKET_FLIGHT){
+ rocket_phase = ROCKET_SEP1;
+ do_first = false;
+ }
+ else if(rocket_phase >= ROCKET_SEP1){
+ separate1();
+ }
+ break;
+
+ case '2': //2段目強制分離
+ if(rocket_phase == ROCKET_SEP1){
+ rocket_phase = ROCKET_SEP2;
+ do_first = false;
+ }
+ else if(rocket_phase >= ROCKET_SEP2){
+ separate2();
+ }
+ break;
+
+ case 'C':
+ if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && save){
+ stopRec();
+ }
+ break;
+
+ case 'O':
+ if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && !save){
+ startRec();
+ }
+ break;
+
+ case 'B':
+ buzzerChange();
+ break;
+ }
+ }
+}
+
+
+/*******************************************************************************
+コマンド情報を表示
+*******************************************************************************/
+void printHelp(){
+ for(int i = 0; i < 20; i ++){
+ pc.printf("*");
+ }
+
+ pc.printf("\r\nCommands\r\n");
+ pc.printf("W : Safety -> Wait\r\n");
+ pc.printf("S : Wait -> Safety\r\n");
+ pc.printf("F : Wait -> Flight\r\n");
+ pc.printf("1(one) : 1st Separate 5[s]\r\n");
+ pc.printf("2(two) : 2nd Separate 5[s]\r\n");
+ pc.printf("C : Stop Recording\r\n");
+ pc.printf("? : HELP\r\n");
+
+ for(int i = 0; i < 20; i ++){
+ pc.printf("*");
+ }
+ pc.printf("\r\n");
+ wait(1.0f);
+}
+
+
+/*******************************************************************************
+ダウンリンクを表示
+*******************************************************************************/
+void sendDownLink(){
+ //無線あり
+ if(!es920_using){
+ es920 << HEADER_DATA;
+
+ es920 << (short)time_reset;
+ es920 << (short)(time_read / 1000);
+
+ es920 << (short)flight_time_reset;
+ es920 << (short)(flight_time_read / 1000);
+
+ es920 << rocket_phase;
+
+ char status = 0x00;
+ if(ex_power) status |= (char)0x01;
+ if(Separate1.status)status |= (char)0x02;
+ if(Separate2.status)status |= (char)0x04;
+ if(ok_up) status |= (char)0x08;
+ if(ok_down) status |= (char)0x10;
+ if(ok_top) status |= (char)0x20;
+ if(save) status |= (char)0x40;
+ if(flight_pin) status |= (char)0x80;
+ es920 << status;
+
+ es920 << (short)(gps_lat / ES920_MAX_100);
+ es920 << (short)(gps_lon / ES920_MAX_500);
+ es920 << (short)(gps_alt / ES920_MAX_500);
+ es920 << (short)(gps_knot / ES920_MAX_200);
+ es920 << (short)(gps_deg / ES920_MAX_1500);
+ es920 << (short)gps_sat;
+
+ es920 << (short)(ina_in_v / ES920_MAX_20);
+ es920 << (short)(ina_in_i / ES920_MAX_20);
+ es920 << (short)(ina_ex_v / ES920_MAX_20);
+ es920 << (short)(ina_ex_i / ES920_MAX_20);
+
+ es920 << (short)(pres33 / ES920_MAX_3000);
+ es920 << (short)(alt33_ave / ES920_MAX_500);
+ es920 << (short)(speed33_ave / ES920_MAX_200);
+ es920 << (short)(temp35 / ES920_MAX_100);
+ es920 << (short)(hum35 / ES920_MAX_200);
+
+ es920 << (short)(pitot_speed / ES920_MAX_200);
+
+ es920.send();
+ }
+ else{
+ es920_using = false;
+ }
+}
+
+
+/*******************************************************************************
+アップリンクを解析
+*******************************************************************************/
+void readUpLink(){
+ //無線あり
+ es920_uplink_command = es920.data[0];
+ pc.printf("GND Command = %c\r\n", es920_uplink_command);
+
+ switch(es920_uplink_command){
+ case 'W': //離床検知モードへ移行
+ if(rocket_phase == ROCKET_SAFETY){
+ rocket_phase = ROCKET_WAIT;
+ do_first = false;
+ }
+ break;
+
+ case 'S': //安全モードへ移行
+ if(rocket_phase == ROCKET_WAIT){
+ rocket_phase = ROCKET_SAFETY;
+ do_first = false;
+ }
+ if(rocket_phase == ROCKET_FLIGHT){
+ rocket_phase = ROCKET_SAFETY;
+ do_first = false;
+ timeout_sep.detach();
+ }
+
+ break;
+
+ case 'F': //フライトモードへ移行（手動）
+ if(rocket_phase == ROCKET_WAIT){
+ rocket_phase = ROCKET_FLIGHT;
+ do_first = false;
+ }
+ break;
+
+ case '1': //1段目強制分離
+ if(rocket_phase == ROCKET_FLIGHT){
+ rocket_phase = ROCKET_SEP1;
+ do_first = false;
+ }
+ else if(rocket_phase >= ROCKET_SEP1){
+ es920_using = true;
+ es920 << HEADER_START;
+ es920 << '1';
+ es920.send();
+
+ separate1();
+ }
+ break;
+
+ case '2': //2段目強制分離
+ if(rocket_phase == ROCKET_SEP1){
+ rocket_phase = ROCKET_SEP2;
+ do_first = false;
+ }
+ else if(rocket_phase >= ROCKET_SEP2){
+ es920_using = true;
+ es920 << HEADER_START;
+ es920 << '2';
+ es920.send();
+
+ separate2();
+ }
+ break;
+
+ case 'C':
+ if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && save){
+ stopRec();
+ }
+ break;
+
+ case 'O':
+ if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && !save){
+ startRec();
+ }
+ break;
+
+ case 'B':
+ buzzerChange();
+ break;
+ }
+}
+
+
+/*******************************************************************************
+ログ記録開始
+*******************************************************************************/
+void startRec(){
+ fp = fopen(file_name, "a");
+ fprintf(fp, "Time,Flight Time,Phase,Flight Pin,Power,Separate1,Separate2,");
+ fprintf(fp, "upper100,under30,top,uplink,");
+ fprintf(fp, "GPS Y/N,GPS Time,Latitude,Longitude,GPS Alt,Knot,Deg,Sattelite,");
+ fprintf(fp, "ADXL_x[G],ADXL_y[G],ADXL_z[G],In_V[V],In_I[A],Ex_V[V],Ex_I[A],");
+ fprintf(fp, "Gyro_x[deg],Gyro_y[deg],Gyro_z[deg],Acc_x[G],Acc_y[G],Acc_z[G],Mag_x[deg],Mag_y[deg],Mag_z[deg],");
+ fprintf(fp, "Q_w,Q_x,Q_y,Q_z,Roll[deg],Pitch[deg],Yaw[deg],");
+ fprintf(fp, "Pres_22[hPa],Temp_22[degC],Alt_22[m],Pres_33[hPa],Temp_33[degC],Alt_33[m],Alt_33_ave[m],Speed_33[m/s],Speed_33_ave[m/s],");
+ fprintf(fp, "Temp_35[degC],Humid_35[%],Lux[lux],Pitot_raw[V]\r\n");
+ save = true;
+}
+
+
+/*******************************************************************************
+ログ記録終了
+*******************************************************************************/
+void stopRec(){
+ save = false;
+ fprintf(fp, "\r\n\n");
+ fclose(fp);
+}
+
+
+/*******************************************************************************
+データ書き込み
+*******************************************************************************/
+void recData(){
+ if(save){
+ fprintf(fp, "%d:%02d.%03d,", time_min, time_sec, time_ms);
+ fprintf(fp, "%d:%02d.%03d,", flight_time_min, flight_time_sec, flight_time_ms);
+
+ switch(rocket_phase){
+ case ROCKET_SETUP:
+ fprintf(fp, "SETUP,");
+ break;
+ case ROCKET_SAFETY:
+ fprintf(fp, "SAFETY,");
+ break;
+ case ROCKET_WAIT:
+ fprintf(fp, "WAIT,");
+ break;
+ case ROCKET_FLIGHT:
+ fprintf(fp, "FLIGHT,");
+ break;
+ case ROCKET_SEP1:
+ fprintf(fp, "SEPARATE1,");
+ break;
+ case ROCKET_SEP2:
+ fprintf(fp, "SEPARATE2,");
+ break;
+ case ROCKET_RECOVERY:
+ fprintf(fp, "RECOVERY,");
+ break;
+ }
+
+ fprintf(fp, "%d,", flight_pin);
+
+ if(ex_power){
+ fprintf(fp, "External,");
+ }
+ else{
+ fprintf(fp, "Battery,");
+ }
+
+ fprintf(fp, "%d,%d,", Separate1.status, Separate2.status);
+ fprintf(fp, "%d,%d,%d,", ok_up, ok_down, ok_top);
+
+ fprintf(fp, "%c,", es920_uplink_command);
+ es920_uplink_command = '-';
+
+ fprintf(fp, "%d,", gps_yn);
+ fprintf(fp, "%d/%d/%d ", (int)gps_utc[0], (int)gps_utc[1], (int)gps_utc[2]); //日付
+ fprintf(fp, "%d:%02d:%02.2f,", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]); //時間
+ fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt); //GPS座標
+ fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg); //GPS速度
+ fprintf(fp, "%d,", gps_sat); //GPS衛星数
+
+ fprintf(fp, "%.2f,%.2f,%.2f,", adxl_acc[0], adxl_acc[1], adxl_acc[2]);
+ fprintf(fp, "%.4f,%.4f,%.4f,%.4f,", ina_in_v, ina_in_i, ina_ex_v, ina_ex_i);
+ fprintf(fp, "%f,%f,%f,", gyro[0], gyro[1], gyro[2]);
+ fprintf(fp, "%f,%f,%f,", acc[0], acc[1], acc[2]);
+ fprintf(fp, "%.2f,%.2f,%.2f,", mag[0], mag[1], mag[2]);
+ fprintf(fp, "%f,%f,%f,%f,", quart.w, quart.x, quart.y, quart.z);
+ fprintf(fp, "%f,%f,%f,", euler[0], euler[1], euler[2]);
+ fprintf(fp, "%f,%.2f,%.2f,", pres22, temp22, alt22);
+ fprintf(fp, "%f,%.2f,%.2f,%.2f,%.2f,%2f,", pres33, temp33, alt33, alt33_ave, speed33, speed33_ave);
+ fprintf(fp, "%.2f,%.2f,", temp35, hum35);
+ fprintf(fp, "%d,", lux);
+ //fprintf(fp, "%f,%f,%f\r\n", pitot_raw, pitot, pitot_speed);
+
+ save_c ++;
+ if(save_c > 10000){
+ fclose(fp);
+ fp = fopen(file_name, "a");
+ save_c = 0;
+ }
+ }
+}
+
+
+/*******************************************************************************
+状態遷移の判断
+*******************************************************************************/
+void modeChange(){
+ //無線あり
+ switch(rocket_phase){
+ case ROCKET_SAFETY://///////////////安全モード
+ if(!do_first){
+ es920_using = true;
+ es920 << HEADER_START;
+ es920 << 'S';
+ es920.send();
+
+ do_first = true;
+ }
+ break;
+
+ case ROCKET_WAIT://////////////////離床検知モード
+ if(!do_first){
+ es920_using = true;
+ es920 << HEADER_START;
+ es920 << 'W';
+ es920.send();
+
+
+ if(!save){
+ startRec();
+ }
+
+ do_first = true;
+ }
+ if(flight_pin){
+ rocket_phase = ROCKET_FLIGHT;
+ do_first = false;
+ }
+ break;
+
+ case ROCKET_FLIGHT:///////////////フライトモード
+ if(!do_first){
+ es920_using = true;
+ es920 << HEADER_START;
+ es920 << 'F';
+ es920.send();
+
+ if(!save){
+ startRec();
+ }
+
+ time_flight.reset();
+ time_flight.start();
+
+ timeout_sep.attach(&changePhase_SEP1, TIME_SEP1);
+ timeout_recovery.attach(&changePhase_RECOVERY, TIME_RECOVERY);
+
+ do_first = true;
+ }
+ if(alt33_ave > SEP1_ALT_UNDER && speed33_ave < SEP1_SPEED){
+ ok_top = true;
+
+ rocket_phase = ROCKET_SEP1;
+ do_first = false;
+ }
+ break;
+
+ case ROCKET_SEP1:////////////////分離1モード
+ if(!do_first){
+ es920_using = true;
+ es920 << HEADER_START;
+ es920 << '1';
+ es920.send();
+
+ timeout_sep.detach();
+
+ stopRec();
+
+ startRec();
+
+ separate1();
+
+ timeout_sep.attach(&changePhase_SEP2, TIME_SEP2 - TIME_SEP1);
+
+ do_first = true;
+ }
+ if(alt33_ave < SEP2_ALT_UP && speed33_ave < 0.0f && ok_up){
+ rocket_phase = ROCKET_SEP2;
+ do_first = false;
+ }
+ break;
+
+ case ROCKET_SEP2:////////////////分離2モード
+ if(!do_first){
+ es920_using = true;
+ es920 << HEADER_START;
+ es920 << '2';
+ es920.send();
+
+ timeout_sep.detach();
+
+ separate2();
+
+ do_first = true;
+ }
+ break;
+
+ case ROCKET_RECOVERY:////////////回収モード
+ if(!do_first){
+ stopRec();
+ startRec();
+
+ Buzzer = 1;
+
+ do_first = true;
+ }
+ break;
+ }
+
+ if(rocket_phase >= ROCKET_FLIGHT){//////////////////フライトモード以降共通
+ if(alt33_ave > SEP1_ALT_UNDER && speed33_ave < SEP1_SPEED){
+ ok_top = true;
+ }
+
+ if(!ok_up && alt33_ave > OK_UP_ALT){
+ ok_up = true;
+ }
+ if(ok_up && !ok_down && alt33_ave < OK_DOWN_ALT){
+ ok_down = true;
+ }
+ if(ok_up && ok_down && speed33_ave > STOP_SPEED_UNDER && speed33_ave < STOP_SPEED_UP){
+ changePhase_RECOVERY();
+ }
+ }
+}
+
+void changePhase_SEP1(){/////////////分離1モードへ
+ rocket_phase = ROCKET_SEP1;
+ do_first = false;
+}
+
+void changePhase_SEP2(){/////////////分離2モードへ
+ rocket_phase = ROCKET_SEP2;
+ do_first = false;
+}
+
+void changePhase_RECOVERY(){/////////回収モードへ
+ rocket_phase = ROCKET_RECOVERY;
+ do_first = false;
+}
+
+
+/*******************************************************************************
+ブザーのON/OFF
+*******************************************************************************/
+void buzzerChange(){
+ if(Buzzer){
+ Buzzer = 0;
+ }
+ else{
+ Buzzer = 1;
+ }
+}
+
+
+/*******************************************************************************
+分離機構1段目作動
+*******************************************************************************/
+void separate1(){
+ if(!Separate1.status && rocket_phase >= ROCKET_FLIGHT){
+ Separate1.fire(6.0f);
+ pc.printf("1st Separation\r\n");
+ }
+}
+
+
+/*******************************************************************************
+分離機構2段目作動
+*******************************************************************************/
+void separate2(){
+ if(!Separate2.status && rocket_phase >= ROCKET_FLIGHT){
+ Separate2.fire(5.0f);
+ pc.printf("2nd Separation\r\n");
+ }
+}
+
+
+/*******************************************************************************
+セットアップ（最初に1回実行）
+*******************************************************************************/
+//無線あり(HEADER_SETUP)
+void setup(){
+ wait(1.0f);
+ char setup_string[1024];
+
+ pc.printf("\r\n**************************************************\r\n");
+ pc.printf("Sensor Setting Start!!\r\n");
+ strcat(setup_string, "Sensor Setting Start!!\r\n");
+ es920 << HEADER_SENSOR_SETUP;
+
+ ///////////////////////////////////////////ADXL375
+ ADXL375.setDataRate(ADXL375_100HZ);
+ if(ADXL375.whoAmI() == 1){
+ pc.printf("ADXL375 : OK\r\n");
+ strcat(setup_string, "ADXL375 : OK!!\r\n");
+ es920 << (char)0x01;
+ }
+ else{
+ pc.printf("ADXL375 : NG...\r\n");
+ strcat(setup_string, "ADXL375 : NG...\r\n");
+ es920 << (char)0x00;
+ }
+ ADXL375.offset(0.0f, 0.0f, 0.0f);
+
+ ///////////////////////////////////////////INA226_in
+ INA226_in.set_callibretion();
+ if(INA226_in.Connection_check() == 0){
+ pc.printf("INA226_in : OK!!\r\n");
+ strcat(setup_string, "INA_in : OK!!\r\n");
+ es920 << (char)0x01;
+ }
+ else{
+ pc.printf("INA226_in : NG...\r\n");
+ strcat(setup_string, "INA_in : NG...\r\n");
+ es920 << (char)0x00;
+ }
+
+ ///////////////////////////////////////////INA226_ex
+ INA226_ex.set_callibretion();
+ if(INA226_ex.Connection_check() == 0){
+ pc.printf("INA226_ex OK!!\r\n");
+ strcat(setup_string, "INA_ex : OK!!\r\n");
+ es920 << (char)0x01;
+ }
+ else{
+ pc.printf("INA226_ex NG...\r\n");
+ strcat(setup_string, "INA_ex : NG...\r\n");
+ es920 << (char)0x00;
+ }
+
+ ///////////////////////////////////////////MPU9250
+ nine.setAcc(_16G);
+ nine.setGyro(_2000DPS);
+ nine.setOffset(0.58424, 0.622428, 0.063746,
+ 0.008125, -0.00106, 0.0024145,
+ -2.25, 3.825, -24.75);
+
+ if(nine.senserTest()){
+ pc.printf("MPU9250 : OK!!\r\n");
+ strcat(setup_string, "MPU9250 : OK!!\r\n");
+ es920 << (char)0x01;
+ }
+ else{
+ pc.printf("MPU9250 : NG...\r\n");
+ strcat(setup_string, "MPU9250 : NG...\r\n");
+ es920 << (char)0x00;
+ }
+ if(nine.mag_senserTest()){
+ pc.printf("MPU9250 MAG : OK!!\r\n");
+ strcat(setup_string, "MPU9250 MAG : OK!!\r\n");
+ es920 << (char)0x01;
+ }
+ else{
+ pc.printf("MPU9250 MAG : NG...\r\n");
+ strcat(setup_string, "MPU9250 MAG : NG...\r\n");
+ es920 << (char)0x00;
+ }
+
+ ///////////////////////////////////////////LPS22HB
+ LPS22HB.begin(50);
+ if(LPS22HB.whoAmI() == 0){
+ pc.printf("LPS22HB : OK!!\r\n");
+ strcat(setup_string, "LPS22HB : OK!!\r\n");
+ es920 << (char)0x01;
+ }
+ else{
+ pc.printf("LPS22HB : NG...\r\n");
+ strcat(setup_string, "LPS22HB : NG...\r\n");
+ es920 << (char)0x00;
+ }
+
+ ///////////////////////////////////////////LPS33HW
+ LPS33HW.begin(50);
+ if(LPS33HW.whoAmI() == 0){
+ pc.printf("LPS33HW : OK!!\r\n");
+ strcat(setup_string, "LPS33HW : OK!!\r\n");
+ es920 << (char)0x01;
+ }
+ else{
+ pc.printf("LPS33HW : NG...\r\n");
+ strcat(setup_string, "LPS33HW : NG...\r\n");
+ es920 << (char)0x00;
+ }
+
+ ///////////////////////////////////////////SHT35
+ int sht_state = SHT35.getState();
+ if(sht_state == 32784 || sht_state == 32848){
+ pc.printf("SHT35 : OK!!\r\n");
+ strcat(setup_string, "SHT35 : OK!!\r\n");
+ es920 << (char)0x01;
+ }
+ else{
+ pc.printf("SHT35 : NG...\r\n");
+ strcat(setup_string, "SHT35 : NG...\r\n");
+ es920 << (char)0x00;
+ }
+
+ ///////////////////////////////////////////TSL2561
+ TSL2561.begin();
+ //lux_time = TSL2561.setRate(1);
+ if(TSL2561.connectCheck() == 1){
+ pc.printf("TSL2561 : OK!!\r\n");
+ strcat(setup_string, "TSL2561 : OK!!\r\n");
+ es920 << (char)0x01;
+ }
+ else{
+ pc.printf("SL2561 : NG...\r\n");
+ strcat(setup_string, "TSL2561 : NG...\r\n");
+ es920 << (char)0x00;
+ }
+
+ ///////////////////////////////////////////SD
+ fp = fopen("/sd/Felix_Luminous_setup.txt", "r");
+ if(fp != NULL){
+ pc.printf("SD : OK!!\r\n");
+ strcat(setup_string, "SD : OK!!\r\n");
+ es920 << (char)0x01;
+ fclose(fp);
+ }
+ else{
+ pc.printf("SD : NG...\r\n");
+ strcat(setup_string, "SD : NG...\r\n");
+ es920 << (char)0x00;
+ }
+
+ pc.printf("Sensor Setting Finished!!\r\n");
+ pc.printf("**************************************************\r\n");
+ strcat(setup_string, "Sensor Setting Finished!!\r\n");
+ es920.send();
+ wait(1.0f);
+
+ pc.printf("\r\n**************************************************\r\n");
+ if(wait_GPS){
+ pc.printf("GPS Setting Start!!\r\n");
+ strcat(setup_string, "GPS Setting Start!!\r\n");
+
+ pc.printf("Waiting : 0");
+ strcat(setup_string, "Wait : ");
+ int gps_wait_count = 0;
+ while(!GPS.gps_readable){
+ pc.printf("\rWaiting : %d", gps_wait_count);//
+ es920 << HEADER_GPS_SETUP;
+ es920 << (char)GPS.gps_readable;
+ es920 << (int)gps_wait_count;
+ es920.send();
+ wait(1.0f);
+ gps_wait_count ++;
+ }
+ char ss[8];
+ sscanf(ss, "%d", &gps_wait_count);
+ strcat(setup_string, ss);
+ pc.printf(" OK!!\r\n");
+ strcat(setup_string, " OK!!\r\n");
+ pc.printf("GPS Setting Finished!!\r\n");
+ strcat(setup_string, "GPS Setting Finished!!\r\n");
+ es920 << HEADER_GPS_SETUP;
+ es920 << (char)0xAA;
+ es920 << (int)0;
+ es920.send();
+ }
+ else{
+ pc.printf("GPS Setting Ignore...\r\n");
+ strcat(setup_string, "GPS Setting Ignore...\r\n");
+ es920 << HEADER_GPS_SETUP;
+ es920 << (char)0xFF;
+ es920 << (int)0;
+ es920.send();
+ }
+ pc.printf("\r\n**************************************************\r\n");
+ fp = fopen(file_name, "a");
+ fprintf(fp, setup_string);
+ fclose(fp);
+ setup_string[0] = NULL;
+
+ printHelp();
+ wait(1.0f);
+}