PQ_Hybrid_Electrical_Equipment_Team / Mbed 2 deprecated Hybrid_IZU2021_MAIN

2021年3月伊豆大島共同打上実験の電装メインプログラム

Dependencies:   PQ_ES920 mbed PQ_LPS22HB SDFileSystem PQ_EEPROM PQ_ADXL375 PQ_MPU9250 PQ_INA226 PQ_GPS

main.cpp@6:d951f75ce5e8, 2021-03-13 (annotated)

Committer:
tanahashi
Date:
Sat Mar 13 12:24:45 2021 +0000
Revision:
6:d951f75ce5e8
Parent:
5:dc80ccf0904e 
version 1.3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tanahashi 6:d951f75ce5e8 1 /*
tanahashi 5:dc80ccf0904e 2 * ハイブリッドロケット（2021年3月伊豆大島共同打上実験）アビオニクスメインモジュール
tanahashi 0:e8c9241fe4cb 3 */
tanahashi 0:e8c9241fe4cb 4
tanahashi 0:e8c9241fe4cb 5 #include "mbed.h"
tanahashi 0:e8c9241fe4cb 6 #include "SDFileSystem.h"
tanahashi 0:e8c9241fe4cb 7
tanahashi 0:e8c9241fe4cb 8 #include "PQ_ADXL375.h"
tanahashi 0:e8c9241fe4cb 9 #include "PQ_GPS.h"
tanahashi 0:e8c9241fe4cb 10 #include "PQ_EEPROM.h"
tanahashi 0:e8c9241fe4cb 11 #include "PQ_ES920.h"
tanahashi 0:e8c9241fe4cb 12 #include "PQ_INA226.h"
tanahashi 0:e8c9241fe4cb 13 #include "PQ_LPS22HB.h"
tanahashi 0:e8c9241fe4cb 14 #include "PQ_MPU9250.h"
tanahashi 0:e8c9241fe4cb 15
tanahashi 5:dc80ccf0904e 16 const float BURN_TIME = 3.27f; // エンジン燃焼時間[s]
tanahashi 6:d951f75ce5e8 17 const float T_APOGEE = 13.44f; // 離床検知から弾道頂点までの時間[s]
tanahashi 5:dc80ccf0904e 18 const float T_SEP = 12.0f; // 分離にかける時間[s]
tanahashi 5:dc80ccf0904e 19 const float SAMPLING_FREQUENCY = 10.0f; // データのサンプリングレート[Hz]
tanahashi 5:dc80ccf0904e 20 const float DOWNLINK_RATE = 2.0f; // ダウンリンクのレート[Hz]
tanahashi 0:e8c9241fe4cb 21
tanahashi 0:e8c9241fe4cb 22 Serial pc(USBTX, USBRX, 115200);
tanahashi 0:e8c9241fe4cb 23 Serial gps_serial(p9, p10, 115200);
tanahashi 0:e8c9241fe4cb 24 Serial es_serial(p13, p14, 115200);
tanahashi 0:e8c9241fe4cb 25
tanahashi 0:e8c9241fe4cb 26 I2C i2c(p28, p27);
tanahashi 0:e8c9241fe4cb 27
tanahashi 0:e8c9241fe4cb 28 ES920 es(es_serial);
tanahashi 0:e8c9241fe4cb 29 GPS gps(gps_serial);
tanahashi 0:e8c9241fe4cb 30
tanahashi 5:dc80ccf0904e 31 ADXL375 adxl(i2c, ADXL375::ALT_ADDRESS_HIGH); // 3軸高加速度
tanahashi 5:dc80ccf0904e 32 INA226 ina_in(i2c, INA226::A0_VS, INA226::A1_VS); // 電圧，電流（バッテリー）
tanahashi 5:dc80ccf0904e 33 INA226 ina_ex(i2c, INA226::A0_GND, INA226::A1_GND); // 電圧，電流（外部電源）
tanahashi 6:d951f75ce5e8 34 //消す！！
tanahashi 6:d951f75ce5e8 35 LPS22HB lps(i2c, LPS22HB::SA0_LOW); // 気圧，温度
tanahashi 6:d951f75ce5e8 36 //LPS22HB lps(i2c, LPS22HB::SA0_HIGH); // 気圧，温度
tanahashi 5:dc80ccf0904e 37 MPU9250 mpu(i2c, MPU9250::AD0_HIGH); // 3軸加速度，3軸角速度，3軸地磁気
tanahashi 0:e8c9241fe4cb 38
tanahashi 5:dc80ccf0904e 39 SDFileSystem sd(p5, p6, p7, p8, "sd");
tanahashi 5:dc80ccf0904e 40 EEPROM eeprom(i2c); // 24FC1025*4（0x000000~0x07FFFF）
tanahashi 0:e8c9241fe4cb 41
tanahashi 0:e8c9241fe4cb 42 Timer mission_timer; // 電源投入からの時間
tanahashi 2:caadd357a486 43 Timer flight_timer; // 離床検知からの時間
tanahashi 5:dc80ccf0904e 44 Timer sd_timer; // SD用（fcloseのため）
tanahashi 5:dc80ccf0904e 45 Timer sep_timer; // 分離時間
tanahashi 0:e8c9241fe4cb 46
tanahashi 2:caadd357a486 47 Ticker downlink_ticker; // ダウンリンク
tanahashi 2:caadd357a486 48 Ticker record_ticker; // データ保存
tanahashi 0:e8c9241fe4cb 49
tanahashi 5:dc80ccf0904e 50 DigitalIn flight(p20); // 離床検知用フライトピン（外部プルアップ）
tanahashi 6:d951f75ce5e8 51 DigitalIn signal(p24); // ミッション基板の生存信号
tanahashi 0:e8c9241fe4cb 52
tanahashi 2:caadd357a486 53 DigitalOut mission(p18); // ミッション基板の動作制御
tanahashi 2:caadd357a486 54 DigitalOut relay(p19); // ミッション基板のリレースイッチング
tanahashi 2:caadd357a486 55 DigitalOut sep(p21); // 分離機構のニクロム線制御
tanahashi 2:caadd357a486 56 DigitalOut buzzer(p23); // 捜索用ブザー
tanahashi 0:e8c9241fe4cb 57
tanahashi 5:dc80ccf0904e 58 void init(); // 初期化処理
tanahashi 5:dc80ccf0904e 59 void read(); // タイマー，GPS，センサの読み取り
tanahashi 5:dc80ccf0904e 60 void command_handler(char *command); // アップリンクコマンドの処理
tanahashi 5:dc80ccf0904e 61 void downlink(); // データのダウンリンク
tanahashi 5:dc80ccf0904e 62 void record(); // EEPROM，SDへの記録
tanahashi 0:e8c9241fe4cb 63
tanahashi 5:dc80ccf0904e 64 char file_name[64];
tanahashi 2:caadd357a486 65 FILE *fp;
tanahashi 0:e8c9241fe4cb 66
tanahashi 5:dc80ccf0904e 67 bool launched = false; // 打ちあがっているか：true=離床済み
tanahashi 5:dc80ccf0904e 68 bool burning = false; // エンジンが燃焼中か：true=燃焼中
tanahashi 6:d951f75ce5e8 69 bool use_apogee = true; // 頂点検知を行うか：true=行う
tanahashi 5:dc80ccf0904e 70 bool apogee = false; // 頂点検知したか：true=頂点検知済み
tanahashi 5:dc80ccf0904e 71 bool separated = false; // 分離したか：true=分離済み
tanahashi 5:dc80ccf0904e 72 bool landed = false; // 着水したか：true=着水済み
tanahashi 5:dc80ccf0904e 73 float t = 0; // 頂点検知用
tanahashi 0:e8c9241fe4cb 74
tanahashi 0:e8c9241fe4cb 75 int addr; // EEPROMの書き込みアドレス
tanahashi 0:e8c9241fe4cb 76
tanahashi 2:caadd357a486 77 int mission_timer_reset; // mission_timerをリセットした回数
tanahashi 5:dc80ccf0904e 78 float mission_time; // ミッション開始からの時間
tanahashi 5:dc80ccf0904e 79 float flight_time; // 離床検知からの時間
tanahashi 2:caadd357a486 80 // 運用フェーズ
tanahashi 2:caadd357a486 81 enum {
tanahashi 5:dc80ccf0904e 82 SAFETY, // 動作無し（READYへコマンドで移行）
tanahashi 5:dc80ccf0904e 83 READY, // 離床検知待機（SAFETY，FLIGHTへコマンドで移行）
tanahashi 5:dc80ccf0904e 84 FLIGHT, // 離床検知（フライトピン）から分離まで（コマンドでSEP，EMERGENCYへ移行）
tanahashi 5:dc80ccf0904e 85 SEP, // パラシュート分離
tanahashi 5:dc80ccf0904e 86 EMERGENCY, // 離床失敗した場合の緊急停止（パラシュート分離の禁止，コマンドによる）
tanahashi 5:dc80ccf0904e 87 RECOVERY // 分離終了から回収まで
tanahashi 2:caadd357a486 88 } phase;
tanahashi 5:dc80ccf0904e 89 // 動作フラグ：1=有効，0=無効
tanahashi 5:dc80ccf0904e 90 char f_sd; // SDカード
tanahashi 5:dc80ccf0904e 91 char f_gps; // GPS
tanahashi 5:dc80ccf0904e 92 char f_adxl; // ADXL375
tanahashi 5:dc80ccf0904e 93 char f_ina_in; // INA226（バッテリー）
tanahashi 5:dc80ccf0904e 94 char f_ina_ex; // INA226（外部電源）
tanahashi 5:dc80ccf0904e 95 char f_lps; // LPS22HB
tanahashi 5:dc80ccf0904e 96 char f_mpu; // MPU9250
tanahashi 5:dc80ccf0904e 97 float lat; // 緯度[deg]
tanahashi 5:dc80ccf0904e 98 float lon; // 経度[deg]
tanahashi 5:dc80ccf0904e 99 int sat; // 衛星数
tanahashi 5:dc80ccf0904e 100 int fix; // 位置特定品質
tanahashi 5:dc80ccf0904e 101 float hdop; // 水平精度低下率
tanahashi 5:dc80ccf0904e 102 float alt; // 海抜高度[m]
tanahashi 5:dc80ccf0904e 103 float geoid; // ジオイド[m]
tanahashi 5:dc80ccf0904e 104 float high_accel[3]; // 高加速度[G}
tanahashi 5:dc80ccf0904e 105 float voltage_in; // バッテリー電圧[mV]
tanahashi 5:dc80ccf0904e 106 float current_in; // バッテリー電流[mA]
tanahashi 5:dc80ccf0904e 107 float voltage_ex; // 外部電源電圧[mV]
tanahashi 5:dc80ccf0904e 108 float current_ex; // 外部電源電流[mA]
tanahashi 5:dc80ccf0904e 109 float press; // 気圧[hPa]
tanahashi 5:dc80ccf0904e 110 float temp; // 温度[C]
tanahashi 5:dc80ccf0904e 111 float accel[3]; // 加速度[G]
tanahashi 5:dc80ccf0904e 112 float gyro[3]; // 角速度[rad/s]
tanahashi 0:e8c9241fe4cb 113
tanahashi 5:dc80ccf0904e 114 char sound = 1;
tanahashi 0:e8c9241fe4cb 115
tanahashi 5:dc80ccf0904e 116 // 移動中央値
tanahashi 5:dc80ccf0904e 117 float press_buf[10];
tanahashi 5:dc80ccf0904e 118 int press_count;
tanahashi 5:dc80ccf0904e 119 float press_median;
tanahashi 5:dc80ccf0904e 120
tanahashi 5:dc80ccf0904e 121 // ローパスフィルタ
tanahashi 0:e8c9241fe4cb 122 float coef = 0.01;
tanahashi 0:e8c9241fe4cb 123 float press_prev_LPF;
tanahashi 0:e8c9241fe4cb 124 float press_LPF;
tanahashi 0:e8c9241fe4cb 125 float press_LPF_prev;
tanahashi 0:e8c9241fe4cb 126 float press_LPF_diff;
tanahashi 3:0678ef03b4f9 127 float press_LPF_ground;
tanahashi 3:0678ef03b4f9 128
tanahashi 0:e8c9241fe4cb 129 int main()
tanahashi 0:e8c9241fe4cb 130 {
tanahashi 5:dc80ccf0904e 131 wait(2); // ES920LRのブートローダー起動待ち
tanahashi 0:e8c9241fe4cb 132 init();
tanahashi 0:e8c9241fe4cb 133
tanahashi 0:e8c9241fe4cb 134 while(1) {
tanahashi 0:e8c9241fe4cb 135 read();
tanahashi 0:e8c9241fe4cb 136 switch(phase) {
tanahashi 0:e8c9241fe4cb 137 case SAFETY:
tanahashi 3:0678ef03b4f9 138 mission = 0;
tanahashi 3:0678ef03b4f9 139 relay = 1;
tanahashi 5:dc80ccf0904e 140 sep = 0;
tanahashi 5:dc80ccf0904e 141 buzzer = 0;
tanahashi 0:e8c9241fe4cb 142 break;
tanahashi 0:e8c9241fe4cb 143 case READY:
tanahashi 3:0678ef03b4f9 144 mission = 0;
tanahashi 3:0678ef03b4f9 145 relay = 1;
tanahashi 5:dc80ccf0904e 146 sep = 0;
tanahashi 5:dc80ccf0904e 147 buzzer = 0;
tanahashi 5:dc80ccf0904e 148 press_LPF_ground = press_LPF;
tanahashi 5:dc80ccf0904e 149 if(flight.read() == 1) phase = FLIGHT;
tanahashi 0:e8c9241fe4cb 150 break;
tanahashi 0:e8c9241fe4cb 151 case FLIGHT:
tanahashi 2:caadd357a486 152 mission = 1;
tanahashi 3:0678ef03b4f9 153 relay = 1;
tanahashi 5:dc80ccf0904e 154 sep = 0;
tanahashi 5:dc80ccf0904e 155 buzzer = 0;
tanahashi 0:e8c9241fe4cb 156 if(!launched) {
tanahashi 0:e8c9241fe4cb 157 flight_timer.start();
tanahashi 0:e8c9241fe4cb 158 launched = true;
tanahashi 0:e8c9241fe4cb 159 burning = true;
tanahashi 0:e8c9241fe4cb 160 }
tanahashi 0:e8c9241fe4cb 161 if(flight_timer.read() > BURN_TIME) {
tanahashi 0:e8c9241fe4cb 162 if(burning) {
tanahashi 0:e8c9241fe4cb 163 burning = false;
tanahashi 0:e8c9241fe4cb 164 press_LPF_prev = press_LPF;
tanahashi 0:e8c9241fe4cb 165 }
tanahashi 6:d951f75ce5e8 166 if(flight_timer.read() - t > 0.25) {
tanahashi 6:d951f75ce5e8 167 if(press_LPF - press_LPF_prev > 0.0f) {
tanahashi 6:d951f75ce5e8 168 if(use_apogee){
tanahashi 6:d951f75ce5e8 169 apogee = true;
tanahashi 6:d951f75ce5e8 170 }
tanahashi 6:d951f75ce5e8 171 }
tanahashi 0:e8c9241fe4cb 172 press_LPF_prev = press_LPF;
tanahashi 6:d951f75ce5e8 173 t = flight_timer.read();
tanahashi 0:e8c9241fe4cb 174 }
tanahashi 2:caadd357a486 175 if(!burning && (apogee || flight_timer.read() > T_APOGEE)) {
tanahashi 0:e8c9241fe4cb 176 phase = SEP;
tanahashi 0:e8c9241fe4cb 177 }
tanahashi 0:e8c9241fe4cb 178 }
tanahashi 0:e8c9241fe4cb 179 break;
tanahashi 0:e8c9241fe4cb 180 case SEP:
tanahashi 3:0678ef03b4f9 181 mission = 0;
tanahashi 3:0678ef03b4f9 182 relay = 1;
tanahashi 5:dc80ccf0904e 183 sep = 1;
tanahashi 5:dc80ccf0904e 184 buzzer = 1;
tanahashi 2:caadd357a486 185 if(!separated) {
tanahashi 2:caadd357a486 186 sep_timer.start();
tanahashi 2:caadd357a486 187 separated = true;
tanahashi 2:caadd357a486 188 }
tanahashi 5:dc80ccf0904e 189 if(sep_timer.read() > T_SEP) {
tanahashi 5:dc80ccf0904e 190 phase = RECOVERY;
tanahashi 5:dc80ccf0904e 191 }
tanahashi 0:e8c9241fe4cb 192 break;
tanahashi 0:e8c9241fe4cb 193 case EMERGENCY:
tanahashi 3:0678ef03b4f9 194 mission = 1;
tanahashi 3:0678ef03b4f9 195 relay = 1;
tanahashi 5:dc80ccf0904e 196 sep = 0;
tanahashi 0:e8c9241fe4cb 197 buzzer = 0;
tanahashi 5:dc80ccf0904e 198 break;
tanahashi 0:e8c9241fe4cb 199 case RECOVERY:
tanahashi 5:dc80ccf0904e 200 mission = 0;
tanahashi 6:d951f75ce5e8 201 if(!landed && press_LPF > press_LPF_ground) {
tanahashi 5:dc80ccf0904e 202 relay = 0;
tanahashi 6:d951f75ce5e8 203 landed = true;
tanahashi 3:0678ef03b4f9 204 }
tanahashi 0:e8c9241fe4cb 205 sep = 0;
tanahashi 5:dc80ccf0904e 206 buzzer = sound;
tanahashi 0:e8c9241fe4cb 207 break;
tanahashi 0:e8c9241fe4cb 208 }
tanahashi 0:e8c9241fe4cb 209 }
tanahashi 0:e8c9241fe4cb 210 }
tanahashi 0:e8c9241fe4cb 211
tanahashi 0:e8c9241fe4cb 212 void init()
tanahashi 0:e8c9241fe4cb 213 {
tanahashi 5:dc80ccf0904e 214 // 連番のファイルを作成
tanahashi 2:caadd357a486 215 char file_name_format[] = "/sd/IZU2021_AVIONICS_%d.dat";
tanahashi 0:e8c9241fe4cb 216 int file_number = 1;
tanahashi 0:e8c9241fe4cb 217 while(1) {
tanahashi 0:e8c9241fe4cb 218 sprintf(file_name, file_name_format, file_number);
tanahashi 2:caadd357a486 219 fp = fopen(file_name, "r");
tanahashi 2:caadd357a486 220 if(fp != NULL) {
tanahashi 2:caadd357a486 221 fclose(fp);
tanahashi 0:e8c9241fe4cb 222 file_number++;
tanahashi 0:e8c9241fe4cb 223 } else {
tanahashi 2:caadd357a486 224 sprintf(file_name, file_name_format, file_number);
tanahashi 0:e8c9241fe4cb 225 break;
tanahashi 0:e8c9241fe4cb 226 }
tanahashi 0:e8c9241fe4cb 227 }
tanahashi 2:caadd357a486 228 fp = fopen(file_name, "w");
tanahashi 2:caadd357a486 229
tanahashi 0:e8c9241fe4cb 230 sd_timer.start();
tanahashi 0:e8c9241fe4cb 231
tanahashi 2:caadd357a486 232 if(fp) {
tanahashi 5:dc80ccf0904e 233 fprintf(fp, "mission_time,flight_time,phase,");
tanahashi 6:d951f75ce5e8 234 fprintf(fp, "use_apogee,apogee,landed,signal,mission,relay,sep,buzzer,");
tanahashi 5:dc80ccf0904e 235 fprintf(fp, "flight,f_sd,f_gps,f_adxl,f_ina_in,f_ina_ex,f_lps,f_mpu,");
tanahashi 2:caadd357a486 236 fprintf(fp, "lat,lon,sat,fix,hdop,alt,geoid,");
tanahashi 2:caadd357a486 237 fprintf(fp, "high_accel_x,high_accel_y,high_accel_z,");
tanahashi 2:caadd357a486 238 fprintf(fp, "voltage_in,current_in,");
tanahashi 2:caadd357a486 239 fprintf(fp, "voltage_ex,current_ex,");
tanahashi 2:caadd357a486 240 fprintf(fp, "press,temp,");
tanahashi 2:caadd357a486 241 fprintf(fp, "accel_x,accel_y,accel_z,");
tanahashi 2:caadd357a486 242 fprintf(fp, "gyro_x,gyro_y,gyro_z,");
tanahashi 2:caadd357a486 243 fprintf(fp, "mag_x,mag_y,mag_z");
tanahashi 2:caadd357a486 244 fprintf(fp, "\r\n");
tanahashi 0:e8c9241fe4cb 245 }
tanahashi 0:e8c9241fe4cb 246
tanahashi 5:dc80ccf0904e 247 record_ticker.attach(record, 1.0f / SAMPLING_FREQUENCY);
tanahashi 5:dc80ccf0904e 248 downlink_ticker.attach(downlink, 1.0f / DOWNLINK_RATE);
tanahashi 5:dc80ccf0904e 249
tanahashi 5:dc80ccf0904e 250 // ミッション基板のリセット
tanahashi 5:dc80ccf0904e 251 mission = 1;
tanahashi 5:dc80ccf0904e 252 relay = 0;
tanahashi 5:dc80ccf0904e 253
tanahashi 5:dc80ccf0904e 254 sep = 0;
tanahashi 5:dc80ccf0904e 255 buzzer = 0;
tanahashi 0:e8c9241fe4cb 256
tanahashi 0:e8c9241fe4cb 257 es.attach(command_handler);
tanahashi 0:e8c9241fe4cb 258
tanahashi 0:e8c9241fe4cb 259 adxl.begin();
tanahashi 0:e8c9241fe4cb 260 ina_in.begin();
tanahashi 0:e8c9241fe4cb 261 ina_ex.begin();
tanahashi 0:e8c9241fe4cb 262 lps.begin();
tanahashi 0:e8c9241fe4cb 263 mpu.begin();
tanahashi 5:dc80ccf0904e 264
tanahashi 5:dc80ccf0904e 265 mission_timer.start();
tanahashi 0:e8c9241fe4cb 266 }
tanahashi 0:e8c9241fe4cb 267
tanahashi 0:e8c9241fe4cb 268 void read()
tanahashi 0:e8c9241fe4cb 269 {
tanahashi 5:dc80ccf0904e 270 if(mission_timer.read() >= 30*60) {
tanahashi 0:e8c9241fe4cb 271 mission_timer.reset();
tanahashi 0:e8c9241fe4cb 272 mission_timer_reset++;
tanahashi 0:e8c9241fe4cb 273 }
tanahashi 5:dc80ccf0904e 274 mission_time = mission_timer.read() + mission_timer_reset*30*60;
tanahashi 2:caadd357a486 275 flight_time = flight_timer.read();
tanahashi 0:e8c9241fe4cb 276
tanahashi 0:e8c9241fe4cb 277 lat = gps.get_lat();
tanahashi 0:e8c9241fe4cb 278 lon = gps.get_lon();
tanahashi 0:e8c9241fe4cb 279 sat = gps.get_sat();
tanahashi 0:e8c9241fe4cb 280 fix = gps.get_fix();
tanahashi 0:e8c9241fe4cb 281 hdop = gps.get_hdop();
tanahashi 0:e8c9241fe4cb 282 alt = gps.get_alt();
tanahashi 0:e8c9241fe4cb 283 geoid = gps.get_geoid();
tanahashi 0:e8c9241fe4cb 284
tanahashi 2:caadd357a486 285 f_sd = (bool)fp;
tanahashi 0:e8c9241fe4cb 286
tanahashi 0:e8c9241fe4cb 287 f_gps = (bool)fix;
tanahashi 0:e8c9241fe4cb 288
tanahashi 0:e8c9241fe4cb 289 f_adxl = adxl.test();
tanahashi 0:e8c9241fe4cb 290 if(f_adxl) {
tanahashi 0:e8c9241fe4cb 291 adxl.read(high_accel);
tanahashi 0:e8c9241fe4cb 292 }
tanahashi 0:e8c9241fe4cb 293
tanahashi 0:e8c9241fe4cb 294 f_ina_in = ina_in.test();
tanahashi 0:e8c9241fe4cb 295 if(f_ina_in) {
tanahashi 0:e8c9241fe4cb 296 ina_in.read_voltage(&voltage_in);
tanahashi 0:e8c9241fe4cb 297 ina_in.read_current(&current_in);
tanahashi 0:e8c9241fe4cb 298 }
tanahashi 0:e8c9241fe4cb 299
tanahashi 0:e8c9241fe4cb 300 f_ina_ex = ina_ex.test();
tanahashi 0:e8c9241fe4cb 301 if(f_ina_ex) {
tanahashi 0:e8c9241fe4cb 302 ina_ex.read_voltage(&voltage_ex);
tanahashi 0:e8c9241fe4cb 303 ina_ex.read_current(&current_ex);
tanahashi 0:e8c9241fe4cb 304 }
tanahashi 0:e8c9241fe4cb 305
tanahashi 0:e8c9241fe4cb 306 f_lps = lps.test();
tanahashi 0:e8c9241fe4cb 307 if(f_lps) {
tanahashi 0:e8c9241fe4cb 308 lps.read_press(&press);
tanahashi 0:e8c9241fe4cb 309 lps.read_temp(&temp);
tanahashi 0:e8c9241fe4cb 310
tanahashi 5:dc80ccf0904e 311 // 移動中央値を求める
tanahashi 5:dc80ccf0904e 312 press_buf[press_count] = press;
tanahashi 5:dc80ccf0904e 313 press_count++;
tanahashi 5:dc80ccf0904e 314 if(press_count > 10) {
tanahashi 5:dc80ccf0904e 315 press_count = 0;
tanahashi 5:dc80ccf0904e 316 }
tanahashi 5:dc80ccf0904e 317
tanahashi 5:dc80ccf0904e 318 // 配列のコピー
tanahashi 5:dc80ccf0904e 319 float buf[10];
tanahashi 5:dc80ccf0904e 320 for(int i = 0; i < 10; i++) {
tanahashi 5:dc80ccf0904e 321 buf[i] = press_buf[i];
tanahashi 5:dc80ccf0904e 322 }
tanahashi 5:dc80ccf0904e 323
tanahashi 5:dc80ccf0904e 324 // バブルソート
tanahashi 5:dc80ccf0904e 325 for(int i = 0; i < 9; i++) {
tanahashi 5:dc80ccf0904e 326 for(int j = 9; j > i; j--) {
tanahashi 5:dc80ccf0904e 327 if(buf[j] < buf[j - 1]) {
tanahashi 5:dc80ccf0904e 328 float temp = buf[j];
tanahashi 5:dc80ccf0904e 329 buf[j] = buf[j - 1];
tanahashi 5:dc80ccf0904e 330 buf[j - 1] = temp;
tanahashi 5:dc80ccf0904e 331 }
tanahashi 5:dc80ccf0904e 332 }
tanahashi 5:dc80ccf0904e 333 }
tanahashi 5:dc80ccf0904e 334
tanahashi 5:dc80ccf0904e 335 // 中央値
tanahashi 5:dc80ccf0904e 336 press_median = (buf[4] + buf[5]) / 2;
tanahashi 5:dc80ccf0904e 337
tanahashi 5:dc80ccf0904e 338 // ローパスフィルタ
tanahashi 5:dc80ccf0904e 339 press_LPF = press_median * coef + press_prev_LPF * (1 - coef);
tanahashi 0:e8c9241fe4cb 340 press_prev_LPF = press_LPF;
tanahashi 0:e8c9241fe4cb 341 }
tanahashi 0:e8c9241fe4cb 342
tanahashi 0:e8c9241fe4cb 343 f_mpu = mpu.test();
tanahashi 0:e8c9241fe4cb 344 if(f_mpu) {
tanahashi 0:e8c9241fe4cb 345 mpu.read_accel(accel);
tanahashi 0:e8c9241fe4cb 346 mpu.read_gyro(gyro);
tanahashi 0:e8c9241fe4cb 347 }
tanahashi 0:e8c9241fe4cb 348 }
tanahashi 0:e8c9241fe4cb 349
tanahashi 0:e8c9241fe4cb 350 void command_handler(char *command)
tanahashi 0:e8c9241fe4cb 351 {
tanahashi 0:e8c9241fe4cb 352 switch(command[0]) {
tanahashi 5:dc80ccf0904e 353 case 0xB0: // '0'
tanahashi 0:e8c9241fe4cb 354 if(phase == READY) phase = SAFETY;
tanahashi 0:e8c9241fe4cb 355 break;
tanahashi 5:dc80ccf0904e 356 case 0xB1: // '1'
tanahashi 0:e8c9241fe4cb 357 if(phase == SAFETY) phase = READY;
tanahashi 0:e8c9241fe4cb 358 break;
tanahashi 5:dc80ccf0904e 359 case 0xB2: // '2'
tanahashi 0:e8c9241fe4cb 360 if(phase == READY) phase = FLIGHT;
tanahashi 0:e8c9241fe4cb 361 break;
tanahashi 5:dc80ccf0904e 362 case 0xB3: // '3'
tanahashi 0:e8c9241fe4cb 363 if(!burning && phase == FLIGHT) phase = SEP;
tanahashi 0:e8c9241fe4cb 364 break;
tanahashi 5:dc80ccf0904e 365 case 0xB4: // '4'
tanahashi 2:caadd357a486 366 if(phase == FLIGHT) phase = EMERGENCY;
tanahashi 0:e8c9241fe4cb 367 break;
tanahashi 5:dc80ccf0904e 368 case 0xB5: // '5'
tanahashi 2:caadd357a486 369 if(phase == SEP) phase = RECOVERY;
tanahashi 0:e8c9241fe4cb 370 break;
tanahashi 6:d951f75ce5e8 371 case 0xE1: // 'a'
tanahashi 6:d951f75ce5e8 372 use_apogee = !use_apogee;
tanahashi 6:d951f75ce5e8 373 break;
tanahashi 6:d951f75ce5e8 374 case 0xE2: // 'b'
tanahashi 6:d951f75ce5e8 375 sound = !sound;
tanahashi 5:dc80ccf0904e 376 break;
tanahashi 5:dc80ccf0904e 377 case 0xFF: // 'DEL'
tanahashi 0:e8c9241fe4cb 378 NVIC_SystemReset();
tanahashi 0:e8c9241fe4cb 379 break;
tanahashi 0:e8c9241fe4cb 380 }
tanahashi 0:e8c9241fe4cb 381 }
tanahashi 0:e8c9241fe4cb 382
tanahashi 0:e8c9241fe4cb 383 void downlink()
tanahashi 0:e8c9241fe4cb 384 {
tanahashi 5:dc80ccf0904e 385 short mission_time_bits = (short)mission_time;;
tanahashi 5:dc80ccf0904e 386 short flight_time_bits = (short)(flight_time * 10);
tanahashi 5:dc80ccf0904e 387 char flags1 = 0;
tanahashi 6:d951f75ce5e8 388 flags1 |= (char)use_apogee << 7;
tanahashi 6:d951f75ce5e8 389 flags1 |= (char)apogee << 6;
tanahashi 6:d951f75ce5e8 390 flags1 |= (char)landed << 5;
tanahashi 6:d951f75ce5e8 391 flags1 |= signal.read() << 4;
tanahashi 6:d951f75ce5e8 392 flags1 |= mission.read() << 3;
tanahashi 6:d951f75ce5e8 393 flags1 |= relay.read() << 2;
tanahashi 6:d951f75ce5e8 394 flags1 |= sep.read() << 1;
tanahashi 6:d951f75ce5e8 395 flags1 |= buzzer.read() << 0;
tanahashi 5:dc80ccf0904e 396 char flags2 = 0;
tanahashi 5:dc80ccf0904e 397 flags2 |= flight.read() << 7;
tanahashi 5:dc80ccf0904e 398 flags2 |= f_sd << 6;
tanahashi 5:dc80ccf0904e 399 flags2 |= f_gps << 5;
tanahashi 5:dc80ccf0904e 400 flags2 |= f_adxl << 4;
tanahashi 5:dc80ccf0904e 401 flags2 |= f_ina_in << 3;
tanahashi 5:dc80ccf0904e 402 flags2 |= f_ina_ex << 2;
tanahashi 5:dc80ccf0904e 403 flags2 |= f_lps << 1;
tanahashi 5:dc80ccf0904e 404 flags2 |= f_mpu << 0;
tanahashi 5:dc80ccf0904e 405 int lat_bits = *((int*)&lat);
tanahashi 5:dc80ccf0904e 406 int lon_bits = *((int*)&lon);
tanahashi 5:dc80ccf0904e 407 int alt_bits = *((int*)&alt);
tanahashi 5:dc80ccf0904e 408 short high_accel_bits[3];
tanahashi 0:e8c9241fe4cb 409 for(int i = 0; i < 3; i++) {
tanahashi 2:caadd357a486 410 high_accel_bits[i] = (short)(high_accel[i] / ADXL375_LSB);
tanahashi 2:caadd357a486 411 }
tanahashi 5:dc80ccf0904e 412 short voltage_in_bits = (short)(voltage_in / INA226_VOLTAGE_LSB);
tanahashi 5:dc80ccf0904e 413 short current_in_bits = (short)(current_in / INA226_CURRENT_LSB);
tanahashi 5:dc80ccf0904e 414 short voltage_ex_bits = (short)(voltage_ex / INA226_VOLTAGE_LSB);
tanahashi 5:dc80ccf0904e 415 short current_ex_bits = (short)(current_ex / INA226_CURRENT_LSB);
tanahashi 5:dc80ccf0904e 416 int press_bits = (int)(press * LPS22HB_PRESS_LSB);
tanahashi 5:dc80ccf0904e 417 short temp_bits = (short)(temp / LPS22HB_TEMP_LSB);
tanahashi 5:dc80ccf0904e 418 short accel_bits[3];
tanahashi 2:caadd357a486 419 for(int i = 0; i < 3; i++) {
tanahashi 2:caadd357a486 420 accel_bits[i] = (short)(accel[i] / (MPU9250_ACCEL_LSB * 8));
tanahashi 0:e8c9241fe4cb 421 }
tanahashi 5:dc80ccf0904e 422 short gyro_bits[3];
tanahashi 0:e8c9241fe4cb 423 for(int i = 0; i < 3; i++) {
tanahashi 2:caadd357a486 424 gyro_bits[i] = (short)(gyro[i] / (MPU9250_GYRO_LSB * 8));
tanahashi 0:e8c9241fe4cb 425 }
tanahashi 0:e8c9241fe4cb 426
tanahashi 0:e8c9241fe4cb 427 char data[50];
tanahashi 2:caadd357a486 428 data[0] = ((char*)&mission_time_bits)[0];
tanahashi 2:caadd357a486 429 data[1] = ((char*)&mission_time_bits)[1];
tanahashi 2:caadd357a486 430 data[2] = ((char*)&flight_time_bits)[0];
tanahashi 2:caadd357a486 431 data[3] = ((char*)&flight_time_bits)[1];
tanahashi 2:caadd357a486 432 data[4] = phase;
tanahashi 5:dc80ccf0904e 433 data[5] = flags1;
tanahashi 5:dc80ccf0904e 434 data[6] = flags2;
tanahashi 3:0678ef03b4f9 435 data[7] = ((char*)&lat_bits)[0];
tanahashi 3:0678ef03b4f9 436 data[8] = ((char*)&lat_bits)[1];
tanahashi 3:0678ef03b4f9 437 data[9] = ((char*)&lat_bits)[2];
tanahashi 3:0678ef03b4f9 438 data[10] = ((char*)&lat_bits)[3];
tanahashi 4:ada7cdefd3e0 439 data[11] = ((char*)&lon_bits)[0];
tanahashi 4:ada7cdefd3e0 440 data[12] = ((char*)&lon_bits)[1];
tanahashi 4:ada7cdefd3e0 441 data[13] = ((char*)&lon_bits)[2];
tanahashi 4:ada7cdefd3e0 442 data[14] = ((char*)&lon_bits)[3];
tanahashi 4:ada7cdefd3e0 443 data[15] = ((char*)&alt_bits)[0];
tanahashi 4:ada7cdefd3e0 444 data[16] = ((char*)&alt_bits)[1];
tanahashi 4:ada7cdefd3e0 445 data[17] = ((char*)&alt_bits)[2];
tanahashi 4:ada7cdefd3e0 446 data[18] = ((char*)&alt_bits)[3];
tanahashi 2:caadd357a486 447 data[19] = ((char*)&high_accel_bits[0])[0];
tanahashi 2:caadd357a486 448 data[20] = ((char*)&high_accel_bits[0])[1];
tanahashi 2:caadd357a486 449 data[21] = ((char*)&high_accel_bits[1])[0];
tanahashi 2:caadd357a486 450 data[22] = ((char*)&high_accel_bits[1])[1];
tanahashi 2:caadd357a486 451 data[23] = ((char*)&high_accel_bits[2])[0];
tanahashi 2:caadd357a486 452 data[24] = ((char*)&high_accel_bits[2])[1];
tanahashi 2:caadd357a486 453 data[25] = ((char*)&voltage_in_bits)[0];
tanahashi 2:caadd357a486 454 data[26] = ((char*)&voltage_in_bits)[1];
tanahashi 2:caadd357a486 455 data[27] = ((char*)&current_in_bits)[0];
tanahashi 2:caadd357a486 456 data[28] = ((char*)&current_in_bits)[1];
tanahashi 2:caadd357a486 457 data[29] = ((char*)&voltage_ex_bits)[0];
tanahashi 2:caadd357a486 458 data[30] = ((char*)&voltage_ex_bits)[1];
tanahashi 2:caadd357a486 459 data[31] = ((char*)&current_ex_bits)[0];
tanahashi 2:caadd357a486 460 data[32] = ((char*)&current_ex_bits)[1];
tanahashi 2:caadd357a486 461 data[33] = ((char*)&press_bits)[0];
tanahashi 2:caadd357a486 462 data[34] = ((char*)&press_bits)[1];
tanahashi 2:caadd357a486 463 data[35] = ((char*)&press_bits)[2];
tanahashi 2:caadd357a486 464 data[36] = ((char*)&temp_bits)[0];
tanahashi 2:caadd357a486 465 data[37] = ((char*)&temp_bits)[1];
tanahashi 2:caadd357a486 466 data[38] = ((char*)&accel_bits[0])[0];
tanahashi 2:caadd357a486 467 data[39] = ((char*)&accel_bits[0])[1];
tanahashi 2:caadd357a486 468 data[40] = ((char*)&accel_bits[1])[0];
tanahashi 2:caadd357a486 469 data[41] = ((char*)&accel_bits[1])[1];
tanahashi 2:caadd357a486 470 data[42] = ((char*)&accel_bits[2])[0];
tanahashi 2:caadd357a486 471 data[43] = ((char*)&accel_bits[2])[1];
tanahashi 2:caadd357a486 472 data[44] = ((char*)&gyro_bits[0])[0];
tanahashi 2:caadd357a486 473 data[45] = ((char*)&gyro_bits[0])[1];
tanahashi 2:caadd357a486 474 data[46] = ((char*)&gyro_bits[1])[0];
tanahashi 2:caadd357a486 475 data[47] = ((char*)&gyro_bits[1])[1];
tanahashi 2:caadd357a486 476 data[48] = ((char*)&gyro_bits[2])[0];
tanahashi 2:caadd357a486 477 data[49] = ((char*)&gyro_bits[2])[1];
tanahashi 0:e8c9241fe4cb 478
tanahashi 0:e8c9241fe4cb 479 es.send(data, 50);
tanahashi 0:e8c9241fe4cb 480 }
tanahashi 0:e8c9241fe4cb 481
tanahashi 0:e8c9241fe4cb 482 void record()
tanahashi 0:e8c9241fe4cb 483 {
tanahashi 5:dc80ccf0904e 484 if(phase >= FLIGHT && !landed) {
tanahashi 0:e8c9241fe4cb 485 char data[128];
tanahashi 2:caadd357a486 486 data[0] = ((char*)&mission_time)[0];
tanahashi 2:caadd357a486 487 data[1] = ((char*)&mission_time)[1];
tanahashi 2:caadd357a486 488 data[2] = ((char*)&mission_time)[2];
tanahashi 2:caadd357a486 489 data[3] = ((char*)&mission_time)[3];
tanahashi 2:caadd357a486 490 data[4] = ((char*)&flight_time)[0];
tanahashi 2:caadd357a486 491 data[5] = ((char*)&flight_time)[1];
tanahashi 2:caadd357a486 492 data[6] = ((char*)&flight_time)[2];
tanahashi 2:caadd357a486 493 data[7] = ((char*)&flight_time)[3];
tanahashi 5:dc80ccf0904e 494 data[8] = phase;
tanahashi 6:d951f75ce5e8 495 data[9] = use_apogee;
tanahashi 5:dc80ccf0904e 496 data[10] = apogee;
tanahashi 6:d951f75ce5e8 497 data[11] = landed;
tanahashi 6:d951f75ce5e8 498 data[12] = signal.read();
tanahashi 5:dc80ccf0904e 499 data[13] = mission.read();
tanahashi 5:dc80ccf0904e 500 data[14] = relay.read();
tanahashi 5:dc80ccf0904e 501 data[15] = sep.read();
tanahashi 5:dc80ccf0904e 502 data[16] = buzzer.read();
tanahashi 5:dc80ccf0904e 503 data[17] = flight.read();
tanahashi 5:dc80ccf0904e 504 data[18] = f_sd;
tanahashi 5:dc80ccf0904e 505 data[19] = f_gps;
tanahashi 5:dc80ccf0904e 506 data[20] = f_adxl;
tanahashi 5:dc80ccf0904e 507 data[21] = f_ina_in;
tanahashi 5:dc80ccf0904e 508 data[22] = f_ina_ex;
tanahashi 5:dc80ccf0904e 509 data[23] = f_lps;
tanahashi 5:dc80ccf0904e 510 data[24] = f_mpu;
tanahashi 5:dc80ccf0904e 511 data[25] = ((char*)&lat)[0];
tanahashi 5:dc80ccf0904e 512 data[26] = ((char*)&lat)[1];
tanahashi 5:dc80ccf0904e 513 data[27] = ((char*)&lat)[2];
tanahashi 5:dc80ccf0904e 514 data[28] = ((char*)&lat)[3];
tanahashi 5:dc80ccf0904e 515 data[29] = ((char*)&lon)[0];
tanahashi 5:dc80ccf0904e 516 data[30] = ((char*)&lon)[1];
tanahashi 5:dc80ccf0904e 517 data[31] = ((char*)&lon)[2];
tanahashi 5:dc80ccf0904e 518 data[32] = ((char*)&lon)[3];
tanahashi 5:dc80ccf0904e 519 data[33] = ((char*)&sat)[0];
tanahashi 5:dc80ccf0904e 520 data[34] = ((char*)&sat)[1];
tanahashi 5:dc80ccf0904e 521 data[35] = ((char*)&sat)[2];
tanahashi 5:dc80ccf0904e 522 data[36] = ((char*)&sat)[3];
tanahashi 5:dc80ccf0904e 523 data[37] = ((char*)&fix)[0];
tanahashi 5:dc80ccf0904e 524 data[38] = ((char*)&fix)[1];
tanahashi 5:dc80ccf0904e 525 data[39] = ((char*)&fix)[2];
tanahashi 5:dc80ccf0904e 526 data[40] = ((char*)&fix)[3];
tanahashi 5:dc80ccf0904e 527 data[41] = ((char*)&hdop)[0];
tanahashi 5:dc80ccf0904e 528 data[42] = ((char*)&hdop)[1];
tanahashi 5:dc80ccf0904e 529 data[43] = ((char*)&hdop)[2];
tanahashi 5:dc80ccf0904e 530 data[44] = ((char*)&hdop)[3];
tanahashi 5:dc80ccf0904e 531 data[45] = ((char*)&alt)[0];
tanahashi 5:dc80ccf0904e 532 data[46] = ((char*)&alt)[1];
tanahashi 5:dc80ccf0904e 533 data[47] = ((char*)&alt)[2];
tanahashi 5:dc80ccf0904e 534 data[48] = ((char*)&alt)[3];
tanahashi 5:dc80ccf0904e 535 data[49] = ((char*)&geoid)[0];
tanahashi 5:dc80ccf0904e 536 data[50] = ((char*)&geoid)[1];
tanahashi 5:dc80ccf0904e 537 data[51] = ((char*)&geoid)[2];
tanahashi 5:dc80ccf0904e 538 data[52] = ((char*)&geoid)[3];
tanahashi 5:dc80ccf0904e 539 data[53] = ((char*)&high_accel[0])[0];
tanahashi 5:dc80ccf0904e 540 data[54] = ((char*)&high_accel[0])[1];
tanahashi 5:dc80ccf0904e 541 data[55] = ((char*)&high_accel[0])[2];
tanahashi 5:dc80ccf0904e 542 data[56] = ((char*)&high_accel[0])[3];
tanahashi 5:dc80ccf0904e 543 data[57] = ((char*)&high_accel[1])[0];
tanahashi 5:dc80ccf0904e 544 data[58] = ((char*)&high_accel[1])[1];
tanahashi 5:dc80ccf0904e 545 data[59] = ((char*)&high_accel[1])[2];
tanahashi 5:dc80ccf0904e 546 data[60] = ((char*)&high_accel[1])[3];
tanahashi 5:dc80ccf0904e 547 data[61] = ((char*)&high_accel[2])[0];
tanahashi 5:dc80ccf0904e 548 data[62] = ((char*)&high_accel[2])[1];
tanahashi 5:dc80ccf0904e 549 data[63] = ((char*)&high_accel[2])[2];
tanahashi 5:dc80ccf0904e 550 data[64] = ((char*)&high_accel[2])[3];
tanahashi 5:dc80ccf0904e 551 data[65] = ((char*)&voltage_in)[0];
tanahashi 5:dc80ccf0904e 552 data[66] = ((char*)&voltage_in)[1];
tanahashi 5:dc80ccf0904e 553 data[67] = ((char*)&voltage_in)[2];
tanahashi 5:dc80ccf0904e 554 data[68] = ((char*)&voltage_in)[3];
tanahashi 5:dc80ccf0904e 555 data[69] = ((char*)&current_in)[0];
tanahashi 5:dc80ccf0904e 556 data[70] = ((char*)&current_in)[1];
tanahashi 5:dc80ccf0904e 557 data[71] = ((char*)&current_in)[2];
tanahashi 5:dc80ccf0904e 558 data[72] = ((char*)&current_in)[3];
tanahashi 5:dc80ccf0904e 559 data[73] = ((char*)&voltage_ex)[0];
tanahashi 5:dc80ccf0904e 560 data[74] = ((char*)&voltage_ex)[1];
tanahashi 5:dc80ccf0904e 561 data[75] = ((char*)&voltage_ex)[2];
tanahashi 5:dc80ccf0904e 562 data[76] = ((char*)&voltage_ex)[3];
tanahashi 5:dc80ccf0904e 563 data[77] = ((char*)&current_ex)[0];
tanahashi 5:dc80ccf0904e 564 data[78] = ((char*)&current_ex)[1];
tanahashi 5:dc80ccf0904e 565 data[79] = ((char*)&current_ex)[2];
tanahashi 5:dc80ccf0904e 566 data[80] = ((char*)&current_ex)[3];
tanahashi 5:dc80ccf0904e 567 data[81] = ((char*)&press)[0];
tanahashi 5:dc80ccf0904e 568 data[82] = ((char*)&press)[1];
tanahashi 5:dc80ccf0904e 569 data[83] = ((char*)&press)[2];
tanahashi 5:dc80ccf0904e 570 data[84] = ((char*)&press)[3];
tanahashi 5:dc80ccf0904e 571 data[85] = ((char*)&temp)[0];
tanahashi 5:dc80ccf0904e 572 data[86] = ((char*)&temp)[1];
tanahashi 5:dc80ccf0904e 573 data[87] = ((char*)&temp)[2];
tanahashi 5:dc80ccf0904e 574 data[88] = ((char*)&temp)[3];
tanahashi 5:dc80ccf0904e 575 data[89] = ((char*)&accel[0])[0];
tanahashi 5:dc80ccf0904e 576 data[90] = ((char*)&accel[0])[1];
tanahashi 5:dc80ccf0904e 577 data[91] = ((char*)&accel[0])[2];
tanahashi 5:dc80ccf0904e 578 data[92] = ((char*)&accel[0])[3];
tanahashi 5:dc80ccf0904e 579 data[93] = ((char*)&accel[1])[0];
tanahashi 5:dc80ccf0904e 580 data[94] = ((char*)&accel[1])[1];
tanahashi 5:dc80ccf0904e 581 data[95] = ((char*)&accel[1])[2];
tanahashi 5:dc80ccf0904e 582 data[96] = ((char*)&accel[1])[3];
tanahashi 5:dc80ccf0904e 583 data[97] = ((char*)&accel[2])[0];
tanahashi 5:dc80ccf0904e 584 data[98] = ((char*)&accel[2])[1];
tanahashi 5:dc80ccf0904e 585 data[99] = ((char*)&accel[2])[2];
tanahashi 5:dc80ccf0904e 586 data[100] = ((char*)&accel[2])[3];
tanahashi 5:dc80ccf0904e 587 data[101] = ((char*)&gyro[0])[0];
tanahashi 5:dc80ccf0904e 588 data[102] = ((char*)&gyro[0])[1];
tanahashi 5:dc80ccf0904e 589 data[103] = ((char*)&gyro[0])[2];
tanahashi 5:dc80ccf0904e 590 data[104] = ((char*)&gyro[0])[3];
tanahashi 5:dc80ccf0904e 591 data[105] = ((char*)&gyro[1])[0];
tanahashi 5:dc80ccf0904e 592 data[106] = ((char*)&gyro[1])[1];
tanahashi 5:dc80ccf0904e 593 data[107] = ((char*)&gyro[1])[2];
tanahashi 5:dc80ccf0904e 594 data[108] = ((char*)&gyro[1])[3];
tanahashi 5:dc80ccf0904e 595 data[109] = ((char*)&gyro[2])[0];
tanahashi 5:dc80ccf0904e 596 data[110] = ((char*)&gyro[2])[1];
tanahashi 5:dc80ccf0904e 597 data[111] = ((char*)&gyro[2])[2];
tanahashi 5:dc80ccf0904e 598 data[112] = ((char*)&gyro[2])[3];
tanahashi 5:dc80ccf0904e 599 data[113] = 0;
tanahashi 5:dc80ccf0904e 600 data[114] = 0;
tanahashi 5:dc80ccf0904e 601 data[115] = 0;
tanahashi 5:dc80ccf0904e 602 data[116] = 0;
tanahashi 5:dc80ccf0904e 603 data[117] = 0;
tanahashi 5:dc80ccf0904e 604 data[118] = 0;
tanahashi 5:dc80ccf0904e 605 data[119] = 0;
tanahashi 5:dc80ccf0904e 606 data[120] = 0;
tanahashi 5:dc80ccf0904e 607 data[121] = 0;
tanahashi 5:dc80ccf0904e 608 data[122] = 0;
tanahashi 5:dc80ccf0904e 609 data[123] = 0;
tanahashi 5:dc80ccf0904e 610 data[124] = 0;
tanahashi 5:dc80ccf0904e 611 data[125] = 0;
tanahashi 5:dc80ccf0904e 612 data[126] = 0;
tanahashi 5:dc80ccf0904e 613 data[127] = 0;
tanahashi 0:e8c9241fe4cb 614
tanahashi 0:e8c9241fe4cb 615 eeprom.write(addr, data, 128);
tanahashi 0:e8c9241fe4cb 616 addr += 0x80;
tanahashi 0:e8c9241fe4cb 617 }
tanahashi 0:e8c9241fe4cb 618
tanahashi 2:caadd357a486 619 if(fp) {
tanahashi 5:dc80ccf0904e 620 char *phase_names[] = {"SAFETY", "READY", "FLIGHT", "SEP", "EMERGENCY", "RECOVERY"};
tanahashi 5:dc80ccf0904e 621 fprintf(fp, "%.3f,%.3f,%s,", mission_time, flight_time, phase_names[phase]);
tanahashi 6:d951f75ce5e8 622 fprintf(fp, "%d,%d,%d,%d,%d,%d,%d,%d,", use_apogee, apogee, landed, signal.read(), mission.read(), relay.read(), sep.read(), buzzer.read());
tanahashi 5:dc80ccf0904e 623 fprintf(fp, "%d,%d,%d,%d,%d,%d,%d,%d,", flight.read(), f_sd, f_gps, f_adxl, f_ina_in, f_ina_ex, f_lps, f_mpu);
tanahashi 5:dc80ccf0904e 624 fprintf(fp, "%.6f,%.6f,%d,%d,%f,%f,%f,", lat, lon, sat, fix, hdop, alt, geoid);
tanahashi 2:caadd357a486 625 fprintf(fp, "%f,%f,%f,", high_accel[0], high_accel[1], high_accel[2]);
tanahashi 2:caadd357a486 626 fprintf(fp, "%f,%f,", voltage_in, current_in);
tanahashi 2:caadd357a486 627 fprintf(fp, "%f,%f,", voltage_ex, current_ex);
tanahashi 2:caadd357a486 628 fprintf(fp, "%f,%f,", press, temp);
tanahashi 2:caadd357a486 629 fprintf(fp, "%f,%f,%f,", accel[0], accel[1], accel[2]);
tanahashi 5:dc80ccf0904e 630 fprintf(fp, "%f,%f,%f", gyro[0], gyro[1], gyro[2]);
tanahashi 2:caadd357a486 631 fprintf(fp, "\r\n");
tanahashi 0:e8c9241fe4cb 632 }
tanahashi 0:e8c9241fe4cb 633
tanahashi 5:dc80ccf0904e 634 if(sd_timer.read() >= 20) {
tanahashi 0:e8c9241fe4cb 635 sd_timer.reset();
tanahashi 2:caadd357a486 636 if(fp) {
tanahashi 2:caadd357a486 637 fclose(fp);
tanahashi 2:caadd357a486 638 fp = fopen(file_name, "a");
tanahashi 0:e8c9241fe4cb 639 }
tanahashi 0:e8c9241fe4cb 640 }
tanahashi 0:e8c9241fe4cb 641 }