PQ_Hybrid_Electrical_Equipment_Team / Mbed 2 deprecated Hybrid_IZU2019

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

main.cpp@0:ca455457108f, 2019-02-26 (annotated)

Committer:
Okamoto009
Date:
Tue Feb 26 12:11:36 2019 +0000
Revision:
0:ca455457108f
IZU2019 main program;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Okamoto009 0:ca455457108f 1 #include "mbed.h"
Okamoto009 0:ca455457108f 2 #include "math.h"
Okamoto009 0:ca455457108f 3
Okamoto009 0:ca455457108f 4 #include "ES920LR.hpp"
Okamoto009 0:ca455457108f 5 #include "Nichrome_lib.h"
Okamoto009 0:ca455457108f 6 #include "SDFileSystem.h"
Okamoto009 0:ca455457108f 7
Okamoto009 0:ca455457108f 8 #include "ADXL375_i2c.h"
Okamoto009 0:ca455457108f 9 #include "INA226.h"
Okamoto009 0:ca455457108f 10 #include "MadgwickFilter.hpp"
Okamoto009 0:ca455457108f 11 #include "mpu9250_i2c.h"
Okamoto009 0:ca455457108f 12 #include "pqLPS22HB_lib.h"
Okamoto009 0:ca455457108f 13 #include "SHT35.h"
Okamoto009 0:ca455457108f 14 #include "TSL2561.h"
Okamoto009 0:ca455457108f 15 #include "GPS_interrupt.h"
Okamoto009 0:ca455457108f 16
Okamoto009 0:ca455457108f 17
Okamoto009 0:ca455457108f 18 /*******************************************************************************
Okamoto009 0:ca455457108f 19 設定値
Okamoto009 0:ca455457108f 20 打ち上げ前に必ず確認！！
Okamoto009 0:ca455457108f 21 *******************************************************************************/
Okamoto009 0:ca455457108f 22 const float TIME_SEP1 = 16.0f; //1段目分離までの時間
Okamoto009 0:ca455457108f 23 const float TIME_SEP2 = 75.0f; //2段目分離までの時間
Okamoto009 0:ca455457108f 24
Okamoto009 0:ca455457108f 25 const float TIME_RECOVERY = 120.0f; //回収モード移行までの時間
Okamoto009 0:ca455457108f 26
Okamoto009 0:ca455457108f 27 const float SEP1_ALT_UNDER = 500.0f;//1段目分離条件（高度）
Okamoto009 0:ca455457108f 28 const float SEP1_SPEED = 5.0f; //1段目分離条件（速度）
Okamoto009 0:ca455457108f 29
Okamoto009 0:ca455457108f 30 const float SEP2_ALT_UP = 300.0f; //2段目分離条件（高度）
Okamoto009 0:ca455457108f 31
Okamoto009 0:ca455457108f 32 const float OK_UP_ALT = 500.0f;
Okamoto009 0:ca455457108f 33 const float OK_DOWN_ALT = 30.0f;
Okamoto009 0:ca455457108f 34 const float STOP_SPEED_UP = 2.0f;
Okamoto009 0:ca455457108f 35 const float STOP_SPEED_UNDER = -2.0f;
Okamoto009 0:ca455457108f 36
Okamoto009 0:ca455457108f 37 bool wait_GPS = true; //GPS受信待ちするか
Okamoto009 0:ca455457108f 38 bool ok_PC = false; //PCを使用するか
Okamoto009 0:ca455457108f 39
Okamoto009 0:ca455457108f 40
Okamoto009 0:ca455457108f 41 /*******************************************************************************
Okamoto009 0:ca455457108f 42 コンストラクタ
Okamoto009 0:ca455457108f 43 *******************************************************************************/
Okamoto009 0:ca455457108f 44 RawSerial pc(USBTX, USBRX, 115200);
Okamoto009 0:ca455457108f 45
Okamoto009 0:ca455457108f 46 RawSerial serial_es920(p9, p10);
Okamoto009 0:ca455457108f 47 ES920LR es920(serial_es920, pc, 115200);
Okamoto009 0:ca455457108f 48
Okamoto009 0:ca455457108f 49 Serial GPS_serial(p13, p14, 38400);
Okamoto009 0:ca455457108f 50 GPS_interrupt GPS(&GPS_serial);
Okamoto009 0:ca455457108f 51 float GPS_freq = 4;
Okamoto009 0:ca455457108f 52
Okamoto009 0:ca455457108f 53 SDFileSystem sd(p5, p6, p7, p8, "sd");
Okamoto009 0:ca455457108f 54 const char file_name[64] = "/sd/Felix_Luminous_LOG.csv";
Okamoto009 0:ca455457108f 55
Okamoto009 0:ca455457108f 56 I2C i2c_bus(p28, p27);
Okamoto009 0:ca455457108f 57 ADXL375_i2c ADXL375(i2c_bus, ADXL375_i2c::ALT_ADDRESS_HIGH);
Okamoto009 0:ca455457108f 58 myINA226 INA226_in(i2c_bus, myINA226::A1_GND, myINA226::A0_GND);
Okamoto009 0:ca455457108f 59 myINA226 INA226_ex(i2c_bus, myINA226::A1_VDD, myINA226::A0_GND);
Okamoto009 0:ca455457108f 60 mpu9250 nine(i2c_bus, AD0_HIGH);
Okamoto009 0:ca455457108f 61 pqLPS22HB_lib LPS22HB(pqLPS22HB_lib::AD0_HIGH, i2c_bus);
Okamoto009 0:ca455457108f 62 pqLPS22HB_lib LPS33HW(pqLPS22HB_lib::AD0_LOW, i2c_bus);
Okamoto009 0:ca455457108f 63 mySHT35 SHT35(i2c_bus, mySHT35::AD0_LOW);
Okamoto009 0:ca455457108f 64 myTSL2561 TSL2561(i2c_bus, myTSL2561::AD0_OPEN);
Okamoto009 0:ca455457108f 65
Okamoto009 0:ca455457108f 66 //InterruptIn FlightPin(p26);
Okamoto009 0:ca455457108f 67 DigitalIn FlightPin(p26);
Okamoto009 0:ca455457108f 68
Okamoto009 0:ca455457108f 69 Nichrome_lib Separate1(p24);
Okamoto009 0:ca455457108f 70 Nichrome_lib Separate2(p25);
Okamoto009 0:ca455457108f 71
Okamoto009 0:ca455457108f 72 DigitalOut Buzzer(p22);
Okamoto009 0:ca455457108f 73 //PwmOut Buzzer_music(p21);
Okamoto009 0:ca455457108f 74
Okamoto009 0:ca455457108f 75 //AnalogIn Pitot(p20);
Okamoto009 0:ca455457108f 76
Okamoto009 0:ca455457108f 77 Timeout timeout_stop;
Okamoto009 0:ca455457108f 78 Timeout timeout_sep;
Okamoto009 0:ca455457108f 79 Timeout timeout_recovery;
Okamoto009 0:ca455457108f 80
Okamoto009 0:ca455457108f 81 Timer time_main;
Okamoto009 0:ca455457108f 82 Timer time_flight;
Okamoto009 0:ca455457108f 83
Okamoto009 0:ca455457108f 84 Ticker tick_gps;
Okamoto009 0:ca455457108f 85 Ticker tick_print;
Okamoto009 0:ca455457108f 86 Ticker tick_header_data;
Okamoto009 0:ca455457108f 87
Okamoto009 0:ca455457108f 88
Okamoto009 0:ca455457108f 89 /*******************************************************************************
Okamoto009 0:ca455457108f 90 関数のプロトタイプ宣言
Okamoto009 0:ca455457108f 91 *******************************************************************************/
Okamoto009 0:ca455457108f 92 void setup(); //無線あり
Okamoto009 0:ca455457108f 93
Okamoto009 0:ca455457108f 94 void readSensor();
Okamoto009 0:ca455457108f 95 void readGPS();
Okamoto009 0:ca455457108f 96
Okamoto009 0:ca455457108f 97 void printData();
Okamoto009 0:ca455457108f 98 void readPC();
Okamoto009 0:ca455457108f 99 void printHelp();
Okamoto009 0:ca455457108f 100
Okamoto009 0:ca455457108f 101 void sendDownLink();//無線あり
Okamoto009 0:ca455457108f 102 void readUpLink(); //無線あり
Okamoto009 0:ca455457108f 103
Okamoto009 0:ca455457108f 104 void startRec();
Okamoto009 0:ca455457108f 105 void stopRec();
Okamoto009 0:ca455457108f 106 void recData();
Okamoto009 0:ca455457108f 107
Okamoto009 0:ca455457108f 108 void modeChange(); //無線あり
Okamoto009 0:ca455457108f 109 void changePhase_SEP1();
Okamoto009 0:ca455457108f 110 void changePhase_SEP2();
Okamoto009 0:ca455457108f 111 void changePhase_RECOVERY();
Okamoto009 0:ca455457108f 112 void buzzerChange();
Okamoto009 0:ca455457108f 113
Okamoto009 0:ca455457108f 114 void separate1();
Okamoto009 0:ca455457108f 115 void separate2();
Okamoto009 0:ca455457108f 116
Okamoto009 0:ca455457108f 117
Okamoto009 0:ca455457108f 118 /*******************************************************************************
Okamoto009 0:ca455457108f 119 変数の宣言
Okamoto009 0:ca455457108f 120 *******************************************************************************/
Okamoto009 0:ca455457108f 121 char rocket_phase;
Okamoto009 0:ca455457108f 122 bool do_first = false;
Okamoto009 0:ca455457108f 123
Okamoto009 0:ca455457108f 124 bool es920_using = false;
Okamoto009 0:ca455457108f 125 char es920_uplink_command = '-';
Okamoto009 0:ca455457108f 126
Okamoto009 0:ca455457108f 127 float adxl_acc[3];
Okamoto009 0:ca455457108f 128
Okamoto009 0:ca455457108f 129 float ina_in_v, ina_in_i, ina_ex_v, ina_ex_i;
Okamoto009 0:ca455457108f 130 bool ex_power;
Okamoto009 0:ca455457108f 131
Okamoto009 0:ca455457108f 132 MadgwickFilter attitude(1.0);
Okamoto009 0:ca455457108f 133 float euler[3];
Okamoto009 0:ca455457108f 134 Quaternion quart(1.0, 0.0, 0.0, 0.0);
Okamoto009 0:ca455457108f 135
Okamoto009 0:ca455457108f 136 float gyro[3], gyro_rad[3], acc[3], mag[3];
Okamoto009 0:ca455457108f 137
Okamoto009 0:ca455457108f 138 float pres22, temp22, alt22, pres22_0, temp22_0;
Okamoto009 0:ca455457108f 139
Okamoto009 0:ca455457108f 140 float pres33, temp33, alt33, pres33_0, temp33_0;
Okamoto009 0:ca455457108f 141 float alt33_buf[10], alt33_ave, alt33_ave_old, speed33, speed33_buf[10], speed33_ave;
Okamoto009 0:ca455457108f 142 int alt33_count = 0, speed33_count = 0;
Okamoto009 0:ca455457108f 143
Okamoto009 0:ca455457108f 144 bool ok_up = false;
Okamoto009 0:ca455457108f 145 bool ok_down = false;
Okamoto009 0:ca455457108f 146 bool ok_top = false;
Okamoto009 0:ca455457108f 147
Okamoto009 0:ca455457108f 148 float temp35, hum35;
Okamoto009 0:ca455457108f 149
Okamoto009 0:ca455457108f 150 int lux;
Okamoto009 0:ca455457108f 151
Okamoto009 0:ca455457108f 152 float pitot_raw, pitot_err, pitot;
Okamoto009 0:ca455457108f 153 float pitot_buf[100];
Okamoto009 0:ca455457108f 154 int pitot_count = 0;
Okamoto009 0:ca455457108f 155 float pitot_speed;
Okamoto009 0:ca455457108f 156
Okamoto009 0:ca455457108f 157 float gps_lat, gps_lon, gps_alt, gps_utc[6], gps_knot, gps_deg;
Okamoto009 0:ca455457108f 158 int gps_sat;
Okamoto009 0:ca455457108f 159 bool gps_yn;
Okamoto009 0:ca455457108f 160
Okamoto009 0:ca455457108f 161 bool flight_pin;
Okamoto009 0:ca455457108f 162
Okamoto009 0:ca455457108f 163 FILE *fp;
Okamoto009 0:ca455457108f 164 bool save = false;
Okamoto009 0:ca455457108f 165 int save_c = 0;
Okamoto009 0:ca455457108f 166
Okamoto009 0:ca455457108f 167 int time_read, time_reset = 0;
Okamoto009 0:ca455457108f 168 int time_min, time_sec, time_ms;
Okamoto009 0:ca455457108f 169
Okamoto009 0:ca455457108f 170 int flight_time_read, flight_time_read_old, flight_time_reset = 0;
Okamoto009 0:ca455457108f 171 int flight_time_min, flight_time_sec, flight_time_ms;
Okamoto009 0:ca455457108f 172
Okamoto009 0:ca455457108f 173
Okamoto009 0:ca455457108f 174 /*******************************************************************************
Okamoto009 0:ca455457108f 175 定数
Okamoto009 0:ca455457108f 176 *******************************************************************************/
Okamoto009 0:ca455457108f 177 const float ES920_MAX_20 = 0.000305176;
Okamoto009 0:ca455457108f 178 const float ES920_MAX_100 = 0.001525879;
Okamoto009 0:ca455457108f 179 const float ES920_MAX_200 = 0.003051758;
Okamoto009 0:ca455457108f 180 const float ES920_MAX_500 = 0.007629395;
Okamoto009 0:ca455457108f 181 const float ES920_MAX_1500 = 0.022888184;
Okamoto009 0:ca455457108f 182 const float ES920_MAX_3000 = 0.045776367;
Okamoto009 0:ca455457108f 183
Okamoto009 0:ca455457108f 184
Okamoto009 0:ca455457108f 185 /*******************************************************************************
Okamoto009 0:ca455457108f 186 無線のヘッダまとめ(ROCKET -> GND)
Okamoto009 0:ca455457108f 187 *******************************************************************************/
Okamoto009 0:ca455457108f 188 const char HEADER_SENSOR_SETUP = 0x01;
Okamoto009 0:ca455457108f 189 // 0x01, ADXL, INA_in, INA_ex, MPU, MPU_m, 22HB, 33HW, SHT, TSL, SD
Okamoto009 0:ca455457108f 190 // 0 1 1 1 1 1 1 1 1 1 1
Okamoto009 0:ca455457108f 191 // 0 1 2 3 4 5 6 7 8 9
Okamoto009 0:ca455457108f 192
Okamoto009 0:ca455457108f 193 const char HEADER_GPS_SETUP = 0x02;
Okamoto009 0:ca455457108f 194 // 0x02, readable, wait_count
Okamoto009 0:ca455457108f 195 // 0 1 4
Okamoto009 0:ca455457108f 196 // 0 1
Okamoto009 0:ca455457108f 197 // 0x00 : NG
Okamoto009 0:ca455457108f 198 // 0x01 : OK
Okamoto009 0:ca455457108f 199 // 0xAA : Finish
Okamoto009 0:ca455457108f 200 // 0xFF : Ignore
Okamoto009 0:ca455457108f 201
Okamoto009 0:ca455457108f 202 const char HEADER_DATA = 0x10;
Okamoto009 0:ca455457108f 203 //起動時間, フライト時間, フェーズ, 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
Okamoto009 0:ca455457108f 204 //4(2,2) 4(2,2) 2 1 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2
Okamoto009 0:ca455457108f 205 //0 2 4 6 8 10 11 13 15 17 19 21 23 25 27 29 31 33 35 37 39 41
Okamoto009 0:ca455457108f 206 // 0x01 : ex_pow
Okamoto009 0:ca455457108f 207 // 0x02 : Sep1
Okamoto009 0:ca455457108f 208 // 0x04 : Sep2
Okamoto009 0:ca455457108f 209 // 0x08 : ok_up
Okamoto009 0:ca455457108f 210 // 0x10 : ok_down
Okamoto009 0:ca455457108f 211 // 0x20 : ok_top
Okamoto009 0:ca455457108f 212 // 0x40 : save
Okamoto009 0:ca455457108f 213 // 0x80 : flight_pin
Okamoto009 0:ca455457108f 214
Okamoto009 0:ca455457108f 215 const char HEADER_START = 0x11;
Okamoto009 0:ca455457108f 216 // 0x11, What
Okamoto009 0:ca455457108f 217 // 0 1
Okamoto009 0:ca455457108f 218 // 0
Okamoto009 0:ca455457108f 219 // 'W' : 安全モード->離床検知モード
Okamoto009 0:ca455457108f 220 // 'S' : 離床検知モード->安全モード
Okamoto009 0:ca455457108f 221 // 'F' : 離床検知モード->フライトモード
Okamoto009 0:ca455457108f 222 // '1' : 1段目分離
Okamoto009 0:ca455457108f 223 // '2' : 2段目分離
Okamoto009 0:ca455457108f 224
Okamoto009 0:ca455457108f 225
Okamoto009 0:ca455457108f 226 /*******************************************************************************
Okamoto009 0:ca455457108f 227 無線のヘッダまとめ(GND -> ROCKET)
Okamoto009 0:ca455457108f 228 *******************************************************************************/
Okamoto009 0:ca455457108f 229 const char HEADER_COMMAND = 0xcd;
Okamoto009 0:ca455457108f 230 // 0xcd,コマンド
Okamoto009 0:ca455457108f 231 // 0 1
Okamoto009 0:ca455457108f 232 // 0 1
Okamoto009 0:ca455457108f 233
Okamoto009 0:ca455457108f 234
Okamoto009 0:ca455457108f 235 /*******************************************************************************
Okamoto009 0:ca455457108f 236 ロケットのフェーズ
Okamoto009 0:ca455457108f 237 *******************************************************************************/
Okamoto009 0:ca455457108f 238 const char ROCKET_SETUP = 0x00; //セットアップ中
Okamoto009 0:ca455457108f 239 const char ROCKET_SAFETY = 0x01; //安全モード
Okamoto009 0:ca455457108f 240 const char ROCKET_WAIT = 0x02; //待機モード
Okamoto009 0:ca455457108f 241 const char ROCKET_FLIGHT = 0x03; //フライトモード
Okamoto009 0:ca455457108f 242 const char ROCKET_SEP1 = 0x04; //分離1モード
Okamoto009 0:ca455457108f 243 const char ROCKET_SEP2 = 0x05; //分離2モード
Okamoto009 0:ca455457108f 244 const char ROCKET_RECOVERY = 0x06; //回収モード
Okamoto009 0:ca455457108f 245
Okamoto009 0:ca455457108f 246
Okamoto009 0:ca455457108f 247 /*******************************************************************************
Okamoto009 0:ca455457108f 248 メイン関数
Okamoto009 0:ca455457108f 249 *******************************************************************************/
Okamoto009 0:ca455457108f 250 int main() {
Okamoto009 0:ca455457108f 251 rocket_phase = ROCKET_SETUP;
Okamoto009 0:ca455457108f 252 setup();
Okamoto009 0:ca455457108f 253
Okamoto009 0:ca455457108f 254 pc.attach(&readPC, Serial::RxIrq);
Okamoto009 0:ca455457108f 255
Okamoto009 0:ca455457108f 256 tick_gps.attach(&readGPS, 1/GPS_freq);
Okamoto009 0:ca455457108f 257 tick_print.attach(&printData, 1.0f);
Okamoto009 0:ca455457108f 258 tick_header_data.attach(&sendDownLink, 1.0f);
Okamoto009 0:ca455457108f 259
Okamoto009 0:ca455457108f 260 es920.attach(&readUpLink, HEADER_COMMAND);
Okamoto009 0:ca455457108f 261
Okamoto009 0:ca455457108f 262 FlightPin.mode(PullUp);
Okamoto009 0:ca455457108f 263
Okamoto009 0:ca455457108f 264 time_main.reset();
Okamoto009 0:ca455457108f 265 time_main.start();
Okamoto009 0:ca455457108f 266 startRec();
Okamoto009 0:ca455457108f 267
Okamoto009 0:ca455457108f 268 rocket_phase = ROCKET_SAFETY;
Okamoto009 0:ca455457108f 269 while(1) {
Okamoto009 0:ca455457108f 270 readSensor();
Okamoto009 0:ca455457108f 271 recData();
Okamoto009 0:ca455457108f 272 modeChange(); //状態遷移の判断
Okamoto009 0:ca455457108f 273 }
Okamoto009 0:ca455457108f 274 }
Okamoto009 0:ca455457108f 275
Okamoto009 0:ca455457108f 276
Okamoto009 0:ca455457108f 277 /*******************************************************************************
Okamoto009 0:ca455457108f 278 センサ読み取り
Okamoto009 0:ca455457108f 279 *******************************************************************************/
Okamoto009 0:ca455457108f 280 void readSensor(){
Okamoto009 0:ca455457108f 281 time_read = time_main.read_ms();
Okamoto009 0:ca455457108f 282 if(time_read >= 30*60*1000){
Okamoto009 0:ca455457108f 283 time_main.reset();
Okamoto009 0:ca455457108f 284 time_reset ++;
Okamoto009 0:ca455457108f 285 }
Okamoto009 0:ca455457108f 286 time_min = time_reset * 30 + floor((double)time_read / (60 * 1000));
Okamoto009 0:ca455457108f 287 time_sec = time_read % (60 * 1000);
Okamoto009 0:ca455457108f 288 time_sec = floor((double)time_sec / 1000);
Okamoto009 0:ca455457108f 289 time_ms = time_read % 1000;
Okamoto009 0:ca455457108f 290
Okamoto009 0:ca455457108f 291 if(rocket_phase >= ROCKET_FLIGHT){
Okamoto009 0:ca455457108f 292 flight_time_read_old = flight_time_read;
Okamoto009 0:ca455457108f 293 flight_time_read = time_flight.read_ms();
Okamoto009 0:ca455457108f 294 }
Okamoto009 0:ca455457108f 295 else{
Okamoto009 0:ca455457108f 296 flight_time_read = 0;
Okamoto009 0:ca455457108f 297 }
Okamoto009 0:ca455457108f 298 if(flight_time_read >= 30*60*1000){
Okamoto009 0:ca455457108f 299 time_flight.reset();
Okamoto009 0:ca455457108f 300 flight_time_reset ++;
Okamoto009 0:ca455457108f 301 }
Okamoto009 0:ca455457108f 302 flight_time_min = flight_time_reset * 30 + floor((double)flight_time_read / (60 * 1000));
Okamoto009 0:ca455457108f 303 flight_time_sec = flight_time_read % (60 * 1000);
Okamoto009 0:ca455457108f 304 flight_time_sec = floor((double)flight_time_sec / 1000);
Okamoto009 0:ca455457108f 305 flight_time_ms = flight_time_read % 1000;
Okamoto009 0:ca455457108f 306
Okamoto009 0:ca455457108f 307 ADXL375.getOutput(adxl_acc);
Okamoto009 0:ca455457108f 308
Okamoto009 0:ca455457108f 309 INA226_in.get_Voltage_current(&ina_in_v, &ina_in_i);
Okamoto009 0:ca455457108f 310 ina_in_v = ina_in_v / 1000;
Okamoto009 0:ca455457108f 311 ina_in_i = ina_in_i / 1000;
Okamoto009 0:ca455457108f 312 if(ina_in_i > 64){
Okamoto009 0:ca455457108f 313 ina_in_i = 0.0f;
Okamoto009 0:ca455457108f 314 }
Okamoto009 0:ca455457108f 315 INA226_ex.get_Voltage_current(&ina_ex_v, &ina_ex_i);
Okamoto009 0:ca455457108f 316 ina_ex_v = ina_ex_v / 1000;
Okamoto009 0:ca455457108f 317 ina_ex_i = ina_ex_i / 1000;
Okamoto009 0:ca455457108f 318 if(ina_ex_i > 64){
Okamoto009 0:ca455457108f 319 ina_ex_i = 0.0f;
Okamoto009 0:ca455457108f 320 }
Okamoto009 0:ca455457108f 321 if(ina_ex_i > 0.1f){
Okamoto009 0:ca455457108f 322 ex_power = true;
Okamoto009 0:ca455457108f 323 }
Okamoto009 0:ca455457108f 324 else{
Okamoto009 0:ca455457108f 325 ex_power = false;
Okamoto009 0:ca455457108f 326 }
Okamoto009 0:ca455457108f 327
Okamoto009 0:ca455457108f 328 nine.getGyro(gyro);
Okamoto009 0:ca455457108f 329 nine.getAcc(acc);
Okamoto009 0:ca455457108f 330 nine.getMag(mag);
Okamoto009 0:ca455457108f 331 for(int i = 0; i < 3; i ++){
Okamoto009 0:ca455457108f 332 gyro_rad[i] = gyro[i] * 3.1415926535 / 180;
Okamoto009 0:ca455457108f 333 }
Okamoto009 0:ca455457108f 334 attitude.MadgwickAHRSupdate(gyro_rad, acc, mag);
Okamoto009 0:ca455457108f 335 attitude.getAttitude(&quart);
Okamoto009 0:ca455457108f 336 attitude.getEulerAngle(euler);
Okamoto009 0:ca455457108f 337 for(int i = 0; i < 3; i ++){
Okamoto009 0:ca455457108f 338 euler[i] = euler[i] * 180 / 3.145926535;
Okamoto009 0:ca455457108f 339 }
Okamoto009 0:ca455457108f 340
Okamoto009 0:ca455457108f 341 pres22 = LPS22HB.getPres();
Okamoto009 0:ca455457108f 342 temp22 = LPS22HB.getTemp();
Okamoto009 0:ca455457108f 343 alt22 = LPS22HB.getAlt(pres22_0, temp22_0);
Okamoto009 0:ca455457108f 344
Okamoto009 0:ca455457108f 345 pres33 = LPS33HW.getPres();
Okamoto009 0:ca455457108f 346 temp33 = LPS33HW.getTemp();
Okamoto009 0:ca455457108f 347 alt33 = LPS33HW.getAlt(pres33_0, temp33_0);
Okamoto009 0:ca455457108f 348
Okamoto009 0:ca455457108f 349 alt33_buf[alt33_count] = alt33;
Okamoto009 0:ca455457108f 350 alt33_count ++;
Okamoto009 0:ca455457108f 351 if(alt33_count > 10){
Okamoto009 0:ca455457108f 352 alt33_count = 0;
Okamoto009 0:ca455457108f 353 }
Okamoto009 0:ca455457108f 354 float alt33_sum = 0;
Okamoto009 0:ca455457108f 355 for(int i = 0; i < 10; i ++){
Okamoto009 0:ca455457108f 356 alt33_sum += alt33_buf[i];
Okamoto009 0:ca455457108f 357 }
Okamoto009 0:ca455457108f 358 alt33_ave_old = alt33_ave;
Okamoto009 0:ca455457108f 359 alt33_ave = alt33_sum / 10;
Okamoto009 0:ca455457108f 360
Okamoto009 0:ca455457108f 361 if(alt33_ave - alt33_ave_old > 0.01 || alt33_ave - alt33_ave_old < -0.01){
Okamoto009 0:ca455457108f 362 speed33 = (alt33_ave - alt33_ave_old) / (((float)flight_time_read - (float)flight_time_read_old) / 1000);
Okamoto009 0:ca455457108f 363 }
Okamoto009 0:ca455457108f 364 if(rocket_phase <= ROCKET_WAIT){
Okamoto009 0:ca455457108f 365 speed33 = 0.0f;
Okamoto009 0:ca455457108f 366 }
Okamoto009 0:ca455457108f 367
Okamoto009 0:ca455457108f 368 speed33_buf[speed33_count] = speed33;
Okamoto009 0:ca455457108f 369 speed33_count ++;
Okamoto009 0:ca455457108f 370 if(speed33_count > 10){
Okamoto009 0:ca455457108f 371 speed33_count = 0;
Okamoto009 0:ca455457108f 372 }
Okamoto009 0:ca455457108f 373 float speed33_sum = 0;
Okamoto009 0:ca455457108f 374 for(int i = 0; i < 10; i ++){
Okamoto009 0:ca455457108f 375 speed33_sum += speed33_buf[i];
Okamoto009 0:ca455457108f 376 }
Okamoto009 0:ca455457108f 377 speed33_ave = speed33_sum / 10;
Okamoto009 0:ca455457108f 378
Okamoto009 0:ca455457108f 379
Okamoto009 0:ca455457108f 380 if(rocket_phase <= ROCKET_WAIT){
Okamoto009 0:ca455457108f 381 pres22_0 = pres22;
Okamoto009 0:ca455457108f 382 temp22_0 = temp22;
Okamoto009 0:ca455457108f 383 pres33_0 = pres33;
Okamoto009 0:ca455457108f 384 temp33_0 = temp33;
Okamoto009 0:ca455457108f 385 }
Okamoto009 0:ca455457108f 386
Okamoto009 0:ca455457108f 387 SHT35.getTempHum(&temp35, &hum35);
Okamoto009 0:ca455457108f 388
Okamoto009 0:ca455457108f 389 lux = TSL2561.getLuminous();
Okamoto009 0:ca455457108f 390
Okamoto009 0:ca455457108f 391 /*pitot_raw = Pitot.read() * 3.3;
Okamoto009 0:ca455457108f 392 pitot = (pitot_raw - pitot_err - 0.2) / (5 * 0.0012858);
Okamoto009 0:ca455457108f 393 if(pitot < 0.0f){
Okamoto009 0:ca455457108f 394 pitot = 0.0f;
Okamoto009 0:ca455457108f 395 }
Okamoto009 0:ca455457108f 396 pitot_speed = sqrt(2 * pitot * 1000 / 1.3);
Okamoto009 0:ca455457108f 397 if(rocket_phase <= ROCKET_WAIT){
Okamoto009 0:ca455457108f 398 pitot_buf[pitot_count] = pitot;
Okamoto009 0:ca455457108f 399 pitot_count ++;
Okamoto009 0:ca455457108f 400 if(pitot_count > 100){
Okamoto009 0:ca455457108f 401 pitot_err = 0;
Okamoto009 0:ca455457108f 402 for(int i = 0; i < 100; i ++){
Okamoto009 0:ca455457108f 403 pitot_err += pitot_buf[pitot_count];
Okamoto009 0:ca455457108f 404 }
Okamoto009 0:ca455457108f 405 pitot_err = pitot_err / 100;
Okamoto009 0:ca455457108f 406 pitot_count = 0;
Okamoto009 0:ca455457108f 407 }
Okamoto009 0:ca455457108f 408 }*/
Okamoto009 0:ca455457108f 409
Okamoto009 0:ca455457108f 410 flight_pin = FlightPin;
Okamoto009 0:ca455457108f 411 }
Okamoto009 0:ca455457108f 412
Okamoto009 0:ca455457108f 413
Okamoto009 0:ca455457108f 414 /*******************************************************************************
Okamoto009 0:ca455457108f 415 GPS読み取り
Okamoto009 0:ca455457108f 416 *******************************************************************************/
Okamoto009 0:ca455457108f 417 void readGPS(){
Okamoto009 0:ca455457108f 418 gps_yn = GPS.gps_readable;
Okamoto009 0:ca455457108f 419 if(gps_yn){
Okamoto009 0:ca455457108f 420 gps_lat = GPS.Latitude();
Okamoto009 0:ca455457108f 421 gps_lon = GPS.Longitude();
Okamoto009 0:ca455457108f 422 gps_alt = GPS.Height();
Okamoto009 0:ca455457108f 423 GPS.getUTC(gps_utc);
Okamoto009 0:ca455457108f 424 gps_utc[3] += 9;
Okamoto009 0:ca455457108f 425 if(gps_utc[3] >= 24){
Okamoto009 0:ca455457108f 426 gps_utc[3] -= 24;
Okamoto009 0:ca455457108f 427 gps_utc[2] += 1;
Okamoto009 0:ca455457108f 428 }
Okamoto009 0:ca455457108f 429 gps_knot = GPS.Knot();
Okamoto009 0:ca455457108f 430 gps_deg = GPS.Degree();
Okamoto009 0:ca455457108f 431 gps_sat = GPS.Number();
Okamoto009 0:ca455457108f 432 }
Okamoto009 0:ca455457108f 433 }
Okamoto009 0:ca455457108f 434
Okamoto009 0:ca455457108f 435
Okamoto009 0:ca455457108f 436 /*******************************************************************************
Okamoto009 0:ca455457108f 437 PCでデータ表示
Okamoto009 0:ca455457108f 438 *******************************************************************************/
Okamoto009 0:ca455457108f 439 void printData(){
Okamoto009 0:ca455457108f 440 if(ok_PC){
Okamoto009 0:ca455457108f 441 pc.printf("Time : %d:%02d\r\n", time_min, time_sec);
Okamoto009 0:ca455457108f 442 pc.printf("FlightTime : %d:%02d\r\n", flight_time_min, flight_time_sec);
Okamoto009 0:ca455457108f 443
Okamoto009 0:ca455457108f 444 pc.printf("Phase : ");
Okamoto009 0:ca455457108f 445 switch(rocket_phase){
Okamoto009 0:ca455457108f 446 case ROCKET_SETUP:
Okamoto009 0:ca455457108f 447 pc.printf("SETUP\r\n");
Okamoto009 0:ca455457108f 448 break;
Okamoto009 0:ca455457108f 449 case ROCKET_SAFETY:
Okamoto009 0:ca455457108f 450 pc.printf("SAFETY\r\n");
Okamoto009 0:ca455457108f 451 break;
Okamoto009 0:ca455457108f 452 case ROCKET_WAIT:
Okamoto009 0:ca455457108f 453 pc.printf("WAIT\r\n");
Okamoto009 0:ca455457108f 454 break;
Okamoto009 0:ca455457108f 455 case ROCKET_FLIGHT:
Okamoto009 0:ca455457108f 456 pc.printf("FLIGHT\r\n");
Okamoto009 0:ca455457108f 457 break;
Okamoto009 0:ca455457108f 458 case ROCKET_SEP1:
Okamoto009 0:ca455457108f 459 pc.printf("SEPARATE 1\r\n");
Okamoto009 0:ca455457108f 460 break;
Okamoto009 0:ca455457108f 461 case ROCKET_SEP2:
Okamoto009 0:ca455457108f 462 pc.printf("SEPARATE 2\r\n");
Okamoto009 0:ca455457108f 463 break;
Okamoto009 0:ca455457108f 464 case ROCKET_RECOVERY:
Okamoto009 0:ca455457108f 465 pc.printf("RECOVERY\r\n");
Okamoto009 0:ca455457108f 466 break;
Okamoto009 0:ca455457108f 467 }
Okamoto009 0:ca455457108f 468
Okamoto009 0:ca455457108f 469 pc.printf("Power : ");
Okamoto009 0:ca455457108f 470 if(ex_power){
Okamoto009 0:ca455457108f 471 pc.printf("External\r\n");
Okamoto009 0:ca455457108f 472 }
Okamoto009 0:ca455457108f 473 else{
Okamoto009 0:ca455457108f 474 pc.printf("Battery\r\n");
Okamoto009 0:ca455457108f 475 }
Okamoto009 0:ca455457108f 476
Okamoto009 0:ca455457108f 477 pc.printf("Separate : %d, %d\r\n", Separate1.status, Separate2.status);
Okamoto009 0:ca455457108f 478 pc.printf("UP DOWN TOP : %d, %d, %d\r\n", ok_up, ok_down, ok_top);
Okamoto009 0:ca455457108f 479 pc.printf("SAVE : %d\r\n", save);
Okamoto009 0:ca455457108f 480 pc.printf("Flight Pin : %d\r\n", flight_pin);
Okamoto009 0:ca455457108f 481
Okamoto009 0:ca455457108f 482 pc.printf("GPS : %3.7f, %3.7f, %.2f\r\n", gps_lat, gps_lon, gps_alt);
Okamoto009 0:ca455457108f 483 pc.printf("Move : %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg);
Okamoto009 0:ca455457108f 484 pc.printf("Sats : %d\r\n", gps_sat);
Okamoto009 0:ca455457108f 485
Okamoto009 0:ca455457108f 486 pc.printf("INA_in : %.2f[V], %.2f[A]\r\n", ina_in_v, ina_in_i);
Okamoto009 0:ca455457108f 487 pc.printf("INA_ex : %.2f[V], %.2f[A]\r\n", ina_ex_v, ina_ex_i);
Okamoto009 0:ca455457108f 488
Okamoto009 0:ca455457108f 489 pc.printf("LPS33HW : %.4f[hPa], %.2f[m], %.2f[m/s]\r\n", pres33, alt33, speed33);
Okamoto009 0:ca455457108f 490 pc.printf("SHT35 : %.2f[degC], %.2f[%%]\r\n", temp35, hum35);
Okamoto009 0:ca455457108f 491 // pc.printf("Pitot : %.2f[m/s]\r\n", pitot_speed);
Okamoto009 0:ca455457108f 492
Okamoto009 0:ca455457108f 493 pc.printf("\n\n\n");
Okamoto009 0:ca455457108f 494 }
Okamoto009 0:ca455457108f 495 }
Okamoto009 0:ca455457108f 496
Okamoto009 0:ca455457108f 497
Okamoto009 0:ca455457108f 498 /*******************************************************************************
Okamoto009 0:ca455457108f 499 PCからコマンド読み取り
Okamoto009 0:ca455457108f 500 *******************************************************************************/
Okamoto009 0:ca455457108f 501 void readPC(){
Okamoto009 0:ca455457108f 502 if(ok_PC){
Okamoto009 0:ca455457108f 503 char command = pc.getc();
Okamoto009 0:ca455457108f 504 pc.printf("PC Command = %c\r\n", command);
Okamoto009 0:ca455457108f 505
Okamoto009 0:ca455457108f 506 switch(command){
Okamoto009 0:ca455457108f 507 case '?':
Okamoto009 0:ca455457108f 508 printHelp();
Okamoto009 0:ca455457108f 509 break;
Okamoto009 0:ca455457108f 510
Okamoto009 0:ca455457108f 511 case 'W': //離床検知モードへ移行
Okamoto009 0:ca455457108f 512 if(rocket_phase == ROCKET_SAFETY){
Okamoto009 0:ca455457108f 513 rocket_phase = ROCKET_WAIT;
Okamoto009 0:ca455457108f 514 do_first = false;
Okamoto009 0:ca455457108f 515 }
Okamoto009 0:ca455457108f 516 break;
Okamoto009 0:ca455457108f 517
Okamoto009 0:ca455457108f 518 case 'S': //安全モードへ移行
Okamoto009 0:ca455457108f 519 if(rocket_phase == ROCKET_WAIT){
Okamoto009 0:ca455457108f 520 rocket_phase = ROCKET_SAFETY;
Okamoto009 0:ca455457108f 521 do_first = false;
Okamoto009 0:ca455457108f 522 }
Okamoto009 0:ca455457108f 523 if(rocket_phase == ROCKET_FLIGHT){
Okamoto009 0:ca455457108f 524 rocket_phase = ROCKET_SAFETY;
Okamoto009 0:ca455457108f 525 do_first = false;
Okamoto009 0:ca455457108f 526 timeout_sep.detach();
Okamoto009 0:ca455457108f 527 }
Okamoto009 0:ca455457108f 528 break;
Okamoto009 0:ca455457108f 529
Okamoto009 0:ca455457108f 530 case 'F': //フライトモードへ移行（手動）
Okamoto009 0:ca455457108f 531 if(rocket_phase == ROCKET_WAIT){
Okamoto009 0:ca455457108f 532 rocket_phase = ROCKET_FLIGHT;
Okamoto009 0:ca455457108f 533 do_first = false;
Okamoto009 0:ca455457108f 534 }
Okamoto009 0:ca455457108f 535 break;
Okamoto009 0:ca455457108f 536
Okamoto009 0:ca455457108f 537 case '1': //1段目強制分離
Okamoto009 0:ca455457108f 538 if(rocket_phase == ROCKET_FLIGHT){
Okamoto009 0:ca455457108f 539 rocket_phase = ROCKET_SEP1;
Okamoto009 0:ca455457108f 540 do_first = false;
Okamoto009 0:ca455457108f 541 }
Okamoto009 0:ca455457108f 542 else if(rocket_phase >= ROCKET_SEP1){
Okamoto009 0:ca455457108f 543 separate1();
Okamoto009 0:ca455457108f 544 }
Okamoto009 0:ca455457108f 545 break;
Okamoto009 0:ca455457108f 546
Okamoto009 0:ca455457108f 547 case '2': //2段目強制分離
Okamoto009 0:ca455457108f 548 if(rocket_phase == ROCKET_SEP1){
Okamoto009 0:ca455457108f 549 rocket_phase = ROCKET_SEP2;
Okamoto009 0:ca455457108f 550 do_first = false;
Okamoto009 0:ca455457108f 551 }
Okamoto009 0:ca455457108f 552 else if(rocket_phase >= ROCKET_SEP2){
Okamoto009 0:ca455457108f 553 separate2();
Okamoto009 0:ca455457108f 554 }
Okamoto009 0:ca455457108f 555 break;
Okamoto009 0:ca455457108f 556
Okamoto009 0:ca455457108f 557 case 'C':
Okamoto009 0:ca455457108f 558 if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && save){
Okamoto009 0:ca455457108f 559 stopRec();
Okamoto009 0:ca455457108f 560 }
Okamoto009 0:ca455457108f 561 break;
Okamoto009 0:ca455457108f 562
Okamoto009 0:ca455457108f 563 case 'O':
Okamoto009 0:ca455457108f 564 if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && !save){
Okamoto009 0:ca455457108f 565 startRec();
Okamoto009 0:ca455457108f 566 }
Okamoto009 0:ca455457108f 567 break;
Okamoto009 0:ca455457108f 568
Okamoto009 0:ca455457108f 569 case 'B':
Okamoto009 0:ca455457108f 570 buzzerChange();
Okamoto009 0:ca455457108f 571 break;
Okamoto009 0:ca455457108f 572 }
Okamoto009 0:ca455457108f 573 }
Okamoto009 0:ca455457108f 574 }
Okamoto009 0:ca455457108f 575
Okamoto009 0:ca455457108f 576
Okamoto009 0:ca455457108f 577 /*******************************************************************************
Okamoto009 0:ca455457108f 578 コマンド情報を表示
Okamoto009 0:ca455457108f 579 *******************************************************************************/
Okamoto009 0:ca455457108f 580 void printHelp(){
Okamoto009 0:ca455457108f 581 for(int i = 0; i < 20; i ++){
Okamoto009 0:ca455457108f 582 pc.printf("*");
Okamoto009 0:ca455457108f 583 }
Okamoto009 0:ca455457108f 584
Okamoto009 0:ca455457108f 585 pc.printf("\r\nCommands\r\n");
Okamoto009 0:ca455457108f 586 pc.printf("W : Safety -> Wait\r\n");
Okamoto009 0:ca455457108f 587 pc.printf("S : Wait -> Safety\r\n");
Okamoto009 0:ca455457108f 588 pc.printf("F : Wait -> Flight\r\n");
Okamoto009 0:ca455457108f 589 pc.printf("1(one) : 1st Separate 5[s]\r\n");
Okamoto009 0:ca455457108f 590 pc.printf("2(two) : 2nd Separate 5[s]\r\n");
Okamoto009 0:ca455457108f 591 pc.printf("C : Stop Recording\r\n");
Okamoto009 0:ca455457108f 592 pc.printf("? : HELP\r\n");
Okamoto009 0:ca455457108f 593
Okamoto009 0:ca455457108f 594 for(int i = 0; i < 20; i ++){
Okamoto009 0:ca455457108f 595 pc.printf("*");
Okamoto009 0:ca455457108f 596 }
Okamoto009 0:ca455457108f 597 pc.printf("\r\n");
Okamoto009 0:ca455457108f 598 wait(1.0f);
Okamoto009 0:ca455457108f 599 }
Okamoto009 0:ca455457108f 600
Okamoto009 0:ca455457108f 601
Okamoto009 0:ca455457108f 602 /*******************************************************************************
Okamoto009 0:ca455457108f 603 ダウンリンクを表示
Okamoto009 0:ca455457108f 604 *******************************************************************************/
Okamoto009 0:ca455457108f 605 void sendDownLink(){
Okamoto009 0:ca455457108f 606 //無線あり
Okamoto009 0:ca455457108f 607 if(!es920_using){
Okamoto009 0:ca455457108f 608 es920 << HEADER_DATA;
Okamoto009 0:ca455457108f 609
Okamoto009 0:ca455457108f 610 es920 << (short)time_reset;
Okamoto009 0:ca455457108f 611 es920 << (short)(time_read / 1000);
Okamoto009 0:ca455457108f 612
Okamoto009 0:ca455457108f 613 es920 << (short)flight_time_reset;
Okamoto009 0:ca455457108f 614 es920 << (short)(flight_time_read / 1000);
Okamoto009 0:ca455457108f 615
Okamoto009 0:ca455457108f 616 es920 << rocket_phase;
Okamoto009 0:ca455457108f 617
Okamoto009 0:ca455457108f 618 char status = 0x00;
Okamoto009 0:ca455457108f 619 if(ex_power) status |= (char)0x01;
Okamoto009 0:ca455457108f 620 if(Separate1.status)status |= (char)0x02;
Okamoto009 0:ca455457108f 621 if(Separate2.status)status |= (char)0x04;
Okamoto009 0:ca455457108f 622 if(ok_up) status |= (char)0x08;
Okamoto009 0:ca455457108f 623 if(ok_down) status |= (char)0x10;
Okamoto009 0:ca455457108f 624 if(ok_top) status |= (char)0x20;
Okamoto009 0:ca455457108f 625 if(save) status |= (char)0x40;
Okamoto009 0:ca455457108f 626 if(flight_pin) status |= (char)0x80;
Okamoto009 0:ca455457108f 627 es920 << status;
Okamoto009 0:ca455457108f 628
Okamoto009 0:ca455457108f 629 es920 << (short)(gps_lat / ES920_MAX_100);
Okamoto009 0:ca455457108f 630 es920 << (short)(gps_lon / ES920_MAX_500);
Okamoto009 0:ca455457108f 631 es920 << (short)(gps_alt / ES920_MAX_500);
Okamoto009 0:ca455457108f 632 es920 << (short)(gps_knot / ES920_MAX_200);
Okamoto009 0:ca455457108f 633 es920 << (short)(gps_deg / ES920_MAX_1500);
Okamoto009 0:ca455457108f 634 es920 << (short)gps_sat;
Okamoto009 0:ca455457108f 635
Okamoto009 0:ca455457108f 636 es920 << (short)(ina_in_v / ES920_MAX_20);
Okamoto009 0:ca455457108f 637 es920 << (short)(ina_in_i / ES920_MAX_20);
Okamoto009 0:ca455457108f 638 es920 << (short)(ina_ex_v / ES920_MAX_20);
Okamoto009 0:ca455457108f 639 es920 << (short)(ina_ex_i / ES920_MAX_20);
Okamoto009 0:ca455457108f 640
Okamoto009 0:ca455457108f 641 es920 << (short)(pres33 / ES920_MAX_3000);
Okamoto009 0:ca455457108f 642 es920 << (short)(alt33_ave / ES920_MAX_500);
Okamoto009 0:ca455457108f 643 es920 << (short)(speed33_ave / ES920_MAX_200);
Okamoto009 0:ca455457108f 644 es920 << (short)(temp35 / ES920_MAX_100);
Okamoto009 0:ca455457108f 645 es920 << (short)(hum35 / ES920_MAX_200);
Okamoto009 0:ca455457108f 646
Okamoto009 0:ca455457108f 647 es920 << (short)(pitot_speed / ES920_MAX_200);
Okamoto009 0:ca455457108f 648
Okamoto009 0:ca455457108f 649 es920.send();
Okamoto009 0:ca455457108f 650 }
Okamoto009 0:ca455457108f 651 else{
Okamoto009 0:ca455457108f 652 es920_using = false;
Okamoto009 0:ca455457108f 653 }
Okamoto009 0:ca455457108f 654 }
Okamoto009 0:ca455457108f 655
Okamoto009 0:ca455457108f 656
Okamoto009 0:ca455457108f 657 /*******************************************************************************
Okamoto009 0:ca455457108f 658 アップリンクを解析
Okamoto009 0:ca455457108f 659 *******************************************************************************/
Okamoto009 0:ca455457108f 660 void readUpLink(){
Okamoto009 0:ca455457108f 661 //無線あり
Okamoto009 0:ca455457108f 662 es920_uplink_command = es920.data[0];
Okamoto009 0:ca455457108f 663 pc.printf("GND Command = %c\r\n", es920_uplink_command);
Okamoto009 0:ca455457108f 664
Okamoto009 0:ca455457108f 665 switch(es920_uplink_command){
Okamoto009 0:ca455457108f 666 case 'W': //離床検知モードへ移行
Okamoto009 0:ca455457108f 667 if(rocket_phase == ROCKET_SAFETY){
Okamoto009 0:ca455457108f 668 rocket_phase = ROCKET_WAIT;
Okamoto009 0:ca455457108f 669 do_first = false;
Okamoto009 0:ca455457108f 670 }
Okamoto009 0:ca455457108f 671 break;
Okamoto009 0:ca455457108f 672
Okamoto009 0:ca455457108f 673 case 'S': //安全モードへ移行
Okamoto009 0:ca455457108f 674 if(rocket_phase == ROCKET_WAIT){
Okamoto009 0:ca455457108f 675 rocket_phase = ROCKET_SAFETY;
Okamoto009 0:ca455457108f 676 do_first = false;
Okamoto009 0:ca455457108f 677 }
Okamoto009 0:ca455457108f 678 if(rocket_phase == ROCKET_FLIGHT){
Okamoto009 0:ca455457108f 679 rocket_phase = ROCKET_SAFETY;
Okamoto009 0:ca455457108f 680 do_first = false;
Okamoto009 0:ca455457108f 681 timeout_sep.detach();
Okamoto009 0:ca455457108f 682 }
Okamoto009 0:ca455457108f 683
Okamoto009 0:ca455457108f 684 break;
Okamoto009 0:ca455457108f 685
Okamoto009 0:ca455457108f 686 case 'F': //フライトモードへ移行（手動）
Okamoto009 0:ca455457108f 687 if(rocket_phase == ROCKET_WAIT){
Okamoto009 0:ca455457108f 688 rocket_phase = ROCKET_FLIGHT;
Okamoto009 0:ca455457108f 689 do_first = false;
Okamoto009 0:ca455457108f 690 }
Okamoto009 0:ca455457108f 691 break;
Okamoto009 0:ca455457108f 692
Okamoto009 0:ca455457108f 693 case '1': //1段目強制分離
Okamoto009 0:ca455457108f 694 if(rocket_phase == ROCKET_FLIGHT){
Okamoto009 0:ca455457108f 695 rocket_phase = ROCKET_SEP1;
Okamoto009 0:ca455457108f 696 do_first = false;
Okamoto009 0:ca455457108f 697 }
Okamoto009 0:ca455457108f 698 else if(rocket_phase >= ROCKET_SEP1){
Okamoto009 0:ca455457108f 699 es920_using = true;
Okamoto009 0:ca455457108f 700 es920 << HEADER_START;
Okamoto009 0:ca455457108f 701 es920 << '1';
Okamoto009 0:ca455457108f 702 es920.send();
Okamoto009 0:ca455457108f 703
Okamoto009 0:ca455457108f 704 separate1();
Okamoto009 0:ca455457108f 705 }
Okamoto009 0:ca455457108f 706 break;
Okamoto009 0:ca455457108f 707
Okamoto009 0:ca455457108f 708 case '2': //2段目強制分離
Okamoto009 0:ca455457108f 709 if(rocket_phase == ROCKET_SEP1){
Okamoto009 0:ca455457108f 710 rocket_phase = ROCKET_SEP2;
Okamoto009 0:ca455457108f 711 do_first = false;
Okamoto009 0:ca455457108f 712 }
Okamoto009 0:ca455457108f 713 else if(rocket_phase >= ROCKET_SEP2){
Okamoto009 0:ca455457108f 714 es920_using = true;
Okamoto009 0:ca455457108f 715 es920 << HEADER_START;
Okamoto009 0:ca455457108f 716 es920 << '2';
Okamoto009 0:ca455457108f 717 es920.send();
Okamoto009 0:ca455457108f 718
Okamoto009 0:ca455457108f 719 separate2();
Okamoto009 0:ca455457108f 720 }
Okamoto009 0:ca455457108f 721 break;
Okamoto009 0:ca455457108f 722
Okamoto009 0:ca455457108f 723 case 'C':
Okamoto009 0:ca455457108f 724 if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && save){
Okamoto009 0:ca455457108f 725 stopRec();
Okamoto009 0:ca455457108f 726 }
Okamoto009 0:ca455457108f 727 break;
Okamoto009 0:ca455457108f 728
Okamoto009 0:ca455457108f 729 case 'O':
Okamoto009 0:ca455457108f 730 if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && !save){
Okamoto009 0:ca455457108f 731 startRec();
Okamoto009 0:ca455457108f 732 }
Okamoto009 0:ca455457108f 733 break;
Okamoto009 0:ca455457108f 734
Okamoto009 0:ca455457108f 735 case 'B':
Okamoto009 0:ca455457108f 736 buzzerChange();
Okamoto009 0:ca455457108f 737 break;
Okamoto009 0:ca455457108f 738 }
Okamoto009 0:ca455457108f 739 }
Okamoto009 0:ca455457108f 740
Okamoto009 0:ca455457108f 741
Okamoto009 0:ca455457108f 742 /*******************************************************************************
Okamoto009 0:ca455457108f 743 ログ記録開始
Okamoto009 0:ca455457108f 744 *******************************************************************************/
Okamoto009 0:ca455457108f 745 void startRec(){
Okamoto009 0:ca455457108f 746 fp = fopen(file_name, "a");
Okamoto009 0:ca455457108f 747 fprintf(fp, "Time,Flight Time,Phase,Flight Pin,Power,Separate1,Separate2,");
Okamoto009 0:ca455457108f 748 fprintf(fp, "upper100,under30,top,uplink,");
Okamoto009 0:ca455457108f 749 fprintf(fp, "GPS Y/N,GPS Time,Latitude,Longitude,GPS Alt,Knot,Deg,Sattelite,");
Okamoto009 0:ca455457108f 750 fprintf(fp, "ADXL_x[G],ADXL_y[G],ADXL_z[G],In_V[V],In_I[A],Ex_V[V],Ex_I[A],");
Okamoto009 0:ca455457108f 751 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],");
Okamoto009 0:ca455457108f 752 fprintf(fp, "Q_w,Q_x,Q_y,Q_z,Roll[deg],Pitch[deg],Yaw[deg],");
Okamoto009 0:ca455457108f 753 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],");
Okamoto009 0:ca455457108f 754 fprintf(fp, "Temp_35[degC],Humid_35[%],Lux[lux],Pitot_raw[V]\r\n");
Okamoto009 0:ca455457108f 755 save = true;
Okamoto009 0:ca455457108f 756 }
Okamoto009 0:ca455457108f 757
Okamoto009 0:ca455457108f 758
Okamoto009 0:ca455457108f 759 /*******************************************************************************
Okamoto009 0:ca455457108f 760 ログ記録終了
Okamoto009 0:ca455457108f 761 *******************************************************************************/
Okamoto009 0:ca455457108f 762 void stopRec(){
Okamoto009 0:ca455457108f 763 save = false;
Okamoto009 0:ca455457108f 764 fprintf(fp, "\r\n\n");
Okamoto009 0:ca455457108f 765 fclose(fp);
Okamoto009 0:ca455457108f 766 }
Okamoto009 0:ca455457108f 767
Okamoto009 0:ca455457108f 768
Okamoto009 0:ca455457108f 769 /*******************************************************************************
Okamoto009 0:ca455457108f 770 データ書き込み
Okamoto009 0:ca455457108f 771 *******************************************************************************/
Okamoto009 0:ca455457108f 772 void recData(){
Okamoto009 0:ca455457108f 773 if(save){
Okamoto009 0:ca455457108f 774 fprintf(fp, "%d:%02d.%03d,", time_min, time_sec, time_ms);
Okamoto009 0:ca455457108f 775 fprintf(fp, "%d:%02d.%03d,", flight_time_min, flight_time_sec, flight_time_ms);
Okamoto009 0:ca455457108f 776
Okamoto009 0:ca455457108f 777 switch(rocket_phase){
Okamoto009 0:ca455457108f 778 case ROCKET_SETUP:
Okamoto009 0:ca455457108f 779 fprintf(fp, "SETUP,");
Okamoto009 0:ca455457108f 780 break;
Okamoto009 0:ca455457108f 781 case ROCKET_SAFETY:
Okamoto009 0:ca455457108f 782 fprintf(fp, "SAFETY,");
Okamoto009 0:ca455457108f 783 break;
Okamoto009 0:ca455457108f 784 case ROCKET_WAIT:
Okamoto009 0:ca455457108f 785 fprintf(fp, "WAIT,");
Okamoto009 0:ca455457108f 786 break;
Okamoto009 0:ca455457108f 787 case ROCKET_FLIGHT:
Okamoto009 0:ca455457108f 788 fprintf(fp, "FLIGHT,");
Okamoto009 0:ca455457108f 789 break;
Okamoto009 0:ca455457108f 790 case ROCKET_SEP1:
Okamoto009 0:ca455457108f 791 fprintf(fp, "SEPARATE1,");
Okamoto009 0:ca455457108f 792 break;
Okamoto009 0:ca455457108f 793 case ROCKET_SEP2:
Okamoto009 0:ca455457108f 794 fprintf(fp, "SEPARATE2,");
Okamoto009 0:ca455457108f 795 break;
Okamoto009 0:ca455457108f 796 case ROCKET_RECOVERY:
Okamoto009 0:ca455457108f 797 fprintf(fp, "RECOVERY,");
Okamoto009 0:ca455457108f 798 break;
Okamoto009 0:ca455457108f 799 }
Okamoto009 0:ca455457108f 800
Okamoto009 0:ca455457108f 801 fprintf(fp, "%d,", flight_pin);
Okamoto009 0:ca455457108f 802
Okamoto009 0:ca455457108f 803 if(ex_power){
Okamoto009 0:ca455457108f 804 fprintf(fp, "External,");
Okamoto009 0:ca455457108f 805 }
Okamoto009 0:ca455457108f 806 else{
Okamoto009 0:ca455457108f 807 fprintf(fp, "Battery,");
Okamoto009 0:ca455457108f 808 }
Okamoto009 0:ca455457108f 809
Okamoto009 0:ca455457108f 810 fprintf(fp, "%d,%d,", Separate1.status, Separate2.status);
Okamoto009 0:ca455457108f 811 fprintf(fp, "%d,%d,%d,", ok_up, ok_down, ok_top);
Okamoto009 0:ca455457108f 812
Okamoto009 0:ca455457108f 813 fprintf(fp, "%c,", es920_uplink_command);
Okamoto009 0:ca455457108f 814 es920_uplink_command = '-';
Okamoto009 0:ca455457108f 815
Okamoto009 0:ca455457108f 816 fprintf(fp, "%d,", gps_yn);
Okamoto009 0:ca455457108f 817 fprintf(fp, "%d/%d/%d ", (int)gps_utc[0], (int)gps_utc[1], (int)gps_utc[2]); //日付
Okamoto009 0:ca455457108f 818 fprintf(fp, "%d:%02d:%02.2f,", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]); //時間
Okamoto009 0:ca455457108f 819 fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt); //GPS座標
Okamoto009 0:ca455457108f 820 fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg); //GPS速度
Okamoto009 0:ca455457108f 821 fprintf(fp, "%d,", gps_sat); //GPS衛星数
Okamoto009 0:ca455457108f 822
Okamoto009 0:ca455457108f 823 fprintf(fp, "%.2f,%.2f,%.2f,", adxl_acc[0], adxl_acc[1], adxl_acc[2]);
Okamoto009 0:ca455457108f 824 fprintf(fp, "%.4f,%.4f,%.4f,%.4f,", ina_in_v, ina_in_i, ina_ex_v, ina_ex_i);
Okamoto009 0:ca455457108f 825 fprintf(fp, "%f,%f,%f,", gyro[0], gyro[1], gyro[2]);
Okamoto009 0:ca455457108f 826 fprintf(fp, "%f,%f,%f,", acc[0], acc[1], acc[2]);
Okamoto009 0:ca455457108f 827 fprintf(fp, "%.2f,%.2f,%.2f,", mag[0], mag[1], mag[2]);
Okamoto009 0:ca455457108f 828 fprintf(fp, "%f,%f,%f,%f,", quart.w, quart.x, quart.y, quart.z);
Okamoto009 0:ca455457108f 829 fprintf(fp, "%f,%f,%f,", euler[0], euler[1], euler[2]);
Okamoto009 0:ca455457108f 830 fprintf(fp, "%f,%.2f,%.2f,", pres22, temp22, alt22);
Okamoto009 0:ca455457108f 831 fprintf(fp, "%f,%.2f,%.2f,%.2f,%.2f,%2f,", pres33, temp33, alt33, alt33_ave, speed33, speed33_ave);
Okamoto009 0:ca455457108f 832 fprintf(fp, "%.2f,%.2f,", temp35, hum35);
Okamoto009 0:ca455457108f 833 fprintf(fp, "%d,", lux);
Okamoto009 0:ca455457108f 834 //fprintf(fp, "%f,%f,%f\r\n", pitot_raw, pitot, pitot_speed);
Okamoto009 0:ca455457108f 835
Okamoto009 0:ca455457108f 836 save_c ++;
Okamoto009 0:ca455457108f 837 if(save_c > 10000){
Okamoto009 0:ca455457108f 838 fclose(fp);
Okamoto009 0:ca455457108f 839 fp = fopen(file_name, "a");
Okamoto009 0:ca455457108f 840 save_c = 0;
Okamoto009 0:ca455457108f 841 }
Okamoto009 0:ca455457108f 842 }
Okamoto009 0:ca455457108f 843 }
Okamoto009 0:ca455457108f 844
Okamoto009 0:ca455457108f 845
Okamoto009 0:ca455457108f 846 /*******************************************************************************
Okamoto009 0:ca455457108f 847 状態遷移の判断
Okamoto009 0:ca455457108f 848 *******************************************************************************/
Okamoto009 0:ca455457108f 849 void modeChange(){
Okamoto009 0:ca455457108f 850 //無線あり
Okamoto009 0:ca455457108f 851 switch(rocket_phase){
Okamoto009 0:ca455457108f 852 case ROCKET_SAFETY://///////////////安全モード
Okamoto009 0:ca455457108f 853 if(!do_first){
Okamoto009 0:ca455457108f 854 es920_using = true;
Okamoto009 0:ca455457108f 855 es920 << HEADER_START;
Okamoto009 0:ca455457108f 856 es920 << 'S';
Okamoto009 0:ca455457108f 857 es920.send();
Okamoto009 0:ca455457108f 858
Okamoto009 0:ca455457108f 859 do_first = true;
Okamoto009 0:ca455457108f 860 }
Okamoto009 0:ca455457108f 861 break;
Okamoto009 0:ca455457108f 862
Okamoto009 0:ca455457108f 863 case ROCKET_WAIT://////////////////離床検知モード
Okamoto009 0:ca455457108f 864 if(!do_first){
Okamoto009 0:ca455457108f 865 es920_using = true;
Okamoto009 0:ca455457108f 866 es920 << HEADER_START;
Okamoto009 0:ca455457108f 867 es920 << 'W';
Okamoto009 0:ca455457108f 868 es920.send();
Okamoto009 0:ca455457108f 869
Okamoto009 0:ca455457108f 870
Okamoto009 0:ca455457108f 871 if(!save){
Okamoto009 0:ca455457108f 872 startRec();
Okamoto009 0:ca455457108f 873 }
Okamoto009 0:ca455457108f 874
Okamoto009 0:ca455457108f 875 do_first = true;
Okamoto009 0:ca455457108f 876 }
Okamoto009 0:ca455457108f 877 if(flight_pin){
Okamoto009 0:ca455457108f 878 rocket_phase = ROCKET_FLIGHT;
Okamoto009 0:ca455457108f 879 do_first = false;
Okamoto009 0:ca455457108f 880 }
Okamoto009 0:ca455457108f 881 break;
Okamoto009 0:ca455457108f 882
Okamoto009 0:ca455457108f 883 case ROCKET_FLIGHT:///////////////フライトモード
Okamoto009 0:ca455457108f 884 if(!do_first){
Okamoto009 0:ca455457108f 885 es920_using = true;
Okamoto009 0:ca455457108f 886 es920 << HEADER_START;
Okamoto009 0:ca455457108f 887 es920 << 'F';
Okamoto009 0:ca455457108f 888 es920.send();
Okamoto009 0:ca455457108f 889
Okamoto009 0:ca455457108f 890 if(!save){
Okamoto009 0:ca455457108f 891 startRec();
Okamoto009 0:ca455457108f 892 }
Okamoto009 0:ca455457108f 893
Okamoto009 0:ca455457108f 894 time_flight.reset();
Okamoto009 0:ca455457108f 895 time_flight.start();
Okamoto009 0:ca455457108f 896
Okamoto009 0:ca455457108f 897 timeout_sep.attach(&changePhase_SEP1, TIME_SEP1);
Okamoto009 0:ca455457108f 898 timeout_recovery.attach(&changePhase_RECOVERY, TIME_RECOVERY);
Okamoto009 0:ca455457108f 899
Okamoto009 0:ca455457108f 900 do_first = true;
Okamoto009 0:ca455457108f 901 }
Okamoto009 0:ca455457108f 902 if(alt33_ave > SEP1_ALT_UNDER && speed33_ave < SEP1_SPEED){
Okamoto009 0:ca455457108f 903 ok_top = true;
Okamoto009 0:ca455457108f 904
Okamoto009 0:ca455457108f 905 rocket_phase = ROCKET_SEP1;
Okamoto009 0:ca455457108f 906 do_first = false;
Okamoto009 0:ca455457108f 907 }
Okamoto009 0:ca455457108f 908 break;
Okamoto009 0:ca455457108f 909
Okamoto009 0:ca455457108f 910 case ROCKET_SEP1:////////////////分離1モード
Okamoto009 0:ca455457108f 911 if(!do_first){
Okamoto009 0:ca455457108f 912 es920_using = true;
Okamoto009 0:ca455457108f 913 es920 << HEADER_START;
Okamoto009 0:ca455457108f 914 es920 << '1';
Okamoto009 0:ca455457108f 915 es920.send();
Okamoto009 0:ca455457108f 916
Okamoto009 0:ca455457108f 917 timeout_sep.detach();
Okamoto009 0:ca455457108f 918
Okamoto009 0:ca455457108f 919 stopRec();
Okamoto009 0:ca455457108f 920
Okamoto009 0:ca455457108f 921 startRec();
Okamoto009 0:ca455457108f 922
Okamoto009 0:ca455457108f 923 separate1();
Okamoto009 0:ca455457108f 924
Okamoto009 0:ca455457108f 925 timeout_sep.attach(&changePhase_SEP2, TIME_SEP2 - TIME_SEP1);
Okamoto009 0:ca455457108f 926
Okamoto009 0:ca455457108f 927 do_first = true;
Okamoto009 0:ca455457108f 928 }
Okamoto009 0:ca455457108f 929 if(alt33_ave < SEP2_ALT_UP && speed33_ave < 0.0f && ok_up){
Okamoto009 0:ca455457108f 930 rocket_phase = ROCKET_SEP2;
Okamoto009 0:ca455457108f 931 do_first = false;
Okamoto009 0:ca455457108f 932 }
Okamoto009 0:ca455457108f 933 break;
Okamoto009 0:ca455457108f 934
Okamoto009 0:ca455457108f 935 case ROCKET_SEP2:////////////////分離2モード
Okamoto009 0:ca455457108f 936 if(!do_first){
Okamoto009 0:ca455457108f 937 es920_using = true;
Okamoto009 0:ca455457108f 938 es920 << HEADER_START;
Okamoto009 0:ca455457108f 939 es920 << '2';
Okamoto009 0:ca455457108f 940 es920.send();
Okamoto009 0:ca455457108f 941
Okamoto009 0:ca455457108f 942 timeout_sep.detach();
Okamoto009 0:ca455457108f 943
Okamoto009 0:ca455457108f 944 separate2();
Okamoto009 0:ca455457108f 945
Okamoto009 0:ca455457108f 946 do_first = true;
Okamoto009 0:ca455457108f 947 }
Okamoto009 0:ca455457108f 948 break;
Okamoto009 0:ca455457108f 949
Okamoto009 0:ca455457108f 950 case ROCKET_RECOVERY:////////////回収モード
Okamoto009 0:ca455457108f 951 if(!do_first){
Okamoto009 0:ca455457108f 952 stopRec();
Okamoto009 0:ca455457108f 953 startRec();
Okamoto009 0:ca455457108f 954
Okamoto009 0:ca455457108f 955 Buzzer = 1;
Okamoto009 0:ca455457108f 956
Okamoto009 0:ca455457108f 957 do_first = true;
Okamoto009 0:ca455457108f 958 }
Okamoto009 0:ca455457108f 959 break;
Okamoto009 0:ca455457108f 960 }
Okamoto009 0:ca455457108f 961
Okamoto009 0:ca455457108f 962 if(rocket_phase >= ROCKET_FLIGHT){//////////////////フライトモード以降共通
Okamoto009 0:ca455457108f 963 if(alt33_ave > SEP1_ALT_UNDER && speed33_ave < SEP1_SPEED){
Okamoto009 0:ca455457108f 964 ok_top = true;
Okamoto009 0:ca455457108f 965 }
Okamoto009 0:ca455457108f 966
Okamoto009 0:ca455457108f 967 if(!ok_up && alt33_ave > OK_UP_ALT){
Okamoto009 0:ca455457108f 968 ok_up = true;
Okamoto009 0:ca455457108f 969 }
Okamoto009 0:ca455457108f 970 if(ok_up && !ok_down && alt33_ave < OK_DOWN_ALT){
Okamoto009 0:ca455457108f 971 ok_down = true;
Okamoto009 0:ca455457108f 972 }
Okamoto009 0:ca455457108f 973 if(ok_up && ok_down && speed33_ave > STOP_SPEED_UNDER && speed33_ave < STOP_SPEED_UP){
Okamoto009 0:ca455457108f 974 changePhase_RECOVERY();
Okamoto009 0:ca455457108f 975 }
Okamoto009 0:ca455457108f 976 }
Okamoto009 0:ca455457108f 977 }
Okamoto009 0:ca455457108f 978
Okamoto009 0:ca455457108f 979 void changePhase_SEP1(){/////////////分離1モードへ
Okamoto009 0:ca455457108f 980 rocket_phase = ROCKET_SEP1;
Okamoto009 0:ca455457108f 981 do_first = false;
Okamoto009 0:ca455457108f 982 }
Okamoto009 0:ca455457108f 983
Okamoto009 0:ca455457108f 984 void changePhase_SEP2(){/////////////分離2モードへ
Okamoto009 0:ca455457108f 985 rocket_phase = ROCKET_SEP2;
Okamoto009 0:ca455457108f 986 do_first = false;
Okamoto009 0:ca455457108f 987 }
Okamoto009 0:ca455457108f 988
Okamoto009 0:ca455457108f 989 void changePhase_RECOVERY(){/////////回収モードへ
Okamoto009 0:ca455457108f 990 rocket_phase = ROCKET_RECOVERY;
Okamoto009 0:ca455457108f 991 do_first = false;
Okamoto009 0:ca455457108f 992 }
Okamoto009 0:ca455457108f 993
Okamoto009 0:ca455457108f 994
Okamoto009 0:ca455457108f 995 /*******************************************************************************
Okamoto009 0:ca455457108f 996 ブザーのON/OFF
Okamoto009 0:ca455457108f 997 *******************************************************************************/
Okamoto009 0:ca455457108f 998 void buzzerChange(){
Okamoto009 0:ca455457108f 999 if(Buzzer){
Okamoto009 0:ca455457108f 1000 Buzzer = 0;
Okamoto009 0:ca455457108f 1001 }
Okamoto009 0:ca455457108f 1002 else{
Okamoto009 0:ca455457108f 1003 Buzzer = 1;
Okamoto009 0:ca455457108f 1004 }
Okamoto009 0:ca455457108f 1005 }
Okamoto009 0:ca455457108f 1006
Okamoto009 0:ca455457108f 1007
Okamoto009 0:ca455457108f 1008 /*******************************************************************************
Okamoto009 0:ca455457108f 1009 分離機構1段目作動
Okamoto009 0:ca455457108f 1010 *******************************************************************************/
Okamoto009 0:ca455457108f 1011 void separate1(){
Okamoto009 0:ca455457108f 1012 if(!Separate1.status && rocket_phase >= ROCKET_FLIGHT){
Okamoto009 0:ca455457108f 1013 Separate1.fire(6.0f);
Okamoto009 0:ca455457108f 1014 pc.printf("1st Separation\r\n");
Okamoto009 0:ca455457108f 1015 }
Okamoto009 0:ca455457108f 1016 }
Okamoto009 0:ca455457108f 1017
Okamoto009 0:ca455457108f 1018
Okamoto009 0:ca455457108f 1019 /*******************************************************************************
Okamoto009 0:ca455457108f 1020 分離機構2段目作動
Okamoto009 0:ca455457108f 1021 *******************************************************************************/
Okamoto009 0:ca455457108f 1022 void separate2(){
Okamoto009 0:ca455457108f 1023 if(!Separate2.status && rocket_phase >= ROCKET_FLIGHT){
Okamoto009 0:ca455457108f 1024 Separate2.fire(5.0f);
Okamoto009 0:ca455457108f 1025 pc.printf("2nd Separation\r\n");
Okamoto009 0:ca455457108f 1026 }
Okamoto009 0:ca455457108f 1027 }
Okamoto009 0:ca455457108f 1028
Okamoto009 0:ca455457108f 1029
Okamoto009 0:ca455457108f 1030 /*******************************************************************************
Okamoto009 0:ca455457108f 1031 セットアップ（最初に1回実行）
Okamoto009 0:ca455457108f 1032 *******************************************************************************/
Okamoto009 0:ca455457108f 1033 //無線あり(HEADER_SETUP)
Okamoto009 0:ca455457108f 1034 void setup(){
Okamoto009 0:ca455457108f 1035 wait(1.0f);
Okamoto009 0:ca455457108f 1036 char setup_string[1024];
Okamoto009 0:ca455457108f 1037
Okamoto009 0:ca455457108f 1038 pc.printf("\r\n**************************************************\r\n");
Okamoto009 0:ca455457108f 1039 pc.printf("Sensor Setting Start!!\r\n");
Okamoto009 0:ca455457108f 1040 strcat(setup_string, "Sensor Setting Start!!\r\n");
Okamoto009 0:ca455457108f 1041 es920 << HEADER_SENSOR_SETUP;
Okamoto009 0:ca455457108f 1042
Okamoto009 0:ca455457108f 1043 ///////////////////////////////////////////ADXL375
Okamoto009 0:ca455457108f 1044 ADXL375.setDataRate(ADXL375_100HZ);
Okamoto009 0:ca455457108f 1045 if(ADXL375.whoAmI() == 1){
Okamoto009 0:ca455457108f 1046 pc.printf("ADXL375 : OK\r\n");
Okamoto009 0:ca455457108f 1047 strcat(setup_string, "ADXL375 : OK!!\r\n");
Okamoto009 0:ca455457108f 1048 es920 << (char)0x01;
Okamoto009 0:ca455457108f 1049 }
Okamoto009 0:ca455457108f 1050 else{
Okamoto009 0:ca455457108f 1051 pc.printf("ADXL375 : NG...\r\n");
Okamoto009 0:ca455457108f 1052 strcat(setup_string, "ADXL375 : NG...\r\n");
Okamoto009 0:ca455457108f 1053 es920 << (char)0x00;
Okamoto009 0:ca455457108f 1054 }
Okamoto009 0:ca455457108f 1055 ADXL375.offset(0.0f, 0.0f, 0.0f);
Okamoto009 0:ca455457108f 1056
Okamoto009 0:ca455457108f 1057 ///////////////////////////////////////////INA226_in
Okamoto009 0:ca455457108f 1058 INA226_in.set_callibretion();
Okamoto009 0:ca455457108f 1059 if(INA226_in.Connection_check() == 0){
Okamoto009 0:ca455457108f 1060 pc.printf("INA226_in : OK!!\r\n");
Okamoto009 0:ca455457108f 1061 strcat(setup_string, "INA_in : OK!!\r\n");
Okamoto009 0:ca455457108f 1062 es920 << (char)0x01;
Okamoto009 0:ca455457108f 1063 }
Okamoto009 0:ca455457108f 1064 else{
Okamoto009 0:ca455457108f 1065 pc.printf("INA226_in : NG...\r\n");
Okamoto009 0:ca455457108f 1066 strcat(setup_string, "INA_in : NG...\r\n");
Okamoto009 0:ca455457108f 1067 es920 << (char)0x00;
Okamoto009 0:ca455457108f 1068 }
Okamoto009 0:ca455457108f 1069
Okamoto009 0:ca455457108f 1070 ///////////////////////////////////////////INA226_ex
Okamoto009 0:ca455457108f 1071 INA226_ex.set_callibretion();
Okamoto009 0:ca455457108f 1072 if(INA226_ex.Connection_check() == 0){
Okamoto009 0:ca455457108f 1073 pc.printf("INA226_ex OK!!\r\n");
Okamoto009 0:ca455457108f 1074 strcat(setup_string, "INA_ex : OK!!\r\n");
Okamoto009 0:ca455457108f 1075 es920 << (char)0x01;
Okamoto009 0:ca455457108f 1076 }
Okamoto009 0:ca455457108f 1077 else{
Okamoto009 0:ca455457108f 1078 pc.printf("INA226_ex NG...\r\n");
Okamoto009 0:ca455457108f 1079 strcat(setup_string, "INA_ex : NG...\r\n");
Okamoto009 0:ca455457108f 1080 es920 << (char)0x00;
Okamoto009 0:ca455457108f 1081 }
Okamoto009 0:ca455457108f 1082
Okamoto009 0:ca455457108f 1083 ///////////////////////////////////////////MPU9250
Okamoto009 0:ca455457108f 1084 nine.setAcc(_16G);
Okamoto009 0:ca455457108f 1085 nine.setGyro(_2000DPS);
Okamoto009 0:ca455457108f 1086 nine.setOffset(0.58424, 0.622428, 0.063746,
Okamoto009 0:ca455457108f 1087 0.008125, -0.00106, 0.0024145,
Okamoto009 0:ca455457108f 1088 -2.25, 3.825, -24.75);
Okamoto009 0:ca455457108f 1089
Okamoto009 0:ca455457108f 1090 if(nine.senserTest()){
Okamoto009 0:ca455457108f 1091 pc.printf("MPU9250 : OK!!\r\n");
Okamoto009 0:ca455457108f 1092 strcat(setup_string, "MPU9250 : OK!!\r\n");
Okamoto009 0:ca455457108f 1093 es920 << (char)0x01;
Okamoto009 0:ca455457108f 1094 }
Okamoto009 0:ca455457108f 1095 else{
Okamoto009 0:ca455457108f 1096 pc.printf("MPU9250 : NG...\r\n");
Okamoto009 0:ca455457108f 1097 strcat(setup_string, "MPU9250 : NG...\r\n");
Okamoto009 0:ca455457108f 1098 es920 << (char)0x00;
Okamoto009 0:ca455457108f 1099 }
Okamoto009 0:ca455457108f 1100 if(nine.mag_senserTest()){
Okamoto009 0:ca455457108f 1101 pc.printf("MPU9250 MAG : OK!!\r\n");
Okamoto009 0:ca455457108f 1102 strcat(setup_string, "MPU9250 MAG : OK!!\r\n");
Okamoto009 0:ca455457108f 1103 es920 << (char)0x01;
Okamoto009 0:ca455457108f 1104 }
Okamoto009 0:ca455457108f 1105 else{
Okamoto009 0:ca455457108f 1106 pc.printf("MPU9250 MAG : NG...\r\n");
Okamoto009 0:ca455457108f 1107 strcat(setup_string, "MPU9250 MAG : NG...\r\n");
Okamoto009 0:ca455457108f 1108 es920 << (char)0x00;
Okamoto009 0:ca455457108f 1109 }
Okamoto009 0:ca455457108f 1110
Okamoto009 0:ca455457108f 1111 ///////////////////////////////////////////LPS22HB
Okamoto009 0:ca455457108f 1112 LPS22HB.begin(50);
Okamoto009 0:ca455457108f 1113 if(LPS22HB.whoAmI() == 0){
Okamoto009 0:ca455457108f 1114 pc.printf("LPS22HB : OK!!\r\n");
Okamoto009 0:ca455457108f 1115 strcat(setup_string, "LPS22HB : OK!!\r\n");
Okamoto009 0:ca455457108f 1116 es920 << (char)0x01;
Okamoto009 0:ca455457108f 1117 }
Okamoto009 0:ca455457108f 1118 else{
Okamoto009 0:ca455457108f 1119 pc.printf("LPS22HB : NG...\r\n");
Okamoto009 0:ca455457108f 1120 strcat(setup_string, "LPS22HB : NG...\r\n");
Okamoto009 0:ca455457108f 1121 es920 << (char)0x00;
Okamoto009 0:ca455457108f 1122 }
Okamoto009 0:ca455457108f 1123
Okamoto009 0:ca455457108f 1124 ///////////////////////////////////////////LPS33HW
Okamoto009 0:ca455457108f 1125 LPS33HW.begin(50);
Okamoto009 0:ca455457108f 1126 if(LPS33HW.whoAmI() == 0){
Okamoto009 0:ca455457108f 1127 pc.printf("LPS33HW : OK!!\r\n");
Okamoto009 0:ca455457108f 1128 strcat(setup_string, "LPS33HW : OK!!\r\n");
Okamoto009 0:ca455457108f 1129 es920 << (char)0x01;
Okamoto009 0:ca455457108f 1130 }
Okamoto009 0:ca455457108f 1131 else{
Okamoto009 0:ca455457108f 1132 pc.printf("LPS33HW : NG...\r\n");
Okamoto009 0:ca455457108f 1133 strcat(setup_string, "LPS33HW : NG...\r\n");
Okamoto009 0:ca455457108f 1134 es920 << (char)0x00;
Okamoto009 0:ca455457108f 1135 }
Okamoto009 0:ca455457108f 1136
Okamoto009 0:ca455457108f 1137 ///////////////////////////////////////////SHT35
Okamoto009 0:ca455457108f 1138 int sht_state = SHT35.getState();
Okamoto009 0:ca455457108f 1139 if(sht_state == 32784 || sht_state == 32848){
Okamoto009 0:ca455457108f 1140 pc.printf("SHT35 : OK!!\r\n");
Okamoto009 0:ca455457108f 1141 strcat(setup_string, "SHT35 : OK!!\r\n");
Okamoto009 0:ca455457108f 1142 es920 << (char)0x01;
Okamoto009 0:ca455457108f 1143 }
Okamoto009 0:ca455457108f 1144 else{
Okamoto009 0:ca455457108f 1145 pc.printf("SHT35 : NG...\r\n");
Okamoto009 0:ca455457108f 1146 strcat(setup_string, "SHT35 : NG...\r\n");
Okamoto009 0:ca455457108f 1147 es920 << (char)0x00;
Okamoto009 0:ca455457108f 1148 }
Okamoto009 0:ca455457108f 1149
Okamoto009 0:ca455457108f 1150 ///////////////////////////////////////////TSL2561
Okamoto009 0:ca455457108f 1151 TSL2561.begin();
Okamoto009 0:ca455457108f 1152 //lux_time = TSL2561.setRate(1);
Okamoto009 0:ca455457108f 1153 if(TSL2561.connectCheck() == 1){
Okamoto009 0:ca455457108f 1154 pc.printf("TSL2561 : OK!!\r\n");
Okamoto009 0:ca455457108f 1155 strcat(setup_string, "TSL2561 : OK!!\r\n");
Okamoto009 0:ca455457108f 1156 es920 << (char)0x01;
Okamoto009 0:ca455457108f 1157 }
Okamoto009 0:ca455457108f 1158 else{
Okamoto009 0:ca455457108f 1159 pc.printf("SL2561 : NG...\r\n");
Okamoto009 0:ca455457108f 1160 strcat(setup_string, "TSL2561 : NG...\r\n");
Okamoto009 0:ca455457108f 1161 es920 << (char)0x00;
Okamoto009 0:ca455457108f 1162 }
Okamoto009 0:ca455457108f 1163
Okamoto009 0:ca455457108f 1164 ///////////////////////////////////////////SD
Okamoto009 0:ca455457108f 1165 fp = fopen("/sd/Felix_Luminous_setup.txt", "r");
Okamoto009 0:ca455457108f 1166 if(fp != NULL){
Okamoto009 0:ca455457108f 1167 pc.printf("SD : OK!!\r\n");
Okamoto009 0:ca455457108f 1168 strcat(setup_string, "SD : OK!!\r\n");
Okamoto009 0:ca455457108f 1169 es920 << (char)0x01;
Okamoto009 0:ca455457108f 1170 fclose(fp);
Okamoto009 0:ca455457108f 1171 }
Okamoto009 0:ca455457108f 1172 else{
Okamoto009 0:ca455457108f 1173 pc.printf("SD : NG...\r\n");
Okamoto009 0:ca455457108f 1174 strcat(setup_string, "SD : NG...\r\n");
Okamoto009 0:ca455457108f 1175 es920 << (char)0x00;
Okamoto009 0:ca455457108f 1176 }
Okamoto009 0:ca455457108f 1177
Okamoto009 0:ca455457108f 1178 pc.printf("Sensor Setting Finished!!\r\n");
Okamoto009 0:ca455457108f 1179 pc.printf("**************************************************\r\n");
Okamoto009 0:ca455457108f 1180 strcat(setup_string, "Sensor Setting Finished!!\r\n");
Okamoto009 0:ca455457108f 1181 es920.send();
Okamoto009 0:ca455457108f 1182 wait(1.0f);
Okamoto009 0:ca455457108f 1183
Okamoto009 0:ca455457108f 1184 pc.printf("\r\n**************************************************\r\n");
Okamoto009 0:ca455457108f 1185 if(wait_GPS){
Okamoto009 0:ca455457108f 1186 pc.printf("GPS Setting Start!!\r\n");
Okamoto009 0:ca455457108f 1187 strcat(setup_string, "GPS Setting Start!!\r\n");
Okamoto009 0:ca455457108f 1188
Okamoto009 0:ca455457108f 1189 pc.printf("Waiting : 0");
Okamoto009 0:ca455457108f 1190 strcat(setup_string, "Wait : ");
Okamoto009 0:ca455457108f 1191 int gps_wait_count = 0;
Okamoto009 0:ca455457108f 1192 while(!GPS.gps_readable){
Okamoto009 0:ca455457108f 1193 pc.printf("\rWaiting : %d", gps_wait_count);//
Okamoto009 0:ca455457108f 1194 es920 << HEADER_GPS_SETUP;
Okamoto009 0:ca455457108f 1195 es920 << (char)GPS.gps_readable;
Okamoto009 0:ca455457108f 1196 es920 << (int)gps_wait_count;
Okamoto009 0:ca455457108f 1197 es920.send();
Okamoto009 0:ca455457108f 1198 wait(1.0f);
Okamoto009 0:ca455457108f 1199 gps_wait_count ++;
Okamoto009 0:ca455457108f 1200 }
Okamoto009 0:ca455457108f 1201 char ss[8];
Okamoto009 0:ca455457108f 1202 sscanf(ss, "%d", &gps_wait_count);
Okamoto009 0:ca455457108f 1203 strcat(setup_string, ss);
Okamoto009 0:ca455457108f 1204 pc.printf(" OK!!\r\n");
Okamoto009 0:ca455457108f 1205 strcat(setup_string, " OK!!\r\n");
Okamoto009 0:ca455457108f 1206 pc.printf("GPS Setting Finished!!\r\n");
Okamoto009 0:ca455457108f 1207 strcat(setup_string, "GPS Setting Finished!!\r\n");
Okamoto009 0:ca455457108f 1208 es920 << HEADER_GPS_SETUP;
Okamoto009 0:ca455457108f 1209 es920 << (char)0xAA;
Okamoto009 0:ca455457108f 1210 es920 << (int)0;
Okamoto009 0:ca455457108f 1211 es920.send();
Okamoto009 0:ca455457108f 1212 }
Okamoto009 0:ca455457108f 1213 else{
Okamoto009 0:ca455457108f 1214 pc.printf("GPS Setting Ignore...\r\n");
Okamoto009 0:ca455457108f 1215 strcat(setup_string, "GPS Setting Ignore...\r\n");
Okamoto009 0:ca455457108f 1216 es920 << HEADER_GPS_SETUP;
Okamoto009 0:ca455457108f 1217 es920 << (char)0xFF;
Okamoto009 0:ca455457108f 1218 es920 << (int)0;
Okamoto009 0:ca455457108f 1219 es920.send();
Okamoto009 0:ca455457108f 1220 }
Okamoto009 0:ca455457108f 1221 pc.printf("\r\n**************************************************\r\n");
Okamoto009 0:ca455457108f 1222 fp = fopen(file_name, "a");
Okamoto009 0:ca455457108f 1223 fprintf(fp, setup_string);
Okamoto009 0:ca455457108f 1224 fclose(fp);
Okamoto009 0:ca455457108f 1225 setup_string[0] = NULL;
Okamoto009 0:ca455457108f 1226
Okamoto009 0:ca455457108f 1227 printHelp();
Okamoto009 0:ca455457108f 1228 wait(1.0f);
Okamoto009 0:ca455457108f 1229 }