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
diff -r 000000000000 -r ca455457108f main.cpp --- /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); +}