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@4:ada7cdefd3e0, 2021-03-09 (annotated)

Committer:
tanahashi
Date:
Tue Mar 09 12:04:29 2021 +0000
Revision:
4:ada7cdefd3e0
Parent:
3:0678ef03b4f9
Child:
5:dc80ccf0904e
version1.2

Who changed what in which revision?

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